Skip to content

feat(autoware_overlay_rviz_plugin): add Last-in priority for turn signal and hazard light #23

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

feat(autoware_overlay_rviz_plugin): add Last-in priority for turn signal and hazard light #23

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5238863
feat: add Last-in priority for turn signal and hazard light
shtokuda Sep 2, 2025
baf3536
style(pre-commit): autofix
pre-commit-ci[bot] Sep 2, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@ private Q_SLOTS:
void updateOverlayPosition();
void updateOverlayColor();
void updateTurnSignalBlinkingMode();
void updateTurnSignalPriority();
void topic_updated_gear();
void topic_updated_steering();
void topic_updated_speed();
Expand All @@ -86,6 +87,7 @@ private Q_SLOTS:
rviz_common::properties::ColorProperty * property_dark_limit_color_;

rviz_common::properties::EnumProperty * property_turn_signal_blinking_mode_;
rviz_common::properties::EnumProperty * property_turn_signal_priority_;

std::unique_ptr<rviz_common::properties::RosTopicProperty> steering_topic_property_;
std::unique_ptr<rviz_common::properties::RosTopicProperty> gear_topic_property_;
Expand Down
4 changes: 4 additions & 0 deletions autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,15 +46,19 @@ class TurnSignalsDisplay
void updateHazardLightsData(
const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg);
void setBlinkingMode(std::string_view mode);
void setPriority(std::string_view mode);

private:
QImage arrowImage;
QColor gray = QColor(79, 79, 79);

std::string blinking_mode_ = "Static";
std::string priority_ = "Hazard";

int current_turn_signal_; // Internal variable to store turn signal state
int current_hazard_lights_; // Internal variable to store hazard lights state
double turn_signal_time_; // Internal variable to store time when hazard lights on
double hazard_lights_time_; // Internal variable to store time when hazard lights on
QImage coloredImage(const QImage & source, const QColor & color);
};

Expand Down
14 changes: 14 additions & 0 deletions autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,11 @@ SignalDisplay::SignalDisplay()
SLOT(updateTurnSignalBlinkingMode()));
property_turn_signal_blinking_mode_->addOption("Static", 0);
property_turn_signal_blinking_mode_->addOption("Blinking", 1);
property_turn_signal_priority_ = new rviz_common::properties::EnumProperty(
"Signal Priority Mode", "Hazard", "Priority of the signal light", this,
SLOT(updateTurnSignalPriority()));
property_turn_signal_priority_->addOption("Hazard", 0);
property_turn_signal_priority_->addOption("Last-in", 1);

// Initialize the component displays
steering_wheel_display_ = std::make_unique<SteeringWheelDisplay>();
Expand Down Expand Up @@ -407,6 +412,15 @@ void SignalDisplay::updateTurnSignalBlinkingMode()
queueRender();
}

void SignalDisplay::updateTurnSignalPriority()
{
std::lock_guard<std::mutex> lock(mutex_);
if (turn_signals_display_) {
turn_signals_display_->setPriority(property_turn_signal_priority_->getStdString());
}
queueRender();
}

void SignalDisplay::topic_updated_gear()
{
// resubscribe to the topic
Expand Down
54 changes: 44 additions & 10 deletions autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,8 @@
namespace autoware_overlay_rviz_plugin
{

TurnSignalsDisplay::TurnSignalsDisplay() : current_turn_signal_(0)
TurnSignalsDisplay::TurnSignalsDisplay()
: current_turn_signal_(0), current_hazard_lights_(0), turn_signal_time_(0), hazard_lights_time_(0)
{
// Load the arrow image
std::string package_path =
Expand All @@ -50,7 +51,16 @@ void TurnSignalsDisplay::updateTurnSignalsData(
{
try {
// Assuming msg->report is the field you're interested in
current_turn_signal_ = msg->report;
int turn_signal = msg->report;
if (current_turn_signal_ != turn_signal) {
if (
turn_signal == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT ||
turn_signal == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT) {
auto now = std::chrono::steady_clock::now().time_since_epoch();
turn_signal_time_ = std::chrono::duration_cast<std::chrono::milliseconds>(now).count();
}
}
current_turn_signal_ = turn_signal;
} catch (const std::exception & e) {
// Log the error
std::cerr << "Error in processMessage: " << e.what() << std::endl;
Expand All @@ -62,7 +72,14 @@ void TurnSignalsDisplay::updateHazardLightsData(
{
try {
// Assuming msg->report is the field you're interested in
current_hazard_lights_ = msg->report;
int hazard_lights = msg->report;
if (current_hazard_lights_ != hazard_lights) {
if (hazard_lights == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE) {
auto now = std::chrono::steady_clock::now().time_since_epoch();
hazard_lights_time_ = std::chrono::duration_cast<std::chrono::milliseconds>(now).count();
}
}
current_hazard_lights_ = hazard_lights;
} catch (const std::exception & e) {
// Log the error
std::cerr << "Error in processMessage: " << e.what() << std::endl;
Expand All @@ -73,6 +90,10 @@ void TurnSignalsDisplay::setBlinkingMode(std::string_view mode)
{
blinking_mode_ = mode;
}
void TurnSignalsDisplay::setPriority(std::string_view mode)
{
priority_ = mode;
}

void TurnSignalsDisplay::drawArrows(
QPainter & painter, const QRectF & backgroundRect, const QColor & color)
Expand All @@ -84,13 +105,26 @@ void TurnSignalsDisplay::drawArrows(
int leftArrowXPos = backgroundRect.left() + scaledLeftArrow.width() * 2 + 180;
int rightArrowXPos = backgroundRect.right() - scaledRightArrow.width() * 3 - 175;

bool leftActive =
(current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT ||
current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE);
bool rightActive =
(current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT ||
current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE);

bool leftActive, rightActive;
if (priority_ == "Last-in") {
leftActive =
current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT ||
(current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE &&
(current_turn_signal_ != autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT ||
hazard_lights_time_ > turn_signal_time_));
rightActive =
current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT ||
(current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE &&
(current_turn_signal_ != autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT ||
hazard_lights_time_ > turn_signal_time_));
} else {
leftActive =
current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT ||
current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE;
rightActive =
current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT ||
current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE;
}
auto now = std::chrono::steady_clock::now().time_since_epoch();
auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now).count();

Expand Down
Loading