gz: use server config file for loading world plugins

ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500

@@ -13,8 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500}
 
 param set-default SIM_GZ_EN 1
 
-param set-default SENS_EN_MAGSIM 1
-
 param set-default CA_AIRFRAME 0
 param set-default CA_ROTOR_COUNT 4
 

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -85,7 +85,16 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
-	// IMU: /world/$WORLD/model/$MODEL/link/base_link/sensor/imu_sensor/imu
+	// 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;
+	}
+
+	// odom: /world/$WORLD/model/$MODEL/link/base_link/odometry_with_covariance
 	std::string odometry_topic = "/model/" + _model_name + "/odometry_with_covariance";
 
 	if (!_node.Subscribe(odometry_topic, &GZBridge::odometryCallback, this)) {
@@ -176,38 +185,64 @@ void GZBridge::clockCallback(const gz::msgs::Clock &msg)
 	px4_clock_settime(CLOCK_MONOTONIC, &ts);
 }
 
-void GZBridge::opticalFlowCallback(const px4::msgs::OpticalFlow &flow)
+void GZBridge::opticalFlowCallback(const px4::msgs::OpticalFlow &msg)
 {
-	sensor_optical_flow_s msg = {};
+	sensor_optical_flow_s report = {};
 
-	msg.timestamp = hrt_absolute_time();
-	msg.timestamp_sample = flow.time_usec();
-	msg.pixel_flow[0] = flow.integrated_x();
-	msg.pixel_flow[1] = flow.integrated_y();
-	msg.quality = flow.quality();
-	msg.integration_timespan_us = flow.integration_time_us();
+	report.timestamp = hrt_absolute_time();
+	report.timestamp_sample = msg.time_usec();
+	report.pixel_flow[0] = msg.integrated_x();
+	report.pixel_flow[1] = msg.integrated_y();
+	report.quality = msg.quality();
+	report.integration_timespan_us = msg.integration_time_us();
 
 	// Static data
 	device::Device::DeviceId id;
 	id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
 	id.devid_s.bus = 0;
 	id.devid_s.address = 0;
 	id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM;
-	msg.device_id = id.devid;
+	report.device_id = id.devid;
 
 	// values taken from PAW3902
-	msg.mode = sensor_optical_flow_s::MODE_LOWLIGHT;
-	msg.max_flow_rate = 7.4f;
-	msg.min_ground_distance = 0.f;
-	msg.max_ground_distance = 30.f;
-	msg.error_count = 0;
+	report.mode = sensor_optical_flow_s::MODE_LOWLIGHT;
+	report.max_flow_rate = 7.4f;
+	report.min_ground_distance = 0.f;
+	report.max_ground_distance = 30.f;
+	report.error_count = 0;
 
 	// No delta angle
 	// No distance
 	// This means that delta angle will come from vehicle gyro
 	// Distance will come from vehicle distance sensor
 
-	_optical_flow_pub.publish(msg);
+	_optical_flow_pub.publish(report);
+}
+
+void GZBridge::magnetometerCallback(const gz::msgs::Magnetometer &msg)
+{
+	const uint64_t timestamp = hrt_absolute_time();
+
+	device::Device::DeviceId id{};
+	id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
+	id.devid_s.devtype = DRV_MAG_DEVTYPE_MAGSIM;
+	id.devid_s.bus = 1;
+	id.devid_s.address = 3; // TODO: any value other than 3 causes Commander to not use the mag.... wtf
+
+	sensor_mag_s report{};
+	report.timestamp = timestamp;
+	report.timestamp_sample = timestamp;
+	report.device_id = id.devid;
+	report.temperature = this->_temperature;
+
+	// FIMEX: once we're on jetty or later
+	// The magnetometer plugin publishes in units of gauss and in a weird left handed coordinate system
+	// https://github.com/gazebosim/gz-sim/pull/2460
+	report.x = -msg.field_tesla().y();
+	report.y = -msg.field_tesla().x();
+	report.z = msg.field_tesla().z();
+
+	_sensor_mag_pub.publish(report);
 }
 
 void GZBridge::barometerCallback(const gz::msgs::FluidPressure &msg)
