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

update 24/2

Browse Source
This commit is contained in:
HiepLM
2026-02-24 14:39:49 +07:00
parent 5069931a87
commit 95c6fe0f1e
9 changed files with 241 additions and 216 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
config/pnkx_local_planner_params.yaml
View File

@@ -53,10 +53,10 @@ LimitedAccelGenerator:
max_vel_theta: 0.7 # max_rot_vel: 1.0 # choose slightly less than the base's capability
min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity
acc_lim_x: 1.0
acc_lim_x: 1.5
acc_lim_y: 0.0 # diff drive robot
acc_lim_theta: 1.5
decel_lim_x: -1.0
decel_lim_x: -1.5
decel_lim_y: -0.0
decel_lim_theta: -1.5
@@ -88,7 +88,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
max_lookahead_dist: 2.0 # The maximum lookahead distance (m) threshold. (default: 0.9)
lookahead_time: 1.9 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
min_journey_squared: 0.2 # Minimum squared journey to consider for goal (default: 0.2)
max_journey_squared: 0.8 # Maximum squared journey to consider for goal (default: 0.2)
max_journey_squared: 0.5 # Maximum squared journey to consider for goal (default: 0.2)
max_lateral_accel: 0.9 # Max lateral accel for speed reduction on curves (m/s^2)
# Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
@@ -105,7 +105,9 @@ MKTAlgorithmDiffPredictiveTrajectory:
final_heading_xy_tolerance: 0.1
final_heading_angle_tolerance: 0.05
final_heading_min_velocity: 0.05
final_heading_kp_angular: 2.0
final_heading_kp_angular: 1.2
final_heading_ki_angular: 0.002
final_heading_kd_angular: 0.12
MKTAlgorithmDiffGoStraight:
library_path: libmkt_algorithm_diff
@@ -140,7 +142,9 @@ MKTAlgorithmDiffGoStraight:
final_heading_xy_tolerance: 0.1
final_heading_angle_tolerance: 0.05
final_heading_min_velocity: 0.05
final_heading_kp_angular: 2.0
final_heading_kp_angular: 1.5
final_heading_ki_angular: 0.2
final_heading_kd_angular: 0.05
MKTAlgorithmDiffRotateToGoal:
library_path: libmkt_algorithm_diff
@@ -176,6 +180,9 @@ MKTAlgorithmDiffRotateToGoal:
final_heading_angle_tolerance: 0.05
final_heading_min_velocity: 0.05
final_heading_kp_angular: 2.0
near_goal_heading_kp: 1.5
near_goal_heading_ki: 0.0
near_goal_heading_kd: 0.0
GoalChecker:
library_path: libmkt_plugins_goal_checker

11
examples/NavigationExample/Program.cs
View File

@@ -241,6 +241,7 @@ namespace NavigationExample
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback);
/// <summary>Helper: copy unmanaged char* to managed string; does not free the pointer.</summary>
public static string MarshalString(IntPtr p)
{
@@ -572,8 +573,14 @@ namespace NavigationExample
Marshal.OffsetOf<NavigationAPI.OccupancyGrid>("data_count"));
NavigationAPI.navigation_add_static_map(navHandle, "/map", occupancyGrid);
// Cleanup
System.Threading.Thread.Sleep(1000);
NavigationAPI.Twist2DStamped twist = new NavigationAPI.Twist2DStamped();
if (NavigationAPI.navigation_get_twist(navHandle, ref twist))
{
Console.WriteLine("Twist: {0}, {1}, {2}, {3}", NavigationAPI.MarshalString(twist.header.frame_id), twist.velocity.x, twist.velocity.y, twist.velocity.theta);
} // Cleanup
NavigationAPI.navigation_destroy(navHandle);
NavigationAPI.tf_listener_destroy(tfHandle);
}

BIN
examples/NavigationExample/libnav_c_api.so
View File

Binary file not shown.

19
src/APIs/c_api/src/convertor.cpp
View File

