From 6ad1803aaa637332e62ae25084632f67a4fe8786 Mon Sep 17 00:00:00 2001 From: DCCONSTRUCTIONS Date: Fri, 24 Jul 2026 22:40:44 +0300 Subject: [PATCH] fix(simulation): use PX4 rover offboard control level --- src/k1link/simulation/px4_rover_control.py | 49 +++++-------------- tests/test_simulation_worker_agent_command.py | 21 +++----- 2 files changed, 21 insertions(+), 49 deletions(-) diff --git a/src/k1link/simulation/px4_rover_control.py b/src/k1link/simulation/px4_rover_control.py index 98dcdd7..191a739 100644 --- a/src/k1link/simulation/px4_rover_control.py +++ b/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, diff --git a/tests/test_simulation_worker_agent_command.py b/tests/test_simulation_worker_agent_command.py index a9be998..8be23ae 100644 --- a/tests/test_simulation_worker_agent_command.py +++ b/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)