Fixes from track - slow laps ok. Zenoh back

src/hardware/steering_actuator/src/component_steering_actuator.cpp

@@ -109,7 +109,7 @@ void SteeringActuator::steering_angle_callback(const std_msgs::msg::Float32::Sha
     if (!steering_ang_received_ && initial_enc_saved_) {
         steering_ang_received_ = true;
         // Calcualed equation was {Encoder value} = 82.493 * {Steering Angle} - 109.22
-        offset_ = int32_t(-82 * msg->data + 109) - initial_enc_;
+        offset_ = int32_t(-82 * (msg->data) + 109) - initial_enc_;
         RCLCPP_INFO(this->get_logger(), "Steering angle received, offset: %d", offset_);
     }
 
@@ -118,9 +118,9 @@ void SteeringActuator::steering_angle_callback(const std_msgs::msg::Float32::Sha
 
 // Get desired steering angle to update steering
 void SteeringActuator::driving_command_callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr msg) {
-    if (!motor_enabled_ && !steering_ang_received_) return;
+    if (!(motor_enabled_ && steering_ang_received_)) return;
     // Use 1 equation for both left and right turns
-    int32_t target = int32_t(-82 * msg->drive.steering_angle + 109) - offset_;
+    int32_t target = int32_t(-82 * (msg->drive.steering_angle + 10) + 109) - offset_;
     // Clamp target to max
     target = std::max(std::min(target, max_position_ - offset_), -max_position_ - offset_);
     RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 250, "Target: %f = %d", msg->drive.steering_angle,

src/navigation/nav_bringup/config/nav2_params.yaml

@@ -202,7 +202,7 @@ controller_server:
     TrackdriveRPP:
       plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
       desired_linear_vel: 3.0  # The desired maximum linear velocity to use.
-      lookahead_dist: 5.0  # The lookahead distance to use to find the lookahead point.
+      lookahead_dist: 4.5  # The lookahead distance to use to find the lookahead point.
       # min_lookahead_dist: 3.0  # The minimum lookahead distance threshold when using velocity scaled lookahead distances.
       # max_lookahead_dist: 5.0  # The maximum lookahead distance threshold when using velocity scaled lookahead distances.
       # lookahead_time: 2.0  # The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain.
@@ -234,7 +234,7 @@ controller_server:
       desired_linear_vel: 4.5  # The desired maximum linear velocity to use.
       # lookahead_dist: 6.0  # The lookahead distance to use to find the lookahead point.
       min_lookahead_dist: 4.0  # The minimum lookahead distance threshold when using velocity scaled lookahead distances.
-      max_lookahead_dist: 12.0  # The maximum lookahead distance threshold when using velocity scaled lookahead distances.
+      max_lookahead_dist: 10.0  # The maximum lookahead distance threshold when using velocity scaled lookahead distances.
       lookahead_time: 3.0  # The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain.
       # rotate_to_heading_angular_vel: 1.8  # If rotate to heading is used, this is the angular velocity to use.
       transform_tolerance: 0.3  # The TF transform tolerance.

src/operations/vehicle_bringup/launch/displays.launch.py

@@ -99,6 +99,6 @@ def generate_launch_description():
             rosboard_node,
             display_node,
             foxglove_node,
-            # zenoh_node,
+            zenoh_node,
         ]
     )

src/operations/vehicle_bringup/vehicle_bringup/node_trackdrive_handler.py

@@ -195,8 +195,8 @@ def timer_callback(self):
             self.get_logger().info(f"Lap {self.laps} completed")
 
         # we have finished lap "1"
-        if self.laps > 1:
-            self.controller_id = "TrackdriveRPPFast"
+        # if self.laps > 1:
+        #     self.controller_id = "TrackdriveRPPFast"
 
         # we have finished lap "10"
         if self.laps == 10: