Skip to content
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.

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

feat: update rviz2 overlay #6883

Merged
merged 16 commits into from
May 10, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ class TurnSignalsDisplay

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

int current_turn_signal_; // Internal variable to store turn signal state
int current_hazard_lights_; // Internal variable to store hazard lights state
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -81,18 +81,19 @@ void GearDisplay::drawGearIndicator(QPainter & painter, const QRectF & backgroun
break;
}

QFont gearFont("Quicksand", 16, QFont::Bold);
QFont gearFont("Quicksand", 12, QFont::Bold);
painter.setFont(gearFont);
QPen borderPen(gray);
borderPen.setWidth(4);
borderPen.setWidth(1);
painter.setPen(borderPen);

int gearBoxSize = 30;
int gearX = backgroundRect.left() + 30 + gearBoxSize;
int gearY = backgroundRect.height() - gearBoxSize - 20;
double gearBoxSize = 37.5;
double gearX = backgroundRect.left() + 54;
double gearY = backgroundRect.height() / 2 - gearBoxSize / 2;
QRect gearRect(gearX, gearY, gearBoxSize, gearBoxSize);
painter.setBrush(QColor(0, 0, 0, 0));
painter.setBrush(gray);
painter.drawRoundedRect(gearRect, 10, 10);
painter.setPen(Qt::black);
painter.drawText(gearRect, Qt::AlignCenter, QString::fromStdString(gearString));
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,11 +39,11 @@ namespace autoware_overlay_rviz_plugin
SignalDisplay::SignalDisplay()
{
property_width_ = new rviz_common::properties::IntProperty(
"Width", 517, "Width of the overlay", this, SLOT(updateOverlaySize()));
"Width", 550, "Width of the overlay", this, SLOT(updateOverlaySize()));
property_height_ = new rviz_common::properties::IntProperty(
"Height", 175, "Height of the overlay", this, SLOT(updateOverlaySize()));
"Height", 100, "Height of the overlay", this, SLOT(updateOverlaySize()));
property_left_ = new rviz_common::properties::IntProperty(
"Left", 10, "Left position of the overlay", this, SLOT(updateOverlayPosition()));
"Left", 0, "Left position of the overlay", this, SLOT(updateOverlayPosition()));
property_top_ = new rviz_common::properties::IntProperty(
"Top", 10, "Top position of the overlay", this, SLOT(updateOverlayPosition()));
property_signal_color_ = new rviz_common::properties::ColorProperty(
Expand Down Expand Up @@ -325,35 +325,31 @@ void SignalDisplay::drawWidget(QImage & hud)
QPainter painter(&hud);
painter.setRenderHint(QPainter::Antialiasing, true);

QRectF backgroundRect(0, 0, 322, hud.height());
QRectF backgroundRect(0, 0, 550, hud.height());
drawHorizontalRoundedRectangle(painter, backgroundRect);

// Draw components
if (steering_wheel_display_) {
steering_wheel_display_->drawSteeringWheel(painter, backgroundRect);
}
if (gear_display_) {
gear_display_->drawGearIndicator(painter, backgroundRect);
}

if (steering_wheel_display_) {
steering_wheel_display_->drawSteeringWheel(painter, backgroundRect);
}

if (speed_display_) {
speed_display_->drawSpeedDisplay(painter, backgroundRect);
}
if (turn_signals_display_) {
turn_signals_display_->drawArrows(painter, backgroundRect, property_signal_color_->getColor());
}

// a 27px space between the two halves of the HUD

QRectF smallerBackgroundRect(340, 0, 190.0 / 2, hud.height());

drawVerticalRoundedRectangle(painter, smallerBackgroundRect);

if (traffic_display_) {
traffic_display_->drawTrafficLightIndicator(painter, smallerBackgroundRect);
traffic_display_->drawTrafficLightIndicator(painter, backgroundRect);
}

if (speed_limit_display_) {
speed_limit_display_->drawSpeedLimitIndicator(painter, smallerBackgroundRect);
speed_limit_display_->drawSpeedLimitIndicator(painter, backgroundRect);
}

painter.end();
Expand All @@ -364,8 +360,8 @@ void SignalDisplay::drawHorizontalRoundedRectangle(
{
painter.setRenderHint(QPainter::Antialiasing, true);
QColor colorFromHSV;
colorFromHSV.setHsv(0, 0, 0); // Hue, Saturation, Value
colorFromHSV.setAlphaF(0.65); // Transparency
colorFromHSV.setHsv(0, 0, 29); // Hue, Saturation, Value
colorFromHSV.setAlphaF(0.60); // Transparency

painter.setBrush(colorFromHSV);

Expand Down Expand Up @@ -404,7 +400,9 @@ void SignalDisplay::updateOverlaySize()
void SignalDisplay::updateOverlayPosition()
{
std::lock_guard<std::mutex> lock(mutex_);
overlay_->setPosition(property_left_->getInt(), property_top_->getInt());
overlay_->setPosition(
property_left_->getInt(), property_top_->getInt(), HorizontalAlignment::CENTER,
VerticalAlignment::TOP);
queueRender();
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@
backgroundRect.width() / 2 - referenceRect.width() / 2 - 5, backgroundRect.height() / 2);

QString speedNumber = QString::number(current_speed_, 'f', 0);
int fontSize = 60;
int fontSize = 40;
QFont speedFont("Quicksand", fontSize);
painter.setFont(speedFont);

Expand All @@ -94,16 +94,17 @@

// Center the speed number in the backgroundRect
QPointF speedPos(
backgroundRect.center().x() - speedNumberRect.width() / 2, backgroundRect.center().y());
backgroundRect.center().x() - speedNumberRect.width() / 2,
backgroundRect.center().y() + speedNumberRect.bottom());
painter.setPen(gray);
painter.drawText(speedPos, speedNumber);

QFont unitFont("Quicksand", 14);
QFont unitFont("Quicksand", 8, QFont::DemiBold);

Check warning on line 102 in common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp

View workflow job for this annotation

GitHub Actions / spell-check-partial

Unknown word (Demi)
painter.setFont(unitFont);
QString speedUnit = "km/h";
QRect unitRect = painter.fontMetrics().boundingRect(speedUnit);
QPointF unitPos(
(backgroundRect.width() / 2 - unitRect.width() / 2), referencePos.y() + unitRect.height());
(backgroundRect.width() / 2 - unitRect.width() / 2), referencePos.y() + unitRect.height() + 15);
painter.drawText(unitPos, speedUnit);
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <OgreTechnique.h>
#include <OgreTexture.h>
#include <OgreTextureManager.h>
#include <qobject.h>

#include <algorithm>
#include <cmath>
Expand Down Expand Up @@ -99,10 +100,9 @@ void SpeedLimitDisplay::drawSpeedLimitIndicator(QPainter & painter, const QRectF
}

// Define the area for the outer circle
QRectF outerCircleRect = backgroundRect;
outerCircleRect.setWidth(backgroundRect.width() - 30);
outerCircleRect.setHeight(backgroundRect.width() - 30);
outerCircleRect.moveTopLeft(QPointF(backgroundRect.left() + 15, backgroundRect.top() + 10));
QRectF outerCircleRect = QRectF(45, 45, 45, 45);
outerCircleRect.moveTopRight(
QPointF(backgroundRect.right() - 44, backgroundRect.top() + outerCircleRect.height() / 2 + 5));

// Now use borderColor for drawing
painter.setPen(QPen(borderColor, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
Expand All @@ -112,24 +112,30 @@ void SpeedLimitDisplay::drawSpeedLimitIndicator(QPainter & painter, const QRectF

// Define the area for the inner circle
QRectF innerCircleRect = outerCircleRect;
innerCircleRect.setWidth(outerCircleRect.width() / 1.25);
innerCircleRect.setHeight(outerCircleRect.height() / 1.25);
innerCircleRect.setWidth(outerCircleRect.width() / 1.09);
innerCircleRect.setHeight(outerCircleRect.height() / 1.09);
innerCircleRect.moveCenter(outerCircleRect.center());

QRectF innerCircleRect2 = innerCircleRect;

painter.setRenderHint(QPainter::Antialiasing, true);
QColor colorFromHSV;
colorFromHSV.setHsv(0, 0, 0); // Hue, Saturation, Value

colorFromHSV.setHsv(0, 0, 29); // Hue, Saturation, Value
colorFromHSV.setAlphaF(0.60); // Transparency
painter.setBrush(colorFromHSV);
painter.drawEllipse(innerCircleRect);

// Add a second inner circle as a mask to make the speed limit indicator look like a ring
// and follow the rest of the background color as close as possible
painter.drawEllipse(innerCircleRect2);

int current_limit_int = std::round(current_limit * 3.6);

// Define the text to be drawn
QString text = QString::number(current_limit_int);

// Set the font and color for the text
QFont font = QFont("Quicksand", 24, QFont::Bold);
QFont font = QFont("Quicksand", 16, QFont::Bold);

painter.setFont(font);
// #C2C2C2
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -82,8 +82,8 @@ void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF &
// Rotate the wheel
float steeringAngle = steering_angle_; // No need to round here
// Calculate the position
int wheelCenterX = backgroundRect.right() - wheel.width() - 17.5;
int wheelCenterY = backgroundRect.height() - wheel.height() + 15;
int wheelCenterX = backgroundRect.left() + wheel.width() + 54 + 54;
int wheelCenterY = backgroundRect.height() / 2;

// Rotate the wheel image
QTransform rotationTransform;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -62,12 +62,10 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF
painter.setBrush(QBrush(tl_gray_, Qt::SolidPattern));
painter.setPen(Qt::NoPen);
// Define the area for the circle (background)
QRectF circleRect = backgroundRect;
circleRect.setWidth(backgroundRect.width() - 30);
circleRect.setHeight(backgroundRect.width() - 30);
QRectF circleRect = QRectF(50, 50, 50, 50);
circleRect.moveTopRight(QPointF(
backgroundRect.left() + circleRect.width() + 15,
backgroundRect.top() + circleRect.height() + 30));
backgroundRect.right() - circleRect.width() - 75,
backgroundRect.top() + circleRect.height() / 2));
painter.drawEllipse(circleRect);

if (!current_traffic_.elements.empty()) {
Expand Down Expand Up @@ -96,7 +94,7 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF
}

// Scaling factor (e.g., 1.5 for 150% size)
float scaleFactor = 1.00;
float scaleFactor = 0.75;

// Calculate the scaled size
QSize scaledSize = traffic_light_image_.size() * scaleFactor;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -72,12 +72,12 @@ void TurnSignalsDisplay::updateHazardLightsData(
void TurnSignalsDisplay::drawArrows(
QPainter & painter, const QRectF & backgroundRect, const QColor & color)
{
QImage scaledLeftArrow = arrowImage.scaled(64, 43, Qt::KeepAspectRatio, Qt::SmoothTransformation);
QImage scaledLeftArrow = arrowImage.scaled(50, 32, Qt::KeepAspectRatio, Qt::SmoothTransformation);
scaledLeftArrow = coloredImage(scaledLeftArrow, gray);
QImage scaledRightArrow = scaledLeftArrow.mirrored(true, false);
int arrowYPos = (backgroundRect.height() / 3 - scaledLeftArrow.height() / 2);
int leftArrowXPos = backgroundRect.width() / 4 - scaledLeftArrow.width(); // Adjust as needed
int rightArrowXPos = backgroundRect.width() * 3 / 4; // Adjust as needed
int arrowYPos = (backgroundRect.height() / 2 - scaledLeftArrow.height() / 2 - 4);
int leftArrowXPos = backgroundRect.left() + scaledLeftArrow.width() * 2 + 180;
int rightArrowXPos = backgroundRect.right() - scaledRightArrow.width() * 3 - 175;

bool leftActive =
(current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT ||
Expand Down
Loading