PX4 · dakejahl · Dec 20, 2024 · Dec 21, 2024 · Dec 24, 2024 · Dec 25, 2024
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow b/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow
@@ -0,0 +1,23 @@
+#!/bin/sh
+#
+# @name Gazebo x500 lidar down
+#
+# @type Quadrotor
+#
+
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_flow}
+
+. ${R}etc/init.d-posix/airframes/4001_gz_x500
+
+param set-default EKF2_RNG_A_VMAX 3
+
+# 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
+# param set-default SYS_HAS_GPS 1
+# param set-default SIM_GPS_USED 25
+# param set-default EKF2_GPS_CTRL 7
+
+param set-default SDLOG_PROFILE 251
+
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
@@ -93,6 +93,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

diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
@@ -86,7 +86,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 
 			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=4 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
 
 			if [ -z "${HEADLESS}" ]; then
 				# HEADLESS not set, starting gui

diff --git a/Tools/simulation/gz b/Tools/simulation/gz
diff --git a/boards/ark/fpv/extras/ark_fpv_bootloader.bin b/boards/ark/fpv/extras/ark_fpv_bootloader.bin
diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board
@@ -52,6 +52,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_COMMON_SIMULATION=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

diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp
@@ -314,8 +314,11 @@ void LoggedTopics::add_thermal_calibration_topics()
 
 void LoggedTopics::add_sensor_comparison_topics()
 {
-	add_topic_multi("sensor_accel", 100, 4);
-	add_topic_multi("sensor_baro", 100, 4);
+	add_topic_multi("sensor_accel");
+	add_topic_multi("sensor_baro");
+	add_topic_multi("distance_sensor");
+	add_topic_multi("sensor_optical_flow");
+
 	add_topic_multi("sensor_gyro", 100, 4);
 	add_topic_multi("sensor_mag", 100, 4);
 }

diff --git a/src/modules/simulation/Kconfig b/src/modules/simulation/Kconfig
@@ -11,6 +11,8 @@ menu "Simulation"
         select MODULES_SIMULATION_SENSOR_MAG_SIM
         select MODULES_SIMULATION_SIMULATOR_MAVLINK
         select MODULES_SIMULATION_SIMULATOR_SIH
+        select MODULES_SIMULATION_GZ_MSG
+        select MODULES_SIMULATION_GZ_PLUGINS
         ---help---
             Enable default set of simulation modules
     rsource "*/Kconfig"

diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt
@@ -71,6 +71,9 @@ if(gz-transport_FOUND)
 			module.yaml
 	)
 
+	# TODO: this doesn't feel right
+	target_link_libraries(modules__simulation__gz_bridge PUBLIC gz_px4_messages)
+
 	px4_add_git_submodule(TARGET git_gz PATH "${PX4_SOURCE_DIR}/Tools/simulation/gz")
 	include(ExternalProject)
 	ExternalProject_Add(gz
@@ -122,14 +125,14 @@ if(gz-transport_FOUND)
 					COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_only} $<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>
 					WORKING_DIRECTORY ${SITL_WORKING_DIR}
 					USES_TERMINAL
-					DEPENDS px4
+					DEPENDS px4 OpticalFlowSystem
 				)
 			endif()
 		endforeach()

diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp
@@ -253,6 +253,14 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
+	// Camera:
+	std::string flow_topic = "/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;
@@ -277,6 +285,46 @@ int GZBridge::init()
 	return OK;
 }
 
+// TODO: change to sensor_msgs::msgs::OpticalFlow
+void GZBridge::opticalFlowCallback(const sensor_msgs::msgs::OpticalFlow &flow)
+{
+
+	// Construct SensorOpticalFlow message
+	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.08f;
+	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
+
+	// Must publish even if quality is zero to initialize flow fusion
+	_optical_flow_pub.publish(msg);
+}
+
 int GZBridge::task_spawn(int argc, char *argv[])
 {
 	const char *world_name = "default";

diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp
@@ -62,6 +62,7 @@
 #include <uORB/topics/vehicle_odometry.h>
 #include <uORB/topics/wheel_encoders.h>
 #include <uORB/topics/obstacle_distance.h>
+#include <uORB/topics/sensor_optical_flow.h>
 
 #include <gz/math.hh>
 #include <gz/msgs.hh>
@@ -75,6 +76,7 @@
 #include <gz/msgs/laserscan.pb.h>
 #include <gz/msgs/stringmsg.pb.h>
 #include <gz/msgs/scene.pb.h>
+#include <optical_flow.pb.h>
 
 using namespace time_literals;
 
@@ -116,6 +118,7 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	void navSatCallback(const gz::msgs::NavSat &nav_sat);
 	void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan);
 	void laserScanCallback(const gz::msgs::LaserScan &scan);
