Merge pull request #6 from axydes/costmap-2d-footprint

Costmap 2d footprint

mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h

@@ -186,9 +186,17 @@ 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 pointer to an intialized instance of costmap_2d::Costmap2dROS
      * @return Robot footprint model used for optimization
      */
-    static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh);
+    static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh, costmap_2d::Costmap2DROS* costmap_ros = nullptr);
+
+    /**
+     * @brief Get the current robot footprint/contour model
+     * @param costmap_ros reference to an intialized instance of costmap_2d::Costmap2dROS
+     * @return Robot footprint model used for optimization
+     */
+    static RobotFootprintModelPtr getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS& costmap_ros);
 
     /**
      * @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, _costmap_ros);
+
+        // 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);
 
@@ -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))
@@ -859,6 +859,18 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF
         return boost::make_shared<teb_local_planner::PointRobotFootprint>();
     }
 
+    // from costmap_2d
+    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.");
+        return getRobotFootprintFromCostmap2d(*costmap_ros);
+    }
+
     // point
     if (model_name.compare("point") == 0)
     {
@@ -977,6 +989,22 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF
     return boost::make_shared<teb_local_planner::PointRobotFootprint>();
 }
 
+teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS& costmap_ros)
+{
+    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)
 {