From 16a3fd16a07345e8a07d7f7ef45385b7388dbba7 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 17 Mar 2022 21:25:59 +0900 Subject: [PATCH] fix(planning_errror_monitor): ignore invalid calculation in curvature validation Signed-off-by: Takamasa Horibe --- .../src/planning_error_monitor_node.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/planning/planning_error_monitor/src/planning_error_monitor_node.cpp b/planning/planning_error_monitor/src/planning_error_monitor_node.cpp index dd43bacfca574..4a8b7bbda8a80 100644 --- a/planning/planning_error_monitor/src/planning_error_monitor_node.cpp +++ b/planning/planning_error_monitor/src/planning_error_monitor_node.cpp @@ -251,6 +251,9 @@ bool PlanningErrorMonitorNode::checkTrajectoryCurvature( } constexpr double points_distance = 1.0; + const auto isValidDistance = [points_distance](const auto & p1, const auto & p2) { + return calcDistance2d(p1, p2) >= points_distance; + }; for (size_t p1_id = 0; p1_id < traj.points.size() - 2; ++p1_id) { // Get Point1 @@ -268,6 +271,9 @@ bool PlanningErrorMonitorNode::checkTrajectoryCurvature( if (p1_id == p2_id || p1_id == p3_id || p2_id == p3_id) { break; } + if (!isValidDistance(p1, p2) || !isValidDistance(p1, p3) || !isValidDistance(p2, p3)) { + break; + } const double curvature = calcCurvature(p1, p2, p3);