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
2026-02-06 15:54:35 +07:00
parent 6e320bbe5c
commit c0ceccf32c
11 changed files with 107 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
View File

@@ -177,6 +177,15 @@ namespace mkt_algorithm
const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D &drive_target,
const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd);
/**
* @brief Generate trajectory
* @param path
* @param sign_x
* @return trajectory
*/
robot_nav_2d_msgs::Path2D generateParallelPath(
const robot_nav_2d_msgs::Path2D &path, const double &sign_x);
/**
* @brief Generate Hermite trajectory
* @param pose

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

@@ -9,6 +9,8 @@ void mkt_algorithm::diff::GoStraight::initialize(
{
nh_ = nh;
nh_priv_ = robot::NodeHandle(nh, name);
robot::log_info_at(__FILE__, __LINE__, "nh_ : %s", nh_.getNamespace().c_str());
robot::log_info_at(__FILE__, __LINE__, "nh_priv_ : %s", nh_priv_.getNamespace().c_str());
name_ = name;
tf_ = tf;
traj_ = traj;
@@ -136,11 +138,14 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
}
else
{
// robot::log_info_at(__FILE__, __LINE__, "journey : %f lookahead_dist : %f",
// journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1), lookahead_dist);
if(fabs(carrot_pose.pose.y) > 0.2)
{
lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
}
robot_nav_2d_msgs::Twist2D drive_target;
robot::log_info_at(__FILE__, __LINE__, "y: %f theta: %f", transformed_plan.poses.front().pose.y, transformed_plan.poses.front().pose.theta);
transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
@@ -200,7 +205,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
fabs(pose_stamped.pose.theta - carrot_pose.pose.theta) < 1e-6)
break;
}
drive_cmd.theta = std::clamp(drive_cmd.theta, -fabs(0.02), fabs(0.02));
result.velocity = drive_cmd;
prevous_drive_cmd_ = drive_cmd;
return result;

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

@@ -10,6 +10,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
{
nh_ = nh;
nh_priv_ = robot::NodeHandle(nh, name);
robot::log_info_at(__FILE__, __LINE__, "nh_ : %s", nh_.getNamespace().c_str());
robot::log_info_at(__FILE__, __LINE__, "nh_priv_ : %s", nh_priv_.getNamespace().c_str());
name_ = name;
tf_ = tf;
traj_ = traj;
@@ -952,7 +954,6 @@ double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const rob
lookahead_dist = fabs(velocity.x) * lookahead_time_;
lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_);
}
return lookahead_dist;
}
@@ -1178,21 +1179,24 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
drive_cmd.theta = 0.0;
return robot_nav_2d_msgs::Path2D();
}
robot::log_info_at(__FILE__, __LINE__, "y: %f theta: %f", path.poses.front().pose.y, path.poses.front().pose.theta);
double max_kappa = calculateMaxKappa(path);
const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x);
drive_cmd.theta = max_vel_theta_;
if (max_kappa <= straight_threshold)
if (max_kappa <= straight_threshold) // nếu đường thẳng
{
if(journey(path.poses, 0, path.poses.size() - 1) <= min_journey_squared_)
{
drive_cmd.theta = 0.05;
// return path;
}
return generateHermiteTrajectory(path.poses.back(), sign_x);
if(fabs(path.poses.front().pose.y) > 0.02)
return generateHermiteTrajectory(path.poses.back(), sign_x);
else
return generateParallelPath(path, sign_x);
}
else
else // nếu đường cong
{
if(fabs(drive_cmd.x) < min_speed_xy_)
drive_cmd.x = std::copysign(min_speed_xy_, sign_x);
@@ -1200,6 +1204,46 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
}
}
robot_nav_2d_msgs::Path2D generateParallelPath(
const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
{
robot_nav_2d_msgs::Path2D parallel_path;
parallel_path.header = path.header;
int N = path.poses.size();
if (N < 2) return parallel_path;
double offset_y = path.poses.front().pose.y;
parallel_path.poses.resize(N);
for (int i = 0; i < N; ++i)
{
const auto& p = path.poses[i].pose;
double dx, dy;
if (i == 0) {
dx = path.poses[i+1].pose.x - p.x;
dy = path.poses[i+1].pose.y - p.y;
} else if (i == N-1) {
dx = p.x - path.poses[i-1].pose.x;
dy = p.y - path.poses[i-1].pose.y;
} else {
dx = path.poses[i+1].pose.x - path.poses[i-1].pose.x;
dy = path.poses[i+1].pose.y - path.poses[i-1].pose.y;
}
double theta = atan2(dy, dx);
double x_off = p.x - offset_y * sin(theta);
double y_off = p.y + offset_y * cos(theta);
parallel_path.poses[i].pose.x = x_off;
parallel_path.poses[i].pose.y = y_off;
parallel_path.poses[i].pose.theta = theta; // hoặc giữ nguyên p.theta
parallel_path.poses[i].header = path.poses[i].header;
}
return parallel_path;
}
robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(
const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x)
{

47
src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp
View File

@@ -153,14 +153,14 @@ namespace two_points_planner
robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
return false;
}
unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
|| end_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE))
{
robot::log_error("[%s:%d]\n TwoPointsPlanner: base_pootprint in infated obstacle", __FILE__, __LINE__);
return false;
}
// unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
// unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
// if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
// || end_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE))
// {
// robot::log_error("[%s:%d]\n TwoPointsPlanner: base_pootprint in infated obstacle", __FILE__, __LINE__);
// return false;
// }
robot::Time plan_time = robot::Time::now();
// Tính toán khoảng cách giữa điểm bắt đầu và điểm đích
const double dx = goal.pose.position.x - start.pose.position.x;
@@ -169,10 +169,11 @@ namespace two_points_planner
double theta;
if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
{
theta = atan2(dy, dx);
tf3::Quaternion goal_quat(goal.pose.orientation.x, goal.pose.orientation.y,
goal.pose.orientation.z, goal.pose.orientation.w);
double goal_theta = tf3::getYaw(goal_quat);
theta = std::atan2(dy, dx);
// tf3::Quaternion goal_quat(goal.pose.orientation.x, goal.pose.orientation.y,
// goal.pose.orientation.z, goal.pose.orientation.w);
// double goal_theta = tf3::getYaw(goal_quat);
double goal_theta = robot_nav_2d_utils::poseStampedToPose2D(goal).pose.theta;
if(cos(goal_theta - theta) < 0) theta += M_PI;
}
else
@@ -192,23 +193,15 @@ namespace two_points_planner
{
double fraction = static_cast<double>(i) / num_points;
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = start.pose.position.x + fraction * dx;
pose.pose.position.y = start.pose.position.y + fraction * dy;
pose.pose.position.z = start.pose.position.z;
robot_nav_2d_msgs::Pose2DStamped pose2d;
pose2d.header.stamp = plan_time;
pose2d.header.frame_id = costmap_robot_->getGlobalFrameID();
pose2d.pose.x = start.pose.position.x + fraction * dx;
pose2d.pose.y = start.pose.position.y + fraction * dy;
pose2d.pose.theta = theta;
// Nội suy hướng
tf3::Quaternion interpolated_quat;
interpolated_quat.setRPY(0, 0, theta);
// Chuyển đổi trực tiếp từ tf3::Quaternion sang robot_geometry_msgs::Quaternion
pose.pose.orientation.x = interpolated_quat.x();
pose.pose.orientation.y = interpolated_quat.y();
pose.pose.orientation.z = interpolated_quat.z();
pose.pose.orientation.w = interpolated_quat.w();
robot_geometry_msgs::PoseStamped pose = robot_nav_2d_utils::pose2DToPoseStamped(pose2d);
plan.push_back(pose);
}
plan.push_back(goal);

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

@@ -95,8 +95,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_nav_name.c_str());
exit(1);
}
robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
nav_algorithm_->initialize(nh_algorithm, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
}
catch (const std::exception& ex)
{
@@ -209,6 +208,8 @@ 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;
cmd_vel.header.stamp = robot::Time::now();
if (ret_nav_)
{
local_plan_.poses.clear();
@@ -217,7 +218,7 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner
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());
robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
local_plan_ = robot_nav_2d_utils::pathToPath(path);
cmd_vel.velocity = traj.velocity;
return cmd_vel;

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

@@ -213,7 +213,7 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::Sc
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());
robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
local_plan_ = robot_nav_2d_utils::pathToPath(path);
cmd_vel.velocity = traj.velocity;
return cmd_vel;

7
src/Navigations/Cores/move_base_core/CMakeLists.txt
View File

@@ -31,6 +31,7 @@ if (NOT BUILDING_WITH_CATKIN)
robot_geometry_msgs
robot_protocol_msgs
robot_nav_2d_msgs
robot_nav_2d_utils
robot_nav_msgs
robot_sensor_msgs
)
@@ -41,17 +42,19 @@ else()
# Catkin specific configuration
# ========================================================
find_package(catkin REQUIRED COMPONENTS
tf3
robot_time
robot_geometry_msgs
robot_protocol_msgs
robot_nav_2d_msgs
robot_nav_2d_utils
)
find_package(tf3)
catkin_package(
INCLUDE_DIRS include
# LIBRARIES không cần vì đây là header-only library
CATKIN_DEPENDS tf3 robot_time robot_geometry_msgs robot_protocol_msgs robot_nav_2d_msgs
CATKIN_DEPENDS robot_time robot_geometry_msgs robot_protocol_msgs robot_nav_2d_msgs
DEPENDS tf3
)
include_directories(

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

@@ -18,6 +18,7 @@
#include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_protocol_msgs/Order.h>
#include <robot_nav_2d_utils/conversions.h>
// tf
#include <tf3/buffer_core.h>
@@ -138,10 +139,7 @@ namespace robot
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);
goal.pose.orientation = robot_nav_2d_utils::pose2DToPose(pose).orientation;
return goal;
}
@@ -161,7 +159,7 @@ namespace robot
robot_geometry_msgs::Pose2D pose2d;
pose2d.x = pose.pose.position.x;
pose2d.y = pose.pose.position.y;
// pose2d.theta = tf2::getYaw(pose.pose.orientation);
pose2d.theta = robot_nav_2d_utils::poseStampedToPose2D(pose).pose.theta;
return offset_goal(pose2d, pose.header.frame_id, offset_distance);
}

2
src/Navigations/Cores/move_base_core/package.xml
View File

@@ -29,6 +29,7 @@
<build_depend>robot_geometry_msgs</build_depend>
<build_depend>robot_protocol_msgs</build_depend>
<build_depend>robot_nav_2d_msgs</build_depend>
<build_depend>robot_nav_2d_utils</build_depend>
<build_depend>robot_nav_msgs</build_depend>
<build_depend>robot_sensor_msgs</build_depend>
<build_depend>robot_map_msgs</build_depend>
@@ -38,6 +39,7 @@
<run_depend>robot_geometry_msgs</run_depend>
<run_depend>robot_protocol_msgs</run_depend>
<run_depend>robot_nav_2d_msgs</run_depend>
<run_depend>robot_nav_2d_utils</run_depend>
<run_depend>robot_nav_msgs</run_depend>
<run_depend>robot_sensor_msgs</run_depend>
<run_depend>robot_map_msgs</run_depend>