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

update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 09:08:41 +07:00
parent 4962cfbf49
commit 63cb260cb2
112 changed files with 456 additions and 456 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h
View File

@@ -4,10 +4,10 @@
#include <memory>
// geometry msgs
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Vector3.h>
#include <robot_geometry_msgs/Twist.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Pose2D.h>
#include <robot_geometry_msgs/Vector3.h>
#include <robot_nav_2d_msgs/Twist2DStamped.h>
// robot protocol msgs
@@ -49,7 +49,7 @@ namespace move_base_core
{
State navigation_state; // Current navigation state
std::string feed_back_str; // Description or debug message
geometry_msgs::Pose2D current_pose; // Robot’s current pose in 2D
robot_geometry_msgs::Pose2D current_pose; // Robot’s current pose in 2D
bool goal_checked; // Whether the current goal is verified
bool is_ready; // Robot is ready for commands
};
@@ -111,9 +111,9 @@ namespace move_base_core
* Positive moves forward, negative moves backward.
* @return A new PoseStamped offset from the input pose, in the given frame.
*/
inline geometry_msgs::PoseStamped offset_goal(const geometry_msgs::Pose2D &pose, const std::string &frame_id, double offset_distance)
inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::Pose2D &pose, const std::string &frame_id, double offset_distance)
{
geometry_msgs::PoseStamped goal;
robot_geometry_msgs::PoseStamped goal;
// goal.header.frame_id = frame_id;
// goal.header.stamp = robot::Time::now();
// goal.pose.position.x = pose.x + offset_distance * cos(pose.theta);
@@ -136,9 +136,9 @@ namespace move_base_core
* @param offset_distance Distance to offset along the current heading direction (in meters).
* @return A new PoseStamped offset from the original pose.
*/
inline geometry_msgs::PoseStamped offset_goal(const geometry_msgs::PoseStamped &pose, double offset_distance)
inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::PoseStamped &pose, double offset_distance)
{
geometry_msgs::Pose2D pose2d;
robot_geometry_msgs::Pose2D pose2d;
pose2d.x = pose.pose.position.x;
pose2d.y = pose.pose.position.y;
// pose2d.theta = tf2::getYaw(pose.pose.orientation);
@@ -184,13 +184,13 @@ namespace move_base_core
| P4(-0.3, -0.2) P1(0.3, -0.2)
+-------------------------------> X
std::vector<geometry_msgs::Point> footprint;
std::vector<robot_geometry_msgs::Point> footprint;
1. footprint.push_back(make_point(0.3, -0.2));
2. footprint.push_back(make_point(0.3, 0.2));
3. footprint.push_back(make_point(-0.3, 0.2));
4. footprint.push_back(make_point(-0.3, -0.2));
*/
virtual void setRobotFootprint(const std::vector<geometry_msgs::Point> &fprt) = 0;
virtual void setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt) = 0;
/**
* @brief Send a goal for the robot to navigate to.
@@ -199,7 +199,7 @@ namespace move_base_core
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if goal was accepted and sent successfully.
*/
virtual bool moveTo(const geometry_msgs::PoseStamped &goal,
virtual bool moveTo(const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
@@ -212,7 +212,7 @@ namespace move_base_core
* @return True if goal was accepted and sent successfully.
*/
virtual bool moveTo(const robot_protocol_msgs::Order &msg,
const geometry_msgs::PoseStamped &goal,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
@@ -225,7 +225,7 @@ namespace move_base_core
* @return True if docking command succeeded.
*/
virtual bool dockTo(const std::string &maker,
const geometry_msgs::PoseStamped &goal,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
@@ -238,7 +238,7 @@ namespace move_base_core
* @return True if docking command succeeded.
*/
virtual bool dockTo(const robot_protocol_msgs::Order &msg,
const geometry_msgs::PoseStamped &goal,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
@@ -248,7 +248,7 @@ namespace move_base_core
* @param xy_goal_tolerance Acceptable positional error (meters).
* @return True if command issued successfully.
*/
virtual bool moveStraightTo(const geometry_msgs::PoseStamped &goal,
virtual bool moveStraightTo(const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0) = 0;
/**
@@ -257,7 +257,7 @@ namespace move_base_core
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if rotation command was sent successfully.
*/
virtual bool rotateTo(const geometry_msgs::PoseStamped &goal,
virtual bool rotateTo(const robot_geometry_msgs::PoseStamped &goal,
double yaw_goal_tolerance = 0.0) = 0;
/**
@@ -280,28 +280,28 @@ namespace move_base_core
* @param linear Linear velocity vector (x, y, z).
* @return True if the command was accepted.
*/
virtual bool setTwistLinear(const geometry_msgs::Vector3 &linear) = 0;
virtual bool setTwistLinear(const robot_geometry_msgs::Vector3 &linear) = 0;
/**
* @brief Send limited angular velocity command.
* @param angular Angular velocity vector (x, y, z).
* @return True if the command was accepted.
*/
virtual bool setTwistAngular(const geometry_msgs::Vector3 &angular) = 0;
virtual bool setTwistAngular(const robot_geometry_msgs::Vector3 &angular) = 0;
/**
* @brief Get the robot’s pose as a PoseStamped.
* @param pose Output parameter with the robot’s current pose.
* @return True if pose was successfully retrieved.
*/
virtual bool getRobotPose(geometry_msgs::PoseStamped &pose) = 0;
virtual bool getRobotPose(robot_geometry_msgs::PoseStamped &pose) = 0;
/**
* @brief Get the robot’s pose as a 2D pose.
* @param pose Output parameter with the robot’s current 2D pose.
* @return True if pose was successfully retrieved.
*/
virtual bool getRobotPose(geometry_msgs::Pose2D &pose) = 0;
virtual bool getRobotPose(robot_geometry_msgs::Pose2D &pose) = 0;
/**
* @brief Get the robot’s twist.

16
src/Navigations/Cores/nav_core/include/nav_core/base_global_planner.h
View File

@@ -37,7 +37,7 @@
#ifndef NAV_CORE_BASE_GLOBAL_PLANNER_H
#define NAV_CORE_BASE_GLOBAL_PLANNER_H
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <robot_protocol_msgs/Order.h>
#include <memory>
@@ -58,8 +58,8 @@ namespace nav_core {
* @param plan The plan... filled by the planner
* @return True if a valid plan was found, false otherwise
*/
virtual bool makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan) = 0;
virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal, std::vector<robot_geometry_msgs::PoseStamped>& plan) = 0;
/**
* @brief Given a goal pose in the world, compute a plan
@@ -69,8 +69,8 @@ namespace nav_core {
* @param cost The plans calculated cost
* @return True if a valid plan was found, false otherwise
*/
virtual bool makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan,
virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal, std::vector<robot_geometry_msgs::PoseStamped>& plan,
double& cost)
{
cost = 0;
@@ -85,9 +85,9 @@ namespace nav_core {
* @return True if a valid plan was found, false otherwise
*/
virtual bool makePlan(const robot_protocol_msgs::Order& msg,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan)
const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
std::vector<robot_geometry_msgs::PoseStamped>& plan)
{
return false;
}

18
src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h
View File

@@ -37,8 +37,8 @@
#ifndef NAV_CORE_BASE_LOCAL_PLANNER_H
#define NAV_CORE_BASE_LOCAL_PLANNER_H
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Twist.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <tf3/buffer_core.h>
#include <memory>
@@ -58,7 +58,7 @@ namespace nav_core
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid velocity command was found, false otherwise
*/
virtual bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel) = 0;
virtual bool computeVelocityCommands(robot_geometry_msgs::Twist &cmd_vel) = 0;
/**
* @brief Swap object will be injected interjace nav_core::BaseLocalPlanner
@@ -72,34 +72,34 @@ namespace nav_core
* @param linear the command velocity
* @return True if set is done, false otherwise
*/
virtual bool setTwistLinear(geometry_msgs::Vector3 linear) { return false; }
virtual bool setTwistLinear(robot_geometry_msgs::Vector3 linear) { return false; }
/**
* @brief Get velocity for x, y asix of the robot
* @param linear The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistLinear(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
virtual robot_geometry_msgs::Vector3 getTwistLinear(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
/**
* @brief Set velocity theta for z asix of the robot
* @param angular the command velocity
* @return True if set is done, false otherwise
*/
virtual bool setTwistAngular(geometry_msgs::Vector3 angular) { return false; }
virtual bool setTwistAngular(robot_geometry_msgs::Vector3 angular) { return false; }
/**
* @brief Get velocity theta for z asix of the robot
* @param direct The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistAngular(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
virtual robot_geometry_msgs::Vector3 getTwistAngular(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
/**
* @brief Get velocity for x, y asix of the robot
* @return The current velocity
*/
virtual geometry_msgs::Twist getActualTwist() { throw std::runtime_error("Function getActualTwist is not Support."); }
virtual robot_geometry_msgs::Twist getActualTwist() { throw std::runtime_error("Function getActualTwist is not Support."); }
/**
* @brief Check if the kinematic parameters are currently locked
@@ -130,7 +130,7 @@ namespace nav_core
* @param plan The plan to pass to the local planner
* @return True if the plan was updated successfully, false otherwise
*/
virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan) = 0;
virtual bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped> &plan) = 0;
/**
* @brief Constructs the local planner

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

@@ -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>&)`|`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.|
|`bool makePlan(const robot_geometry_msgs::PoseStamped&, const robot_geometry_msgs::PoseStamped&, std::vector<robot_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).
@@ -39,8 +39,8 @@ Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-pl
|---|--|---|
|`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 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 setPlan(const std::vector<robot_geometry_msgs::PoseStamped>&)`|`setPlan(const robot_nav_2d_msgs::Path2D&)`||
|`bool computeVelocityCommands(robot_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

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

@@ -40,7 +40,7 @@
#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 <robot_geometry_msgs/Twist.h>
#include <string>
#include <costmap_2d/costmap_2d_robot.h>
#include <memory>
@@ -110,34 +110,34 @@ namespace nav_core2
* @param linear the velocity command
* @return True if set is done, false otherwise
*/
virtual bool setTwistLinear(geometry_msgs::Vector3 linear) { return true; }
virtual bool setTwistLinear(robot_geometry_msgs::Vector3 linear) { return true; }
/**
* @brief get velocity for x, y asix of the robot
* @param linear The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistLinear(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
virtual robot_geometry_msgs::Vector3 getTwistLinear(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
/**
* @brief Set velocity theta for z asix of the robot
* @param angular the command velocity
* @return True if set is done, false otherwise
*/
virtual bool setTwistAngular(geometry_msgs::Vector3 angular) { return false; }
virtual bool setTwistAngular(robot_geometry_msgs::Vector3 angular) { return false; }
/**
* @brief Get velocity theta for z asix of the robot
* @param direct The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistAngular(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
virtual robot_geometry_msgs::Vector3 getTwistAngular(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
/**
* @brief Get velocity for x, y asix of the robot
* @return The current velocity
*/
virtual geometry_msgs::Twist getActualTwist() { throw std::runtime_error("Function getActualTwist is not Support."); }
virtual robot_geometry_msgs::Twist getActualTwist() { throw std::runtime_error("Function getActualTwist is not Support."); }
/**
* @brief Check if the kinematic parameters are currently locked

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

@@ -84,7 +84,7 @@ namespace nav_core_adapter
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid velocity command was found, false otherwise
*/
bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel) override;
bool computeVelocityCommands(robot_geometry_msgs::Twist &cmd_vel) override;
/**
* @brief Swap object will be injected interjace nav_core::BaseLocalPlanner
@@ -98,34 +98,34 @@ namespace nav_core_adapter
* @param linear The velocity command
* @return True if set is done, false otherwise
*/
virtual bool setTwistLinear(geometry_msgs::Vector3 linear) override;
virtual bool setTwistLinear(robot_geometry_msgs::Vector3 linear) override;
/**
* @brief get velocity for x, y asix of the robot
* @param linear The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistLinear(bool direct) override;
virtual robot_geometry_msgs::Vector3 getTwistLinear(bool direct) override;
/**
* @brief Set velocity theta for z asix of the robot
* @param angular the command velocity
* @return True if set is done, false otherwise
*/
virtual bool setTwistAngular(geometry_msgs::Vector3 angular) override;
virtual bool setTwistAngular(robot_geometry_msgs::Vector3 angular) override;
/**
* @brief Get velocity theta for z asix of the robot
* @param direct The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistAngular(bool direct) override;
virtual robot_geometry_msgs::Vector3 getTwistAngular(bool direct) override;
/**
* @brief Get actual velocity for x, y asix of the robot
* @return The current velocity
*/
virtual geometry_msgs::Twist getActualTwist() override;
virtual robot_geometry_msgs::Twist getActualTwist() override;
/**
* @brief Check if the kinematic parameters are currently locked
@@ -156,7 +156,7 @@ namespace nav_core_adapter
* @param plan The plan to pass to the local planner
* @return True if the plan was updated successfully, false otherwise
*/
bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan) override;
bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped> &plan) override;
/**
* @brief Create a new LocalPlannerAdapter

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

@@ -81,9 +81,9 @@ namespace nav_core_adapter
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 = robot_nav_2d_utils::pose2DToPoseStamped(start),
robot_geometry_msgs::PoseStamped start3d = robot_nav_2d_utils::pose2DToPoseStamped(start),
goal3d = robot_nav_2d_utils::pose2DToPoseStamped(goal);
std::vector<geometry_msgs::PoseStamped> plan;
std::vector<robot_geometry_msgs::PoseStamped> plan;
bool ret = planner_->makePlan(start3d, goal3d, plan);
if (!ret)
{

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

@@ -165,7 +165,7 @@ namespace nav_core_adapter
/**
* @brief Collect the additional information needed by nav_core2 and call the child computeVelocityCommands
*/
bool LocalPlannerAdapter::computeVelocityCommands(geometry_msgs::Twist &cmd_vel)
bool LocalPlannerAdapter::computeVelocityCommands(robot_geometry_msgs::Twist &cmd_vel)
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
if (!has_active_goal_)
@@ -196,7 +196,7 @@ namespace nav_core_adapter
return true;
}
bool LocalPlannerAdapter::setTwistLinear(geometry_msgs::Vector3 linear)
bool LocalPlannerAdapter::setTwistLinear(robot_geometry_msgs::Vector3 linear)
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
try
@@ -212,7 +212,7 @@ namespace nav_core_adapter
}
}
geometry_msgs::Vector3 LocalPlannerAdapter::getTwistLinear(bool direct)
robot_geometry_msgs::Vector3 LocalPlannerAdapter::getTwistLinear(bool direct)
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
try
@@ -228,7 +228,7 @@ namespace nav_core_adapter
}
}
bool LocalPlannerAdapter::setTwistAngular(geometry_msgs::Vector3 angular)
bool LocalPlannerAdapter::setTwistAngular(robot_geometry_msgs::Vector3 angular)
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
try
@@ -244,7 +244,7 @@ namespace nav_core_adapter
}
}
geometry_msgs::Vector3 LocalPlannerAdapter::getTwistAngular(bool direct)
robot_geometry_msgs::Vector3 LocalPlannerAdapter::getTwistAngular(bool direct)
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
try
@@ -260,7 +260,7 @@ namespace nav_core_adapter
}
}
geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
robot_geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
{
if (odom_sub_)
return robot_nav_2d_utils::twist2Dto3D(odom_sub_->getTwist());
@@ -342,7 +342,7 @@ namespace nav_core_adapter
/**
* @brief Convert from 2d to 3d and call child setPlan
*/
bool LocalPlannerAdapter::setPlan(const std::vector<geometry_msgs::PoseStamped> &orig_global_plan)
bool LocalPlannerAdapter::setPlan(const std::vector<robot_geometry_msgs::PoseStamped> &orig_global_plan)
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
robot_nav_2d_msgs::Path2D path = robot_nav_2d_utils::posesToPath2D(orig_global_plan);
@@ -384,7 +384,7 @@ namespace nav_core_adapter
bool LocalPlannerAdapter::getRobotPose(robot_nav_2d_msgs::Pose2DStamped &pose2d)
{
geometry_msgs::PoseStamped current_pose;
robot_geometry_msgs::PoseStamped current_pose;
if (!costmap_robot_->getRobotPose(current_pose))
{
std::cout << "Could not get robot pose" << std::endl;

2
src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp
View File

@@ -43,7 +43,7 @@ TEST(LocalPlannerAdapter, unload_local_planner)
// This empty transform is added to satisfy the constructor of
// Costmap2DROBOT, which waits for the transform from map to base_link
// to become available.
geometry_msgs::TransformStamped base_rel_map;
robot_geometry_msgs::TransformStamped base_rel_map;
base_rel_map.child_frame_id = "base_link";
base_rel_map.header.frame_id = "map";
base_rel_map.transform.rotation.w = 1.0;

52
src/Navigations/Packages/move_base/include/move_base/move_base.h
View File

@@ -82,7 +82,7 @@ namespace move_base
* @param fprt A vector of points representing the robot's footprint polygon.
* The points should be ordered counter-clockwise.
*/
virtual void setRobotFootprint(const std::vector<geometry_msgs::Point> &fprt) override;
virtual void setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt) override;
/**
* @brief Send a goal for the robot to navigate to.
@@ -91,7 +91,7 @@ namespace move_base
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if goal was accepted and sent successfully.
*/
virtual bool moveTo(const geometry_msgs::PoseStamped &goal,
virtual bool moveTo(const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) override;
@@ -104,7 +104,7 @@ namespace move_base
* @return True if goal was accepted and sent successfully.
*/
virtual bool moveTo(const robot_protocol_msgs::Order &msg,
const geometry_msgs::PoseStamped &goal,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) override;
@@ -117,7 +117,7 @@ namespace move_base
* @return True if docking command succeeded.
*/
virtual bool dockTo(const std::string &maker,
const geometry_msgs::PoseStamped &goal,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) override;
@@ -130,7 +130,7 @@ namespace move_base
* @return True if docking command succeeded.
*/
virtual bool dockTo(const robot_protocol_msgs::Order &msg,
const geometry_msgs::PoseStamped &goal,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) override;
@@ -141,7 +141,7 @@ namespace move_base
* @param xy_goal_tolerance Acceptable positional error (meters).
* @return True if command issued successfully.
*/
virtual bool moveStraightTo(const geometry_msgs::PoseStamped &goal,
virtual bool moveStraightTo(const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0) override;
/**
@@ -150,7 +150,7 @@ namespace move_base
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if rotation command was sent successfully.
*/
virtual bool rotateTo(const geometry_msgs::PoseStamped &goal,
virtual bool rotateTo(const robot_geometry_msgs::PoseStamped &goal,
double yaw_goal_tolerance = 0.0) override;
/**
@@ -173,28 +173,28 @@ namespace move_base
* @param linear Linear velocity vector (x, y, z).
* @return True if the command was accepted.
*/
virtual bool setTwistLinear(const geometry_msgs::Vector3 &linear) override;
virtual bool setTwistLinear(const robot_geometry_msgs::Vector3 &linear) override;
/**
* @brief Send limited angular velocity command.
* @param angular Angular velocity vector (x, y, z).
* @return True if the command was accepted.
*/
virtual bool setTwistAngular(const geometry_msgs::Vector3 &angular) override;
virtual bool setTwistAngular(const robot_geometry_msgs::Vector3 &angular) override;
/**
* @brief Get the robot’s pose as a PoseStamped.
* @param pose Output parameter with the robot’s current pose.
* @return True if pose was successfully retrieved.
*/
virtual bool getRobotPose(geometry_msgs::PoseStamped &pose) override;
virtual bool getRobotPose(robot_geometry_msgs::PoseStamped &pose) override;
/**
* @brief Get the robot’s pose as a 2D pose.
* @param pose Output parameter with the robot’s current 2D pose.
* @return True if pose was successfully retrieved.
*/
virtual bool getRobotPose(geometry_msgs::Pose2D &pose) override;
virtual bool getRobotPose(robot_geometry_msgs::Pose2D &pose) override;
/**
@@ -229,7 +229,7 @@ namespace move_base
* @param goal A reference to the goal to pursue
* @return True if processing of the goal is done, false otherwise
*/
bool executeCycle(geometry_msgs::PoseStamped &goal);
bool executeCycle(robot_geometry_msgs::PoseStamped &goal);
/**
* @brief Set velocity for yaw goal tolerance of the robot
@@ -255,7 +255,7 @@ namespace move_base
* @param plan Will be filled in with the plan made by the planner
* @return True if planning succeeds, false otherwise
*/
bool makePlan(const geometry_msgs::PoseStamped &goal, std::vector<geometry_msgs::PoseStamped> &plan);
bool makePlan(const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan);
/**
* @brief Load the recovery behaviors for the navigation stack from the parameter server
@@ -285,19 +285,19 @@ namespace move_base
*/
void resetState();
// void goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal);
// void goalCB(const robot_geometry_msgs::PoseStamped::ConstPtr &goal);
void planThread();
// void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal);
bool isQuaternionValid(const geometry_msgs::Quaternion &q);
bool isQuaternionValid(const robot_geometry_msgs::Quaternion &q);
bool getRobotPose(geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap);
bool getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap);
double distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2);
double distance(const robot_geometry_msgs::PoseStamped &p1, const robot_geometry_msgs::PoseStamped &p2);
geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped &goal_pose_msg);
robot_geometry_msgs::PoseStamped goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg);
// /**
// * @brief This is used to wake the planner at periodic intervals.
@@ -324,7 +324,7 @@ namespace move_base
unsigned int recovery_index_;
bool recovery_behavior_enabled_;
geometry_msgs::PoseStamped global_pose_;
robot_geometry_msgs::PoseStamped global_pose_;
double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
double planner_patience_, controller_patience_;
int32_t max_planning_retries_;
@@ -341,18 +341,18 @@ namespace move_base
RecoveryTrigger recovery_trigger_;
robot::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_;
geometry_msgs::PoseStamped oscillation_pose_;
robot_geometry_msgs::PoseStamped oscillation_pose_;
// set up plan triple buffer
std::vector<geometry_msgs::PoseStamped> *planner_plan_;
std::vector<geometry_msgs::PoseStamped> *latest_plan_;
std::vector<geometry_msgs::PoseStamped> *controller_plan_;
std::vector<robot_geometry_msgs::PoseStamped> *planner_plan_;
std::vector<robot_geometry_msgs::PoseStamped> *latest_plan_;
std::vector<robot_geometry_msgs::PoseStamped> *controller_plan_;
// set up the planner's thread
bool runPlanner_;
boost::recursive_mutex planner_mutex_;
boost::condition_variable_any planner_cond_;
geometry_msgs::PoseStamped planner_goal_;
robot_geometry_msgs::PoseStamped planner_goal_;
boost::thread *planner_thread_;
move_base::MoveBaseConfig last_config_;
@@ -363,8 +363,8 @@ namespace move_base
bool cancel_ctr_;
bool pause_ctr_, paused_;
double min_approach_linear_velocity_{0.1};
geometry_msgs::Vector3 old_linear_fwd_, old_linear_bwd_;
geometry_msgs::Vector3 old_angular_fwd_, old_angular_rev_;
robot_geometry_msgs::Vector3 old_linear_fwd_, old_linear_bwd_;
robot_geometry_msgs::Vector3 old_angular_fwd_, old_angular_rev_;
double original_xy_goal_tolerance_, original_yaw_goal_tolerance_;
// defined planner name

62
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -152,9 +152,9 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
private_nh_.getParam("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
min_approach_linear_velocity_ *= 1.2;
// set up plan triple buffer
planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
planner_plan_ = new std::vector<robot_geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<robot_geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<robot_geometry_msgs::PoseStamped>();
// set up the planner's thread
planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this));
@@ -357,7 +357,7 @@ void move_base::MoveBase::swapPlanner(std::string base_global_planner)
{
}
void move_base::MoveBase::setRobotFootprint(const std::vector<geometry_msgs::Point> &fprt)
void move_base::MoveBase::setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt)
{
if (planner_costmap_robot_)
planner_costmap_robot_->setUnpaddedRobotFootprint(fprt);
@@ -366,7 +366,7 @@ void move_base::MoveBase::setRobotFootprint(const std::vector<geometry_msgs::Poi
controller_costmap_robot_->setUnpaddedRobotFootprint(fprt);
}
bool move_base::MoveBase::moveTo(const geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance)
bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance)
{
if (setup_)
{
@@ -427,14 +427,14 @@ bool move_base::MoveBase::moveTo(const geometry_msgs::PoseStamped &goal, double
}
bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
const geometry_msgs::PoseStamped &goal,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
return false;
}
bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::PoseStamped &goal,
bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
std::string maker_sources;
@@ -521,13 +521,13 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::
}
bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
const geometry_msgs::PoseStamped &goal,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
return false;
}
bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal, double xy_goal_tolerance)
bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance)
{
if (setup_)
{
@@ -575,7 +575,7 @@ bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal,
return true;
}
bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, double yaw_goal_tolerance)
bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal, double yaw_goal_tolerance)
{
if (setup_)
{
@@ -619,7 +619,7 @@ bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, doubl
}
if (cancel_ctr_)
cancel_ctr_ = false;
geometry_msgs::PoseStamped pose;
robot_geometry_msgs::PoseStamped pose;
if (!this->getRobotPose(pose))
{
if (nav_feedback_)
@@ -693,7 +693,7 @@ void move_base::MoveBase::cancel()
cancel_ctr_ = true;
}
bool move_base::MoveBase::getRobotPose(geometry_msgs::PoseStamped &pose)
bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &pose)
{
if (!initialized_)
return false;
@@ -701,11 +701,11 @@ bool move_base::MoveBase::getRobotPose(geometry_msgs::PoseStamped &pose)
return true;
}
bool move_base::MoveBase::getRobotPose(geometry_msgs::Pose2D &pose)
bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::Pose2D &pose)
{
if (!initialized_)
return false;
geometry_msgs::PoseStamped global_pose;
robot_geometry_msgs::PoseStamped global_pose;
this->getRobotPose(global_pose, planner_costmap_robot_);
pose.x = global_pose.pose.position.x;
pose.y = global_pose.pose.position.y;
@@ -720,7 +720,7 @@ bool move_base::MoveBase::getRobotPose(geometry_msgs::Pose2D &pose)
return true;
}
bool move_base::MoveBase::setTwistLinear(const geometry_msgs::Vector3 &linear)
bool move_base::MoveBase::setTwistLinear(const robot_geometry_msgs::Vector3 &linear)
{
try
{
@@ -747,7 +747,7 @@ bool move_base::MoveBase::setTwistLinear(const geometry_msgs::Vector3 &linear)
}
}
bool move_base::MoveBase::setTwistAngular(const geometry_msgs::Vector3 &angular)
bool move_base::MoveBase::setTwistAngular(const robot_geometry_msgs::Vector3 &angular)
{
if (tc_ && !cancel_ctr_)
{
@@ -799,7 +799,7 @@ void move_base::MoveBase::setXyGoalTolerance(double xy_goal_tolerance)
// }
bool move_base::MoveBase::makePlan(const geometry_msgs::PoseStamped &goal, std::vector<geometry_msgs::PoseStamped> &plan)
bool move_base::MoveBase::makePlan(const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan)
{
// make sure to set the plan to be empty initially
plan.clear();
@@ -814,14 +814,14 @@ bool move_base::MoveBase::makePlan(const geometry_msgs::PoseStamped &goal, std::
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_robot_->getCostmap()->getMutex()));
// get the starting pose of the robot
geometry_msgs::PoseStamped global_pose;
robot_geometry_msgs::PoseStamped global_pose;
if (!getRobotPose(global_pose, planner_costmap_robot_))
{
std::cerr << "Unable to get starting pose of robot, unable to create global plan" << std::endl;
return false;
}
const geometry_msgs::PoseStamped &start = global_pose;
const robot_geometry_msgs::PoseStamped &start = global_pose;
// if the planner fails or returns a zero length plan, planning failed
if (!planner_->makePlan(start, goal, plan) || plan.empty())
@@ -1002,15 +1002,15 @@ void move_base::MoveBase::loadDefaultRecoveryBehaviors()
void move_base::MoveBase::clearCostmapWindows(double size_x, double size_y)
{
geometry_msgs::PoseStamped global_pose;
robot_geometry_msgs::PoseStamped global_pose;
// clear the planner's costmap
getRobotPose(global_pose, planner_costmap_robot_);
std::vector<geometry_msgs::Point> clear_poly;
std::vector<robot_geometry_msgs::Point> clear_poly;
double x = global_pose.pose.position.x;
double y = global_pose.pose.position.y;
geometry_msgs::Point pt;
robot_geometry_msgs::Point pt;
pt.x = x - size_x / 2;
pt.y = y - size_y / 2;
@@ -1084,7 +1084,7 @@ void move_base::MoveBase::resetState()
}
}
// void goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal);
// void goalCB(const robot_geometry_msgs::PoseStamped::ConstPtr &goal);
void move_base::MoveBase::planThread()
{
@@ -1106,7 +1106,7 @@ void move_base::MoveBase::planThread()
// robot::Time start_time = robot::Time::now();
// // time to plan! get a copy of the goal and unlock the mutex
// geometry_msgs::PoseStamped temp_goal = planner_goal_;
// robot_geometry_msgs::PoseStamped temp_goal = planner_goal_;
// lock.unlock();
// std::cout << "Planning..." << std::endl;
// // run planner
@@ -1118,7 +1118,7 @@ void move_base::MoveBase::planThread()
// {
// std::cout << "Got Plan with " << planner_plan_->size() << " points!" << std::endl;
// // pointer swap the plans under mutex (the controller will pull from latest_plan_)
// std::vector<geometry_msgs::PoseStamped> *temp_plan = planner_plan_;
// std::vector<robot_geometry_msgs::PoseStamped> *temp_plan = planner_plan_;
// lock.lock();
// planner_plan_ = latest_plan_;
@@ -1185,7 +1185,7 @@ void move_base::MoveBase::planThread()
// void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal);
bool move_base::MoveBase::isQuaternionValid(const geometry_msgs::Quaternion &q)
bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternion &q)
{
// first we need to check if the quaternion has nan's or infs
if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
@@ -1219,11 +1219,11 @@ bool move_base::MoveBase::isQuaternionValid(const geometry_msgs::Quaternion &q)
return true;
}
bool move_base::MoveBase::getRobotPose(geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap)
bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap)
{
tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
geometry_msgs::PoseStamped robot_pose;
robot_geometry_msgs::PoseStamped robot_pose;
tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose);
robot_pose.header.frame_id = robot_base_frame_;
robot_pose.header.stamp = robot::Time(); // latest available
@@ -1263,15 +1263,15 @@ bool move_base::MoveBase::getRobotPose(geometry_msgs::PoseStamped &global_pose,
return true;
}
double move_base::MoveBase::distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2)
double move_base::MoveBase::distance(const robot_geometry_msgs::PoseStamped &p1, const robot_geometry_msgs::PoseStamped &p2)
{
return hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y);
}
geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped &goal_pose_msg)
robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg)
{
std::string global_frame = planner_costmap_robot_->getGlobalFrameID();
geometry_msgs::PoseStamped goal_pose, global_pose;
robot_geometry_msgs::PoseStamped goal_pose, global_pose;
goal_pose = goal_pose_msg;
goal_pose.header.stamp = robot::Time(); // latest available