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 new file mode 100644 index 0000000000..5f0aababbd --- /dev/null +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 4ae56f51e9..b4e107799f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index ede64dcd34..b396115491 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/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 diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 23170a9125..5bbae38b4f 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 23170a91255d99aea8960d1101541afce0f209d9 +Subproject commit 5bbae38b4f942521b4f3288c298083571ea5718c diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index b0b7a8a2b3..8eb8ebec8c 100644 --- a/boards/px4/sitl/default.px4board +++ b/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 diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index e57c602045..784c5a5ca8 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/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 - ) - - # 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__ + # 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} $ + add_custom_target(gz_${model_name} + COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_name} $ 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} $ + add_custom_target(gz_${model_name}_${world_name} + COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_name} PX4_GZ_WORLD=${world_name} $ WORKING_DIRECTORY ${SITL_WORKING_DIR} USES_TERMINAL - DEPENDS px4 + 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() diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index bb2e7282da..c890ad8d5c 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/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(); diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 5e157f43b2..6128e8c175 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -77,6 +78,8 @@ #include #include #include +// Custom PX4 proto +#include 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 _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; - uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; uORB::PublicationMulti _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; + uORB::PublicationMulti _optical_flow_pub{ORB_ID(sensor_optical_flow)}; GZMixingInterfaceESC _mixing_interface_esc{_node}; GZMixingInterfaceServo _mixing_interface_servo{_node}; diff --git a/src/modules/simulation/gz_bridge/Kconfig b/src/modules/simulation/gz_bridge/Kconfig index 0d4ab457d1..dd34a251b3 100644 --- a/src/modules/simulation/gz_bridge/Kconfig +++ b/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 diff --git a/src/modules/simulation/gz_bridge/gz_env.sh.in b/src/modules/simulation/gz_bridge/gz_env.sh.in index 810bc88948..3267318bfc 100644 --- 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_msgs/CMakeLists.txt b/src/modules/simulation/gz_msgs/CMakeLists.txt new file mode 100644 index 0000000000..c74c010f91 --- /dev/null +++ b/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() diff --git a/src/modules/simulation/gz_msgs/Kconfig b/src/modules/simulation/gz_msgs/Kconfig new file mode 100644 index 0000000000..65b2a6453b --- /dev/null +++ b/src/modules/simulation/gz_msgs/Kconfig @@ -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 diff --git a/src/modules/simulation/gz_msgs/opticalflow.proto b/src/modules/simulation/gz_msgs/opticalflow.proto new file mode 100644 index 0000000000..c03a96b9a8 --- /dev/null +++ b/src/modules/simulation/gz_msgs/opticalflow.proto @@ -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; +} diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt new file mode 100644 index 0000000000..c4400b0204 --- /dev/null +++ b/src/modules/simulation/gz_plugins/CMakeLists.txt @@ -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() diff --git a/src/modules/simulation/gz_plugins/Kconfig b/src/modules/simulation/gz_plugins/Kconfig new file mode 100644 index 0000000000..eae335fbc5 --- /dev/null +++ b/src/modules/simulation/gz_plugins/Kconfig @@ -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 diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp b/src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp new file mode 100644 index 0000000000..4c0dac72d7 --- /dev/null +++ b/src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp @@ -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 +#include +#include + +#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(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("name") == "flow_camera") { + auto cameraElem = sensorElem->GetElement("camera"); + update_rate = sensorElem->GetElement("update_rate")->Get(); + hfov = cameraElem->GetElement("horizontal_fov")->Get(); + + auto imageElem = cameraElem->GetElement("image"); + image_width = imageElem->GetElement("width")->Get(); + image_height = imageElem->GetElement("height")->Get(); + 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(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; +} diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp b/src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp new file mode 100644 index 0000000000..497fd79276 --- /dev/null +++ b/src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include + +#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 _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 diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp b/src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp new file mode 100644 index 0000000000..3459f8a386 --- /dev/null +++ b/src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp @@ -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 +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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( + [&](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(data); + + if (sensor == nullptr) + { + gzerr << "Failed to create optical flow sensor [" << sensorScopedName << "]" << std::endl; + return false; + } + + auto parentName = _ecm.Component(_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( + [&](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") diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp b/src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp new file mode 100644 index 0000000000..eecaaa2f6f --- /dev/null +++ b/src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp @@ -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 +#include +#include + +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> entitySensorMap; +}; +} // end namespace custom diff --git a/src/modules/simulation/gz_plugins/optical_flow.cmake b/src/modules/simulation/gz_plugins/optical_flow.cmake new file mode 100644 index 0000000000..8ac1c1f649 --- /dev/null +++ b/src/modules/simulation/gz_plugins/optical_flow.cmake @@ -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= + 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()