configuration files with dynamic reconfigure tools replaced

mrpt_rbpf_slam/CMakeLists.txt

@@ -36,6 +36,12 @@ IF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug")
   add_compile_options(-O3)
 ENDIF()
 
+# Add dynamic reconfigure api
+generate_dynamic_reconfigure_options(
+  cfg/General.cfg
+  cfg/Motion.cfg
+)
+
 catkin_package(
 	CATKIN_DEPENDS
   nav_msgs
@@ -44,6 +50,7 @@ catkin_package(
   visualization_msgs
 )
 
+
 ###########
 ## Build ##
 ###########
@@ -55,16 +62,19 @@ include_directories(
 
 
 
-add_executable(mrpt_rbpf_slam
+add_executable(${PROJECT_NAME}
     src/mrpt_rbpf_slam.cpp
     src/mrpt_rbpf_slam_wrapper.cpp
     src/mrpt_rbpf_slam_node.cpp
     src/options.cpp
 )
 
-TARGET_LINK_LIBRARIES(mrpt_rbpf_slam ${MRPT_LIBRARIES}
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${MRPT_LIBRARIES}
     ${catkin_LIBRARIES}
 )
+add_dependencies(${PROJECT_NAME}
+   ${PROJECT_NAME}_gencfg
+)
 
 #############
 ## Install ##

mrpt_rbpf_slam/cfg/General.cfg

@@ -0,0 +1,17 @@
+#! /usr/bin/env python
+
+PACKAGE='mrpt_rbpf_slam'
+from dynamic_reconfigure.parameter_generator_catkin import *
+gen = ParameterGenerator()
+
+gen.add("folder_simplemap",    str_t,    0, "folder where to save *.simplemap",  "/tmp")
+gen.add("update_while_stopped", bool_t, 0, "keep updating filter while the robot is stopped", True)
+gen.add("update_sensor_pose", bool_t, 0, "keep updating the sensor pose using the tf tree", False)
+
+gen.add("visualization_on", bool_t, 0, "On true it opens the mrpt viewer", True)
+gen.add("visualization_update_delay", int_t, 0, "window update delay", 1, 0, 10)
+gen.add("visualization_width", int_t, 0, "window width", 600, 320, 1920)
+gen.add("visualization_height", int_t, 0, "height", 500, 280, 1080)
+gen.add("visualization_follow_robot", bool_t, 0, "Camera follows the robot", True)
+
+exit(gen.generate(PACKAGE, "mrpt_rbpf_slam", "General"))

mrpt_rbpf_slam/cfg/Motion.cfg

@@ -0,0 +1,24 @@
+#! /usr/bin/env python
+
+PACKAGE='mrpt_rbpf_slam'
+from dynamic_reconfigure.parameter_generator_catkin import *
+gen = ParameterGenerator()
+
+enum_motion_noise_type = gen.enum([gen.const("gaussian", int_t, 0, "gaussian"), gen.const("Thrun", int_t, 1, "Thrun")], "motion type")
+gen.add("motion_noise_type", int_t, 0, "motion noise", 0, 0, 1, edit_method=enum_motion_noise_type)
+gen.add("gaussian_alpha_1", double_t, 0, "alpha_1", 0.034, 0, 1)
+gen.add("gaussian_alpha_2", double_t, 0, "alpha_2", 0.057, 0, 1)
+gen.add("gaussian_alpha_3", double_t, 0, "alpha_3", 0.014, 0, 1)
+gen.add("gaussian_alpha_4", double_t, 0, "alpha_4", 0.097, 0, 1)
+gen.add("gaussian_alpha_xy", double_t, 0, "alpha_xy", 0.005, 0, 1)
+gen.add("gaussian_alpha_phi", double_t, 0, "alpha_phi", 0.05, 0, 3.14)
+
+gen.add("thrun_particle_count", int_t, 0, "alpha_1", 100, 0, 1000)
+gen.add("thrun_alpha_1_rot_rot", double_t, 0, "rot_rot", 0.1, 0, 3.14)
+gen.add("thrun_alpha_2_rot_trans", double_t, 0, "rot_trans", 0.1, 0, 3.14)
+gen.add("thrun_alpha_3_trans_trans", double_t, 0, "trans_trans", 0.1, 0, 1)
+gen.add("thrun_alpha_4_trans_rot", double_t, 0, "trans_rot", 0.1, 0, 1)
+gen.add("thrun_additional_std_XY", double_t, 0, "std_XY", 0.1, 0, 3.14)
+gen.add("thrun_additional_std_phi", double_t, 0, "std_phi", 0.1, 0, 3.14)
+
+exit(gen.generate(PACKAGE, "mrpt_rbpf_slam", "Motion"))

mrpt_rbpf_slam/include/mrpt_rbpf_slam/mrpt_rbpf_slam.h

@@ -47,6 +47,8 @@ namespace mrpt_rbpf_slam
 class PFslam
 {
 public:
+    static const int MOTION_TYPE_GAUSSIAN = 0;   /// must correspond with the dynamic reconfigure values for the motion type
+    static const int MOTION_TYPE_TRUN     = 1;   /// must correspond with the dynamic reconfigure values for the motion type
   struct Options
   {
     mrpt::obs::CActionRobotMovement2D::TMotionModelOptions motion_model_options_;  ///< used with odom value motion
@@ -76,7 +78,7 @@ class PFslam
   /**
    * @brief initialize the SLAM
    */
-  void initSlam(Options options);
+  void initSlam();
 
   /**
    * @brief Read pairs of actions and observations from rawlog file

mrpt_rbpf_slam/include/mrpt_rbpf_slam/mrpt_rbpf_slam_wrapper.h

@@ -16,6 +16,12 @@
 #include <ros/package.h>
 #include <tf/transform_listener.h>
 #include <tf/transform_broadcaster.h>
+
+// add ros dynamic_reconfigure
+#include <mrpt_rbpf_slam/GeneralConfig.h>
+#include <mrpt_rbpf_slam/MotionConfig.h>
+#include <dynamic_reconfigure/server.h>
+
 // add ros msgs
 #include <nav_msgs/OccupancyGrid.h>
 #include "nav_msgs/MapMetaData.h"
@@ -153,7 +159,6 @@ class PFslamWrapper : public PFslam
 
   // Sensor source
   std::string sensor_source_;    ///< 2D laser scans
-  bool update_sensor_pose_;      ///< on true the sensor pose is updated on every sensor reading
 
   std::map<std::string, mrpt::poses::CPose3D> laser_poses_;   ///< laser scan poses with respect to the map
   std::map<std::string, mrpt::poses::CPose3D> beacon_poses_;  ///< beacon poses with respect to the map
@@ -179,5 +184,15 @@ class PFslamWrapper : public PFslam
   mrpt::utils::CTicTac tictac_;
 #endif
   float t_exec_;  ///< the time which take one SLAM update execution
+
+
+    mrpt_rbpf_slam::MotionConfig config_motion_;
+    mrpt_rbpf_slam::GeneralConfig config_general_;
+    dynamic_reconfigure::Server<mrpt_rbpf_slam::MotionConfig> reconfigureServerSlam_; /// parameter server stuff
+    dynamic_reconfigure::Server<mrpt_rbpf_slam::MotionConfig>::CallbackType reconfigureFncSlam_;  /// parameter server stuff
+    void callbackConfigSlam ( mrpt_rbpf_slam::MotionConfig &config, uint32_t level ); /// callback function on incoming parameter changes
+    dynamic_reconfigure::Server<mrpt_rbpf_slam::GeneralConfig> reconfigureServerGeneral_; /// parameter server stuff
+    dynamic_reconfigure::Server<mrpt_rbpf_slam::GeneralConfig>::CallbackType reconfigureFncGeneral_;  /// parameter server stuff
+    void callbackConfigGeneral ( mrpt_rbpf_slam::GeneralConfig &config, uint32_t level ); /// callback function on incoming parameter changes
 };
 }  // namespace mrpt_rbpf_slam

mrpt_rbpf_slam/src/mrpt_rbpf_slam.cpp

@@ -101,11 +101,17 @@ void PFslam::observation(
   }
 }
 
-void PFslam::initSlam(PFslam::Options options) {
+void PFslam::initSlam() {
   log4cxx::LoggerPtr ros_logger =
       log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
-  mapBuilder_.setVerbosityLevel(
-      mrpt_bridge::rosLoggerLvlToMRPTLoggerLvl(ros_logger->getLevel()));
+  VerbosityLevel level;
+  try{
+      level = mrpt_bridge::rosLoggerLvlToMRPTLoggerLvl(ros_logger->getLevel());
+  } catch (...){
+      ROS_ERROR("Could not find log Level, doing to debug");
+      level = VerbosityLevel::LVL_DEBUG;
+  }
+  mapBuilder_.setVerbosityLevel(level);
   mapBuilder_.logging_enable_console_output = false;
 #if MRPT_VERSION < 0x199
   mapBuilder_.logRegisterCallback(static_cast<output_logger_callback_t>(
@@ -122,29 +128,6 @@ void PFslam::initSlam(PFslam::Options options) {
 #else
   mrpt::random::randomGenerator.randomize();
 #endif
-
-  options_ = std::move(options);
-
-  //  use_motion_model_default_options_ = false;
-  //  //  motion_model_default_options_.modelSelection =
-  //  mrpt::obs::CActionRobotMovement2D::mmGaussian;
-  //  //  motion_model_default_options_.gaussianModel.minStdXY = 0.10;
-  //  //  motion_model_default_options_.gaussianModel.minStdPHI = 2.0;
-
-  //  motion_model_options_.modelSelection =
-  //  mrpt::obs::CActionRobotMovement2D::mmGaussian;
-  //  motion_model_options_.gaussianModel.a1 = 0.034f;
-  //  motion_model_options_.gaussianModel.a2 = 0.057f;
-  //  motion_model_options_.gaussianModel.a3 = 0.014f;
-  //  motion_model_options_.gaussianModel.a4 = 0.097f;
-  //  motion_model_options_.gaussianModel.minStdXY = 0.005f;
-  //  motion_model_options_.gaussianModel.minStdPHI = 0.05f;
-
-  //  PROGRESS_WINDOW_WIDTH_ = 600;
-  //  PROGRESS_WINDOW_HEIGHT_ = 500;
-  //  SHOW_PROGRESS_IN_WINDOW_ = false;
-  //  SHOW_PROGRESS_IN_WINDOW_DELAY_MS_ = 0;
-  //  CAMERA_3DSCENE_FOLLOWS_ROBOT_ = false;
 }
 
 void PFslam::init3Dwindow() {

mrpt_rbpf_slam/src/mrpt_rbpf_slam_wrapper.cpp

@@ -39,20 +39,14 @@ bool PFslamWrapper::getParams(const ros::NodeHandle &nh_p) {
 
   nh_p.param<std::string>("sensor_source", sensor_source_, "scan");
   ROS_INFO("sensor_source: %s", sensor_source_.c_str());
-
-  nh_p.param<std::string>("sensor_source", sensor_source_, "scan");
-  ROS_INFO("sensor_source: %s", sensor_source_.c_str());
-
-  nh_p.param<bool>("update_sensor_pose", update_sensor_pose_, true);
-  ROS_INFO("update_sensor_pose: %s", (update_sensor_pose_?"TRUE":"FALSE"));
 
+  /// dynamic reconfigure callback bindings
+  reconfigureFncSlam_ = boost::bind ( &PFslamWrapper::callbackConfigSlam, this,  _1, _2 );
+  reconfigureServerSlam_.setCallback ( reconfigureFncSlam_ );
+  reconfigureFncGeneral_ = boost::bind ( &PFslamWrapper::callbackConfigGeneral, this,  _1, _2 );
+  reconfigureServerGeneral_.setCallback ( reconfigureFncGeneral_ );
 
-  PFslam::Options options;
-  if (!loadOptions(nh_p, options)) {
-    ROS_ERROR("Not able to read all parameters!");
-    return false;
-  }
-  initSlam(std::move(options));
+  initSlam();
   return true;
 }
 
@@ -85,6 +79,7 @@ bool PFslamWrapper::init(ros::NodeHandle &nh) {
   beacon_viz_pub_ =
       nh.advertise<visualization_msgs::MarkerArray>("/beacons_viz", 1);
 
+
   // read sensor topics
   std::vector<std::string> lstSources;
   mrpt::system::tokenize(sensor_source_, " ,\t\n", lstSources);
@@ -110,6 +105,44 @@ bool PFslamWrapper::init(ros::NodeHandle &nh) {
   return true;
 }
 
+void PFslamWrapper::callbackConfigGeneral ( mrpt_rbpf_slam::GeneralConfig &config, uint32_t level ) {
+    ROS_INFO ("callbackConfigGeneral");
+    config_general_ = config;
+    options_.SHOW_PROGRESS_IN_WINDOW_ = config_general_.visualization_on;
+    options_.PROGRESS_WINDOW_WIDTH_ = config_general_.visualization_width;
+    options_.PROGRESS_WINDOW_HEIGHT_ = config_general_.visualization_height;
+    options_.SHOW_PROGRESS_IN_WINDOW_DELAY_MS_ = config_general_.visualization_update_delay;
+    options_.CAMERA_3DSCENE_FOLLOWS_ROBOT_ = config_general_.visualization_follow_robot;
+
+
+    options_.simplemap_path_prefix = config_general_.folder_simplemap;
+}
+
+void PFslamWrapper::callbackConfigSlam ( mrpt_rbpf_slam::MotionConfig &config, uint32_t level ) {
+    ROS_INFO ("callbackConfigSlam");
+    config_motion_ = config;
+    if(config_motion_.motion_noise_type == MOTION_TYPE_TRUN){
+        options_.motion_model_options_.modelSelection =  mrpt::obs::CActionRobotMovement2D::mmThrun;
+    } else if(config_motion_.motion_noise_type == MOTION_TYPE_GAUSSIAN){
+        options_.motion_model_options_.modelSelection = mrpt::obs::CActionRobotMovement2D::mmGaussian;
+    }
+
+    /// update trun parameters
+    options_.motion_model_options_.thrunModel.alfa1_rot_rot = config_motion_.thrun_alpha_1_rot_rot;
+    options_.motion_model_options_.thrunModel.alfa1_rot_rot = config_motion_.thrun_alpha_2_rot_trans;
+    options_.motion_model_options_.thrunModel.alfa3_trans_trans = config_motion_.thrun_alpha_3_trans_trans;
+    options_.motion_model_options_.thrunModel.alfa4_trans_rot = config_motion_.thrun_alpha_4_trans_rot;
+    options_.motion_model_options_.thrunModel.additional_std_XY = config_motion_.thrun_additional_std_XY;
+    options_.motion_model_options_.thrunModel.additional_std_phi = config_motion_.thrun_additional_std_phi;
+
+    options_.motion_model_options_.gaussianModel.a1 = config_motion_.gaussian_alpha_1;
+    options_.motion_model_options_.gaussianModel.a2 = config_motion_.gaussian_alpha_2;
+    options_.motion_model_options_.gaussianModel.a3 = config_motion_.gaussian_alpha_3;
+    options_.motion_model_options_.gaussianModel.a4 = config_motion_.gaussian_alpha_4;
+    options_.motion_model_options_.gaussianModel.minStdXY = config_motion_.gaussian_alpha_xy;
+    options_.motion_model_options_.gaussianModel.minStdPHI = config_motion_.gaussian_alpha_phi;
+}
+
 void PFslamWrapper::odometryForCallback(
     mrpt::obs::CObservationOdometry::Ptr &odometry,
     const std_msgs::Header &_msg_header) {
@@ -146,6 +179,7 @@ bool PFslamWrapper::waitForTransform(
 }
 
 void PFslamWrapper::laserCallback(const sensor_msgs::LaserScan &msg) {
+  ROS_INFO ("laserCallback");
   using namespace mrpt::maps;
   using namespace mrpt::obs;
   CObservation2DRangeScan::Ptr laser = CObservation2DRangeScan::Create();
@@ -156,7 +190,7 @@ void PFslamWrapper::laserCallback(const sensor_msgs::LaserScan &msg) {
     updateSensorPose(msg.header.frame_id);
   } else {
     // update sensor pose 
-    if(update_sensor_pose_) updateSensorPose(msg.header.frame_id); 
+    if(config_general_.update_sensor_pose) updateSensorPose(msg.header.frame_id); 
     mrpt::poses::CPose3D pose = laser_poses_[msg.header.frame_id];
     mrpt_bridge::convert(msg, laser_poses_[msg.header.frame_id], *laser);
 
@@ -181,6 +215,7 @@ void PFslamWrapper::laserCallback(const sensor_msgs::LaserScan &msg) {
 
 void PFslamWrapper::callbackBeacon(
     const mrpt_msgs::ObservationRangeBeacon &msg) {
+  ROS_INFO ("callbackBeacon");
   using namespace mrpt::maps;
   using namespace mrpt::obs;
 
@@ -191,7 +226,7 @@ void PFslamWrapper::callbackBeacon(
     updateSensorPose(msg.header.frame_id);
   } else {
     // update sensor pose 
-    if(update_sensor_pose_) updateSensorPose(msg.header.frame_id); 
+    if(config_general_.update_sensor_pose) updateSensorPose(msg.header.frame_id); 
 
     mrpt_bridge::convert(msg, beacon_poses_[msg.header.frame_id], *beacon);