update

src/Libraries/costmap_2d

@@ -1 +1 @@
-Subproject commit 0344c31e5b4185da41bfb253db720d9c91b802bd
+Subproject commit a28f05c6d5cb6cf2d8c4ffb52a637ecfa2878db2

src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h

@@ -39,7 +39,6 @@
 #include <nav_core2/bounds.h>
 #include <vector>
 
-using namespace robot;
 /**
  * @brief A set of utility functions for Bounds objects interacting with other messages/types
  */

src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h

@@ -98,8 +98,8 @@ namespace robot_nav_2d_utils
   nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo &info);
 
   // Bounds Transformations
-  ::robot_nav_2d_msgs::UIntBounds toMsg(const robot::nav_core2::UIntBounds &bounds);
+  robot_nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds &bounds);
-  robot::nav_core2::UIntBounds fromMsg(const ::robot_nav_2d_msgs::UIntBounds &msg);
+  nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg);
 
 } // namespace robot_nav_2d_utils
 

src/Libraries/robot_nav_2d_utils/src/bounds.cpp

@@ -38,7 +38,6 @@
 #include <stdexcept>
 #include <vector>
 
-using namespace robot;
 namespace robot_nav_2d_utils
 {
   nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info)

src/Libraries/robot_nav_2d_utils/src/conversions.cpp

@@ -318,7 +318,7 @@ namespace robot_nav_2d_utils
     return metadata;
   }
 
-  robot_nav_2d_msgs::UIntBounds toMsg(const robot::nav_core2::UIntBounds &bounds)
+  robot_nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds &bounds)
   {
     robot_nav_2d_msgs::UIntBounds msg;
     msg.min_x = bounds.getMinX();
@@ -328,9 +328,9 @@ namespace robot_nav_2d_utils
     return msg;
   }
 
-  robot::nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg)
+  nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg)
   {
-    return robot::nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y);
+    return nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y);
   }
 
 } // namespace robot_nav_2d_utils

src/Navigations/Cores/nav_core2/CMakeLists.txt

@@ -27,7 +27,7 @@ target_link_libraries(nav_core2 INTERFACE
         costmap_2d 
         tf3 
         nav_grid 
-        nav_2d_msgs
+        robot_nav_2d_msgs
         robot_cpp
 )
 

src/Navigations/Cores/nav_core2/README.md

