From 2833832968ceb31b95801d77c0d947c346efc288 Mon Sep 17 00:00:00 2001 From: dsix-ls2n <79255350+dsix-ls2n@users.noreply.github.com> Date: Tue, 22 Nov 2022 17:26:11 +0100 Subject: [PATCH] simulation/gz_bridge: ignition with no lockstep (#20561) When using ignition SITL simulation with NO_LOCKSTEP, the SITL PX4 fails to update the IMU data from Ignition Gazebo. The timestamp for the IMU data is taken from the ignition message: - In LOCKSTEP mode the clock from the ignition simulation and the one from PX4 SITL are synchronized, hence everything works fine - In NO_LOCKSTEP mode, those clocks are not synchronized anymore, so the timestamp for the IMU data should not be the one from Ignition but the current time in PX4 SITL when receiving the message. --- src/modules/simulation/gz_bridge/GZBridge.cpp | 33 ++++++++++++++++--- 1 file changed, 28 insertions(+), 5 deletions(-) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 42cd6cea18..e21554bd42 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -282,7 +282,6 @@ int GZBridge::task_spawn(int argc, char *argv[]) #else return PX4_OK; - #endif // ENABLE_LOCKSTEP_SCHEDULER //return PX4_OK; @@ -353,14 +352,19 @@ void GZBridge::imuCallback(const ignition::msgs::IMU &imu) // publish accel sensor_accel_s sensor_accel{}; +#if defined(ENABLE_LOCKSTEP_SCHEDULER) sensor_accel.timestamp_sample = time_us; + sensor_accel.timestamp = time_us; +#else + sensor_accel.timestamp_sample = hrt_absolute_time(); + sensor_accel.timestamp = hrt_absolute_time(); +#endif sensor_accel.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION sensor_accel.x = accel_b.X(); sensor_accel.y = accel_b.Y(); sensor_accel.z = accel_b.Z(); sensor_accel.temperature = NAN; sensor_accel.samples = 1; - sensor_accel.timestamp = time_us; // hrt_absolute_time(); _sensor_accel_pub.publish(sensor_accel); @@ -371,14 +375,19 @@ void GZBridge::imuCallback(const ignition::msgs::IMU &imu) // publish gyro sensor_gyro_s sensor_gyro{}; +#if defined(ENABLE_LOCKSTEP_SCHEDULER) sensor_gyro.timestamp_sample = time_us; + sensor_gyro.timestamp = time_us; +#else + sensor_gyro.timestamp_sample = hrt_absolute_time(); + sensor_gyro.timestamp = hrt_absolute_time(); +#endif sensor_gyro.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION sensor_gyro.x = gyro_b.X(); sensor_gyro.y = gyro_b.Y(); sensor_gyro.z = gyro_b.Z(); sensor_gyro.temperature = NAN; sensor_gyro.samples = 1; - sensor_gyro.timestamp = time_us; // hrt_absolute_time(); _sensor_gyro_pub.publish(sensor_gyro); pthread_mutex_unlock(&_mutex); @@ -430,7 +439,11 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose) // publish attitude groundtruth vehicle_attitude_s vehicle_attitude_groundtruth{}; +#if defined(ENABLE_LOCKSTEP_SCHEDULER) vehicle_attitude_groundtruth.timestamp_sample = time_us; +#else + vehicle_attitude_groundtruth.timestamp_sample = hrt_absolute_time(); +#endif vehicle_attitude_groundtruth.q[0] = q_nb.W(); vehicle_attitude_groundtruth.q[1] = q_nb.X(); vehicle_attitude_groundtruth.q[2] = q_nb.Y(); @@ -441,8 +454,11 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose) // publish angular velocity groundtruth const matrix::Eulerf euler{matrix::Quatf(vehicle_attitude_groundtruth.q)}; vehicle_angular_velocity_s vehicle_angular_velocity_groundtruth{}; +#if defined(ENABLE_LOCKSTEP_SCHEDULER) vehicle_angular_velocity_groundtruth.timestamp_sample = time_us; - +#else + vehicle_angular_velocity_groundtruth.timestamp_sample = hrt_absolute_time(); +#endif const matrix::Vector3f angular_velocity = (euler - _euler_prev) / dt; _euler_prev = euler; angular_velocity.copyTo(vehicle_angular_velocity_groundtruth.xyz); @@ -455,8 +471,11 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose) } vehicle_local_position_s local_position_groundtruth{}; +#if defined(ENABLE_LOCKSTEP_SCHEDULER) local_position_groundtruth.timestamp_sample = time_us; - +#else + local_position_groundtruth.timestamp_sample = hrt_absolute_time(); +#endif // position ENU -> NED const matrix::Vector3d position{pose_position.y(), pose_position.x(), -pose_position.z()}; const matrix::Vector3d velocity{(position - _position_prev) / dt}; @@ -486,7 +505,11 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose) if (_pos_ref.isInitialized()) { // publish position groundtruth vehicle_global_position_s global_position_groundtruth{}; +#if defined(ENABLE_LOCKSTEP_SCHEDULER) global_position_groundtruth.timestamp_sample = time_us; +#else + global_position_groundtruth.timestamp_sample = hrt_absolute_time(); +#endif _pos_ref.reproject(local_position_groundtruth.x, local_position_groundtruth.y, global_position_groundtruth.lat, global_position_groundtruth.lon);