diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png index 07b1e33132e43..04ea298a46829 100644 Binary files a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png and b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png differ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp index 0831ded468c99..5f5c9cee2da43 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp @@ -76,6 +76,7 @@ private Q_SLOTS: rviz_common::properties::IntProperty * property_left_; rviz_common::properties::IntProperty * property_top_; rviz_common::properties::ColorProperty * property_signal_color_; + rviz_common::properties::FloatProperty * property_handle_angle_scale_; std::unique_ptr steering_topic_property_; std::unique_ptr gear_topic_property_; std::unique_ptr speed_topic_property_; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp index dada3cddab911..0e42368f2e017 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp @@ -36,7 +36,8 @@ class SteeringWheelDisplay { public: SteeringWheelDisplay(); - void drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect); + void drawSteeringWheel( + QPainter & painter, const QRectF & backgroundRect, float handle_angle_scale_); void updateSteeringData(const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); private: @@ -46,6 +47,7 @@ class SteeringWheelDisplay QImage wheelImage; QImage scaledWheelImage; QImage coloredImage(const QImage & source, const QColor & color); + autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr last_msg_ptr_; }; } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp index ccfdaa69f5d2a..1fd33a95fb34d 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp @@ -49,6 +49,9 @@ SignalDisplay::SignalDisplay() property_signal_color_ = new rviz_common::properties::ColorProperty( "Signal Color", QColor(QString("#00E678")), "Color of the signal arrows", this, SLOT(updateOverlayColor())); + property_handle_angle_scale_ = new rviz_common::properties::FloatProperty( + "Handle Angle Scale", 17.0, "Scale of the steering wheel handle angle", this, + SLOT(updateOverlaySize())); // Initialize the component displays steering_wheel_display_ = std::make_unique(); @@ -285,7 +288,8 @@ void SignalDisplay::drawWidget(QImage & hud) } if (steering_wheel_display_) { - steering_wheel_display_->drawSteeringWheel(painter, backgroundRect); + steering_wheel_display_->drawSteeringWheel( + painter, backgroundRect, property_handle_angle_scale_->getFloat()); } if (speed_display_) { diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp index d2390b16e5373..28bd2b5c2a52e 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp @@ -59,19 +59,17 @@ void SteeringWheelDisplay::updateSteeringData( const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) { try { - // Assuming msg->steering_angle is the field you're interested in - float steeringAngle = msg->steering_tire_angle; - // we received it as a radian value, but we want to display it in degrees - steering_angle_ = - (steeringAngle * -180 / M_PI) * - 17; // 17 is the ratio between the steering wheel and the steering tire angle i assume + last_msg_ptr_ = msg; + + steering_angle_ = msg->steering_tire_angle; } catch (const std::exception & e) { // Log the error std::cerr << "Error in processMessage: " << e.what() << std::endl; } } -void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect) +void SteeringWheelDisplay::drawSteeringWheel( + QPainter & painter, const QRectF & backgroundRect, float handle_angle_scale_) { // Enable Antialiasing for smoother drawing painter.setRenderHint(QPainter::Antialiasing, true); @@ -80,7 +78,7 @@ void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF & QImage wheel = coloredImage(scaledWheelImage, gray); // Rotate the wheel - float steeringAngle = steering_angle_; // No need to round here + float steeringAngle = std::round(handle_angle_scale_ * (steering_angle_ / M_PI) * -180.0); // Calculate the position int wheelCenterX = backgroundRect.left() + wheel.width() + 54 + 54; int wheelCenterY = backgroundRect.height() / 2; @@ -99,16 +97,40 @@ void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF & // Draw the rotated image painter.drawImage(drawPoint.x(), drawPoint.y(), rotatedWheel); - QString steeringAngleStringAfterModulo = QString::number(fmod(steeringAngle, 360), 'f', 0); + std::ostringstream steering_angle_ss; + if (last_msg_ptr_) { + steering_angle_ss << std::fixed << std::setprecision(1) << steering_angle_ * 180.0 / M_PI + << "°"; + } else { + steering_angle_ss << "N/A"; + } + + QString steeringAngleString = QString::fromStdString(steering_angle_ss.str()); + // if the string doesn't have a negative sign, add a space to the front of the string + // to align the text in both cases (negative and positive) + if (steeringAngleString[0] != '-') { + steeringAngleString = " " + steeringAngleString; + } // Draw the steering angle text - QFont steeringFont("Quicksand", 9, QFont::Bold); + QFont steeringFont("Quicksand", 8, QFont::Bold); painter.setFont(steeringFont); painter.setPen(QColor(0, 0, 0, 255)); - QRect steeringRect( - wheelCenterX - wheelImage.width() / 2, wheelCenterY - wheelImage.height() / 2, - wheelImage.width(), wheelImage.height()); - painter.drawText(steeringRect, Qt::AlignCenter, steeringAngleStringAfterModulo + "°"); + // QRect steeringRect( + // wheelCenterX - wheelImage.width() / 2 + 1, wheelCenterY - wheelImage.height() / 2, + // wheelImage.width(), wheelImage.height()); + // painter.drawText(steeringRect, Qt::AlignCenter, steeringAngleString); + + // Measure the text + QFontMetrics fontMetrics(steeringFont); + QRect textRect = fontMetrics.boundingRect(steeringAngleString); + + // Center the text + int textX = wheelCenterX - textRect.width() / 2; + int textY = wheelCenterY - textRect.height() / 2; + + QRect steeringRect(textX, textY, textRect.width(), textRect.height()); + painter.drawText(steeringRect, Qt::AlignCenter, steeringAngleString); } QImage SteeringWheelDisplay::coloredImage(const QImage & source, const QColor & color)