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 3923b44cb4..2add7a5cb4 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/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna index ea7b3a2f03..96dd5fa151 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 69b0cec559..5bd432abde 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 e988f1f9b0..4370a677ed 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 8479f2e38c..8ac407bac4 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 5ff39b3f96..2d36aed7ce 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 a56cf93005..83f387b56e 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 5867b4d3e3..9c24991fcf 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 1af6e721bc..bced062c1e 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 8325f67a2c..ff7805cb77 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 0f6748edaf..be7a32431b 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 5bbae38b4f..6c18846a4c 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 5bbae38b4f942521b4f3288c298083571ea5718c +Subproject commit 6c18846a4c7f9fe786840a29bf4e3237f908611b diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 784c5a5ca8..5fdc455cc8 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/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index c890ad8d5c..6cf1734da5 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(), @@ -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{}; diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 6128e8c175..ac096744c2 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,12 +117,13 @@ private: 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 @@ private: 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/gz_env.sh.in b/src/modules/simulation/gz_bridge/gz_env.sh.in index 3267318bfc..bff7180b03 100644 --- a/src/modules/simulation/gz_bridge/gz_env.sh.in +++ b/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 diff --git a/src/modules/simulation/gz_bridge/server.config b/src/modules/simulation/gz_bridge/server.config new file mode 100644 index 0000000000..93bff3f442 --- /dev/null +++ b/src/modules/simulation/gz_bridge/server.config @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + ogre2 + + + + + diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt index c4400b0204..4bb3307108 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,43 +61,26 @@ 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) + 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) - 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) + # 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}) - 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 - ) - - 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/optical_flow/CMakeLists.txt b/src/modules/simulation/gz_plugins/optical_flow/CMakeLists.txt new file mode 100644 index 0000000000..2a57ba3c25 --- /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 0000000000..f5e2935ef6 --- /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) 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 0000000000..dda5fbcbf5 --- /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 0000000000..ce07329224 --- /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 +} 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 0000000000..840dec1c33 --- /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 diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp index 7ee92ad750..18c4adbf1d 100644 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp +++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp @@ -78,7 +78,7 @@ private: 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};