Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 20:03:54 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

[wip] gz plugins (#24153)

Browse Source 
* added optical flow to gz bridge

* log high rate sensor data

* it builds

* it builds and publishes, need to figure out build system now

* single library

* rename files

* add gz_msg for proto, fix build, test basic flow impl

* update rate, no blur

* PX4-OpticalFlow impl

* rename OpticalFlowSensor

* rename plugins

* disable gps, add plugin path

* cleanup

* fix plugin path export

* properly add OpticalFlowSystem dependency to gz

* move everything under gz_bridge

* cleanup

* add GZ_VEBOSE

* cleanup model/world build target cmake

* added GZ_DISTRO env, harmonic or ionic

* fix gz transport, unstage ark fpv bootloader

* unstage logged_topics.cpp

* cleanup

* make format

* ci fixes

* fix cmake

* remove required for gz-transport

* use model/world namespace for multi vehicle sim. Make format

* make format

* license

* remove needless member var

* made separate Kconfig for gz_msgs, gz_plugins, and gz_bridge

* move OpticalFlow build to it's own cmake

* fix clang

* cleanup comments

* fix rebase
This commit is contained in:
Jacob Dahl
2025-03-03 12:21:28 -09:00
committed by GitHub
parent 3b2d74b017
commit 6dc39d9deb
20 changed files with 764 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow
+15
View File
@@ -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
+1
View File
@@ -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
+5 -4
View File
@@ -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
Tools/simulation/gz
+1 -1
Submodule Tools/simulation/gz updated: 23170a9125...5bbae38b4f
boards/px4/sitl/default.px4board
+2
View File
@@ -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
+57 -46
View File
@@ -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
)
# 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)
# 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_*)
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
+42
View File
@@ -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
+5 -1
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_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 @@ 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);
static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU);
@@ -128,12 +132,12 @@ private:
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 -1
View File
@@ -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
View File
@@ -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
+55
View File
@@ -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()
src/modules/simulation/gz_msgs/Kconfig
+6
View File
@@ -0,0 +1,6 @@
menuconfig MODULES_SIMULATION_GZ_MSGS
bool "gz_msgs"
default n
depends on PLATFORM_POSIX
---help---
Enable proto generation for custom PX4 messages
src/modules/simulation/gz_msgs/opticalflow.proto
+19
View File
@@ -0,0 +1,19 @@
syntax = "proto3";
package px4.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;
}
src/modules/simulation/gz_plugins/CMakeLists.txt
+70
View File
@@ -0,0 +1,70 @@
project(OpticalFlowSystem)
if(NOT DEFINED ENV{GZ_DISTRO})
set(GZ_DISTRO "harmonic" CACHE STRING "Gazebo distribution to use")
else()
set(GZ_DISTRO $ENV{GZ_DISTRO})
endif()
# 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()
# Use gz-transport as litmus test for prescence of gz
find_package(gz-transport${GZ_TRANSPORT_VERSION})
if (gz-transport${GZ_TRANSPORT_VERSION}_FOUND)
gz_find_package(gz-cmake${GZ_CMAKE_VERSION} REQUIRED)
gz_find_package(gz-msgs${GZ_MSGS_VERSION} REQUIRED)
gz_find_package(Protobuf REQUIRED)
gz_find_package(gz-plugin${GZ_PLUGIN_VERSION} REQUIRED COMPONENTS register)
gz_find_package(gz-sim${GZ_SIM_VERSION} REQUIRED)
gz_find_package(gz-sensors${GZ_SENSORS_VERSION} REQUIRED)
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)
endif()
src/modules/simulation/gz_plugins/Kconfig
+6
View File
@@ -0,0 +1,6 @@
menuconfig MODULES_SIMULATION_GZ_PLUGINS
bool "gz_plugins"
default n
depends on PLATFORM_POSIX && MODULES_SIMULATION_GZ_MSGS
---help---
Enable support for gz_plugins
src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp
+165
View File
@@ -0,0 +1,165 @@
/****************************************************************************
*
* 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 <gz/common/Console.hh>
#include <gz/msgs/Utility.hh>
#include <gz/sensors/Util.hh>
#include "OpticalFlowSensor.hpp"
#include "opticalflow.pb.h"
using namespace custom;
bool OpticalFlowSensor::Load(const sdf::Sensor &_sdf)
{
auto type = gz::sensors::customType(_sdf);
if ("optical_flow" != type) {
gzerr << "Trying to load [optical_flow] sensor, but got type [" << type << "] instead." << std::endl;
return false;
}
gz::sensors::Sensor::Load(_sdf);
std::string output_topic = this->Topic();
_publisher = _node.Advertise<px4::msgs::OpticalFlow>(output_topic);
gzdbg << "Advertising optical flow data on: " << output_topic << std::endl;
std::string camera_topic = output_topic;
size_t last_segment = camera_topic.rfind("/optical_flow/optical_flow");
if (last_segment != std::string::npos) {
camera_topic = camera_topic.substr(0, last_segment) + "/flow_camera/image";
}
int image_width = 0;
int image_height = 0;
int update_rate = 0;
float hfov = 0;
auto sensorElem = _sdf.Element()->GetParent()->GetElement("sensor");
while (sensorElem) {
if (sensorElem->Get<std::string>("name") == "flow_camera") {
auto cameraElem = sensorElem->GetElement("camera");
update_rate = sensorElem->GetElement("update_rate")->Get<int>();
hfov = cameraElem->GetElement("horizontal_fov")->Get<double>();
auto imageElem = cameraElem->GetElement("image");
image_width = imageElem->GetElement("width")->Get<int>();
image_height = imageElem->GetElement("height")->Get<int>();
break;
}
sensorElem = sensorElem->GetNextElement("sensor");
}
gzdbg << "Camera parameters:" << std::endl
<< " image_width: " << image_width << std::endl
<< " image_height: " << image_height << std::endl
<< " update_rate: " << update_rate << std::endl
<< " hfov: " << hfov << std::endl;
gzdbg << "Subscribing to camera topic for flow: " << camera_topic << std::endl;
if (!_node.Subscribe(camera_topic, &OpticalFlowSensor::OnImage, this)) {
gzerr << "Failed to subscribe to camera topic: " << camera_topic << std::endl;
return false;
}
// Assume pinhole camera and 1:1 aspect ratio
float focal_length = (image_width / 2.0f) / tan(hfov / 2.0f);
_optical_flow = std::make_shared<OpticalFlowOpenCV>(focal_length, focal_length,
update_rate, image_width, image_height);
return true;
}
void OpticalFlowSensor::OnImage(const gz::msgs::Image &image_msg)
{
if (image_msg.width() == 0 || image_msg.height() == 0) {
gzerr << "Invalid image dimensions" << std::endl;
return;
}
if (image_msg.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8) {
cv::Mat temp(image_msg.height(), image_msg.width(), CV_8UC3);
std::memcpy(temp.data, image_msg.data().c_str(), image_msg.data().size());
cv::cvtColor(temp, _last_image_gray, cv::COLOR_RGB2GRAY);
} else if (image_msg.pixel_format_type() == gz::msgs::PixelFormatType::L_INT8) {
std::memcpy(_last_image_gray.data, image_msg.data().c_str(), image_msg.data().size());
} else {
gzerr << "Unsupported image format" << std::endl;
return;
}
uint32_t current_timestamp = (image_msg.header().stamp().sec() * 1000000ULL +
image_msg.header().stamp().nsec() / 1000ULL) & 0xFFFFFFFF;
if (_last_image_timestamp != 0) {
_integration_time_us = (current_timestamp - _last_image_timestamp) & 0xFFFFFFFF;
}
_last_image_timestamp = current_timestamp;
_new_image_available = true;
}
bool OpticalFlowSensor::Update(const std::chrono::steady_clock::duration &_now)
{
if (!_new_image_available) {
return true;
}
px4::msgs::OpticalFlow msg;
msg.set_time_usec(_last_image_timestamp);
float flow_x = 0.f;
float flow_y = 0.f;
int quality = _optical_flow->calcFlow(_last_image_gray.data, _last_image_timestamp,
_integration_time_us, flow_x, flow_y);
msg.set_integrated_x(flow_x);
msg.set_integrated_y(flow_y);
msg.set_integration_time_us(_integration_time_us);
msg.set_quality(quality);
if (!_publisher.Publish(msg)) {
gzwarn << "Failed to publish optical flow message" << std::endl;
}
_new_image_available = false;
return true;
}
src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp
+75
View File
@@ -0,0 +1,75 @@
/****************************************************************************
*
* 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/sensors/Sensor.hh>
#include <gz/sensors/CameraSensor.hh>
#include <gz/sensors/SensorTypes.hh>
#include <gz/transport/Node.hh>
#include <gz/msgs/image.pb.h>
#include <opencv2/opencv.hpp>
#include <numeric>
#include <vector>
#include <memory>
#include "flow_opencv.hpp"
namespace custom
{
class OpticalFlowSensor : public gz::sensors::Sensor
{
public:
virtual bool Load(const sdf::Sensor &_sdf) override;
virtual bool Update(const std::chrono::steady_clock::duration &_now) override;
private:
void OnImage(const gz::msgs::Image &_msg);
gz::transport::Node _node;
gz::transport::Node::Publisher _publisher;
// Flow
std::shared_ptr<OpticalFlowOpenCV> _optical_flow {nullptr};
int _integration_time_us;
// Camera
double _horizontal_fov {0.0};
double _vertical_fov {0.0};
cv::Mat _last_image_gray;
uint32_t _last_image_timestamp {0};
bool _new_image_available {false};
};
} // end namespace custom
src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp
+125
View File
@@ -0,0 +1,125 @@
/****************************************************************************
*
* 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 <string>
#include <unordered_map>
#include <utility>
#include <gz/plugin/Register.hh>
#include <gz/sensors/SensorFactory.hh>
#include <sdf/Sensor.hh>
#include <gz/sim/components/CustomSensor.hh>
#include <gz/sim/components/Name.hh>
#include <gz/sim/components/ParentEntity.hh>
#include <gz/sim/components/Sensor.hh>
#include <gz/sim/EntityComponentManager.hh>
#include <gz/sim/Util.hh>
#include "OpticalFlowSensor.hpp"
#include "OpticalFlowSystem.hpp"
using namespace custom;
void OpticalFlowSystem::PreUpdate(const gz::sim::UpdateInfo &, gz::sim::EntityComponentManager &_ecm)
{
// Register each new custom sensor
_ecm.EachNew<gz::sim::components::CustomSensor, gz::sim::components::ParentEntity>(
[&](const gz::sim::Entity & _entity,
const gz::sim::components::CustomSensor * _custom,
const gz::sim::components::ParentEntity * _parent)->bool {
auto sensorScopedName = gz::sim::removeParentScope(gz::sim::scopedName(_entity, _ecm, "::", false), "::");
sdf::Sensor data = _custom->Data();
data.SetName(sensorScopedName);
if (data.Topic().empty())
{
std::string topic = scopedName(_entity, _ecm) + "/optical_flow";
data.SetTopic(topic);
}
gz::sensors::SensorFactory sensorFactory;
auto sensor = sensorFactory.CreateSensor<custom::OpticalFlowSensor>(data);
if (sensor == nullptr)
{
gzerr << "Failed to create optical flow sensor [" << sensorScopedName << "]" << std::endl;
return false;
}
auto parentName = _ecm.Component<gz::sim::components::Name>(_parent->Data())->Data();
sensor->SetParent(parentName);
_ecm.CreateComponent(_entity, gz::sim::components::SensorTopic(sensor->Topic()));
this->entitySensorMap.insert(std::make_pair(_entity, std::move(sensor)));
gzdbg << "OpticalFlowSystem PreUpdate" << std::endl;
return true;
});
}
void OpticalFlowSystem::PostUpdate(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm)
{
if (!_info.paused) {
for (auto &[entity, sensor] : this->entitySensorMap) {
sensor->Update(_info.simTime);
}
}
this->RemoveSensorEntities(_ecm);
}
void OpticalFlowSystem::RemoveSensorEntities(const gz::sim::EntityComponentManager &_ecm)
{
_ecm.EachRemoved<gz::sim::components::CustomSensor>(
[&](const gz::sim::Entity & _entity,
const gz::sim::components::CustomSensor *)->bool {
if (this->entitySensorMap.erase(_entity) == 0)
{
gzerr << "Internal error, missing optical flow sensor for entity ["
<< _entity << "]" << std::endl;
}
return true;
});
}
GZ_ADD_PLUGIN(OpticalFlowSystem, gz::sim::System,
OpticalFlowSystem::ISystemPreUpdate,
OpticalFlowSystem::ISystemPostUpdate
)
GZ_ADD_PLUGIN_ALIAS(OpticalFlowSystem, "custom::OpticalFlowSystem")
src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp
+59
View File
@@ -0,0 +1,59 @@
/****************************************************************************
*
* 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/sensors/Sensor.hh>
#include <gz/transport/Node.hh>
namespace custom
{
class OpticalFlowSystem:
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:
void RemoveSensorEntities(const gz::sim::EntityComponentManager &_ecm);
std::unordered_map<gz::sim::Entity, std::shared_ptr<OpticalFlowSensor>> entitySensorMap;
};
} // end namespace custom
src/modules/simulation/gz_plugins/optical_flow.cmake
+53
View File
@@ -0,0 +1,53 @@
############################################################################
#
# 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(ExternalProject)
find_package(OpenCV REQUIRED)
if(NOT TARGET OpticalFlow)
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
UPDATE_DISCONNECTED ON
BUILD_ALWAYS OFF
STEP_TARGETS build
)
ExternalProject_Get_Property(OpticalFlow install_dir)
set(OpticalFlow_INCLUDE_DIRS ${install_dir}/include CACHE INTERNAL "")
set(OpticalFlow_LIBS ${install_dir}/lib/libOpticalFlow.so CACHE INTERNAL "")
endif()