+	void opticalFlowCallback(const sensor_msgs::msgs::OpticalFlow &image_msg);
 
 	/**
 	 * @brief Call Entityfactory service
@@ -176,11 +179,11 @@ 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_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_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, _node_mutex};
 	GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex};

diff --git a/src/modules/simulation/gz_bridge/gz_env.sh.in b/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
diff --git a/src/modules/simulation/gz_msg/CMakeLists.txt b/src/modules/simulation/gz_msg/CMakeLists.txt
@@ -0,0 +1,24 @@
+find_package(Protobuf REQUIRED)
+
+PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS
+	optical_flow.proto
+)
+
+# add_library(gz_px4_messages STATIC
+#     ${PROTO_SRCS}
+#     ${PROTO_HDRS}
+# )
+
+px4_add_library(gz_px4_messages
+    ${PROTO_SRCS}
+    ${PROTO_HDRS}
+)
+
+target_include_directories(gz_px4_messages
+    PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
+    PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+target_link_libraries(gz_px4_messages
+    PUBLIC ${PROTOBUF_LIBRARIES}
+)
diff --git a/src/modules/simulation/gz_msg/Kconfig b/src/modules/simulation/gz_msg/Kconfig
@@ -0,0 +1,6 @@
+menuconfig MODULES_SIMULATION_GZ_MSG
+	bool "gz_msg"
+	default n
+	depends on PLATFORM_POSIX
+	---help---
+		Enable support for gz_msg
diff --git a/src/modules/simulation/gz_msg/optical_flow.proto b/src/modules/simulation/gz_msg/optical_flow.proto
@@ -0,0 +1,20 @@
+// msgs/optical_flow.proto
+syntax = "proto3";
+package sensor_msgs.msgs;
+
+message OpticalFlow {
+	// Timestamp (microseconds, since system start)
+	int64 time_usec = 1;
+
+	// Integration time
+	uint32 integration_time_us = 2;
+
+	// Integrated x-axis flow (rad)
+	float integrated_x = 3;
+
+	// Integrated y-axis flow (rad)
+	float integrated_y = 4;
+
+	// Quality of optical flow measurement (0: bad, 255: maximum quality)
+	float quality = 5;
+}
diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt
@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 3.11.0 FATAL_ERROR)
+find_package(gz-cmake3 REQUIRED)
+
+project(OpticalFlowSystem)
+
+gz_find_package(gz-plugin2 REQUIRED COMPONENTS register)
+gz_find_package(gz-sim8 REQUIRED)
+gz_find_package(gz-sensors8 REQUIRED)
+gz_find_package(gz-transport12 REQUIRED)
+find_package(OpenCV REQUIRED)
+
+include(ExternalProject)
+
+ExternalProject_Add(OpticalFlow
+    GIT_REPOSITORY https://github.com/PX4/PX4-OpticalFlow.git
+    GIT_TAG master
+    PREFIX ${CMAKE_BINARY_DIR}/OpticalFlow
+    INSTALL_DIR ${CMAKE_BINARY_DIR}/OpticalFlow/install
+    CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=<INSTALL_DIR>
+    BUILD_BYPRODUCTS ${CMAKE_BINARY_DIR}/OpticalFlow/install/lib/libOpticalFlow.so
+)
+
+ExternalProject_Get_Property(OpticalFlow install_dir)
+
+set(OpticalFlow_INCLUDE_DIRS ${install_dir}/include)
+set(OpticalFlow_LIBS ${install_dir}/lib/libOpticalFlow.so)
+
+add_library(${PROJECT_NAME} SHARED
+    OpticalFlowSensor.cpp
+    OpticalFlowSystem.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+    PUBLIC gz_px4_messages
+    PUBLIC gz-sensors8::gz-sensors8
+    PUBLIC gz-rendering8::gz-rendering8
+    PUBLIC gz-plugin2::gz-plugin2
+    PUBLIC gz-sim8::gz-sim8
+    PUBLIC gz-transport12::gz-transport12
+    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}
+)
+
+add_dependencies(${PROJECT_NAME} OpticalFlow)
diff --git a/src/modules/simulation/gz_plugins/Kconfig b/src/modules/simulation/gz_plugins/Kconfig
@@ -0,0 +1,6 @@
+menuconfig MODULES_SIMULATION_GZ_PLUGINS
+	bool "gz_plugin"
+	default n
+	depends on PLATFORM_POSIX
+	---help---
+		Enable support for gz_plugin
+ −		models/arucotag/tiled_meseeks_small.png
+11 −0		models/optical_flow/model.config
+79 −0		models/optical_flow/model.sdf
+2 −2		models/x500_base/model.sdf
+11 −0		models/x500_flow/model.config
+72 −0		models/x500_flow/model.sdf
+9 −3		worlds/aruco.sdf