@@ -2,7 +2,7 @@
 A replacement interface for [nav_core](https://github.com/ros-planning/navigation/tree/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core) that defines basic two dimensional planner interfaces.
 
 There were a few key reasons for creating new interfaces rather than extending the old ones.
- * Use `nav_2d_msgs` to eliminate unused data fields
+ * Use `robot_nav_2d_msgs` to eliminate unused data fields
  * Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `costmap_2d::Costmap2DROBOT`.
  * Provide more data in the interfaces for easier testing.
  * Use Exceptions rather than booleans to provide more information about the types of errors encountered.
@@ -30,7 +30,7 @@ Let us compare the old [nav_core::BaseGlobalPlanner](https://github.com/ros-plan
 | `nav_core` | `nav_core2` | comments |
 |---|--|---|
 |`void initialize(std::string, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
-|`bool makePlan(const geometry_msgs::PoseStamped&, const geometry_msgs::PoseStamped&, std::vector<geometry_msgs::PoseStamped>&)`|`nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.|
+|`bool makePlan(const geometry_msgs::PoseStamped&, const geometry_msgs::PoseStamped&, std::vector<geometry_msgs::PoseStamped>&)`|`robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.|
 
 ## Local Planner
 Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core/base_local_planner.h) to the new [nav_core2/LocalPlanner](include/nav_core2/local_planner.h).
@@ -38,10 +38,10 @@ Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-pl
 | `nav_core` | `nav_core2` | comments |
 |---|--|---|
 |`void initialize(std::string, tf::TransformListener*, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
-|(no equivalent)|`void setGoalPose(const nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan`
+|(no equivalent)|`void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan`
-|`bool setPlan(const std::vector<geometry_msgs::PoseStamped>&)`|`setPlan(const nav_2d_msgs::Path2D&)`||
+|`bool setPlan(const std::vector<geometry_msgs::PoseStamped>&)`|`setPlan(const robot_nav_2d_msgs::Path2D&)`||
-|`bool computeVelocityCommands(geometry_msgs::Twist&)`|`nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Twist2D&)`|Explicitly provides the current pose and velocity for more explicit data control and easier testing. Uses exceptions for errors instead of returning a bool, which frees up the return for the actual command.|
+|`bool computeVelocityCommands(geometry_msgs::Twist&)`|`robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Twist2D&)`|Explicitly provides the current pose and velocity for more explicit data control and easier testing. Uses exceptions for errors instead of returning a bool, which frees up the return for the actual command.|
-|`bool isGoalReached()` | `bool isGoalReached(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Twist2D&)` | Explicitly provide the current pose and velocity for more explicit data control and easier testing. |
+|`bool isGoalReached()` | `bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Twist2D&)` | Explicitly provide the current pose and velocity for more explicit data control and easier testing. |
 
 ## Exceptions
 A hierarchical collection of [exceptions](include/nav_core2/exceptions.h) is provided to allow for reacting to navigation failures in a more robust and contextual way.

src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h

@@ -34,7 +34,7 @@
 #ifndef NAV_CORE2_EXCEPTIONS_H
 #define NAV_CORE2_EXCEPTIONS_H
 
-#include <nav_2d_msgs/Pose2DStamped.h>
+#include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <stdexcept>
 #include <exception>
 #include <string>
@@ -69,7 +69,7 @@
 namespace nav_core2
 {
 
-inline std::string poseToString(const nav_2d_msgs::Pose2DStamped& pose)
+inline std::string poseToString(const robot_nav_2d_msgs::Pose2DStamped& pose)
 {
   return "(" + std::to_string(pose.pose.x) + ", " + std::to_string(pose.pose.y) + ", " + std::to_string(pose.pose.theta)
          + " : " + pose.header.frame_id + ")";
@@ -199,7 +199,7 @@ class InvalidStartPoseException: public GlobalPlannerException
 public:
   explicit InvalidStartPoseException(const std::string& description, int result_code = 5)
     : GlobalPlannerException(description, result_code) {}
-  InvalidStartPoseException(const nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 5) :
+  InvalidStartPoseException(const robot_nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 5) :
     InvalidStartPoseException("The starting pose " + poseToString(pose) + " is " + problem, result_code) {}
 };
 
@@ -212,7 +212,7 @@ class StartBoundsException: public InvalidStartPoseException
 public:
   explicit StartBoundsException(const std::string& description, int result_code = 6)
     : InvalidStartPoseException(description, result_code) {}
-  explicit StartBoundsException(const nav_2d_msgs::Pose2DStamped& pose) :
+  explicit StartBoundsException(const robot_nav_2d_msgs::Pose2DStamped& pose) :
     InvalidStartPoseException(pose, "out of bounds", 6) {}
 };
 
@@ -225,7 +225,7 @@ class OccupiedStartException: public InvalidStartPoseException
 public:
   explicit OccupiedStartException(const std::string& description, int result_code = 7)
     : InvalidStartPoseException(description, result_code) {}
-  explicit OccupiedStartException(const nav_2d_msgs::Pose2DStamped& pose) :
+  explicit OccupiedStartException(const robot_nav_2d_msgs::Pose2DStamped& pose) :
     InvalidStartPoseException(pose, "occupied", 7) {}
 };
 
@@ -238,7 +238,7 @@ class InvalidGoalPoseException: public GlobalPlannerException
 public:
   explicit InvalidGoalPoseException(const std::string& description, int result_code = 8)
     : GlobalPlannerException(description, result_code) {}
-  InvalidGoalPoseException(const nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 8) :
+  InvalidGoalPoseException(const robot_nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 8) :
     GlobalPlannerException("The goal pose " + poseToString(pose) + " is " + problem, result_code) {}
 };
 
@@ -251,7 +251,7 @@ class GoalBoundsException: public InvalidGoalPoseException
 public:
   explicit GoalBoundsException(const std::string& description, int result_code = 9)
     : InvalidGoalPoseException(description, result_code) {}
-  explicit GoalBoundsException(const nav_2d_msgs::Pose2DStamped& pose) :
+  explicit GoalBoundsException(const robot_nav_2d_msgs::Pose2DStamped& pose) :
     InvalidGoalPoseException(pose, "out of bounds", 9) {}
 };
 
