Fix trajectory generation bug in docking controller (#4807)

* Fix trajectory in docking controller Signed-off-by: Alberto Tudela <[email protected]> * Ceil and remove resolution Signed-off-by: Alberto Tudela <[email protected]> * Update nav2_docking/opennav_docking/src/controller.cpp Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Update nav2_docking/opennav_docking/src/controller.cpp Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> --------- Signed-off-by: Alberto Tudela <[email protected]> Co-authored-by: Steve Macenski <[email protected]>
ros-navigation · Dec 20, 2024 · 017ffa0 · 017ffa0
1 parent b1d9347
commit 017ffa0
Showing 1 changed file with 8 additions and 2 deletions.
diff --git 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<double>::max();
+  unsigned int max_iter = static_cast<unsigned int>(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);