fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

gz: use server config file for loading world plugins (#24441)

Browse Source 
* gz: use server config file for loading world plugins

* submodule

* use server.config in tree

* newlines

* format

* gzbridge: rename function

* format

* gzbridge: add magnetometer callback

* change gz_find_package to find_package

* fix up directory structure and cmake to allow multiple plugins

* newlines

* add comment block explaining gz_env.sh

* remove dupe readme

* remove SENS_EN_MAGSIM from all gz airframe files except spacecraft

* update gz submodule
This commit is contained in:
Jacob Dahl 2025-03-05 15:37:16 -09:00 committed by GitHub
parent 0ab3e45c13
commit ea8bcd9cef
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
29 changed files with 425 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500
View File

@ -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

3
ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol
View File

@ -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

2
ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane
View File

@ -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

3
ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover
View File

@ -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

2
ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower
View File

@ -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

3
ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann
View File

@ -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

2
ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter
View File

@ -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

2
ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter
View File

@ -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

2
Tools/simulation/gz

@ -1 +1 @@
Subproject commit 5bbae38b4f942521b4f3288c298083571ea5718c
Subproject commit 6c18846a4c7f9fe786840a29bf4e3237f908611b

4
src/modules/simulation/gz_bridge/CMakeLists.txt
View File

@ -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()

75
src/modules/simulation/gz_bridge/GZBridge.cpp
View File

@ -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{};

10
src/modules/simulation/gz_bridge/GZBridge.hpp
View File

@ -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 @@ 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_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};

13
src/modules/simulation/gz_bridge/gz_env.sh.in
View File

@ -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

19
src/modules/simulation/gz_bridge/server.config Normal file
View File

@ -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>

84
src/modules/simulation/gz_plugins/CMakeLists.txt
View File

@ -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()

65
src/modules/simulation/gz_plugins/optical_flow/CMakeLists.txt Normal file
View File

@ -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)

0
src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp → src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.cpp
View File

0
src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp → src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.hpp
View File

0
src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp → src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.cpp
View File

0
src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp → src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.hpp
View File

0
src/modules/simulation/gz_plugins/optical_flow.cmake → src/modules/simulation/gz_plugins/optical_flow/optical_flow.cmake
View File

68
src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt Normal file
View File

@ -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)

30
src/modules/simulation/gz_plugins/template_plugin/README.md Normal file
View File

@ -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 entity_name="*" entity_type="world" filename="libYourPluginSystem.so" name="custom::YourPluginSystem"/>
```
## 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.

58
src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp Normal file
View File

@ -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 <gz/plugin/Register.hh>
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
}

57
src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.hpp Normal file
View File

@ -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 <gz/sim/System.hh>
#include <gz/transport/Node.hh>
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

2
src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp
View File

@ -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};