mpc_local_planner/CMakeLists.txt

@@ -132,13 +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
 
-#add dynamic reconfigure api
+## Generate dynamic reconfigure parameters in the 'cfg' folder
-#find_package(catkin REQUIRED dynamic_reconfigure)
+# generate_dynamic_reconfigure_options(
-generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
-  cfg/mpc_collision.cfg
+#   cfg/DynReconf2.cfg
-  cfg/mpc_controller.cfg
+# )
-  cfg/mpc_footprint.cfg
-)
 
 ###################################
 ## catkin specific configuration ##
@@ -151,8 +149,8 @@ generate_dynamic_reconfigure_options(
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
   INCLUDE_DIRS include ${EXTERNAL_INCLUDE_DIRS}
-  LIBRARIES mpc_local_planner_utils mpc_local_planner_optimal_control mpc_local_planner
+  LIBRARIES mpc_local_planner_numerics mpc_local_planner_optimization
-  CATKIN_DEPENDS roscpp mpc_local_planner_msgs control_box_rst teb_local_planner dynamic_reconfigure
+  CATKIN_DEPENDS roscpp mpc_local_planner_msgs control_box_rst teb_local_planner
   # DEPENDS
 )
 
@@ -190,9 +188,6 @@ 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)
-
 ## 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_collision.cfg

@@ -1,28 +0,0 @@
-#!/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

@@ -1,33 +0,0 @@
-#!/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)
-
-
-exit(gen.generate("mpc_local_planner", "mpc_local_planner", "ControllerReconfigure"))

mpc_local_planner/cfg/mpc_footprint.cfg

@@ -1,22 +0,0 @@
-#!/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

@@ -58,10 +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/ControllerReconfigureConfig.h>
+// #include <mpc_local_planner/MpcLocalPlannerReconfigureConfig.h>
-#include <mpc_local_planner/CollisionReconfigureConfig.h>
-#include <mpc_local_planner/FootprintReconfigureConfig.h>
 
 #include <boost/shared_ptr.hpp>
 
@@ -223,11 +221,6 @@ 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:
@@ -265,31 +258,13 @@ 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 controller node.
+     * @brief Callback for the dynamic_reconfigure 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 reconfigureControllerCB(ControllerReconfigureConfig& config, uint32_t level);
+    // void reconfigureCB(MpcLocalPlannerReconfigureConfig& 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
@@ -398,12 +373,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
 
-    boost::shared_ptr<dynamic_reconfigure::Server<ControllerReconfigureConfig>>
+    // std::shared_ptr<dynamic_reconfigure::Server<MpcLocalPlannerReconfigureConfig>>
-        dynamic_controller_recfg_;                             //!< Dynamic reconfigure server to allow config modifications at runtime
+    //    dynamic_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
@@ -452,8 +423,6 @@ 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,9 +46,7 @@ MpcLocalPlannerROS::MpcLocalPlannerROS()
       _tf(nullptr),
       _costmap_model(nullptr),
       _costmap_converter_loader("costmap_converter", "costmap_converter::BaseCostmapToPolygons"),
-      dynamic_controller_recfg_(NULL),
+      /*dynamic_recfg_(NULL),*/
-      dynamic_collision_recfg_(NULL),
-      dynamic_footprint_recfg_(NULL),
       _goal_reached(false),
       _no_infeasible_plans(0),
       /*last_preferred_rotdir_(RotType::none),*/
@@ -58,44 +56,22 @@ MpcLocalPlannerROS::MpcLocalPlannerROS()
 
 MpcLocalPlannerROS::~MpcLocalPlannerROS() {}
 
