Merge pull request autowarefoundation#220 from atanasko/main

Add GT visualization and fix decoding of steering angle from canbus

Author: m-zain-khawaja

VisionPilot/Production_Releases/0.5/images/wheel_white.png

@@ -8,13 +8,22 @@
 #include <cmath>
 #include <limits>
 #include <chrono>
+#include <memory>
 
 namespace autoware_pov::drivers {
 
 struct CanVehicleState {
     double speed_kmph = std::numeric_limits<double>::quiet_NaN();           // Speed, CAN ID 0xA1
     double steering_angle_deg = std::numeric_limits<double>::quiet_NaN();   // Steering, CAN ID 0xA4
     bool is_valid = false;
+    bool is_steering_angle = false;
+
+    void clear()
+    {
+      speed_kmph = std::numeric_limits<double>::quiet_NaN();
+      is_steering_angle = false;
+      steering_angle_deg = std::numeric_limits<double>::quiet_NaN();
+    }
 };
 
 /**
@@ -31,7 +40,7 @@ class CanInterface {
      * @param interface_name "can0", "vcan0", or path to .asc file (e.g. "./assets/test.asc")
      */
     explicit CanInterface(const std::string& interface_name);
-    
+
     ~CanInterface();
 
     /**
@@ -74,7 +83,7 @@ class CanInterface {
     static constexpr int ID_STEERING = 0xA4; // Or A4, 164
 
     void parseFrame(
-        int can_id, 
+        int can_id,
         const std::vector<uint8_t>& data
     );
 
@@ -84,6 +93,9 @@ class CanInterface {
 
 };
 
+// extern declaration
+extern std::unique_ptr<CanInterface> can_interface;
+
 } // namespace autoware_pov::drivers
 
 #endif // AUTOWARE_POV_DRIVERS_CAN_INTERFACE_HPP_

VisionPilot/Production_Releases/0.5/include/drivers/can_interface.hpp

@@ -4,6 +4,7 @@
 #include "../inference/lane_segmentation.hpp"
 #include "../lane_tracking/lane_tracking.hpp"
 #include <opencv2/opencv.hpp>
+#include <optional>
 
 namespace autoware_pov::vision::egolanes
 {
@@ -88,7 +89,13 @@ void drawMetricVerification(
 );
 
 cv::Mat rotateSteeringWheel(const cv::Mat& img, float steering_angle_deg);
-void visualizeSteering(cv::Mat& img, float steering_angle, const cv::Mat& steeringWheelImage);
+void visualizeWheel(const cv::Mat& img, const cv::Mat& wheelImg, const int x, const int y);
+void visualizeSteering(
+  cv::Mat& img,
+  float steering_angle,
+  const cv::Mat& rotatedPredSteeringWheelImg,
+  std::optional<float> gtSteeringAngle,
+  const cv::Mat& rotatedGtSteeringWheelImg);
 
 }  // namespace autoware_pov::vision::egolanes
 

VisionPilot/Production_Releases/0.5/include/visualization/draw_lanes.hpp

@@ -556,8 +556,11 @@ void displayThread(
          std::cerr << "Error: could not open " << csv_log_path << " for writing" << std::endl;
     }
 
-  std::string image_path = "images/wheel_green.png";
-  cv::Mat steeringWheelImg = cv::imread(image_path, cv::IMREAD_UNCHANGED);
+    // Load steering wheel images
+    std::string predSteeringImagePath = "images/wheel_green.png";
+    cv::Mat predSteeringWheelImg = cv::imread(predSteeringImagePath, cv::IMREAD_UNCHANGED);
+    std::string gtSteeringImagePath = "images/wheel_white.png";
+    cv::Mat gtSteeringWheelImg = cv::imread(gtSteeringImagePath, cv::IMREAD_UNCHANGED);
 
     while (running.load()) {
         InferenceResult result;
@@ -588,11 +591,25 @@ void displayThread(
             //      cv::Scalar(0,0,0)
             //  );
 
+            // Display wheels over frame image
             float steering_angle = result.steering_angle;
-            cv::Mat rotatedSteeringWheelImg = rotateSteeringWheel(steeringWheelImg, steering_angle);
-            visualizeSteering(view_debug, steering_angle, rotatedSteeringWheelImg);
+            cv::Mat rotatedPredSteeringWheelImg = rotateSteeringWheel(predSteeringWheelImg, steering_angle);
+
+            // Read GT steering from CAN frame
+            std::optional<float> gtSteeringAngle;
+            cv::Mat rotatedGtSteeringWheelImg;
+            if (can_interface) {
+              // float gtSteeringSpeed = can_interface->getState().speed_kmph;
+              if (can_interface->getState().is_valid && can_interface->getState().is_steering_angle) {
+                gtSteeringAngle = can_interface->getState().steering_angle_deg;
+                if (gtSteeringAngle.has_value()) {
+                  rotatedGtSteeringWheelImg = rotateSteeringWheel(gtSteeringWheelImg, gtSteeringAngle.value());
+                }
+              }
+            }
 
-            // std::cout<< "************ " << result.steering_angle << std::endl;
+            // rotatedGtSteeringWheelImg = rotateSteeringWheel(gtSteeringWheelImg, gtSteeringAngle.value());
+            visualizeSteering(view_debug, steering_angle, rotatedPredSteeringWheelImg, gtSteeringAngle, rotatedGtSteeringWheelImg);
 
              // 2. Draw 3 views
             drawRawMasksInPlace(
@@ -1151,7 +1168,7 @@ int main(int argc, char** argv)
     }
 
     // Initialize CAN Interface (optional - ground truth)
-    std::unique_ptr<CanInterface> can_interface;
+    // std::unique_ptr<CanInterface> can_interface;
     if (!can_interface_name.empty()) {
         try {
             can_interface = std::make_unique<CanInterface>(can_interface_name);

VisionPilot/Production_Releases/0.5/main.cpp

@@ -56,6 +56,8 @@ CanInterface::~CanInterface() {
 
 // Main update loop
 bool CanInterface::update() {
+    // clear because data get from state!
+    current_state_.clear();
 
     if (is_file_mode_) {
         return readFileLine();
@@ -89,6 +91,7 @@ void CanInterface::parseFrame(
         double val = decodeSteering(data);
         current_state_.steering_angle_deg = val;
         current_state_.is_valid = true;
+        current_state_.is_steering_angle = true;
     }
 }
 
@@ -109,31 +112,34 @@ double CanInterface::decodeSpeed(const std::vector<uint8_t>& data) {
     return static_cast<double>(raw) * 0.01;
 }
 
-// SSA (Steering angle) : Start Bit 46 | Length 15 | Signed | Factor 0.1
-// Format: Motorola (Big Endian: https://en.wikipedia.org/wiki/Endianness)
-// Bit 46 is in byte 5 (bit 6).
-// This is complex. We verify strictly against provided logic if possible.
-// Lacking exact bit-matrix, we assume standard Big Endian alignment crossing byte 5 and 6.
+// Steering angle
+// SSA (Measured steering angle)    : 46|15@0- (0.1,0) [0|0] "-"
+// SSAZ (Steering zero point)       : 29|15@0- (0.1,0) [0|0] "-"
 double CanInterface::decodeSteering(const std::vector<uint8_t>& data) {
-    
-    if (data.size() < 8) return 0.0;
 
-    // Masking logic based on 15-bit signed integer
-    // Assuming MSB is in byte 5, LSB in byte 6
-    uint16_t raw_high = data[5];
-    uint16_t raw_low  = data[6];
-    
-    // Combine
-    uint16_t raw = (raw_high << 8) | raw_low;
-    
-    // If it's 15 bits, we might need to shift or mask. 
-    // Assuming the 15 bits are right-aligned in the extraction:
-    // (This is a best-guess implementation until A4 is actually observed in logs)
-    
-    // Convert to signed 16-bit to handle negative values (Two's complement)
-    int16_t signed_val = static_cast<int16_t>(raw);
-    
-    return static_cast<double>(signed_val) * 0.1;
+    // Sanity: need at least 8 bytes
+    if (data.size() < 8) return std::numeric_limits<double>::quiet_NaN();
+
+    // SSAZ
+    uint16_t b6 = (data[5] & 0xC0) >> 7;  // Bit 7 from byte 6  BigEndian
+    uint16_t b5 = data[4];                // Byte 5
+    bool ssazSign = (data[3] & 0x10) != 0;       // bit 4 → sign
+    int16_t b4 = data[3] & 0x0F;     // bits 3..0 → magnitude
+
+    uint16_t ssaz = (b4 << 10) | (b5 << 2) | b6;
+    int16_t ssazSigned = ssazSign ? -ssaz : ssaz;
+
+    // SSA
+    uint16_t ssa_b7 = data[6];
+    bool ssaSign = (data[5] & 0x40) != 0;   // bit 6 = sign
+    uint16_t ssa_b6 = data[5] & 0x3F;       // bits 5..0 = magnitude
+
+    int16_t ssa =  (ssa_b6 << 7) | ssa_b7;
+    int16_t ssaSigned = ssaSign ? -ssa : ssa;
+
+    double steering_angle = ssaSigned - ssazSigned;
+
+    return steering_angle;
 }
 
 // ============================== REAL-TIME INFERENCE (SocketCAN) ============================== //
@@ -318,4 +324,7 @@ bool CanInterface::readFileLine() {
     return false; // End of file
 }
 
+// definition of global pointer
+std::unique_ptr<CanInterface> can_interface=nullptr;
+
 } // namespace autoware_pov::drivers

VisionPilot/Production_Releases/0.5/src/drivers/can_interface.cpp

@@ -872,24 +872,21 @@ cv::Mat rotateSteeringWheel(const cv::Mat& img, float steering_angle_deg)
   return rotated;
 }
 
-void visualizeSteering(cv::Mat& img, const float steering_angle, const cv::Mat& overlay)
+void visualizeWheel(const cv::Mat& img, const cv::Mat& wheelImg, const int x, const int y)
 {
-  int x = 10;
-  int y = 10;
+  if (wheelImg.empty() || img.empty()) return;
 
-  if (overlay.empty() || img.empty()) return;
-
-  int w = overlay.cols;
-  int h = overlay.rows;
+  int w = wheelImg.cols;
+  int h = wheelImg.rows;
 
   if (x < 0 || y < 0 || x + w > img.cols || y + h > img.rows) return;
 
   cv::Mat roi = img(cv::Rect(x, y, w, h));
 
-  if (overlay.channels() == 4) { // has alpha
+  if (wheelImg.channels() == 4) { // has alpha
     cv::Mat overlay_rgb, alpha;
     std::vector<cv::Mat> channels(4);
-    cv::split(overlay, channels);
+    cv::split(wheelImg, channels);
     alpha = channels[3]; // alpha channel
     cv::merge(std::vector<cv::Mat>{channels[0], channels[1], channels[2]}, overlay_rgb);
 
@@ -908,9 +905,26 @@ void visualizeSteering(cv::Mat& img, const float steering_angle, const cv::Mat&
     // Write back to base image
     blended.convertTo(img(cv::Rect(x, y, w, h)), CV_8UC3, 255.0);
   } else {
-    overlay.copyTo(roi); // no alpha, hard paste
+    wheelImg.copyTo(roi); // no alpha, hard paste
   }
+}
+
+void visualizeSteering(
+  cv::Mat& img,
+  const float steering_angle,
+  const cv::Mat& rotatedPredSteeringWheelImg,
+  std::optional<float> gtSteeringAngle,
+  const cv::Mat& rotatedGtSteeringWheelImg)
+{
+  int w = img.cols;
+  int h = img.rows;
 
+  visualizeWheel(img, rotatedPredSteeringWheelImg, 10, 10);
+  if (!rotatedGtSteeringWheelImg.empty()) {
+    visualizeWheel(img, rotatedGtSteeringWheelImg, w - 80, 10);
+  }
+
+  // Prediction
   std::ostringstream oss;
   oss << std::fixed << std::setprecision(2) << steering_angle;
   std::string steeringAngleText = "Predicted angle: " + oss.str();
@@ -924,6 +938,24 @@ void visualizeSteering(cv::Mat& img, const float steering_angle, const cv::Mat&
     cv::Scalar(62, 202, 130),
     2
   );
+
+  if (gtSteeringAngle.has_value() && !std::isnan(gtSteeringAngle.value())) {
+    // GT
+    std::ostringstream oss1;
+    oss1 << std::fixed << std::setprecision(2) << gtSteeringAngle.value();
+    std::string gtAngleText = "GT angle: " + oss1.str();
+
+    cv::putText(
+      img,
+      gtAngleText,
+      cv::Point(w - 180, 100),
+      cv::FONT_HERSHEY_SIMPLEX,
+      0.6,
+      cv::Scalar(255, 255, 255),
+      2
+    );
+  }
+
 }
 
 }  // namespace autoware_pov::vision::egolanes