fix(behavior_velocity_planner): fix virtual_traffic_light singed_arc_length and state (autowarefoundation#306)

* fix(behavior_velocity): fix virtual_traffic_light singed_arc_length and state

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Use <> to include from another packages

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Author: kosuke55

common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp

@@ -166,6 +166,44 @@ size_t findNearestSegmentIndex(const T & points, const geometry_msgs::msg::Point
   return nearest_idx;
 }
 
+/**
+ * @brief find nearest segment index to pose
+ *        segment is straight path between two continuous points of trajectory
+ *        When pose is on a trajectory point whose index is nearest_idx, return nearest_idx - 1
+ * @param points points of trajectory
+ * @param pose pose to which to find nearest segment index
+ * @param max_dist max distance to search
+ * @param max_yaw max yaw to search
+ * @return nearest index
+ */
+template <class T>
+boost::optional<size_t> findNearestSegmentIndex(
+  const T & points, const geometry_msgs::msg::Pose & pose,
+  const double max_dist = std::numeric_limits<double>::max(),
+  const double max_yaw = std::numeric_limits<double>::max())
+{
+  const auto nearest_idx = findNearestIndex(points, pose, max_dist, max_yaw);
+
+  if (!nearest_idx) {
+    return boost::none;
+  }
+
+  if (*nearest_idx == 0) {
+    return 0;
+  }
+  if (*nearest_idx == points.size() - 1) {
+    return points.size() - 2;
+  }
+
+  const double signed_length = calcLongitudinalOffsetToSegment(points, *nearest_idx, pose.position);
+
+  if (signed_length <= 0) {
+    return *nearest_idx - 1;
+  }
+
+  return *nearest_idx;
+}
+
 /**
  * @brief calculate lateral offset from p_target (length from p_target to trajectory)
  *        If seg_idx point is after that nearest point, length is negative
@@ -265,6 +303,34 @@ double calcSignedArcLength(
   return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset;
 }
 
+/**
+ * @brief calcSignedArcLength from pose to point
+ */
+template <class T>
+boost::optional<double> calcSignedArcLength(
+  const T & points, const geometry_msgs::msg::Pose & src_pose,
+  const geometry_msgs::msg::Point & dst_point,
+  const double max_dist = std::numeric_limits<double>::max(),
+  const double max_yaw = std::numeric_limits<double>::max())
+{
+  validateNonEmpty(points);
+
+  const auto src_seg_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw);
+  if (!src_seg_idx) {
+    return boost::none;
+  }
+
+  const size_t dst_seg_idx = findNearestSegmentIndex(points, dst_point);
+
+  const double signed_length_on_traj = calcSignedArcLength(points, *src_seg_idx, dst_seg_idx);
+  const double signed_length_src_offset =
+    calcLongitudinalOffsetToSegment(points, *src_seg_idx, src_pose.position);
+  const double signed_length_dst_offset =
+    calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point);
+
+  return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset;
+}
+
 /**
  * @brief calcArcLength for the whole length
  */

planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml

@@ -4,4 +4,5 @@
       max_delay_sec: 3.0
       near_line_distance: 1.0
       dead_line_margin: 1.0
+      max_yaw_deviation_deg: 90.0
       check_timeout_after_stop_line: true

planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp

@@ -68,6 +68,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface
     double max_delay_sec;
     double near_line_distance;
     double dead_line_margin;
+    double max_yaw_deviation_rad;
     bool check_timeout_after_stop_line;
   };
 

planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <scene_module/virtual_traffic_light/manager.hpp>
+#include <tier4_autoware_utils/math/unit_conversion.hpp>
 
 #include <memory>
 #include <set>
@@ -70,6 +71,8 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node
     p.max_delay_sec = node.declare_parameter(ns + ".max_delay_sec", 3.0);
     p.near_line_distance = node.declare_parameter(ns + ".near_line_distance", 1.0);
     p.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin", 1.0);
+    p.max_yaw_deviation_rad =
+      tier4_autoware_utils::deg2rad(node.declare_parameter(ns + ".max_yaw_deviation_deg", 90.0));
     p.check_timeout_after_stop_line =
       node.declare_parameter(ns + ".check_timeout_after_stop_line", true);
   }

planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp

@@ -19,9 +19,20 @@
 #include <tier4_v2x_msgs/msg/key_value.hpp>
 
 #include <algorithm>
+#include <limits>
 #include <string>
 #include <vector>
 
