From 90f9a84106a05725093f81eaaee658009f74b9d8 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Fri, 13 Oct 2023 13:31:26 +0900 Subject: [PATCH 01/24] first implemantation Signed-off-by: Yuki Takagi --- .../behavior_velocity_planner.launch.xml | 1 + .../behavior_velocity_planner/package.xml | 2 + .../behavior_velocity_planner/src/node.cpp | 56 ++++++++++++++ .../behavior_velocity_planner/src/node.hpp | 7 ++ .../planner_data.hpp | 8 ++ .../package.xml | 1 + .../src/manager.cpp | 5 ++ .../src/scene.cpp | 73 +++++++++++++++++-- .../src/scene.hpp | 2 + 9 files changed, 150 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index b517b77b328c7..379b60cff7f94 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -52,6 +52,7 @@ + diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 5a307398e9afd..acfba2ebc7d8b 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -59,6 +59,8 @@ tier4_planning_msgs tier4_v2x_msgs visualization_msgs + jpn_signal_v2i_msgs + ament_cmake_ros ament_lint_auto diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 438fdd7f37e3a..8d923eee8e884 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -35,6 +35,36 @@ #include #include +#define debug(var) \ + do { \ + std::cerr << __func__ << ": " << __LINE__ << ", " << #var << " : "; \ + view(var); \ + } while (0) +template +void view(T e) +{ + std::cerr << e << std::endl; +} +template +void view(const std::vector & v) +{ + for (const auto & e : v) { + std::cerr << e << " "; + } + std::cerr << std::endl; +} +template +void view(const std::vector> & vv) +{ + for (const auto & v : vv) { + view(v); + } +} +#define line() \ + { \ + std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; \ + } + namespace { rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) @@ -103,6 +133,11 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio "~/input/traffic_signals", 1, std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1), createSubscriptionOptions(this)); + sub_traffic_signals_raw_v2i_ = + this->create_subscription( + "~/input/traffic_signals_raw_v2i", 1, + std::bind(&BehaviorVelocityPlannerNode::onTrafficSignalsRawV2I, this, _1), + createSubscriptionOptions(this)); sub_external_velocity_limit_ = this->create_subscription( "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local(), std::bind(&BehaviorVelocityPlannerNode::onExternalVelocityLimit, this, _1)); @@ -301,6 +336,27 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( } } +void BehaviorVelocityPlannerNode::onTrafficSignalsRawV2I( + const jpn_signal_v2i_msgs::msg::TrafficLightInfo::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + line(); + + for (const auto & car_light : msg->car_lights) { + for (const auto & state : car_light.states) { + planner_data_.traffic_light_time_to_red_id_map[state.traffic_signal_id] = + car_light.min_rest_time; // msg->min_rest_time_to_red; + + size_t set_id = state.traffic_signal_id; + size_t set_value = car_light.min_rest_time; + + debug(set_id); + debug(set_value); + } + } +} + void BehaviorVelocityPlannerNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) { std::lock_guard lock(mutex_); diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index b3084db19d1e8..91480bfca53b8 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -28,6 +28,10 @@ #include #include #include +#include +#include + + #include #include #include @@ -64,6 +68,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_lanelet_map_; rclcpp::Subscription::SharedPtr sub_traffic_signals_; + rclcpp::Subscription::SharedPtr + sub_traffic_signals_raw_v2i_; rclcpp::Subscription::SharedPtr sub_virtual_traffic_light_states_; rclcpp::Subscription::SharedPtr sub_occupancy_grid_; @@ -79,6 +85,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); void onTrafficSignals( const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + void onTrafficSignalsRawV2I(const jpn_signal_v2i_msgs::msg::TrafficLightInfo::ConstSharedPtr msg); void onVirtualTrafficLightStates( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 2668d83b0f510..2013078e97d34 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -84,6 +84,7 @@ struct PlannerData // other internal data std::map traffic_light_id_map; + std::map traffic_light_time_to_red_id_map; boost::optional external_velocity_limit; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; @@ -139,6 +140,13 @@ struct PlannerData } return std::make_shared(traffic_light_id_map.at(id)); } + double getRestTimeToRedSignal(const int id) const + { + if (traffic_light_time_to_red_id_map.count(id) == 0) { + return std::numeric_limits::max(); + } + return traffic_light_time_to_red_id_map.at(id); + } }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index aedbab65068fb..cc13b64dd8db1 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -47,6 +47,7 @@ tier4_v2x_msgs vehicle_info_util visualization_msgs + jpn_signal_v2i_msgs ament_cmake_ros ament_lint_auto diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index f035544b81592..0a95b8f76066d 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -41,6 +41,11 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); + planner_param_.enable_conjecture_by_v2i = + getOrDeclareParameter(node, ns + ".enable_conjecture_by_v2i"); + planner_param_.time_duration_to_conject_by_v2i = + getOrDeclareParameter(node, ns + ".time_duration_to_conject_by_v2i"); + pub_tl_state_ = node.create_publisher( "~/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 index e0633926e35df..f87cfd71a7589 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -34,6 +34,36 @@ #include #include +#define debug(var) \ + do { \ + std::cerr << __func__ << ": " << __LINE__ << ", " << #var << " : "; \ + view(var); \ + } while (0) +template +void view(T e) +{ + std::cerr << e << std::endl; +} +template +void view(const std::vector & v) +{ + for (const auto & e : v) { + std::cerr << e << " "; + } + std::cerr << std::endl; +} +template +void view(const std::vector > & vv) +{ + for (const auto & v : vv) { + view(v); + } +} +#define line() \ + { \ + std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; \ + } + namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -213,8 +243,14 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * input_path.points, self_pose->pose.position, stop_line_point_msg); setDistance(signed_arc_length_to_stop_point); + // line(); + // RCLCPP_INFO(logger_, "\nstate_: State::%s", (state_ == State::APPROACH) ? "APPROACH" : + // "GO_OUT"); RCLCPP_INFO(logger_, "\nis_prev_state_stop_ is: %s", is_prev_state_stop_ ? "true" : + // "false"); RCLCPP_INFO(logger_, "\nis_activated: %s", isActivated() ? "true" : "false"); + // Check state if (state_ == State::APPROACH) { + // line(); // Move to go out state if ego vehicle over deadline. constexpr double signed_deadline_length = -2.0; if (signed_arc_length_to_stop_point < signed_deadline_length) { @@ -223,15 +259,45 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } + // line(); + // RCLCPP_INFO( + // logger_, "\nstate_: State::%s", (state_ == State::APPROACH) ? "APPROACH" : "GO_OUT"); + // RCLCPP_INFO(logger_, "\nis_prev_state_stop_ is: %s", is_prev_state_stop_ ? "true" : "false"); + // RCLCPP_INFO(logger_, "\nis_activated: %s", isActivated() ? "true" : "false"); + first_ref_stop_path_point_index_ = stop_line_point_idx; + double time_remained_allowed_to_go_ahead = 4.0; + // Check if stop is coming. setSafe(!isStopSignal()); + if (isActivated()) { - is_prev_state_stop_ = false; - return true; + const bool do_conject_by_v2i = + planner_param_.enable_conjecture_by_v2i && + time_remained_allowed_to_go_ahead <= planner_param_.time_duration_to_conject_by_v2i; + if (do_conject_by_v2i) { + const double reachable_distance = + planner_data_->current_velocity->twist.linear.x * time_remained_allowed_to_go_ahead; + if (reachable_distance < signed_arc_length_to_stop_point) { + *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + is_prev_state_stop_ = true; + } + } else { + is_prev_state_stop_ = false; + return true; + } } + // double time_to_red = planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id()); + // debug(time_to_red); + + // line(); + // RCLCPP_INFO( + // logger_, "\nstate_: State::%s", (state_ == State::APPROACH) ? "APPROACH" : "GO_OUT"); + // RCLCPP_INFO(logger_, "\nis_prev_state_stop_ is: %s", is_prev_state_stop_ ? "true" : "false"); + // RCLCPP_INFO(logger_, "\nis_activated: %s", isActivated() ? "true" : "false"); + // Decide whether to stop or pass even if a stop signal is received. if (!isPassthrough(signed_arc_length_to_stop_point)) { *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); @@ -250,7 +316,6 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } return true; } - return false; } @@ -284,7 +349,6 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const const double max_acc = planner_data_->max_stop_acceleration_threshold; const double max_jerk = planner_data_->max_stop_jerk_threshold; const double delay_response_time = planner_data_->delay_response_time; - const double reachable_distance = planner_data_->current_velocity->twist.linear.x * planner_param_.yellow_lamp_period; @@ -375,7 +439,6 @@ bool TrafficLightModule::findValidTrafficSignal(TrafficSignal & valid_traffic_si "time diff: " << (clock_->now() - traffic_signal_stamped->stamp).seconds()); return false; } - valid_traffic_signal = traffic_signal_stamped->signal; return true; } diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index a24db2c440e91..663581177857e 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -61,6 +61,8 @@ 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_conject_by_v2i; }; public: From 4c3bb922375be4ab25a085bb89a0b50a8dedd7e0 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Fri, 13 Oct 2023 18:00:59 +0900 Subject: [PATCH 02/24] complete the impementation and seems to works well Signed-off-by: Yuki Takagi --- .../behavior_planning.launch.py | 4 +++ .../behavior_velocity_planner/src/node.cpp | 15 +++++----- .../src/manager.cpp | 2 ++ .../src/scene.cpp | 28 ++++++++++++++----- .../src/scene.hpp | 1 + 5 files changed, 36 insertions(+), 14 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 62d4c5b7188ee..5dcaa586506e5 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -177,6 +177,10 @@ def launch_setup(context, *args, **kwargs): "~/input/traffic_signals", "/perception/traffic_light_recognition/traffic_signals", ), + ( + "~/input/traffic_signals_raw_v2i", + "/v2i/external/v2i_traffic_light_info", + ), ( "~/input/external_velocity_limit_mps", "/planning/scenario_planning/max_velocity_default", diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 8d923eee8e884..05e08c5352003 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -328,6 +328,8 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( { std::lock_guard lock(mutex_); + // RCLCPP_WARN(get_logger(), "329"); + for (const auto & signal : msg->signals) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; @@ -341,18 +343,17 @@ void BehaviorVelocityPlannerNode::onTrafficSignalsRawV2I( { std::lock_guard lock(mutex_); - line(); + // RCLCPP_WARN(get_logger(), "344"); for (const auto & car_light : msg->car_lights) { for (const auto & state : car_light.states) { planner_data_.traffic_light_time_to_red_id_map[state.traffic_signal_id] = - car_light.min_rest_time; // msg->min_rest_time_to_red; - - size_t set_id = state.traffic_signal_id; - size_t set_value = car_light.min_rest_time; + car_light.min_rest_time_to_red; + // size_t set_id = state.traffic_signal_id; + // double set_value = car_light.min_rest_time; - debug(set_id); - debug(set_value); + // debug(set_id); + // debug(set_value); } } } diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 0a95b8f76066d..a3584493b757c 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -45,6 +45,8 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".enable_conjecture_by_v2i"); planner_param_.time_duration_to_conject_by_v2i = getOrDeclareParameter(node, ns + ".time_duration_to_conject_by_v2i"); + planner_param_.last_time_allowed_to_pass_by_v2i = + getOrDeclareParameter(node, ns + ".last_time_allowed_to_pass_by_v2i"); pub_tl_state_ = node.create_publisher( "~/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 index f87cfd71a7589..1ecbbe958050d 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -267,21 +267,38 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * first_ref_stop_path_point_index_ = stop_line_point_idx; - double time_remained_allowed_to_go_ahead = 4.0; - // Check if stop is coming. setSafe(!isStopSignal()); + 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; + debug(rest_time_to_red_signal); + if (isActivated()) { const bool do_conject_by_v2i = planner_param_.enable_conjecture_by_v2i && - time_remained_allowed_to_go_ahead <= planner_param_.time_duration_to_conject_by_v2i; + rest_time_to_go_ahead_allowed <= planner_param_.time_duration_to_conject_by_v2i && + rest_time_to_go_ahead_allowed > 1e-6; + + RCLCPP_INFO(logger_, "\ndo_conject_by_v2i: %s, ", do_conject_by_v2i ? "true" : "false"); + // RCLCPP_INFO(logger_, "rest_time_allowed_to_go_ahead: %5f", rest_time_to_go_ahead_allowed); + if (do_conject_by_v2i) { const double reachable_distance = - planner_data_->current_velocity->twist.linear.x * time_remained_allowed_to_go_ahead; + planner_data_->current_velocity->twist.linear.x * rest_time_to_go_ahead_allowed; + + debug(signed_arc_length_t2.0o_stop_point); + debug(reachable_distance); + if (reachable_distance < signed_arc_length_to_stop_point) { + line(); *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); is_prev_state_stop_ = true; + } else { + is_prev_state_stop_ = false; + return true; } } else { is_prev_state_stop_ = false; @@ -289,9 +306,6 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } } - // double time_to_red = planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id()); - // debug(time_to_red); - // line(); // RCLCPP_INFO( // logger_, "\nstate_: State::%s", (state_ == State::APPROACH) ? "APPROACH" : "GO_OUT"); diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 663581177857e..caf51afcb9218 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -63,6 +63,7 @@ class TrafficLightModule : public SceneModuleInterface bool enable_pass_judge; bool enable_conjecture_by_v2i; double time_duration_to_conject_by_v2i; + double last_time_allowed_to_pass_by_v2i; }; public: From 1ab43df4d0c366d7c877760e12bcd2a85123737b Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 13 Oct 2023 10:32:05 +0000 Subject: [PATCH 03/24] style(pre-commit): autofix --- planning/behavior_velocity_planner/package.xml | 3 +-- planning/behavior_velocity_planner/src/node.hpp | 6 ++---- planning/behavior_velocity_planner_common/package.xml | 2 +- .../behavior_velocity_traffic_light_module/src/scene.cpp | 2 +- 4 files changed, 5 insertions(+), 8 deletions(-) diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index acfba2ebc7d8b..229bf1f7d05ba 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -40,6 +40,7 @@ diagnostic_msgs eigen geometry_msgs + jpn_signal_v2i_msgs lanelet2_extension libboost-dev motion_utils @@ -59,8 +60,6 @@ tier4_planning_msgs tier4_v2x_msgs visualization_msgs - jpn_signal_v2i_msgs - ament_cmake_ros ament_lint_auto diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 91480bfca53b8..598595b33a7b6 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -25,13 +25,11 @@ #include #include #include +#include +#include #include #include #include -#include -#include - - #include #include #include diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index cc13b64dd8db1..8441be5b18dbb 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -26,6 +26,7 @@ eigen geometry_msgs interpolation + jpn_signal_v2i_msgs lanelet2_extension libboost-dev motion_utils @@ -47,7 +48,6 @@ tier4_v2x_msgs vehicle_info_util visualization_msgs - jpn_signal_v2i_msgs ament_cmake_ros ament_lint_auto diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 1ecbbe958050d..4f96a0a2c5f1d 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -289,7 +289,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const double reachable_distance = planner_data_->current_velocity->twist.linear.x * rest_time_to_go_ahead_allowed; - debug(signed_arc_length_t2.0o_stop_point); + debug(signed_arc_length_t2 .0o_stop_point); debug(reachable_distance); if (reachable_distance < signed_arc_length_to_stop_point) { From f91e9ad17c2cb0feb4c7252021838a7885bcfe2a Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 17 Oct 2023 16:01:07 +0900 Subject: [PATCH 04/24] fix missing charcter, and remove some debug codes Signed-off-by: Yuki Takagi --- .../behavior_velocity_planner/src/node.cpp | 39 ------------------- .../src/scene.cpp | 23 +---------- 2 files changed, 2 insertions(+), 60 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 05e08c5352003..fe5200904c91b 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -35,36 +35,6 @@ #include #include -#define debug(var) \ - do { \ - std::cerr << __func__ << ": " << __LINE__ << ", " << #var << " : "; \ - view(var); \ - } while (0) -template -void view(T e) -{ - std::cerr << e << std::endl; -} -template -void view(const std::vector & v) -{ - for (const auto & e : v) { - std::cerr << e << " "; - } - std::cerr << std::endl; -} -template -void view(const std::vector> & vv) -{ - for (const auto & v : vv) { - view(v); - } -} -#define line() \ - { \ - std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; \ - } - namespace { rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) @@ -328,8 +298,6 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( { std::lock_guard lock(mutex_); - // RCLCPP_WARN(get_logger(), "329"); - for (const auto & signal : msg->signals) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; @@ -343,17 +311,10 @@ void BehaviorVelocityPlannerNode::onTrafficSignalsRawV2I( { std::lock_guard lock(mutex_); - // RCLCPP_WARN(get_logger(), "344"); - for (const auto & car_light : msg->car_lights) { for (const auto & state : car_light.states) { planner_data_.traffic_light_time_to_red_id_map[state.traffic_signal_id] = car_light.min_rest_time_to_red; - // size_t set_id = state.traffic_signal_id; - // double set_value = car_light.min_rest_time; - - // debug(set_id); - // debug(set_value); } } } diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 4f96a0a2c5f1d..96d98497d9353 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -243,14 +243,8 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * input_path.points, self_pose->pose.position, stop_line_point_msg); setDistance(signed_arc_length_to_stop_point); - // line(); - // RCLCPP_INFO(logger_, "\nstate_: State::%s", (state_ == State::APPROACH) ? "APPROACH" : - // "GO_OUT"); RCLCPP_INFO(logger_, "\nis_prev_state_stop_ is: %s", is_prev_state_stop_ ? "true" : - // "false"); RCLCPP_INFO(logger_, "\nis_activated: %s", isActivated() ? "true" : "false"); - // Check state if (state_ == State::APPROACH) { - // line(); // Move to go out state if ego vehicle over deadline. constexpr double signed_deadline_length = -2.0; if (signed_arc_length_to_stop_point < signed_deadline_length) { @@ -259,12 +253,6 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } - // line(); - // RCLCPP_INFO( - // logger_, "\nstate_: State::%s", (state_ == State::APPROACH) ? "APPROACH" : "GO_OUT"); - // RCLCPP_INFO(logger_, "\nis_prev_state_stop_ is: %s", is_prev_state_stop_ ? "true" : "false"); - // RCLCPP_INFO(logger_, "\nis_activated: %s", isActivated() ? "true" : "false"); - first_ref_stop_path_point_index_ = stop_line_point_idx; // Check if stop is coming. @@ -283,14 +271,13 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * rest_time_to_go_ahead_allowed > 1e-6; RCLCPP_INFO(logger_, "\ndo_conject_by_v2i: %s, ", do_conject_by_v2i ? "true" : "false"); - // RCLCPP_INFO(logger_, "rest_time_allowed_to_go_ahead: %5f", rest_time_to_go_ahead_allowed); if (do_conject_by_v2i) { const double reachable_distance = planner_data_->current_velocity->twist.linear.x * rest_time_to_go_ahead_allowed; - debug(signed_arc_length_t2 .0o_stop_point); - debug(reachable_distance); + // debug(signed_arc_length_to_stop_point); + // debug(reachable_distance); if (reachable_distance < signed_arc_length_to_stop_point) { line(); @@ -306,12 +293,6 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } } - // line(); - // RCLCPP_INFO( - // logger_, "\nstate_: State::%s", (state_ == State::APPROACH) ? "APPROACH" : "GO_OUT"); - // RCLCPP_INFO(logger_, "\nis_prev_state_stop_ is: %s", is_prev_state_stop_ ? "true" : "false"); - // RCLCPP_INFO(logger_, "\nis_activated: %s", isActivated() ? "true" : "false"); - // Decide whether to stop or pass even if a stop signal is received. if (!isPassthrough(signed_arc_length_to_stop_point)) { *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); From f3fb92d13b41ee720b1da4f051e4cdd1f4667f31 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 17 Oct 2023 16:10:54 +0900 Subject: [PATCH 05/24] fix spell Signed-off-by: Yuki Takagi --- .../src/manager.cpp | 4 ++-- .../behavior_velocity_traffic_light_module/src/scene.cpp | 8 ++++---- .../behavior_velocity_traffic_light_module/src/scene.hpp | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index a3584493b757c..858c6693ec0c4 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -43,8 +43,8 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".yellow_lamp_period"); planner_param_.enable_conjecture_by_v2i = getOrDeclareParameter(node, ns + ".enable_conjecture_by_v2i"); - planner_param_.time_duration_to_conject_by_v2i = - getOrDeclareParameter(node, ns + ".time_duration_to_conject_by_v2i"); + planner_param_.time_duration_to_conjecture_by_v2i = + getOrDeclareParameter(node, ns + ".time_duration_to_conjecture_by_v2i"); planner_param_.last_time_allowed_to_pass_by_v2i = getOrDeclareParameter(node, ns + ".last_time_allowed_to_pass_by_v2i"); diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 96d98497d9353..08b6e0ae2b935 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -265,14 +265,14 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * debug(rest_time_to_red_signal); if (isActivated()) { - const bool do_conject_by_v2i = + const bool do_conjecture_by_v2i = planner_param_.enable_conjecture_by_v2i && - rest_time_to_go_ahead_allowed <= planner_param_.time_duration_to_conject_by_v2i && + rest_time_to_go_ahead_allowed <= planner_param_.time_duration_to_conjecture_by_v2i && rest_time_to_go_ahead_allowed > 1e-6; - RCLCPP_INFO(logger_, "\ndo_conject_by_v2i: %s, ", do_conject_by_v2i ? "true" : "false"); + RCLCPP_INFO(logger_, "\ndo_conjecture_by_v2i: %s, ", do_conjecture_by_v2i ? "true" : "false"); - if (do_conject_by_v2i) { + if (do_conjecture_by_v2i) { const double reachable_distance = planner_data_->current_velocity->twist.linear.x * rest_time_to_go_ahead_allowed; diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index caf51afcb9218..8f467e229dfd5 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -62,7 +62,7 @@ class TrafficLightModule : public SceneModuleInterface double yellow_lamp_period; bool enable_pass_judge; bool enable_conjecture_by_v2i; - double time_duration_to_conject_by_v2i; + double time_duration_to_conjecture_by_v2i; double last_time_allowed_to_pass_by_v2i; }; From 19fca541a2fd4bd8078353abd530f35cfcfe64e1 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 17 Oct 2023 22:16:15 +0900 Subject: [PATCH 06/24] add new param, move the params' name space Signed-off-by: Yuki Takagi --- .../config/traffic_light.param.yaml | 7 +++++++ .../src/manager.cpp | 14 ++++++++------ .../src/scene.cpp | 11 ++++++----- .../src/scene.hpp | 7 ++++--- 4 files changed, 25 insertions(+), 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 index e8e0357daa4a1..792f57962f3af 100644 --- 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 index 858c6693ec0c4..2506920098720 100644 --- 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(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); - planner_param_.enable_conjecture_by_v2i = - getOrDeclareParameter(node, ns + ".enable_conjecture_by_v2i"); - planner_param_.time_duration_to_conjecture_by_v2i = - getOrDeclareParameter(node, ns + ".time_duration_to_conjecture_by_v2i"); - planner_param_.last_time_allowed_to_pass_by_v2i = - getOrDeclareParameter(node, ns + ".last_time_allowed_to_pass_by_v2i"); + planner_param_.v2i_enable_conjecture = + getOrDeclareParameter(node, ns + ".v2i.enable_conjecture"); + planner_param_.v2i_time_duration_to_conjecture = + getOrDeclareParameter(node, ns + ".v2i.time_duration_to_conjecture"); + planner_param_.v2i_last_time_allowed_to_pass = + getOrDeclareParameter(node, ns + ".v2i.last_time_allowed_to_pass"); + planner_param_.v2i_min_assumed_velocity = + getOrDeclareParameter(node, ns + ".v2i.min_assumed_velocity"); pub_tl_state_ = node.create_publisher( "~/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 index 08b6e0ae2b935..38f8b434dac56 100644 --- 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 index 8f467e229dfd5..820eae2a37ed9 100644 --- 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: From 35ab1220e6b53fa31df3a733beb86e2dfc7977b5 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 17 Oct 2023 13:18:35 +0000 Subject: [PATCH 07/24] style(pre-commit): autofix --- .../config/traffic_light.param.yaml | 1 - 1 file changed, 1 deletion(-) 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 index 792f57962f3af..db21816783be7 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -12,4 +12,3 @@ 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 - From aeac2c825df59d2efc90416b23140c8b4278ef5f Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Thu, 19 Oct 2023 14:17:45 +0900 Subject: [PATCH 08/24] reconstruct the decision logic Signed-off-by: Yuki Takagi --- .../src/scene.cpp | 48 ++++--------------- 1 file changed, 10 insertions(+), 38 deletions(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 38f8b434dac56..e25f181c37fd5 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -255,49 +255,21 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * first_ref_stop_path_point_index_ = stop_line_point_idx; - // Check if stop is coming. - setSafe(!isStopSignal()); - const double rest_time_to_red_signal = planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id()); + debug(rest_time_to_red_signal); const double rest_time_to_go_ahead_allowed = 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_.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 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); - - if (reachable_distance < signed_arc_length_to_stop_point) { - line(); - *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); - is_prev_state_stop_ = true; - } else { - is_prev_state_stop_ = false; - return true; - } - } else { - is_prev_state_stop_ = false; - return true; + const double ego_v = planner_data_->current_velocity->twist.linear.x; + if (ego_v >= 0.5) { + if (ego_v * rest_time_to_go_ahead_allowed <= signed_arc_length_to_stop_point) { + *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + } + } else { + if (rest_time_to_go_ahead_allowed < 5.0) { + *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); } - } - - // Decide whether to stop or pass even if a stop signal is received. - if (!isPassthrough(signed_arc_length_to_stop_point)) { - *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); - is_prev_state_stop_ = true; } return true; } else if (state_ == State::GO_OUT) { @@ -340,7 +312,7 @@ bool TrafficLightModule::updateTrafficSignal() return true; } -bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const +bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const //unused in the V2I temporary implimentation { const double max_acc = planner_data_->max_stop_acceleration_threshold; const double max_jerk = planner_data_->max_stop_jerk_threshold; From 40f7fc619dc6e8882b77a40d164335544af88855 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Thu, 19 Oct 2023 14:38:03 +0900 Subject: [PATCH 09/24] set up the required parameters Signed-off-by: Yuki Takagi --- .../config/traffic_light.param.yaml | 5 ++--- .../src/manager.cpp | 10 ++++------ .../src/scene.cpp | 7 ++++--- .../src/scene.hpp | 5 ++--- 4 files changed, 12 insertions(+), 15 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 index db21816783be7..c0290a6dd03b3 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -8,7 +8,6 @@ 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 + velocity_threshold: 0.5 # change the decision logic whether the current velocity is faster or not + required_time_to departure: 5.0 diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 2506920098720..02390310520d1 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -41,14 +41,12 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); - planner_param_.v2i_enable_conjecture = - getOrDeclareParameter(node, ns + ".v2i.enable_conjecture"); - planner_param_.v2i_time_duration_to_conjecture = - getOrDeclareParameter(node, ns + ".v2i.time_duration_to_conjecture"); planner_param_.v2i_last_time_allowed_to_pass = getOrDeclareParameter(node, ns + ".v2i.last_time_allowed_to_pass"); - planner_param_.v2i_min_assumed_velocity = - getOrDeclareParameter(node, ns + ".v2i.min_assumed_velocity"); + planner_param_.v2i_velocity_threshold = + getOrDeclareParameter(node, ns + ".v2i.velocity_threshold"); + planner_param_.v2i_required_time_to_departure = + getOrDeclareParameter(node, ns + ".v2i.required_time_to departure"); pub_tl_state_ = node.create_publisher( "~/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 index e25f181c37fd5..a895a8b58e641 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -262,12 +262,12 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * rest_time_to_red_signal - planner_param_.v2i_last_time_allowed_to_pass; const double ego_v = planner_data_->current_velocity->twist.linear.x; - if (ego_v >= 0.5) { + if (ego_v >= planner_param_.v2i_velocity_threshold) { if (ego_v * rest_time_to_go_ahead_allowed <= signed_arc_length_to_stop_point) { *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); } } else { - if (rest_time_to_go_ahead_allowed < 5.0) { + if (rest_time_to_go_ahead_allowed < planner_param_.v2i_required_time_to_departure) { *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); } } @@ -312,7 +312,8 @@ bool TrafficLightModule::updateTrafficSignal() return true; } -bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const //unused in the V2I temporary implimentation +bool TrafficLightModule::isPassthrough( + const double & signed_arc_length) const // unused in the V2I temporary implimentation { const double max_acc = planner_data_->max_stop_acceleration_threshold; const double max_jerk = planner_data_->max_stop_jerk_threshold; diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 820eae2a37ed9..c48686ae793ea 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -61,10 +61,9 @@ class TrafficLightModule : public SceneModuleInterface double tl_state_timeout; double yellow_lamp_period; bool enable_pass_judge; - bool v2i_enable_conjecture; double v2i_last_time_allowed_to_pass; - double v2i_time_duration_to_conjecture; - double v2i_min_assumed_velocity; + double v2i_velocity_threshold; + double v2i_required_time_to_departure; }; public: From 76d6365dfd86ba9de22995895ec433d7504b4e8f Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Thu, 19 Oct 2023 15:03:04 +0900 Subject: [PATCH 10/24] clean up the debug msg Signed-off-by: Yuki Takagi --- .../src/scene.cpp | 35 +++---------------- 1 file changed, 4 insertions(+), 31 deletions(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index a895a8b58e641..306f99ef039fc 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -34,36 +34,6 @@ #include #include -#define debug(var) \ - do { \ - std::cerr << __func__ << ": " << __LINE__ << ", " << #var << " : "; \ - view(var); \ - } while (0) -template -void view(T e) -{ - std::cerr << e << std::endl; -} -template -void view(const std::vector & v) -{ - for (const auto & e : v) { - std::cerr << e << " "; - } - std::cerr << std::endl; -} -template -void view(const std::vector > & vv) -{ - for (const auto & v : vv) { - view(v); - } -} -#define line() \ - { \ - std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; \ - } - namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -257,7 +227,10 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const double rest_time_to_red_signal = planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id()); - debug(rest_time_to_red_signal); + + // RCLCPP_DEBUG(logger_, "rest time to the next signal turn to red: %2.2f", rest_time_to_red_signal); + RCLCPP_INFO(logger_, "rest time to the next signal turn to red: %2.2f", rest_time_to_red_signal); + const double rest_time_to_go_ahead_allowed = rest_time_to_red_signal - planner_param_.v2i_last_time_allowed_to_pass; From 92f381bf8c813f3fcd1587c6720bf70d15e35344 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 19 Oct 2023 06:04:58 +0000 Subject: [PATCH 11/24] style(pre-commit): autofix --- .../behavior_velocity_traffic_light_module/src/scene.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 306f99ef039fc..daf1766198402 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -227,9 +227,11 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const double rest_time_to_red_signal = planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id()); - - // RCLCPP_DEBUG(logger_, "rest time to the next signal turn to red: %2.2f", rest_time_to_red_signal); - RCLCPP_INFO(logger_, "rest time to the next signal turn to red: %2.2f", rest_time_to_red_signal); + + // RCLCPP_DEBUG(logger_, "rest time to the next signal turn to red: %2.2f", + // rest_time_to_red_signal); + RCLCPP_INFO( + logger_, "rest time to the next signal turn to red: %2.2f", rest_time_to_red_signal); const double rest_time_to_go_ahead_allowed = rest_time_to_red_signal - planner_param_.v2i_last_time_allowed_to_pass; From a9c848f917ac08014ad848cac79913f6f55703bc Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Thu, 19 Oct 2023 16:04:23 +0900 Subject: [PATCH 12/24] set up RCLCPP_DEBUG print Signed-off-by: Yuki Takagi --- .../config/traffic_light.param.yaml | 2 +- .../src/scene.cpp | 26 ++++++++++++------- 2 files changed, 18 insertions(+), 10 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 index c0290a6dd03b3..8a2c9ac71cc33 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -10,4 +10,4 @@ v2i: last_time_allowed_to_pass: 2.0 # # relative time against at the time of turn to red velocity_threshold: 0.5 # change the decision logic whether the current velocity is faster or not - required_time_to departure: 5.0 + required_time_to departure: 3.0 diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index daf1766198402..7adffa166cbff 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -227,22 +227,30 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const double rest_time_to_red_signal = planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id()); - - // RCLCPP_DEBUG(logger_, "rest time to the next signal turn to red: %2.2f", - // rest_time_to_red_signal); - RCLCPP_INFO( - logger_, "rest time to the next signal turn to red: %2.2f", rest_time_to_red_signal); - - const double rest_time_to_go_ahead_allowed = + const double rest_time_allowed_to_go_ahead = rest_time_to_red_signal - planner_param_.v2i_last_time_allowed_to_pass; + RCLCPP_DEBUG( + logger_, "\nrest_time_allowed_to_go_ahead: %2.2f = %2.2f - %2.2f", + rest_time_allowed_to_go_ahead, rest_time_to_red_signal, + planner_param_.v2i_last_time_allowed_to_pass); + const double ego_v = planner_data_->current_velocity->twist.linear.x; if (ego_v >= planner_param_.v2i_velocity_threshold) { - if (ego_v * rest_time_to_go_ahead_allowed <= signed_arc_length_to_stop_point) { + RCLCPP_DEBUG( + logger_, "\nego moves enough fast %2.2f > %2.2f", ego_v, + planner_param_.v2i_velocity_threshold); + if (ego_v * rest_time_allowed_to_go_ahead <= signed_arc_length_to_stop_point) { + RCLCPP_DEBUG( + logger_, "\nplan to stop because ego will not reach %2.2f <= %2.2f", + ego_v * rest_time_allowed_to_go_ahead, signed_arc_length_to_stop_point); *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); } } else { - if (rest_time_to_go_ahead_allowed < planner_param_.v2i_required_time_to_departure) { + if (rest_time_allowed_to_go_ahead < planner_param_.v2i_required_time_to_departure) { + RCLCPP_DEBUG( + logger_, "\nplan to stop because there is enough rest time to depature %2.2f < %2.2f", + rest_time_allowed_to_go_ahead, planner_param_.v2i_required_time_to_departure); *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); } } From 789a2142a9ef716dca9171bd85d9afe5a2fcb218 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Thu, 19 Oct 2023 16:54:55 +0900 Subject: [PATCH 13/24] add explicit include Signed-off-by: Yuki Takagi --- .../include/behavior_velocity_planner_common/planner_data.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 2013078e97d34..ebc72e40c7cef 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -51,6 +51,7 @@ #include #include #include +#include namespace behavior_velocity_planner { From b477a7206ea3fd2b3ea5856215a0560694cf287a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 19 Oct 2023 07:57:23 +0000 Subject: [PATCH 14/24] style(pre-commit): autofix --- .../include/behavior_velocity_planner_common/planner_data.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index ebc72e40c7cef..fc301620093da 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -48,10 +48,10 @@ #include #include +#include #include #include #include -#include namespace behavior_velocity_planner { From 0d64d000670820cb2cc2522b98b17d15c1ad6e1f Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Wed, 25 Oct 2023 14:31:35 +0900 Subject: [PATCH 15/24] Update planning/behavior_velocity_traffic_light_module/src/scene.cpp fix spelling Co-authored-by: Fumiya Watanabe --- planning/behavior_velocity_traffic_light_module/src/scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 7adffa166cbff..80cf6ac93fd68 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -249,7 +249,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } else { if (rest_time_allowed_to_go_ahead < planner_param_.v2i_required_time_to_departure) { RCLCPP_DEBUG( - logger_, "\nplan to stop because there is enough rest time to depature %2.2f < %2.2f", + logger_, "\nplan to stop because there is enough rest time to departure %2.2f < %2.2f", rest_time_allowed_to_go_ahead, planner_param_.v2i_required_time_to_departure); *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); } From 352fe0d8e3c498a088ed5162b70be9ff32c36418 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Wed, 25 Oct 2023 14:32:00 +0900 Subject: [PATCH 16/24] Update planning/behavior_velocity_traffic_light_module/src/scene.cpp fix spelling Co-authored-by: Fumiya Watanabe --- planning/behavior_velocity_traffic_light_module/src/scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 80cf6ac93fd68..f8c701fc17cfb 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -296,7 +296,7 @@ bool TrafficLightModule::updateTrafficSignal() } bool TrafficLightModule::isPassthrough( - const double & signed_arc_length) const // unused in the V2I temporary implimentation + const double & signed_arc_length) const // unused in the V2I temporary imprementation { const double max_acc = planner_data_->max_stop_acceleration_threshold; const double max_jerk = planner_data_->max_stop_jerk_threshold; From 59df872aaabd39b1f33d77afcf2b221701e980b1 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Wed, 25 Oct 2023 14:33:57 +0900 Subject: [PATCH 17/24] Update planning/behavior_velocity_traffic_light_module/src/manager.cpp missing "_" Co-authored-by: Fumiya Watanabe --- planning/behavior_velocity_traffic_light_module/src/manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 02390310520d1..f9080f9004258 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -46,7 +46,7 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) planner_param_.v2i_velocity_threshold = getOrDeclareParameter(node, ns + ".v2i.velocity_threshold"); planner_param_.v2i_required_time_to_departure = - getOrDeclareParameter(node, ns + ".v2i.required_time_to departure"); + getOrDeclareParameter(node, ns + ".v2i.required_time_to_departure"); pub_tl_state_ = node.create_publisher( "~/output/traffic_signal", 1); From 78ed4ab5f558774fdad0614b1fb3cac10c246e6b Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Wed, 25 Oct 2023 14:34:05 +0900 Subject: [PATCH 18/24] Update planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml missing "_" Co-authored-by: Fumiya Watanabe --- .../config/traffic_light.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 8a2c9ac71cc33..73e0944067c9b 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -10,4 +10,4 @@ v2i: last_time_allowed_to_pass: 2.0 # # relative time against at the time of turn to red velocity_threshold: 0.5 # change the decision logic whether the current velocity is faster or not - required_time_to departure: 3.0 + required_time_to_departure: 3.0 From daa5b44ae5e0689aaa354b4c06da55c85f5e8aed Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Wed, 25 Oct 2023 14:41:11 +0900 Subject: [PATCH 19/24] fix spelling Signed-off-by: Yuki Takagi --- planning/behavior_velocity_traffic_light_module/src/scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index f8c701fc17cfb..80cf6ac93fd68 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -296,7 +296,7 @@ bool TrafficLightModule::updateTrafficSignal() } bool TrafficLightModule::isPassthrough( - const double & signed_arc_length) const // unused in the V2I temporary imprementation + const double & signed_arc_length) const // unused in the V2I temporary implimentation { const double max_acc = planner_data_->max_stop_acceleration_threshold; const double max_jerk = planner_data_->max_stop_jerk_threshold; From 40654c7f51a805d5eb40decf0f66546f9ad6807b Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Wed, 25 Oct 2023 16:16:27 +0900 Subject: [PATCH 20/24] fix spell Signed-off-by: Yuki Takagi --- planning/behavior_velocity_traffic_light_module/src/scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 80cf6ac93fd68..4e0664a3385aa 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -296,7 +296,7 @@ bool TrafficLightModule::updateTrafficSignal() } bool TrafficLightModule::isPassthrough( - const double & signed_arc_length) const // unused in the V2I temporary implimentation + const double & signed_arc_length) const // unused in the V2I temporary implementation { const double max_acc = planner_data_->max_stop_acceleration_threshold; const double max_jerk = planner_data_->max_stop_jerk_threshold; From d5be6b1545643ec95caf0402d82fb694b2665352 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Mon, 30 Oct 2023 16:28:16 +0900 Subject: [PATCH 21/24] address non-V2I environments Signed-off-by: Yuki Takagi --- .../planner_data.hpp | 10 ++-- .../config/traffic_light.param.yaml | 5 +- .../src/manager.cpp | 1 + .../src/scene.cpp | 54 +++++++++---------- .../src/scene.hpp | 1 + 5 files changed, 36 insertions(+), 35 deletions(-) diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index fc301620093da..ad374294d0686 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -141,12 +141,14 @@ struct PlannerData } return std::make_shared(traffic_light_id_map.at(id)); } - double getRestTimeToRedSignal(const int id) const + + std::optional getRestTimeToRedSignal(const int id) const { - if (traffic_light_time_to_red_id_map.count(id) == 0) { - return std::numeric_limits::max(); + try { + return traffic_light_time_to_red_id_map.at(id); + } catch (std::out_of_range &) { + return std::nullopt; } - return traffic_light_time_to_red_id_map.at(id); } }; } // namespace behavior_velocity_planner 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 index 73e0944067c9b..5d4bc392571ea 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -8,6 +8,7 @@ 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: - last_time_allowed_to_pass: 2.0 # # relative time against at the time of turn to red + use_rest_time: true + last_time_allowed_to_pass: 2.0 # relative time against at the time of turn to red velocity_threshold: 0.5 # change the decision logic whether the current velocity is faster or not - required_time_to_departure: 3.0 + required_time_to_departure: 3.0 # prevent low speed pass \ No newline at end of file diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index f9080f9004258..8ef247847c503 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -41,6 +41,7 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); + planner_param_.v2i_use_rest_time = getOrDeclareParameter(node, ns + ".v2i.use_rest_time"); planner_param_.v2i_last_time_allowed_to_pass = getOrDeclareParameter(node, ns + ".v2i.last_time_allowed_to_pass"); planner_param_.v2i_velocity_threshold = diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 4e0664a3385aa..8c3620f07b1f6 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -225,36 +225,33 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * first_ref_stop_path_point_index_ = stop_line_point_idx; - const double rest_time_to_red_signal = + const std::optional rest_time_to_red_signal = planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id()); - const double rest_time_allowed_to_go_ahead = - rest_time_to_red_signal - planner_param_.v2i_last_time_allowed_to_pass; - - RCLCPP_DEBUG( - logger_, "\nrest_time_allowed_to_go_ahead: %2.2f = %2.2f - %2.2f", - rest_time_allowed_to_go_ahead, rest_time_to_red_signal, - planner_param_.v2i_last_time_allowed_to_pass); - - const double ego_v = planner_data_->current_velocity->twist.linear.x; - if (ego_v >= planner_param_.v2i_velocity_threshold) { - RCLCPP_DEBUG( - logger_, "\nego moves enough fast %2.2f > %2.2f", ego_v, - planner_param_.v2i_velocity_threshold); - if (ego_v * rest_time_allowed_to_go_ahead <= signed_arc_length_to_stop_point) { - RCLCPP_DEBUG( - logger_, "\nplan to stop because ego will not reach %2.2f <= %2.2f", - ego_v * rest_time_allowed_to_go_ahead, signed_arc_length_to_stop_point); - *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); - } - } else { - if (rest_time_allowed_to_go_ahead < planner_param_.v2i_required_time_to_departure) { - RCLCPP_DEBUG( - logger_, "\nplan to stop because there is enough rest time to departure %2.2f < %2.2f", - rest_time_allowed_to_go_ahead, planner_param_.v2i_required_time_to_departure); - *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + if (planner_param_.v2i_use_rest_time && rest_time_to_red_signal) { + const double rest_time_allowed_to_go_ahead = + rest_time_to_red_signal.value() - planner_param_.v2i_last_time_allowed_to_pass; + + const double ego_v = planner_data_->current_velocity->twist.linear.x; + if (ego_v >= planner_param_.v2i_velocity_threshold) { + if (ego_v * rest_time_allowed_to_go_ahead <= signed_arc_length_to_stop_point) { + *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + } + } else { + if (rest_time_allowed_to_go_ahead < planner_param_.v2i_required_time_to_departure) { + *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + } } + return true; + } + setSafe(!isStopSignal()); + if (isActivated()) { + is_prev_state_stop_ = false; + return true; + } + if (!isPassthrough(signed_arc_length_to_stop_point)) { + *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + is_prev_state_stop_ = true; } - return true; } else if (state_ == State::GO_OUT) { // Initialize if vehicle is far from stop_line constexpr bool use_initialization_after_start = true; @@ -295,8 +292,7 @@ bool TrafficLightModule::updateTrafficSignal() return true; } -bool TrafficLightModule::isPassthrough( - const double & signed_arc_length) const // unused in the V2I temporary implementation +bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const { const double max_acc = planner_data_->max_stop_acceleration_threshold; const double max_jerk = planner_data_->max_stop_jerk_threshold; diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index c48686ae793ea..b53f9c55e7bb6 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -61,6 +61,7 @@ class TrafficLightModule : public SceneModuleInterface double tl_state_timeout; double yellow_lamp_period; bool enable_pass_judge; + bool v2i_use_rest_time; double v2i_last_time_allowed_to_pass; double v2i_velocity_threshold; double v2i_required_time_to_departure; From a74638c05c35a126fe6b4e0f06853e8d4e8837cd Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 30 Oct 2023 07:31:57 +0000 Subject: [PATCH 22/24] style(pre-commit): autofix --- .../config/traffic_light.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 5d4bc392571ea..a80c97779edaf 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -11,4 +11,4 @@ use_rest_time: true last_time_allowed_to_pass: 2.0 # relative time against at the time of turn to red velocity_threshold: 0.5 # change the decision logic whether the current velocity is faster or not - required_time_to_departure: 3.0 # prevent low speed pass \ No newline at end of file + required_time_to_departure: 3.0 # prevent low speed pass From b41ad886e99ef9056a4fddc087fd5282dbf4ec3c Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Mon, 30 Oct 2023 16:41:40 +0900 Subject: [PATCH 23/24] clean up Signed-off-by: Yuki Takagi --- .../behavior_velocity_traffic_light_module/src/scene.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 8c3620f07b1f6..04f050e389087 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -243,15 +243,20 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } return true; } + + // Check if stop is coming. setSafe(!isStopSignal()); if (isActivated()) { is_prev_state_stop_ = false; return true; } + // Decide whether to stop or pass even if a stop signal is received. if (!isPassthrough(signed_arc_length_to_stop_point)) { *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); is_prev_state_stop_ = true; } + return true; + } else if (state_ == State::GO_OUT) { // Initialize if vehicle is far from stop_line constexpr bool use_initialization_after_start = true; @@ -264,6 +269,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } return true; } + return false; } @@ -297,6 +303,7 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const const double max_acc = planner_data_->max_stop_acceleration_threshold; const double max_jerk = planner_data_->max_stop_jerk_threshold; const double delay_response_time = planner_data_->delay_response_time; + const double reachable_distance = planner_data_->current_velocity->twist.linear.x * planner_param_.yellow_lamp_period; @@ -387,6 +394,7 @@ bool TrafficLightModule::findValidTrafficSignal(TrafficSignal & valid_traffic_si "time diff: " << (clock_->now() - traffic_signal_stamped->stamp).seconds()); return false; } + valid_traffic_signal = traffic_signal_stamped->signal; return true; } From 054bfa0602774a47da6c605b5d06ca3481757d8d Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 31 Oct 2023 17:04:04 +0900 Subject: [PATCH 24/24] restore blank lines Signed-off-by: Yuki Takagi --- planning/behavior_velocity_traffic_light_module/src/scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 04f050e389087..e8b3c0598514a 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -250,13 +250,13 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * is_prev_state_stop_ = false; return true; } + // Decide whether to stop or pass even if a stop signal is received. if (!isPassthrough(signed_arc_length_to_stop_point)) { *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); is_prev_state_stop_ = true; } return true; - } else if (state_ == State::GO_OUT) { // Initialize if vehicle is far from stop_line constexpr bool use_initialization_after_start = true;