From aeccd15ac6ea6165586ddecff03977dd61ba2dbf Mon Sep 17 00:00:00 2001
From: Alexander Xydes <6345526+axydes@users.noreply.github.com>
Date: Sun, 8 Mar 2020 10:17:46 -0700
Subject: [PATCH] Changing getRobotFootprintFromCostmap2d function to take a
 const reference of costmap2dros. moving check of costmap_ros pointer into the
 function that calls it.

---
 .../mpc_local_planner/mpc_local_planner_ros.h   |  4 ++--
 mpc_local_planner/src/mpc_local_planner_ros.cpp | 17 ++++++++---------
 2 files changed, 10 insertions(+), 11 deletions(-)

diff --git a/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h b/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h
index 78f9f79..ab60bce 100644
--- a/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h
+++ b/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h
@@ -186,7 +186,7 @@ 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 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
      */
     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
      * @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.
diff --git a/mpc_local_planner/src/mpc_local_planner_ros.cpp b/mpc_local_planner/src/mpc_local_planner_ros.cpp
index 9031f43..0dc5aa9 100644
--- a/mpc_local_planner/src/mpc_local_planner_ros.cpp
+++ b/mpc_local_planner/src/mpc_local_planner_ros.cpp
@@ -862,8 +862,13 @@ teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintF
     // 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);
+        return getRobotFootprintFromCostmap2d(*costmap_ros);
     }
 
     // point
@@ -984,17 +989,11 @@ 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)
+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;
     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)
     {