diff --git a/mpc_local_planner/CMakeLists.txt b/mpc_local_planner/CMakeLists.txt index 425e6be..227704c 100644 --- a/mpc_local_planner/CMakeLists.txt +++ b/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 ) ################################### diff --git a/mpc_local_planner/cfg/mpc_collision.cfg b/mpc_local_planner/cfg/mpc_collision.cfg new file mode 100644 index 0000000..9f83fa7 --- /dev/null +++ b/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")) \ No newline at end of file diff --git a/mpc_local_planner/cfg/mpc_reconfigure.cfg b/mpc_local_planner/cfg/mpc_controller.cfg similarity index 59% rename from mpc_local_planner/cfg/mpc_reconfigure.cfg rename to mpc_local_planner/cfg/mpc_controller.cfg index 9ce259a..83df884 100644 --- a/mpc_local_planner/cfg/mpc_reconfigure.cfg +++ b/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 - -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 +exit(gen.generate("mpc_local_planner", "mpc_local_planner", "ControllerReconfigure")) \ No newline at end of file diff --git a/mpc_local_planner/cfg/mpc_footprint.cfg b/mpc_local_planner/cfg/mpc_footprint.cfg new file mode 100644 index 0000000..09a4379 --- /dev/null +++ b/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")) \ 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 0a2582d..d1ef128 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 @@ -59,7 +59,9 @@ // dynamic reconfigure #include -#include +#include +#include +#include #include @@ -263,13 +265,31 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap void updateViaPointsContainer(const std::vector& 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_loader; //!< Load costmap converter plugins at runtime boost::shared_ptr _costmap_converter; //!< Store the current costmap_converter - boost::shared_ptr> - dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime + boost::shared_ptr> + dynamic_controller_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime + boost::shared_ptr> + dynamic_collision_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime + boost::shared_ptr> + 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 diff --git a/mpc_local_planner/src/mpc_local_planner_ros.cpp b/mpc_local_planner/src/mpc_local_planner_ros.cpp index 6ad10bb..dd0214a 100644 --- a/mpc_local_planner/src/mpc_local_planner_ros.cpp +++ b/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>(nh); - dynamic_reconfigure::Server::CallbackType cb = - boost::bind(&MpcLocalPlannerROS::reconfigureCB, this, _1, _2); - dynamic_recfg_->setCallback(cb); + ros::NodeHandle controller_nh(nh, "controller"); + dynamic_controller_recfg_ = boost::make_shared>(controller_nh); + dynamic_reconfigure::Server::CallbackType controller_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>(collision_nh); + dynamic_reconfigure::Server::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>(footprint_nh); + dynamic_reconfigure::Server::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());