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 050036c..78f9f79 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
@@ -189,14 +189,14 @@ 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 getRobotFootprintFromParamServer(const ros::NodeHandle& nh, costmap_2d::Costmap2DROS* costmap_ros);
+    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);
+    static RobotFootprintModelPtr getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS* costmap_ros = nullptr);
 
     /**
      * @brief Set the footprint from the given XmlRpcValue.
diff --git a/mpc_local_planner/src/test_mpc_optim_node.cpp b/mpc_local_planner/src/test_mpc_optim_node.cpp
index 2d665d0..135e53c 100644
--- a/mpc_local_planner/src/test_mpc_optim_node.cpp
+++ b/mpc_local_planner/src/test_mpc_optim_node.cpp
@@ -91,7 +91,7 @@ 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, nullptr);
+    teb_local_planner::RobotFootprintModelPtr robot_model = mpc_local_planner::MpcLocalPlannerROS::getRobotFootprintFromParamServer(nh);
 
     mpc_local_planner::Controller controller;
     if (!controller.configure(nh, _obstacles, robot_model, _via_points))