feat: added color interpolation for speed limiter's donut according to current speed

Signed-off-by: KhalilSelyan <khalil@leodrive.ai>

Author: KhalilSelyan

common/autoware_overlay_rviz_plugin/autoware_vehicle_overlay_rviz_plugin/include/speed_limit_display.hpp

@@ -23,6 +23,7 @@
 #include <rviz_common/properties/int_property.hpp>
 #include <rviz_common/ros_topic_display.hpp>
 
+#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
 
 #include <OgreColourValue.h>
@@ -38,9 +39,11 @@ class SpeedLimitDisplay
   SpeedLimitDisplay();
   void drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect);
   void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg);
+  void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg);
 
 private:
-  float current_limit;  // Internal variable to store current gear
+  float current_limit;   // Internal variable to store current gear
+  float current_speed_;  // Internal variable to store current speed
   QColor gray = QColor(194, 194, 194);
 };
 

common/autoware_overlay_rviz_plugin/autoware_vehicle_overlay_rviz_plugin/src/speed_limit_display.cpp

@@ -34,7 +34,7 @@
 namespace autoware_vehicle_overlay_rviz_plugin
 {
 
-SpeedLimitDisplay::SpeedLimitDisplay() : current_limit(0.0)
+SpeedLimitDisplay::SpeedLimitDisplay() : current_limit(0.0), current_speed_(0.0)
 {
   std::string package_path =
     ament_index_cpp::get_package_share_directory("autoware_vehicle_overlay_rviz_plugin");
@@ -55,31 +55,75 @@ void SpeedLimitDisplay::updateSpeedLimitData(
   current_limit = msg->max_velocity;
 }
 
+void SpeedLimitDisplay::updateSpeedData(
+  const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg)
+{
+  try {
+    float speed = msg->longitudinal_velocity;
+    current_speed_ = speed;
+  } catch (const std::exception & e) {
+    // Log the error
+    std::cerr << "Error in processMessage: " << e.what() << std::endl;
+  }
+}
+
 void SpeedLimitDisplay::drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect)
 {
   // Enable Antialiasing for smoother drawing
   painter.setRenderHint(QPainter::Antialiasing, true);
   painter.setRenderHint(QPainter::SmoothPixmapTransform, true);
 
-  // #C2C2C2
-  painter.setPen(QPen(gray, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
-  painter.setBrush(QBrush(gray, Qt::SolidPattern));
+  // Convert color_min and color_max to QColor
+  QColor colorMin = QColor("#FF9999");
+  QColor colorMax = QColor("#FF3333");
+
+  // Calculate the ratio between the current speed and the speed limit
+  double speed_to_limit_ratio = current_speed_ / current_limit;
+
+  QColor borderColor = gray;
+
+  if (speed_to_limit_ratio > 0.7) {
+    // Adjust the interpolation to start more gradually
+    // Calculate a new factor that slows down the transition
+    double adjusted_speed_to_limit_ratio = (speed_to_limit_ratio - 0.7) / (1.0 - 0.7);
+    double smoothFactor =
+      pow(adjusted_speed_to_limit_ratio, 2);  // Use a power to make it interpolate slower
+
+    int r = std::min(
+      std::max(
+        0, colorMin.red() + static_cast<int>((colorMax.red() - colorMin.red()) * smoothFactor)),
+      255);
+    int g = std::min(
+      std::max(
+        0,
+        colorMin.green() + static_cast<int>((colorMax.green() - colorMin.green()) * smoothFactor)),
+      255);
+    int b = std::min(
+      std::max(
+        0, colorMin.blue() + static_cast<int>((colorMax.blue() - colorMin.blue()) * smoothFactor)),
+      255);
+
+    borderColor = QColor(r, g, b);
+  }
 
   // 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));
 
+  // Now use borderColor for drawing
+  painter.setPen(QPen(borderColor, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
+  painter.setBrush(borderColor);
+  // Draw the outer circle for the speed limit indicator
+  painter.drawEllipse(outerCircleRect);
+
   // Define the area for the inner circle
   QRectF innerCircleRect = outerCircleRect;
   innerCircleRect.setWidth(outerCircleRect.width() / 1.25);
   innerCircleRect.setHeight(outerCircleRect.height() / 1.25);
   innerCircleRect.moveCenter(outerCircleRect.center());
 
-  // Draw the outer circle
-  painter.drawEllipse(outerCircleRect);
-
   painter.setRenderHint(QPainter::Antialiasing, true);
   QColor colorFromHSV;
   colorFromHSV.setHsv(0, 0, 0);  // Hue, Saturation, Value