@@ -264,7 +264,7 @@ class OccupiedGoalException: public InvalidGoalPoseException
 public:
   explicit OccupiedGoalException(const std::string& description, int result_code = 10)
     : InvalidGoalPoseException(description, result_code) {}
-  explicit OccupiedGoalException(const nav_2d_msgs::Pose2DStamped& pose) :
+  explicit OccupiedGoalException(const robot_nav_2d_msgs::Pose2DStamped& pose) :
     InvalidGoalPoseException(pose, "occupied", 10) {}
 };
 

src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h

@@ -36,8 +36,8 @@
 
 #include <nav_core2/common.h>
 #include <nav_core2/costmap.h>
-#include <nav_2d_msgs/Path2D.h>
+#include <robot_nav_2d_msgs/Path2D.h>
-#include <nav_2d_msgs/Pose2DStamped.h>
+#include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <string>
 #include <memory>
 
@@ -77,8 +77,8 @@ public:
    * @param goal  The goal pose of the robot
    * @return      The sequence of poses to get from start to goal, if any
    */
-  virtual nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start,
+  virtual robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped& start,
-                                       const nav_2d_msgs::Pose2DStamped& goal) = 0;
+                                       const robot_nav_2d_msgs::Pose2DStamped& goal) = 0;
 };
 }  // namespace nav_core2
 

src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h

@@ -36,10 +36,10 @@
 
 #include <nav_core2/common.h>
 #include <nav_core2/costmap.h>
-#include <nav_2d_msgs/Path2D.h>
+#include <robot_nav_2d_msgs/Path2D.h>
-#include <nav_2d_msgs/Pose2DStamped.h>
+#include <robot_nav_2d_msgs/Pose2DStamped.h>
-#include <nav_2d_msgs/Twist2D.h>
+#include <robot_nav_2d_msgs/Twist2D.h>
-#include <nav_2d_msgs/Twist2DStamped.h>
+#include <robot_nav_2d_msgs/Twist2DStamped.h>
 #include <geometry_msgs/Twist.h>
 #include <string>
 #include <costmap_2d/costmap_2d_robot.h>
@@ -83,14 +83,14 @@ namespace nav_core2
      * @brief Sets the global goal for this local planner.
      * @param goal_pose The Goal Pose
      */
-    virtual void setGoalPose(const nav_2d_msgs::Pose2DStamped &goal_pose) = 0;
+    virtual void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped &goal_pose) = 0;
 
     /**
      * @brief Sets the global plan for this local planner.
      *
      * @param path The global plan
      */
-    virtual void setPlan(const nav_2d_msgs::Path2D &path) = 0;
+    virtual void setPlan(const robot_nav_2d_msgs::Path2D &path) = 0;
 
     /**
      * @brief Compute the best command given the current pose, velocity and goal
@@ -102,8 +102,8 @@ namespace nav_core2
      * @param velocity  Current robot velocity
      * @return          The best computed velocity
      */
-    virtual nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose,
+    virtual robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose,
-                                                                const nav_2d_msgs::Twist2D &velocity) = 0;
+                                                                const robot_nav_2d_msgs::Twist2D &velocity) = 0;
 
     /**
      * @brief set velocity for x, y asix of the robot
@@ -166,7 +166,7 @@ namespace nav_core2
      * @param velocity   Velocity to check
      * @return           True if the goal conditions have been met
      */
-    virtual bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) = 0;
+    virtual bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) = 0;
   };
 } // namespace nav_core2
 

src/Navigations/Cores/nav_core_adapter/CMakeLists.txt

@@ -11,7 +11,7 @@ if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
 endif()
 
-set(PACKAGES_DIR geometry_msgs std_msgs nav_core nav_core2 nav_2d_utils pthread)
+set(PACKAGES_DIR geometry_msgs std_msgs nav_core nav_core2 robot_nav_2d_utils pthread)
 
 find_package(Boost REQUIRED COMPONENTS filesystem system)
 

src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h