+namespace tier4_autoware_utils
+{
+template <>
+inline geometry_msgs::msg::Pose getPose(
+  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
+{
+  return p.point.pose;
+}
+}  // namespace tier4_autoware_utils
+
 namespace behavior_velocity_planner
 {
 namespace
@@ -182,7 +193,7 @@ SegmentIndexWithOffset findBackwardOffsetSegment(
   const double offset_length)
 {
   double sum_length = 0.0;
-  for (size_t i = base_idx - 1; i > 0; --i) {
+  for (size_t i = base_idx; i > 0; --i) {
     const auto p_front = path.points.at(i - 1);
     const auto p_back = path.points.at(i);
 
@@ -399,10 +410,10 @@ bool VirtualTrafficLightModule::modifyPathVelocity(
     return true;
   }
 
-  // Stop at stop_line if state is timeout
-  if (isBeforeStopLine() || planner_param_.check_timeout_after_stop_line) {
+  // Stop at stop_line if state is timeout before stop_line
+  if (isBeforeStopLine()) {
     if (isStateTimeout(*virtual_traffic_light_state)) {
-      RCLCPP_DEBUG(logger_, "state is timeout");
+      RCLCPP_DEBUG(logger_, "state is timeout before stop line");
       insertStopVelocityAtStopLine(path, stop_reason);
     }
 
@@ -413,6 +424,15 @@ bool VirtualTrafficLightModule::modifyPathVelocity(
   // After stop_line
   state_ = State::PASSING;
 
+  // Check timeout after stop_line if the option is on
+  if (
+    planner_param_.check_timeout_after_stop_line && isStateTimeout(*virtual_traffic_light_state)) {
+    RCLCPP_DEBUG(logger_, "state is timeout after stop line");
+    insertStopVelocityAtStopLine(path, stop_reason);
+    updateInfrastructureCommand();
+    return true;
+  }
+
   // Stop at stop_line if finalization isn't completed
   if (!virtual_traffic_light_state->is_finalized) {
     RCLCPP_DEBUG(logger_, "finalization isn't completed");
@@ -453,10 +473,12 @@ bool VirtualTrafficLightModule::isBeforeStartLine()
     return false;
   }
 
+  const double max_dist = std::numeric_limits<double>::max();
   const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength(
-    module_data_.path.points, module_data_.head_pose.position, collision->point);
+    module_data_.path.points, module_data_.head_pose, collision->point, max_dist,
+    planner_param_.max_yaw_deviation_rad);
 
-  return signed_arc_length > 0;
+  return *signed_arc_length > 0;
 }
 
 bool VirtualTrafficLightModule::isBeforeStopLine()
@@ -469,10 +491,12 @@ bool VirtualTrafficLightModule::isBeforeStopLine()
     return false;
   }
 
+  const double max_dist = std::numeric_limits<double>::max();
   const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength(
-    module_data_.path.points, module_data_.head_pose.position, collision->point);
+    module_data_.path.points, module_data_.head_pose, collision->point, max_dist,
+    planner_param_.max_yaw_deviation_rad);
 
-  return signed_arc_length > -planner_param_.dead_line_margin;
+  return *signed_arc_length > -planner_param_.dead_line_margin;
 }
 
 bool VirtualTrafficLightModule::isAfterAnyEndLine()
@@ -484,16 +508,18 @@ bool VirtualTrafficLightModule::isAfterAnyEndLine()
 
   const auto collision = findCollision(module_data_.path.points, map_data_.end_lines);
 
-  // Since the module is registered, a collision should be detected usually.
-  // Therefore if no collision found, vehicle's path is fully after the line.
+  // If the goal is set before the end line, collision will not be detected.
+  // Therefore if there is no collision, the ego vehicle is assumed to be before the end line.
   if (!collision) {
     return false;
   }
 
+  const double max_dist = std::numeric_limits<double>::max();
   const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength(
-    module_data_.path.points, module_data_.head_pose.position, collision->point);
+    module_data_.path.points, module_data_.head_pose, collision->point, max_dist,
+    planner_param_.max_yaw_deviation_rad);
 
-  return signed_arc_length < -planner_param_.dead_line_margin;
+  return *signed_arc_length < -planner_param_.dead_line_margin;
 }
 
 bool VirtualTrafficLightModule::isNearAnyEndLine()
@@ -504,10 +530,12 @@ bool VirtualTrafficLightModule::isNearAnyEndLine()
     return false;
   }
 
+  const double max_dist = std::numeric_limits<double>::max();
   const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength(
-    module_data_.path.points, module_data_.head_pose.position, collision->point);
+    module_data_.path.points, module_data_.head_pose, collision->point, max_dist,
+    planner_param_.max_yaw_deviation_rad);
 
-  return std::abs(signed_arc_length) < planner_param_.near_line_distance;
+  return std::abs(*signed_arc_length) < planner_param_.near_line_distance;
 }
 
 boost::optional<tier4_v2x_msgs::msg::VirtualTrafficLightState>