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

Adding method to get robot footprint from costmap_2d instead of parameter server.

master
Alexander Xydes 2020-03-06 18:02:07 -08:00
parent e3658ac44d
commit 31471d2ed6
3 changed files with 38 additions and 12 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

@ -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.

43
mpc_local_planner/src/mpc_local_planner_ros.cpp
View File

@ -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<teb_local_planner::PointRobotFootprint>();
}
// 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::PointRobotFootprint>();
}
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<teb_local_planner::PolygonRobotFootprint>(footprint);
}
teb_local_planner::Point2dContainer MpcLocalPlannerROS::makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc,
const std::string& full_param_name)
{

3
mpc_local_planner/src/test_mpc_optim_node.cpp
View File

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