feat: add handle angle scale property to signal display (#7774)

* feat: add handle angle scale property to signal display * fix: set default steering angle to 0.0° when not received Signed-off-by: KhalilSelyan <[email protected]> * fix: set default steering angle to N/A when not received and check for msg_ptr instead of float value Signed-off-by: KhalilSelyan <[email protected]> * chore: update steering wheel font size and wheel icon center is bigger Signed-off-by: KhalilSelyan <[email protected]> * chore: update steering wheel display to center the steering angle text Signed-off-by: KhalilSelyan <[email protected]> * chore: align steering angle text in both negative and positive cases Signed-off-by: KhalilSelyan <[email protected]> --------- Signed-off-by: KhalilSelyan <[email protected]>
autowarefoundation · Jul 1, 2024 · 6029eae · 6029eae
1 parent c1dbd5b
commit 6029eae
Show file tree

Hide file tree

Showing 5 changed files with 45 additions and 16 deletions.
diff --git a/...toware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png b/...toware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png
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
@@ -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<rviz_common::properties::RosTopicProperty> steering_topic_property_;
   std::unique_ptr<rviz_common::properties::RosTopicProperty> gear_topic_property_;
   std::unique_ptr<rviz_common::properties::RosTopicProperty> speed_topic_property_;

diff --git a/...oware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp b/...oware_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
@@ -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<SteeringWheelDisplay>();
@@ -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/.../autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp b/.../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)