From a219a7e6f5f77e8f9b32744b71921ff7cdd418c0 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Wed, 17 Jan 2024 16:12:32 +0900 Subject: [PATCH] chore(mission_planner): add logs for reroute safety check (#6096) * chore(mission_planner): add logs for reroute safety check Signed-off-by: Makoto Kurihara * style(pre-commit): autofix --------- Signed-off-by: Makoto Kurihara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../mission_planner/src/mission_planner/mission_planner.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 9ee874928d7d1..e0181065150fd 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -731,6 +731,7 @@ bool MissionPlanner::check_reroute_safety( const LaneletRoute & original_route, const LaneletRoute & target_route) { if (original_route.segments.empty() || target_route.segments.empty() || !map_ptr_ || !odometry_) { + RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Route, map or odometry is not set."); return false; } @@ -803,6 +804,8 @@ bool MissionPlanner::check_reroute_safety( return std::nullopt; }); if (!start_idx_opt.has_value()) { + RCLCPP_ERROR( + get_logger(), "Check reroute safety failed. Cannot find the start index of the route."); return false; } const size_t start_idx = start_idx_opt.value(); @@ -838,6 +841,7 @@ bool MissionPlanner::check_reroute_safety( // get closest lanelet in start lanelets lanelet::ConstLanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet(start_lanelets, current_pose, &closest_lanelet)) { + RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Cannot find the closest lanelet."); return false; }