Add dynamic reconfigure

mpc_local_planner/CMakeLists.txt

@@ -132,11 +132,11 @@ set(EXTERNAL_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
 ##   * uncomment the "generate_dynamic_reconfigure_options" section below
 ##     and list every .cfg file to be processed
 
-## Generate dynamic reconfigure parameters in the 'cfg' folder
+#add dynamic reconfigure api
-# generate_dynamic_reconfigure_options(
+#find_package(catkin REQUIRED dynamic_reconfigure)
-#   cfg/DynReconf1.cfg
+generate_dynamic_reconfigure_options(
-#   cfg/DynReconf2.cfg
+  cfg/mpc_reconfigure.cfg
-# )
+)
 
 ###################################
 ## catkin specific configuration ##
@@ -150,7 +150,7 @@ set(EXTERNAL_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
 catkin_package(
   INCLUDE_DIRS include ${EXTERNAL_INCLUDE_DIRS}
   LIBRARIES mpc_local_planner_numerics mpc_local_planner_optimization
-  CATKIN_DEPENDS roscpp mpc_local_planner_msgs control_box_rst teb_local_planner
+  CATKIN_DEPENDS roscpp mpc_local_planner_msgs control_box_rst teb_local_planner dynamic_reconfigure
   # DEPENDS
 )
 
@@ -188,6 +188,11 @@ add_library(mpc_local_planner
    src/mpc_local_planner_ros.cpp
 )
 
+# Dynamic reconfigure: make sure configure headers are built before any node using them
+add_dependencies(mpc_local_planner ${PROJECT_NAME}_gencfg)
+# Generate messages before compiling the lib
+add_dependencies(mpc_local_planner ${PROJECT_NAME}_generate_messages_cpp)
+
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide

mpc_local_planner/cfg/mpc_reconfigure.cfg

@@ -0,0 +1,50 @@
+#!/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
+
+grp_controller = gen.add_group("Controller", type="tab")
+
+# Controller
+
+grp_controller.add("xy_goal_tolerance", double_t, 0, "Allowed final euclidean distance to the goal position", 0.2, 0, 1)
+
+grp_controller.add("yaw_goal_tolerance", double_t, 0, "Allowed final orientation error to the goal orientation", 0.1, 0, 1)
+
+grp_controller.add("global_plan_overwrite_orientation", bool_t, 0, "Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically", True)
+
+grp_controller.add("global_plan_prune_distance", double_t, 0, "", 1.0, 0, 10)
+
+grp_controller.add("max_global_plan_lookahead_dist", double_t, 0, "Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]", 1.5, 0, 10)
+
+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
+
+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/include/mpc_local_planner/mpc_local_planner_ros.h

@@ -58,8 +58,8 @@
 #include <costmap_converter/costmap_converter_interface.h>
 
 // dynamic reconfigure
-// #include <dynamic_reconfigure/server.h>
+#include <dynamic_reconfigure/server.h>
-// #include <mpc_local_planner/MpcLocalPlannerReconfigureConfig.h>
+#include <mpc_local_planner/MpcLocalPlannerReconfigureConfig.h>
 
 #include <boost/shared_ptr.hpp>
 
@@ -221,6 +221,11 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
      */
     static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name);
 
+    /**
+     * @brief Return the internal config mutex
+     */
+    boost::mutex& configMutex() { return config_mutex_; }
+
     //@}
 
  protected:
@@ -264,7 +269,7 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
      * @param config Reference to the dynamic reconfigure config
      * @param level Dynamic reconfigure level
      */
-    // void reconfigureCB(MpcLocalPlannerReconfigureConfig& config, uint32_t level);
+    void reconfigureCB(MpcLocalPlannerReconfigureConfig& config, uint32_t level);
 
     /**
      * @brief Callback for custom obstacles that are not obtained from the costmap
@@ -373,8 +378,8 @@ 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
 
-    // std::shared_ptr<dynamic_reconfigure::Server<MpcLocalPlannerReconfigureConfig>>
+    boost::shared_ptr<dynamic_reconfigure::Server<MpcLocalPlannerReconfigureConfig>>
-    //    dynamic_recfg_;                                        //!< Dynamic reconfigure server to allow config modifications at runtime
+        dynamic_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
@@ -423,6 +428,8 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
 
     } _params;
 
+    boost::mutex config_mutex_;  //!< Mutex for config accesses and changes
+
     struct CostmapConverterPlugin
     {
         std::string costmap_converter_plugin;

mpc_local_planner/src/mpc_local_planner_ros.cpp

@@ -46,7 +46,7 @@ MpcLocalPlannerROS::MpcLocalPlannerROS()
       _tf(nullptr),
       _costmap_model(nullptr),
       _costmap_converter_loader("costmap_converter", "costmap_converter::BaseCostmapToPolygons"),
-      /*dynamic_recfg_(NULL),*/
+      dynamic_recfg_(NULL),
       _goal_reached(false),
       _no_infeasible_plans(0),
       /*last_preferred_rotdir_(RotType::none),*/
@@ -56,12 +56,22 @@ MpcLocalPlannerROS::MpcLocalPlannerROS()
 
 MpcLocalPlannerROS::~MpcLocalPlannerROS() {}
 
-/*
+void MpcLocalPlannerROS::reconfigureCB(MpcLocalPlannerReconfigureConfig& config, uint32_t level)
-void MpcLocalPlannerROS::reconfigureCB(TebLocalPlannerReconfigureConfig& config, uint32_t level)
 {
-  cfg_.reconfigure(config);
+    boost::mutex::scoped_lock l(config_mutex_);
+
+    _params.xy_goal_tolerance                      = config.xy_goal_tolerance;
+    _params.yaw_goal_tolerance                     = config.yaw_goal_tolerance;
+    _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.is_footprint_dynamic                   = config.is_footprint_dynamic;
+    _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;
 }
-*/
 
 void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros)
 {
@@ -161,12 +171,10 @@ 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);
         dynamic_reconfigure::Server<MpcLocalPlannerReconfigureConfig>::CallbackType cb =
-            boost::bind(&MpcLocalPlanner::reconfigureCB, this, _1, _2);
+            boost::bind(&MpcLocalPlannerROS::reconfigureCB, this, _1, _2);
         dynamic_recfg_->setCallback(cb);
-        */
 
         // validate optimization footprint and costmap footprint
         validateFootprints(_robot_model->getInscribedRadius(), _robot_inscribed_radius, _controller.getInequalityConstraint()->getMinimumDistance());
@@ -332,8 +340,7 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt
     // updateControlFromTwist()
 
     // Do not allow config changes during the following optimization step
-    // TODO(roesmann): we need something similar in case we allow dynamic reconfiguration:
+    boost::mutex::scoped_lock cfg_lock(configMutex());
-    // boost::mutex::scoped_lock cfg_lock(cfg_.configMutex());
 
     // Now perform the actual planning
     // bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);