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;