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] 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))