@@ -253,7 +288,6 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &msg)
 
 void GZBridge::imuCallback(const gz::msgs::IMU &msg)
 {
-
 	const uint64_t timestamp = hrt_absolute_time();
 
 	// FLU -> FRD
@@ -284,7 +318,6 @@ void GZBridge::imuCallback(const gz::msgs::IMU &msg)
 	accel.samples = 1;
 	_sensor_accel_pub.publish(accel);
 
-
 	gz::math::Vector3d gyro_b = q_FLU_to_FRD.RotateVector(gz::math::Vector3d(
 					    msg.angular_velocity().x(),
 					    msg.angular_velocity().y(),
@@ -484,8 +517,8 @@ static float generate_wgn()
 	return X;
 }
 
-void GZBridge::addRealisticGpsNoise(double &latitude, double &longitude, double &altitude,
-				    float &vel_north, float &vel_east, float &vel_down)
+void GZBridge::addGpsNoise(double &latitude, double &longitude, double &altitude,
+			   float &vel_north, float &vel_east, float &vel_down)
 {
 	_gps_pos_noise_n = _pos_markov_time * _gps_pos_noise_n +
 			   _pos_random_walk * generate_wgn() * _pos_noise_amplitude -
@@ -546,7 +579,7 @@ void GZBridge::navSatCallback(const gz::msgs::NavSat &msg)
 	_gpos_ground_truth_pub.publish(gps_truth);
 
 	// Apply noise model (based on ublox F9P)
-	addRealisticGpsNoise(latitude, longitude, altitude, vel_north, vel_east, vel_down);
+	addGpsNoise(latitude, longitude, altitude, vel_north, vel_east, vel_down);
 
 	// Device ID
 	device::Device::DeviceId id{};

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -57,6 +57,7 @@
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/sensor_optical_flow.h>
 #include <uORB/topics/obstacle_distance.h>
 #include <uORB/topics/wheel_encoders.h>
@@ -116,12 +117,13 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	void navSatCallback(const gz::msgs::NavSat &msg);
 	void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg);
 	void laserScanCallback(const gz::msgs::LaserScan &msg);
-	void opticalFlowCallback(const px4::msgs::OpticalFlow &image_msg);
+	void opticalFlowCallback(const px4::msgs::OpticalFlow &msg);
+	void magnetometerCallback(const gz::msgs::Magnetometer &msg);
 
 	static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU);
 
-	void addRealisticGpsNoise(double &latitude, double &longitude, double &altitude,
-				  float &vel_north, float &vel_east, float &vel_down);
+	void addGpsNoise(double &latitude, double &longitude, double &altitude,
+			 float &vel_north, float &vel_east, float &vel_down);
 
 	uORB::SubscriptionInterval                    _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
@@ -136,9 +138,11 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	uORB::PublicationMulti<sensor_baro_s>         _sensor_baro_pub{ORB_ID(sensor_baro)};
 	uORB::PublicationMulti<sensor_accel_s>        _sensor_accel_pub{ORB_ID(sensor_accel)};
 	uORB::PublicationMulti<sensor_gyro_s>         _sensor_gyro_pub{ORB_ID(sensor_gyro)};
+	uORB::PublicationMulti<sensor_mag_s>          _sensor_mag_pub{ORB_ID(sensor_mag)};
 	uORB::PublicationMulti<vehicle_odometry_s>    _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
 	uORB::PublicationMulti<sensor_optical_flow_s> _optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
+
 	GZMixingInterfaceESC   _mixing_interface_esc{_node};
 	GZMixingInterfaceServo _mixing_interface_servo{_node};
 	GZMixingInterfaceWheel _mixing_interface_wheel{_node};

src/modules/simulation/gz_bridge/gz_env.sh.in

@@ -1,8 +1,11 @@
 #!/usr/bin/env bash
-
 export PX4_GZ_MODELS=@PX4_SOURCE_DIR@/Tools/simulation/gz/models
 export PX4_GZ_WORLDS=@PX4_SOURCE_DIR@/Tools/simulation/gz/worlds
 export PX4_GZ_PLUGINS=@PX4_BINARY_DIR@/src/modules/simulation/gz_plugins
+export PX4_GZ_SERVER_CONFIG=@PX4_SOURCE_DIR@/src/modules/simulation/gz_bridge/server.config
 
 export GZ_SIM_RESOURCE_PATH=$GZ_SIM_RESOURCE_PATH:$PX4_GZ_MODELS:$PX4_GZ_WORLDS
 export GZ_SIM_SYSTEM_PLUGIN_PATH=$GZ_SIM_SYSTEM_PLUGIN_PATH:$PX4_GZ_PLUGINS
+# We load plugins specific for PX4 through our own server config file
+# See https://gazebosim.org/api/sim/8/server_config.html
+export GZ_SIM_SERVER_CONFIG_PATH=$PX4_GZ_SERVER_CONFIG

src/modules/simulation/gz_bridge/server.config

@@ -0,0 +1,18 @@
+<server_config>
+  <plugins>
+    <plugin entity_name="*" entity_type="world" filename="gz-sim-physics-system" name="gz::sim::systems::Physics"/>
+    <plugin entity_name="*" entity_type="world" filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands"/>
+    <plugin entity_name="*" entity_type="world" filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"/>
+    <plugin entity_name="*" entity_type="world" filename="gz-sim-contact-system" name="gz::sim::systems::Contact"/>
+    <plugin entity_name="*" entity_type="world" filename="gz-sim-imu-system" name="gz::sim::systems::Imu"/>
+    <plugin entity_name="*" entity_type="world" filename="gz-sim-air-pressure-system" name="gz::sim::systems::AirPressure"/>
+    <plugin entity_name="*" entity_type="world" filename="gz-sim-air-speed-system" name="gz::sim::systems::AirSpeed"/>
+    <plugin entity_name="*" entity_type="world" filename="gz-sim-apply-link-wrench-system" name="gz::sim::systems::ApplyLinkWrench"/>
+    <plugin entity_name="*" entity_type="world" filename="gz-sim-navsat-system" name="gz::sim::systems::NavSat"/>
+    <plugin entity_name="*" entity_type="world" filename="gz-sim-magnetometer-system" name="gz::sim::systems::Magnetometer"/>
+    <plugin entity_name="*" entity_type="world" filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <plugin entity_name="*" entity_type="world" filename="libOpticalFlowSystem.so" name="custom::OpticalFlowSystem"/>
+  </plugins>
+</server_config>

src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp

@@ -78,7 +78,7 @@ class SensorMagSim : public ModuleBase<SensorMagSim>, public ModuleParams, publi
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude_groundtruth)};
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};
 
-	PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+	PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 3, TYPE: SIMULATION
 
 	bool _mag_earth_available{false};