-void MpcLocalPlannerROS::reconfigureControllerCB(ControllerReconfigureConfig& config, uint32_t level)
+/*
+void MpcLocalPlannerROS::reconfigureCB(TebLocalPlannerReconfigureConfig& config, uint32_t level)
 {
-    boost::mutex::scoped_lock l(config_mutex_);
+  cfg_.reconfigure(config);
-
-    _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.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.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)
 {
-    // 检查插件是否已经初始化
+    // check if the plugin is already initialized
     if (!_initialized)
     {
-        // 创建一个Node Handle，使用插件的名称（如在move_base中加载）
+        // create Node Handle with name of plugin (as used in move_base for loading)
         ros::NodeHandle nh("~/" + name);
 
-        // 加载插件相关的主要参数
+        // load plugin related main parameters
         nh.param("controller/xy_goal_tolerance", _params.xy_goal_tolerance, _params.xy_goal_tolerance);
         nh.param("controller/yaw_goal_tolerance", _params.yaw_goal_tolerance, _params.yaw_goal_tolerance);
         nh.param("controller/global_plan_overwrite_orientation", _params.global_plan_overwrite_orientation,
@@ -105,11 +81,9 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm
         nh.param("controller/global_plan_viapoint_sep", _params.global_plan_viapoint_sep, _params.global_plan_viapoint_sep);
         _controller.setInitialPlanEstimateOrientation(_params.global_plan_overwrite_orientation);
 
-        // 特殊参数
+        // 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);
@@ -118,46 +92,46 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm
         nh.param("collision_avoidance/collision_check_min_resolution_angular", _params.collision_check_min_resolution_angular,
                  _params.collision_check_min_resolution_angular);
 
-        // 代价地图转换器插件相关参数
+        // costmap converter plugin related parameters
         nh.param("costmap_converter_plugin", _costmap_conv_params.costmap_converter_plugin, _costmap_conv_params.costmap_converter_plugin);
         nh.param("costmap_converter_rate", _costmap_conv_params.costmap_converter_rate, _costmap_conv_params.costmap_converter_rate);
         nh.param("costmap_converter_spin_thread", _costmap_conv_params.costmap_converter_spin_thread,
                  _costmap_conv_params.costmap_converter_spin_thread);
 
-        // 预留一些内存用于障碍物
+        // reserve some memory for obstacles
         _obstacles.reserve(700);
 
-        // 初始化其他变量
+        // init other variables
         _tf          = tf;
         _costmap_ros = costmap_ros;
-        _costmap     = _costmap_ros->getCostmap();   // 应在MoveBase中进行锁定
+        _costmap     = _costmap_ros->getCostmap();  // locking should be done in MoveBase.
 
         _costmap_model = std::make_shared<base_local_planner::CostmapModel>(*_costmap);
 
         _global_frame     = _costmap_ros->getGlobalFrameID();
         _robot_base_frame = _costmap_ros->getBaseFrameID();
 
-        // 创建用于优化的机器人足迹/轮廓模型
+        // create robot footprint/contour model for optimization
         _robot_model = getRobotFootprintFromParamServer(nh, _costmap_ros);
 
-        // 创建规划器实例
+        // create the planner instance
         if (!_controller.configure(nh, _obstacles, _robot_model, _via_points))
         {
             ROS_ERROR("Controller configuration failed.");
             return;
         }
 
-        // 创建规划器实例
+        // create visualization instance
         _publisher.initialize(nh, _controller.getRobotDynamics(), _global_frame);
 
-        // 初始化代价地图到多边形的转换器
+        // Initialize a costmap to polygon converter
         if (!_costmap_conv_params.costmap_converter_plugin.empty())
         {
             try
             {
                 _costmap_converter         = _costmap_converter_loader.createInstance(_costmap_conv_params.costmap_converter_plugin);
                 std::string converter_name = _costmap_converter_loader.getName(_costmap_conv_params.costmap_converter_plugin);
-                // 将'::'替换为'/'，以将C++命名空间转换为NodeHandle命名空间
+                // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace
                 boost::replace_all(converter_name, "::", "/");
                 _costmap_converter->setOdomTopic(_params.odom_topic);
                 _costmap_converter->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name));
@@ -179,46 +153,35 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm
         else
             ROS_INFO("No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles.");
 
-        // 获取机器人的足迹，以及从机器人中心到其足迹顶点的最小和最大距离
+        // Get footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices.
         _footprint_spec = _costmap_ros->getRobotFootprint();
         costmap_2d::calculateMinAndMaxDistances(_footprint_spec, _robot_inscribed_radius, _robot_circumscribed_radius);
 
-        // 初始化里程计助手以从里程计消息中接收机器人的速度
+        // init the odom helper to receive the robot's velocity from odom messages
         _odom_helper.setOdomTopic(_params.odom_topic);
 
-        // 设置动态重配置
+        // setup dynamic reconfigure
-        ros::NodeHandle controller_nh(nh, "controller");
+        /*
-        dynamic_controller_recfg_ = boost::make_shared<dynamic_reconfigure::Server<ControllerReconfigureConfig>>(controller_nh);
+        dynamic_recfg_ = boost::make_shared<dynamic_reconfigure::Server<MpcLocalPlannerReconfigureConfig>>(nh);
-        dynamic_reconfigure::Server<ControllerReconfigureConfig>::CallbackType controller_cb =
+        dynamic_reconfigure::Server<MpcLocalPlannerReconfigureConfig>::CallbackType cb =
-            boost::bind(&MpcLocalPlannerROS::reconfigureControllerCB, this, _1, _2);
+            boost::bind(&MpcLocalPlanner::reconfigureCB, this, _1, _2);
-        dynamic_controller_recfg_->setCallback(controller_cb);
+        dynamic_recfg_->setCallback(cb);
+        */
 
