Merge pull request #24 from matijazigic/mzigic/reconfigure_fix

Prevent reconfigure from overwriting parameters at the initialization

mpc_local_planner/CMakeLists.txt

@@ -135,7 +135,9 @@ set(EXTERNAL_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
 #add dynamic reconfigure api
 #find_package(catkin REQUIRED dynamic_reconfigure)
 generate_dynamic_reconfigure_options(
-  cfg/mpc_reconfigure.cfg
+  cfg/mpc_collision.cfg
+  cfg/mpc_controller.cfg
+  cfg/mpc_footprint.cfg
 )
 
 ###################################

mpc_local_planner/cfg/mpc_collision.cfg

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# This unusual line allows to reuse existing parameter definitions
+# that concern all localplanners
+#add_generic_localplanner_params(gen)
+
+# For integers and doubles:
+#       Name                    Type      Reconfiguration level
+#       Description
+#       Default  Min  Max
+
+# Collision avoidance
+
+grp_collision = gen.add_group("Collision avoidance", type="tab")
+
+grp_collision.add("include_costmap_obstacles", bool_t, 0, "Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)", True)
+
+grp_collision.add("costmap_obstacles_behind_robot_dist", double_t, 0, "Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)", 1.5, 0, 10)
+
+grp_collision.add("collision_check_min_resolution_angular", double_t, 0, "", 3.1415, -3.1415, 3.1415)
+grp_collision.add("collision_check_no_poses", int_t, 0, "", -1, -1, 100)
+
+
+exit(gen.generate("mpc_local_planner", "mpc_local_planner", "CollisionReconfigure"))

mpc_local_planner/cfg/mpc_controller.cfg

@@ -29,22 +29,5 @@ grp_controller.add("max_global_plan_lookahead_dist", double_t, 0, "Specify maxim
 
 grp_controller.add("global_plan_viapoint_sep", double_t, 0, "Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]", -1, -1, 10)
 
-grp_footprint = gen.add_group("Footprint", type="tab")
 
-# Footprint model
+exit(gen.generate("mpc_local_planner", "mpc_local_planner", "ControllerReconfigure"))
-
-grp_footprint.add("is_footprint_dynamic", bool_t, 0, "If true, updated the footprint before checking trajectory feasibility", False)
-
-grp_collision = gen.add_group("Collision avoidance", type="tab")
-
-# Collision avoidance
-
-grp_collision.add("include_costmap_obstacles", bool_t, 0, "Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)", True)
-
-grp_collision.add("costmap_obstacles_behind_robot_dist", double_t, 0, "Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)", 1.5, 0, 10)
-
-grp_collision.add("collision_check_min_resolution_angular", double_t, 0, "", 3.1415, -3.1415, 3.1415)
-grp_collision.add("collision_check_no_poses", int_t, 0, "", -1, -1, 100)
-
-
-exit(gen.generate("mpc_local_planner", "mpc_local_planner", "MpcLocalPlannerReconfigure"))

mpc_local_planner/cfg/mpc_footprint.cfg

@@ -0,0 +1,22 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# This unusual line allows to reuse existing parameter definitions
+# that concern all localplanners
+#add_generic_localplanner_params(gen)
+
+# For integers and doubles:
+#       Name                    Type      Reconfiguration level
+#       Description
+#       Default  Min  Max
+
+# Footprint model
+
+grp_footprint = gen.add_group("Footprint", type="tab")
+
+grp_footprint.add("is_footprint_dynamic", bool_t, 0, "If true, updated the footprint before checking trajectory feasibility", False)
+
+exit(gen.generate("mpc_local_planner", "mpc_local_planner", "FootprintReconfigure"))

mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h

@@ -59,7 +59,9 @@
 
 // dynamic reconfigure
 #include <dynamic_reconfigure/server.h>
-#include <mpc_local_planner/MpcLocalPlannerReconfigureConfig.h>
+#include <mpc_local_planner/ControllerReconfigureConfig.h>
+#include <mpc_local_planner/CollisionReconfigureConfig.h>
+#include <mpc_local_planner/FootprintReconfigureConfig.h>
 
 #include <boost/shared_ptr.hpp>
 
@@ -263,13 +265,31 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
     void updateViaPointsContainer(const std::vector<geometry_msgs::PoseStamped>& transformed_plan, double min_separation);
 
     /**
-     * @brief Callback for the dynamic_reconfigure node.
+     * @brief Callback for the dynamic_reconfigure controller node.
      *
      * This callback allows to modify parameters dynamically at runtime without restarting the node
      * @param config Reference to the dynamic reconfigure config
      * @param level Dynamic reconfigure level
      */
-    void reconfigureCB(MpcLocalPlannerReconfigureConfig& config, uint32_t level);
+    void reconfigureControllerCB(ControllerReconfigureConfig& config, uint32_t level);
+
+    /**
+     * @brief Callback for the dynamic_reconfigure collision node.
+     *
+     * This callback allows to modify parameters dynamically at runtime without restarting the node
+     * @param config Reference to the dynamic reconfigure config
+     * @param level Dynamic reconfigure level
+     */
+    void reconfigureCollisionCB(CollisionReconfigureConfig& config, uint32_t level);
+
+    /**
+     * @brief Callback for the dynamic_reconfigure footprint collision node.
+     *
+     * This callback allows to modify parameters dynamically at runtime without restarting the node
+     * @param config Reference to the dynamic reconfigure config
+     * @param level Dynamic reconfigure level
+     */
+    void reconfigureFootprintCB(FootprintReconfigureConfig& config, uint32_t level);
 
     /**
      * @brief Callback for custom obstacles that are not obtained from the costmap
@@ -378,8 +398,12 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
     pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons> _costmap_converter_loader;  //!< Load costmap converter plugins at runtime
     boost::shared_ptr<costmap_converter::BaseCostmapToPolygons> _costmap_converter;              //!< Store the current costmap_converter
 
-    boost::shared_ptr<dynamic_reconfigure::Server<MpcLocalPlannerReconfigureConfig>>
+    boost::shared_ptr<dynamic_reconfigure::Server<ControllerReconfigureConfig>>
-        dynamic_recfg_;                                        //!< Dynamic reconfigure server to allow config modifications at runtime
+        dynamic_controller_recfg_;                             //!< Dynamic reconfigure server to allow config modifications at runtime
+    boost::shared_ptr<dynamic_reconfigure::Server<CollisionReconfigureConfig>>
+        dynamic_collision_recfg_;                              //!< Dynamic reconfigure server to allow config modifications at runtime
+    boost::shared_ptr<dynamic_reconfigure::Server<FootprintReconfigureConfig>>
+        dynamic_footprint_recfg_;                              //!< Dynamic reconfigure server to allow config modifications at runtime
     ros::Subscriber _custom_obst_sub;                          //!< Subscriber for custom obstacles received via a ObstacleMsg.
     std::mutex _custom_obst_mutex;                             //!< Mutex that locks the obstacle array (multi-threaded)
     costmap_converter::ObstacleArrayMsg _custom_obstacle_msg;  //!< Copy of the most recent obstacle message

mpc_local_planner/src/mpc_local_planner_ros.cpp

@@ -46,7 +46,9 @@ MpcLocalPlannerROS::MpcLocalPlannerROS()
       _tf(nullptr),
       _costmap_model(nullptr),
       _costmap_converter_loader("costmap_converter", "costmap_converter::BaseCostmapToPolygons"),
-      dynamic_recfg_(NULL),
+      dynamic_controller_recfg_(NULL),
+      dynamic_collision_recfg_(NULL),
+      dynamic_footprint_recfg_(NULL),
       _goal_reached(false),
       _no_infeasible_plans(0),
       /*last_preferred_rotdir_(RotType::none),*/
@@ -56,7 +58,7 @@ MpcLocalPlannerROS::MpcLocalPlannerROS()
 
 MpcLocalPlannerROS::~MpcLocalPlannerROS() {}
 
-void MpcLocalPlannerROS::reconfigureCB(MpcLocalPlannerReconfigureConfig& config, uint32_t level)
+void MpcLocalPlannerROS::reconfigureControllerCB(ControllerReconfigureConfig& config, uint32_t level)
 {
     boost::mutex::scoped_lock l(config_mutex_);
 
@@ -65,10 +67,22 @@ void MpcLocalPlannerROS::reconfigureCB(MpcLocalPlannerReconfigureConfig& config,
     _params.global_plan_overwrite_orientation      = config.global_plan_overwrite_orientation;
     _params.global_plan_prune_distance             = config.global_plan_prune_distance;
     _params.max_global_plan_lookahead_dist         = config.max_global_plan_lookahead_dist;
+    _params.global_plan_viapoint_sep               = config.global_plan_viapoint_sep;
+}
+
+void MpcLocalPlannerROS::reconfigureFootprintCB(FootprintReconfigureConfig& config, uint32_t level)
+{
+    boost::mutex::scoped_lock l(config_mutex_);
+
     _params.is_footprint_dynamic                   = config.is_footprint_dynamic;
+}
+
+void MpcLocalPlannerROS::reconfigureCollisionCB(CollisionReconfigureConfig& config, uint32_t level)
+{
+    boost::mutex::scoped_lock l(config_mutex_);
+
     _params.include_costmap_obstacles              = config.include_costmap_obstacles;
     _params.costmap_obstacles_behind_robot_dist    = config.costmap_obstacles_behind_robot_dist;
-    _params.global_plan_viapoint_sep               = config.global_plan_viapoint_sep;
     _params.collision_check_min_resolution_angular = config.collision_check_min_resolution_angular;
     _params.collision_check_no_poses               = config.collision_check_no_poses;
 }
@@ -93,7 +107,9 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm
 
         // special parameters
         nh.param("odom_topic", _params.odom_topic, _params.odom_topic);
+
         nh.param("footprint_model/is_footprint_dynamic", _params.is_footprint_dynamic, _params.is_footprint_dynamic);
+
         nh.param("collision_avoidance/include_costmap_obstacles", _params.include_costmap_obstacles, _params.include_costmap_obstacles);
         nh.param("collision_avoidance/costmap_obstacles_behind_robot_dist", _params.costmap_obstacles_behind_robot_dist,
                  _params.costmap_obstacles_behind_robot_dist);
@@ -171,10 +187,23 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm
         _odom_helper.setOdomTopic(_params.odom_topic);
 
         // setup dynamic reconfigure
-        dynamic_recfg_ = boost::make_shared<dynamic_reconfigure::Server<MpcLocalPlannerReconfigureConfig>>(nh);
+        ros::NodeHandle controller_nh(nh, "controller");
-        dynamic_reconfigure::Server<MpcLocalPlannerReconfigureConfig>::CallbackType cb =
+        dynamic_controller_recfg_ = boost::make_shared<dynamic_reconfigure::Server<ControllerReconfigureConfig>>(controller_nh);
-            boost::bind(&MpcLocalPlannerROS::reconfigureCB, this, _1, _2);
+        dynamic_reconfigure::Server<ControllerReconfigureConfig>::CallbackType controller_cb =
-        dynamic_recfg_->setCallback(cb);
+            boost::bind(&MpcLocalPlannerROS::reconfigureControllerCB, this, _1, _2);
+        dynamic_controller_recfg_->setCallback(controller_cb);
+
+        ros::NodeHandle collision_nh(nh, "collision");
+        dynamic_collision_recfg_ = boost::make_shared<dynamic_reconfigure::Server<CollisionReconfigureConfig>>(collision_nh);
+        dynamic_reconfigure::Server<CollisionReconfigureConfig>::CallbackType collision_cb =
+            boost::bind(&MpcLocalPlannerROS::reconfigureCollisionCB, this, _1, _2);
+        dynamic_collision_recfg_->setCallback(collision_cb);
+
+        ros::NodeHandle footprint_nh(nh, "footprint_model");
+        dynamic_footprint_recfg_ = boost::make_shared<dynamic_reconfigure::Server<FootprintReconfigureConfig>>(footprint_nh);
+        dynamic_reconfigure::Server<FootprintReconfigureConfig>::CallbackType footprint_cb =
+            boost::bind(&MpcLocalPlannerROS::reconfigureFootprintCB, this, _1, _2);
+        dynamic_footprint_recfg_->setCallback(footprint_cb);
 
         // validate optimization footprint and costmap footprint
         validateFootprints(_robot_model->getInscribedRadius(), _robot_inscribed_radius, _controller.getInequalityConstraint()->getMinimumDistance());