12345qiupeng
/
mpc_local_planner
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Changing getRobotFootprintFromCostmap2d function to take a const reference of costmap2dros. moving check of costmap_ros pointer into the function that calls it.

master
Alexander Xydes 2020-03-08 10:17:46 -07:00
parent 0221bc3f8d
commit aeccd15ac6
2 changed files with 10 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h
View File

@ -186,7 +186,7 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
/** /**
* @brief Get the current robot footprint/contour model * @brief Get the current robot footprint/contour model
* @param nh const reference to the local ros::NodeHandle * @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 * @return Robot footprint model used for optimization
*/ */
static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh, costmap_2d::Costmap2DROS* costmap_ros = nullptr); 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 * @param costmap_ros reference to an intialized instance of costmap_2d::Costmap2dROS
* @return Robot footprint model used for optimization * @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. * @brief Set the footprint from the given XmlRpcValue.

17
mpc_local_planner/src/mpc_local_planner_ros.cpp
View File

@ -862,8 +862,13 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF
// from costmap_2d // from costmap_2d
if (model_name.compare("costmap_2d") == 0) 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<teb_local_planner::PointRobotFootprint>();
}
ROS_INFO("Footprint model loaded from costmap_2d for trajectory optimization."); ROS_INFO("Footprint model loaded from costmap_2d for trajectory optimization.");
return getRobotFootprintFromCostmap2d(costmap_ros); return getRobotFootprintFromCostmap2d(*costmap_ros);
} }
// point // point
@ -984,17 +989,11 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF
return boost::make_shared<teb_local_planner::PointRobotFootprint>(); return boost::make_shared<teb_local_planner::PointRobotFootprint>();
} }
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<teb_local_planner::PointRobotFootprint>();
}
Point2dContainer footprint; Point2dContainer footprint;
Eigen::Vector2d pt; 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) for (int i = 0; i < polygon.points.size(); ++i)
{ {