@@ -10,8 +10,6 @@ robot_sensor_msgs::LaserScan convert2CppLaserScan(const LaserScan& laser_scan)
robot_laser_scan.header.seq = laser_scan.header.seq;
robot_laser_scan.header.stamp.sec = laser_scan.header.sec;
robot_laser_scan.header.stamp.nsec = laser_scan.header.nsec;
robot::log_info("LaserScan header.stamp.sec=%d header.stamp.nsec=%d", laser_scan.header.sec, laser_scan.header.nsec);
robot::log_info("LaserScan header.frame_id=%s", laser_scan.header.frame_id);
robot_laser_scan.header.frame_id = laser_scan.header.frame_id;
robot_laser_scan.angle_min = laser_scan.angle_min;
robot_laser_scan.angle_max = laser_scan.angle_max;
@@ -59,13 +57,10 @@ robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occu
size_t data_size = occupancy_grid.data ? occupancy_grid.data_count : 0;
// robot_occupancy_grid.data.resize(data_size);
// robot::log_info("convertOccupancyGrid: 2");
std::stringstream ss;
robot_occupancy_grid.data.resize(data_size);
for (size_t i = 0; i < data_size; i++) {
ss << occupancy_grid.data[i] << " ";
robot_occupancy_grid.data[i] = occupancy_grid.data[i];
}
robot::log_info("occupancy_grid.data: %s", ss.str().c_str());
return robot_occupancy_grid;
}
@@ -75,9 +70,9 @@ robot_nav_msgs::Odometry convert2CppOdometry(const Odometry& odometry)
robot_odometry.header.seq = odometry.header.seq;
robot_odometry.header.stamp.sec = odometry.header.sec;
robot_odometry.header.stamp.nsec = odometry.header.nsec;
robot_odometry.header.frame_id = odometry.header.frame_id ? odometry.header.frame_id : "";
robot_odometry.header.frame_id = odometry.header.frame_id ? odometry.header.frame_id : "base_link";
robot_odometry.child_frame_id = odometry.child_frame_id ? odometry.child_frame_id : "";
robot_odometry.child_frame_id = odometry.child_frame_id ? odometry.child_frame_id : "base_link";
robot_odometry.pose.pose.position.x = odometry.pose.pose.position.x;
robot_odometry.pose.pose.position.y = odometry.pose.pose.position.y;
robot_odometry.pose.pose.position.z = odometry.pose.pose.position.z;
@@ -136,7 +131,7 @@ PoseStamped convert2CPoseStamped(const robot_geometry_msgs::PoseStamped& cpp_pos
c_pose.header.seq = cpp_pose.header.seq;
c_pose.header.sec = cpp_pose.header.stamp.sec;
c_pose.header.nsec = cpp_pose.header.stamp.nsec;
c_pose.header.frame_id = (char*)cpp_pose.header.frame_id.c_str();
c_pose.header.frame_id = cpp_pose.header.frame_id.empty() ? nullptr : strdup(cpp_pose.header.frame_id.c_str());
c_pose.pose.position.x = cpp_pose.pose.position.x;
c_pose.pose.position.y = cpp_pose.pose.position.y;
c_pose.pose.position.z = cpp_pose.pose.position.z;
@@ -156,13 +151,13 @@ Pose2D convert2CPose2D(const robot_geometry_msgs::Pose2D& cpp_pose)
c_pose.theta = cpp_pose.theta;
return c_pose;
}
Header convert2CHeader(const robot_std_msgs::Header& cpp_header)
Header convert2CHeader(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;
c_header.frame_id = (char*)cpp_header.frame_id.c_str();
c_header.frame_id = cpp_header.frame_id.empty() ? nullptr : strdup(cpp_header.frame_id.c_str());
return c_header;
}

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

