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 2026-01-16 15:13:14 +07:00
parent 0f58db3481
commit ebda1f81a1
33 changed files with 3597 additions and 515 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
config/costmap_common_params.yaml
View File

@ -1,3 +1,10 @@
position_planner_name: PNKXLocalPlanner
docking_planner_name: PNKXDockingLocalPlanner
go_straight_planner_name: PNKXGoStraightLocalPlanner
rotate_planner_name: PNKXRotateLocalPlanner
base_local_planner: LocalPlannerAdapter
base_global_planner: CustomPlanner
robot_base_frame: base_footprint
transform_tolerance: 1.0
obstacle_range: 3.0

1
config/custom_global_params.yaml
View File

@ -1,4 +1,3 @@
base_global_planner: CustomPlanner
CustomPlanner:
library_path: libcustom_planner
environment_type: XYThetaLattice

24
config/pnkx_local_planner_params.yaml
View File

@ -1,10 +1,4 @@
position_planner_name: PNKXLocalPlanner
docking_planner_name: PNKXDockingLocalPlanner
go_straight_planner_name: PNKXGoStraightLocalPlanner
rotate_planner_name: PNKXRotateLocalPlanner
base_local_planner: LocalPlannerAdapter
LocalPlannerAdapter:
library_path: liblocal_planner_adapter
yaw_goal_tolerance: 0.017
@ -92,7 +86,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
# only when false:
lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
# only when true:
min_lookahead_dist: 1.0 # The minimum lookahead distance (m) threshold. (default: 0.3)
min_lookahead_dist: 0.6 # The minimum lookahead distance (m) threshold. (default: 0.3)
max_lookahead_dist: 2.0 # The maximum lookahead distance (m) threshold. (default: 0.9)
lookahead_time: 1.6 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2)
@ -120,6 +114,20 @@ MKTAlgorithmDiffPredictiveTrajectory:
cost_scaling_dist: 0.2 # (default: 0.6)
cost_scaling_gain: 2.0 # (default: 1.0)
use_mpc: true
mpc_horizon: 10
mpc_dt: 0.1
mpc_w_pos: 6.0
mpc_w_theta: 2.0
mpc_w_v: 0.2
mpc_w_w: 0.2
mpc_w_dv: 0.4
mpc_w_dw: 0.4
mpc_iterations: 3
mpc_step: 0.15
mpc_eps: 0.001
MKTAlgorithmDiffGoStraight:
library_path: libmkt_algorithm_diff
avoid_obstacles: false
@ -133,7 +141,7 @@ MKTAlgorithmDiffGoStraight:
# only when false:
lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
# only when true:
min_lookahead_dist: 0.8 # The minimum lookahead distance (m) threshold. (default: 0.3)
min_lookahead_dist: 1.0 # The minimum lookahead distance (m) threshold. (default: 0.3)
max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9)
lookahead_time: 2.5 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2)

1
config/two_points_global_params.yaml
View File

@ -1,4 +1,3 @@
base_global_planner: TwoPointsPlanner
TwoPointsPlanner:
library_path: libtwo_points_planner
lethal_obstacle: 20

28
examples/CSharpExample.cs
View File

