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
 

ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna

@@ -12,11 +12,8 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=rc_cessna}
 
 param set-default SIM_GZ_EN 1
 
-param set-default SENS_EN_MAGSIM 1
 param set-default SENS_EN_ARSPDSIM 1
 
-
-
 param set-default FW_LND_ANG 8
 
 param set-default NPFG_PERIOD 12

ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol

@@ -13,7 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol}
 
 param set-default SIM_GZ_EN 1
 
-param set-default SENS_EN_MAGSIM 1
 param set-default SENS_EN_ARSPDSIM 1
 
 # TODO: Enable motor failure detection when the

ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision

@@ -14,8 +14,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=px4vision}
 
 param set-default SIM_GZ_EN 1
 
-param set-default SENS_EN_MAGSIM 1
-
 # Commander Parameters
 param set-default COM_DISARM_LAND 0.5
 

ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane

@@ -11,7 +11,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=advanced_plane}
 
 param set-default SIM_GZ_EN 1
 
-param set-default SENS_EN_MAGSIM 1
 param set-default SENS_EN_ARSPDSIM 1
 
 param set-default FW_LND_ANG 8

ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover

@@ -45,9 +45,6 @@ param set-default PP_LOOKAHD_GAIN 1
 param set-default PP_LOOKAHD_MAX 10
 param set-default PP_LOOKAHD_MIN 1
 
-# Simulated sensors
-param set-default SENS_EN_MAGSIM 1
-
 # Actuator mapping
 param set-default SIM_GZ_WH_FUNC1 101 # right wheel
 param set-default SIM_GZ_WH_MIN1 70

ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower

@@ -11,8 +11,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower}
 
 param set-default SIM_GZ_EN 1 # Gazebo bridge
 
-# Simulated sensors
-param set-default SENS_EN_MAGSIM 1
 # We can arm and drive in manual mode when it slides and GPS check fails:
 param set-default COM_ARM_WO_GPS 1
 

ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann

@@ -44,9 +44,6 @@ param set-default PP_LOOKAHD_GAIN 1
 param set-default PP_LOOKAHD_MAX 10
 param set-default PP_LOOKAHD_MIN 1
 
-# Simulated sensors
-param set-default SENS_EN_MAGSIM 1
-
 # Wheels
 param set-default SIM_GZ_WH_FUNC1 101
 param set-default SIM_GZ_WH_MIN1 0

ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter

@@ -13,8 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadtailsitter}
 
 param set-default SIM_GZ_EN 1 # Gazebo bridge
 
-param set-default SENS_EN_MAGSIM 1
-
 param set-default MAV_TYPE 20
 
 param set-default CA_AIRFRAME 4

ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor

@@ -13,8 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=tiltrotor}
 
 param set-default SIM_GZ_EN 1 # Gazebo bridge
 
-param set-default SENS_EN_MAGSIM 1
-
 param set-default MAV_TYPE 21
 
 param set-default CA_AIRFRAME 3

ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter

@@ -83,7 +83,6 @@ param set-default CA_ROTOR7_AY -0.211325
 param set-default CA_ROTOR7_AZ -0.57735
 
 param set-default SIM_GZ_EN 1
-param set-default SENS_EN_MAGSIM 1
 
 param set-default SIM_GZ_EC_FUNC1 101
 param set-default SIM_GZ_EC_FUNC2 102

src/modules/simulation/gz_bridge/CMakeLists.txt

@@ -132,14 +132,14 @@ if (gz-transport${GZ_TRANSPORT_VERSION}_FOUND)
 					COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_name} $<TARGET_FILE:px4>
 					WORKING_DIRECTORY ${SITL_WORKING_DIR}
 					USES_TERMINAL
-					DEPENDS px4 OpticalFlowSystem
+					DEPENDS px4 px4_gz_plugins
 				)
 			else()
 				add_custom_target(gz_${model_name}_${world_name}
 					COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_name} PX4_GZ_WORLD=${world_name} $<TARGET_FILE:px4>
 					WORKING_DIRECTORY ${SITL_WORKING_DIR}
 					USES_TERMINAL
-					DEPENDS px4 OpticalFlowSystem
+					DEPENDS px4 px4_gz_plugins
 				)
 			endif()
 		endforeach()

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,21 @@
 #!/usr/bin/env bash
+# -----------------------------------------------------------------------
+# Gazebo Environment Configuration
+# -----------------------------------------------------------------------
+# GZ_SIM_RESOURCE_PATH: Where Gazebo looks for models and worlds
+# GZ_SIM_SYSTEM_PLUGIN_PATH: Where Gazebo looks for plugin libraries
+# GZ_SIM_SERVER_CONFIG_PATH: Custom Gazebo server configuration file
+#
+# See Gazebo docs
+# https://gazebosim.org/api/sim/8/resources.html
+# https://gazebosim.org/api/sim/8/server_config.html
+# -----------------------------------------------------------------------
 
 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
+export GZ_SIM_SERVER_CONFIG_PATH=$PX4_GZ_SERVER_CONFIG

src/modules/simulation/gz_bridge/server.config

@@ -0,0 +1,19 @@
+<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"/>
+    <!-- <plugin entity_name="*" entity_type="world" filename="libTemplatePlugin.so" name="custom::TemplateSystem"/> -->
+  </plugins>
+</server_config>