@@ -549,8 +549,10 @@ extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped &ou
try
{
robot::move_base_core::BaseNavigation *nav =
static_cast<robot::move_base_core::BaseNavigation *>(handle.ptr);
auto *nav_ptr = static_cast<std::shared_ptr<robot::move_base_core::BaseNavigation> *>(handle.ptr);
if (!nav_ptr || !*nav_ptr)
return false;
auto *nav = nav_ptr->get();
robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav->getTwist();
out_twist = convert2CTwist2DStamped(cpp_twist);
return true;

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

@@ -30,7 +30,8 @@ namespace mkt_algorithm
class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm
{
public:
PredictiveTrajectory() : initialized_(false), nav_stop_(false) {};
PredictiveTrajectory() : initialized_(false), nav_stop_(false),
near_goal_heading_integral_(0.0), near_goal_heading_last_error_(0.0), near_goal_heading_was_active_(false) {};
virtual ~PredictiveTrajectory();
@@ -255,6 +256,19 @@ namespace mkt_algorithm
const double &sign_x,
const double &dt,
robot_nav_2d_msgs::Twist2D &cmd_vel);
/**
* @brief PID controller
* @param error
* @param integral
* @param last_error
* @param dt
* @param kp
* @param ki
* @param kd
* @param output
*/
void pid(const double &error, double &integral, double &last_error, const double &dt, const double &kp, const double &ki, const double &kd, double &output);
/**
* @brief the robot is moving acceleration limits
@@ -368,6 +382,12 @@ namespace mkt_algorithm
double final_heading_angle_tolerance_; // Angle threshold to consider heading reached
double final_heading_min_velocity_; // Minimum linear velocity during alignment
double final_heading_kp_angular_; // Proportional gain for angular control
double final_heading_ki_angular_;
double final_heading_kd_angular_;
double near_goal_heading_integral_;
double near_goal_heading_last_error_;
bool near_goal_heading_was_active_;
// Regulated linear velocity scaling
bool use_regulated_linear_velocity_scaling_;

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

@@ -117,6 +117,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
nh_priv_.param<double>("final_heading_angle_tolerance", final_heading_angle_tolerance_, 0.05);
nh_priv_.param<double>("final_heading_min_velocity", final_heading_min_velocity_, 0.05);
nh_priv_.param<double>("final_heading_kp_angular", final_heading_kp_angular_, 1.5);
nh_priv_.param<double>("final_heading_ki_angular", final_heading_ki_angular_, 0.0);
nh_priv_.param<double>("final_heading_kd_angular", final_heading_kd_angular_, 0.0);
nh_priv_.param<double>("rot_stopped_velocity", rot_stopped_velocity_, 0.01);
nh_priv_.param<double>("trans_stopped_velocity", trans_stopped_velocity_, 0.01);
@@ -178,6 +180,9 @@ void mkt_algorithm::diff::PredictiveTrajectory::reset()
this->nav_stop_ = false;
last_actuator_update_ = robot::Time::now();
prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D();
near_goal_heading_integral_ = 0.0;
near_goal_heading_last_error_ = 0.0;
near_goal_heading_was_active_ = false;
if (kf_)
{
@@ -387,7 +392,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
}
mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
{
mkt_msgs::Trajectory2D result;
result.velocity.x = result.velocity.y = result.velocity.theta = 0.0;
@@ -472,16 +477,16 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
// Use Arc Motion controller for final heading alignment
alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
#ifdef BUILD_WITH_ROS
ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
ROS_INFO("heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
#endif
}
else
{
if(fabs(carrot_pose.pose.y) > 0.2)
{
lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * 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;
transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
@@ -512,24 +517,24 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
return result;
}
Eigen::VectorXd y(2);
y << drive_cmd.x, drive_cmd.theta;
// Eigen::VectorXd y(2);
// y << drive_cmd.x, drive_cmd.theta;
// Cập nhật lại A nếu dt thay đổi
for (int i = 0; i < n_; ++i)
for (int j = 0; j < n_; ++j)
A(i, j) = (i == j ? 1.0 : 0.0);
for (int i = 0; i < n_; i += 3)
{
A(i, i + 1) = dt;
A(i, i + 2) = 0.5 * dt * dt;
A(i + 1, i + 2) = dt;
}
kf_->update(y, dt, A);
double v_min = min_approach_linear_velocity_;
drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
// // Cập nhật lại A nếu dt thay đổi
// for (int i = 0; i < n_; ++i)
// for (int j = 0; j < n_; ++j)
// A(i, j) = (i == j ? 1.0 : 0.0);
// for (int i = 0; i < n_; i += 3)
// {
// A(i, i + 1) = dt;
// A(i, i + 2) = 0.5 * dt * dt;
// A(i + 1, i + 2) = dt;
// }
// kf_->update(y, dt, A);
// double v_min = min_approach_linear_velocity_;
// drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
// drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
// drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
}
result.poses.clear();
@@ -593,7 +598,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
double w_target = v_target * kappa;
if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
{
const double k_heading = 0.3;
if (trajectory.poses.size() >= 2) {
const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose;
double heading_ref = 0.0;
@@ -610,17 +614,31 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
break;
}
}
// robot heading in base_link = 0, so error = heading_ref
double w_heading = k_heading * angles::normalize_angle(heading_ref);
w_target += w_heading;
const double error = angles::normalize_angle(heading_ref);
double w_heading = 0.0;
pid(error,
near_goal_heading_integral_,
near_goal_heading_last_error_,
dt,
final_heading_kp_angular_,
final_heading_ki_angular_,
final_heading_kd_angular_,
w_heading);
// Apply acceleration limits
double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
w_target = velocity.theta + dw_heading;
}
else
{
w_target = 0.0;
near_goal_heading_was_active_ = false;
}
}
else
{
near_goal_heading_was_active_ = false;
}
w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
// 6) Apply acceleration limits (linear + angular)
@@ -629,7 +647,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
drive_cmd.x = velocity.x + dv;
drive_cmd.theta = velocity.theta + dw;
// robot::log_info("v_target: %f, w_target: %f, dv: %f, dw: %f, drive_cmd.x: %f, drive_cmd.theta: %f", v_target, w_target, dv, dw, drive_cmd.x, drive_cmd.theta);
}
void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -715,14 +732,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
// (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate;
bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate;
#ifdef BUILD_WITH_ROS
if (result)
ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
// if (result)
// ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
// else if(fabs(velocity.x) < min_speed_xy_)
// {
// ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
// ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
// }
#endif
return result;
}
@@ -879,7 +898,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
if (abs_heading_error > final_heading_angle_tolerance_)
{
// Method 1: Proportional control
double omega_p = final_heading_kp_angular_ * heading_error;
double omega_p = 0.0;
pid(heading_error,
near_goal_heading_integral_,
near_goal_heading_last_error_,
dt,
final_heading_kp_angular_,
final_heading_ki_angular_,
final_heading_kd_angular_,
omega_p);
// Method 2: Arc-based feedforward (coordinate linear and angular motion)
double omega_arc = 0.0;
@@ -911,7 +938,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
}
}
// --- Apply acceleration limits ---
// Linear
double v_current = velocity.x;
@@ -944,6 +970,26 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
#endif
}
void mkt_algorithm::diff::PredictiveTrajectory::pid(
const double &error, double &integral, double &last_error, const double &dt, const double &kp, const double &ki, const double &kd, double &output)
{
if (!near_goal_heading_was_active_)
{
integral = 0.0;
last_error = error;
near_goal_heading_was_active_ = true;
}
const double dt_safe = std::max(dt, 1e-6);
double w_p = kp * error;
integral += error * dt_safe;
const double integral_cap = (std::fabs(ki) > 1e-9)? (2.0 / ki) : 1e6;
integral = std::clamp(integral, -integral_cap, integral_cap);
double w_i = ki * integral;
double w_d = kd * (error - last_error) / dt_safe;
last_error = error;
output = w_p + w_i + w_d;
}
double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity)
{
// If using velocity-scaled look ahead distances, find and clamp the dist
@@ -957,71 +1003,6 @@ double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const rob
return lookahead_dist;
}
// std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
// mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan)
// {
// if (global_plan.poses.empty())
// {
// throw robot_nav_core2::PlannerTFException("The global plan passed to the local planner is empty.");
// }
// if ((unsigned)global_plan.poses.size() < 2)
// {
// auto goal_pose_it = std::prev(global_plan.poses.end());
// return goal_pose_it;
// }
// unsigned int goal_index = (unsigned)global_plan.poses.size() - 1;
// double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x;
// double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y;
// double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x;
// double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y;
// // make sure that atan2 is defined
// double start_angle = atan2(start_direction_y, start_direction_x);
// double end_angle = atan2(end_direction_y, end_direction_x);
// double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2);
// for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++)
// {
// if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9)
// {
// const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x;
// const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y;
// if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
// {
// double current_angle = atan2(current_direction_y, current_direction_x);
// goal_index = i;
// if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_))
// continue;
// if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold)
// {
// goal_index = i;
// break;
// }
// }
// }
// }
// // Find the first pose which is at a distance greater than the lookahead distance
// auto goal_pose_it = std::find_if(
// global_plan.poses.begin(), global_plan.poses.begin() + goal_index,
// [&](const auto &ps)
// {
// return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist;
// });
// // If the number of poses is not far enough, take the last pose
// if (goal_pose_it == global_plan.poses.end())
// {
// goal_pose_it = std::prev(global_plan.poses.end());
// }
// return goal_pose_it;
// }
std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan)
{
@@ -1144,18 +1125,29 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
double v_target,
const double &sign_x)
{
if (journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
return min_speed_xy_ * sign_x;
if(trajectory.poses.size() < 2)
return min_approach_linear_velocity_ * sign_x;
double preview_distance = std::clamp(std::fabs(velocity.x) * lookahead_time_, min_lookahead_dist_, max_lookahead_dist_);
double max_kappa = calculateMaxKappa(trajectory, preview_distance);
double v_limit = std::fabs(v_target);
double journey_distance = journey(trajectory.poses, 0, trajectory.poses.size() - 1);
if (journey_distance < min_journey_squared_)
{
v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, min_speed_xy_) * sign_x;
}
if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6)
{
const double v_curve = std::sqrt(max_lateral_accel_ / max_kappa);
v_limit = std::min(v_limit, v_curve);
}
if(trajectory.poses.size() > 2 && fabs(trajectory.poses.front().pose.theta) >= angle_threshold_)
v_limit = min_speed_xy_ * sign_x;
if (fabs(decel_lim_x_) > 1e-6)
{
const double remaining = journey(trajectory.poses, 0, trajectory.poses.size() - 1);
@@ -1179,22 +1171,18 @@ 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) // nếu đường thẳng
if (max_kappa <= straight_threshold && fabs(path.poses.back().pose.x) < min_lookahead_dist_) // nếu đường thẳng
{
if(journey(path.poses, 0, path.poses.size() - 1) <= min_journey_squared_)
if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.front().pose.x) < (min_lookahead_dist_ + max_path_distance_))
{
drive_cmd.theta = 0.05;
}
if(fabs(path.poses.front().pose.y) > 0.02)
return generateHermiteTrajectory(path.poses.back(), sign_x);
else
return generateParallelPath(path, sign_x);
}
return generateHermiteTrajectory(path.poses.back(), sign_x);
}
else // nếu đường cong
{
@@ -1204,7 +1192,7 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
}
}
robot_nav_2d_msgs::Path2D generateParallelPath(
robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateParallelPath(
const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
{
robot_nav_2d_msgs::Path2D parallel_path;
@@ -1232,8 +1220,8 @@ robot_nav_2d_msgs::Path2D generateParallelPath(
}
double theta = atan2(dy, dx);
double x_off = p.x - offset_y * sin(theta);
double y_off = p.y + offset_y * cos(theta);
double x_off = p.x - offset_y * sin(theta)*sign_x;
double y_off = p.y - offset_y * cos(theta)*sign_x;
parallel_path.poses[i].pose.x = x_off;
parallel_path.poses[i].pose.y = y_off;

35
src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp
View File

@@ -29,8 +29,6 @@ void mkt_plugins::GoalChecker::intParam(const robot::NodeHandle &nh)
{
yaw_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1);
}
// ROS_INFO_THROTTLE(0.5, "%s", nh_.getNamespace().c_str());
// ROS_INFO_THROTTLE(0.5, "[GoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f",xy_goal_tolerance_, yaw_goal_tolerance_);
}
void mkt_plugins::GoalChecker::reset()
@@ -49,19 +47,27 @@ bool mkt_plugins::GoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStam
if (!path.poses.empty() && path.poses.size() > 2)
{
double theta = angles::normalize_angle(query_pose.pose.theta - goal_pose.pose.theta);
for(int i = path.poses.size() - 1; i >= 0; i--)
{
double dx = path.poses[i].pose.x - path.poses.back().pose .x;
double dy = path.poses[i].pose.y - path.poses.back().pose.y;
if(std::sqrt(dx * dx + dy * dy) > 0.05 // TODO: get resolution from costmap
&& fabs(dx) > 1e-6 && fabs(dy) > 1e-6)
{
theta = angles::normalize_angle(query_pose.pose.theta - atan2(dy, dx));
break;
}
}
double x = cos(goal_pose.pose.theta) * dx + sin(goal_pose.pose.theta) * dy;
double y = -sin(goal_pose.pose.theta) * dx + cos(goal_pose.pose.theta) * dy;
if (xy_tolerance <= xy_goal_tolerance_ + 0.3)
if (xy_tolerance <= xy_goal_tolerance_ + 0.1)
{
double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
// ROS_INFO_THROTTLE(0.1, "Goal checker 1 %f %f %f %f %f %f", tolerance, old_xy_goal_tolerance_, goal_pose.pose.theta, x, y, theta);
if(
(fabs(tolerance) <= xy_goal_tolerance_ || tolerance * old_xy_goal_tolerance_ < 0)
// && (fabs(cos(theta))- fabs(sin(theta))) > cos(M_PI_4)
)
if(fabs(tolerance) <= xy_goal_tolerance_)
{
robot::log_info_throttle(0.1, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), xy_goal_tolerance_, yaw_goal_tolerance_);
robot::log_info_throttle(0.1, "Goal checker 1 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
robot::log_info_at(__FILE__, __LINE__, "%x %x", fabs(tolerance) <= xy_goal_tolerance_, tolerance * old_xy_goal_tolerance_ < 0);
robot::log_info_at(__FILE__, __LINE__, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), xy_goal_tolerance_, yaw_goal_tolerance_);
robot::log_info_at(__FILE__, __LINE__, "Goal checker 1 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
return true;
}
}
@@ -73,16 +79,15 @@ bool mkt_plugins::GoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStam
double theta = angles::normalize_angle(query_pose.pose.theta - goal_pose.pose.theta);
double x = cos(goal_pose.pose.theta) * dx + sin(goal_pose.pose.theta) * dy;
double y = -sin(goal_pose.pose.theta) * dx + cos(goal_pose.pose.theta) * dy;
if (xy_tolerance <= xy_goal_tolerance_ + 0.3)
if (xy_tolerance <= xy_goal_tolerance_ + 0.1)
{
double tolerance = fabs(x) > fabs(y) ? x : y;
if(
(fabs(tolerance) <= xy_goal_tolerance_ || tolerance * old_xy_goal_tolerance_ < 0)
// && (fabs(cos(theta))- fabs(sin(theta))) > cos(M_PI_4)
)
{
robot::log_info_throttle(0.1, "%f %f", fabs(cos(theta)), fabs(sin(theta)));
robot::log_info_throttle(0.1, "Goal checker 2 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
robot::log_info_at(__FILE__, __LINE__, "%f %f", fabs(cos(theta)), fabs(sin(theta)));
robot::log_info_at(__FILE__, __LINE__, "Goal checker 2 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
return true;
}
}
@@ -92,7 +97,7 @@ bool mkt_plugins::GoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStam
if (xy_tolerance <= xy_goal_tolerance_)
{
robot::log_info_at(__FILE__, __LINE__, "Goal checker 0 %f %f", xy_tolerance, xy_goal_tolerance_);
robot::log_info_at(__FILE__, __LINE__, "Goal checker ok %f %f", xy_tolerance, xy_goal_tolerance_);
return true;
}
return false;

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

@@ -416,24 +416,24 @@ void move_base::MoveBase::addStaticMap(const std::string &map_name, robot_nav_ms
{
it->second = map;
}
robot::log_info("map header: %s", map.header.frame_id.c_str());
robot::log_info("map header: %ld.%ld", map.header.stamp.sec, map.header.stamp.nsec);
robot::log_info("map header: %d", map.header.seq);
robot::log_info("map info resolution: %f", map.info.resolution);
robot::log_info("map info width: %d", map.info.width);
robot::log_info("map info height: %d", map.info.height);
robot::log_info("map info origin position x: %f", map.info.origin.position.x);
robot::log_info("map info origin position y: %f", map.info.origin.position.y);
robot::log_info("map info origin position z: %f", map.info.origin.position.z);
robot::log_info("map info origin orientation x: %f", map.info.origin.orientation.x);
robot::log_info("map info origin orientation y: %f", map.info.origin.orientation.y);
robot::log_info("map info origin orientation z: %f", map.info.origin.orientation.z);
robot::log_info("map info origin orientation w: %f", map.info.origin.orientation.w);
robot::log_info("map data size: %zu", map.data.size());
for(size_t i = 0; i < map.data.size(); i++) {
robot::log_info("map data[%zu]: %d", i, map.data[i]);
}
robot::log_info("--------------------------------");
// robot::log_info("map header: %s", map.header.frame_id.c_str());
// robot::log_info("map header: %ld.%ld", map.header.stamp.sec, map.header.stamp.nsec);
// robot::log_info("map header: %d", map.header.seq);
// robot::log_info("map info resolution: %f", map.info.resolution);
// robot::log_info("map info width: %d", map.info.width);
// robot::log_info("map info height: %d", map.info.height);
// robot::log_info("map info origin position x: %f", map.info.origin.position.x);
// robot::log_info("map info origin position y: %f", map.info.origin.position.y);
// robot::log_info("map info origin position z: %f", map.info.origin.position.z);
// robot::log_info("map info origin orientation x: %f", map.info.origin.orientation.x);
// robot::log_info("map info origin orientation y: %f", map.info.origin.orientation.y);
// robot::log_info("map info origin orientation z: %f", map.info.origin.orientation.z);
// robot::log_info("map info origin orientation w: %f", map.info.origin.orientation.w);
// robot::log_info("map data size: %zu", map.data.size());
// for(size_t i = 0; i < map.data.size(); i++) {
// robot::log_info("map data[%zu]: %d", i, map.data[i]);
// }
// robot::log_info("--------------------------------");
updateGlobalCostmap<robot_nav_msgs::OccupancyGrid>(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name);
updateLocalCostmap<robot_nav_msgs::OccupancyGrid>(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name);
}
@@ -459,28 +459,28 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot
{
it->second = laser_scan;
}
robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec);
robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str());
robot::log_info("angle_min: %f", laser_scan.angle_min);
robot::log_info("angle_max: %f", laser_scan.angle_max);
robot::log_info("angle_increment: %f", laser_scan.angle_increment);
robot::log_info("time_increment: %f", laser_scan.time_increment);
robot::log_info("scan_time: %f", laser_scan.scan_time);
robot::log_info("range_min: %f", laser_scan.range_min);
robot::log_info("range_max: %f", laser_scan.range_max);
robot::log_info("ranges_size: %zu", laser_scan.ranges.size());
robot::log_info("intensities_size: %zu", laser_scan.intensities.size());
std::stringstream ranges_str;
for (size_t i = 0; i < laser_scan.ranges.size(); i++) {
ranges_str << laser_scan.ranges[i] << " ";
}
robot::log_info("ranges: %s", ranges_str.str().c_str());
std::stringstream intensities_str;
for (size_t i = 0; i < laser_scan.intensities.size(); i++) {
intensities_str << laser_scan.intensities[i] << " ";
}
robot::log_info("intensities: %s", intensities_str.str().c_str());
robot::log_info("--------------------------------");
// robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec);
// robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str());
// robot::log_info("angle_min: %f", laser_scan.angle_min);
// robot::log_info("angle_max: %f", laser_scan.angle_max);
// robot::log_info("angle_increment: %f", laser_scan.angle_increment);
// robot::log_info("time_increment: %f", laser_scan.time_increment);
// robot::log_info("scan_time: %f", laser_scan.scan_time);
// robot::log_info("range_min: %f", laser_scan.range_min);
// robot::log_info("range_max: %f", laser_scan.range_max);
// robot::log_info("ranges_size: %zu", laser_scan.ranges.size());
// robot::log_info("intensities_size: %zu", laser_scan.intensities.size());
// std::stringstream ranges_str;
// for (size_t i = 0; i < laser_scan.ranges.size(); i++) {
// ranges_str << laser_scan.ranges[i] << " ";
// }
// robot::log_info("ranges: %s", ranges_str.str().c_str());
// std::stringstream intensities_str;
// for (size_t i = 0; i < laser_scan.intensities.size(); i++) {
// intensities_str << laser_scan.intensities[i] << " ";
// }
// robot::log_info("intensities: %s", intensities_str.str().c_str());
// robot::log_info("--------------------------------");
updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
@@ -699,32 +699,33 @@ 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("addOdometry: %s", odometry_name.c_str());
robot::log_info("odometry header: %s", odometry.header.frame_id.c_str());
robot::log_info("odometry child_frame_id: %s", odometry.child_frame_id.c_str());
robot::log_info("odometry pose position x: %f", odometry.pose.pose.position.x);
robot::log_info("odometry pose position y: %f", odometry.pose.pose.position.y);
robot::log_info("odometry pose position z: %f", odometry.pose.pose.position.z);
robot::log_info("odometry pose orientation x: %f", odometry.pose.pose.orientation.x);
robot::log_info("odometry pose orientation y: %f", odometry.pose.pose.orientation.y);
robot::log_info("odometry pose orientation z: %f", odometry.pose.pose.orientation.z);
robot::log_info("odometry pose orientation w: %f", odometry.pose.pose.orientation.w);
std::stringstream pose_covariance_str;
for(int i = 0; i < 36; i++) {
pose_covariance_str << odometry.pose.covariance[i] << " ";
}
robot::log_info("odometry pose covariance: %s", pose_covariance_str.str().c_str());
robot::log_info("odometry twist linear x: %f", odometry.twist.twist.linear.x);
robot::log_info("odometry twist linear y: %f", odometry.twist.twist.linear.y);
robot::log_info("odometry twist linear z: %f", odometry.twist.twist.linear.z);
robot::log_info("odometry twist angular x: %f", odometry.twist.twist.angular.x);
robot::log_info("odometry twist angular y: %f", odometry.twist.twist.angular.y);
robot::log_info("odometry twist angular z: %f", odometry.twist.twist.angular.z);
std::stringstream twist_covariance_str;
for(int i = 0; i < 36; i++) {
twist_covariance_str << odometry.twist.covariance[i] << " ";
}
robot::log_info("odometry twist covariance: %s", twist_covariance_str.str().c_str());
// robot::log_info("addOdometry: %s", odometry_name.c_str());
// robot::log_info("odometry header: %s", odometry.header.frame_id.c_str());
// robot::log_info("odometry child_frame_id: %s", odometry.child_frame_id.c_str());
// robot::log_info("odometry pose position x: %f", odometry.pose.pose.position.x);
// robot::log_info("odometry pose position y: %f", odometry.pose.pose.position.y);
// robot::log_info("odometry pose position z: %f", odometry.pose.pose.position.z);
// robot::log_info("odometry pose orientation x: %f", odometry.pose.pose.orientation.x);
// robot::log_info("odometry pose orientation y: %f", odometry.pose.pose.orientation.y);
// robot::log_info("odometry pose orientation z: %f", odometry.pose.pose.orientation.z);
// robot::log_info("odometry pose orientation w: %f", odometry.pose.pose.orientation.w);
// std::stringstream pose_covariance_str;
// for(int i = 0; i < 36; i++) {
// pose_covariance_str << odometry.pose.covariance[i] << " ";
// }
// robot::log_info("odometry pose covariance: %s", pose_covariance_str.str().c_str());
// robot::log_info("odometry twist linear x: %f", odometry.twist.twist.linear.x);
// robot::log_info("odometry twist linear y: %f", odometry.twist.twist.linear.y);
// robot::log_info("odometry twist linear z: %f", odometry.twist.twist.linear.z);
// robot::log_info("odometry twist angular x: %f", odometry.twist.twist.angular.x);
// robot::log_info("odometry twist angular y: %f", odometry.twist.twist.angular.y);
// robot::log_info("odometry twist angular z: %f", odometry.twist.twist.angular.z);
// std::stringstream twist_covariance_str;
// for(int i = 0; i < 36; i++) {
// twist_covariance_str << odometry.twist.covariance[i] << " ";
// }
// robot::log_info("odometry twist covariance: %s", twist_covariance_str.str().c_str());
// robot::log_info("--------------------------------");
odometry_ = odometry;
}