From 017ffa09c38501e24cb540d663880369c5975386 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Fri, 20 Dec 2024 20:31:13 +0100 Subject: [PATCH] Fix trajectory generation bug in docking controller (#4807) * Fix trajectory in docking controller Signed-off-by: Alberto Tudela * Ceil and remove resolution Signed-off-by: Alberto Tudela * Update nav2_docking/opennav_docking/src/controller.cpp Co-authored-by: Steve Macenski Signed-off-by: Alberto Tudela * Update nav2_docking/opennav_docking/src/controller.cpp Co-authored-by: Steve Macenski Signed-off-by: Alberto Tudela --------- Signed-off-by: Alberto Tudela Co-authored-by: Steve Macenski --- nav2_docking/opennav_docking/src/controller.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index c0e45ee4223..ec2a1de9ab9 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -144,7 +144,10 @@ bool Controller::isTrajectoryCollisionFree( } // Generate path - for (double t = 0; t < projection_time_; t += simulation_time_step_) { + double distance = std::numeric_limits::max(); + unsigned int max_iter = static_cast(ceil(projection_time_ / simulation_time_step_)); + + do{ // Apply velocities to calculate next pose next_pose.pose = control_law_->calculateNextPose( simulation_time_step_, target_pose, next_pose.pose, backward); @@ -177,7 +180,10 @@ bool Controller::isTrajectoryCollisionFree( trajectory_pub_->publish(trajectory); return false; } - } + + // Check if we reach the goal + distance = nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose); + }while(distance > 1e-2 && trajectory.poses.size() < max_iter); trajectory_pub_->publish(trajectory);