Clean logs

VisionPilot/Production_Releases/0.5/include/rerun/rerun_logger.hpp

@@ -51,19 +51,21 @@
      * @param lanes Lane segmentation masks
      * @param stacked_view Final visualization (BGR)
      * @param vehicle_state CAN bus data (steering, speed)
-     * @param steering_angle PID steering angle (radians)
+     * @param steering_angle_raw Raw PID output before filtering (degrees)
+     * @param steering_angle Filtered PID output (final steering, degrees)
      * @param autosteer_angle AutoSteer steering angle (degrees)
      * @param path_output PathFinder output (CTE, yaw, curvature)
      * @param inference_time_us Inference time in microseconds
      */
     void logData(
         int frame_number,
         const cv::Mat& resized_frame,
         const autoware_pov::vision::egolanes::LaneSegmentation& lanes,
         const cv::Mat& stacked_view,
         const autoware_pov::drivers::CanVehicleState& vehicle_state,
+        double steering_angle_raw,
         double steering_angle,
         float autosteer_angle,
         const autoware_pov::vision::path_planning::PathFinderOutput& path_output,
         long inference_time_us);
 

VisionPilot/Production_Releases/0.5/main.cpp

@@ -13,17 +13,17 @@
 #include "inference/tensorrt_engine.hpp"
 #include "inference/tensorrt_autosteer_engine.hpp"
 using EgoLanesEngine =
   autoware_pov::vision::egolanes::EgoLanesTensorRTEngine;
 using AutoSteerEngine =
   autoware_pov::vision::egolanes::AutoSteerTensorRTEngine;
 #else
 // Use ONNX Runtime
 #include "inference/onnxruntime_engine.hpp"
 #include "inference/autosteer_engine.hpp"
 using EgoLanesEngine =
   autoware_pov::vision::egolanes::EgoLanesOnnxEngine;
 using AutoSteerEngine =
   autoware_pov::vision::egolanes::AutoSteerOnnxEngine;
 #endif
 #include "visualization/draw_lanes.hpp"
 #include "lane_filtering/lane_filter.hpp"
@@ -53,7 +53,7 @@
  #define M_PI 3.14159265358979323846
  #endif
 
 using namespace autoware_pov::vision::egolanes;
 using namespace autoware_pov::vision::camera;
 using namespace autoware_pov::vision::path_planning;
 using namespace autoware_pov::vision::steering_control;
@@ -138,10 +138,11 @@
     int frame_number;
     steady_clock::time_point capture_time;
     steady_clock::time_point inference_time;
-    double steering_angle = 0.0;  // Steering angle from PID controller (radians)
+    double steering_angle_raw = 0.0;  // Raw PID output before filtering (degrees)
+    double steering_angle = 0.0;  // Filtered PID output (final steering, degrees)
     PathFinderOutput path_output; // Added for metric debug
     float autosteer_angle = 0.0f;  // Steering angle from AutoSteer (degrees)
     bool autosteer_valid = false;  // Whether AutoSteer ran (skips first frame)
     CanVehicleState vehicle_state; // CAN bus data from capture thread
 };
 
@@ -349,6 +350,8 @@
         // AUTOSTEER INTEGRATION
         // ========================================
         float autosteer_steering = 0.0f;
