From ae89b1966b76261cc6fe11b0e96bca6d50504aa5 Mon Sep 17 00:00:00 2001 From: kosmastsk Date: Wed, 12 Aug 2020 20:35:34 +0300 Subject: [PATCH] Add dynamic reconfigure --- mpc_local_planner/CMakeLists.txt | 17 ++++--- mpc_local_planner/cfg/mpc_reconfigure.cfg | 50 +++++++++++++++++++ .../mpc_local_planner/mpc_local_planner_ros.h | 17 +++++-- .../src/mpc_local_planner_ros.cpp | 27 ++++++---- 4 files changed, 90 insertions(+), 21 deletions(-) create mode 100644 mpc_local_planner/cfg/mpc_reconfigure.cfg diff --git a/mpc_local_planner/CMakeLists.txt b/mpc_local_planner/CMakeLists.txt index 21a51db..91f53a7 100644 --- a/mpc_local_planner/CMakeLists.txt +++ b/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 -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) +#add dynamic reconfigure api +#find_package(catkin REQUIRED dynamic_reconfigure) +generate_dynamic_reconfigure_options( + 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 diff --git a/mpc_local_planner/cfg/mpc_reconfigure.cfg b/mpc_local_planner/cfg/mpc_reconfigure.cfg new file mode 100644 index 0000000..9ce259a --- /dev/null +++ b/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")) \ No newline at end of file diff --git a/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h b/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h index 1839955..0a2582d 100644 --- a/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h +++ b/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h @@ -58,8 +58,8 @@ #include // dynamic reconfigure -// #include -// #include +#include +#include #include @@ -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_loader; //!< Load costmap converter plugins at runtime boost::shared_ptr _costmap_converter; //!< Store the current costmap_converter - // std::shared_ptr> - // dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime + boost::shared_ptr> + 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; diff --git a/mpc_local_planner/src/mpc_local_planner_ros.cpp b/mpc_local_planner/src/mpc_local_planner_ros.cpp index a923fcd..6ad10bb 100644 --- a/mpc_local_planner/src/mpc_local_planner_ros.cpp +++ b/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(TebLocalPlannerReconfigureConfig& config, uint32_t level) +void MpcLocalPlannerROS::reconfigureCB(MpcLocalPlannerReconfigureConfig& 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>(nh); dynamic_reconfigure::Server::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(cfg_.configMutex()); + boost::mutex::scoped_lock cfg_lock(configMutex()); // Now perform the actual planning // bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);