@ -53,6 +53,14 @@ namespace NavigationExample
public double theta;
}
[StructLayout(LayoutKind.Sequential)]
public struct Twist2D
{
public double x;
public double y;
public double theta;
}
[StructLayout(LayoutKind.Sequential)]
public struct Quaternion
{
@ -93,6 +101,13 @@ namespace NavigationExample
public Pose pose;
}
[StructLayout(LayoutKind.Sequential)]
public struct Twist2DStamped
{
public Header header;
public Twist2D velocity;
}
[StructLayout(LayoutKind.Sequential)]
public struct Vector3
{
@ -180,6 +195,14 @@ namespace NavigationExample
public static extern bool navigation_set_robot_footprint(
IntPtr handle, Point[] points, UIntPtr point_count);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_robot_footprint(
IntPtr handle, out IntPtr out_points, out UIntPtr out_count);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void navigation_free_points(IntPtr points);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_move_to(
@ -231,6 +254,11 @@ namespace NavigationExample
public static extern bool navigation_get_robot_pose_2d(
IntPtr handle, ref Pose2D out_pose);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_twist(
IntPtr handle, ref Twist2DStamped out_twist);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_feedback(

28
examples/NavigationExample/Program.cs
View File

@ -53,6 +53,14 @@ namespace NavigationExample
public double theta;
}
[StructLayout(LayoutKind.Sequential)]
public struct Twist2D
{
public double x;
public double y;
public double theta;
}
[StructLayout(LayoutKind.Sequential)]
public struct Quaternion
{
@ -93,6 +101,13 @@ namespace NavigationExample
public Pose pose;
}
[StructLayout(LayoutKind.Sequential)]
public struct Twist2DStamped
{
public Header header;
public Twist2D velocity;
}
[StructLayout(LayoutKind.Sequential)]
public struct Vector3
{
@ -180,6 +195,14 @@ namespace NavigationExample
public static extern bool navigation_set_robot_footprint(
IntPtr handle, Point[] points, UIntPtr point_count);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_robot_footprint(
IntPtr handle, out IntPtr out_points, out UIntPtr out_count);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void navigation_free_points(IntPtr points);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_move_to(
@ -231,6 +254,11 @@ namespace NavigationExample
public static extern bool navigation_get_robot_pose_2d(
IntPtr handle, ref Pose2D out_pose);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_twist(
IntPtr handle, ref Twist2DStamped out_twist);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_feedback(

BIN
examples/NavigationExample/libnav_c_api.so
View File

Binary file not shown.

41
src/APIs/c_api/include/nav_c_api.h
View File

@ -59,6 +59,15 @@ typedef struct {
double theta;
} Pose2D;
/**
* @brief Twist2D structure (x, y, theta velocities)
*/
typedef struct {
double x;
double y;
double theta;
} Twist2D;
/**
* @brief Quaternion structure
*/
@ -104,6 +113,14 @@ typedef struct {
Pose pose;
} PoseStamped;
/**
* @brief Twist2DStamped structure
*/
typedef struct {
Header header;
Twist2D velocity;
} Twist2DStamped;
/**
* @brief Vector3 structure
*/
@ -250,6 +267,21 @@ bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
*/
bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count);
/**
* @brief Get the robot's footprint (outline shape)
* @param handle Navigation handle
* @param out_points Output array of points (allocated by library, free with navigation_free_points)
* @param out_count Output number of points in the array
* @return true on success, false on failure
*/
bool navigation_get_robot_footprint(NavigationHandle handle, Point** out_points, size_t* out_count);
/**
* @brief Free a points array allocated by navigation_get_robot_footprint
* @param points Pointer to point array
*/
void navigation_free_points(Point* points);
/**
* @brief Send a goal for the robot to navigate to
* @param handle Navigation handle
@ -350,6 +382,15 @@ bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out
*/
bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose);
/**
* @brief Get the robot's current twist
* @param handle Navigation handle
* @param out_twist Output parameter with the robot's current twist
* @return true if twist was successfully retrieved
* @note out_twist->header.frame_id must be freed using nav_c_api_free_string
*/
bool navigation_get_twist(NavigationHandle handle, Twist2DStamped* out_twist);
/**
* @brief Get navigation feedback
* @param handle Navigation handle

94
src/APIs/c_api/src/nav_c_api.cpp
View File

@ -6,12 +6,14 @@
#include <robot_geometry_msgs/Pose2D.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_geometry_msgs/Vector3.h>
#include <robot_std_msgs/Header.h>
#include <robot_nav_2d_msgs/Twist2DStamped.h>
#include <tf3/buffer_core.h>
#include <string>
#include <vector>
#include <cstring>
#include <cstdlib>
#include <boost/dll/import.hpp>
#include <stdexcept>
#include <robot/robot.h>
@ -65,6 +67,25 @@ namespace {
c_pose->theta = cpp_pose.theta;
}
void cpp_to_c_header(const robot_std_msgs::Header& cpp_header, Header* c_header) {
c_header->seq = cpp_header.seq;
c_header->sec = cpp_header.stamp.sec;
c_header->nsec = cpp_header.stamp.nsec;
if (!cpp_header.frame_id.empty()) {
c_header->frame_id = strdup(cpp_header.frame_id.c_str());
} else {
c_header->frame_id = nullptr;
}
}
void cpp_to_c_twist2d_stamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist,
Twist2DStamped* c_twist) {
cpp_to_c_header(cpp_twist.header, &c_twist->header);
c_twist->velocity.x = cpp_twist.velocity.x;
c_twist->velocity.y = cpp_twist.velocity.y;
c_twist->velocity.theta = cpp_twist.velocity.theta;
}
// Convert C++ NavFeedback to C NavFeedback
void cpp_to_c_nav_feedback(const robot::move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) {
c_feedback->navigation_state = static_cast<NavigationState>(static_cast<int>(cpp_feedback.navigation_state));
@ -226,22 +247,13 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle
}
try {
printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__);
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(tf_handle);
robot::PluginLoaderHelper loader;
std::string lib_path = loader.findLibraryPath("MoveBase");
auto create_plugin = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
lib_path,
"MoveBase",
boost::dll::load_mode::append_decorations);
robot::move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
printf("[%s:%d]\n Navigation created\n", __FILE__, __LINE__);
if (nav_ptr == nullptr) {
printf("[%s:%d]\n Error: Failed to create navigation\n", __FILE__, __LINE__);
if (!tf_ptr || !(*tf_ptr)) {
printf("[%s:%d]\n Error: Invalid TF listener\n", __FILE__, __LINE__);
return false;
}
nav_ptr->initialize(*tf_ptr);
nav->initialize(*tf_ptr);
return true;
} catch (const std::exception &e) {
@ -276,6 +288,45 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po
}
}
extern "C" bool navigation_get_robot_footprint(NavigationHandle handle, Point** out_points, size_t* out_count) {
if (!handle || !out_points || !out_count) {
return false;
}
try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
std::vector<robot_geometry_msgs::Point> footprint = nav->getRobotFootprint();
if (footprint.empty()) {
*out_points = nullptr;
*out_count = 0;
return true;
}
Point* points = static_cast<Point*>(malloc(sizeof(Point) * footprint.size()));
if (!points) {
return false;
}
for (size_t i = 0; i < footprint.size(); ++i) {
points[i].x = footprint[i].x;
points[i].y = footprint[i].y;
points[i].z = footprint[i].z;
}
*out_points = points;
*out_count = footprint.size();
return true;
} catch (...) {
return false;
}
}
extern "C" void navigation_free_points(Point* points) {
if (points) {
free(points);
}
}
extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal,
double xy_goal_tolerance, double yaw_goal_tolerance) {
if (!handle || !goal) {
@ -442,6 +493,21 @@ extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* ou
}
}
extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped* out_twist) {
if (!handle || !out_twist) {
return false;
}
try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav->getTwist();
cpp_to_c_twist2d_stamped(cpp_twist, out_twist);
return true;
} catch (...) {
return false;
}
}
extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback) {
if (!handle || !out_feedback) {
return false;

1
src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt
View File

@ -84,6 +84,7 @@ add_library(${PROJECT_NAME}_diff SHARED
src/diff/diff_predictive_trajectory.cpp
src/diff/diff_rotate_to_goal.cpp
src/diff/diff_go_straight.cpp
# src/diff/pure_pursuit.cpp
)
if(BUILDING_WITH_CATKIN)

50
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h Normal file
View File

@ -0,0 +1,50 @@
#ifndef _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__
#define _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__
#include <robot/robot.h>
#include <score_algorithm/score_algorithm.h>
#include <boost/dll/alias.hpp>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PointStamped.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_core2/costmap.h>
#include <nav_grid/coordinate_conversion.h>
#include <angles/angles.h>
namespace mkt_algorithm
{
namespace diff
{
class PurePursuit : public score_algorithm::ScoreAlgorithm
{
public:
PurePursuit();
~PurePursuit();
void computePurePursuit(
const double &velocity_min,
const double &velocity_max,
const double &linear_vel,
const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
const double &min_dist2,
const double &curvature_dist2_floor,
const double &dist2_override);
private:
void applyConstraints(const double &dist_error, const double &lookahead_dist,
const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity,
const double &pose_cost, double &linear_vel, double &sign_x);
double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
const double &journey_plan);
double computeDecelerationFactor(const double &effective_journey, const double &d_reduce);
bool detectWobbleByCarrotAngle(std::vector<double> &angle_history, const double &carrot_angle,
const double &amplitude_threshold, const size_t &window_size);
};
}
}
#endif

3
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp
View File

@ -107,7 +107,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
robot_nav_2d_msgs::Twist2D twist;
traj_->startNewIteration(velocity); // Nhận tốc độ hiện tại từ odom
robot::Rate r(50);
while (traj_->hasMoreTwists())
while (robot::ok() && traj_->hasMoreTwists())
{
twist = traj_->nextTwist();
// ROS_INFO("Twist: x: %.2f, y: %.2f, theta: %.2f", twist.x, twist.y, twist.theta);
@ -184,6 +184,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
double v_min =
journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
v_min *= sign_x;
double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
journey_plan += max_path_distance_;

1327
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy 2.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

1320
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

12
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
View File

@ -391,15 +391,25 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
robot_nav_2d_msgs::Twist2D drive_cmd;
double sign_x = sign(x_direction_);
robot_nav_2d_msgs::Twist2D twist;
robot::log_info("[%s:%d] velocity in predictive trajectory: %f %f %f", __FILE__, __LINE__, velocity.x, velocity.y, velocity.theta);
traj_->startNewIteration(velocity);
while (traj_->hasMoreTwists())
while (robot::ok() && traj_->hasMoreTwists())
{
twist = traj_->nextTwist();
}
drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5);
robot::log_info("[%s:%d] twist: %f %f %f", __FILE__, __LINE__, twist.x, twist.y, twist.theta);
robot::log_info("--------------------------------");
double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
result.poses.clear();
result.poses.reserve(transformed_plan.poses.size());
for (const auto &pose_stamped : transformed_plan.poses)
{
result.poses.push_back(pose_stamped.pose);
}
if (transformed_plan.poses.empty())
{
robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__);

52
src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp Normal file
View File

@ -0,0 +1,52 @@
#include <mkt_algorithm/diff/pure_pursuit.h>
void mkt_algorithm::diff::PurePursuit::computePurePursuit(const double &velocity_min,
const double &velocity_max,
const double &linear_vel,
const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
const double &min_dist2,
const double &curvature_dist2_floor,
const double &dist2_override)
{
const double vel_x_reduce = std::min(fabs(velocity_max), 0.3);
double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
carrot_dist2 = std::max(carrot_dist2, 0.05);
double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
if (max_lateral_accel_ > 1e-6)
{
const double curvature_abs = std::max(fabs(curvature), 1e-6);
const double v_lateral_limit = sqrt(max_lateral_accel_ / curvature_abs);
drive_cmd.x = sign_x > 0 ? std::min(drive_cmd.x, v_lateral_limit) : std::max(drive_cmd.x, -v_lateral_limit);
}
const auto &plan_back_pose = compute_plan_.poses.back();
double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y);
post_cost = std::max(post_cost, center_cost_);
this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);
const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
double d_reduce = std::clamp(journey_plan, min_S, max_S);
double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
double v_min =
journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
v_min *= sign_x;
double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
double vel_reduce = sign_x > 0
? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min)
: std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min);
drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce;
double v_theta = drive_cmd.x * curvature;
double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_))
{
carrot_dist2 *= 0.6;
curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
v_theta = drive_cmd.x * curvature;
}
}

17
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h
View File

@ -270,6 +270,23 @@ namespace mkt_plugins
using Ptr = std::shared_ptr<KinematicParameters>;
protected:
/**
* @brief Configure the kinematic parameters
* @param nh NodeHandle
*
* Configures the kinematic parameters from the given NodeHandle.
*/
void configure(const robot::NodeHandle &nh);
/**
* @brief Reconfigure the kinematic parameters
* @param nh NodeHandle
*
* Reconfigures the kinematic parameters from the given NodeHandle.
*/
void reconfigure(const robot::NodeHandle &nh);
// For parameter descriptions, see cfg/KinematicParams.cfg
int xytheta_direct_[3];
double min_vel_x_, min_vel_y_;

10
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h
View File

@ -116,9 +116,13 @@ namespace mkt_plugins
* current velocity within acc_time seconds, respecting acceleration/deceleration limits.
* The velocities range from min_vel_ to max_vel_ (projected from current velocity).
*/
OneDVelocityIterator(double current, int direct, double min, double max, double acc_limit, double decel_limit, double acc_time,
int num_samples)
OneDVelocityIterator(
std::string name, double current, int direct, double min, double max, double acc_limit, double decel_limit, double acc_time, int num_samples)
: name_(name)
{
if(name_ == "x_it_")
robot::log_warning("[%s:%d] one_d_velocity_iterator: [%s] %f %d %f %f %f %f %f %d",
__FILE__, __LINE__, name_.c_str(), current, direct, min, max, acc_limit, decel_limit, acc_time, num_samples);
// if (current < min)
// {
// current = min;
@ -203,8 +207,6 @@ namespace mkt_plugins
*/
bool isFinished() const
{
// if(name_ == std::string("th_it_"))
// ROS_INFO("%s %f %f", name_.c_str(), current_, max_vel_ + EPSILON);
double limit_vel = direct_ > 0? max_vel_ : min_vel_;
return fabs(current_) > fabs(limit_vel) + EPSILON;
}

42
src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp
View File

@ -54,10 +54,48 @@ namespace mkt_plugins
setDecelerationAsNeeded(nh, "y");
setDecelerationAsNeeded(nh, "theta");
configure(nh);
xytheta_direct_[0] = xytheta_direct_[1] = xytheta_direct_[2] = 1;
}
void KinematicParameters::configure(const robot::NodeHandle &nh)
{
nh.param("max_vel_x", original_max_vel_x_, 0.0);
nh.param("max_vel_y", original_max_vel_y_, 0.0);
nh.param("max_vel_theta", original_max_vel_theta_, 0.0);
nh.param("min_vel_x", original_min_vel_x_, 0.0);
nh.param("min_vel_y", original_min_vel_y_, 0.0);
nh.param("min_speed_xy", original_min_speed_xy_, 0.0);
nh.param("max_speed_xy", original_max_speed_xy_, 0.0);
nh.param("min_speed_theta", original_min_speed_theta_, 0.0);
nh.param("acc_lim_x", original_acc_lim_x_, 0.0);
nh.param("acc_lim_y", original_acc_lim_y_, 0.0);
nh.param("acc_lim_theta", original_acc_lim_theta_, 0.0);
nh.param("decel_lim_x", original_decel_lim_x_, 0.0);
nh.param("decel_lim_y", original_decel_lim_y_, 0.0);
nh.param("decel_lim_theta", original_decel_lim_theta_, 0.0);
reconfigure(nh);
}
void KinematicParameters::reconfigure(const robot::NodeHandle &nh)
{
nh.param("max_vel_x", max_vel_x_, 0.0);
nh.param("max_vel_y", max_vel_y_, 0.0);
nh.param("max_vel_theta", max_vel_theta_, 0.0);
nh.param("min_vel_x", min_vel_x_, 0.0);
nh.param("min_vel_y", min_vel_y_, 0.0);
nh.param("min_speed_xy", min_speed_xy_, 0.0);
nh.param("max_speed_xy", max_speed_xy_, 0.0);
nh.param("min_speed_theta", min_speed_theta_, 0.0);
nh.param("acc_lim_x", acc_lim_x_, 0.0);
nh.param("acc_lim_y", acc_lim_y_, 0.0);
nh.param("acc_lim_theta", acc_lim_theta_, 0.0);
nh.param("decel_lim_x", decel_lim_x_, 0.0);
nh.param("decel_lim_y", decel_lim_y_, 0.0);
nh.param("decel_lim_theta", decel_lim_theta_, 0.0);
}
void KinematicParameters::setDirect(int *xytheta_direct)
{
xytheta_direct_[0] = xytheta_direct[0];
@ -111,13 +149,11 @@ namespace mkt_plugins
void KinematicParameters::setMinX(double value)
{
min_vel_x_ = fabs(value) < fabs(original_min_vel_x_) + EPSILON ? value : min_vel_x_;
// ROS_INFO_THROTTLE(10.0, "vx_min %f %f %f", value , original_min_vel_x_, min_vel_x_);
}
void KinematicParameters::setMaxX(double value)
{
max_vel_x_ = fabs(value) <= fabs(original_max_vel_x_) + EPSILON ? value : original_max_vel_x_;
// ROS_INFO_THROTTLE(10, "vx %f %f %f", value , original_max_vel_x_, max_vel_x_);
}
void KinematicParameters::setAccX(double value)

2
src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp
View File

@ -18,6 +18,7 @@ void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh)
else
{
double controller_frequency = robot_nav_2d_utils::searchAndGetParam(nh, "controller_frequency", 20.0);
robot::log_warning("[%s:%d] limited_accel_generator: %f", __FILE__, __LINE__, controller_frequency);
if (controller_frequency > 0)
{
acceleration_time_ = 1.0 / controller_frequency;
@ -29,6 +30,7 @@ void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh)
acceleration_time_ = 0.05;
}
}
}
void LimitedAccelGenerator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity)

