From 712c11c5cb70d0a8b7e298e4777c4d5e3fb2bbea Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Thu, 25 May 2023 20:14:43 +0900 Subject: [PATCH] fix(mission_planner): subtract goal distance from the accumulated length (#3818) * refactor(mission_planner): clean reroute code Signed-off-by: yutaka * feat(mission_planner): add minimum reroute length Signed-off-by: yutaka * fix(mission_planner): subtract goal distance from safety distance Signed-off-by: yutaka --------- Signed-off-by: yutaka --- .../src/mission_planner/mission_planner.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index fd1dabbc58dc1..b3df2d0a0010a 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -491,6 +491,25 @@ bool MissionPlanner::checkRerouteSafety( accumulated_length += *std::min_element(lanelets_length.begin(), lanelets_length.end()); } + // check if the goal is inside of the target terminal lanelet + const auto & target_end_primitives = target_route.segments.at(end_idx - start_idx).primitives; + const auto & target_goal = target_route.goal_pose; + for (const auto & target_end_primitive : target_end_primitives) { + const auto lanelet = lanelet_map_ptr_->laneletLayer.get(target_end_primitive.id); + if (lanelet::utils::isInLanelet(target_goal, lanelet)) { + const auto target_goal_position = + lanelet::utils::conversion::toLaneletPoint(target_goal.position); + const double dist_to_goal = lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(lanelet.centerline()), + lanelet::utils::to2D(target_goal_position).basicPoint()) + .length; + const double target_lanelet_length = lanelet::utils::getLaneletLength2d(lanelet); + const double dist = target_lanelet_length - dist_to_goal; + accumulated_length = std::max(accumulated_length - dist, 0.0); + break; + } + } + // check safety const auto current_velocity = odometry_->twist.twist.linear.x; const double safety_length =