fix(behavior_velocity_planner): fix before/after line of virtual_traf…

…fic_light (autowarefoundation#228) * fix(behavior_velocity_planner): fix before/after line of virtual_traffic_light Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * Rename params to dead_line_marging Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
badai-nguyen · Jan 6, 2022 · 26989d8 · 26989d8
1 parent c7c2f28
commit 26989d8
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 10 deletions.
diff --git a/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml b/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml
@@ -2,3 +2,6 @@
   ros__parameters:
     virtual_traffic_light:
       max_delay_sec: 3.0
+      near_line_distance: 1.0
+      dead_line_margin: 1.0
+      check_timeout_after_stop_line: true
diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp
@@ -66,6 +66,9 @@ class VirtualTrafficLightModule : public SceneModuleInterface
   struct PlannerParam
   {
     double max_delay_sec;
+    double near_line_distance;
+    double dead_line_margin;
+    bool check_timeout_after_stop_line;
   };
 
 public:

diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp
@@ -68,6 +68,10 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node
   {
     auto & p = planner_param_;
     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.check_timeout_after_stop_line =
+      node.declare_parameter(ns + ".check_timeout_after_stop_line", true);
   }
 }
 

diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp
@@ -399,9 +399,9 @@ bool VirtualTrafficLightModule::modifyPathVelocity(
     return true;
   }
 
-  // Stop at stop_line if state is timeout before stop_line
-  if (isBeforeStopLine()) {
-    if (!isStateTimeout(*virtual_traffic_light_state)) {
+  // Stop at stop_line if state is timeout
+  if (isBeforeStopLine() || planner_param_.check_timeout_after_stop_line) {
+    if (isStateTimeout(*virtual_traffic_light_state)) {
       RCLCPP_DEBUG(logger_, "state is timeout");
       insertStopVelocityAtStopLine(path, stop_reason);
     }
@@ -472,7 +472,7 @@ bool VirtualTrafficLightModule::isBeforeStopLine()
   const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength(
     module_data_.path.points, module_data_.head_pose.position, collision->point);
 
-  return signed_arc_length > 0;
+  return signed_arc_length > -planner_param_.dead_line_margin;
 }
 
 bool VirtualTrafficLightModule::isAfterAnyEndLine()
@@ -493,8 +493,7 @@ bool VirtualTrafficLightModule::isAfterAnyEndLine()
   const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength(
     module_data_.path.points, module_data_.head_pose.position, collision->point);
 
-  constexpr double max_excess_distance = 3.0;
-  return signed_arc_length < -max_excess_distance;
+  return signed_arc_length < -planner_param_.dead_line_margin;
 }
 
 bool VirtualTrafficLightModule::isNearAnyEndLine()
@@ -508,8 +507,7 @@ bool VirtualTrafficLightModule::isNearAnyEndLine()
   const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength(
     module_data_.path.points, module_data_.head_pose.position, collision->point);
 
-  constexpr double near_distance = 1.0;
-  return std::abs(signed_arc_length) < near_distance;
+  return std::abs(signed_arc_length) < planner_param_.near_line_distance;
 }
 
 boost::optional<tier4_v2x_msgs::msg::VirtualTrafficLightState>
@@ -535,10 +533,10 @@ bool VirtualTrafficLightModule::isStateTimeout(
   const auto delay = (clock_->now() - rclcpp::Time(state.stamp)).seconds();
   if (delay > planner_param_.max_delay_sec) {
     RCLCPP_DEBUG(logger_, "delay=%f, max_delay=%f", delay, planner_param_.max_delay_sec);
-    return false;
+    return true;
   }
 
-  return true;
+  return false;
 }
 
 bool VirtualTrafficLightModule::hasRightOfWay(