From d5d7321c1c8b994bc4a86dadb18d9ffaa3aaf5b6 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 3 Mar 2025 13:39:33 -0900 Subject: [PATCH 01/15] gz: use server config file for loading world plugins --- Tools/simulation/gz | 2 +- src/modules/simulation/gz_bridge/gz_env.sh.in | 8 ++- .../simulation/gz_bridge/server.config | 60 +++++++++++++++++++ 3 files changed, 68 insertions(+), 2 deletions(-) create mode 100644 src/modules/simulation/gz_bridge/server.config diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 5bbae38b4f94..d7d0d2e7d01b 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 5bbae38b4f942521b4f3288c298083571ea5718c +Subproject commit d7d0d2e7d01bfa7c4254d6153e08b7ac4259eb8d diff --git a/src/modules/simulation/gz_bridge/gz_env.sh.in b/src/modules/simulation/gz_bridge/gz_env.sh.in index 3267318bfc11..998b42da74f7 100644 --- a/src/modules/simulation/gz_bridge/gz_env.sh.in +++ b/src/modules/simulation/gz_bridge/gz_env.sh.in @@ -1,8 +1,14 @@ #!/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 +export GZ_SIM_SERVER_CONFIG_PATH=$HOME/.gz/sim/px4/server.config + +# We load plugins specific for PX4 through our own server config file +# See https://gazebosim.org/api/sim/8/server_config.html +mkdir -p $HOME/.gz/sim/px4/ +cp $PX4_GZ_SERVER_CONFIG $GZ_SIM_SERVER_CONFIG_PATH diff --git a/src/modules/simulation/gz_bridge/server.config b/src/modules/simulation/gz_bridge/server.config new file mode 100644 index 000000000000..53553f24b703 --- /dev/null +++ b/src/modules/simulation/gz_bridge/server.config @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + ogre2 + + + + + From 8a02f9d5885b1cf9c1f4e874b4188b7e68edc37e Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 3 Mar 2025 15:18:25 -0900 Subject: [PATCH 02/15] submodule --- Tools/simulation/gz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gz b/Tools/simulation/gz index d7d0d2e7d01b..3282786692a3 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit d7d0d2e7d01bfa7c4254d6153e08b7ac4259eb8d +Subproject commit 3282786692a30fb29d1c3200e4e264207aaaa9fe From 35fb095488ab2b860bffce0a1c50990595df53a1 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 3 Mar 2025 18:28:23 -0900 Subject: [PATCH 03/15] use server.config in tree --- src/modules/simulation/gz_bridge/gz_env.sh.in | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/simulation/gz_bridge/gz_env.sh.in b/src/modules/simulation/gz_bridge/gz_env.sh.in index 998b42da74f7..8443301d371f 100644 --- a/src/modules/simulation/gz_bridge/gz_env.sh.in +++ b/src/modules/simulation/gz_bridge/gz_env.sh.in @@ -6,9 +6,7 @@ export PX4_GZ_SERVER_CONFIG=@PX4_SOURCE_DIR@/src/modules/simulation/gz_bridge/se 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=$HOME/.gz/sim/px4/server.config - # We load plugins specific for PX4 through our own server config file # See https://gazebosim.org/api/sim/8/server_config.html -mkdir -p $HOME/.gz/sim/px4/ -cp $PX4_GZ_SERVER_CONFIG $GZ_SIM_SERVER_CONFIG_PATH +export GZ_SIM_SERVER_CONFIG_PATH=$PX4_GZ_SERVER_CONFIG + From f1d2113b8d3143eb53ec6635ce0a69fb4465ea43 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 3 Mar 2025 18:31:48 -0900 Subject: [PATCH 04/15] newlines --- src/modules/simulation/gz_bridge/gz_env.sh.in | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/simulation/gz_bridge/gz_env.sh.in b/src/modules/simulation/gz_bridge/gz_env.sh.in index 8443301d371f..aa1896b8b712 100644 --- a/src/modules/simulation/gz_bridge/gz_env.sh.in +++ b/src/modules/simulation/gz_bridge/gz_env.sh.in @@ -9,4 +9,3 @@ 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 - From c93642eca9f250cc6761b9615d3dc7a1f8678f52 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 3 Mar 2025 19:16:13 -0900 Subject: [PATCH 05/15] format --- .../simulation/gz_bridge/server.config | 69 ++++--------------- 1 file changed, 13 insertions(+), 56 deletions(-) diff --git a/src/modules/simulation/gz_bridge/server.config b/src/modules/simulation/gz_bridge/server.config index 53553f24b703..aa8c5d4886a1 100644 --- a/src/modules/simulation/gz_bridge/server.config +++ b/src/modules/simulation/gz_bridge/server.config @@ -1,60 +1,17 @@ - - - - - - - - - - - - - - - - - - - - ogre2 - - - + + + + + + + + + + + ogre2 + + From b60edf1d136921dd50e497e3b8484768f195af5b Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 3 Mar 2025 19:41:53 -0900 Subject: [PATCH 06/15] gzbridge: rename function --- src/modules/simulation/gz_bridge/GZBridge.cpp | 4 ++-- src/modules/simulation/gz_bridge/GZBridge.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index c890ad8d5c94..104e7bc5db40 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -484,7 +484,7 @@ static float generate_wgn() return X; } -void GZBridge::addRealisticGpsNoise(double &latitude, double &longitude, double &altitude, +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 + @@ -546,7 +546,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{}; diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 6128e8c17503..9e66263a01ac 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -120,7 +120,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S 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, + 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}; From 61cc95a6cb5b39de2eb95cb6ba90cf406b64c03c Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 3 Mar 2025 19:49:20 -0900 Subject: [PATCH 07/15] format --- src/modules/simulation/gz_bridge/GZBridge.cpp | 2 +- src/modules/simulation/gz_bridge/GZBridge.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 104e7bc5db40..4b6fe9250243 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -485,7 +485,7 @@ static float generate_wgn() } void GZBridge::addGpsNoise(double &latitude, double &longitude, double &altitude, - float &vel_north, float &vel_east, float &vel_down) + 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 - diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 9e66263a01ac..5c7e914e09e6 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -121,7 +121,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU); void addGpsNoise(double &latitude, double &longitude, double &altitude, - float &vel_north, float &vel_east, float &vel_down); + float &vel_north, float &vel_east, float &vel_down); uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; From 8f6e5146c3d498d6958041c07d112d8e7ebd9df8 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 3 Mar 2025 22:02:14 -0900 Subject: [PATCH 08/15] gzbridge: add magnetometer callback --- .../init.d-posix/airframes/4001_gz_x500 | 2 - Tools/simulation/gz | 2 +- src/modules/simulation/gz_bridge/GZBridge.cpp | 69 ++++++++++++++----- src/modules/simulation/gz_bridge/GZBridge.hpp | 6 +- .../simulation/gz_bridge/server.config | 1 + .../sensor_mag_sim/SensorMagSim.hpp | 2 +- 6 files changed, 59 insertions(+), 23 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 index 3923b44cb4ab..2add7a5cb4b1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 +++ b/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 diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 3282786692a3..4e1a2fdca122 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 3282786692a30fb29d1c3200e4e264207aaaa9fe +Subproject commit 4e1a2fdca122deb78e7ecb815beb2d5b338433d3 diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 4b6fe9250243..6cf1734da5e3 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/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,16 +185,16 @@ 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; @@ -193,21 +202,47 @@ void GZBridge::opticalFlowCallback(const px4::msgs::OpticalFlow &flow) 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(), diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 5c7e914e09e6..ac096744c267 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -116,7 +117,8 @@ class GZBridge : public ModuleBase, 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); @@ -136,9 +138,11 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; 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 _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::PublicationMulti _optical_flow_pub{ORB_ID(sensor_optical_flow)}; + GZMixingInterfaceESC _mixing_interface_esc{_node}; GZMixingInterfaceServo _mixing_interface_servo{_node}; GZMixingInterfaceWheel _mixing_interface_wheel{_node}; diff --git a/src/modules/simulation/gz_bridge/server.config b/src/modules/simulation/gz_bridge/server.config index aa8c5d4886a1..cd2ed4a6148b 100644 --- a/src/modules/simulation/gz_bridge/server.config +++ b/src/modules/simulation/gz_bridge/server.config @@ -9,6 +9,7 @@ + ogre2 diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp index 7ee92ad75024..18c4adbf1dee 100644 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp +++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp @@ -78,7 +78,7 @@ class SensorMagSim : public ModuleBase, 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}; From 8407eb5077dbfb10c0124f211b0f2854c6d4604c Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 4 Mar 2025 14:04:45 -0900 Subject: [PATCH 09/15] change gz_find_package to find_package --- src/modules/simulation/gz_plugins/CMakeLists.txt | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt index c4400b0204d7..e39254d671b2 100644 --- a/src/modules/simulation/gz_plugins/CMakeLists.txt +++ b/src/modules/simulation/gz_plugins/CMakeLists.txt @@ -33,12 +33,12 @@ find_package(gz-transport${GZ_TRANSPORT_VERSION}) if (gz-transport${GZ_TRANSPORT_VERSION}_FOUND) - gz_find_package(gz-cmake${GZ_CMAKE_VERSION} REQUIRED) - gz_find_package(gz-msgs${GZ_MSGS_VERSION} REQUIRED) - gz_find_package(Protobuf REQUIRED) - gz_find_package(gz-plugin${GZ_PLUGIN_VERSION} REQUIRED COMPONENTS register) - gz_find_package(gz-sim${GZ_SIM_VERSION} REQUIRED) - gz_find_package(gz-sensors${GZ_SENSORS_VERSION} REQUIRED) + find_package(gz-cmake${GZ_CMAKE_VERSION} REQUIRED) + find_package(gz-msgs${GZ_MSGS_VERSION} REQUIRED) + find_package(Protobuf REQUIRED) + find_package(gz-plugin${GZ_PLUGIN_VERSION} REQUIRED COMPONENTS register) + find_package(gz-sim${GZ_SIM_VERSION} REQUIRED) + find_package(gz-sensors${GZ_SENSORS_VERSION} REQUIRED) include(${CMAKE_CURRENT_SOURCE_DIR}/optical_flow.cmake) From de76eaf42b7b20668b622b1510d97cdf8271f10a Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 4 Mar 2025 14:43:58 -0900 Subject: [PATCH 10/15] fix up directory structure and cmake to allow multiple plugins --- .../simulation/gz_bridge/CMakeLists.txt | 4 +- .../simulation/gz_bridge/server.config | 1 + .../simulation/gz_plugins/CMakeLists.txt | 72 +++++++++++-------- src/modules/simulation/gz_plugins/README.md | 39 ++++++++++ .../gz_plugins/optical_flow/CMakeLists.txt | 65 +++++++++++++++++ .../{ => optical_flow}/OpticalFlowSensor.cpp | 0 .../{ => optical_flow}/OpticalFlowSensor.hpp | 0 .../{ => optical_flow}/OpticalFlowSystem.cpp | 0 .../{ => optical_flow}/OpticalFlowSystem.hpp | 0 .../{ => optical_flow}/optical_flow.cmake | 0 .../gz_plugins/template_plugin/CMakeLists.txt | 68 ++++++++++++++++++ .../gz_plugins/template_plugin/README.md | 30 ++++++++ .../template_plugin/TemplateSystem.cpp | 58 +++++++++++++++ .../template_plugin/TemplateSystem.hpp | 57 +++++++++++++++ 14 files changed, 364 insertions(+), 30 deletions(-) create mode 100644 src/modules/simulation/gz_plugins/README.md create mode 100644 src/modules/simulation/gz_plugins/optical_flow/CMakeLists.txt rename src/modules/simulation/gz_plugins/{ => optical_flow}/OpticalFlowSensor.cpp (100%) rename src/modules/simulation/gz_plugins/{ => optical_flow}/OpticalFlowSensor.hpp (100%) rename src/modules/simulation/gz_plugins/{ => optical_flow}/OpticalFlowSystem.cpp (100%) rename src/modules/simulation/gz_plugins/{ => optical_flow}/OpticalFlowSystem.hpp (100%) rename src/modules/simulation/gz_plugins/{ => optical_flow}/optical_flow.cmake (100%) create mode 100644 src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt create mode 100644 src/modules/simulation/gz_plugins/template_plugin/README.md create mode 100644 src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp create mode 100644 src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.hpp diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 784c5a5ca89c..5fdc455cc8b0 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/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} $ 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} $ WORKING_DIRECTORY ${SITL_WORKING_DIR} USES_TERMINAL - DEPENDS px4 OpticalFlowSystem + DEPENDS px4 px4_gz_plugins ) endif() endforeach() diff --git a/src/modules/simulation/gz_bridge/server.config b/src/modules/simulation/gz_bridge/server.config index cd2ed4a6148b..93bff3f442ac 100644 --- a/src/modules/simulation/gz_bridge/server.config +++ b/src/modules/simulation/gz_bridge/server.config @@ -14,5 +14,6 @@ ogre2 + diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt index e39254d671b2..4bb330710811 100644 --- a/src/modules/simulation/gz_plugins/CMakeLists.txt +++ b/src/modules/simulation/gz_plugins/CMakeLists.txt @@ -1,4 +1,37 @@ -project(OpticalFlowSystem) +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +project(px4_gz_plugins) if(NOT DEFINED ENV{GZ_DISTRO}) set(GZ_DISTRO "harmonic" CACHE STRING "Gazebo distribution to use") @@ -28,11 +61,10 @@ else() message(FATAL_ERROR "Unknown Gazebo distribution: ${GZ_DISTRO}. Valid options are: harmonic or ionic") endif() -# Use gz-transport as litmus test for prescence of gz +# Use gz-transport as litmus test for presence of gz find_package(gz-transport${GZ_TRANSPORT_VERSION}) if (gz-transport${GZ_TRANSPORT_VERSION}_FOUND) - find_package(gz-cmake${GZ_CMAKE_VERSION} REQUIRED) find_package(gz-msgs${GZ_MSGS_VERSION} REQUIRED) find_package(Protobuf REQUIRED) @@ -40,31 +72,15 @@ if (gz-transport${GZ_TRANSPORT_VERSION}_FOUND) find_package(gz-sim${GZ_SIM_VERSION} REQUIRED) find_package(gz-sensors${GZ_SENSORS_VERSION} REQUIRED) - include(${CMAKE_CURRENT_SOURCE_DIR}/optical_flow.cmake) - - add_library(${PROJECT_NAME} SHARED - OpticalFlowSensor.cpp - OpticalFlowSystem.cpp - ) - - target_link_libraries(${PROJECT_NAME} - PUBLIC px4_gz_msgs - PUBLIC gz-sensors${GZ_SENSORS_VERSION}::gz-sensors${GZ_SENSORS_VERSION} - PUBLIC gz-plugin${GZ_PLUGIN_VERSION}::gz-plugin${GZ_PLUGIN_VERSION} - PUBLIC gz-sim${GZ_SIM_VERSION}::gz-sim${GZ_SIM_VERSION} - PUBLIC gz-transport${GZ_TRANSPORT_VERSION}::gz-transport${GZ_TRANSPORT_VERSION} - PUBLIC ${OpenCV_LIBS} - PUBLIC ${OpticalFlow_LIBS} - ) - - target_include_directories(${PROJECT_NAME} - PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} - PUBLIC ${CMAKE_CURRENT_BINARY_DIR} - PUBLIC ${OpenCV_INCLUDE_DIRS} - PUBLIC ${OpticalFlow_INCLUDE_DIRS} - PUBLIC px4_gz_msgs - ) + # Create a flat output directory for all plugin libraries + set(PX4_GZ_PLUGIN_OUTPUT_DIR "${CMAKE_CURRENT_BINARY_DIR}" CACHE PATH "Directory for all Gazebo plugin libraries") + file(MAKE_DIRECTORY ${PX4_GZ_PLUGIN_OUTPUT_DIR}) + set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PX4_GZ_PLUGIN_OUTPUT_DIR}) - add_dependencies(${PROJECT_NAME} OpticalFlow) + # Add our plugins as subdirectories + add_subdirectory(optical_flow) + add_subdirectory(template_plugin) + # Add an alias target for each plugin + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem TemplatePlugin) endif() diff --git a/src/modules/simulation/gz_plugins/README.md b/src/modules/simulation/gz_plugins/README.md new file mode 100644 index 000000000000..904fb05910f8 --- /dev/null +++ b/src/modules/simulation/gz_plugins/README.md @@ -0,0 +1,39 @@ +# Gazebo Plugins for PX4 + +This directory contains the Gazebo plugins used by PX4 for simulation. + +## Directory Structure + +Each plugin is contained in its own subdirectory with all of its source code and build files. + +Current plugins: +- `optical_flow`: Optical flow sensor simulation + +## Adding New Plugins + +To add a new plugin: + +1. Copy the `template_plugin` directory and rename it to your plugin name +2. Update the project name in the CMakeLists.txt file +3. Rename and update the TemplateSystem files to match your plugin functionality +4. Add your plugin to the top-level CMakeLists.txt by adding: + ```cmake + add_subdirectory(your_plugin_directory) + ``` +5. Add your plugin's target to the `gz_plugins` target dependencies in the top-level CMakeLists.txt: + ```cmake + add_custom_target(gz_plugins ALL DEPENDS OpticalFlowSystem YourPluginSystem) + ``` +6. Update the server.config file to load your plugin: + ```xml + + ``` + +## Building + +The plugins are built automatically when building PX4 simulation targets. + +## Using in Gazebo + +To use a plugin in your Gazebo simulation, it needs to be loaded in the server configuration file. +See `gz_bridge/server.config` for an example of how plugins are loaded. \ No newline at end of file diff --git a/src/modules/simulation/gz_plugins/optical_flow/CMakeLists.txt b/src/modules/simulation/gz_plugins/optical_flow/CMakeLists.txt new file mode 100644 index 000000000000..2a57ba3c2542 --- /dev/null +++ b/src/modules/simulation/gz_plugins/optical_flow/CMakeLists.txt @@ -0,0 +1,65 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +project(OpticalFlowSystem) + +# Include the OpticalFlow external dependency +include(${CMAKE_CURRENT_SOURCE_DIR}/optical_flow.cmake) + +# Find OpenCV +find_package(OpenCV REQUIRED) + +add_library(${PROJECT_NAME} SHARED + OpticalFlowSensor.cpp + OpticalFlowSystem.cpp +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC px4_gz_msgs + PUBLIC gz-sensors${GZ_SENSORS_VERSION}::gz-sensors${GZ_SENSORS_VERSION} + PUBLIC gz-plugin${GZ_PLUGIN_VERSION}::gz-plugin${GZ_PLUGIN_VERSION} + PUBLIC gz-sim${GZ_SIM_VERSION}::gz-sim${GZ_SIM_VERSION} + PUBLIC gz-transport${GZ_TRANSPORT_VERSION}::gz-transport${GZ_TRANSPORT_VERSION} + PUBLIC ${OpenCV_LIBS} + PUBLIC ${OpticalFlow_LIBS} +) + +target_include_directories(${PROJECT_NAME} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC ${OpenCV_INCLUDE_DIRS} + PUBLIC ${OpticalFlow_INCLUDE_DIRS} + PUBLIC px4_gz_msgs +) + +add_dependencies(${PROJECT_NAME} OpticalFlow) diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.cpp similarity index 100% rename from src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp rename to src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.cpp diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.hpp similarity index 100% rename from src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp rename to src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.hpp diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.cpp similarity index 100% rename from src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp rename to src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.cpp diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.hpp similarity index 100% rename from src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp rename to src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.hpp diff --git a/src/modules/simulation/gz_plugins/optical_flow.cmake b/src/modules/simulation/gz_plugins/optical_flow/optical_flow.cmake similarity index 100% rename from src/modules/simulation/gz_plugins/optical_flow.cmake rename to src/modules/simulation/gz_plugins/optical_flow/optical_flow.cmake diff --git a/src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt b/src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt new file mode 100644 index 000000000000..0d9156a1fa8f --- /dev/null +++ b/src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt @@ -0,0 +1,68 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# Template for a new plugin project +# Replace TemplatePlugin with your plugin name +project(TemplatePlugin) + +# Add external dependencies if needed +# include(${CMAKE_CURRENT_SOURCE_DIR}/dependency.cmake) + +# Find required packages +# find_package(PackageName REQUIRED) + +add_library(${PROJECT_NAME} SHARED + # Add your source files here + TemplateSystem.cpp +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC px4_gz_msgs + PUBLIC gz-sensors${GZ_SENSORS_VERSION}::gz-sensors${GZ_SENSORS_VERSION} + PUBLIC gz-plugin${GZ_PLUGIN_VERSION}::gz-plugin${GZ_PLUGIN_VERSION} + PUBLIC gz-sim${GZ_SIM_VERSION}::gz-sim${GZ_SIM_VERSION} + PUBLIC gz-transport${GZ_TRANSPORT_VERSION}::gz-transport${GZ_TRANSPORT_VERSION} + # Add other dependencies as needed + # PUBLIC ${OtherLib_LIBS} +) + +target_include_directories(${PROJECT_NAME} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC px4_gz_msgs + # Add other include directories as needed + # PUBLIC ${OtherLib_INCLUDE_DIRS} +) + +# Add dependencies if needed +# add_dependencies(${PROJECT_NAME} ExternalDependency) \ No newline at end of file diff --git a/src/modules/simulation/gz_plugins/template_plugin/README.md b/src/modules/simulation/gz_plugins/template_plugin/README.md new file mode 100644 index 000000000000..dda5fbcbf55d --- /dev/null +++ b/src/modules/simulation/gz_plugins/template_plugin/README.md @@ -0,0 +1,30 @@ +# Template Gazebo Plugin + +This is a template for creating new Gazebo plugins for PX4. Follow these steps to create your own plugin: + +1. Copy this directory and rename it to your plugin name +2. Update the project name in CMakeLists.txt +3. Rename and implement the TemplateSystem.hpp/cpp files +4. Add your plugin to the top-level CMakeLists.txt in the gz_plugins directory: + ```cmake + add_subdirectory(your_plugin_directory) + ``` +5. Add your plugin's target to the `px4_gz_plugins` target dependencies in the top-level CMakeLists.txt: + ```cmake + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem YourPluginSystem) + ``` +6. Update the server.config file to load your plugin: + ```xml + + ``` + +## Plugin Structure + +This template follows the standard Gazebo plugin structure: + +- `TemplateSystem.hpp/cpp`: The main plugin system class that is loaded by Gazebo +- CMakeLists.txt: Build configuration for this plugin + +## Testing Your Plugin + +After building, you can test your plugin by adding it to the server.config file and running a simulation. diff --git a/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp b/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp new file mode 100644 index 000000000000..5f8c2310779c --- /dev/null +++ b/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "TemplateSystem.hpp" + +#include + +using namespace custom; + +// Register the plugin +GZ_ADD_PLUGIN( + TemplateSystem, + gz::sim::System, + TemplateSystem::ISystemPreUpdate, + TemplateSystem::ISystemPostUpdate +) + +void TemplateSystem::PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + // Implement pre-update logic here +} + +void TemplateSystem::PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) +{ + // Implement post-update logic here +} \ No newline at end of file diff --git a/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.hpp b/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.hpp new file mode 100644 index 000000000000..840dec1c332f --- /dev/null +++ b/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.hpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +namespace custom +{ +class TemplateSystem: + public gz::sim::System, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate +{ +public: + void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) final; + + void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) final; + +private: + // Add your private member variables and methods here + gz::transport::Node _node; +}; +} // end namespace custom From 029189cd4a47dae065f6343d85cf12e7d444a8a4 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 4 Mar 2025 14:46:01 -0900 Subject: [PATCH 11/15] newlines --- .../simulation/gz_plugins/template_plugin/CMakeLists.txt | 2 +- .../simulation/gz_plugins/template_plugin/TemplateSystem.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt b/src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt index 0d9156a1fa8f..f5e2935ef670 100644 --- a/src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt +++ b/src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt @@ -65,4 +65,4 @@ target_include_directories(${PROJECT_NAME} ) # Add dependencies if needed -# add_dependencies(${PROJECT_NAME} ExternalDependency) \ No newline at end of file +# add_dependencies(${PROJECT_NAME} ExternalDependency) diff --git a/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp b/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp index 5f8c2310779c..ce073292240c 100644 --- a/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp +++ b/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp @@ -55,4 +55,4 @@ void TemplateSystem::PostUpdate(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm) { // Implement post-update logic here -} \ No newline at end of file +} From ca1bea20f0555bc146694f80be1acafbdedb1050 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 4 Mar 2025 14:52:43 -0900 Subject: [PATCH 12/15] add comment block explaining gz_env.sh --- src/modules/simulation/gz_bridge/gz_env.sh.in | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/modules/simulation/gz_bridge/gz_env.sh.in b/src/modules/simulation/gz_bridge/gz_env.sh.in index aa1896b8b712..bff7180b03d7 100644 --- a/src/modules/simulation/gz_bridge/gz_env.sh.in +++ b/src/modules/simulation/gz_bridge/gz_env.sh.in @@ -1,4 +1,16 @@ #!/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 @@ -6,6 +18,4 @@ export PX4_GZ_SERVER_CONFIG=@PX4_SOURCE_DIR@/src/modules/simulation/gz_bridge/se 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 From d336f514c68ef1c72c81291dcc4a385975bcb1a9 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 4 Mar 2025 14:59:34 -0900 Subject: [PATCH 13/15] remove dupe readme --- src/modules/simulation/gz_plugins/README.md | 39 --------------------- 1 file changed, 39 deletions(-) delete mode 100644 src/modules/simulation/gz_plugins/README.md diff --git a/src/modules/simulation/gz_plugins/README.md b/src/modules/simulation/gz_plugins/README.md deleted file mode 100644 index 904fb05910f8..000000000000 --- a/src/modules/simulation/gz_plugins/README.md +++ /dev/null @@ -1,39 +0,0 @@ -# Gazebo Plugins for PX4 - -This directory contains the Gazebo plugins used by PX4 for simulation. - -## Directory Structure - -Each plugin is contained in its own subdirectory with all of its source code and build files. - -Current plugins: -- `optical_flow`: Optical flow sensor simulation - -## Adding New Plugins - -To add a new plugin: - -1. Copy the `template_plugin` directory and rename it to your plugin name -2. Update the project name in the CMakeLists.txt file -3. Rename and update the TemplateSystem files to match your plugin functionality -4. Add your plugin to the top-level CMakeLists.txt by adding: - ```cmake - add_subdirectory(your_plugin_directory) - ``` -5. Add your plugin's target to the `gz_plugins` target dependencies in the top-level CMakeLists.txt: - ```cmake - add_custom_target(gz_plugins ALL DEPENDS OpticalFlowSystem YourPluginSystem) - ``` -6. Update the server.config file to load your plugin: - ```xml - - ``` - -## Building - -The plugins are built automatically when building PX4 simulation targets. - -## Using in Gazebo - -To use a plugin in your Gazebo simulation, it needs to be loaded in the server configuration file. -See `gz_bridge/server.config` for an example of how plugins are loaded. \ No newline at end of file From b001a2e12550908ff9a86f58e763024327c7d0ab Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 5 Mar 2025 14:31:42 -0900 Subject: [PATCH 14/15] remove SENS_EN_MAGSIM from all gz airframe files except spacecraft --- ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna | 3 --- .../px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol | 1 - ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision | 2 -- .../init.d-posix/airframes/4008_gz_advanced_plane | 1 - ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover | 3 --- ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower | 2 -- .../init.d-posix/airframes/4012_gz_rover_ackermann | 3 --- .../init.d-posix/airframes/4018_gz_quadtailsitter | 2 -- ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor | 2 -- ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter | 1 - Tools/simulation/gz | 2 +- 11 files changed, 1 insertion(+), 21 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna index ea7b3a2f03fb..96dd5fa15140 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol index 69b0cec559c1..5bd432abded7 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index e988f1f9b0d1..4370a677ed12 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane index 8479f2e38c34..8ac407bac447 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index 5ff39b3f9693..2d36aed7ce5e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index a56cf9300540..83f387b56eb2 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index 5867b4d3e394..9c24991fcfe5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter index 1af6e721bc71..bced062c1eb5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor index 8325f67a2c98..ff7805cb7764 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter index 0f6748edafc6..be7a32431b0f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter +++ b/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 diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 4e1a2fdca122..060772ac0efb 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 4e1a2fdca122deb78e7ecb815beb2d5b338433d3 +Subproject commit 060772ac0efbb9128118ae2badd35433cbca48b3 From b3498dd515e8449997e5ec12cdba2acd3bc5043a Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 5 Mar 2025 14:57:11 -0900 Subject: [PATCH 15/15] update gz submodule --- Tools/simulation/gz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 060772ac0efb..6c18846a4c7f 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 060772ac0efbb9128118ae2badd35433cbca48b3 +Subproject commit 6c18846a4c7f9fe786840a29bf4e3237f908611b