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

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.

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

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