From be5842836db95d7eedf86549261804ac5eb1a6dc Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Sat, 18 May 2024 00:21:11 +0900 Subject: [PATCH] feat(behavior_velocity_traffic_light): do not overwrite unknown with past known color (#7044) feat(behavior_velocity_traffic_light): do not overwrite UNKNOWN with past KNOWN color remove first_stop_path_point_index Revert "remove first_stop_path_point_index" This reverts commit 09bba6981c0478832d90e3c0af1212ff2b94d928. fix Signed-off-by: Mamoru Sobue --- .../behavior_velocity_planner/src/node.cpp | 1 + .../package.xml | 1 + .../src/manager.cpp | 39 ++++++------------- .../src/manager.hpp | 2 +- .../src/scene.cpp | 9 ++--- .../src/scene.hpp | 5 +-- 6 files changed, 20 insertions(+), 37 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index aa9613a35dd7c..48f263e14ba72 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -352,6 +352,7 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = msg->stamp; } else { + // if (1)the observation is not UNKNOWN or (2)the very first observation is UNKNOWN planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; } } diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index 33bb471e5920c..544e14f130a7e 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -8,6 +8,7 @@ Satoshi Ota Tomoya Kimura Shumpei Wakabayashi + Mamoru Sobue Apache License 2.0 diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 89817f3342dbd..bdc7f04360ff0 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -62,7 +62,7 @@ void TrafficLightModuleManager::modifyPathVelocity( stop_reason_array.header.stamp = path->header.stamp; first_stop_path_point_index_ = static_cast(path->points.size() - 1); - first_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); + nearest_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); for (const auto & scene_module : scene_modules_) { tier4_planning_msgs::msg::StopReason stop_reason; std::shared_ptr traffic_light_scene_module( @@ -85,8 +85,8 @@ void TrafficLightModuleManager::modifyPathVelocity( } if ( traffic_light_scene_module->getFirstRefStopPathPointIndex() < - first_ref_stop_path_point_index_) { - first_ref_stop_path_point_index_ = + nearest_ref_stop_path_point_index_) { + nearest_ref_stop_path_point_index_ = traffic_light_scene_module->getFirstRefStopPathPointIndex(); if ( traffic_light_scene_module->getTrafficLightModuleState() != @@ -126,14 +126,13 @@ void TrafficLightModuleManager::launchNewModules( // Use lanelet_id to unregister module when the route is changed const auto lane_id = traffic_light_reg_elem.second.id(); - const auto module_id = lane_id; - if (!isModuleRegisteredFromExistingAssociatedModule(module_id)) { + if (!isModuleRegisteredFromExistingAssociatedModule(lane_id)) { registerModule(std::make_shared( - module_id, lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, - planner_param_, logger_.get_child("traffic_light_module"), clock_)); - generateUUID(module_id); + lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, + logger_.get_child("traffic_light_module"), clock_)); + generateUUID(lane_id); updateRTCStatus( - getUUID(module_id), true, std::numeric_limits::lowest(), path.header.stamp); + getUUID(lane_id), true, std::numeric_limits::lowest(), path.header.stamp); } } } @@ -145,9 +144,10 @@ TrafficLightModuleManager::getModuleExpiredFunction( const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [this, lanelet_id_set](const std::shared_ptr & scene_module) { + return [this, lanelet_id_set]( + [[maybe_unused]] const std::shared_ptr & scene_module) { for (const auto & id : lanelet_id_set) { - if (isModuleRegisteredFromRegElement(id, scene_module->getModuleId())) { + if (isModuleRegisteredFromExistingAssociatedModule(id)) { return false; } } @@ -155,23 +155,6 @@ TrafficLightModuleManager::getModuleExpiredFunction( }; } -bool TrafficLightModuleManager::isModuleRegisteredFromRegElement( - const lanelet::Id & id, const size_t module_id) const -{ - const auto lane = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(id); - - const auto registered_lane = - planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(module_id); - for (const auto & registered_element : registered_lane.regulatoryElementsAs()) { - for (const auto & element : lane.regulatoryElementsAs()) { - if (hasSameTrafficLight(element, registered_element)) { - return true; - } - } - } - return false; -} - bool TrafficLightModuleManager::isModuleRegisteredFromExistingAssociatedModule( const lanelet::Id & id) const { diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index 87213d8a5ed3f..959ef2f91d36c 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -58,7 +58,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC // Debug rclcpp::Publisher::SharedPtr pub_tl_state_; - std::optional first_ref_stop_path_point_index_; + std::optional nearest_ref_stop_path_point_index_; }; class TrafficLightModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 413ce78beacee..0158251bb42b4 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -162,11 +162,10 @@ bool calcStopPointAndInsertIndex( } // namespace TrafficLightModule::TrafficLightModule( - const int64_t module_id, const int64_t lane_id, - const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, - const PlannerParam & planner_param, const rclcpp::Logger logger, + const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, + lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), +: SceneModuleInterface(lane_id, logger, clock), lane_id_(lane_id), traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), @@ -365,7 +364,7 @@ bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_tra { // get traffic signal associated with the regulatory element id const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal( - traffic_light_reg_elem_.id(), true /* traffic light module keeps last observation */); + traffic_light_reg_elem_.id(), false /* traffic light module does not keep last observation */); if (!traffic_signal_stamped_opt) { RCLCPP_WARN_STREAM_ONCE( logger_, "the traffic signal data associated with regulatory element id " diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 102ddbe2e9fa8..d6a409ca7034a 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -66,9 +66,8 @@ class TrafficLightModule : public SceneModuleInterface public: TrafficLightModule( - const int64_t module_id, const int64_t lane_id, - const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, - const PlannerParam & planner_param, const rclcpp::Logger logger, + const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, + lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;