+        double steering_angle_raw = 0.0;
+        double steering_angle = 0.0;
 
         if (autosteer_engine != nullptr) {
             // 1. Copy raw EgoLanes tensor [1, 3, 80, 160] for temporal buffer
@@ -398,8 +401,7 @@
           // ========================================
           // PATHFINDER (Polynomial Fitting + Bayes Filter) + STEERING CONTROL
           // ========================================
-          double steering_angle = 0.0;  // Initialize steering angle (PID controller output)
-          PathFinderOutput path_output; // Declare at higher scope for result storage
+          PathFinderOutput path_output; // Declaring at higher scope for result storage
           path_output.fused_valid = false; // Initialize as invalid
 
           if (path_finder != nullptr && final_metrics.bev_visuals.valid) {
@@ -423,15 +425,17 @@
               path_output = path_finder->update(left_bev_meters, right_bev_meters, autosteer_steering);
 
               // 4. Compute steering angle (if controller available)
+            //   double steering_angle_raw = 0.0;  // Raw PID output before filtering
               if (steering_controller != nullptr && path_output.fused_valid) {
-                  steering_angle = steering_controller->computeSteering(
+                  steering_angle_raw = steering_controller->computeSteering(
                       path_output.cte,
                       path_output.yaw_error * 180 / M_PI,
                       path_output.curvature
                   );
               }
 
-              steering_angle = steering_filter.filter(steering_angle, 0.1);
+              // Filter the raw PID output
+              steering_angle = steering_filter.filter(steering_angle_raw, 0.1);
 
               // 5. Print output (cross-track error, yaw error, curvature, lane width + variances + steering)
               if (path_output.fused_valid) {
@@ -485,7 +489,8 @@
         result.frame_number = tf.frame_number;
         result.capture_time = tf.timestamp;
         result.inference_time = t_inference_end;
-        result.steering_angle = steering_angle;  // Store PID steering angle
+        result.steering_angle_raw = steering_angle_raw;  // Store raw PID output (before filtering)
+        result.steering_angle = steering_angle;  // Store filtered PID output (final steering)
         result.path_output = path_output;        // Store for viz
         result.autosteer_angle = autosteer_steering;  // Store AutoSteer angle
         result.autosteer_valid = (autosteer_engine != nullptr && egolanes_tensor_buffer.full());
@@ -539,12 +544,12 @@
      std::ofstream csv_file;
      csv_file.open(csv_log_path);
      if (csv_file.is_open()) {
-         // Write header
-         csv_file << "frame_id,timestamp_ms,"
-                  << "orig_lane_offset,orig_yaw_offset,orig_curvature,"
-                  << "pathfinder_cte,pathfinder_yaw_error,pathfinder_curvature,"
-                  << "pid_steering_rad,pid_steering_deg,"
-                  << "autosteer_angle_deg,autosteer_valid\n";
+        // Write header
+        csv_file << "frame_id,timestamp_ms,"
+                 << "orig_lane_offset,orig_yaw_offset,orig_curvature,"
+                 << "pathfinder_cte,pathfinder_yaw_error,pathfinder_curvature,"
+                 << "pid_steering_raw_deg,pid_steering_filtered_deg,"
+                 << "autosteer_angle_deg,autosteer_valid\n";
 
          std::cout << "CSV logging enabled: " << csv_log_path << std::endl;
      } else {
@@ -611,7 +616,8 @@
                     result.lanes,                   // Lane masks
                     view_debug,                     // Final visualization (with steering wheel + lane masks)
                     result.vehicle_state,           // CAN bus data
-                    result.steering_angle,          // PID steering (radians)
+                    result.steering_angle_raw,      // Raw PID output (degrees, before filtering)
+                    result.steering_angle,          // Filtered PID output (degrees, final steering)
                     result.autosteer_angle,         // AutoSteer steering (degrees)
                     result.path_output,             // PathFinder outputs
                     inference_time_us               // Inference time
@@ -737,8 +743,8 @@
                       << (result.path_output.fused_valid ? result.path_output.cte : 0.0) << ","
                       << (result.path_output.fused_valid ? result.path_output.yaw_error : 0.0) << ","
                       << (result.path_output.fused_valid ? result.path_output.curvature : 0.0) << ","
-                      // PID Controller steering angle (always log if controller exists)
-                      << std::fixed << std::setprecision(6) << result.steering_angle  *  M_PI / 180.0 << ","
+                      // PID Controller steering angles (all in degrees)
+                      << std::fixed << std::setprecision(6) << result.steering_angle_raw << ","
                       << result.steering_angle << ","
                       // AutoSteer steering angle (degrees) and validity
                       << result.autosteer_angle << ","

VisionPilot/Production_Releases/0.5/src/rerun/rerun_logger.cpp

@@ -89,6 +89,7 @@ void RerunLogger::logData(
     const autoware_pov::vision::egolanes::LaneSegmentation& lanes,
     const cv::Mat& stacked_view,
     const autoware_pov::drivers::CanVehicleState& vehicle_state,
+    double steering_angle_raw,
     double steering_angle,
     float autosteer_angle,
     const autoware_pov::vision::path_planning::PathFinderOutput& path_output,
@@ -127,11 +128,11 @@ void RerunLogger::logData(
     rec_->log("can/speed_kmph", 
               rerun::archetypes::Scalars(rerun::Collection<rerun::components::Scalar>({can_scalars[1]})));
 
-    // Log control outputs (scalars)
-    rec_->log("control/pid_steering_rad", 
+    // Log control outputs (all in degrees)
+    rec_->log("control/pid_steering_raw_deg", 
+              rerun::archetypes::Scalars(rerun::Collection<rerun::components::Scalar>({rerun::components::Scalar(steering_angle_raw)})));
+    rec_->log("control/pid_steering_filtered_deg", 
               rerun::archetypes::Scalars(rerun::Collection<rerun::components::Scalar>({rerun::components::Scalar(steering_angle)})));
-    rec_->log("control/pid_steering_deg", 
-              rerun::archetypes::Scalars(rerun::Collection<rerun::components::Scalar>({rerun::components::Scalar(steering_angle * 180.0 / M_PI)})));
     rec_->log("control/autosteer_angle_deg", 
               rerun::archetypes::Scalars(rerun::Collection<rerun::components::Scalar>({rerun::components::Scalar(autosteer_angle)})));
 
@@ -155,6 +156,7 @@ void RerunLogger::logData(
     (void)lanes;
     (void)stacked_view;
     (void)vehicle_state;
+    (void)steering_angle_raw;
     (void)steering_angle;
     (void)autosteer_angle;
     (void)path_output;