@@ -57,8 +57,8 @@ public:
   // Nav Core 2 Global Planner Interface
   void initialize(const robot::NodeHandle& parent, const std::string& name,
                   TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
-  nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start,
+  robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped& start,
-                               const nav_2d_msgs::Pose2DStamped& goal) override;
+                               const robot_nav_2d_msgs::Pose2DStamped& goal) override;
 
   static nav_core2::GlobalPlanner::Ptr create();
 

src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h

@@ -39,7 +39,7 @@
 #include <nav_core/base_local_planner.h>
 #include <nav_core2/local_planner.h>
 #include <nav_core_adapter/costmap_adapter.h>
-#include <nav_2d_utils/odom_subscriber.h>
+#include <robot_nav_2d_utils/odom_subscriber.h>
 #include <boost/dll/import.hpp>
 #include <memory>
 #include <string>
@@ -168,22 +168,22 @@ namespace nav_core_adapter
     /**
      * @brief Get the robot pose from the costmap and store as Pose2DStamped
      */
-    bool getRobotPose(nav_2d_msgs::Pose2DStamped &pose2d);
+    bool getRobotPose(robot_nav_2d_msgs::Pose2DStamped &pose2d);
 
     /**
      * @brief See if the back of the global plan matches the most recent goal pose
      * @return True if the plan has changed
      */
-    bool hasGoalChanged(const nav_2d_msgs::Pose2DStamped &new_goal);
+    bool hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal);
 
     // The most recent goal pose
-    nav_2d_msgs::Pose2DStamped last_goal_;
+    robot_nav_2d_msgs::Pose2DStamped last_goal_;
     bool has_active_goal_;
 
     /**
      * @brief helper class for subscribing to odometry
      */
-    std::shared_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
+    std::shared_ptr<robot_nav_2d_utils::OdomSubscriber> odom_sub_;
 
     // Plugin handling
     boost::function<nav_core2::LocalPlanner::Ptr()> planner_loader_;

src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp

@@ -34,8 +34,8 @@
 
 #include <nav_core_adapter/global_planner_adapter.h>
 #include <nav_core_adapter/costmap_adapter.h>
-#include <nav_2d_utils/conversions.h>
+#include <robot_nav_2d_utils/conversions.h>
-#include <nav_2d_utils/tf_help.h>
+#include <robot_nav_2d_utils/tf_help.h>
 #include <nav_core2/exceptions.h>
 #include <boost/dll/alias.hpp>
 #include <memory>
@@ -78,18 +78,18 @@ namespace nav_core_adapter
     planner_->initialize(planner_name, costmap_robot_);
   }
 
-  nav_2d_msgs::Path2D GlobalPlannerAdapter::makePlan(const nav_2d_msgs::Pose2DStamped& start,
+  robot_nav_2d_msgs::Path2D GlobalPlannerAdapter::makePlan(const robot_nav_2d_msgs::Pose2DStamped& start,
-                                                      const nav_2d_msgs::Pose2DStamped& goal)
+                                                      const robot_nav_2d_msgs::Pose2DStamped& goal)
   {
-    geometry_msgs::PoseStamped start3d = nav_2d_utils::pose2DToPoseStamped(start),
+    geometry_msgs::PoseStamped start3d = robot_nav_2d_utils::pose2DToPoseStamped(start),
-                              goal3d = nav_2d_utils::pose2DToPoseStamped(goal);
+                              goal3d = robot_nav_2d_utils::pose2DToPoseStamped(goal);
     std::vector<geometry_msgs::PoseStamped> plan;
     bool ret = planner_->makePlan(start3d, goal3d, plan);
     if (!ret)
     {
       throw nav_core2::PlannerException("Generic Global Planner Error");
     }
-    return nav_2d_utils::posesToPath2D(plan);
+    return robot_nav_2d_utils::posesToPath2D(plan);
   }
 
   nav_core2::GlobalPlanner::Ptr GlobalPlannerAdapter::create() 

src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp

@@ -36,9 +36,9 @@
 #include <nav_core_adapter/local_planner_adapter.h>
 #include <nav_core_adapter/costmap_adapter.h>
 #include <nav_core_adapter/shared_pointers.h>
-#include <nav_2d_utils/conversions.h>
+#include <robot_nav_2d_utils/conversions.h>
-#include <nav_2d_utils/tf_help.h>
+#include <robot_nav_2d_utils/tf_help.h>
-#include <nav_2d_utils/parameters.h>
+#include <robot_nav_2d_utils/parameters.h>
 #include <nav_core2/exceptions.h>
 #include <boost/dll/alias.hpp>
 #include <boost/dll/import.hpp>
