diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml
index c422931..9c205af 100644
--- a/config/pnkx_local_planner_params.yaml
+++ b/config/pnkx_local_planner_params.yaml
@@ -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
diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs
index 768c885..520c281 100644
--- a/examples/NavigationExample/Program.cs
+++ b/examples/NavigationExample/Program.cs
@@ -241,6 +241,7 @@ namespace NavigationExample
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback);
+
/// Helper: copy unmanaged char* to managed string; does not free the pointer.
public static string MarshalString(IntPtr p)
{
@@ -572,8 +573,14 @@ namespace NavigationExample
Marshal.OffsetOf("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);
}
diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so
index 3ba8536..a758b26 100755
Binary files a/examples/NavigationExample/libnav_c_api.so and b/examples/NavigationExample/libnav_c_api.so differ
diff --git a/src/APIs/c_api/src/convertor.cpp b/src/APIs/c_api/src/convertor.cpp
index 1960f3a..216f950 100644
--- a/src/APIs/c_api/src/convertor.cpp
+++ b/src/APIs/c_api/src/convertor.cpp
@@ -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;
}
diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp
index 1b1967c..e7b2fd4 100644
--- a/src/APIs/c_api/src/nav_c_api.cpp
+++ b/src/APIs/c_api/src/nav_c_api.cpp
@@ -549,8 +549,10 @@ extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped &ou
try
{
- robot::move_base_core::BaseNavigation *nav =
- static_cast(handle.ptr);
+ auto *nav_ptr = static_cast *>(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;
diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
index 3232788..b7951bc 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
+++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
@@ -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_;
diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
index 00bf276..70b0ed5 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
+++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
@@ -117,6 +117,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
nh_priv_.param("final_heading_angle_tolerance", final_heading_angle_tolerance_, 0.05);
nh_priv_.param("final_heading_min_velocity", final_heading_min_velocity_, 0.05);
nh_priv_.param("final_heading_kp_angular", final_heading_kp_angular_, 1.5);
+ nh_priv_.param("final_heading_ki_angular", final_heading_ki_angular_, 0.0);
+ nh_priv_.param("final_heading_kd_angular", final_heading_kd_angular_, 0.0);
nh_priv_.param("rot_stopped_velocity", rot_stopped_velocity_, 0.01);
nh_priv_.param("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::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::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;
diff --git a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp
index 09fe4c8..6fa90d2 100644
--- a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp
+++ b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp
@@ -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;
diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp
index 7e520e5..7ce4f58 100644
--- a/src/Navigations/Packages/move_base/src/move_base.cpp
+++ b/src/Navigations/Packages/move_base/src/move_base.cpp
@@ -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(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name);
updateLocalCostmap(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(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
updateGlobalCostmap(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;
}