12
src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp
View File

@ -14,19 +14,15 @@ void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameter
void XYThetaIterator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity, double dt)
{
x_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.x, kinematics_->getDirect()[0], kinematics_->getMinX(), kinematics_->getMaxX(),
robot::log_info("[%s:%d] xy_theta_iterator: %f %f %f %f", __FILE__, __LINE__, current_velocity.x, current_velocity.y, current_velocity.theta, dt);
x_it_ = std::make_shared<OneDVelocityIterator>("x_it_", current_velocity.x, kinematics_->getDirect()[0], kinematics_->getMinX(), kinematics_->getMaxX(),
kinematics_->getAccX(), kinematics_->getDecelX(), dt, vx_samples_);
x_it_->name_ = std::string("x_it_");
y_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.y, kinematics_->getDirect()[1], kinematics_->getMinY(), kinematics_->getMaxY(),
y_it_ = std::make_shared<OneDVelocityIterator>("y_it_", current_velocity.y, kinematics_->getDirect()[1], kinematics_->getMinY(), kinematics_->getMaxY(),
kinematics_->getAccY(), kinematics_->getDecelY(), dt, vy_samples_);
y_it_->name_ = std::string("y_it_");
th_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.theta, kinematics_->getDirect()[2], kinematics_->getMinTheta(), kinematics_->getMaxTheta(),
th_it_ = std::make_shared<OneDVelocityIterator>("th_it_", current_velocity.theta, kinematics_->getDirect()[2], kinematics_->getMinTheta(), kinematics_->getMaxTheta(),
kinematics_->getAccTheta(), kinematics_->getDecelTheta(), dt, vtheta_samples_);
th_it_->name_ = std::string("th_it_");
}
bool XYThetaIterator::hasMoreTwists()

