add new param, move the params' name space

Signed-off-by: Yuki Takagi <[email protected]>
tier4 · Oct 17, 2023 · c6c31ed · c6c31ed
1 parent 04141c4
commit c6c31ed
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 14 deletions.
diff --git a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml
@@ -6,3 +6,10 @@
       yellow_lamp_period: 2.75
       enable_pass_judge: true
       enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
+
+      v2i:
+        enable_conjecture: true
+        last_time_allowed_to_pass: 2.0 # # relative time against at the time of turn to red
+        time_duration_to_conjecture: 15.0 # against at the time of turn to red
+        min_assumed_velocity: 1.0 # [m/s] minimum velocity used to calculate the enter and exit times
+
diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp
@@ -41,12 +41,14 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node)
   planner_param_.enable_pass_judge = getOrDeclareParameter<bool>(node, ns + ".enable_pass_judge");
   planner_param_.yellow_lamp_period =
     getOrDeclareParameter<double>(node, ns + ".yellow_lamp_period");
-  planner_param_.enable_conjecture_by_v2i =
-    getOrDeclareParameter<bool>(node, ns + ".enable_conjecture_by_v2i");
-  planner_param_.time_duration_to_conjecture_by_v2i =
-    getOrDeclareParameter<double>(node, ns + ".time_duration_to_conjecture_by_v2i");
-  planner_param_.last_time_allowed_to_pass_by_v2i =
-    getOrDeclareParameter<double>(node, ns + ".last_time_allowed_to_pass_by_v2i");
+  planner_param_.v2i_enable_conjecture =
+    getOrDeclareParameter<bool>(node, ns + ".v2i.enable_conjecture");
+  planner_param_.v2i_time_duration_to_conjecture =
+    getOrDeclareParameter<double>(node, ns + ".v2i.time_duration_to_conjecture");
+  planner_param_.v2i_last_time_allowed_to_pass =
+    getOrDeclareParameter<double>(node, ns + ".v2i.last_time_allowed_to_pass");
+  planner_param_.v2i_min_assumed_velocity =
+    getOrDeclareParameter<double>(node, ns + ".v2i.min_assumed_velocity");
 
   pub_tl_state_ = node.create_publisher<autoware_perception_msgs::msg::TrafficSignal>(
     "~/output/traffic_signal", 1);

diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
@@ -261,20 +261,21 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     const double rest_time_to_red_signal =
       planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id());
     const double rest_time_to_go_ahead_allowed =
-      rest_time_to_red_signal - planner_param_.last_time_allowed_to_pass_by_v2i;
+      rest_time_to_red_signal - planner_param_.v2i_last_time_allowed_to_pass;
     debug(rest_time_to_red_signal);
 
     if (isActivated()) {
       const bool do_conjecture_by_v2i =
-        planner_param_.enable_conjecture_by_v2i &&
-        rest_time_to_go_ahead_allowed <= planner_param_.time_duration_to_conjecture_by_v2i &&
+        planner_param_.v2i_enable_conjecture &&
+        rest_time_to_go_ahead_allowed <= planner_param_.v2i_time_duration_to_conjecture &&
         rest_time_to_go_ahead_allowed > 1e-6;
 
       RCLCPP_INFO(logger_, "\ndo_conjecture_by_v2i: %s, ", do_conjecture_by_v2i ? "true" : "false");
 
       if (do_conjecture_by_v2i) {
-        const double reachable_distance =
-          planner_data_->current_velocity->twist.linear.x * rest_time_to_go_ahead_allowed;
+        const double assumed_velocity = std::max(
+          planner_data_->current_velocity->twist.linear.x, planner_param_.v2i_min_assumed_velocity);
+        const double reachable_distance = assumed_velocity * rest_time_to_go_ahead_allowed;
 
         // debug(signed_arc_length_to_stop_point);
         // debug(reachable_distance);

diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp
@@ -61,9 +61,10 @@ class TrafficLightModule : public SceneModuleInterface
     double tl_state_timeout;
     double yellow_lamp_period;
     bool enable_pass_judge;
-    bool enable_conjecture_by_v2i;
-    double time_duration_to_conjecture_by_v2i;
-    double last_time_allowed_to_pass_by_v2i;
+    bool v2i_enable_conjecture;
+    double v2i_last_time_allowed_to_pass;
+    double v2i_time_duration_to_conjecture;
+    double v2i_min_assumed_velocity;
   };
 
 public: