[wip] gz plugins

ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow

@@ -0,0 +1,15 @@
+#!/bin/sh
+#
+# @name Gazebo x500 with downward optical flow and distance sensor
+#
+# @type Quadrotor
+#
+
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_flow}
+
+. ${R}etc/init.d-posix/airframes/4001_gz_x500
+
+echo "Disabling Sim GPS"
+param set-default SYS_HAS_GPS 0
+param set-default SIM_GPS_USED 0
+param set-default EKF2_GPS_CTRL 0

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -91,6 +91,7 @@ px4_add_romfs_files(
 	4018_gz_quadtailsitter
 	4019_gz_x500_gimbal
 	4020_gz_tiltrotor
+	4021_gz_x500_flow
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator

@@ -87,9 +87,9 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 				. ../gz_env.sh
 			fi
 
-			# Start gazebo with default world
 			echo "INFO  [init] Starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
-			${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
+
+			${gz_command} ${gz_sub_command} --verbose=${GZ_VERBOSE:=1} -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
 
 			if [ -z "${HEADLESS}" ]; then
 				echo "INFO  [init] Starting gz gui"
@@ -124,9 +124,10 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 			echo "INFO  [init] Spawning model at position: ${pos_x} ${pos_y} ${pos_z}"
 		fi
 
+		echo "INFO  [init] Spawning model"
 		# Spawn model
 		${gz_command} service -s "/world/${PX4_GZ_WORLD}/create" --reqtype gz.msgs.EntityFactory \
-				--reptype gz.msgs.Boolean --timeout 1000 \
+				--reptype gz.msgs.Boolean --timeout 5000 \
 				--req "sdf_filename: \"${PX4_GZ_MODELS}/${MODEL_NAME}\", name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false${POSE_ARG}" > /dev/null 2>&1
 
 		# Start gz_bridge
@@ -163,7 +164,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 		if [ -n "${PX4_SIM_SPEED_FACTOR}" ]; then
 			echo "INFO  [init] Setting simulation speed factor: ${PX4_SIM_SPEED_FACTOR}"
 			${gz_command} service -s "/world/${PX4_GZ_WORLD}/set_physics" --reqtype gz.msgs.Physics \
-						--reptype gz.msgs.Boolean --timeout 1000 \
+						--reptype gz.msgs.Boolean --timeout 5000 \
 						--req "real_time_factor: ${PX4_SIM_SPEED_FACTOR}" > /dev/null 2>&1
 		fi
 

boards/px4/sitl/default.px4board

@@ -50,7 +50,9 @@ CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_COMMON_SIMULATION=y
+CONFIG_MODULES_SIMULATION_GZ_MSGS=y
 CONFIG_MODULES_SIMULATION_GZ_BRIDGE=y
+CONFIG_MODULES_SIMULATION_GZ_PLUGINS=y
 CONFIG_MODULES_SIMULATION_SENSOR_AGP_SIM=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y

src/modules/simulation/gz_bridge/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+#   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
@@ -31,21 +31,42 @@
 #
 ############################################################################
 
-# Find the gz_Transport library
-# Look for GZ Ionic or Harmonic
-find_package(gz-transport NAMES gz-transport14 gz-transport13)
+if(NOT DEFINED ENV{GZ_DISTRO})
+    set(GZ_DISTRO "harmonic" CACHE STRING "Gazebo distribution to use")
+else()
+    set(GZ_DISTRO $ENV{GZ_DISTRO})
+endif()
 
-if(gz-transport_FOUND)
+# Define library version combinations for different Gazebo distributions
+# https://github.com/gazebo-tooling/gazebodistro/blob/master/collection-harmonic.yaml
+if(GZ_DISTRO STREQUAL "harmonic")
+    set(GZ_CMAKE_VERSION "3")
+    set(GZ_MSGS_VERSION "10")
+    set(GZ_TRANSPORT_VERSION "13")
+    set(GZ_PLUGIN_VERSION "2")
+    set(GZ_SIM_VERSION "8")
+    set(GZ_SENSORS_VERSION "8")
+    message(STATUS "Using Gazebo Harmonic (cmake:${GZ_CMAKE_VERSION}, msgs:${GZ_MSGS_VERSION}, transport:${GZ_TRANSPORT_VERSION})")
+elseif(GZ_DISTRO STREQUAL "ionic")
+    set(GZ_CMAKE_VERSION "4")
+    set(GZ_MSGS_VERSION "11")
+    set(GZ_TRANSPORT_VERSION "14")
+    set(GZ_PLUGIN_VERSION "3")
+    set(GZ_SIM_VERSION "9")
+    set(GZ_SENSORS_VERSION "9")
+    message(STATUS "Using Gazebo Ionic (cmake:${GZ_CMAKE_VERSION}, msgs:${GZ_MSGS_VERSION}, transport:${GZ_TRANSPORT_VERSION})")
+else()
+    message(FATAL_ERROR "Unknown Gazebo distribution: ${GZ_DISTRO}. Valid options are: harmonic or ionic")
+endif()
 
-	add_compile_options(-frtti -fexceptions)
+# Use gz-transport as litmus test for prescence of gz
+find_package(gz-transport${GZ_TRANSPORT_VERSION})
 
-	set(GZ_TRANSPORT_VER ${gz-transport_VERSION_MAJOR})
+if (gz-transport${GZ_TRANSPORT_VERSION}_FOUND)
 
-	if(GZ_TRANSPORT_VER GREATER_EQUAL 12)
-		set(GZ_TRANSPORT_LIB gz-transport${GZ_TRANSPORT_VER}::core)
-	else()
-		set(GZ_TRANSPORT_LIB ignition-transport${GZ_TRANSPORT_VER}::core)
-	endif()
+	find_package(gz-cmake${GZ_CMAKE_VERSION} REQUIRED)
+	find_package(gz-msgs${GZ_MSGS_VERSION} REQUIRED)
+	find_package(Protobuf REQUIRED)
 
 	px4_add_module(
 		MODULE modules__simulation__gz_bridge
@@ -66,11 +87,19 @@ if(gz-transport_FOUND)
 		DEPENDS
 			mixer_module
 			px4_work_queue
-			${GZ_TRANSPORT_LIB}
+			gz-transport${GZ_TRANSPORT_VERSION}::core
 		MODULE_CONFIG
 			module.yaml
 	)
 
+    target_include_directories(modules__simulation__gz_bridge
+        PUBLIC
+            ${PX4_GZ_MSGS_BINARY_DIR}
+    )
+
+	target_include_directories(modules__simulation__gz_bridge PUBLIC px4_gz_msgs)
+	target_link_libraries(modules__simulation__gz_bridge PUBLIC px4_gz_msgs)
+
 	px4_add_git_submodule(TARGET git_gz PATH "${PX4_SOURCE_DIR}/Tools/simulation/gz")
 	include(ExternalProject)
 	ExternalProject_Add(gz
@@ -82,59 +111,41 @@ if(gz-transport_FOUND)
 		USES_TERMINAL_CONFIGURE true
 		USES_TERMINAL_BUILD true
 		EXCLUDE_FROM_ALL true
-		BUILD_ALWAYS 1
 	)
 
-	set(gz_worlds
-		default
-		windy
-		baylands
-  		lawn
-		aruco
-		rover
-		walls
-	)
+	# Below we setup the build targets for our worlds and models
+	# Syntax: gz_<model_name>_<world_name>
+	# Example: gz_x500_flow_forest
+	file(GLOB gz_worlds ${PX4_SOURCE_DIR}/Tools/simulation/gz/worlds/*.sdf)
+	file(GLOB gz_airframes ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d-posix/airframes/*_gz_*)
 
-	# find corresponding airframes
-	file(GLOB gz_airframes
-	     RELATIVE ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d-posix/airframes
-	     ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d-posix/airframes/*_gz_*
-	)
-
-	# remove any .post files
 	foreach(gz_airframe IN LISTS gz_airframes)
-		if(gz_airframe MATCHES ".post")
-			list(REMOVE_ITEM gz_airframes ${gz_airframe})
-		endif()
-	endforeach()
-	list(REMOVE_DUPLICATES gz_airframes)
-
-	foreach(gz_airframe IN LISTS gz_airframes)
-		set(model_only)
-		string(REGEX REPLACE ".*_gz_" "" model_only ${gz_airframe})
+		set(model_name)
+		string(REGEX REPLACE ".*_gz_" "" model_name ${gz_airframe})
 
 		foreach(world ${gz_worlds})
 
 			get_filename_component("world_name" ${world} NAME_WE)
 
 			if(world_name STREQUAL "default")
-				add_custom_target(gz_${model_only}
-					COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_only} $<TARGET_FILE:px4>
+				add_custom_target(gz_${model_name}
+					COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_name} $<TARGET_FILE:px4>
 					WORKING_DIRECTORY ${SITL_WORKING_DIR}
 					USES_TERMINAL
-					DEPENDS px4
+					DEPENDS px4 OpticalFlowSystem
 				)
 			else()
-				add_custom_target(gz_${model_only}_${world_name}
-					COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_only} PX4_GZ_WORLD=${world_name} $<TARGET_FILE:px4>
+				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
+					DEPENDS px4 OpticalFlowSystem
 				)
 			endif()
 		endforeach()
 	endforeach()
-	# PX4_GZ_MODELS, PX4_GZ_WORLDS, GZ_SIM_RESOURCE_PATH
+
+	# Setup the environment variables: PX4_GZ_MODELS, PX4_GZ_WORLDS, GZ_SIM_RESOURCE_PATH
 	configure_file(gz_env.sh.in ${PX4_BINARY_DIR}/rootfs/gz_env.sh)
 
 endif()

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -135,6 +135,14 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
+	std::string flow_topic = "/world/" + _world_name + "/model/" + _model_name +
+				 "/link/flow_link/sensor/optical_flow/optical_flow";
+
+	if (!_node.Subscribe(flow_topic, &GZBridge::opticalFlowCallback, this)) {
+		PX4_ERR("failed to subscribe to %s", flow_topic.c_str());
+		return PX4_ERROR;
+	}
+
 	if (!_mixing_interface_esc.init(_model_name)) {
 		PX4_ERR("failed to init ESC output");
 		return PX4_ERROR;
@@ -168,6 +176,40 @@ void GZBridge::clockCallback(const gz::msgs::Clock &msg)
 	px4_clock_settime(CLOCK_MONOTONIC, &ts);
 }
 
+void GZBridge::opticalFlowCallback(const px4::msgs::OpticalFlow &flow)
+{
+	sensor_optical_flow_s msg = {};
+
+	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();
+
+	// Static data
+	device::Device::DeviceId id;
+	id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
+	id.devid_s.bus = 0;
+	id.devid_s.address = 0;
+	id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM;
+	msg.device_id = id.devid;
+
+	// 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;
+
+	// 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);
+}
+
 void GZBridge::barometerCallback(const gz::msgs::FluidPressure &msg)
 {
 	const uint64_t timestamp = hrt_absolute_time();

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -57,6 +57,7 @@
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/sensor_optical_flow.h>
 #include <uORB/topics/obstacle_distance.h>
 #include <uORB/topics/wheel_encoders.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
@@ -77,6 +78,8 @@
 #include <gz/msgs/laserscan.pb.h>
 #include <gz/msgs/stringmsg.pb.h>
 #include <gz/msgs/scene.pb.h>
+// Custom PX4 proto
+#include <opticalflow.pb.h>
 
 using namespace time_literals;
 
@@ -113,6 +116,7 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	void navSatCallback(const gz::msgs::NavSat &msg);
 	void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg);
 	void laserScanCallback(const gz::msgs::LaserScan &msg);
+	void opticalFlowCallback(const px4::msgs::OpticalFlow &image_msg);
 
 	static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU);
 
@@ -128,12 +132,12 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	uORB::Publication<vehicle_attitude_s>         _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
 	uORB::Publication<vehicle_global_position_s>  _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};
 	uORB::Publication<vehicle_local_position_s>   _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)};
-
 	uORB::PublicationMulti<sensor_gps_s>          _sensor_gps_pub{ORB_ID(sensor_gps)};
 	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<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};

src/modules/simulation/gz_bridge/Kconfig

@@ -1,6 +1,6 @@
 menuconfig MODULES_SIMULATION_GZ_BRIDGE
 	bool "gz_bridge"
 	default n
-	depends on PLATFORM_POSIX
+	depends on PLATFORM_POSIX && MODULES_SIMULATION_GZ_MSGS
 	---help---
 		Enable support for gz_bridge

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

@@ -2,5 +2,7 @@
 
 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 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

src/modules/simulation/gz_msgs/CMakeLists.txt

@@ -0,0 +1,55 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+############################################################################
+
+# message(FATAL_ERROR "JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE ")
+
+# Check for Gazebo first
+if(NOT DEFINED ENV{GZ_DISTRO})
+    set(GZ_DISTRO "harmonic" CACHE STRING "Gazebo distribution to use")
+else()
+    set(GZ_DISTRO $ENV{GZ_DISTRO})
+endif()
+
+# Set versions based on distribution
+if(GZ_DISTRO STREQUAL "harmonic")
+    set(GZ_CMAKE_VERSION "3")
+    set(GZ_MSGS_VERSION "10")
+    set(GZ_TRANSPORT_VERSION "13")
+elseif(GZ_DISTRO STREQUAL "ionic")
+    set(GZ_CMAKE_VERSION "4")
+    set(GZ_MSGS_VERSION "11")
+    set(GZ_TRANSPORT_VERSION "14")
+else()
+    message(FATAL_ERROR "Unknown Gazebo distribution: ${GZ_DISTRO}")
+endif()
+
+# Find required packages
+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)
+
+    # Generate protobuf messages
+    file(GLOB MSGS_PROTOS "${CMAKE_CURRENT_SOURCE_DIR}/*.proto")
+    PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS ${MSGS_PROTOS})
+
+    # Create library
+    add_library(px4_gz_msgs STATIC
+        ${PROTO_SRCS}
+        ${PROTO_HDRS}
+    )
+
+    target_include_directories(px4_gz_msgs
+        PUBLIC
+            ${CMAKE_CURRENT_BINARY_DIR}
+            ${Protobuf_INCLUDE_DIRS}
+    )
+    target_link_libraries(px4_gz_msgs PUBLIC ${PROTOBUF_LIBRARIES})
+
+    # Export the binary dir for other modules
+    set(PX4_GZ_MSGS_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR} CACHE INTERNAL "")
+endif()