diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index d9e1c997bc92..d078dd477fd9 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -154,6 +154,15 @@ int GZBridge::init() return PX4_ERROR; } + // MAG: /world/$WORLD/model/$MODEL/link/base_link/sensor/magnetometer_sensor/magnetometer + std::string mag_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/base_link/sensor/magnetometer_sensor/magnetometer"; + + if (!_node.Subscribe(mag_topic, &GZBridge::magnetometerCallback, this)) { + PX4_ERR("failed to subscribe to %s", mag_topic.c_str()); + return PX4_ERROR; + } + // GPS: /world/$WORLD/model/$MODEL/link/gps1_link/sensor/gps/navsat std::string gps_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/gps1_link/sensor/gps/navsat"; @@ -162,6 +171,27 @@ int GZBridge::init() return PX4_ERROR; } + // Air pressure: /world/$WORLD/model/$MODEL/link/base_link/sensor/air_pressure_sensor/air_pressure + std::string air_pressure_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/base_link/sensor/air_pressure_sensor/air_pressure"; + + if (!_node.Subscribe(air_pressure_topic, &GZBridge::barometerCallback, this)) { + PX4_ERR("failed to subscribe to %s", air_pressure_topic.c_str()); + return PX4_ERROR; + } + +#if 0 + // Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed + std::string airpressure_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/airspeed_link/sensor/air_speed/air_speed"; + + if (!_node.Subscribe(airpressure_topic, &GZBridge::airspeedCallback, this)) { + PX4_ERR("failed to subscribe to %s", airpressure_topic.c_str()); + return PX4_ERROR; + } + +#endif + // ESC feedback: /x500/command/motor_speed if (_vehicle_type != "rover") { std::string motor_speed_topic = "/" + _model_name + "/command/motor_speed"; @@ -429,6 +459,50 @@ void GZBridge::imuCallback(const gz::msgs::IMU &imu) pthread_mutex_unlock(&_mutex); } +void GZBridge::magnetometerCallback(const gz::msgs::Magnetometer &magnetometer) +{ + if (hrt_absolute_time() == 0) { + return; + } + + pthread_mutex_lock(&_mutex); + + const uint64_t time_us = (magnetometer.header().stamp().sec() * 1000000) + (magnetometer.header().stamp().nsec() / + 1000); + + if (time_us > _world_time_us.load()) { + updateClock(magnetometer.header().stamp().sec(), magnetometer.header().stamp().nsec()); + } + + // FLU -> FRD + static const auto q_FLU_to_FRD = gz::math::Quaterniond(0, 1, 0, 0); + + gz::math::Vector3d mag_b = q_FLU_to_FRD.RotateVector(gz::math::Vector3d( + magnetometer.field_tesla().x(), + magnetometer.field_tesla().y(), + magnetometer.field_tesla().z())); + + // publish mag + sensor_mag_s sensor_mag{}; +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + sensor_mag.timestamp_sample = time_us; + sensor_mag.timestamp = time_us; +#else + sensor_mag.timestamp_sample = hrt_absolute_time(); + sensor_mag.timestamp = hrt_absolute_time(); +#endif + sensor_mag.device_id = 197388; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + sensor_mag.x = mag_b.X(); + sensor_mag.y = mag_b.Y(); + sensor_mag.z = mag_b.Z(); + sensor_mag.temperature = NAN; + sensor_mag.error_count = 0; + + _sensor_mag_pub.publish(sensor_mag); + + pthread_mutex_unlock(&_mutex); +} + void GZBridge::gpsCallback(const gz::msgs::NavSat &gps) { if (hrt_absolute_time() == 0) { @@ -504,6 +578,57 @@ void GZBridge::gpsCallback(const gz::msgs::NavSat &gps) pthread_mutex_unlock(&_mutex); } +void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure) +{ + if (hrt_absolute_time() == 0) { + return; + } + + pthread_mutex_lock(&_mutex); + + const uint64_t time_us = (air_pressure.header().stamp().sec() * 1000000) + + (air_pressure.header().stamp().nsec() / 1000); + + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = time_us; + sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION + sensor_baro.pressure = air_pressure.pressure(); + sensor_baro.temperature = this->_temperature; + sensor_baro.timestamp = hrt_absolute_time(); + _sensor_baro_pub.publish(sensor_baro); + + pthread_mutex_unlock(&_mutex); +} + +#if 0 +void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) +{ + if (hrt_absolute_time() == 0) { + return; + } + + pthread_mutex_lock(&_mutex); + + const uint64_t time_us = (air_speed.header().stamp().sec() * 1000000) + + (air_speed.header().stamp().nsec() / 1000); + + double air_speed_value = air_speed.diff_pressure(); + + differential_pressure_s report{}; + report.timestamp_sample = time_us; + report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION + report.differential_pressure_pa = static_cast(air_speed_value); // hPa to Pa; + report.temperature = static_cast(air_speed.temperature()) + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // K to C + report.timestamp = hrt_absolute_time();; + _differential_pressure_pub.publish(report); + + this->_temperature = report.temperature; + + pthread_mutex_unlock(&_mutex); +} +#endif + void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) { if (hrt_absolute_time() == 0) { diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index cbf14f466ed2..4552c890dd0a 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -46,9 +46,12 @@ #include #include #include +#include #include #include +#include #include +#include #include #include #include @@ -59,6 +62,7 @@ #include #include #include +#include using namespace time_literals; @@ -95,7 +99,10 @@ class GZBridge : public ModuleBase, public OutputModuleInterface void clockCallback(const gz::msgs::Clock &clock); void imuCallback(const gz::msgs::IMU &imu); + void magnetometerCallback(const gz::msgs::Magnetometer &mag); void gpsCallback(const gz::msgs::NavSat &gps); + void barometerCallback(const gz::msgs::FluidPressure &air_pressure); + void poseInfoCallback(const gz::msgs::Pose_V &pose); void motorSpeedCallback(const gz::msgs::Actuators &actuators); void updateCmdVel(); @@ -109,10 +116,14 @@ class GZBridge : public ModuleBase, public OutputModuleInterface uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; + //uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; + uORB::PublicationMulti _sensor_mag_pub{ORB_ID(sensor_mag)}; uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + px4::atomic _world_time_us{0}; @@ -133,6 +144,8 @@ class GZBridge : public ModuleBase, public OutputModuleInterface MixingOutput _mixing_output{"SIM_GZ", 8, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + float _temperature{288.15}; // 15 degrees + gz::transport::Node _node; gz::transport::Node::Publisher _actuators_pub; gz::transport::Node::Publisher _cmd_vel_pub;