-        ros::NodeHandle collision_nh(nh, "collision");
+        // validate optimization footprint and costmap footprint
-        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);
-
-        // 验证优化足迹和代价地图足迹
         validateFootprints(_robot_model->getInscribedRadius(), _robot_inscribed_radius, _controller.getInequalityConstraint()->getMinimumDistance());
 
-        // 设置自定义障碍物的回调函数
+        // setup callback for custom obstacles
         _custom_obst_sub = nh.subscribe("obstacles", 1, &MpcLocalPlannerROS::customObstacleCB, this);
 
-        // 设置自定义路径点的回调函数
+        // setup callback for custom via-points
         _via_points_sub = nh.subscribe("via_points", 1, &MpcLocalPlannerROS::customViaPointsCB, this);
 
-        // 其他move_base参数
+        // additional move base params
         ros::NodeHandle nh_move_base("~");
         nh_move_base.param("controller_frequency", _params.controller_frequency, _params.controller_frequency);
 
-        // 设置已初始化标志
+        // set initialized flag
         _initialized = true;
 
         ROS_DEBUG("mpc_local_planner plugin initialized.");
@@ -229,31 +192,23 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm
     }
 }
 
-/**
- * @brief 设置全局路径计划
- *
- * 该函数用于将全局路径计划设置到局部规划器中。
- *
- * @param orig_global_plan 输入的全局路径计划，包含一系列的目标位姿。
- * @return bool 返回true表示设置成功，返回false表示设置失败（如插件未初始化）。
- */
 bool MpcLocalPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)
 {
-    // 检查插件是否已初始化
+    // check if plugin is initialized
     if (!_initialized)
     {
         ROS_ERROR("mpc_local_planner has not been initialized, please call initialize() before using this planner");
         return false;
     }
 
-    // 存储全局路径计划
+    // store the global plan
     _global_plan.clear();
     _global_plan = orig_global_plan;
 
-    // 这里不清除局部规划器，因为每当全局规划器更新计划时，setPlan 会被频繁调用。
+    // we do not clear the local planner here, since setPlan is called frequently whenever the global planner updates the plan.
-    // 局部规划器在每次速度计算步骤中检查是否需要重新初始化轨迹。
+    // the local planner checks whether it is required to reinitialize the trajectory or not within each velocity computation step.
 
-    // 重置目标到达标志
+    // reset goal_reached_ flag
     _goal_reached = false;
 
     return true;
@@ -377,7 +332,8 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt
     // updateControlFromTwist()
 
     // Do not allow config changes during the following optimization step
-    boost::mutex::scoped_lock cfg_lock(configMutex());
+    // TODO(roesmann): we need something similar in case we allow dynamic reconfiguration:
+    // 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);

mpc_local_planner_examples/package.xml

@@ -20,7 +20,6 @@
   <exec_depend>move_base</exec_depend>
   <exec_depend>map_server</exec_depend>
   <exec_depend>amcl</exec_depend>
-  <exec_depend>global_planner</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->