Issue with PX4 SITL Lockstep Synchronization

[yalindem]

Hi everyone,

I am experiencing issues with Lockstep synchronization between PX4 (SITL) and Cosys-AirSim. Despite enabling lockstep in the configuration files, the simulation does not seem to maintain a synchronous clock.

Setup

Cosys-Airsim: 5.5-v3.3 (docker container)
PX4: v1.16.0 (host)
QGroundControl: v5.0.8 (host)
Operating System: Ubuntu 22.04

The Core Issue:

The flight in the simulation is stable and responsive. I have added a LiDAR to the drone to test the PX4 Collision Prevention feature. The simulator publishes the sensor topics as expected and I can echo them successfully.

I am currently processing and converting the /airsim_node/PX4/lidar/points/lidar_horizontal topic and re-publishing it to /fmu/in/obstacle_distance. However, PX4 appears to reject this data. When I run the listener obstacle_distance command in the QGroundControl MAVLink Console, I noticed that the messages are consistently flagged as being '~1.2s ago'.

I suspect that PX4 is rejecting the data because it perceives it as stale/old. It seems there is a synchronization gap or a timestamp mismatch between the AirSim clock and the PX4 lockstep clock, specifically affecting the obstacle data pipeline.

To confirm the issue, I compared the timestamps of the topics /airsim_node/PX4/imu/imu and /fmu/out/sensor_combined. There is clearly a ~1-second gap between them. That is why I wanted to investigate the lockstep feature in Cosys-AirSim.

I set the relevant configuration options according to the documentation. I also debugged the code at the following location:
https://github.com/Cosys-Lab/Cosys-AirSim/blob/main/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp#L993

However, even when lockstep is enabled in the configuration, the variable lock_step_enabled_ always remains false.

Configuration

My settings.json file:

{
  "SettingsVersion": 2.0,
  "SimMode": "Multirotor",
  "ClockType": "SteppableClock",
  "PawnPaths": {
    "DefaultQuadrotor": {
      "PawnBP": "Class'/Game/Geometry/Meshes/drone8/drone_m_v1_poc2_BP.drone_m_v1_poc2_BP_C'"
    }
  },
  "CameraDefaults": {
    "CaptureSettings": [
      {
        "ImageType": 0,
        "Width": 640,
        "Height": 360,
        "FOV_Degrees": 90
      }
    ]
  },
  "Vehicles": {
    "PX4": {
      "VehicleType": "PX4Multirotor",
      "UseSerial": false,
      "LockStep": true,
      "UseTcp": true,
      "TcpPort": 4560,
      "ControlPortLocal": 14540,
      "ControlPortRemote": 14580,
      "Sensors": {
        "barometer": {
          "SensorType": 1,
          "Enabled": true,
          "X": 0,
          "Y": 0,
          "Z": -0.0230131783400001,
          "Roll": 0,
          "Pitch": 0,
          "Yaw": 0,
          "PressureFactorSigma": 0.0012,
          "PressureFactorTau": 3600,
          "UncorrelatedNoiseSigma": 0.1,
          "UpdateLatency": 0.009,
          "UpdateFrequency": 50,
          "StartupDelay": 0.01
        },
        "imu": {
          "Enabled": true,
          "SensorType": 2,
          "X": 0,
          "Y": 0,
          "Z": -0.0230131783400001,
          "Roll": 0,
          "Pitch": 0,
          "Yaw": 0,
          "UpdateRate": 200,
          "AngularRandomWalk": 0.25,
          "GyroBiasStabilityTau": 500,
          "GyroBiasStability": 4.0,
          "VelocityRandomWalk": 0.21,
          "AccelBiasStabilityTau": 800,
          "AccelBiasStability": 30
        },
        "gps": {
          "Enabled": true,
          "SensorType": 3,
          "X": 0.34038367521363305,
          "Y": -0.44916859822052396,
          "Z": -0.10979996163945907,
          "Roll": 0,
          "Pitch": 0,
          "Yaw": 0,
          "UpdateFrequency": 10,
          "EPH_TimeConstant": 0.9,
          "EPV_TimeConstant": 0.9,
          "EphInitial": 1.5,
          "EpvInitial": 2.0,
          "EphFinal": 0.7,
          "EpvFinal": 1.0,
          "EphMin3d": 1.0,
          "EphMin2d": 1.0,
          "StartupDelay": 25.0
        },
        "magnetometer": {
          "Enabled": true,
          "SensorType": 4
        },
        "lidar_horizontal": {
          "SensorType": 6,
          "Enabled": true,
          "External": false,
          "NumberOfChannels": 1,
          "Range": 40,
          "UpdateFrequency": 10.0,
          "RotationsPerSecond": 10,
          "MeasurementsPerCycle": 50,
          "X": 0.041304643733013284,
          "Y": 0,
          "Z": -0.19857589582336602,
          "Roll": 0,
          "Pitch": 0,
          "Yaw": 180,
          "VerticalFOVUpper": 0,
          "VerticalFOVLower": 0,
          "HorizontalFOVStart": 0,
          "HorizontalFOVEnd": 360,
          "DrawDebugPoints": false,
          "IgnoreMarked": true,
          "GenerateNoise": true,
          "DrawSensor": false,
          "StartupDelay": 0.0
        },
        "lidar_vertical": {
          "SensorType": 6,
          "Enabled": false,
          "External": false,
          "NumberOfChannels": 1,
          "Range": 40,
          "RotationsPerSecond": 10,
          "MeasurementsPerCycle": 50,
          "X": -0.22784134841842799,
          "Y": 0.0010974808245300493,
          "Z": -0.04219995880569029,
          "Roll": 0,
          "Pitch": 90,
          "Yaw": 0,
          "VerticalFOVUpper": 0,
          "VerticalFOVLower": 0,
          "HorizontalFOVStart": 0,
          "HorizontalFOVEnd": 360,
          "DrawDebugPoints": false,
          "IgnoreMarked": true,
          "GenerateNoise": true,
          "DrawSensor": false
        }
      },
      "Parameters": {
        "LPE_LAT": 47.641468,
        "LPE_LON": -122.140165
      }
    }
  }
}

