HiepLM/pnkx_nav_core
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
HiepLM 2025-12-29 14:05:57 +07:00
parent 307a9c84f9
commit 6411f729b5
15 changed files with 65 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/Libraries/costmap_2d

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

1
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h
View File

@ -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
*/

4
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h
View File

@ -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

1
src/Libraries/robot_nav_2d_utils/src/bounds.cpp
View File

@ -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)

6
src/Libraries/robot_nav_2d_utils/src/conversions.cpp
View File

@ -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

2
src/Navigations/Cores/nav_core2/CMakeLists.txt
View File

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

12
src/Navigations/Cores/nav_core2/README.md
View File

@ -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`
|`bool setPlan(const std::vector<geometry_msgs::PoseStamped>&)`|`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<geometry_msgs::PoseStamped>&)`|`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.

16
src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h
View File

@ -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) {}
};

8
src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h
View File

@ -36,8 +36,8 @@
#include <nav_core2/common.h>
#include <nav_core2/costmap.h>
#include <nav_2d_msgs/Path2D.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_2d_msgs/Path2D.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,
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

18
src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h
View File

@ -36,10 +36,10 @@
#include <nav_core2/common.h>
#include <nav_core2/costmap.h>
#include <nav_2d_msgs/Path2D.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <nav_2d_msgs/Twist2D.h>
#include <nav_2d_msgs/Twist2DStamped.h>
#include <robot_nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_2d_msgs/Twist2D.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,
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

2
src/Navigations/Cores/nav_core_adapter/CMakeLists.txt
View File

@ -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)

4
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h
View File

@ -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();

10
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h
View File

@ -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_;

14
src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp
View File

@ -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 <nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/conversions.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,
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<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()

32
src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp
View File

@ -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 <nav_2d_utils/tf_help.h>
#include <nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.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();
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<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;
}