From 31471d2ed66de6ac85a787f32e2d26d68015ffea Mon Sep 17 00:00:00 2001 From: Alexander Xydes <6345526+axydes@users.noreply.github.com> Date: Fri, 6 Mar 2020 18:02:07 -0800 Subject: [PATCH 1/6] Adding method to get robot footprint from costmap_2d instead of parameter server. --- .../mpc_local_planner/mpc_local_planner_ros.h | 4 +- .../src/mpc_local_planner_ros.cpp | 43 ++++++++++++++----- mpc_local_planner/src/test_mpc_optim_node.cpp | 3 +- 3 files changed, 38 insertions(+), 12 deletions(-) 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 c1921bb..a30ef69 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 @@ -188,7 +188,9 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap * @param nh const reference to the local ros::NodeHandle * @return Robot footprint model used for optimization */ - static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh); + RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh); + + RobotFootprintModelPtr getRobotFootprintFromCostmap2d(); /** * @brief Set the footprint from the given XmlRpcValue. diff --git a/mpc_local_planner/src/mpc_local_planner_ros.cpp b/mpc_local_planner/src/mpc_local_planner_ros.cpp index 782a19d..b1e1fb0 100644 --- a/mpc_local_planner/src/mpc_local_planner_ros.cpp +++ b/mpc_local_planner/src/mpc_local_planner_ros.cpp @@ -97,16 +97,6 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm // reserve some memory for obstacles _obstacles.reserve(700); - // create robot footprint/contour model for optimization - _robot_model = getRobotFootprintFromParamServer(nh); - - // create the planner instance - if (!_controller.configure(nh, _obstacles, _robot_model, _via_points)) - { - ROS_ERROR("Controller configuration failed."); - return; - } - // init other variables _tf = tf; _costmap_ros = costmap_ros; @@ -117,6 +107,16 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm _global_frame = _costmap_ros->getGlobalFrameID(); _robot_base_frame = _costmap_ros->getBaseFrameID(); + // create robot footprint/contour model for optimization + _robot_model = getRobotFootprintFromParamServer(nh); + + // 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); @@ -859,6 +859,13 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF return boost::make_shared(); } + // from costmap_2d + if (model_name.compare("costmap_2d") == 0) + { + ROS_INFO("Footprint model loaded from costmap_2d for trajectory optimization."); + return getRobotFootprintFromCostmap2d(); + } + // point if (model_name.compare("point") == 0) { @@ -977,6 +984,22 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF return boost::make_shared(); } +teb_local_planner::RobotFootprintModelPtr getRobotFootprintFromCostmap2d() +{ + Point2dContainer footprint; + Eigen::Vector2d pt; + geometry_msgs::Polygon polygon = _costmap_ros.getRobotFootprintPolygon(); + + for (int i = 0; i < polygon.points.size(); ++i) + { + pt.x() = polygon.points[i].x; + pt.y() = polygon.points[i].y; + + footprint.push_back(pt); + } + return boost::make_shared(footprint); +} + teb_local_planner::Point2dContainer MpcLocalPlannerROS::makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name) { diff --git a/mpc_local_planner/src/test_mpc_optim_node.cpp b/mpc_local_planner/src/test_mpc_optim_node.cpp index 135e53c..242749e 100644 --- a/mpc_local_planner/src/test_mpc_optim_node.cpp +++ b/mpc_local_planner/src/test_mpc_optim_node.cpp @@ -91,7 +91,8 @@ void TestMpcOptimNode::start(ros::NodeHandle& nh) ros::Subscriber via_points_sub = nh.subscribe("via_points", 1, &TestMpcOptimNode::CB_via_points, this); // Setup robot shape model - teb_local_planner::RobotFootprintModelPtr robot_model = mpc_local_planner::MpcLocalPlannerROS::getRobotFootprintFromParamServer(nh); + mpc_local_planner::MpcLocalPlannerROS mpcl; + teb_local_planner::RobotFootprintModelPtr robot_model = mpcl.getRobotFootprintFromParamServer(nh); mpc_local_planner::Controller controller; if (!controller.configure(nh, _obstacles, robot_model, _via_points)) From e1b218c0a9171803e7398860583b46958bb9e528 Mon Sep 17 00:00:00 2001 From: Alexander Xydes <6345526+axydes@users.noreply.github.com> Date: Fri, 6 Mar 2020 18:19:53 -0800 Subject: [PATCH 2/6] costmap_ros is a pointer and adding namespace to new function. --- mpc_local_planner/src/mpc_local_planner_ros.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mpc_local_planner/src/mpc_local_planner_ros.cpp b/mpc_local_planner/src/mpc_local_planner_ros.cpp index b1e1fb0..0593a65 100644 --- a/mpc_local_planner/src/mpc_local_planner_ros.cpp +++ b/mpc_local_planner/src/mpc_local_planner_ros.cpp @@ -984,11 +984,11 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF return boost::make_shared(); } -teb_local_planner::RobotFootprintModelPtr getRobotFootprintFromCostmap2d() +teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintFromCostmap2d() { Point2dContainer footprint; Eigen::Vector2d pt; - geometry_msgs::Polygon polygon = _costmap_ros.getRobotFootprintPolygon(); + geometry_msgs::Polygon polygon = _costmap_ros->getRobotFootprintPolygon(); for (int i = 0; i < polygon.points.size(); ++i) { From f2ebb686fca5ce34ec34d96b1e7adbf0ebef0b2c Mon Sep 17 00:00:00 2001 From: Alexander Xydes <6345526+axydes@users.noreply.github.com> Date: Sat, 7 Mar 2020 08:32:45 -0800 Subject: [PATCH 3/6] Making getRobotFootprint functions static again, checking for validity of the costmap pointer passed in. --- .../mpc_local_planner/mpc_local_planner_ros.h | 9 +++++++-- mpc_local_planner/src/mpc_local_planner_ros.cpp | 16 +++++++++++----- mpc_local_planner/src/test_mpc_optim_node.cpp | 3 +-- 3 files changed, 19 insertions(+), 9 deletions(-) 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 a30ef69..32705f4 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 @@ -186,11 +186,16 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap /** * @brief Get the current robot footprint/contour model * @param nh const reference to the local ros::NodeHandle + * @param costmap_ros reference to an intialized instance of costmap_2d::Costmap2dROS * @return Robot footprint model used for optimization */ - RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh); + static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh, costmap_2d::Costmap2DROS* costmap_ros); - RobotFootprintModelPtr getRobotFootprintFromCostmap2d(); + /** + * @brief Get the current robot footprint/contour model + * @return Robot footprint model used for optimization + */ + static RobotFootprintModelPtr getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS* costmap_ros); /** * @brief Set the footprint from the given XmlRpcValue. diff --git a/mpc_local_planner/src/mpc_local_planner_ros.cpp b/mpc_local_planner/src/mpc_local_planner_ros.cpp index 0593a65..9031f43 100644 --- a/mpc_local_planner/src/mpc_local_planner_ros.cpp +++ b/mpc_local_planner/src/mpc_local_planner_ros.cpp @@ -108,7 +108,7 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm _robot_base_frame = _costmap_ros->getBaseFrameID(); // create robot footprint/contour model for optimization - _robot_model = getRobotFootprintFromParamServer(nh); + _robot_model = getRobotFootprintFromParamServer(nh, _costmap_ros); // create the planner instance if (!_controller.configure(nh, _obstacles, _robot_model, _via_points)) @@ -850,7 +850,7 @@ void MpcLocalPlannerROS::customViaPointsCB(const nav_msgs::Path::ConstPtr& via_p _custom_via_points_active = !_via_points.empty(); } -teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintFromParamServer(const ros::NodeHandle& nh) +teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintFromParamServer(const ros::NodeHandle& nh, costmap_2d::Costmap2DROS* costmap_ros) { std::string model_name; if (!nh.getParam("footprint_model/type", model_name)) @@ -863,7 +863,7 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF if (model_name.compare("costmap_2d") == 0) { ROS_INFO("Footprint model loaded from costmap_2d for trajectory optimization."); - return getRobotFootprintFromCostmap2d(); + return getRobotFootprintFromCostmap2d(costmap_ros); } // point @@ -984,11 +984,17 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF return boost::make_shared(); } -teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintFromCostmap2d() +teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS* costmap_ros) { + if (!costmap_ros) + { + ROS_WARN_STREAM("Costmap 2d pointer is null. Using point model instead."); + return boost::make_shared(); + } + Point2dContainer footprint; Eigen::Vector2d pt; - geometry_msgs::Polygon polygon = _costmap_ros->getRobotFootprintPolygon(); + geometry_msgs::Polygon polygon = costmap_ros->getRobotFootprintPolygon(); for (int i = 0; i < polygon.points.size(); ++i) { diff --git a/mpc_local_planner/src/test_mpc_optim_node.cpp b/mpc_local_planner/src/test_mpc_optim_node.cpp index 242749e..2d665d0 100644 --- a/mpc_local_planner/src/test_mpc_optim_node.cpp +++ b/mpc_local_planner/src/test_mpc_optim_node.cpp @@ -91,8 +91,7 @@ void TestMpcOptimNode::start(ros::NodeHandle& nh) ros::Subscriber via_points_sub = nh.subscribe("via_points", 1, &TestMpcOptimNode::CB_via_points, this); // Setup robot shape model - mpc_local_planner::MpcLocalPlannerROS mpcl; - teb_local_planner::RobotFootprintModelPtr robot_model = mpcl.getRobotFootprintFromParamServer(nh); + teb_local_planner::RobotFootprintModelPtr robot_model = mpc_local_planner::MpcLocalPlannerROS::getRobotFootprintFromParamServer(nh, nullptr); mpc_local_planner::Controller controller; if (!controller.configure(nh, _obstacles, robot_model, _via_points)) From bb30d27d3ab658c9ba9db9905cc58d27250be18e Mon Sep 17 00:00:00 2001 From: Alexander Xydes <6345526+axydes@users.noreply.github.com> Date: Sat, 7 Mar 2020 08:34:44 -0800 Subject: [PATCH 4/6] Forgot a param in the function comment. --- .../include/mpc_local_planner/mpc_local_planner_ros.h | 1 + 1 file changed, 1 insertion(+) 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 32705f4..050036c 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 @@ -193,6 +193,7 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap /** * @brief Get the current robot footprint/contour model + * @param costmap_ros reference to an intialized instance of costmap_2d::Costmap2dROS * @return Robot footprint model used for optimization */ static RobotFootprintModelPtr getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS* costmap_ros); From 0221bc3f8d6acc0804856e64fc8f2c30e082f691 Mon Sep 17 00:00:00 2001 From: Alexander Xydes <6345526+axydes@users.noreply.github.com> Date: Sat, 7 Mar 2020 08:41:32 -0800 Subject: [PATCH 5/6] Changing costmap_ros to optional parameter. --- .../include/mpc_local_planner/mpc_local_planner_ros.h | 4 ++-- mpc_local_planner/src/test_mpc_optim_node.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) 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 050036c..78f9f79 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 @@ -189,14 +189,14 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap * @param costmap_ros reference to an intialized instance of costmap_2d::Costmap2dROS * @return Robot footprint model used for optimization */ - static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh, costmap_2d::Costmap2DROS* costmap_ros); + static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh, costmap_2d::Costmap2DROS* costmap_ros = nullptr); /** * @brief Get the current robot footprint/contour model * @param costmap_ros reference to an intialized instance of costmap_2d::Costmap2dROS * @return Robot footprint model used for optimization */ - static RobotFootprintModelPtr getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS* costmap_ros); + static RobotFootprintModelPtr getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS* costmap_ros = nullptr); /** * @brief Set the footprint from the given XmlRpcValue. diff --git a/mpc_local_planner/src/test_mpc_optim_node.cpp b/mpc_local_planner/src/test_mpc_optim_node.cpp index 2d665d0..135e53c 100644 --- a/mpc_local_planner/src/test_mpc_optim_node.cpp +++ b/mpc_local_planner/src/test_mpc_optim_node.cpp @@ -91,7 +91,7 @@ void TestMpcOptimNode::start(ros::NodeHandle& nh) ros::Subscriber via_points_sub = nh.subscribe("via_points", 1, &TestMpcOptimNode::CB_via_points, this); // Setup robot shape model - teb_local_planner::RobotFootprintModelPtr robot_model = mpc_local_planner::MpcLocalPlannerROS::getRobotFootprintFromParamServer(nh, nullptr); + teb_local_planner::RobotFootprintModelPtr robot_model = mpc_local_planner::MpcLocalPlannerROS::getRobotFootprintFromParamServer(nh); mpc_local_planner::Controller controller; if (!controller.configure(nh, _obstacles, robot_model, _via_points)) From aeccd15ac6ea6165586ddecff03977dd61ba2dbf Mon Sep 17 00:00:00 2001 From: Alexander Xydes <6345526+axydes@users.noreply.github.com> Date: Sun, 8 Mar 2020 10:17:46 -0700 Subject: [PATCH 6/6] Changing getRobotFootprintFromCostmap2d function to take a const reference of costmap2dros. moving check of costmap_ros pointer into the function that calls it. --- .../mpc_local_planner/mpc_local_planner_ros.h | 4 ++-- mpc_local_planner/src/mpc_local_planner_ros.cpp | 17 ++++++++--------- 2 files changed, 10 insertions(+), 11 deletions(-) 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 78f9f79..ab60bce 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 @@ -186,7 +186,7 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap /** * @brief Get the current robot footprint/contour model * @param nh const reference to the local ros::NodeHandle - * @param costmap_ros reference to an intialized instance of costmap_2d::Costmap2dROS + * @param costmap_ros pointer to an intialized instance of costmap_2d::Costmap2dROS * @return Robot footprint model used for optimization */ static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh, costmap_2d::Costmap2DROS* costmap_ros = nullptr); @@ -196,7 +196,7 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap * @param costmap_ros reference to an intialized instance of costmap_2d::Costmap2dROS * @return Robot footprint model used for optimization */ - static RobotFootprintModelPtr getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS* costmap_ros = nullptr); + static RobotFootprintModelPtr getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS& costmap_ros); /** * @brief Set the footprint from the given XmlRpcValue. diff --git a/mpc_local_planner/src/mpc_local_planner_ros.cpp b/mpc_local_planner/src/mpc_local_planner_ros.cpp index 9031f43..0dc5aa9 100644 --- a/mpc_local_planner/src/mpc_local_planner_ros.cpp +++ b/mpc_local_planner/src/mpc_local_planner_ros.cpp @@ -862,8 +862,13 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF // from costmap_2d if (model_name.compare("costmap_2d") == 0) { + if (!costmap_ros) + { + ROS_WARN_STREAM("Costmap 2d pointer is null. Using point model instead."); + return boost::make_shared(); + } ROS_INFO("Footprint model loaded from costmap_2d for trajectory optimization."); - return getRobotFootprintFromCostmap2d(costmap_ros); + return getRobotFootprintFromCostmap2d(*costmap_ros); } // point @@ -984,17 +989,11 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF return boost::make_shared(); } -teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS* costmap_ros) +teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS& costmap_ros) { - if (!costmap_ros) - { - ROS_WARN_STREAM("Costmap 2d pointer is null. Using point model instead."); - return boost::make_shared(); - } - Point2dContainer footprint; Eigen::Vector2d pt; - geometry_msgs::Polygon polygon = costmap_ros->getRobotFootprintPolygon(); + geometry_msgs::Polygon polygon = costmap_ros.getRobotFootprintPolygon(); for (int i = 0; i < polygon.points.size(); ++i) {