From 6411f729b5bb11857d79f375f588ad464d609739 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Mon, 29 Dec 2025 14:05:57 +0700 Subject: [PATCH] update --- src/Libraries/costmap_2d | 2 +- .../include/robot_nav_2d_utils/bounds.h | 1 - .../include/robot_nav_2d_utils/conversions.h | 4 +-- .../robot_nav_2d_utils/src/bounds.cpp | 1 - .../robot_nav_2d_utils/src/conversions.cpp | 6 ++-- .../Cores/nav_core2/CMakeLists.txt | 2 +- src/Navigations/Cores/nav_core2/README.md | 12 +++---- .../nav_core2/include/nav_core2/exceptions.h | 16 +++++----- .../include/nav_core2/global_planner.h | 8 ++--- .../include/nav_core2/local_planner.h | 18 +++++------ .../Cores/nav_core_adapter/CMakeLists.txt | 2 +- .../nav_core_adapter/global_planner_adapter.h | 4 +-- .../nav_core_adapter/local_planner_adapter.h | 10 +++--- .../src/global_planner_adapter.cpp | 14 ++++---- .../src/local_planner_adapter.cpp | 32 +++++++++---------- 15 files changed, 65 insertions(+), 67 deletions(-) diff --git a/src/Libraries/costmap_2d b/src/Libraries/costmap_2d index 0344c31..a28f05c 160000 --- a/src/Libraries/costmap_2d +++ b/src/Libraries/costmap_2d @@ -1 +1 @@ -Subproject commit 0344c31e5b4185da41bfb253db720d9c91b802bd +Subproject commit a28f05c6d5cb6cf2d8c4ffb52a637ecfa2878db2 diff --git a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h index 043fa77..05f6e21 100755 --- a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h +++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h @@ -39,7 +39,6 @@ #include #include -using namespace robot; /** * @brief A set of utility functions for Bounds objects interacting with other messages/types */ diff --git a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h index a4dbda6..17d1d1b 100755 --- a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h +++ b/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_core2::UIntBounds fromMsg(const ::robot_nav_2d_msgs::UIntBounds &msg); + robot_nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds &bounds); + nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg); } // namespace robot_nav_2d_utils diff --git a/src/Libraries/robot_nav_2d_utils/src/bounds.cpp b/src/Libraries/robot_nav_2d_utils/src/bounds.cpp index 728edfe..241317e 100755 --- a/src/Libraries/robot_nav_2d_utils/src/bounds.cpp +++ b/src/Libraries/robot_nav_2d_utils/src/bounds.cpp @@ -38,7 +38,6 @@ #include #include -using namespace robot; namespace robot_nav_2d_utils { nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info) diff --git a/src/Libraries/robot_nav_2d_utils/src/conversions.cpp b/src/Libraries/robot_nav_2d_utils/src/conversions.cpp index 666ebcc..be735cc 100755 --- a/src/Libraries/robot_nav_2d_utils/src/conversions.cpp +++ b/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 diff --git a/src/Navigations/Cores/nav_core2/CMakeLists.txt b/src/Navigations/Cores/nav_core2/CMakeLists.txt index c11242c..2d055de 100755 --- a/src/Navigations/Cores/nav_core2/CMakeLists.txt +++ b/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 ) diff --git a/src/Navigations/Cores/nav_core2/README.md b/src/Navigations/Cores/nav_core2/README.md index dc9250e..b5fe550 100755 --- a/src/Navigations/Cores/nav_core2/README.md +++ b/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&)`|`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&)`|`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` -|`bool setPlan(const std::vector&)`|`setPlan(const 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 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. | +|(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&)`|`setPlan(const robot_nav_2d_msgs::Path2D&)`|| +|`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 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. diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h b/src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h index f461434..bf79037 100755 --- a/src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h +++ b/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 +#include #include #include #include @@ -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) {} }; diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h b/src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h index 5657018..35a1754 100755 --- a/src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h +++ b/src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h @@ -36,8 +36,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -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, - const nav_2d_msgs::Pose2DStamped& goal) = 0; + virtual robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped& start, + const robot_nav_2d_msgs::Pose2DStamped& goal) = 0; }; } // namespace nav_core2 diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h b/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h index c20e283..a58289c 100755 --- a/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h +++ b/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h @@ -36,10 +36,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -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, - const nav_2d_msgs::Twist2D &velocity) = 0; + virtual robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose, + 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 diff --git a/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt b/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt index f610875..ea5aa66 100755 --- a/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt +++ b/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) diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h index 79edd91..4ab4d97 100755 --- a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h +++ b/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, - const nav_2d_msgs::Pose2DStamped& goal) override; + robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped& start, + const robot_nav_2d_msgs::Pose2DStamped& goal) override; static nav_core2::GlobalPlanner::Ptr create(); diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h index cbbc3b7..da40894 100755 --- a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h +++ b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include #include #include @@ -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 odom_sub_; + std::shared_ptr odom_sub_; // Plugin handling boost::function planner_loader_; diff --git a/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp b/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp index 643cbbf..44f56aa 100755 --- a/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp +++ b/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp @@ -34,8 +34,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -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, - const nav_2d_msgs::Pose2DStamped& goal) + robot_nav_2d_msgs::Path2D GlobalPlannerAdapter::makePlan(const robot_nav_2d_msgs::Pose2DStamped& start, + const robot_nav_2d_msgs::Pose2DStamped& goal) { - geometry_msgs::PoseStamped start3d = nav_2d_utils::pose2DToPoseStamped(start), - goal3d = nav_2d_utils::pose2DToPoseStamped(goal); + geometry_msgs::PoseStamped start3d = robot_nav_2d_utils::pose2DToPoseStamped(start), + goal3d = robot_nav_2d_utils::pose2DToPoseStamped(goal); std::vector 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() diff --git a/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp b/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp index 43ef0ba..72d957f 100755 --- a/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp +++ b/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp @@ -36,9 +36,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -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(); - nav_2d_msgs::Twist2DStamped cmd_vel_2d; + robot_nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist(); + 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 &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; }