2
src/Algorithms/Packages/global_planners/custom_planner

@ -1 +1 @@
Subproject commit a2bebb5be969471a135601da78866cc9d1db84b1
Subproject commit 49afcce5b2247793282fe5e94d0e27f062562325

9
src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h
View File

@ -46,6 +46,12 @@ namespace pnkx_local_planner
*/
void setPlan(const robot_nav_2d_msgs::Path2D &path) override;
/**
* @brief robot_nav_core2 getPlan - Gets the current global plan
* @param path The global plan
*/
void getPlan(robot_nav_2d_msgs::Path2D &path) override;
/**
* @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
*
@ -179,7 +185,8 @@ namespace pnkx_local_planner
std::string name_;
robot::NodeHandle parent_, planner_nh_;
robot_nav_2d_msgs::Path2D global_plan_; ///< Saved Global Plan
robot_nav_2d_msgs::Path2D transformed_plan_;
robot_nav_2d_msgs::Path2D transformed_global_plan_;
robot_nav_2d_msgs::Path2D local_plan_;
robot_nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose
robot_costmap_2d::Costmap2D *costmap_;

17
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp
View File

@ -180,7 +180,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const robot_nav_2d_
try
{
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
}
catch(const robot_nav_core2::LocalPlannerException& e)
@ -189,7 +189,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const robot_nav_2d_
}
double x_direction, y_direction, theta_direction;
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
{
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
}
@ -206,9 +206,16 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner
robot_nav_2d_msgs::Twist2D twist;
mkt_msgs::Trajectory2D traj;
robot_nav_2d_msgs::Twist2DStamped cmd_vel;
if (!ret_nav_)
traj = nav_algorithm_->calculator(pose, velocity);
if (ret_nav_)
{
local_plan_.poses.clear();
return cmd_vel;
}
traj = nav_algorithm_->calculator(pose, velocity);
local_plan_.header.stamp = robot::Time::now();
robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now());
local_plan_ = robot_nav_2d_utils::pathToPath(path);
cmd_vel.velocity = traj.velocity;
return cmd_vel;
}
@ -222,7 +229,7 @@ bool pnkx_local_planner::PNKXGoStraightLocalPlanner::isGoalReached(const robot_n
}
// Update time stamp of goal pose
// goal_pose_.header.stamp = pose.header.stamp;
ret_nav_ = goal_checker_->isGoalReached(transformPoseToLocal(pose), transformPoseToLocal(goal_pose_), transformed_plan_, velocity);
ret_nav_ = goal_checker_->isGoalReached(transformPoseToLocal(pose), transformPoseToLocal(goal_pose_), transformed_global_plan_, velocity);
if (ret_nav_)
robot::log_info_at(__FILE__, __LINE__, "Goal reached!");

31
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp
View File

@ -208,6 +208,15 @@ void pnkx_local_planner::PNKXLocalPlanner::setPlan(const robot_nav_2d_msgs::Path
}
}
void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &path)
{
if (local_plan_.poses.empty())
{
return;
}
path = local_plan_;
}
void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
{
this->getParams(planner_nh_);
@ -231,7 +240,8 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
local_goal_pose = this->transformPoseToLocal(goal_pose_);
// robot::log_info("local_start_pose: %f %f %f", local_start_pose.pose.x, local_start_pose.pose.y, local_start_pose.pose.theta);
// robot::log_info("local_goal_pose: %f %f %f", local_goal_pose.pose.x, local_goal_pose.pose.y, local_goal_pose.pose.theta);
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
if (!pnkx_local_planner::transformGlobalPlan(
tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
{
robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
@ -240,7 +250,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
double x_direction, y_direction, theta_direction;
if (!ret_nav_)
{
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
{
robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
@ -248,7 +258,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
}
else
{
if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
{
robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
@ -291,11 +301,24 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlg
cmd_vel.header.stamp = robot::Time::now();
if (ret_nav_ && ret_angle_)
{
return cmd_vel;
local_plan_.poses.clear();
}
else if (!ret_nav_)
{
traj = nav_algorithm_->calculator(pose, velocity);
local_plan_.header.stamp = robot::Time::now();
robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now());
local_plan_ = robot_nav_2d_utils::pathToPath(path);
}
else
{
traj = rotate_algorithm_->calculator(pose, velocity);
local_plan_.header.stamp = robot::Time::now();
robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now());
local_plan_ = robot_nav_2d_utils::pathToPath(path);
}
cmd_vel.velocity = traj.velocity;
return cmd_vel;
@ -404,7 +427,7 @@ bool pnkx_local_planner::PNKXLocalPlanner::isGoalReached(const robot_nav_2d_msgs
// goal_pose_.header.stamp = pose.header.stamp;
robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
robot_nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_);
robot_nav_2d_msgs::Path2D plan = transformed_plan_;
robot_nav_2d_msgs::Path2D plan = transformed_global_plan_;
if (!ret_nav_)
{
ret_nav_ = goal_checker_->isGoalReached(local_pose, local_goal, plan, velocity);

11
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp
View File

@ -155,7 +155,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs
try
{
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
}
catch(const robot_nav_core2::LocalPlannerException& e)
@ -164,7 +164,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs
}
double x_direction, y_direction, theta_direction;
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
{
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
}
@ -199,10 +199,17 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::Sc
robot_nav_2d_msgs::Twist2D twist;
mkt_msgs::Trajectory2D traj;
robot_nav_2d_msgs::Twist2DStamped cmd_vel;
cmd_vel.header.stamp = robot::Time::now();
if (ret_nav_)
{
local_plan_.poses.clear();
return cmd_vel;
}
traj = nav_algorithm_->calculator(pose, velocity);
local_plan_.header.stamp = robot::Time::now();
robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now());
local_plan_ = robot_nav_2d_utils::pathToPath(path);
cmd_vel.velocity = traj.velocity;
return cmd_vel;
}

16
src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h
View File

@ -159,20 +159,20 @@ protected:
*/
TF3SIMD_FORCE_INLINE void setMax(const QuadWord& other)
{
// tf3SetMax(m_floats[0], other.m_floats[0]);
// tf3SetMax(m_floats[1], other.m_floats[1]);
// tf3SetMax(m_floats[2], other.m_floats[2]);
// tf3SetMax(m_floats[3], other.m_floats[3]);
tf3SetMax(m_floats[0], other.m_floats[0]);
tf3SetMax(m_floats[1], other.m_floats[1]);
tf3SetMax(m_floats[2], other.m_floats[2]);
tf3SetMax(m_floats[3], other.m_floats[3]);
}
/**@brief Set each element to the min of the current values and the values of another QuadWord
* @param other The other QuadWord to compare with
*/
TF3SIMD_FORCE_INLINE void setMin(const QuadWord& other)
{
// tf3SetMin(m_floats[0], other.m_floats[0]);
// tf3SetMin(m_floats[1], other.m_floats[1]);
// tf3SetMin(m_floats[2], other.m_floats[2]);
// tf3SetMin(m_floats[3], other.m_floats[3]);
tf3SetMin(m_floats[0], other.m_floats[0]);
tf3SetMin(m_floats[1], other.m_floats[1]);
tf3SetMin(m_floats[2], other.m_floats[2]);
tf3SetMin(m_floats[3], other.m_floats[3]);
}

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

