From cd52f83067f48478f729836c4b768ee2f9959ef1 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 7 Mar 2025 15:29:23 +0800 Subject: [PATCH] Use waypoint merge radius override when computing plan starts Signed-off-by: Michael X. Grey --- rmf_traffic/src/rmf_traffic/agv/Planner.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rmf_traffic/src/rmf_traffic/agv/Planner.cpp b/rmf_traffic/src/rmf_traffic/agv/Planner.cpp index 5bd91d46..fad7ca97 100644 --- a/rmf_traffic/src/rmf_traffic/agv/Planner.cpp +++ b/rmf_traffic/src/rmf_traffic/agv/Planner.cpp @@ -1169,8 +1169,10 @@ std::vector compute_plan_starts( continue; const Eigen::Vector2d wp_location = wp.get_location(); + const auto merge_radius = + wp.merge_radius().value_or(max_merge_waypoint_distance); - if ( (p_location - wp_location).norm() < max_merge_waypoint_distance) + if ( (p_location - wp_location).norm() < merge_radius) { return {Plan::Start(start_time, wp.index(), start_yaw)}; }