fix(simulation): use PX4 rover offboard control level

src/k1link/simulation/px4_rover_control.py

@@ -13,6 +13,7 @@ from k1link.simulation.contracts import AckermannControlSetpoint
 OFFBOARD_HEARTBEAT_SECONDS: Final = 0.1
 OFFBOARD_WARMUP_HEARTBEATS: Final = 12
 OFFBOARD_ADMISSION_TIMEOUT_SECONDS: Final = 25.0
+STOCK_ROVER_MAX_THROTTLE_SPEED_MPS: Final = 3.1
 VEHICLE_COMMAND_DO_SET_MODE: Final = 176
 VEHICLE_COMMAND_COMPONENT_ARM_DISARM: Final = 400
 
@@ -177,9 +178,9 @@ class Ros2Px4AckermannControl:
                 "/fmu/in/offboard_control_mode",
                 qos,
             )
-            speed_publisher = node.create_publisher(
-                messages.RoverSpeedSetpoint,
-                "/fmu/in/rover_speed_setpoint",
+            throttle_publisher = node.create_publisher(
+                messages.RoverThrottleSetpoint,
+                "/fmu/in/rover_throttle_setpoint",
                 qos,
             )
             steering_publisher = node.create_publisher(
@@ -187,16 +188,6 @@ class Ros2Px4AckermannControl:
                 "/fmu/in/rover_steering_setpoint",
                 qos,
             )
-            attitude_publisher = node.create_publisher(
-                messages.RoverAttitudeSetpoint,
-                "/fmu/in/rover_attitude_setpoint",
-                qos,
-            )
-            rate_publisher = node.create_publisher(
-                messages.RoverRateSetpoint,
-                "/fmu/in/rover_rate_setpoint",
-                qos,
-            )
             command_publisher = node.create_publisher(
                 messages.VehicleCommand,
                 "/fmu/in/vehicle_command",
@@ -245,10 +236,8 @@ class Ros2Px4AckermannControl:
                 self._publish_setpoint(
                     messages,
                     offboard_publisher,
-                    speed_publisher,
+                    throttle_publisher,
                     steering_publisher,
-                    attitude_publisher,
-                    rate_publisher,
                     timestamp_us,
                     speed_mps,
                     steering_normalized,
@@ -306,10 +295,8 @@ class Ros2Px4AckermannControl:
     def _publish_setpoint(
         messages: Any,
         offboard_publisher: Any,
-        speed_publisher: Any,
+        throttle_publisher: Any,
         steering_publisher: Any,
-        attitude_publisher: Any,
-        rate_publisher: Any,
         timestamp_us: int,
         speed_mps: float,
         steering_normalized: float,
@@ -317,35 +304,25 @@ class Ros2Px4AckermannControl:
         offboard = messages.OffboardControlMode()
         offboard.timestamp = timestamp_us
         offboard.position = False
-        offboard.velocity = True
+        offboard.velocity = False
         offboard.acceleration = False
         offboard.attitude = False
         offboard.body_rate = False
-        offboard.thrust_and_torque = False
+        offboard.thrust_and_torque = True
         offboard.direct_actuator = False
         offboard_publisher.publish(offboard)
 
-        speed = messages.RoverSpeedSetpoint()
-        speed.timestamp = timestamp_us
-        speed.speed_body_x = float(speed_mps)
-        speed.speed_body_y = math.nan
-        speed_publisher.publish(speed)
+        throttle = messages.RoverThrottleSetpoint()
+        throttle.timestamp = timestamp_us
+        throttle.throttle_body_x = float(speed_mps / STOCK_ROVER_MAX_THROTTLE_SPEED_MPS)
+        throttle.throttle_body_y = math.nan
+        throttle_publisher.publish(throttle)
 
         steering = messages.RoverSteeringSetpoint()
         steering.timestamp = timestamp_us
         steering.normalized_steering_setpoint = float(steering_normalized)
         steering_publisher.publish(steering)
 
-        attitude = messages.RoverAttitudeSetpoint()
-        attitude.timestamp = timestamp_us
-        attitude.yaw_setpoint = math.nan
-        attitude_publisher.publish(attitude)
-
-        rate = messages.RoverRateSetpoint()
-        rate.timestamp = timestamp_us
-        rate.yaw_rate_setpoint = math.nan
-        rate_publisher.publish(rate)
-
     @staticmethod
     def _publish_vehicle_command(
         messages: Any,

tests/test_simulation_worker_agent_command.py

@@ -59,10 +59,8 @@ class _Message:
 
 class _Messages:
     OffboardControlMode = _Message
-    RoverSpeedSetpoint = _Message
+    RoverThrottleSetpoint = _Message
     RoverSteeringSetpoint = _Message
-    RoverAttitudeSetpoint = _Message
-    RoverRateSetpoint = _Message
 
 
 class _Publisher:
@@ -173,8 +171,8 @@ def test_worker_agent_command_dispatch_fails_closed_on_unsafe_envelope() -> None
         )
 
 
-def test_px4_adapter_publishes_speed_steering_without_direct_actuators() -> None:
-    publishers = [_Publisher() for _ in range(5)]
+def test_px4_adapter_maps_speed_to_stock_rover_throttle_without_direct_actuators() -> None:
+    publishers = [_Publisher() for _ in range(3)]
 
     Ros2Px4AckermannControl._publish_setpoint(
         _Messages,
@@ -185,14 +183,11 @@ def test_px4_adapter_publishes_speed_steering_without_direct_actuators() -> None
     )
 
     offboard = publishers[0].messages[0]
-    speed = publishers[1].messages[0]
+    throttle = publishers[1].messages[0]
     steering = publishers[2].messages[0]
-    attitude = publishers[3].messages[0]
-    rate = publishers[4].messages[0]
-    assert offboard.velocity is True
+    assert offboard.velocity is False
+    assert offboard.thrust_and_torque is True
     assert offboard.direct_actuator is False
-    assert speed.speed_body_x == 1.0
-    assert isnan(speed.speed_body_y)
+    assert throttle.throttle_body_x == pytest.approx(1.0 / 3.1)
+    assert isnan(throttle.throttle_body_y)
     assert steering.normalized_steering_setpoint == -0.55
-    assert isnan(attitude.yaw_setpoint)
-    assert isnan(rate.yaw_rate_setpoint)