ROS2 launch file:

import os

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.launch_description_sources import PythonLaunchDescriptionSource

from ament_index_python.packages import get_package_share_directory


def generate_launch_description():

    output = DeclareLaunchArgument("output", default_value="screen")
    publish_clock = DeclareLaunchArgument("publish_clock", default_value="True")
    is_vulkan = DeclareLaunchArgument("is_vulkan", default_value="True")
    host_ip = DeclareLaunchArgument("host_ip", default_value="localhost")
    host_port = DeclareLaunchArgument("host_port", default_value="41451")
    enable_api_control = DeclareLaunchArgument(
        "enable_api_control", default_value="False"
    )
    enable_object_transforms_list = DeclareLaunchArgument(
        "enable_object_transforms_list", default_value="True"
    )

    airsim_node = Node(
        package="airsim_ros_pkgs",
        executable="airsim_node",
        name="airsim_node",
        output=LaunchConfiguration("output"),
        parameters=[
            {
                "is_vulkan": LaunchConfiguration("is_vulkan"),
                "update_airsim_img_response_every_n_sec": 0.05,
                "update_airsim_control_every_n_sec": 0.005,
                "update_lidar_every_n_sec": 0.1,
                "update_gpulidar_every_n_sec": 0.01,
                "update_echo_every_n_sec": 0.01,
                "publish_clock": LaunchConfiguration("publish_clock"),
                "host_ip": LaunchConfiguration("host_ip"),
                "host_port": LaunchConfiguration("host_port"),
                "enable_api_control": LaunchConfiguration("enable_api_control"),
                "enable_object_transforms_list": LaunchConfiguration(
                    "enable_object_transforms_list"
                ),
                "use_sim_time": True,
            }
        ],
        remappings=[
            ("/airsim_node/clock", "/clock"),
        ],
    )

    ld = LaunchDescription()

    ld.add_action(output)
    ld.add_action(publish_clock)
    ld.add_action(is_vulkan)
    ld.add_action(host_ip)
    ld.add_action(host_port)
    ld.add_action(enable_api_control)
    ld.add_action(enable_object_transforms_list)
    ld.add_action(airsim_node)

    return ld

Docker-compose file

services:
  airsim:
    image: "xyz"
    container_name: "cosys_airsim_simulation"
    command: ["sleep", "infinity"]
    network_mode: host
    gpus: all
    environment:
      - DISPLAY
      - XAUTHORITY=/home/ue4/.Xauthority
      - ROS_DOMAIN_ID=${ROS_DOMAIN_ID}
      - ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY}
      - RMW_IMPLEMENTATION=rmw_fastrtps_cpp
      - TZ=Europe/Berlin
    volumes:
      - /tmp/.X11-unix:/tmp/.X11-unix:rw
      - ${HOME}/.Xauthority:/home/ue4/.Xauthority
      - ../airsim_share_folder:/home/ue4/airsim_share_folder:rw
      - ./launch/:/home/ue4/Cosys-AirSim/ros2/src/airsim_ros_pkgs/launch/
      - /etc/localtime:/etc/localtime:ro
      - /etc/timezone:/etc/timezone:ro
    restart: "no"

How i launch the airsim ros wrapper:

ros2 launch /home/ue4/Cosys-AirSim/ros2/src/airsim_ros_pkgs/launch/t7_airsim_node.launch.py use_sim_time:=true

** How i launch the px4:**

env PX4_SYS_AUTOSTART=<custom_airframe_for_my_use_case> ./build/px4_sitl_default/bin/px4

i would be very happy for any help.