Skip to content

Commit

Permalink
fix(crosswalk_traffic_light_estimator): remove last detect results th…
Browse files Browse the repository at this point in the history 
…at have stopped updating (#1382)

* fix(crosswalk_traffic_light_estimator): add timeout param for last detect color

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(behavior_velocity_planner): remove old detected result

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
@satoshi-ota
satoshi-ota authored Jul 25, 2022
1 parent d9d3932 commit 4b3b569
Show file tree
Hide file tree
Showing 2 changed files with 53 additions and 35 deletions.
14 changes: 6 additions & 8 deletions ...tion/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ using autoware_auto_planning_msgs::msg::HADMapRoute;
using tier4_autoware_utils::DebugPublisher;
using tier4_autoware_utils::StopWatch;
using tier4_debug_msgs::msg::Float64Stamped;
using TrafficLightIdMap = std::unordered_map<lanelet::Id, TrafficSignal>;

class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node
{
Expand All @@ -72,28 +73,25 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node
void onRoute(const HADMapRoute::ConstSharedPtr msg);
void onTrafficLightArray(const TrafficSignalArray::ConstSharedPtr msg);

void updateLastDetectedSignal(const lanelet::Id & id, const uint8_t color);
void updateLastDetectedSignal(const TrafficLightIdMap & traffic_signals);
void setCrosswalkTrafficSignal(
const lanelet::ConstLanelet & crosswalk, const uint8_t color, TrafficSignalArray & msg) const;

lanelet::ConstLanelets getGreenLanelets(
const lanelet::ConstLanelets & lanelets,
const std::unordered_map<lanelet::Id, TrafficSignal> & traffic_light_id_map);
const lanelet::ConstLanelets & lanelets, const TrafficLightIdMap & traffic_light_id_map) const;

uint8_t estimateCrosswalkTrafficSignal(
const lanelet::ConstLanelet & crosswalk, const lanelet::ConstLanelets & green_lanelets) const;

uint8_t getHighestConfidenceTrafficSignal(
boost::optional<uint8_t> getHighestConfidenceTrafficSignal(
const lanelet::ConstLineStringsOrPolygons3d & traffic_lights,
const std::unordered_map<lanelet::Id, TrafficSignal> & traffic_light_id_map) const;

uint8_t getLastDetectedTrafficSignal(const lanelet::Id & id) const;
const TrafficLightIdMap & traffic_light_id_map) const;

// Node param
bool use_last_detect_color_;

// Signal history
std::unordered_map<uint32_t, uint8_t> last_detect_color_;
TrafficLightIdMap last_detect_color_;

// Stop watch
StopWatch<std::chrono::milliseconds> stop_watch_;
Expand Down
74 changes: 47 additions & 27 deletions perception/crosswalk_traffic_light_estimator/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -171,24 +171,45 @@ void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray(
setCrosswalkTrafficSignal(crosswalk, crosswalk_tl_color, output);
}

updateLastDetectedSignal(traffic_light_id_map);

pub_traffic_light_array_->publish(output);
pub_processing_time_->publish<Float64Stamped>("processing_time_ms", stop_watch.toc("Total"));

return;
}

void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal(
const lanelet::Id & id, const uint8_t color)
const TrafficLightIdMap & traffic_light_id_map)
{
if (color == TrafficLight::UNKNOWN) {
return;
}
for (const auto & input_traffic_signal : traffic_light_id_map) {
const auto & lights = input_traffic_signal.second.lights;

if (lights.empty()) {
continue;
}

if (lights.front().color == TrafficLight::UNKNOWN) {
continue;
}

const auto & id = input_traffic_signal.second.map_primitive_id;

if (last_detect_color_.count(id) == 0) {
last_detect_color_.insert(std::make_pair(id, input_traffic_signal.second));
continue;
}

if (last_detect_color_.count(id) == 0) {
last_detect_color_.insert(std::make_pair(id, color));
last_detect_color_.at(id) = input_traffic_signal.second;
}

last_detect_color_.at(id) = color;
for (auto & last_traffic_signal : last_detect_color_) {
const auto & id = last_traffic_signal.second.map_primitive_id;

if (traffic_light_id_map.count(id) == 0) {
last_detect_color_.erase(id);
}
}
}

void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal(
Expand All @@ -214,8 +235,7 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal(
}

lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getGreenLanelets(
const lanelet::ConstLanelets & lanelets,
const std::unordered_map<lanelet::Id, TrafficSignal> & traffic_light_id_map)
const lanelet::ConstLanelets & lanelets, const TrafficLightIdMap & traffic_light_id_map) const
{
lanelet::ConstLanelets green_lanelets{};

Expand All @@ -231,13 +251,23 @@ lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getGreenLanelets(

const auto current_detected_signal =
getHighestConfidenceTrafficSignal(traffic_lights_for_vehicle, traffic_light_id_map);
const auto is_green = current_detected_signal == TrafficLight::GREEN;

const auto last_detected_signal = getLastDetectedTrafficSignal(tl_reg_elem->id());
const auto was_green = current_detected_signal == TrafficLight::UNKNOWN &&
last_detected_signal == TrafficLight::GREEN && use_last_detect_color_;
if (!current_detected_signal) {
continue;
}

const auto is_green = current_detected_signal.get() == TrafficLight::GREEN;

updateLastDetectedSignal(tl_reg_elem->id(), current_detected_signal);
const auto last_detected_signal =
getHighestConfidenceTrafficSignal(traffic_lights_for_vehicle, last_detect_color_);

if (!last_detected_signal) {
continue;
}

const auto was_green = current_detected_signal.get() == TrafficLight::UNKNOWN &&
last_detected_signal.get() == TrafficLight::GREEN &&
use_last_detect_color_;

if (!is_green && !was_green) {
continue;
Expand Down Expand Up @@ -285,11 +315,11 @@ uint8_t CrosswalkTrafficLightEstimatorNode::estimateCrosswalkTrafficSignal(
: TrafficLight::UNKNOWN;
}

uint8_t CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal(
boost::optional<uint8_t> CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal(
const lanelet::ConstLineStringsOrPolygons3d & traffic_lights,
const std::unordered_map<lanelet::Id, TrafficSignal> & traffic_light_id_map) const
const TrafficLightIdMap & traffic_light_id_map) const
{
uint8_t ret = TrafficLight::UNKNOWN;
boost::optional<uint8_t> ret{boost::none};

double highest_confidence = 0.0;
for (const auto & traffic_light : traffic_lights) {
Expand Down Expand Up @@ -319,16 +349,6 @@ uint8_t CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal(

return ret;
}

uint8_t CrosswalkTrafficLightEstimatorNode::getLastDetectedTrafficSignal(
const lanelet::Id & id) const
{
if (last_detect_color_.count(id) == 0) {
return TrafficLight::UNKNOWN;
}

return last_detect_color_.at(id);
}
} // namespace traffic_light

#include <rclcpp_components/register_node_macro.hpp>
Expand Down

0 comments on commit 4b3b569

Please sign in to comment.