@ -27,495 +27,494 @@
#include <robot/time.h>
#include <move_base_core/common.h>
namespace robot
{
namespace move_base_core
{
// Navigation states, including planning and controller status
enum class State
namespace move_base_core
{
PENDING,
ACTIVE,
PREEMPTED,
SUCCEEDED,
ABORTED,
REJECTED,
PREEMPTING,
RECALLING,
RECALLED,
LOST,
PLANNING,
CONTROLLING,
CLEARING,
PAUSED
};
/**
* @brief Feedback structure to describe current navigation status
*/
struct NavFeedback
{
State navigation_state; // Current navigation state
std::string feed_back_str; // Description or debug message
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
};
/**
* @brief Planner data output structure.
*/
struct PlannerDataOutput
{
robot_nav_2d_msgs::Path2D plan;
robot_nav_msgs::OccupancyGrid costmap;
robot_map_msgs::OccupancyGridUpdate costmap_update;
bool is_costmap_updated;
robot_geometry_msgs::PolygonStamped footprint;
};
/**
* @brief Convert a State enum to its string representation.
* Useful for debugging/logging or displaying in UI.
*
* @param state Enum value of move_base_core::State
* @return std::string The corresponding string, e.g. "PENDING"
*/
inline std::string toString(move_base_core::State state)
{
using move_base_core::State;
switch (state)
// Navigation states, including planning and controller status
enum class State
{
case State::PENDING:
return "PENDING"; // Chờ xử lý
case State::ACTIVE:
return "ACTIVE"; // Đang hoạt động
case State::PREEMPTED:
return "PREEMPTED"; // Đã bị huỷ bởi yêu cầu mới
case State::SUCCEEDED:
return "SUCCEEDED"; // Thành công
case State::ABORTED:
return "ABORTED"; // Bị lỗi
case State::REJECTED:
return "REJECTED"; // Từ chối bởi planner hoặc controller
case State::PREEMPTING:
return "PREEMPTING"; // Đang huỷ bỏ theo yêu cầu
case State::RECALLING:
return "RECALLING"; // Đang huỷ bỏ nội bộ
case State::RECALLED:
return "RECALLED"; // Đã được thu hồi
case State::LOST:
return "LOST"; // Mất trạng thái
case State::PLANNING:
return "PLANNING"; // Đang lập kế hoạch đường đi
case State::CONTROLLING:
return "CONTROLLING"; // Đang điều khiển robot di chuyển theo plan
case State::CLEARING:
return "CLEARING"; // Đang dọn dẹp bản đồ / costmap
case State::PAUSED:
return "PAUSED"; // Tạm dừng
default:
return "UNKNOWN_STATE"; // Không xác định
PENDING,
ACTIVE,
PREEMPTED,
SUCCEEDED,
ABORTED,
REJECTED,
PREEMPTING,
RECALLING,
RECALLED,
LOST,
PLANNING,
CONTROLLING,
CLEARING,
PAUSED
};
/**
* @brief Feedback structure to describe current navigation status
*/
struct NavFeedback
{
State navigation_state; // Current navigation state
std::string feed_back_str; // Description or debug message
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
};
/**
* @brief Planner data output structure.
*/
struct PlannerDataOutput
{
robot_nav_2d_msgs::Path2D plan;
robot_nav_msgs::OccupancyGrid costmap;
robot_map_msgs::OccupancyGridUpdate costmap_update;
bool is_costmap_updated;
robot_geometry_msgs::PolygonStamped footprint;
};
/**
* @brief Convert a State enum to its string representation.
* Useful for debugging/logging or displaying in UI.
*
* @param state Enum value of move_base_core::State
* @return std::string The corresponding string, e.g. "PENDING"
*/
inline std::string toString(move_base_core::State state)
{
using move_base_core::State;
switch (state)
{
case State::PENDING:
return "PENDING"; // Chờ xử lý
case State::ACTIVE:
return "ACTIVE"; // Đang hoạt động
case State::PREEMPTED:
return "PREEMPTED"; // Đã bị huỷ bởi yêu cầu mới
case State::SUCCEEDED:
return "SUCCEEDED"; // Thành công
case State::ABORTED:
return "ABORTED"; // Bị lỗi
case State::REJECTED:
return "REJECTED"; // Từ chối bởi planner hoặc controller
case State::PREEMPTING:
return "PREEMPTING"; // Đang huỷ bỏ theo yêu cầu
case State::RECALLING:
return "RECALLING"; // Đang huỷ bỏ nội bộ
case State::RECALLED:
return "RECALLED"; // Đã được thu hồi
case State::LOST:
return "LOST"; // Mất trạng thái
case State::PLANNING:
return "PLANNING"; // Đang lập kế hoạch đường đi
case State::CONTROLLING:
return "CONTROLLING"; // Đang điều khiển robot di chuyển theo plan
case State::CLEARING:
return "CLEARING"; // Đang dọn dẹp bản đồ / costmap
case State::PAUSED:
return "PAUSED"; // Tạm dừng
default:
return "UNKNOWN_STATE"; // Không xác định
}
}
}
/**
* @brief Creates a target pose (PoseStamped) by offsetting a given 2D pose along its heading direction.
*
* This function calculates a new position by moving forward (or backward if negative)
* a certain `offset_distance` from the current position, following the given heading angle (theta).
*
* @param pose The original 2D pose (x, y, theta) in the local plane.
* @param frame_id The coordinate frame in which the output PoseStamped will be expressed.
* @param offset_distance The distance to offset along the heading direction, in meters.
* Positive moves forward, negative moves backward.
* @return A new PoseStamped offset from the input pose, in the given frame.
*/
inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::Pose2D &pose, const std::string &frame_id, double offset_distance)
{
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);
goal.pose.position.y = pose.y + offset_distance * sin(pose.theta);
tf3::Quaternion q;
q.setRPY(0, 0, pose.theta);
goal.pose.orientation = tf3::toMsg(q);
return goal;
}
/**
* @brief Overloaded version: creates an offset target pose from a given PoseStamped.
*
* This function extracts the 2D position and orientation (yaw) from the given PoseStamped,
* offsets it forward (or backward) along the current heading direction,
* and returns a new PoseStamped in the same frame.
*
* @param pose The original pose with full position and orientation.
* @param offset_distance Distance to offset along the current heading direction (in meters).
* @return A new PoseStamped offset from the original pose.
*/
inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::PoseStamped &pose, double offset_distance)
{
robot_geometry_msgs::Pose2D pose2d;
pose2d.x = pose.pose.position.x;
pose2d.y = pose.pose.position.y;
// pose2d.theta = tf2::getYaw(pose.pose.orientation);
return offset_goal(pose2d, pose.header.frame_id, offset_distance);
}
/**
* @class BaseNavigation
* @brief Abstract interface for robot navigation modules.
*
* Provides core methods for setting goals, moving, rotating, and handling motion control.
* All navigation logic must implement this interface.
*/
class BaseNavigation
{
public:
using Ptr = std::shared_ptr<BaseNavigation>;
virtual ~BaseNavigation() {}
/**
* @brief Initialize the navigation system.
* @param tf Shared pointer to the TF listener or buffer.
*/
virtual void initialize(TFListenerPtr tf) = 0;
/**
* @brief Set the robot's footprint (outline shape) in the global frame.
* This can be used for planning or collision checking.
* @brief Creates a target pose (PoseStamped) by offsetting a given 2D pose along its heading direction.
*
* @param fprt A vector of points representing the robot's footprint polygon.
* The points should be ordered counter-clockwise.
* Example:
* This function calculates a new position by moving forward (or backward if negative)
* a certain `offset_distance` from the current position, following the given heading angle (theta).
*
^ Y
|
| P3(-0.3, 0.2) P2(0.3, 0.2)
| ●---------------●
| | |
| | Robot | (view Top )
| | |
| ●---------------●
| P4(-0.3, -0.2) P1(0.3, -0.2)
+-------------------------------> X
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));
* @param pose The original 2D pose (x, y, theta) in the local plane.
* @param frame_id The coordinate frame in which the output PoseStamped will be expressed.
* @param offset_distance The distance to offset along the heading direction, in meters.
* Positive moves forward, negative moves backward.
* @return A new PoseStamped offset from the input pose, in the given frame.
*/
virtual void setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt) = 0;
inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::Pose2D &pose, const std::string &frame_id, double offset_distance)
{
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);
goal.pose.position.y = pose.y + offset_distance * sin(pose.theta);
tf3::Quaternion q;
q.setRPY(0, 0, pose.theta);
goal.pose.orientation = tf3::toMsg(q);
return goal;
}
/**
* @brief Get the robot's footprint (outline shape) in the global frame.
* @return A vector of points representing the robot's footprint polygon.
* @brief Overloaded version: creates an offset target pose from a given PoseStamped.
*
* This function extracts the 2D position and orientation (yaw) from the given PoseStamped,
* offsets it forward (or backward) along the current heading direction,
* and returns a new PoseStamped in the same frame.
*
* @param pose The original pose with full position and orientation.
* @param offset_distance Distance to offset along the current heading direction (in meters).
* @return A new PoseStamped offset from the original pose.
*/
virtual std::vector<robot_geometry_msgs::Point> getRobotFootprint() = 0;
inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::PoseStamped &pose, double offset_distance)
{
robot_geometry_msgs::Pose2D pose2d;
pose2d.x = pose.pose.position.x;
pose2d.y = pose.pose.position.y;
// pose2d.theta = tf2::getYaw(pose.pose.orientation);
return offset_goal(pose2d, pose.header.frame_id, offset_distance);
}
/**
* @brief Add a static map to the navigation system.
* @param map_name The name of the map.
* @param map The map to add.
* @class BaseNavigation
* @brief Abstract interface for robot navigation modules.
*
* Provides core methods for setting goals, moving, rotating, and handling motion control.
* All navigation logic must implement this interface.
*/
virtual void addStaticMap(const std::string &map_name, robot_nav_msgs::OccupancyGrid map) = 0;
class BaseNavigation
{
public:
using Ptr = std::shared_ptr<BaseNavigation>;
/**
* @brief Add a laser scan to the navigation system.
* @param laser_scan_name The name of the laser scan.
* @param laser_scan The laser scan to add.
*/
virtual void addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) = 0;
virtual ~BaseNavigation() {}
/**
* @brief Add a point cloud to the navigation system.
* @param point_cloud_name The name of the point cloud.
* @param point_cloud The point cloud to add.
*/
virtual void addPointCloud(const std::string &point_cloud_name, robot_sensor_msgs::PointCloud point_cloud) = 0;
/**
* @brief Initialize the navigation system.
* @param tf Shared pointer to the TF listener or buffer.
*/
virtual void initialize(TFListenerPtr tf) = 0;
/**
* @brief Add a point cloud2 to the navigation system.
* @param point_cloud2_name The name of the point cloud2.
* @param point_cloud2 The point cloud2 to add.
*/
virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) = 0;
/**
* @brief Set the robot's footprint (outline shape) in the global frame.
* This can be used for planning or collision checking.
*
* @param fprt A vector of points representing the robot's footprint polygon.
* The points should be ordered counter-clockwise.
* Example:
*
^ Y
|
| P3(-0.3, 0.2) P2(0.3, 0.2)
| ●---------------●
| | |
| | Robot | (view Top )
| | |
| ●---------------●
| P4(-0.3, -0.2) P1(0.3, -0.2)
+-------------------------------> X
/**
* @brief Get a static map from the navigation system.
* @param map_name The name of the map.
* @return The map.
*/
virtual robot_nav_msgs::OccupancyGrid getStaticMap(const std::string &map_name) = 0;
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<robot_geometry_msgs::Point> &fprt) = 0;
/**
* @brief Get a laser scan from the navigation system.
* @param laser_scan_name The name of the laser scan.
* @return The laser scan.
*/
virtual robot_sensor_msgs::LaserScan getLaserScan(const std::string &laser_scan_name) = 0;
/**
* @brief Get the robot's footprint (outline shape) in the global frame.
* @return A vector of points representing the robot's footprint polygon.
*/
virtual std::vector<robot_geometry_msgs::Point> getRobotFootprint() = 0;
/**
* @brief Get a point cloud from the navigation system.
* @param point_cloud_name The name of the point cloud.
* @return The point cloud.
*/
virtual robot_sensor_msgs::PointCloud getPointCloud(const std::string &point_cloud_name) = 0;
/**
* @brief Add a static map to the navigation system.
* @param map_name The name of the map.
* @param map The map to add.
*/
virtual void addStaticMap(const std::string &map_name, robot_nav_msgs::OccupancyGrid map) = 0;
/**
* @brief Get a point cloud2 from the navigation system.
* @param point_cloud2_name The name of the point cloud2.
* @return The point cloud2.
*/
virtual robot_sensor_msgs::PointCloud2 getPointCloud2(const std::string &point_cloud2_name) = 0;
/**
* @brief Add a laser scan to the navigation system.
* @param laser_scan_name The name of the laser scan.
* @param laser_scan The laser scan to add.
*/
virtual void addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) = 0;
/**
* @brief Get all static maps from the navigation system.
* @return The static maps.
*/
virtual std::map<std::string, robot_nav_msgs::OccupancyGrid> getAllStaticMaps() = 0;
/**
* @brief Add a point cloud to the navigation system.
* @param point_cloud_name The name of the point cloud.
* @param point_cloud The point cloud to add.
*/
virtual void addPointCloud(const std::string &point_cloud_name, robot_sensor_msgs::PointCloud point_cloud) = 0;
/**
* @brief Get all laser scans from the navigation system.
* @return The laser scans.
*/
virtual std::map<std::string, robot_sensor_msgs::LaserScan> getAllLaserScans() = 0;
/**
* @brief Add a point cloud2 to the navigation system.
* @param point_cloud2_name The name of the point cloud2.
* @param point_cloud2 The point cloud2 to add.
*/
virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) = 0;
/**
* @brief Get all point clouds from the navigation system.
* @return The point clouds.
*/
virtual std::map<std::string, robot_sensor_msgs::PointCloud> getAllPointClouds() = 0;
/**
* @brief Get a static map from the navigation system.
* @param map_name The name of the map.
* @return The map.
*/
virtual robot_nav_msgs::OccupancyGrid getStaticMap(const std::string &map_name) = 0;
/**
* @brief Get all point cloud2s from the navigation system.
* @return The point cloud2s.
*/
virtual std::map<std::string, robot_sensor_msgs::PointCloud2> getAllPointCloud2s() = 0;
/**
* @brief Get a laser scan from the navigation system.
* @param laser_scan_name The name of the laser scan.
* @return The laser scan.
*/
virtual robot_sensor_msgs::LaserScan getLaserScan(const std::string &laser_scan_name) = 0;
/**
* @brief Remove a static map from the navigation system.
* @param map_name The name of the map.
* @return True if the map was removed successfully.
*/
virtual bool removeStaticMap(const std::string &map_name) = 0;
/**
* @brief Get a point cloud from the navigation system.
* @param point_cloud_name The name of the point cloud.
* @return The point cloud.
*/
virtual robot_sensor_msgs::PointCloud getPointCloud(const std::string &point_cloud_name) = 0;
/**
* @brief Remove a laser scan from the navigation system.
* @param laser_scan_name The name of the laser scan.
* @return True if the laser scan was removed successfully.
*/
virtual bool removeLaserScan(const std::string &laser_scan_name) = 0;
/**
* @brief Get a point cloud2 from the navigation system.
* @param point_cloud2_name The name of the point cloud2.
* @return The point cloud2.
*/
virtual robot_sensor_msgs::PointCloud2 getPointCloud2(const std::string &point_cloud2_name) = 0;
/**
* @brief Remove a point cloud from the navigation system.
* @param point_cloud_name The name of the point cloud.
* @return True if the point cloud was removed successfully.
*/
virtual bool removePointCloud(const std::string &point_cloud_name) = 0;
/**
* @brief Get all static maps from the navigation system.
* @return The static maps.
*/
virtual std::map<std::string, robot_nav_msgs::OccupancyGrid> getAllStaticMaps() = 0;
/**
* @brief Remove a point cloud2 from the navigation system.
* @param point_cloud2_name The name of the point cloud2.
* @return True if the point cloud2 was removed successfully.
*/
virtual bool removePointCloud2(const std::string &point_cloud2_name) = 0;
/**
* @brief Get all laser scans from the navigation system.
* @return The laser scans.
*/
virtual std::map<std::string, robot_sensor_msgs::LaserScan> getAllLaserScans() = 0;
/**
* @brief Remove all static maps from the navigation system.
* @return True if the static maps were removed successfully.
*/
virtual bool removeAllStaticMaps() = 0;
/**
* @brief Get all point clouds from the navigation system.
* @return The point clouds.
*/
virtual std::map<std::string, robot_sensor_msgs::PointCloud> getAllPointClouds() = 0;
/**
* @brief Remove all laser scans from the navigation system.
* @return True if the laser scans were removed successfully.
*/
virtual bool removeAllLaserScans() = 0;
/**
* @brief Get all point cloud2s from the navigation system.
* @return The point cloud2s.
*/
virtual std::map<std::string, robot_sensor_msgs::PointCloud2> getAllPointCloud2s() = 0;
/**
* @brief Remove all point clouds from the navigation system.
* @return True if the point clouds were removed successfully.
*/
virtual bool removeAllPointClouds() = 0;
/**
* @brief Remove a static map from the navigation system.
* @param map_name The name of the map.
* @return True if the map was removed successfully.
*/
virtual bool removeStaticMap(const std::string &map_name) = 0;
/**
* @brief Remove all point cloud2s from the navigation system.
* @return True if the point cloud2s were removed successfully.
*/
virtual bool removeAllPointCloud2s() = 0;
/**
* @brief Remove a laser scan from the navigation system.
* @param laser_scan_name The name of the laser scan.
* @return True if the laser scan was removed successfully.
*/
virtual bool removeLaserScan(const std::string &laser_scan_name) = 0;
/**
* @brief Remove all data from the navigation system.
* @return True if the data was removed successfully.
*/
virtual bool removeAllData() = 0;
/**
* @brief Remove a point cloud from the navigation system.
* @param point_cloud_name The name of the point cloud.
* @return True if the point cloud was removed successfully.
*/
virtual bool removePointCloud(const std::string &point_cloud_name) = 0;
/**
* @brief Add an odometry to the navigation system.
* @param odometry_name The name of the odometry.
* @param odometry The odometry to add.
*/
virtual void addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry) = 0;
/**
* @brief Remove a point cloud2 from the navigation system.
* @param point_cloud2_name The name of the point cloud2.
* @return True if the point cloud2 was removed successfully.
*/
virtual bool removePointCloud2(const std::string &point_cloud2_name) = 0;
/**
* @brief Send a goal for the robot to navigate to.
* @param goal Target pose in the global frame.
* @param xy_goal_tolerance Acceptable error in X/Y (meters).
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if goal was accepted and sent successfully.
*/
virtual bool moveTo(const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
/**
* @brief Remove all static maps from the navigation system.
* @return True if the static maps were removed successfully.
*/
virtual bool removeAllStaticMaps() = 0;
/**
* @brief Send a goal for the robot to navigate to.
* @param msg Order message.
* @param goal Target pose in the global frame.
* @param xy_goal_tolerance Acceptable error in X/Y (meters).
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if goal was accepted and sent successfully.
*/
virtual bool moveTo(const robot_protocol_msgs::Order &msg,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
/**
* @brief Remove all laser scans from the navigation system.
* @return True if the laser scans were removed successfully.
*/
virtual bool removeAllLaserScans() = 0;
/**
* @brief Send a docking goal to a predefined marker (e.g. charger).
* @param maker Marker name or ID.
* @param goal Target pose for docking.
* @param xy_goal_tolerance Acceptable XY error (meters).
* @param yaw_goal_tolerance Acceptable heading error (radians).
* @return True if docking command succeeded.
*/
virtual bool dockTo(const std::string &maker,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
/**
* @brief Remove all point clouds from the navigation system.
* @return True if the point clouds were removed successfully.
*/
virtual bool removeAllPointClouds() = 0;
/**
* @brief Send a docking goal to a predefined marker (e.g. charger).
* @param msg Order message.
* @param goal Target pose for docking.
* @param xy_goal_tolerance Acceptable XY error (meters).
* @param yaw_goal_tolerance Acceptable heading error (radians).
* @return True if docking command succeeded.
*/
virtual bool dockTo(const robot_protocol_msgs::Order &msg,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
/**
* @brief Remove all point cloud2s from the navigation system.
* @return True if the point cloud2s were removed successfully.
*/
virtual bool removeAllPointCloud2s() = 0;
/**
* @brief Move straight toward the target position (X-axis).
* @param goal Target pose.
* @param xy_goal_tolerance Acceptable positional error (meters).
* @return True if command issued successfully.
*/
virtual bool moveStraightTo(const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0) = 0;
/**
* @brief Remove all data from the navigation system.
* @return True if the data was removed successfully.
*/
virtual bool removeAllData() = 0;
/**
* @brief Rotate in place to align with target orientation.
* @param goal Pose containing desired heading (only Z-axis used).
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if rotation command was sent successfully.
*/
virtual bool rotateTo(const robot_geometry_msgs::PoseStamped &goal,
/**
* @brief Add an odometry to the navigation system.
* @param odometry_name The name of the odometry.
* @param odometry The odometry to add.
*/
virtual void addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry) = 0;
/**
* @brief Send a goal for the robot to navigate to.
* @param goal Target pose in the global frame.
* @param xy_goal_tolerance Acceptable error in X/Y (meters).
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if goal was accepted and sent successfully.
*/
virtual bool moveTo(const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
/**
* @brief Pause the robot's movement (e.g. during obstacle avoidance).
*/
virtual void pause() = 0;
/**
* @brief Send a goal for the robot to navigate to.
* @param msg Order message.
* @param goal Target pose in the global frame.
* @param xy_goal_tolerance Acceptable error in X/Y (meters).
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if goal was accepted and sent successfully.
*/
virtual bool moveTo(const robot_protocol_msgs::Order &msg,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
/**
* @brief Resume motion after a pause.
*/
virtual void resume() = 0;
/**
* @brief Send a docking goal to a predefined marker (e.g. charger).
* @param maker Marker name or ID.
* @param goal Target pose for docking.
* @param xy_goal_tolerance Acceptable XY error (meters).
* @param yaw_goal_tolerance Acceptable heading error (radians).
* @return True if docking command succeeded.
*/
virtual bool dockTo(const std::string &maker,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
/**
* @brief Cancel the current goal and stop the robot.
*/
virtual void cancel() = 0;
/**
* @brief Send a docking goal to a predefined marker (e.g. charger).
* @param msg Order message.
* @param goal Target pose for docking.
* @param xy_goal_tolerance Acceptable XY error (meters).
* @param yaw_goal_tolerance Acceptable heading error (radians).
* @return True if docking command succeeded.
*/
virtual bool dockTo(const robot_protocol_msgs::Order &msg,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
/**
* @brief Send limited linear velocity command.
* @param linear Linear velocity vector (x, y, z).
* @return True if the command was accepted.
*/
virtual bool setTwistLinear(const robot_geometry_msgs::Vector3 &linear) = 0;
/**
* @brief Move straight toward the target position (X-axis).
* @param goal Target pose.
* @param xy_goal_tolerance Acceptable positional error (meters).
* @return True if command issued successfully.
*/
virtual bool moveStraightTo(const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0) = 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 robot_geometry_msgs::Vector3 &angular) = 0;
/**
* @brief Rotate in place to align with target orientation.
* @param goal Pose containing desired heading (only Z-axis used).
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if rotation command was sent successfully.
*/
virtual bool rotateTo(const robot_geometry_msgs::PoseStamped &goal,
double yaw_goal_tolerance = 0.0) = 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(robot_geometry_msgs::PoseStamped &pose) = 0;
/**
* @brief Pause the robot's movement (e.g. during obstacle avoidance).
*/
virtual void pause() = 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(robot_geometry_msgs::Pose2D &pose) = 0;
/**
* @brief Resume motion after a pause.
*/
virtual void resume() = 0;
/**
* @brief Get the robot’s twist.
* @return The robot’s current twist.
*/
virtual robot_nav_2d_msgs::Twist2DStamped getTwist() = 0;
/**
* @brief Cancel the current goal and stop the robot.
*/
virtual void cancel() = 0;
/**
* @brief Get the navigation feedback.
* @return Pointer to the navigation feedback.
*/
virtual NavFeedback *getFeedback() = 0;
/**
* @brief Send limited linear velocity command.
* @param linear Linear velocity vector (x, y, z).
* @return True if the command was accepted.
*/
virtual bool setTwistLinear(const robot_geometry_msgs::Vector3 &linear) = 0;
/**
* @brief Get the global data.
* @return The global data.
*/
virtual PlannerDataOutput getGlobalData() = 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 robot_geometry_msgs::Vector3 &angular) = 0;
/**
* @brief Get the local data.
* @return The local data.
*/
virtual PlannerDataOutput getLocalData() = 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(robot_geometry_msgs::PoseStamped &pose) = 0;
protected:
// Shared feedback data for navigation status tracking
std::shared_ptr<NavFeedback> nav_feedback_;
robot_nav_2d_msgs::Twist2DStamped twist_;
robot_nav_msgs::Odometry odometry_;
PlannerDataOutput global_data_;
PlannerDataOutput local_data_;
/**
* @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(robot_geometry_msgs::Pose2D &pose) = 0;
std::map<std::string, robot_nav_msgs::OccupancyGrid> static_maps_;
std::map<std::string, robot_sensor_msgs::LaserScan> laser_scans_;
std::map<std::string, robot_sensor_msgs::PointCloud> point_clouds_;
std::map<std::string, robot_sensor_msgs::PointCloud2> point_cloud2s_;
/**
* @brief Get the robot’s twist.
* @return The robot’s current twist.
*/
virtual robot_nav_2d_msgs::Twist2DStamped getTwist() = 0;
BaseNavigation() = default;
};
/**
* @brief Get the navigation feedback.
* @return Pointer to the navigation feedback.
*/
virtual NavFeedback *getFeedback() = 0;
} // namespace move_base_core
/**
* @brief Get the global data.
* @return The global data.
*/
virtual PlannerDataOutput getGlobalData() = 0;
/**
* @brief Get the local data.
* @return The local data.
*/
virtual PlannerDataOutput getLocalData() = 0;
protected:
// Shared feedback data for navigation status tracking
std::shared_ptr<NavFeedback> nav_feedback_;
robot_nav_2d_msgs::Twist2DStamped twist_;
robot_nav_msgs::Odometry odometry_;
PlannerDataOutput global_data_;
PlannerDataOutput local_data_;
std::map<std::string, robot_nav_msgs::OccupancyGrid> static_maps_;
std::map<std::string, robot_sensor_msgs::LaserScan> laser_scans_;
std::map<std::string, robot_sensor_msgs::PointCloud> point_clouds_;
std::map<std::string, robot_sensor_msgs::PointCloud2> point_cloud2s_;
BaseNavigation() = default;
};
} // namespace move_base_core
};
#endif // _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_

7
src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h
View File

@ -133,6 +133,13 @@ namespace robot_nav_core
*/
virtual bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped> &plan) = 0;
/**
* @brief Gets the global plan for this local planner.
*
* @param path The global plan
*/
virtual void getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
/**
* @brief Constructs the local planner
* @param name The name to give this instance of the local planner

7
src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h
View File

@ -92,6 +92,13 @@ namespace robot_nav_core2
*/
virtual void setPlan(const robot_nav_2d_msgs::Path2D &path) = 0;
/**
* @brief Gets the global plan for this local planner.
*
* @param path The global plan
*/
virtual void getPlan(robot_nav_2d_msgs::Path2D &path) = 0;
/**
* @brief Compute the best command given the current pose, velocity and goal
*

8
src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h
View File

@ -159,6 +159,14 @@ namespace robot_nav_core_adapter
*/
bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped> &plan) override;
/**
* @brief Gets the global plan for this local planner.
*
* @param path The global plan
*/
virtual void getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) override;
/**
* @brief Create a new LocalPlannerAdapter
* @return A shared pointer to the new LocalPlannerAdapter

11
src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp
View File

@ -366,6 +366,17 @@ namespace robot_nav_core_adapter
}
}
void LocalPlannerAdapter::getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path)
{
if (!planner_)
{
return;
}
robot_nav_2d_msgs::Path2D path2d;
planner_->getPlan(path2d);
path = robot_nav_2d_utils::pathToPath(path2d).poses;
}
bool LocalPlannerAdapter::hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal)
{
if (last_goal_.header.frame_id != new_goal.header.frame_id ||

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

@ -607,7 +607,6 @@ void move_base::MoveBase::updateLocalCostmap(const T& value, robot_costmap_2d::L
void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry)
{
robot::log_info("[%s:%d] addOdometry called for: %s", __FILE__, __LINE__, odometry_name.c_str());
odometry_ = odometry;
}
@ -2206,6 +2205,20 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
{
if (tc_->computeVelocityCommands(odometry_.twist.twist, cmd_vel))
{
robot_nav_msgs::Path path;
tc_->getPlan(path.poses);
if (!path.poses.empty())
{
path.header.stamp = path.poses[0].header.stamp;
path.header.frame_id = path.poses[0].header.frame_id;
}
else
{
path.header.stamp = robot::Time::now();
path.header.frame_id = controller_costmap_robot_->getGlobalFrameID();
}
local_data_.plan = robot_nav_2d_utils::pathToPath(path);
last_valid_control_ = robot::Time::now();
// make sure that we send the velocity command to the base
@ -2283,6 +2296,9 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
lock.unlock();
}
}
twist_.velocity = robot_nav_2d_utils::twist3Dto2D(cmd_vel);
twist_.header.stamp = robot::Time::now();
twist_.header.frame_id = controller_costmap_robot_->getGlobalFrameID();
}
else
{