Skip to content

Commit

Permalink
fix(behavior_velocity_planner, behavior_path_planner): refresh raw tr…
Browse files Browse the repository at this point in the history
…affic light id map every callback (autowarefoundation#6061)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
  • Loading branch information
soblin authored and shmpwk committed Jan 26, 2024
1 parent 6e52feb commit a14dc6c
Show file tree
Hide file tree
Showing 2 changed files with 12 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -784,6 +784,7 @@ void BehaviorPathPlannerNode::onTrafficSignals(const TrafficSignalArray::ConstSh
{
std::lock_guard<std::mutex> lock(mutex_pd_);

planner_data_->traffic_light_id_map.clear();
for (const auto & signal : msg->signals) {
TrafficSignalStamped traffic_signal;
traffic_signal.stamp = msg->stamp;
Expand Down
14 changes: 11 additions & 3 deletions planning/behavior_velocity_planner/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -323,6 +323,11 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
{
std::lock_guard<std::mutex> lock(mutex_);

// clear previous observation
planner_data_.traffic_light_id_map_raw_.clear();
const auto traffic_light_id_map_last_observed_old =
planner_data_.traffic_light_id_map_last_observed_;
planner_data_.traffic_light_id_map_last_observed_.clear();
for (const auto & signal : msg->signals) {
TrafficSignalStamped traffic_signal;
traffic_signal.stamp = msg->stamp;
Expand All @@ -334,9 +339,12 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
});
// if the observation is UNKNOWN and past observation is available, only update the timestamp
// and keep the body of the info
if (
is_unknown_observation &&
planner_data_.traffic_light_id_map_last_observed_.count(signal.traffic_signal_id) == 1) {
const auto old_data = traffic_light_id_map_last_observed_old.find(signal.traffic_signal_id);
if (is_unknown_observation && old_data != traffic_light_id_map_last_observed_old.end()) {
// copy last observation
planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] =
old_data->second;
// update timestamp
planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp =
msg->stamp;
} else {
Expand Down

0 comments on commit a14dc6c

Please sign in to comment.