@@ -90,7 +90,7 @@ namespace nav_core_adapter
     }
     else
     {
-      planner_name = nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
+      planner_name = robot_nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
       adapter_nh_.setParam("planner_name", planner_name);
     }
     try
@@ -174,15 +174,15 @@ namespace nav_core_adapter
     }
 
     // Get the Pose
-    nav_2d_msgs::Pose2DStamped pose2d;
+    robot_nav_2d_msgs::Pose2DStamped pose2d;
     if (!getRobotPose(pose2d))
     {
       return false;
     }
 
     // Get the Velocity
-    nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
+    robot_nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
-    nav_2d_msgs::Twist2DStamped cmd_vel_2d;
+    robot_nav_2d_msgs::Twist2DStamped cmd_vel_2d;
     try
     {
       cmd_vel_2d = planner_->computeVelocityCommands(pose2d, velocity);
@@ -192,7 +192,7 @@ namespace nav_core_adapter
       std::cerr << "computeVelocityCommands exception: " << e.what() << std::endl;
       return false;
     }
-    cmd_vel = nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity);
+    cmd_vel = robot_nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity);
     return true;
   }
 
@@ -263,7 +263,7 @@ namespace nav_core_adapter
   geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
   {
     if (odom_sub_)
-      return nav_2d_utils::twist2Dto3D(odom_sub_->getTwist());
+      return robot_nav_2d_utils::twist2Dto3D(odom_sub_->getTwist());
     else
       throw std::runtime_error("Failed to get twist");
   }
@@ -326,11 +326,11 @@ namespace nav_core_adapter
   {
     boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
     // Get the Pose
-    nav_2d_msgs::Pose2DStamped pose2d;
+    robot_nav_2d_msgs::Pose2DStamped pose2d;
     if (!getRobotPose(pose2d))
       return false;
 
-    nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
+    robot_nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
     bool ret = planner_->isGoalReached(pose2d, velocity);
     if (ret)
     {
@@ -345,12 +345,12 @@ namespace nav_core_adapter
   bool LocalPlannerAdapter::setPlan(const std::vector<geometry_msgs::PoseStamped> &orig_global_plan)
   {
     boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
-    nav_2d_msgs::Path2D path = nav_2d_utils::posesToPath2D(orig_global_plan);
+    robot_nav_2d_msgs::Path2D path = robot_nav_2d_utils::posesToPath2D(orig_global_plan);
     try
     {
       if (path.poses.size() > 0)
       {
-        nav_2d_msgs::Pose2DStamped goal_pose;
+        robot_nav_2d_msgs::Pose2DStamped goal_pose;
         goal_pose = path.poses.back();
 
         if (!has_active_goal_ || hasGoalChanged(goal_pose))
@@ -370,7 +370,7 @@ namespace nav_core_adapter
     }
   }
 
-  bool LocalPlannerAdapter::hasGoalChanged(const nav_2d_msgs::Pose2DStamped &new_goal)
+  bool LocalPlannerAdapter::hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal)
   {
     if (last_goal_.header.frame_id != new_goal.header.frame_id ||
         last_goal_.header.stamp.toSec() != new_goal.header.stamp.toSec())
@@ -382,7 +382,7 @@ namespace nav_core_adapter
            last_goal_.pose.theta != new_goal.pose.theta;
   }
 
-  bool LocalPlannerAdapter::getRobotPose(nav_2d_msgs::Pose2DStamped &pose2d)
+  bool LocalPlannerAdapter::getRobotPose(robot_nav_2d_msgs::Pose2DStamped &pose2d)
   {
     geometry_msgs::PoseStamped current_pose;
     if (!costmap_robot_->getRobotPose(current_pose))
@@ -390,7 +390,7 @@ namespace nav_core_adapter
       std::cout << "Could not get robot pose" << std::endl;
       return false;
     }
-    pose2d = nav_2d_utils::poseStampedToPose2D(current_pose);
+    pose2d = robot_nav_2d_utils::poseStampedToPose2D(current_pose);
     return true;
   }