fix(obstacle_stop_planner): use utils function to caluculate nearest …

…index from ego (tier4#651)

Signed-off-by: Azumi Suzuki <azumi.suzuki@tier4.jp>

planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml

@@ -6,6 +6,7 @@
       step_length: 1.0 # step length for pointcloud search range [m]
       extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance
       expand_stop_range: 0.0 # margin of vehicle footprint [m]
+      max_yaw_deviation_deg: 90.0 # maximum ego yaw deviation from trajectory [deg] (measures against overlapping lanes)
 
     slow_down_planner:
       # slow down planner parameters

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

@@ -24,8 +24,10 @@
 #include <pcl_ros/transforms.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <signal_processing/lowpass_filter_1d.hpp>
+#include <tier4_autoware_utils/math/unit_conversion.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 #include <tier4_autoware_utils/trajectory/tmp_conversion.hpp>
+#include <tier4_autoware_utils/trajectory/trajectory.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
@@ -96,10 +98,12 @@ class ObstacleStopPlannerNode : public rclcpp::Node
 
   struct NodeParam
   {
-    bool enable_slow_down;     // set True, slow down for obstacle beside the path
-    double max_velocity;       // max velocity [m/s]
-    double lowpass_gain;       // smoothing calculated current acceleration [-]
-    double hunting_threshold;  // keep slow down or stop state if obstacle vanished [s]
+    bool enable_slow_down;         // set True, slow down for obstacle beside the path
+    double max_velocity;           // max velocity [m/s]
+    double lowpass_gain;           // smoothing calculated current acceleration [-]
+    double hunting_threshold;      // keep slow down or stop state if obstacle vanished [s]
+    double max_yaw_deviation_rad;  // maximum ego yaw deviation from trajectory [rad] (measures
+                                   // against overlapping lanes)
   };
 
   struct StopParam

planning/obstacle_stop_planner/src/node.cpp

@@ -445,6 +445,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
     p.hunting_threshold = declare_parameter("hunting_threshold", 0.5);
     p.lowpass_gain = declare_parameter("lowpass_gain", 0.9);
     lpf_acc_ = std::make_shared<LowpassFilter1d>(0.0, p.lowpass_gain);
+    const double max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg", 90.0);
+    p.max_yaw_deviation_rad = tier4_autoware_utils::deg2rad(max_yaw_deviation_deg);
   }
 
   {
@@ -1221,18 +1223,13 @@ TrajectoryPoints ObstacleStopPlannerNode::trimTrajectoryWithIndexFromSelfPose(
 {
   TrajectoryPoints output{};
 
-  double min_distance = 0.0;
   size_t min_distance_index = 0;
-  bool is_init = false;
-  for (size_t i = 0; i < input.size(); ++i) {
-    const double x = input.at(i).pose.position.x - self_pose.position.x;
-    const double y = input.at(i).pose.position.y - self_pose.position.y;
-    const double squared_distance = x * x + y * y;
-    if (!is_init || squared_distance < min_distance * min_distance) {
-      is_init = true;
-      min_distance = std::sqrt(squared_distance);
-      min_distance_index = i;
-    }
+  const auto nearest_index = tier4_autoware_utils::findNearestIndex(
+    input, self_pose, 10.0, node_param_.max_yaw_deviation_rad);
+  if (!nearest_index) {
+    min_distance_index = tier4_autoware_utils::findNearestIndex(input, self_pose.position);
+  } else {
+    min_distance_index = nearest_index.value();
   }
   for (size_t i = min_distance_index; i < input.size(); ++i) {
     output.push_back(input.at(i));