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

Making getRobotFootprint functions static again, checking for validity of the costmap pointer passed in.

master
Alexander Xydes 2020-03-07 08:32:45 -08:00
parent e1b218c0a9
commit f2ebb686fc
3 changed files with 19 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -186,11 +186,16 @@ 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
* @return Robot footprint model used for optimization * @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. * @brief Set the footprint from the given XmlRpcValue.

16
mpc_local_planner/src/mpc_local_planner_ros.cpp
View File

@ -108,7 +108,7 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm
_robot_base_frame = _costmap_ros->getBaseFrameID(); _robot_base_frame = _costmap_ros->getBaseFrameID();
// create robot footprint/contour model for optimization // create robot footprint/contour model for optimization
_robot_model = getRobotFootprintFromParamServer(nh); _robot_model = getRobotFootprintFromParamServer(nh, _costmap_ros);
// create the planner instance // create the planner instance
if (!_controller.configure(nh, _obstacles, _robot_model, _via_points)) 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(); _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; std::string model_name;
if (!nh.getParam("footprint_model/type", 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) if (model_name.compare("costmap_2d") == 0)
{ {
ROS_INFO("Footprint model loaded from costmap_2d for trajectory optimization."); ROS_INFO("Footprint model loaded from costmap_2d for trajectory optimization.");
return getRobotFootprintFromCostmap2d(); return getRobotFootprintFromCostmap2d(costmap_ros);
} }
// point // point
@ -984,11 +984,17 @@ 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() 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)
{ {

3
mpc_local_planner/src/test_mpc_optim_node.cpp
View File

@ -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); ros::Subscriber via_points_sub = nh.subscribe("via_points", 1, &TestMpcOptimNode::CB_via_points, this);
// Setup robot shape model // Setup robot shape model
mpc_local_planner::MpcLocalPlannerROS mpcl; teb_local_planner::RobotFootprintModelPtr robot_model = mpc_local_planner::MpcLocalPlannerROS::getRobotFootprintFromParamServer(nh, nullptr);
teb_local_planner::RobotFootprintModelPtr robot_model = mpcl.getRobotFootprintFromParamServer(nh);
mpc_local_planner::Controller controller; mpc_local_planner::Controller controller;
if (!controller.configure(nh, _obstacles, robot_model, _via_points)) if (!controller.configure(nh, _obstacles, robot_model, _via_points))