From 5636ddacab9aacb08f55dabd489ddbb95afef7f0 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Fri, 20 Dec 2024 14:31:36 -0900 Subject: [PATCH 01/16] added optical flow to gz bridge --- .../init.d-posix/airframes/4021_gz_x500_flow | 19 +++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + Tools/simulation/gz | 2 +- .../simulation/gz_bridge/CMakeLists.txt | 3 + src/modules/simulation/gz_bridge/GZBridge.cpp | 57 +++++++++++++ src/modules/simulation/gz_bridge/GZBridge.hpp | 4 + .../gz_bridge/camera/CMakeLists.txt | 83 +++++++++++++++++++ .../simulation/gz_bridge/camera/gz_camera.cpp | 35 ++++++++ .../simulation/gz_bridge/camera/gz_camera.hpp | 6 ++ 9 files changed, 209 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow create mode 100644 src/modules/simulation/gz_bridge/camera/CMakeLists.txt create mode 100644 src/modules/simulation/gz_bridge/camera/gz_camera.cpp create mode 100644 src/modules/simulation/gz_bridge/camera/gz_camera.hpp 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 000000000000..e1713d2c399b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow @@ -0,0 +1,19 @@ +#!/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 sats" +param set-default SYS_HAS_GPS 0 +param set-default SIM_GPS_USED 0 + +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 index da741d2a64db..921b7d4d67b0 100644 --- 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/Tools/simulation/gz b/Tools/simulation/gz index 230450cc817d..37f58d9dc28b 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 230450cc817dd7675612ed5ec72ee59b6989d367 +Subproject commit 37f58d9dc28b2719b3cf81bfc49f90990dfb43d3 diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index e57c6020451b..e692eb0964db 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -31,6 +31,8 @@ # ############################################################################ +add_subdirectory(camera) + # Find the gz_Transport library # Look for GZ Ionic or Harmonic find_package(gz-transport NAMES gz-transport14 gz-transport13) @@ -66,6 +68,7 @@ if(gz-transport_FOUND) DEPENDS mixer_module px4_work_queue + gz_camera ${GZ_TRANSPORT_LIB} MODULE_CONFIG module.yaml diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 2dc1ef13e4f4..c9871af4367b 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -43,6 +43,8 @@ #include #include +#include "camera/gz_camera.hpp" + GZBridge::GZBridge(const char *world, const char *name, const char *model, const char *pose_str) : ModuleParams(nullptr), @@ -253,6 +255,14 @@ int GZBridge::init() return PX4_ERROR; } + // Camera: + std::string camera_topic = "/camera"; + + if (!_node.Subscribe(camera_topic, &GZBridge::cameraCallback, this)) { + PX4_ERR("failed to subscribe to %s", camera_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 +287,53 @@ int GZBridge::init() return OK; } +void GZBridge::cameraCallback(const gz::msgs::Image &image_msg) +{ + float flow_x = 0; + float flow_y = 0; + int integration_time = 0; + int quality = calculate_flow(image_msg, hrt_absolute_time(), integration_time, flow_x, flow_y); + + if (quality <= 0) { + quality = 0; + } + + // Construct SensorOpticalFlow message + sensor_optical_flow_s msg = {}; + + msg.pixel_flow[0] = flow_x; + msg.pixel_flow[1] = flow_y; + msg.quality = quality; + msg.integration_timespan_us = integration_time; + // msg.integration_timespan_us = {1000000 / 30}; // 30 fps; + + // Static data + msg.timestamp = hrt_absolute_time(); + msg.timestamp_sample = msg.timestamp; + 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.max_ground_distance = 30; + 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 index 27ef4249564e..ba91faf5bb5f 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -62,6 +62,7 @@ #include #include #include +#include #include #include @@ -116,6 +117,7 @@ class GZBridge : public ModuleBase, 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 cameraCallback(const gz::msgs::Image &image_msg); /** * @brief Call Entityfactory service @@ -182,6 +184,8 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S 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, _node_mutex}; GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex}; GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex}; diff --git a/src/modules/simulation/gz_bridge/camera/CMakeLists.txt b/src/modules/simulation/gz_bridge/camera/CMakeLists.txt new file mode 100644 index 000000000000..40bd37c262db --- /dev/null +++ b/src/modules/simulation/gz_bridge/camera/CMakeLists.txt @@ -0,0 +1,83 @@ +############################################################################ +# +# Copyright (c) 2019 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. +# +############################################################################ + +find_package(gz-transport NAMES gz-transport13) +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= + BUILD_BYPRODUCTS ${CMAKE_BINARY_DIR}/OpticalFlow/install/lib/libOpticalFlow.so +) + +ExternalProject_Get_Property(OpticalFlow install_dir) + +message(WARNING "OpticalFlow install dir: ${install_dir}") + +set(OpticalFlow_INCLUDE_DIRS ${install_dir}/include) + +# If Harmonic not found, look for GZ Garden libraries +if(NOT gz-transport_FOUND) + find_package(gz-transport NAMES gz-transport12) +endif() + +if(gz-transport_FOUND) + + add_compile_options(-frtti -fexceptions) + + set(GZ_TRANSPORT_VER ${gz-transport_VERSION_MAJOR}) + + 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() + + px4_add_library(gz_camera + gz_camera.hpp + gz_camera.cpp + ) + + set(OpticalFlow_LIBS ${install_dir}/lib/libOpticalFlow.so) + + target_compile_options(gz_camera PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) + target_include_directories(gz_camera PRIVATE ${CMAKE_CURRENT_BINARY_DIR} ${OpenCV_INCLUDE_DIRS} ${OpticalFlow_INCLUDE_DIRS}) + target_link_libraries(gz_camera PRIVATE ${GZ_TRANSPORT_LIB} ${OpenCV_LIBS} ${OpticalFlow_LIBS}) + + add_dependencies(gz_camera OpticalFlow) +endif() diff --git a/src/modules/simulation/gz_bridge/camera/gz_camera.cpp b/src/modules/simulation/gz_bridge/camera/gz_camera.cpp new file mode 100644 index 000000000000..f77d574be1e3 --- /dev/null +++ b/src/modules/simulation/gz_bridge/camera/gz_camera.cpp @@ -0,0 +1,35 @@ +#include "gz_camera.hpp" + +#include +#include +#include + +// #include +#include "flow_opencv.hpp" +#include + +OpticalFlowOpenCV *_optical_flow = nullptr; +int _dt_us = 0; + +int calculate_flow(const gz::msgs::Image &image_msg, uint64_t sim_time, int &integration_time, float &flow_x, + float &flow_y) +{ + if (!_optical_flow) { + float hfov = 1.74; + int output_rate = 30; + int image_width = image_msg.width(); + int image_height = image_msg.height(); + float focal_length = (image_width / 2.0f) / tan(hfov / 2.0f); + + _optical_flow = new OpticalFlowOpenCV(focal_length, focal_length, output_rate, image_width, image_height); + } + + cv::Mat image = cv::Mat(image_msg.height(), image_msg.width(), CV_8UC3, (void *)image_msg.data().c_str()); + + cv::Mat gray_image; + cv::cvtColor(image, gray_image, cv::COLOR_RGB2GRAY); + + int quality = _optical_flow->calcFlow(gray_image.data, sim_time, integration_time, flow_x, flow_y); + + return quality; +} diff --git a/src/modules/simulation/gz_bridge/camera/gz_camera.hpp b/src/modules/simulation/gz_bridge/camera/gz_camera.hpp new file mode 100644 index 000000000000..05a88fb71822 --- /dev/null +++ b/src/modules/simulation/gz_bridge/camera/gz_camera.hpp @@ -0,0 +1,6 @@ +#pragma once + +#include + +int calculate_flow(const gz::msgs::Image &image_msg, uint64_t sim_time, int &integration_time, float &flow_x, + float &flow_y); From 33c029c166fb93ed8b6ca103f86b0a09c45d0b1e Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Fri, 20 Dec 2024 16:28:34 -0900 Subject: [PATCH 02/16] log high rate sensor data --- .../px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow | 3 +-- Tools/simulation/gz | 2 +- src/modules/logger/logged_topics.cpp | 7 +++++-- 3 files changed, 7 insertions(+), 5 deletions(-) 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 index e1713d2c399b..b2b993352d2c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow @@ -11,9 +11,8 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_flow} param set-default EKF2_RNG_A_VMAX 3 -echo "disabling Sim GPS sats" +echo "disabling Sim GPS" param set-default SYS_HAS_GPS 0 param set-default SIM_GPS_USED 0 - param set-default SDLOG_PROFILE 251 diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 37f58d9dc28b..5f6aeff9547f 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 37f58d9dc28b2719b3cf81bfc49f90990dfb43d3 +Subproject commit 5f6aeff9547fe0bb183e30eff0404670538c4e7a diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index d1e67a5cd7c9..49bd4f472e11 100644 --- 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); } From 83a7c6817394d0294c60deff7a2003c6b311e42e Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 24 Dec 2024 12:49:45 -0900 Subject: [PATCH 03/16] it builds --- .../init.d-posix/airframes/4021_gz_x500_flow | 2 + boards/px4/sitl/default.px4board | 1 + .../simulation/gz_bridge/CMakeLists.txt | 4 +- src/modules/simulation/gz_bridge/GZBridge.cpp | 100 ++++++------- src/modules/simulation/gz_bridge/GZBridge.hpp | 2 +- .../simulation/gz_bridge/camera/gz_camera.cpp | 5 +- .../simulation/gz_plugin/CMakeLists.txt | 27 ++++ src/modules/simulation/gz_plugin/Kconfig | 6 + .../simulation/gz_plugin/OdometerSystem.cc | 132 ++++++++++++++++++ .../simulation/gz_plugin/OdometerSystem.hh | 56 ++++++++ .../gz_plugin/msgs/optical_flow.proto | 41 ++++++ .../gz_plugin/sensors/CMakeLists.txt | 10 ++ .../simulation/gz_plugin/sensors/Odometer.cc | 109 +++++++++++++++ .../simulation/gz_plugin/sensors/Odometer.hh | 68 +++++++++ 14 files changed, 509 insertions(+), 54 deletions(-) create mode 100644 src/modules/simulation/gz_plugin/CMakeLists.txt create mode 100644 src/modules/simulation/gz_plugin/Kconfig create mode 100644 src/modules/simulation/gz_plugin/OdometerSystem.cc create mode 100644 src/modules/simulation/gz_plugin/OdometerSystem.hh create mode 100644 src/modules/simulation/gz_plugin/msgs/optical_flow.proto create mode 100644 src/modules/simulation/gz_plugin/sensors/CMakeLists.txt create mode 100644 src/modules/simulation/gz_plugin/sensors/Odometer.cc create mode 100644 src/modules/simulation/gz_plugin/sensors/Odometer.hh 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 index b2b993352d2c..95efb9c1426c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow @@ -14,5 +14,7 @@ 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 SDLOG_PROFILE 251 diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 1584f93ef647..ed93517d80dc 100644 --- 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_PLUGIN=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 e692eb0964db..cead68d0d05a 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -add_subdirectory(camera) +# add_subdirectory(camera) # Find the gz_Transport library # Look for GZ Ionic or Harmonic @@ -68,7 +68,7 @@ if(gz-transport_FOUND) DEPENDS mixer_module px4_work_queue - gz_camera + # gz_camera ${GZ_TRANSPORT_LIB} MODULE_CONFIG module.yaml diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index c9871af4367b..b5e8da4f0822 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -258,10 +258,10 @@ int GZBridge::init() // Camera: std::string camera_topic = "/camera"; - if (!_node.Subscribe(camera_topic, &GZBridge::cameraCallback, this)) { - PX4_ERR("failed to subscribe to %s", camera_topic.c_str()); - return PX4_ERROR; - } + // if (!_node.Subscribe(camera_topic, &GZBridge::cameraCallback, this)) { + // PX4_ERR("failed to subscribe to %s", camera_topic.c_str()); + // return PX4_ERROR; + // } if (!_mixing_interface_esc.init(_model_name)) { PX4_ERR("failed to init ESC output"); @@ -287,52 +287,52 @@ int GZBridge::init() return OK; } -void GZBridge::cameraCallback(const gz::msgs::Image &image_msg) -{ - float flow_x = 0; - float flow_y = 0; - int integration_time = 0; - int quality = calculate_flow(image_msg, hrt_absolute_time(), integration_time, flow_x, flow_y); - - if (quality <= 0) { - quality = 0; - } - - // Construct SensorOpticalFlow message - sensor_optical_flow_s msg = {}; - - msg.pixel_flow[0] = flow_x; - msg.pixel_flow[1] = flow_y; - msg.quality = quality; - msg.integration_timespan_us = integration_time; - // msg.integration_timespan_us = {1000000 / 30}; // 30 fps; - - // Static data - msg.timestamp = hrt_absolute_time(); - msg.timestamp_sample = msg.timestamp; - 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.max_ground_distance = 30; - 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); -} +// void GZBridge::cameraCallback(const gz::msgs::Image &image_msg) +// { +// float flow_x = 0; +// float flow_y = 0; +// int integration_time = 0; +// int quality = calculate_flow(image_msg, hrt_absolute_time(), integration_time, flow_x, flow_y); + +// if (quality <= 0) { +// quality = 0; +// } + +// // Construct SensorOpticalFlow message +// sensor_optical_flow_s msg = {}; + +// msg.pixel_flow[0] = flow_x; +// msg.pixel_flow[1] = flow_y; +// msg.quality = quality; +// msg.integration_timespan_us = integration_time; +// // msg.integration_timespan_us = {1000000 / 30}; // 30 fps; + +// // Static data +// msg.timestamp = hrt_absolute_time(); +// msg.timestamp_sample = msg.timestamp; +// 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.max_ground_distance = 30; +// 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[]) { diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index ba91faf5bb5f..280dc6ba67ce 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -117,7 +117,7 @@ class GZBridge : public ModuleBase, 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 cameraCallback(const gz::msgs::Image &image_msg); + // void cameraCallback(const gz::msgs::Image &image_msg); /** * @brief Call Entityfactory service diff --git a/src/modules/simulation/gz_bridge/camera/gz_camera.cpp b/src/modules/simulation/gz_bridge/camera/gz_camera.cpp index f77d574be1e3..aafbe8280dc8 100644 --- a/src/modules/simulation/gz_bridge/camera/gz_camera.cpp +++ b/src/modules/simulation/gz_bridge/camera/gz_camera.cpp @@ -16,11 +16,14 @@ int calculate_flow(const gz::msgs::Image &image_msg, uint64_t sim_time, int &int { if (!_optical_flow) { float hfov = 1.74; - int output_rate = 30; + int output_rate = 30; // TODO: calculate it int image_width = image_msg.width(); int image_height = image_msg.height(); float focal_length = (image_width / 2.0f) / tan(hfov / 2.0f); + printf("width %d\n", image_width); + printf("height %d\n", image_height); + _optical_flow = new OpticalFlowOpenCV(focal_length, focal_length, output_rate, image_width, image_height); } diff --git a/src/modules/simulation/gz_plugin/CMakeLists.txt b/src/modules/simulation/gz_plugin/CMakeLists.txt new file mode 100644 index 000000000000..7b92df918ca7 --- /dev/null +++ b/src/modules/simulation/gz_plugin/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.11.0 FATAL_ERROR) + +find_package(gz-cmake3 REQUIRED) + +project(OdometerSystem) + +gz_find_package(gz-plugin2 REQUIRED COMPONENTS register) +set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) + +gz_find_package(gz-sim8 REQUIRED) +set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) + +find_package(gz-sensors8 REQUIRED) +set(GZ_SENSORS_VER ${gz-sensors8_VERSION_MAJOR}) + +add_subdirectory(sensors) + +add_library(${PROJECT_NAME} SHARED ${PROJECT_NAME}.cc) +target_link_libraries(${PROJECT_NAME} + PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} + PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} + PRIVATE gz-sensors${GZ_SENSORS_VER}::gz-sensors${GZ_SENSORS_VER} + PRIVATE odometer +) + +target_include_directories(${PROJECT_NAME} + PUBLIC ${PROJECT_SOURCE_DIR}/sensors) diff --git a/src/modules/simulation/gz_plugin/Kconfig b/src/modules/simulation/gz_plugin/Kconfig new file mode 100644 index 000000000000..bec6f3cf49a4 --- /dev/null +++ b/src/modules/simulation/gz_plugin/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_SIMULATION_GZ_PLUGIN + bool "gz_plugin" + default n + depends on PLATFORM_POSIX + ---help--- + Enable support for gz_plugin diff --git a/src/modules/simulation/gz_plugin/OdometerSystem.cc b/src/modules/simulation/gz_plugin/OdometerSystem.cc new file mode 100644 index 000000000000..9144f1b059ee --- /dev/null +++ b/src/modules/simulation/gz_plugin/OdometerSystem.cc @@ -0,0 +1,132 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "Odometer.hh" +#include "OdometerSystem.hh" + +using namespace custom; + +////////////////////////////////////////////////// +void OdometerSystem::PreUpdate(const gz::sim::UpdateInfo &, + gz::sim::EntityComponentManager &_ecm) +{ + _ecm.EachNew( + [&](const gz::sim::Entity &_entity, + const gz::sim::components::CustomSensor *_custom, + const gz::sim::components::ParentEntity *_parent)->bool + { + // Get sensor's scoped name without the world + auto sensorScopedName = gz::sim::removeParentScope( + gz::sim::scopedName(_entity, _ecm, "::", false), "::"); + sdf::Sensor data = _custom->Data(); + data.SetName(sensorScopedName); + + // Default to scoped name as topic + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/odometer"; + data.SetTopic(topic); + } + + gz::sensors::SensorFactory sensorFactory; + auto sensor = sensorFactory.CreateSensor(data); + if (nullptr == sensor) + { + gzerr << "Failed to create odometer [" << sensorScopedName << "]" + << std::endl; + return false; + } + + // Set sensor parent + auto parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); + + // Set topic on Gazebo + _ecm.CreateComponent(_entity, + gz::sim::components::SensorTopic(sensor->Topic())); + + // Keep track of this sensor + this->entitySensorMap.insert(std::make_pair(_entity, + std::move(sensor))); + + return true; + }); +} + +////////////////////////////////////////////////// +void OdometerSystem::PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) +{ + // Only update and publish if not paused. + if (!_info.paused) + { + for (auto &[entity, sensor] : this->entitySensorMap) + { + sensor->NewPosition(gz::sim::worldPose(entity, _ecm).Pos()); + sensor->Update(_info.simTime); + } + } + + this->RemoveSensorEntities(_ecm); +} + +////////////////////////////////////////////////// +void OdometerSystem::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 odometer for entity [" + << _entity << "]" << std::endl; + } + return true; + }); +} + +GZ_ADD_PLUGIN(OdometerSystem, gz::sim::System, + OdometerSystem::ISystemPreUpdate, + OdometerSystem::ISystemPostUpdate +) + +GZ_ADD_PLUGIN_ALIAS(OdometerSystem, "custom::OdometerSystem") diff --git a/src/modules/simulation/gz_plugin/OdometerSystem.hh b/src/modules/simulation/gz_plugin/OdometerSystem.hh new file mode 100644 index 000000000000..58c29013295e --- /dev/null +++ b/src/modules/simulation/gz_plugin/OdometerSystem.hh @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef ODOMETERSYSTEM_HH_ +#define ODOMETERSYSTEM_HH_ + +#include +#include +#include + +namespace custom +{ + /// \brief Example showing how to tie a custom sensor, in this case an + /// odometer, into simulation + class OdometerSystem: + public gz::sim::System, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate + { + // Documentation inherited. + // During PreUpdate, check for new sensors that were inserted + // into simulation and create more components as needed. + public: void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) final; + + // Documentation inherited. + // During PostUpdate, update the known sensors and publish their data. + // Also remove sensors that have been deleted. + public: void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) final; + + /// \brief Remove custom sensors if their entities have been removed from + /// simulation. + /// \param[in] _ecm Immutable reference to ECM. + private: void RemoveSensorEntities( + const gz::sim::EntityComponentManager &_ecm); + + /// \brief A map of custom entities to their sensors + private: std::unordered_map> entitySensorMap; + }; +} +#endif diff --git a/src/modules/simulation/gz_plugin/msgs/optical_flow.proto b/src/modules/simulation/gz_plugin/msgs/optical_flow.proto new file mode 100644 index 000000000000..1723775e9181 --- /dev/null +++ b/src/modules/simulation/gz_plugin/msgs/optical_flow.proto @@ -0,0 +1,41 @@ +// msgs/optical_flow.proto +syntax = "proto3"; +package sensor_msgs.msgs; + +message OpticalFlow { + // Timestamp (microseconds, since system start) + int64 time_usec = 1; + + // Sensor ID + float sensor_id = 2; + + // Integration time + int32 integration_time_us = 3; + + // Integrated x-axis flow (rad) + float integrated_x = 4; + + // Integrated y-axis flow (rad) + float integrated_y = 5; + + // Integrated x-axis gyro rate (rad) + float integrated_xgyro = 6; + + // Integrated y-axis gyro rate (rad) + float integrated_ygyro = 7; + + // Integrated z-axis gyro rate (rad) + float integrated_zgyro = 8; + + // Temperature (degrees C) + float temperature = 9; + + // Quality of optical flow measurement (0: bad, 255: maximum quality) + float quality = 10; + + // Time since last measurement (microseconds) + int32 time_delta_distance_us = 11; + + // Distance to ground (meters) + float distance = 12; +} diff --git a/src/modules/simulation/gz_plugin/sensors/CMakeLists.txt b/src/modules/simulation/gz_plugin/sensors/CMakeLists.txt new file mode 100644 index 000000000000..308fa30e399f --- /dev/null +++ b/src/modules/simulation/gz_plugin/sensors/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.22.1 FATAL_ERROR) + +project(odometer) + +find_package(gz-cmake3 REQUIRED) +find_package(gz-sensors8 REQUIRED) + +add_library(${PROJECT_NAME} SHARED Odometer.cc) +target_link_libraries(${PROJECT_NAME} + PUBLIC gz-sensors8::gz-sensors8) diff --git a/src/modules/simulation/gz_plugin/sensors/Odometer.cc b/src/modules/simulation/gz_plugin/sensors/Odometer.cc new file mode 100644 index 000000000000..07dbaceac4c3 --- /dev/null +++ b/src/modules/simulation/gz_plugin/sensors/Odometer.cc @@ -0,0 +1,109 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +#include +#include +#include +#include + +#include "Odometer.hh" + +using namespace custom; + +////////////////////////////////////////////////// +bool Odometer::Load(const sdf::Sensor &_sdf) +{ + auto type = gz::sensors::customType(_sdf); + if ("odometer" != type) + { + gzerr << "Trying to load [odometer] sensor, but got type [" + << type << "] instead." << std::endl; + return false; + } + + // Load common sensor params + gz::sensors::Sensor::Load(_sdf); + + // Advertise topic where data will be published + this->pub = this->node.Advertise(this->Topic()); + + if (!_sdf.Element()->HasElement("gz:odometer")) + { + gzdbg << "No custom configuration for [" << this->Topic() << "]" + << std::endl; + return true; + } + + // Load custom sensor params + auto customElem = _sdf.Element()->GetElement("gz:odometer"); + + if (!customElem->HasElement("noise")) + { + gzdbg << "No noise for [" << this->Topic() << "]" << std::endl; + return true; + } + + sdf::Noise noiseSdf; + noiseSdf.Load(customElem->GetElement("noise")); + this->noise = gz::sensors::NoiseFactory::NewNoiseModel(noiseSdf); + if (nullptr == this->noise) + { + gzerr << "Failed to load noise." << std::endl; + return false; + } + + return true; +} + +////////////////////////////////////////////////// +bool Odometer::Update(const std::chrono::steady_clock::duration &_now) +{ + gz::msgs::Double msg; + *msg.mutable_header()->mutable_stamp() = gz::msgs::Convert(_now); + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->Name()); + + this->totalDistance = this->noise->Apply(this->totalDistance); + + msg.set_data(this->totalDistance); + + this->AddSequence(msg.mutable_header()); + this->pub.Publish(msg); + + return true; +} + +////////////////////////////////////////////////// +void Odometer::NewPosition(const gz::math::Vector3d &_pos) +{ + if (!isnan(this->prevPos.X())) + { + this->totalDistance += this->prevPos.Distance(_pos); + } + this->prevPos = _pos; +} + +////////////////////////////////////////////////// +const gz::math::Vector3d &Odometer::Position() const +{ + return this->prevPos; +} diff --git a/src/modules/simulation/gz_plugin/sensors/Odometer.hh b/src/modules/simulation/gz_plugin/sensors/Odometer.hh new file mode 100644 index 000000000000..87410c7a7ebd --- /dev/null +++ b/src/modules/simulation/gz_plugin/sensors/Odometer.hh @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef ODOMETER_HH_ +#define ODOMETER_HH_ + +#include +#include +#include + +namespace custom +{ + /// \brief Example sensor that publishes the total distance travelled by a + /// robot, with noise. + class Odometer : public gz::sensors::Sensor + { + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return True if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return True if the update was successfull + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; + + /// \brief Set the current postiion of the robot, so the odometer can + /// calculate the distance travelled. + /// \param[in] _pos Current position in world coordinates. + public: void NewPosition(const gz::math::Vector3d &_pos); + + /// \brief Get the latest world postiion of the robot. + /// \return The latest position given to the odometer. + public: const gz::math::Vector3d &Position() const; + + /// \brief Previous position of the robot. + private: gz::math::Vector3d prevPos{std::nan(""), std::nan(""), + std::nan("")}; + + /// \brief Latest total distance. + private: double totalDistance{0.0}; + + /// \brief Noise that will be applied to the sensor data + private: gz::sensors::NoisePtr noise{nullptr}; + + /// \brief Node for communication + private: gz::transport::Node node; + + /// \brief Publishes sensor data + private: gz::transport::Node::Publisher pub; + }; +} + +#endif From 45014b4242fd32433b1d65f5cb9b6f5c7426631d Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 24 Dec 2024 17:03:55 -0900 Subject: [PATCH 04/16] it builds and publishes, need to figure out build system now --- .../init.d-posix/airframes/4021_gz_x500_flow | 11 +- .../init.d-posix/px4-rc.simulator | 2 +- Tools/simulation/gz | 2 +- .../simulation/gz_bridge/CMakeLists.txt | 3 - src/modules/simulation/gz_bridge/GZBridge.cpp | 105 +++++++------- src/modules/simulation/gz_bridge/GZBridge.hpp | 4 +- .../gz_bridge/camera/CMakeLists.txt | 83 ----------- .../simulation/gz_bridge/camera/gz_camera.cpp | 38 ----- .../simulation/gz_bridge/camera/gz_camera.hpp | 6 - .../simulation/gz_plugin/CMakeLists.txt | 48 ++++++- .../simulation/gz_plugin/OdometerSystem.cc | 132 ------------------ .../simulation/gz_plugin/OdometerSystem.hh | 56 -------- .../simulation/gz_plugin/OpticalFlow.cc | 114 +++++++++++++++ .../simulation/gz_plugin/OpticalFlow.hh | 30 ++++ .../simulation/gz_plugin/OpticalFlowSystem.cc | 91 ++++++++++++ .../simulation/gz_plugin/OpticalFlowSystem.hh | 26 ++++ .../gz_plugin/msgs/optical_flow.proto | 41 ------ .../simulation/gz_plugin/optical_flow.proto | 23 +++ .../gz_plugin/sensors/CMakeLists.txt | 10 -- .../simulation/gz_plugin/sensors/Odometer.cc | 109 --------------- .../simulation/gz_plugin/sensors/Odometer.hh | 68 --------- 21 files changed, 391 insertions(+), 611 deletions(-) delete mode 100644 src/modules/simulation/gz_bridge/camera/CMakeLists.txt delete mode 100644 src/modules/simulation/gz_bridge/camera/gz_camera.cpp delete mode 100644 src/modules/simulation/gz_bridge/camera/gz_camera.hpp delete mode 100644 src/modules/simulation/gz_plugin/OdometerSystem.cc delete mode 100644 src/modules/simulation/gz_plugin/OdometerSystem.hh create mode 100644 src/modules/simulation/gz_plugin/OpticalFlow.cc create mode 100644 src/modules/simulation/gz_plugin/OpticalFlow.hh create mode 100644 src/modules/simulation/gz_plugin/OpticalFlowSystem.cc create mode 100644 src/modules/simulation/gz_plugin/OpticalFlowSystem.hh delete mode 100644 src/modules/simulation/gz_plugin/msgs/optical_flow.proto create mode 100644 src/modules/simulation/gz_plugin/optical_flow.proto delete mode 100644 src/modules/simulation/gz_plugin/sensors/CMakeLists.txt delete mode 100644 src/modules/simulation/gz_plugin/sensors/Odometer.cc delete mode 100644 src/modules/simulation/gz_plugin/sensors/Odometer.hh 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 index 95efb9c1426c..c4d5e04028d9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow @@ -11,10 +11,13 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_flow} 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 +# 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/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index 90e86aa6880e..0a1de832b588 100644 --- 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 index 5f6aeff9547f..887090a7cf34 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 5f6aeff9547fe0bb183e30eff0404670538c4e7a +Subproject commit 887090a7cf3464a834417081f0b864ca91f0663d diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index cead68d0d05a..e57c6020451b 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -31,8 +31,6 @@ # ############################################################################ -# add_subdirectory(camera) - # Find the gz_Transport library # Look for GZ Ionic or Harmonic find_package(gz-transport NAMES gz-transport14 gz-transport13) @@ -68,7 +66,6 @@ if(gz-transport_FOUND) DEPENDS mixer_module px4_work_queue - # gz_camera ${GZ_TRANSPORT_LIB} MODULE_CONFIG module.yaml diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index b5e8da4f0822..69e4eae936bf 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -43,8 +43,6 @@ #include #include -#include "camera/gz_camera.hpp" - GZBridge::GZBridge(const char *world, const char *name, const char *model, const char *pose_str) : ModuleParams(nullptr), @@ -256,12 +254,12 @@ int GZBridge::init() } // Camera: - std::string camera_topic = "/camera"; + std::string flow_topic = "/optical_flow"; - // if (!_node.Subscribe(camera_topic, &GZBridge::cameraCallback, this)) { - // PX4_ERR("failed to subscribe to %s", camera_topic.c_str()); - // return PX4_ERROR; - // } + 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"); @@ -287,52 +285,53 @@ int GZBridge::init() return OK; } -// void GZBridge::cameraCallback(const gz::msgs::Image &image_msg) -// { -// float flow_x = 0; -// float flow_y = 0; -// int integration_time = 0; -// int quality = calculate_flow(image_msg, hrt_absolute_time(), integration_time, flow_x, flow_y); - -// if (quality <= 0) { -// quality = 0; -// } - -// // Construct SensorOpticalFlow message -// sensor_optical_flow_s msg = {}; - -// msg.pixel_flow[0] = flow_x; -// msg.pixel_flow[1] = flow_y; -// msg.quality = quality; -// msg.integration_timespan_us = integration_time; -// // msg.integration_timespan_us = {1000000 / 30}; // 30 fps; - -// // Static data -// msg.timestamp = hrt_absolute_time(); -// msg.timestamp_sample = msg.timestamp; -// 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.max_ground_distance = 30; -// 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); -// } +// TODO: change to sensor_msgs::msgs::OpticalFlow +void GZBridge::opticalFlowCallback(const gz::msgs::Image &image_msg) +{ + // float flow_x = 0; + // float flow_y = 0; + // int integration_time = 0; + // int quality = calculate_flow(image_msg, hrt_absolute_time(), integration_time, flow_x, flow_y); + + // if (quality <= 0) { + // quality = 0; + // } + + // Construct SensorOpticalFlow message + sensor_optical_flow_s msg = {}; + + // msg.pixel_flow[0] = flow_x; + // msg.pixel_flow[1] = flow_y; + // msg.quality = quality; + // msg.integration_timespan_us = integration_time; + // msg.integration_timespan_us = {1000000 / 30}; // 30 fps; + + // Static data + msg.timestamp = hrt_absolute_time(); + msg.timestamp_sample = msg.timestamp; + 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.max_ground_distance = 30; + 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[]) { diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 280dc6ba67ce..b7777b861b2e 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -117,7 +117,9 @@ class GZBridge : public ModuleBase, 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 cameraCallback(const gz::msgs::Image &image_msg); + + // TODO: change to sensor_msgs::msgs::OpticalFlow + void opticalFlowCallback(const gz::msgs::Image &image_msg); /** * @brief Call Entityfactory service diff --git a/src/modules/simulation/gz_bridge/camera/CMakeLists.txt b/src/modules/simulation/gz_bridge/camera/CMakeLists.txt deleted file mode 100644 index 40bd37c262db..000000000000 --- a/src/modules/simulation/gz_bridge/camera/CMakeLists.txt +++ /dev/null @@ -1,83 +0,0 @@ -############################################################################ -# -# Copyright (c) 2019 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. -# -############################################################################ - -find_package(gz-transport NAMES gz-transport13) -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= - BUILD_BYPRODUCTS ${CMAKE_BINARY_DIR}/OpticalFlow/install/lib/libOpticalFlow.so -) - -ExternalProject_Get_Property(OpticalFlow install_dir) - -message(WARNING "OpticalFlow install dir: ${install_dir}") - -set(OpticalFlow_INCLUDE_DIRS ${install_dir}/include) - -# If Harmonic not found, look for GZ Garden libraries -if(NOT gz-transport_FOUND) - find_package(gz-transport NAMES gz-transport12) -endif() - -if(gz-transport_FOUND) - - add_compile_options(-frtti -fexceptions) - - set(GZ_TRANSPORT_VER ${gz-transport_VERSION_MAJOR}) - - 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() - - px4_add_library(gz_camera - gz_camera.hpp - gz_camera.cpp - ) - - set(OpticalFlow_LIBS ${install_dir}/lib/libOpticalFlow.so) - - target_compile_options(gz_camera PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) - target_include_directories(gz_camera PRIVATE ${CMAKE_CURRENT_BINARY_DIR} ${OpenCV_INCLUDE_DIRS} ${OpticalFlow_INCLUDE_DIRS}) - target_link_libraries(gz_camera PRIVATE ${GZ_TRANSPORT_LIB} ${OpenCV_LIBS} ${OpticalFlow_LIBS}) - - add_dependencies(gz_camera OpticalFlow) -endif() diff --git a/src/modules/simulation/gz_bridge/camera/gz_camera.cpp b/src/modules/simulation/gz_bridge/camera/gz_camera.cpp deleted file mode 100644 index aafbe8280dc8..000000000000 --- a/src/modules/simulation/gz_bridge/camera/gz_camera.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include "gz_camera.hpp" - -#include -#include -#include - -// #include -#include "flow_opencv.hpp" -#include - -OpticalFlowOpenCV *_optical_flow = nullptr; -int _dt_us = 0; - -int calculate_flow(const gz::msgs::Image &image_msg, uint64_t sim_time, int &integration_time, float &flow_x, - float &flow_y) -{ - if (!_optical_flow) { - float hfov = 1.74; - int output_rate = 30; // TODO: calculate it - int image_width = image_msg.width(); - int image_height = image_msg.height(); - float focal_length = (image_width / 2.0f) / tan(hfov / 2.0f); - - printf("width %d\n", image_width); - printf("height %d\n", image_height); - - _optical_flow = new OpticalFlowOpenCV(focal_length, focal_length, output_rate, image_width, image_height); - } - - cv::Mat image = cv::Mat(image_msg.height(), image_msg.width(), CV_8UC3, (void *)image_msg.data().c_str()); - - cv::Mat gray_image; - cv::cvtColor(image, gray_image, cv::COLOR_RGB2GRAY); - - int quality = _optical_flow->calcFlow(gray_image.data, sim_time, integration_time, flow_x, flow_y); - - return quality; -} diff --git a/src/modules/simulation/gz_bridge/camera/gz_camera.hpp b/src/modules/simulation/gz_bridge/camera/gz_camera.hpp deleted file mode 100644 index 05a88fb71822..000000000000 --- a/src/modules/simulation/gz_bridge/camera/gz_camera.hpp +++ /dev/null @@ -1,6 +0,0 @@ -#pragma once - -#include - -int calculate_flow(const gz::msgs::Image &image_msg, uint64_t sim_time, int &integration_time, float &flow_x, - float &flow_y); diff --git a/src/modules/simulation/gz_plugin/CMakeLists.txt b/src/modules/simulation/gz_plugin/CMakeLists.txt index 7b92df918ca7..18947bcd5afb 100644 --- a/src/modules/simulation/gz_plugin/CMakeLists.txt +++ b/src/modules/simulation/gz_plugin/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.11.0 FATAL_ERROR) find_package(gz-cmake3 REQUIRED) -project(OdometerSystem) +project(OpticalFlowSystem) gz_find_package(gz-plugin2 REQUIRED COMPONENTS register) set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) @@ -13,15 +13,53 @@ set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) find_package(gz-sensors8 REQUIRED) set(GZ_SENSORS_VER ${gz-sensors8_VERSION_MAJOR}) -add_subdirectory(sensors) +find_package(OpenCV REQUIRED) + +# add_subdirectory(sensors optical_flow) + +PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS optical_flow.proto) + +add_library(optical_flow_plugin SHARED + ${PROTO_SRCS} + ${PROTO_HDRS} + OpticalFlow.cc +) + +set(sensors + optical_flow_plugin +) + +foreach(sensor ${sensors}) + target_link_libraries(${sensor} + PUBLIC gz-sensors8::gz-sensors8 + PUBLIC gz-rendering8::gz-rendering8 + PUBLIC ${OpenCV_LIBS} + PUBLIC ${PROTOBUF_LIBRARIES} + ) + + target_include_directories(${sensor} + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC ${OpenCV_INCLUDE_DIRS} + ) + +endforeach() + +add_library(${PROJECT_NAME} SHARED + OpticalFlowSystem.cc +) + +# add_dependencies(${PROJECT_NAME} optical_flow) -add_library(${PROJECT_NAME} SHARED ${PROJECT_NAME}.cc) target_link_libraries(${PROJECT_NAME} + PUBLIC ${sensors} PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} PRIVATE gz-sensors${GZ_SENSORS_VER}::gz-sensors${GZ_SENSORS_VER} - PRIVATE odometer + PRIVATE ${OpenCV_LIBS} ) target_include_directories(${PROJECT_NAME} - PUBLIC ${PROJECT_SOURCE_DIR}/sensors) + PUBLIC ${PROJECT_SOURCE_DIR} + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC ${OpenCV_INCLUDE_DIRS} +) diff --git a/src/modules/simulation/gz_plugin/OdometerSystem.cc b/src/modules/simulation/gz_plugin/OdometerSystem.cc deleted file mode 100644 index 9144f1b059ee..000000000000 --- a/src/modules/simulation/gz_plugin/OdometerSystem.cc +++ /dev/null @@ -1,132 +0,0 @@ -/* - * Copyright (C) 2021 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#include - -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "Odometer.hh" -#include "OdometerSystem.hh" - -using namespace custom; - -////////////////////////////////////////////////// -void OdometerSystem::PreUpdate(const gz::sim::UpdateInfo &, - gz::sim::EntityComponentManager &_ecm) -{ - _ecm.EachNew( - [&](const gz::sim::Entity &_entity, - const gz::sim::components::CustomSensor *_custom, - const gz::sim::components::ParentEntity *_parent)->bool - { - // Get sensor's scoped name without the world - auto sensorScopedName = gz::sim::removeParentScope( - gz::sim::scopedName(_entity, _ecm, "::", false), "::"); - sdf::Sensor data = _custom->Data(); - data.SetName(sensorScopedName); - - // Default to scoped name as topic - if (data.Topic().empty()) - { - std::string topic = scopedName(_entity, _ecm) + "/odometer"; - data.SetTopic(topic); - } - - gz::sensors::SensorFactory sensorFactory; - auto sensor = sensorFactory.CreateSensor(data); - if (nullptr == sensor) - { - gzerr << "Failed to create odometer [" << sensorScopedName << "]" - << std::endl; - return false; - } - - // Set sensor parent - auto parentName = _ecm.Component( - _parent->Data())->Data(); - sensor->SetParent(parentName); - - // Set topic on Gazebo - _ecm.CreateComponent(_entity, - gz::sim::components::SensorTopic(sensor->Topic())); - - // Keep track of this sensor - this->entitySensorMap.insert(std::make_pair(_entity, - std::move(sensor))); - - return true; - }); -} - -////////////////////////////////////////////////// -void OdometerSystem::PostUpdate(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm) -{ - // Only update and publish if not paused. - if (!_info.paused) - { - for (auto &[entity, sensor] : this->entitySensorMap) - { - sensor->NewPosition(gz::sim::worldPose(entity, _ecm).Pos()); - sensor->Update(_info.simTime); - } - } - - this->RemoveSensorEntities(_ecm); -} - -////////////////////////////////////////////////// -void OdometerSystem::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 odometer for entity [" - << _entity << "]" << std::endl; - } - return true; - }); -} - -GZ_ADD_PLUGIN(OdometerSystem, gz::sim::System, - OdometerSystem::ISystemPreUpdate, - OdometerSystem::ISystemPostUpdate -) - -GZ_ADD_PLUGIN_ALIAS(OdometerSystem, "custom::OdometerSystem") diff --git a/src/modules/simulation/gz_plugin/OdometerSystem.hh b/src/modules/simulation/gz_plugin/OdometerSystem.hh deleted file mode 100644 index 58c29013295e..000000000000 --- a/src/modules/simulation/gz_plugin/OdometerSystem.hh +++ /dev/null @@ -1,56 +0,0 @@ -/* - * Copyright (C) 2021 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ -#ifndef ODOMETERSYSTEM_HH_ -#define ODOMETERSYSTEM_HH_ - -#include -#include -#include - -namespace custom -{ - /// \brief Example showing how to tie a custom sensor, in this case an - /// odometer, into simulation - class OdometerSystem: - public gz::sim::System, - public gz::sim::ISystemPreUpdate, - public gz::sim::ISystemPostUpdate - { - // Documentation inherited. - // During PreUpdate, check for new sensors that were inserted - // into simulation and create more components as needed. - public: void PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) final; - - // Documentation inherited. - // During PostUpdate, update the known sensors and publish their data. - // Also remove sensors that have been deleted. - public: void PostUpdate(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm) final; - - /// \brief Remove custom sensors if their entities have been removed from - /// simulation. - /// \param[in] _ecm Immutable reference to ECM. - private: void RemoveSensorEntities( - const gz::sim::EntityComponentManager &_ecm); - - /// \brief A map of custom entities to their sensors - private: std::unordered_map> entitySensorMap; - }; -} -#endif diff --git a/src/modules/simulation/gz_plugin/OpticalFlow.cc b/src/modules/simulation/gz_plugin/OpticalFlow.cc new file mode 100644 index 000000000000..0e972e29f597 --- /dev/null +++ b/src/modules/simulation/gz_plugin/OpticalFlow.cc @@ -0,0 +1,114 @@ +// OpticalFlow.cc +#include +#include +#include + +#include "OpticalFlow.hh" +#include "optical_flow.pb.h" + +using namespace custom; + +bool OpticalFlow::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; + } + + gzdbg << "Loading optical flow sensor..." << std::endl; + gz::sensors::Sensor::Load(_sdf); + + gzdbg << "Adverising optical flow sensor on: " << this->Topic() << std::endl; + this->pub = this->node.Advertise(this->Topic()); + + auto elem = _sdf.Element(); + auto opticalFlowElem = elem->GetElement("gz:optical_flow"); + auto cameraTopic = opticalFlowElem->Get("camera_topic"); + + gzdbg << "Subscribing to camera topic: " << cameraTopic << std::endl; + + if (!this->node.Subscribe(cameraTopic, &OpticalFlow::OnImage, this)) { + gzerr << "Failed to subscribe to camera topic: " << cameraTopic << std::endl; + return false; + } + + this->lastUpdateTime = std::chrono::steady_clock::now(); + + return true; +} + +void OpticalFlow::OnImage(const gz::msgs::Image &_msg) +{ + cv::Mat frame; + + if (_msg.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8) { + frame = cv::Mat(_msg.height(), _msg.width(), CV_8UC3, (void *)_msg.data().c_str()); + cv::cvtColor(frame, frame, cv::COLOR_RGB2GRAY); + + } else if (_msg.pixel_format_type() == gz::msgs::PixelFormatType::L_INT8) { + frame = cv::Mat(_msg.height(), _msg.width(), CV_8UC1, (void *)_msg.data().c_str()); + + } else { + gzerr << "Unsupported image format" << std::endl; + return; + } + + if (prevFrame.empty()) { + frame.copyTo(prevFrame); + return; + } + + cv::Mat flow; + cv::calcOpticalFlowFarneback(prevFrame, frame, flow, + 0.5, // Pyramid scale + 3, // Pyramid levels + 15, // Window size + 3, // Iteratpions + 5, // Poly_n + 1.2, // Poly_sigma + 0); // Flags + + // Calculate average flow and quality + cv::Scalar meanFlow = cv::mean(flow); + + // Update integrated flow (in radians) + // TODO: FOV from SDF + // Assuming 60° horizontal field of view and 45° vertical field of view + const double rad_per_pixel_x = (M_PI / 3.0) / frame.cols; // 60° in radians / width + const double rad_per_pixel_y = (M_PI / 4.0) / frame.rows; // 45° in radians / height + + this->integrated_x = meanFlow[0] * rad_per_pixel_x; + this->integrated_y = meanFlow[1] * rad_per_pixel_y; + + // Calculate quality (0-255) + cv::Mat magnitude, angle; + cv::cartToPolar(flow.col(0), flow.col(1), magnitude, angle); + this->quality = cv::mean(magnitude)[0] * 255.0; + + if (this->quality > 255.0f) { + this->quality = 255.0f; + } + + frame.copyTo(prevFrame); +} + +bool OpticalFlow::Update(const std::chrono::steady_clock::duration &_now) +{ + auto currentTime = std::chrono::steady_clock::now(); + auto deltaTime = std::chrono::duration_cast(currentTime - this->lastUpdateTime); + + sensor_msgs::msgs::OpticalFlow msg; + msg.set_time_usec(std::chrono::duration_cast(_now).count()); + msg.set_integration_time_us(deltaTime.count()); + msg.set_integrated_x(this->integrated_x); + msg.set_integrated_y(this->integrated_y); + msg.set_quality(this->quality); + msg.set_time_delta_distance_us(deltaTime.count()); + + this->pub.Publish(msg); + this->lastUpdateTime = currentTime; + + return true; +} diff --git a/src/modules/simulation/gz_plugin/OpticalFlow.hh b/src/modules/simulation/gz_plugin/OpticalFlow.hh new file mode 100644 index 000000000000..11401b6b932b --- /dev/null +++ b/src/modules/simulation/gz_plugin/OpticalFlow.hh @@ -0,0 +1,30 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace custom +{ +class OpticalFlow : 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); + + cv::Mat prevFrame; + gz::transport::Node node; + gz::transport::Node::Publisher pub; + + float integrated_x{0.0}; + float integrated_y{0.0}; + float quality{0.0}; + std::chrono::steady_clock::time_point lastUpdateTime; +}; +} // end namespace custom diff --git a/src/modules/simulation/gz_plugin/OpticalFlowSystem.cc b/src/modules/simulation/gz_plugin/OpticalFlowSystem.cc new file mode 100644 index 000000000000..6d46ca37d7e2 --- /dev/null +++ b/src/modules/simulation/gz_plugin/OpticalFlowSystem.cc @@ -0,0 +1,91 @@ +// OpticalFlowSystem.cc +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "OpticalFlow.hh" +#include "OpticalFlowSystem.hh" + +using namespace custom; + +void OpticalFlowSystem::PreUpdate(const gz::sim::UpdateInfo &, gz::sim::EntityComponentManager &_ecm) +{ + _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_plugin/OpticalFlowSystem.hh b/src/modules/simulation/gz_plugin/OpticalFlowSystem.hh new file mode 100644 index 000000000000..c2095d5e9c10 --- /dev/null +++ b/src/modules/simulation/gz_plugin/OpticalFlowSystem.hh @@ -0,0 +1,26 @@ +#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_plugin/msgs/optical_flow.proto b/src/modules/simulation/gz_plugin/msgs/optical_flow.proto deleted file mode 100644 index 1723775e9181..000000000000 --- a/src/modules/simulation/gz_plugin/msgs/optical_flow.proto +++ /dev/null @@ -1,41 +0,0 @@ -// msgs/optical_flow.proto -syntax = "proto3"; -package sensor_msgs.msgs; - -message OpticalFlow { - // Timestamp (microseconds, since system start) - int64 time_usec = 1; - - // Sensor ID - float sensor_id = 2; - - // Integration time - int32 integration_time_us = 3; - - // Integrated x-axis flow (rad) - float integrated_x = 4; - - // Integrated y-axis flow (rad) - float integrated_y = 5; - - // Integrated x-axis gyro rate (rad) - float integrated_xgyro = 6; - - // Integrated y-axis gyro rate (rad) - float integrated_ygyro = 7; - - // Integrated z-axis gyro rate (rad) - float integrated_zgyro = 8; - - // Temperature (degrees C) - float temperature = 9; - - // Quality of optical flow measurement (0: bad, 255: maximum quality) - float quality = 10; - - // Time since last measurement (microseconds) - int32 time_delta_distance_us = 11; - - // Distance to ground (meters) - float distance = 12; -} diff --git a/src/modules/simulation/gz_plugin/optical_flow.proto b/src/modules/simulation/gz_plugin/optical_flow.proto new file mode 100644 index 000000000000..b7012cc42d00 --- /dev/null +++ b/src/modules/simulation/gz_plugin/optical_flow.proto @@ -0,0 +1,23 @@ +// msgs/optical_flow.proto +syntax = "proto3"; +package sensor_msgs.msgs; + +message OpticalFlow { + // Timestamp (microseconds, since system start) + int64 time_usec = 1; + + // Integration time + int32 integration_time_us = 3; + + // Integrated x-axis flow (rad) + float integrated_x = 4; + + // Integrated y-axis flow (rad) + float integrated_y = 5; + + // Quality of optical flow measurement (0: bad, 255: maximum quality) + float quality = 10; + + // Time since last measurement (microseconds) + int32 time_delta_distance_us = 11; +} diff --git a/src/modules/simulation/gz_plugin/sensors/CMakeLists.txt b/src/modules/simulation/gz_plugin/sensors/CMakeLists.txt deleted file mode 100644 index 308fa30e399f..000000000000 --- a/src/modules/simulation/gz_plugin/sensors/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -cmake_minimum_required(VERSION 3.22.1 FATAL_ERROR) - -project(odometer) - -find_package(gz-cmake3 REQUIRED) -find_package(gz-sensors8 REQUIRED) - -add_library(${PROJECT_NAME} SHARED Odometer.cc) -target_link_libraries(${PROJECT_NAME} - PUBLIC gz-sensors8::gz-sensors8) diff --git a/src/modules/simulation/gz_plugin/sensors/Odometer.cc b/src/modules/simulation/gz_plugin/sensors/Odometer.cc deleted file mode 100644 index 07dbaceac4c3..000000000000 --- a/src/modules/simulation/gz_plugin/sensors/Odometer.cc +++ /dev/null @@ -1,109 +0,0 @@ -/* - * Copyright (C) 2021 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#include - -#include - -#include -#include -#include -#include - -#include "Odometer.hh" - -using namespace custom; - -////////////////////////////////////////////////// -bool Odometer::Load(const sdf::Sensor &_sdf) -{ - auto type = gz::sensors::customType(_sdf); - if ("odometer" != type) - { - gzerr << "Trying to load [odometer] sensor, but got type [" - << type << "] instead." << std::endl; - return false; - } - - // Load common sensor params - gz::sensors::Sensor::Load(_sdf); - - // Advertise topic where data will be published - this->pub = this->node.Advertise(this->Topic()); - - if (!_sdf.Element()->HasElement("gz:odometer")) - { - gzdbg << "No custom configuration for [" << this->Topic() << "]" - << std::endl; - return true; - } - - // Load custom sensor params - auto customElem = _sdf.Element()->GetElement("gz:odometer"); - - if (!customElem->HasElement("noise")) - { - gzdbg << "No noise for [" << this->Topic() << "]" << std::endl; - return true; - } - - sdf::Noise noiseSdf; - noiseSdf.Load(customElem->GetElement("noise")); - this->noise = gz::sensors::NoiseFactory::NewNoiseModel(noiseSdf); - if (nullptr == this->noise) - { - gzerr << "Failed to load noise." << std::endl; - return false; - } - - return true; -} - -////////////////////////////////////////////////// -bool Odometer::Update(const std::chrono::steady_clock::duration &_now) -{ - gz::msgs::Double msg; - *msg.mutable_header()->mutable_stamp() = gz::msgs::Convert(_now); - auto frame = msg.mutable_header()->add_data(); - frame->set_key("frame_id"); - frame->add_value(this->Name()); - - this->totalDistance = this->noise->Apply(this->totalDistance); - - msg.set_data(this->totalDistance); - - this->AddSequence(msg.mutable_header()); - this->pub.Publish(msg); - - return true; -} - -////////////////////////////////////////////////// -void Odometer::NewPosition(const gz::math::Vector3d &_pos) -{ - if (!isnan(this->prevPos.X())) - { - this->totalDistance += this->prevPos.Distance(_pos); - } - this->prevPos = _pos; -} - -////////////////////////////////////////////////// -const gz::math::Vector3d &Odometer::Position() const -{ - return this->prevPos; -} diff --git a/src/modules/simulation/gz_plugin/sensors/Odometer.hh b/src/modules/simulation/gz_plugin/sensors/Odometer.hh deleted file mode 100644 index 87410c7a7ebd..000000000000 --- a/src/modules/simulation/gz_plugin/sensors/Odometer.hh +++ /dev/null @@ -1,68 +0,0 @@ -/* - * Copyright (C) 2021 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ -#ifndef ODOMETER_HH_ -#define ODOMETER_HH_ - -#include -#include -#include - -namespace custom -{ - /// \brief Example sensor that publishes the total distance travelled by a - /// robot, with noise. - class Odometer : public gz::sensors::Sensor - { - /// \brief Load the sensor with SDF parameters. - /// \param[in] _sdf SDF Sensor parameters. - /// \return True if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf) override; - - /// \brief Update the sensor and generate data - /// \param[in] _now The current time - /// \return True if the update was successfull - public: virtual bool Update( - const std::chrono::steady_clock::duration &_now) override; - - /// \brief Set the current postiion of the robot, so the odometer can - /// calculate the distance travelled. - /// \param[in] _pos Current position in world coordinates. - public: void NewPosition(const gz::math::Vector3d &_pos); - - /// \brief Get the latest world postiion of the robot. - /// \return The latest position given to the odometer. - public: const gz::math::Vector3d &Position() const; - - /// \brief Previous position of the robot. - private: gz::math::Vector3d prevPos{std::nan(""), std::nan(""), - std::nan("")}; - - /// \brief Latest total distance. - private: double totalDistance{0.0}; - - /// \brief Noise that will be applied to the sensor data - private: gz::sensors::NoisePtr noise{nullptr}; - - /// \brief Node for communication - private: gz::transport::Node node; - - /// \brief Publishes sensor data - private: gz::transport::Node::Publisher pub; - }; -} - -#endif From 2286fa8572d041c743bb59ce3e133b8b491d4f1f Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 24 Dec 2024 18:16:39 -0900 Subject: [PATCH 05/16] single library --- .../simulation/gz_plugin/CMakeLists.txt | 55 +++++-------------- .../simulation/gz_plugin/OpticalFlow.cc | 4 ++ 2 files changed, 17 insertions(+), 42 deletions(-) diff --git a/src/modules/simulation/gz_plugin/CMakeLists.txt b/src/modules/simulation/gz_plugin/CMakeLists.txt index 18947bcd5afb..e726d31942be 100644 --- a/src/modules/simulation/gz_plugin/CMakeLists.txt +++ b/src/modules/simulation/gz_plugin/CMakeLists.txt @@ -1,65 +1,36 @@ cmake_minimum_required(VERSION 3.11.0 FATAL_ERROR) - find_package(gz-cmake3 REQUIRED) project(OpticalFlowSystem) gz_find_package(gz-plugin2 REQUIRED COMPONENTS register) -set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) - gz_find_package(gz-sim8 REQUIRED) -set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) - -find_package(gz-sensors8 REQUIRED) -set(GZ_SENSORS_VER ${gz-sensors8_VERSION_MAJOR}) - +gz_find_package(gz-sensors8 REQUIRED) +gz_find_package(gz-transport12 REQUIRED) find_package(OpenCV REQUIRED) - -# add_subdirectory(sensors optical_flow) +find_package(Protobuf REQUIRED) PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS optical_flow.proto) -add_library(optical_flow_plugin SHARED - ${PROTO_SRCS} - ${PROTO_HDRS} - OpticalFlow.cc -) - -set(sensors - optical_flow_plugin +add_library(${PROJECT_NAME} SHARED + OpticalFlow.cc + OpticalFlowSystem.cc + ${PROTO_SRCS} + ${PROTO_HDRS} ) -foreach(sensor ${sensors}) - target_link_libraries(${sensor} +target_link_libraries(${PROJECT_NAME} 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 ${PROTOBUF_LIBRARIES} - ) - - target_include_directories(${sensor} - PUBLIC ${CMAKE_CURRENT_BINARY_DIR} - PUBLIC ${OpenCV_INCLUDE_DIRS} - ) - -endforeach() - -add_library(${PROJECT_NAME} SHARED - OpticalFlowSystem.cc -) - -# add_dependencies(${PROJECT_NAME} optical_flow) - -target_link_libraries(${PROJECT_NAME} - PUBLIC ${sensors} - PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} - PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} - PRIVATE gz-sensors${GZ_SENSORS_VER}::gz-sensors${GZ_SENSORS_VER} - PRIVATE ${OpenCV_LIBS} ) target_include_directories(${PROJECT_NAME} - PUBLIC ${PROJECT_SOURCE_DIR} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} PUBLIC ${CMAKE_CURRENT_BINARY_DIR} PUBLIC ${OpenCV_INCLUDE_DIRS} ) diff --git a/src/modules/simulation/gz_plugin/OpticalFlow.cc b/src/modules/simulation/gz_plugin/OpticalFlow.cc index 0e972e29f597..8938d8362af4 100644 --- a/src/modules/simulation/gz_plugin/OpticalFlow.cc +++ b/src/modules/simulation/gz_plugin/OpticalFlow.cc @@ -41,6 +41,8 @@ bool OpticalFlow::Load(const sdf::Sensor &_sdf) void OpticalFlow::OnImage(const gz::msgs::Image &_msg) { + // gzdbg << "OpticalFlow::OnImage" << std::endl; + cv::Mat frame; if (_msg.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8) { @@ -96,6 +98,8 @@ void OpticalFlow::OnImage(const gz::msgs::Image &_msg) bool OpticalFlow::Update(const std::chrono::steady_clock::duration &_now) { + // gzdbg << "OpticalFlow::Update" << std::endl; + auto currentTime = std::chrono::steady_clock::now(); auto deltaTime = std::chrono::duration_cast(currentTime - this->lastUpdateTime); From b4a77dbadab0d91cf8db57c245857843861d2f96 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 24 Dec 2024 18:20:42 -0900 Subject: [PATCH 06/16] rename files --- src/modules/simulation/gz_plugin/CMakeLists.txt | 4 ++-- .../simulation/gz_plugin/{OpticalFlow.cc => OpticalFlow.cpp} | 3 +-- .../simulation/gz_plugin/{OpticalFlow.hh => OpticalFlow.hpp} | 0 .../gz_plugin/{OpticalFlowSystem.cc => OpticalFlowSystem.cpp} | 4 ++-- .../gz_plugin/{OpticalFlowSystem.hh => OpticalFlowSystem.hpp} | 0 5 files changed, 5 insertions(+), 6 deletions(-) rename src/modules/simulation/gz_plugin/{OpticalFlow.cc => OpticalFlow.cpp} (98%) rename src/modules/simulation/gz_plugin/{OpticalFlow.hh => OpticalFlow.hpp} (100%) rename src/modules/simulation/gz_plugin/{OpticalFlowSystem.cc => OpticalFlowSystem.cpp} (97%) rename src/modules/simulation/gz_plugin/{OpticalFlowSystem.hh => OpticalFlowSystem.hpp} (100%) diff --git a/src/modules/simulation/gz_plugin/CMakeLists.txt b/src/modules/simulation/gz_plugin/CMakeLists.txt index e726d31942be..36e348816bad 100644 --- a/src/modules/simulation/gz_plugin/CMakeLists.txt +++ b/src/modules/simulation/gz_plugin/CMakeLists.txt @@ -13,8 +13,8 @@ find_package(Protobuf REQUIRED) PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS optical_flow.proto) add_library(${PROJECT_NAME} SHARED - OpticalFlow.cc - OpticalFlowSystem.cc + OpticalFlow.cpp + OpticalFlowSystem.cpp ${PROTO_SRCS} ${PROTO_HDRS} ) diff --git a/src/modules/simulation/gz_plugin/OpticalFlow.cc b/src/modules/simulation/gz_plugin/OpticalFlow.cpp similarity index 98% rename from src/modules/simulation/gz_plugin/OpticalFlow.cc rename to src/modules/simulation/gz_plugin/OpticalFlow.cpp index 8938d8362af4..87694ae85006 100644 --- a/src/modules/simulation/gz_plugin/OpticalFlow.cc +++ b/src/modules/simulation/gz_plugin/OpticalFlow.cpp @@ -1,9 +1,8 @@ -// OpticalFlow.cc #include #include #include -#include "OpticalFlow.hh" +#include "OpticalFlow.hpp" #include "optical_flow.pb.h" using namespace custom; diff --git a/src/modules/simulation/gz_plugin/OpticalFlow.hh b/src/modules/simulation/gz_plugin/OpticalFlow.hpp similarity index 100% rename from src/modules/simulation/gz_plugin/OpticalFlow.hh rename to src/modules/simulation/gz_plugin/OpticalFlow.hpp diff --git a/src/modules/simulation/gz_plugin/OpticalFlowSystem.cc b/src/modules/simulation/gz_plugin/OpticalFlowSystem.cpp similarity index 97% rename from src/modules/simulation/gz_plugin/OpticalFlowSystem.cc rename to src/modules/simulation/gz_plugin/OpticalFlowSystem.cpp index 6d46ca37d7e2..78dd64a800c3 100644 --- a/src/modules/simulation/gz_plugin/OpticalFlowSystem.cc +++ b/src/modules/simulation/gz_plugin/OpticalFlowSystem.cpp @@ -14,8 +14,8 @@ #include #include -#include "OpticalFlow.hh" -#include "OpticalFlowSystem.hh" +#include "OpticalFlow.hpp" +#include "OpticalFlowSystem.hpp" using namespace custom; diff --git a/src/modules/simulation/gz_plugin/OpticalFlowSystem.hh b/src/modules/simulation/gz_plugin/OpticalFlowSystem.hpp similarity index 100% rename from src/modules/simulation/gz_plugin/OpticalFlowSystem.hh rename to src/modules/simulation/gz_plugin/OpticalFlowSystem.hpp From b670ad4cc555e2e13b8c16077636e67dd2928c30 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 30 Dec 2024 14:40:03 -0900 Subject: [PATCH 07/16] add gz_msg for proto, fix build, test basic flow impl --- src/modules/simulation/Kconfig | 2 + .../simulation/gz_bridge/CMakeLists.txt | 3 + src/modules/simulation/gz_bridge/GZBridge.cpp | 28 +- src/modules/simulation/gz_bridge/GZBridge.hpp | 3 +- src/modules/simulation/gz_msg/CMakeLists.txt | 24 ++ src/modules/simulation/gz_msg/Kconfig | 6 + .../simulation/gz_msg/optical_flow.proto | 20 ++ .../simulation/gz_plugin/CMakeLists.txt | 8 +- .../simulation/gz_plugin/OpticalFlow.cpp | 271 +++++++++++------- .../simulation/gz_plugin/OpticalFlow.hpp | 42 ++- 10 files changed, 275 insertions(+), 132 deletions(-) create mode 100644 src/modules/simulation/gz_msg/CMakeLists.txt create mode 100644 src/modules/simulation/gz_msg/Kconfig create mode 100644 src/modules/simulation/gz_msg/optical_flow.proto diff --git a/src/modules/simulation/Kconfig b/src/modules/simulation/Kconfig index a417092ed94b..249d66e012c7 100644 --- 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_PLUGIN ---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 index e57c6020451b..553ef653255b 100644 --- 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 diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 69e4eae936bf..167a3bfde729 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -286,29 +286,20 @@ int GZBridge::init() } // TODO: change to sensor_msgs::msgs::OpticalFlow -void GZBridge::opticalFlowCallback(const gz::msgs::Image &image_msg) +void GZBridge::opticalFlowCallback(const sensor_msgs::msgs::OpticalFlow &flow) { - // float flow_x = 0; - // float flow_y = 0; - // int integration_time = 0; - // int quality = calculate_flow(image_msg, hrt_absolute_time(), integration_time, flow_x, flow_y); - - // if (quality <= 0) { - // quality = 0; - // } // Construct SensorOpticalFlow message sensor_optical_flow_s msg = {}; - // msg.pixel_flow[0] = flow_x; - // msg.pixel_flow[1] = flow_y; - // msg.quality = quality; - // msg.integration_timespan_us = integration_time; - // msg.integration_timespan_us = {1000000 / 30}; // 30 fps; + 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 - msg.timestamp = hrt_absolute_time(); - msg.timestamp_sample = msg.timestamp; device::Device::DeviceId id; id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; id.devid_s.bus = 0; @@ -319,8 +310,9 @@ void GZBridge::opticalFlowCallback(const gz::msgs::Image &image_msg) // 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.max_ground_distance = 30; + // 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 diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index b7777b861b2e..18bc783c2806 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -76,6 +76,7 @@ #include #include #include +#include using namespace time_literals; @@ -119,7 +120,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S void laserScanCallback(const gz::msgs::LaserScan &scan); // TODO: change to sensor_msgs::msgs::OpticalFlow - void opticalFlowCallback(const gz::msgs::Image &image_msg); + void opticalFlowCallback(const sensor_msgs::msgs::OpticalFlow &image_msg); /** * @brief Call Entityfactory service diff --git a/src/modules/simulation/gz_msg/CMakeLists.txt b/src/modules/simulation/gz_msg/CMakeLists.txt new file mode 100644 index 000000000000..3f0488a5d24e --- /dev/null +++ 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 new file mode 100644 index 000000000000..9acc0542d314 --- /dev/null +++ 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 new file mode 100644 index 000000000000..b922b81b434d --- /dev/null +++ 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_plugin/CMakeLists.txt b/src/modules/simulation/gz_plugin/CMakeLists.txt index 36e348816bad..eb642b7ba040 100644 --- a/src/modules/simulation/gz_plugin/CMakeLists.txt +++ b/src/modules/simulation/gz_plugin/CMakeLists.txt @@ -8,25 +8,21 @@ gz_find_package(gz-sim8 REQUIRED) gz_find_package(gz-sensors8 REQUIRED) gz_find_package(gz-transport12 REQUIRED) find_package(OpenCV REQUIRED) -find_package(Protobuf REQUIRED) - -PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS optical_flow.proto) add_library(${PROJECT_NAME} SHARED OpticalFlow.cpp OpticalFlowSystem.cpp - ${PROTO_SRCS} - ${PROTO_HDRS} ) 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 ${PROTOBUF_LIBRARIES} + # PUBLIC ${PROTOBUF_LIBRARIES} ) target_include_directories(${PROJECT_NAME} diff --git a/src/modules/simulation/gz_plugin/OpticalFlow.cpp b/src/modules/simulation/gz_plugin/OpticalFlow.cpp index 87694ae85006..f907639538f2 100644 --- a/src/modules/simulation/gz_plugin/OpticalFlow.cpp +++ b/src/modules/simulation/gz_plugin/OpticalFlow.cpp @@ -9,109 +9,186 @@ using namespace custom; bool OpticalFlow::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; - } - - gzdbg << "Loading optical flow sensor..." << std::endl; - gz::sensors::Sensor::Load(_sdf); - - gzdbg << "Adverising optical flow sensor on: " << this->Topic() << std::endl; - this->pub = this->node.Advertise(this->Topic()); - - auto elem = _sdf.Element(); - auto opticalFlowElem = elem->GetElement("gz:optical_flow"); - auto cameraTopic = opticalFlowElem->Get("camera_topic"); - - gzdbg << "Subscribing to camera topic: " << cameraTopic << std::endl; - - if (!this->node.Subscribe(cameraTopic, &OpticalFlow::OnImage, this)) { - gzerr << "Failed to subscribe to camera topic: " << cameraTopic << std::endl; - return false; - } - - this->lastUpdateTime = std::chrono::steady_clock::now(); - - return true; + 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); + + this->pub = this->node.Advertise(this->Topic()); + gzdbg << "Advertising optical flow data on: " << this->Topic() << std::endl; + + // Get camera topic from our sensor config + auto elem = _sdf.Element(); + auto opticalFlowElem = elem->GetElement("gz:optical_flow"); + auto cameraTopic = opticalFlowElem->Get("camera_topic"); + + // Get FOV from the actual camera sensor's config + auto cameraElem = elem->GetParent()->GetElement("sensor"); + while (cameraElem) { + if (cameraElem->Get("name") == "flow_camera") { + auto camera = cameraElem->GetElement("camera"); + this->horizontal_fov = camera->GetElement("horizontal_fov")->Get(); + this->vertical_fov = this->horizontal_fov * 0.75; // Assume 4:3 aspect ratio + break; + } + cameraElem = cameraElem->GetNextElement("sensor"); + } + + gzdbg << "Using camera FOV - horizontal: " << this->horizontal_fov + << " vertical: " << this->vertical_fov << std::endl; + + // Subscribe to camera + gzdbg << "Subscribing to camera topic: " << cameraTopic << std::endl; + if (!this->node.Subscribe(cameraTopic, &OpticalFlow::OnImage, this)) { + gzerr << "Failed to subscribe to camera topic: " << cameraTopic << std::endl; + return false; + } + + this->lastUpdateTime = std::chrono::steady_clock::now(); + return true; } void OpticalFlow::OnImage(const gz::msgs::Image &_msg) { - // gzdbg << "OpticalFlow::OnImage" << std::endl; - - cv::Mat frame; - - if (_msg.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8) { - frame = cv::Mat(_msg.height(), _msg.width(), CV_8UC3, (void *)_msg.data().c_str()); - cv::cvtColor(frame, frame, cv::COLOR_RGB2GRAY); - - } else if (_msg.pixel_format_type() == gz::msgs::PixelFormatType::L_INT8) { - frame = cv::Mat(_msg.height(), _msg.width(), CV_8UC1, (void *)_msg.data().c_str()); - - } else { - gzerr << "Unsupported image format" << std::endl; - return; - } - - if (prevFrame.empty()) { - frame.copyTo(prevFrame); - return; - } - - cv::Mat flow; - cv::calcOpticalFlowFarneback(prevFrame, frame, flow, - 0.5, // Pyramid scale - 3, // Pyramid levels - 15, // Window size - 3, // Iteratpions - 5, // Poly_n - 1.2, // Poly_sigma - 0); // Flags - - // Calculate average flow and quality - cv::Scalar meanFlow = cv::mean(flow); - - // Update integrated flow (in radians) - // TODO: FOV from SDF - // Assuming 60° horizontal field of view and 45° vertical field of view - const double rad_per_pixel_x = (M_PI / 3.0) / frame.cols; // 60° in radians / width - const double rad_per_pixel_y = (M_PI / 4.0) / frame.rows; // 45° in radians / height - - this->integrated_x = meanFlow[0] * rad_per_pixel_x; - this->integrated_y = meanFlow[1] * rad_per_pixel_y; - - // Calculate quality (0-255) - cv::Mat magnitude, angle; - cv::cartToPolar(flow.col(0), flow.col(1), magnitude, angle); - this->quality = cv::mean(magnitude)[0] * 255.0; - - if (this->quality > 255.0f) { - this->quality = 255.0f; - } - - frame.copyTo(prevFrame); + if (_msg.width() == 0 || _msg.height() == 0) { + gzerr << "Invalid image dimensions" << std::endl; + return; + } + + cv::Mat frame; + + // Convert image to grayscale + if (_msg.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8) { + frame = cv::Mat(_msg.height(), _msg.width(), CV_8UC3, (void *)_msg.data().c_str()); + cv::cvtColor(frame, frame, cv::COLOR_RGB2GRAY); + } else if (_msg.pixel_format_type() == gz::msgs::PixelFormatType::L_INT8) { + frame = cv::Mat(_msg.height(), _msg.width(), CV_8UC1, (void *)_msg.data().c_str()); + } else { + gzerr << "Unsupported image format" << std::endl; + return; + } + + // Preprocess image + cv::GaussianBlur(frame, frame, this->blur_size, this->blur_sigma); + + // Scale down for performance + cv::Mat scaled_frame; + cv::resize(frame, scaled_frame, cv::Size(), this->scale_factor, this->scale_factor); + + ProcessFlow(scaled_frame); } -bool OpticalFlow::Update(const std::chrono::steady_clock::duration &_now) +void OpticalFlow::ProcessFlow(const cv::Mat ¤t_frame) { - // gzdbg << "OpticalFlow::Update" << std::endl; - - auto currentTime = std::chrono::steady_clock::now(); - auto deltaTime = std::chrono::duration_cast(currentTime - this->lastUpdateTime); - - sensor_msgs::msgs::OpticalFlow msg; - msg.set_time_usec(std::chrono::duration_cast(_now).count()); - msg.set_integration_time_us(deltaTime.count()); - msg.set_integrated_x(this->integrated_x); - msg.set_integrated_y(this->integrated_y); - msg.set_quality(this->quality); - msg.set_time_delta_distance_us(deltaTime.count()); - - this->pub.Publish(msg); - this->lastUpdateTime = currentTime; + if (!flow_initialized) { + current_frame.copyTo(prevFrame); + flow_initialized = true; + return; + } + + // Detect features in previous frame + std::vector current_points; + std::vector status; + std::vector err; + + if (prev_points.empty()) { + cv::goodFeaturesToTrack(prevFrame, prev_points, max_corners, quality_level, min_distance); + } + + if (prev_points.empty()) { + gzwarn << "No features detected in previous frame" << std::endl; + current_frame.copyTo(prevFrame); + return; + } + + // Calculate optical flow + cv::calcOpticalFlowPyrLK(prevFrame, current_frame, prev_points, current_points, status, err); + + // Filter valid points and calculate flow + std::vector good_old, good_new; + for (size_t i = 0; i < status.size(); i++) { + if (status[i]) { + good_old.push_back(prev_points[i]); + good_new.push_back(current_points[i]); + } + } + + if (good_new.empty() || good_old.empty()) { + gzwarn << "No valid flow vectors" << std::endl; + quality = 0; + current_frame.copyTo(prevFrame); + prev_points.clear(); + return; + } + + // Calculate average flow + cv::Point2f mean_flow(0, 0); + for (size_t i = 0; i < good_new.size(); i++) { + mean_flow += good_new[i] - good_old[i]; + } + mean_flow = mean_flow * (1.0f / good_new.size()); + + // Convert to radians using FOV and resolution + double rad_per_pixel_x = horizontal_fov / (current_frame.cols / double(scale_factor)); + double rad_per_pixel_y = vertical_fov / (current_frame.rows / double(scale_factor)); + + integrated_x = (double)mean_flow.x * rad_per_pixel_x; + integrated_y = (double)mean_flow.y * rad_per_pixel_y; + + // Calculate quality metric + std::vector flow_magnitudes; + for (size_t i = 0; i < good_new.size(); i++) { + cv::Point2f flow = good_new[i] - good_old[i]; + flow_magnitudes.push_back(cv::norm(flow)); + } + + float avg_magnitude = 0; + if (!flow_magnitudes.empty()) { + avg_magnitude = std::accumulate(flow_magnitudes.begin(), flow_magnitudes.end(), 0.0f) + / flow_magnitudes.size(); + } + + // Compute quality based on flow consistency and magnitude + float std_dev = 0; + for (float mag : flow_magnitudes) { + std_dev += (mag - avg_magnitude) * (mag - avg_magnitude); + } + std_dev = std_dev > 0 ? sqrt(std_dev / flow_magnitudes.size()) : 0; + + // Higher quality when flow is consistent (low std_dev) and has reasonable magnitude + quality = std::min(255.0f, (avg_magnitude * 100.0f) / (std_dev + 1.0f)); + + // Check for excessive motion + if (std::abs(integrated_x) > M_PI_2 || std::abs(integrated_y) > M_PI_2) { + gzwarn << "Excessive motion detected" << std::endl; + quality = 0; + } + + // Update state for next iteration + current_frame.copyTo(prevFrame); + prev_points = good_new; // Use current good points for next iteration +} - return true; +bool OpticalFlow::Update(const std::chrono::steady_clock::duration &_now) +{ + auto currentTime = std::chrono::steady_clock::now(); + auto deltaTime = std::chrono::duration_cast( + currentTime - this->lastUpdateTime); + + sensor_msgs::msgs::OpticalFlow msg; + msg.set_time_usec(std::chrono::duration_cast(_now).count()); + msg.set_integration_time_us(deltaTime.count()); + msg.set_integrated_x(this->integrated_x); + msg.set_integrated_y(this->integrated_y); + msg.set_quality(this->quality); + + if (!this->pub.Publish(msg)) { + gzwarn << "Failed to publish optical flow message" << std::endl; + } + + this->lastUpdateTime = currentTime; + return true; } diff --git a/src/modules/simulation/gz_plugin/OpticalFlow.hpp b/src/modules/simulation/gz_plugin/OpticalFlow.hpp index 11401b6b932b..19645a136e7f 100644 --- a/src/modules/simulation/gz_plugin/OpticalFlow.hpp +++ b/src/modules/simulation/gz_plugin/OpticalFlow.hpp @@ -6,25 +6,47 @@ #include #include #include +#include +#include namespace custom { class OpticalFlow : public gz::sensors::Sensor { public: - virtual bool Load(const sdf::Sensor &_sdf) override; - virtual bool Update(const std::chrono::steady_clock::duration &_now) override; + 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); + void OnImage(const gz::msgs::Image &_msg); + void ProcessFlow(const cv::Mat ¤t_frame); - cv::Mat prevFrame; - gz::transport::Node node; - gz::transport::Node::Publisher pub; + cv::Mat prevFrame; + gz::transport::Node node; + gz::transport::Node::Publisher pub; - float integrated_x{0.0}; - float integrated_y{0.0}; - float quality{0.0}; - std::chrono::steady_clock::time_point lastUpdateTime; + // Camera parameters + double horizontal_fov{0.79}; // Default FOV in radians + double vertical_fov{0.6}; // Default FOV in radians + + // Flow computation parameters + const int max_corners{100}; + const double quality_level{0.3}; + const double min_distance{7.0}; + + // Flow state + double integrated_x{0.0}; + double integrated_y{0.0}; + double quality{0.0}; + std::chrono::steady_clock::time_point lastUpdateTime; + + // Image processing parameters + const cv::Size blur_size{5, 5}; + const double blur_sigma{1.5}; + const float scale_factor{0.5}; // Scale image down for performance + + // Previous points for optical flow + std::vector prev_points; + bool flow_initialized{false}; }; } // end namespace custom From 5f8c48322edbcc17c6bb41970fb3af7e97447800 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 30 Dec 2024 15:26:14 -0900 Subject: [PATCH 08/16] update rate, no blur --- .../simulation/gz_plugin/OpticalFlow.cpp | 24 +++++++++++++------ .../simulation/gz_plugin/OpticalFlow.hpp | 2 ++ 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/src/modules/simulation/gz_plugin/OpticalFlow.cpp b/src/modules/simulation/gz_plugin/OpticalFlow.cpp index f907639538f2..927bcab6511c 100644 --- a/src/modules/simulation/gz_plugin/OpticalFlow.cpp +++ b/src/modules/simulation/gz_plugin/OpticalFlow.cpp @@ -72,13 +72,14 @@ void OpticalFlow::OnImage(const gz::msgs::Image &_msg) } // Preprocess image - cv::GaussianBlur(frame, frame, this->blur_size, this->blur_sigma); + // cv::GaussianBlur(frame, frame, this->blur_size, this->blur_sigma); - // Scale down for performance - cv::Mat scaled_frame; - cv::resize(frame, scaled_frame, cv::Size(), this->scale_factor, this->scale_factor); + // // Scale down for performance + // cv::Mat scaled_frame; + // cv::resize(frame, scaled_frame, cv::Size(), this->scale_factor, this->scale_factor); - ProcessFlow(scaled_frame); + // ProcessFlow(scaled_frame); + ProcessFlow(frame); } void OpticalFlow::ProcessFlow(const cv::Mat ¤t_frame) @@ -132,8 +133,10 @@ void OpticalFlow::ProcessFlow(const cv::Mat ¤t_frame) mean_flow = mean_flow * (1.0f / good_new.size()); // Convert to radians using FOV and resolution - double rad_per_pixel_x = horizontal_fov / (current_frame.cols / double(scale_factor)); - double rad_per_pixel_y = vertical_fov / (current_frame.rows / double(scale_factor)); + // double rad_per_pixel_x = horizontal_fov / (current_frame.cols / double(scale_factor)); + // double rad_per_pixel_y = vertical_fov / (current_frame.rows / double(scale_factor)); + double rad_per_pixel_x = horizontal_fov / current_frame.cols; + double rad_per_pixel_y = vertical_fov / current_frame.rows; integrated_x = (double)mean_flow.x * rad_per_pixel_x; integrated_y = (double)mean_flow.y * rad_per_pixel_y; @@ -170,10 +173,16 @@ void OpticalFlow::ProcessFlow(const cv::Mat ¤t_frame) // Update state for next iteration current_frame.copyTo(prevFrame); prev_points = good_new; // Use current good points for next iteration + + flow_updated = true; } bool OpticalFlow::Update(const std::chrono::steady_clock::duration &_now) { + if (!flow_updated) { + return true; + } + auto currentTime = std::chrono::steady_clock::now(); auto deltaTime = std::chrono::duration_cast( currentTime - this->lastUpdateTime); @@ -189,6 +198,7 @@ bool OpticalFlow::Update(const std::chrono::steady_clock::duration &_now) gzwarn << "Failed to publish optical flow message" << std::endl; } + flow_updated = false; this->lastUpdateTime = currentTime; return true; } diff --git a/src/modules/simulation/gz_plugin/OpticalFlow.hpp b/src/modules/simulation/gz_plugin/OpticalFlow.hpp index 19645a136e7f..b53f0d7bf500 100644 --- a/src/modules/simulation/gz_plugin/OpticalFlow.hpp +++ b/src/modules/simulation/gz_plugin/OpticalFlow.hpp @@ -45,6 +45,8 @@ class OpticalFlow : public gz::sensors::Sensor const double blur_sigma{1.5}; const float scale_factor{0.5}; // Scale image down for performance + bool flow_updated{false}; + // Previous points for optical flow std::vector prev_points; bool flow_initialized{false}; From e5b6194087a582ce56bd80b7c2643fd7f47c87a2 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 6 Jan 2025 21:57:51 -0900 Subject: [PATCH 09/16] PX4-OpticalFlow impl --- Tools/simulation/gz | 2 +- .../simulation/gz_plugin/CMakeLists.txt | 20 ++ .../simulation/gz_plugin/OpticalFlow.cpp | 202 ++++++------------ .../simulation/gz_plugin/OpticalFlow.hpp | 46 ++-- .../gz_plugin/OpticalFlowSystem.cpp | 2 +- .../gz_plugin/OpticalFlowSystem.hpp | 2 +- 6 files changed, 103 insertions(+), 171 deletions(-) diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 887090a7cf34..a530ba14166b 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 887090a7cf3464a834417081f0b864ca91f0663d +Subproject commit a530ba14166b756848b285efbba9d49f7a0a546e diff --git a/src/modules/simulation/gz_plugin/CMakeLists.txt b/src/modules/simulation/gz_plugin/CMakeLists.txt index eb642b7ba040..13453014dc51 100644 --- a/src/modules/simulation/gz_plugin/CMakeLists.txt +++ b/src/modules/simulation/gz_plugin/CMakeLists.txt @@ -9,6 +9,22 @@ 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= + 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 OpticalFlow.cpp OpticalFlowSystem.cpp @@ -22,6 +38,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC gz-sim8::gz-sim8 PUBLIC gz-transport12::gz-transport12 PUBLIC ${OpenCV_LIBS} + PUBLIC ${OpticalFlow_LIBS} # PUBLIC ${PROTOBUF_LIBRARIES} ) @@ -29,4 +46,7 @@ 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_plugin/OpticalFlow.cpp b/src/modules/simulation/gz_plugin/OpticalFlow.cpp index 927bcab6511c..2b594ad44b43 100644 --- a/src/modules/simulation/gz_plugin/OpticalFlow.cpp +++ b/src/modules/simulation/gz_plugin/OpticalFlow.cpp @@ -7,7 +7,7 @@ using namespace custom; -bool OpticalFlow::Load(const sdf::Sensor &_sdf) +bool OpticalFlowSensor::Load(const sdf::Sensor &_sdf) { auto type = gz::sensors::customType(_sdf); if ("optical_flow" != type) { @@ -17,188 +17,110 @@ bool OpticalFlow::Load(const sdf::Sensor &_sdf) gz::sensors::Sensor::Load(_sdf); - this->pub = this->node.Advertise(this->Topic()); + _publisher = _node.Advertise(this->Topic()); gzdbg << "Advertising optical flow data on: " << this->Topic() << std::endl; // Get camera topic from our sensor config auto elem = _sdf.Element(); auto opticalFlowElem = elem->GetElement("gz:optical_flow"); - auto cameraTopic = opticalFlowElem->Get("camera_topic"); + auto camera_topic = opticalFlowElem->Get("camera_topic"); + + std::string topic; + int image_width = 0; + int image_height = 0; + int update_rate = 0; + float hfov = 0; // Get FOV from the actual camera sensor's config - auto cameraElem = elem->GetParent()->GetElement("sensor"); - while (cameraElem) { - if (cameraElem->Get("name") == "flow_camera") { - auto camera = cameraElem->GetElement("camera"); - this->horizontal_fov = camera->GetElement("horizontal_fov")->Get(); - this->vertical_fov = this->horizontal_fov * 0.75; // Assume 4:3 aspect ratio + auto sensorElem = elem->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; } - cameraElem = cameraElem->GetNextElement("sensor"); + sensorElem = sensorElem->GetNextElement("sensor"); } - gzdbg << "Using camera FOV - horizontal: " << this->horizontal_fov - << " vertical: " << this->vertical_fov << std::endl; + gzdbg << "image_width: " << image_width << std::endl; + gzdbg << "image_height: " << image_height << std::endl; + gzdbg << "update_rate: " << update_rate << std::endl; + gzdbg << "hfov: " << hfov << std::endl; // Subscribe to camera - gzdbg << "Subscribing to camera topic: " << cameraTopic << std::endl; - if (!this->node.Subscribe(cameraTopic, &OpticalFlow::OnImage, this)) { - gzerr << "Failed to subscribe to camera topic: " << cameraTopic << std::endl; + gzdbg << "Subscribing to camera topic: " << 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; } - this->lastUpdateTime = std::chrono::steady_clock::now(); + // TODO: get from sdf + float focal_length = (image_width / 2.0f) / tan(hfov / 2.0f); + + // Create OpticalFlow + _optical_flow = std::make_shared(focal_length, focal_length, update_rate, image_width, image_height); + return true; } -void OpticalFlow::OnImage(const gz::msgs::Image &_msg) +void OpticalFlowSensor::OnImage(const gz::msgs::Image &image_msg) { - if (_msg.width() == 0 || _msg.height() == 0) { + if (image_msg.width() == 0 || image_msg.height() == 0) { gzerr << "Invalid image dimensions" << std::endl; return; } - cv::Mat frame; + 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()); - // Convert image to grayscale - if (_msg.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8) { - frame = cv::Mat(_msg.height(), _msg.width(), CV_8UC3, (void *)_msg.data().c_str()); - cv::cvtColor(frame, frame, cv::COLOR_RGB2GRAY); - } else if (_msg.pixel_format_type() == gz::msgs::PixelFormatType::L_INT8) { - frame = cv::Mat(_msg.height(), _msg.width(), CV_8UC1, (void *)_msg.data().c_str()); } else { gzerr << "Unsupported image format" << std::endl; return; } - // Preprocess image - // cv::GaussianBlur(frame, frame, this->blur_size, this->blur_sigma); - - // // Scale down for performance - // cv::Mat scaled_frame; - // cv::resize(frame, scaled_frame, cv::Size(), this->scale_factor, this->scale_factor); - - // ProcessFlow(scaled_frame); - ProcessFlow(frame); -} - -void OpticalFlow::ProcessFlow(const cv::Mat ¤t_frame) -{ - if (!flow_initialized) { - current_frame.copyTo(prevFrame); - flow_initialized = true; - return; - } - - // Detect features in previous frame - std::vector current_points; - std::vector status; - std::vector err; - - if (prev_points.empty()) { - cv::goodFeaturesToTrack(prevFrame, prev_points, max_corners, quality_level, min_distance); - } - - if (prev_points.empty()) { - gzwarn << "No features detected in previous frame" << std::endl; - current_frame.copyTo(prevFrame); - return; - } - - // Calculate optical flow - cv::calcOpticalFlowPyrLK(prevFrame, current_frame, prev_points, current_points, status, err); - - // Filter valid points and calculate flow - std::vector good_old, good_new; - for (size_t i = 0; i < status.size(); i++) { - if (status[i]) { - good_old.push_back(prev_points[i]); - good_new.push_back(current_points[i]); - } - } - - if (good_new.empty() || good_old.empty()) { - gzwarn << "No valid flow vectors" << std::endl; - quality = 0; - current_frame.copyTo(prevFrame); - prev_points.clear(); - return; - } - - // Calculate average flow - cv::Point2f mean_flow(0, 0); - for (size_t i = 0; i < good_new.size(); i++) { - mean_flow += good_new[i] - good_old[i]; - } - mean_flow = mean_flow * (1.0f / good_new.size()); - - // Convert to radians using FOV and resolution - // double rad_per_pixel_x = horizontal_fov / (current_frame.cols / double(scale_factor)); - // double rad_per_pixel_y = vertical_fov / (current_frame.rows / double(scale_factor)); - double rad_per_pixel_x = horizontal_fov / current_frame.cols; - double rad_per_pixel_y = vertical_fov / current_frame.rows; - - integrated_x = (double)mean_flow.x * rad_per_pixel_x; - integrated_y = (double)mean_flow.y * rad_per_pixel_y; - - // Calculate quality metric - std::vector flow_magnitudes; - for (size_t i = 0; i < good_new.size(); i++) { - cv::Point2f flow = good_new[i] - good_old[i]; - flow_magnitudes.push_back(cv::norm(flow)); - } + // Store current timestamp for integration time calculation + uint32_t current_timestamp = (image_msg.header().stamp().sec() * 1000000ULL + + image_msg.header().stamp().nsec() / 1000ULL) & 0xFFFFFFFF; - float avg_magnitude = 0; - if (!flow_magnitudes.empty()) { - avg_magnitude = std::accumulate(flow_magnitudes.begin(), flow_magnitudes.end(), 0.0f) - / flow_magnitudes.size(); + if (_last_image_timestamp != 0) { + _integration_time_us = (current_timestamp - _last_image_timestamp) & 0xFFFFFFFF; } - // Compute quality based on flow consistency and magnitude - float std_dev = 0; - for (float mag : flow_magnitudes) { - std_dev += (mag - avg_magnitude) * (mag - avg_magnitude); - } - std_dev = std_dev > 0 ? sqrt(std_dev / flow_magnitudes.size()) : 0; - - // Higher quality when flow is consistent (low std_dev) and has reasonable magnitude - quality = std::min(255.0f, (avg_magnitude * 100.0f) / (std_dev + 1.0f)); - - // Check for excessive motion - if (std::abs(integrated_x) > M_PI_2 || std::abs(integrated_y) > M_PI_2) { - gzwarn << "Excessive motion detected" << std::endl; - quality = 0; - } - - // Update state for next iteration - current_frame.copyTo(prevFrame); - prev_points = good_new; // Use current good points for next iteration - - flow_updated = true; + _last_image_timestamp = current_timestamp; + _new_image_available = true; } -bool OpticalFlow::Update(const std::chrono::steady_clock::duration &_now) +bool OpticalFlowSensor::Update(const std::chrono::steady_clock::duration &_now) { - if (!flow_updated) { + if (!_new_image_available) { return true; } - auto currentTime = std::chrono::steady_clock::now(); - auto deltaTime = std::chrono::duration_cast( - currentTime - this->lastUpdateTime); - sensor_msgs::msgs::OpticalFlow msg; - msg.set_time_usec(std::chrono::duration_cast(_now).count()); - msg.set_integration_time_us(deltaTime.count()); - msg.set_integrated_x(this->integrated_x); - msg.set_integrated_y(this->integrated_y); - msg.set_quality(this->quality); + msg.set_time_usec(_last_image_timestamp); + + 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 (!this->pub.Publish(msg)) { + if (!_publisher.Publish(msg)) { gzwarn << "Failed to publish optical flow message" << std::endl; } - flow_updated = false; - this->lastUpdateTime = currentTime; + _new_image_available = false; return true; } diff --git a/src/modules/simulation/gz_plugin/OpticalFlow.hpp b/src/modules/simulation/gz_plugin/OpticalFlow.hpp index b53f0d7bf500..ae62daf2532c 100644 --- a/src/modules/simulation/gz_plugin/OpticalFlow.hpp +++ b/src/modules/simulation/gz_plugin/OpticalFlow.hpp @@ -8,10 +8,13 @@ #include #include #include +#include + +#include "flow_opencv.hpp" namespace custom { -class OpticalFlow : public gz::sensors::Sensor +class OpticalFlowSensor : public gz::sensors::Sensor { public: virtual bool Load(const sdf::Sensor &_sdf) override; @@ -19,36 +22,23 @@ class OpticalFlow : public gz::sensors::Sensor private: void OnImage(const gz::msgs::Image &_msg); - void ProcessFlow(const cv::Mat ¤t_frame); - - cv::Mat prevFrame; - gz::transport::Node node; - gz::transport::Node::Publisher pub; - - // Camera parameters - double horizontal_fov{0.79}; // Default FOV in radians - double vertical_fov{0.6}; // Default FOV in radians - // Flow computation parameters - const int max_corners{100}; - const double quality_level{0.3}; - const double min_distance{7.0}; + gz::transport::Node _node; + gz::transport::Node::Publisher _publisher; - // Flow state - double integrated_x{0.0}; - double integrated_y{0.0}; - double quality{0.0}; - std::chrono::steady_clock::time_point lastUpdateTime; + // Flow + std::shared_ptr _optical_flow {nullptr}; + float _flow_x {0.0f}; + float _flow_y {0.0f}; + int _integration_time_us; - // Image processing parameters - const cv::Size blur_size{5, 5}; - const double blur_sigma{1.5}; - const float scale_factor{0.5}; // Scale image down for performance + // Camera + double _horizontal_fov {0.0}; + double _vertical_fov {0.0}; - bool flow_updated{false}; - - // Previous points for optical flow - std::vector prev_points; - bool flow_initialized{false}; + 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_plugin/OpticalFlowSystem.cpp b/src/modules/simulation/gz_plugin/OpticalFlowSystem.cpp index 78dd64a800c3..12424f121602 100644 --- a/src/modules/simulation/gz_plugin/OpticalFlowSystem.cpp +++ b/src/modules/simulation/gz_plugin/OpticalFlowSystem.cpp @@ -37,7 +37,7 @@ void OpticalFlowSystem::PreUpdate(const gz::sim::UpdateInfo &, gz::sim::EntityCo } gz::sensors::SensorFactory sensorFactory; - auto sensor = sensorFactory.CreateSensor(data); + auto sensor = sensorFactory.CreateSensor(data); if (sensor == nullptr) { gzerr << "Failed to create optical flow sensor [" << sensorScopedName << "]" << std::endl; diff --git a/src/modules/simulation/gz_plugin/OpticalFlowSystem.hpp b/src/modules/simulation/gz_plugin/OpticalFlowSystem.hpp index c2095d5e9c10..3f868c6eeb65 100644 --- a/src/modules/simulation/gz_plugin/OpticalFlowSystem.hpp +++ b/src/modules/simulation/gz_plugin/OpticalFlowSystem.hpp @@ -21,6 +21,6 @@ class OpticalFlowSystem: private: void RemoveSensorEntities(const gz::sim::EntityComponentManager &_ecm); - std::unordered_map> entitySensorMap; + std::unordered_map> entitySensorMap; }; } // end namespace custom From 64890163a236b521c13a7b29679742430d079251 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 6 Jan 2025 23:36:39 -0900 Subject: [PATCH 10/16] rename OpticalFlowSensor --- src/modules/simulation/gz_plugin/CMakeLists.txt | 2 +- .../gz_plugin/{OpticalFlow.cpp => OpticalFlowSensor.cpp} | 2 +- .../gz_plugin/{OpticalFlow.hpp => OpticalFlowSensor.hpp} | 0 src/modules/simulation/gz_plugin/OpticalFlowSystem.cpp | 2 +- 4 files changed, 3 insertions(+), 3 deletions(-) rename src/modules/simulation/gz_plugin/{OpticalFlow.cpp => OpticalFlowSensor.cpp} (99%) rename src/modules/simulation/gz_plugin/{OpticalFlow.hpp => OpticalFlowSensor.hpp} (100%) diff --git a/src/modules/simulation/gz_plugin/CMakeLists.txt b/src/modules/simulation/gz_plugin/CMakeLists.txt index 13453014dc51..d490d2223c0f 100644 --- a/src/modules/simulation/gz_plugin/CMakeLists.txt +++ b/src/modules/simulation/gz_plugin/CMakeLists.txt @@ -26,7 +26,7 @@ set(OpticalFlow_INCLUDE_DIRS ${install_dir}/include) set(OpticalFlow_LIBS ${install_dir}/lib/libOpticalFlow.so) add_library(${PROJECT_NAME} SHARED - OpticalFlow.cpp + OpticalFlowSensor.cpp OpticalFlowSystem.cpp ) diff --git a/src/modules/simulation/gz_plugin/OpticalFlow.cpp b/src/modules/simulation/gz_plugin/OpticalFlowSensor.cpp similarity index 99% rename from src/modules/simulation/gz_plugin/OpticalFlow.cpp rename to src/modules/simulation/gz_plugin/OpticalFlowSensor.cpp index 2b594ad44b43..8249d80ff97e 100644 --- a/src/modules/simulation/gz_plugin/OpticalFlow.cpp +++ b/src/modules/simulation/gz_plugin/OpticalFlowSensor.cpp @@ -2,7 +2,7 @@ #include #include -#include "OpticalFlow.hpp" +#include "OpticalFlowSensor.hpp" #include "optical_flow.pb.h" using namespace custom; diff --git a/src/modules/simulation/gz_plugin/OpticalFlow.hpp b/src/modules/simulation/gz_plugin/OpticalFlowSensor.hpp similarity index 100% rename from src/modules/simulation/gz_plugin/OpticalFlow.hpp rename to src/modules/simulation/gz_plugin/OpticalFlowSensor.hpp diff --git a/src/modules/simulation/gz_plugin/OpticalFlowSystem.cpp b/src/modules/simulation/gz_plugin/OpticalFlowSystem.cpp index 12424f121602..47eda76b6f00 100644 --- a/src/modules/simulation/gz_plugin/OpticalFlowSystem.cpp +++ b/src/modules/simulation/gz_plugin/OpticalFlowSystem.cpp @@ -14,7 +14,7 @@ #include #include -#include "OpticalFlow.hpp" +#include "OpticalFlowSensor.hpp" #include "OpticalFlowSystem.hpp" using namespace custom; From 80ffbfe6814c3620418a7ac28157411a65ed3afa Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 7 Jan 2025 00:37:14 -0900 Subject: [PATCH 11/16] rename plugins --- boards/ark/fpv/extras/ark_fpv_bootloader.bin | Bin 46268 -> 0 bytes boards/px4/sitl/default.px4board | 2 +- src/modules/simulation/Kconfig | 2 +- .../{gz_plugin => gz_plugins}/CMakeLists.txt | 0 .../{gz_plugin => gz_plugins}/Kconfig | 2 +- .../OpticalFlowSensor.cpp | 0 .../OpticalFlowSensor.hpp | 0 .../OpticalFlowSystem.cpp | 0 .../OpticalFlowSystem.hpp | 0 .../optical_flow.proto | 0 10 files changed, 3 insertions(+), 3 deletions(-) delete mode 100755 boards/ark/fpv/extras/ark_fpv_bootloader.bin rename src/modules/simulation/{gz_plugin => gz_plugins}/CMakeLists.txt (100%) rename src/modules/simulation/{gz_plugin => gz_plugins}/Kconfig (70%) rename src/modules/simulation/{gz_plugin => gz_plugins}/OpticalFlowSensor.cpp (100%) rename src/modules/simulation/{gz_plugin => gz_plugins}/OpticalFlowSensor.hpp (100%) rename src/modules/simulation/{gz_plugin => gz_plugins}/OpticalFlowSystem.cpp (100%) rename src/modules/simulation/{gz_plugin => gz_plugins}/OpticalFlowSystem.hpp (100%) rename src/modules/simulation/{gz_plugin => gz_plugins}/optical_flow.proto (100%) diff --git a/boards/ark/fpv/extras/ark_fpv_bootloader.bin b/boards/ark/fpv/extras/ark_fpv_bootloader.bin deleted file mode 100755 index 9ca59954b374a13b5a6b739d301164a0ff7b46a1..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 46268 zcmeFZdw5e-+BdxRA<1qV+Vp^=2WXOAux*M4TC|9Vkc8bs&=%2oIXIe9)LqL+6`Vn3 zN}#AkW}ZQ+GaQ^56lVrSg;eTTv8Dn#I*#)+(9T<_JXTQyE$9v{WcQfww-V~isPkR# z_uu!&o9o(H`>+o8TI*i-x{s?qljLYPBD~1{-|PR5!T*17fL#7X&K=qQELmd6p4dg(K+ZXJc9cbZl;OZ zO{`?#buUfr0|&i#xKF6Y9Lr$c3X77&IurqI8govlS$NmG!lBslob}J=LGOy#Gs-A+ z$WOo1{HA|-HszF+A60X6O7U-t{uk%47s>3Ssy$}`-W4D}$#E$AZas>W-ao~RbkQ^g zX)z!oIok+*qtA6HTC||g1(dmtLAwL3Mem>DN4jPrC!f0=ceBbZ=48(a)c`1TfQmyI z18O$puu3s&%pVGzP;Wzz?6*1w30l1k-n!Q6u5;@lLfHb&zVO!XbAz@_mBupCIi-+F6v0+In+RoyXK( zygP$;XONyldJd_AR6+U$(l3w>A{{I;v=KjrpLofld%qd)BP7B^7-Fa^iJY??!dlu) zM6czW0Yk%suW!QYXc#awQmnngJ*6D;TAbX-4SFA1=VN9ids7-8@*VLJk$(T8c{6e! z_VFg$TSU3#jYU3Y?)f0e|E2@jyGBTYY?q( zLhI&O>l-h(t}6?mEG_d6Qex)a>E08~bmd0Ee~k8pmbJB9hJAXh^kbJxbF$dSFsGa2 zoveqJv9(3cZ8}ISS!D+=KC&U9n^+td@)xblO~ze(t`YyjVLIo`c4qfh2YYJI#YgJS z1~Ko2Ow+|+>@9O@W$~An-(8Bnd+p-=8;#2>Y^G+*siYflbw>e)~!;lFIK&b?= z0K-5Z(q;k+fq7{&C%)E|T-y?VvtF{w39VUDo`=~+l#6Pf%MiW}Vs-GVs|2@t<%Z zkCVGtI}qG}r^yTJ zqEF|7&lIp9S11vip)Gmgw&=Uhg9bOBo8aLB6`)|Lp5sAHE{l6os0OvsJFhqpOv*hS z(79>rz!yLN8R->p>2Vow<(x;_i+3%wSEe%B!g85YlDK7AT&P&AD%?%xt9-&%$4G$Y zFqUDhO|4M8vp{L4g4W?OO85M^|jSQ5)P6H&s$ zsKs(z&ALHUHaMM1uk#4*6_Mi3ir0lUFJJN2Ekm~SF>HoM4W_n`Z}ZLjfqe1K49`%ot|Zpq6KcWC6Kd`a zbdMw7TbHkxnNp$@53PEM2!Fkw$roCL4mLl&Bcrv`BCIBgs1lRV7QdQv(>miEo0FqKQvROP?m@690h#P>L4j5K(xQd5g5g zp80%F4^gG46^<7Vg&JZXKC|I~8{-NuhTP z?wCpRj>8=%&^sOObR4})!d+6^86}&=9z*g$wA&^iWUl+qPqBXgf9YRt{+u4#N3@x3 zQrUSGeW}is1KG}szBJd}1KF;MzR3&j8OUBx(RYP(??ARx(Kn?;9>^}K=rfkq3}ly9 z^re^GH;`Rc(U(zPJCI#o(U<97HIVJ5@K^L@EkvCk#>VWUle9m4{%?BdcullL)8|7x z`(|*WfB$c@!RJRVtvf)=(|H4|;zT{MdE{BFO+w;{V8A7`Fv{)Y3|0$IF|36s0W8JcCgopD!ZJ^H-3DFZb6?$y2789 zohCU|zSbgf{k&e6pW6Mouv*dSbR(?edOMddx;Fdy>u3%ppZz#ip7o{IDQz$JvpV6r zznmraoDOpN|L9hNO#b_U^g5=OU2HqS10~nfNoCbglc^ zieqOrgOL$ZuIcVM{1#)c~O=MZ^`noqP31uDn>zn7*1&$=fy%#q0Cqp zoU+MY!YRkc^W8?g;XS6(=Qa@Kweh|3w9@4@cX=4)?eR=dl%3-l`2P^^HI|GAi6tBK zgF~%Exot?NxJI(v2|l8HHJ*u-QQjQMh^Et2M)`b%Q9c>dD8Cxx#kf*h;_u^{Xvw*% zHkvwz7-h@&Xs}RTPMSfB4uTd{>M$ewiLhl75%%HPgJ&7nLR@#?D#fMAB0|e_B0M&Y z(pu23@Un=;ZNK!0W2Koq%>&*WMoki>wZ#9_ULq_+eKeoOjs0fD_~ksVZ}a#PiEsz< z{2{llMI-5@jBe1_$3bJi!+m76M#9Ns7D=~ID81BdSLfJC|GUKDc^vxz=Cf_JGMS8^ zA6t@$uo-avAy;V8cgL0As@iPJeQbX_d#k*oMC;Cwae0I`V$rxS1v4aChUTB>D=jZj z9_9z={57ESk+5t0Ucj5^zP5#zrpnj{3yFn!kuxz(M3Ep#{CdnR5v5S&C4-MKMZ(vN zvl1ia;@!PtCg9xQC~d0_`;T7wFZmsLuJ4y}y!@)e5ZK%LWNZ6bu8BWlLh)LSWW)V=)f{cF7%6_2T=zh%|RN8b^j7UOy9rI$t1H}@j{C9+rY`*G;Py1+@NcgqLlwM{R$-j6qv51)s z&PLfADMk(Nj2T;*7io#tBUI*LPLkXU9VUEvU8ODf#)#?f!FJi;Ymm1(2v$9j)nMeD z#+@l!eVXzVIi)q@7tHL8&C%88r(;0(zf`xOB||>qOF_Q>TG7)`TkSboOH6w`#lR*8*hK5H`MAbRp6LWn=B1ER zJgX})v_!%eBK$PX$(0UHSv}5sFup}5$uM%eu8|8x3NxEI zujUjnnO2@}rd)Dy8xc(_HJ~-sn;GR-BZr18gybq6cE{I2+G>9k9s}l^Qlk$e+|=O? zLmP$UT|lCZLXsbY1iT+HjRw`0DN?#yi&ZsMj)bp^B>8Gr&GU5nqS$STj`OAmU#I;2 zCP)|TRmmOPRcRf}RcV3yR+;5^C&{^QWlxmDCrUCTp}dGmQrpb(eXA%J^-a(spQn)V z=dgz8SQ*{6mZ|b%2P0uVvT7B@_-n?$aPoHU=^U)yc$C(FPv-U!rA~EbF_NK!*~abT z6yG?rljcsP`08V(()-oGdZJvVa=^7O1Cek)_PI|lw^Q8G5F25O*90j8eehoH132dU z*7__DKk3j0xR8mlwFEH>IEsTgC&J=ze%x$j6?;6auD8jInX zp^dfb0~9mAKckwmz93^gkNw4f_4)B(;_uXxoWJ2J1~p@H=$hh`TgOeUOpyuuPz+0o zA^Xr>aFP_aj+gRYbJ;k>KF;Loq_)FWLfCT zmzR0&lXrP~gPF1t{1MjJi0}4NPMJSuYP}8V4Pzwtc!yPXdTQkc4{H4Z+~zUb=bJfY zV3gwW;ouMV!w%ZbDU-*dkmhy;3?@#IMo8}74y>;=9|a@f;8@A-mT?3^VTuqR(qs3mMw#A4e_NrT;lj zk?=cXDAD*sqph_Hf$xs*v%_}{H>!=a#MMFva}t=ZwSh7#8&@j{+W#FCH)yO}fXDhJ z%8jGq2A%brfOVZWb`^~4# zDbUBncRYc2oRU4t?B+b|?&{6DgMjx}m6B#VuyRzhi;nTRj?cDgw$C3V4}8`zA8nyEZbX$QZ3zV!K4n`A+&pEs;n zb-R62UjjSX{#aiE>fjXZsAhMJl0?Gi#^`?0FuEsL*Yfy5jkL{_D1CI6pUo*{!@Sht zBj)!SxTo|x*{2eAGEc?tJcV!MsXUY^R5hTubWh3|&Xu$W>)K4xm%x<`Lt)UN1kGSD zyG8pf*&dJjm?9#kgHC=F;lYb!x9FZ_pVmJ6(OFWID#c5zITCJy!Iyi8a(QEBfY!p>x56-DOgIIr9FRD1SnZ z&wKQ{=Hn|-_Ti}~TH{}bC`W~vdxH3XoI!+{XTue|lqfOVcS@L#C%%utpQ@vKaCuLv zq>`ArRbpf26qhH0|%p0(hJ=bqjwD%D)7 zlMlJr(+!bu8#ukqYSuvpJZmJZj!eNCs0_8))nt^O5Bd=IGJ(EDip>0qT=K=dI-p8@WPxh~$mK(&w{_6EwQ+Hp}n zmGY~GL%&sdJM%PFmOBz&=`V8Akt2C&{w>l@Nhgt<$x@kQw5$dFojz77J)!;rRNOcw zW!-$ZKw^uEC2jv^27HC8g@-PnAtsK}u!XJ;;QBJsMKPWn4{iW2YM9WueP`ET z6o)#ohZVP^f-gCJnFC^@oZB-adu7H1sFIi0ER%0lO*VJy>8*dLiE&9nAX(a15()o9 zWo(U(Zvu=-4Klz?PWjcadYKnFx|NqQHIqB9YxT;xKIbEhl?@bOG%pTQnwK5j!ykzL zw+^4)>Kr81ryC4yRE|5X5}y~6oW^aG!DDUUtW2O?ru-F^!D9Rb<#p&fpmmw?Pj^|O z{HzskEker|&79jT8Nt~c24_K((g@{23@t`8E%jW)CYGk~8<06&*wBiN(s zS}>b~+v_;+G`!Rf-f-@SW*QH=!YXPcSE_5{Lj~lJN2?qI?gBGL7}y{9P&^^iIVEeD z@;nKezF@~$c6Nq`u`GL#N}Zc!ja1~T#t85qGxMq+B;tPXkrb}X5&C~?bVTyO(-#n} zvO8My6ntTowV_$LQ?-JQCuF2@5sX(xD9Q9S(OlNKfy~xZe?sVC^n+ZX3Pm4Kg=cN$f6wn22Flk zK(i{pk(r&+obM%;`I}k5b@k;DpnRzcSv}XCcZz$9ZW9{iQ5vv^VN5}em9J{Xci)L2Y6QS0a+*Tv{e0&TVDUo}~ z&_?;d3D_Zn{QN)3AQ-2}1aAJH$snxq8{qsz-E=*X&_WGe_sb)dpbxi2?{AM3w?Yz` zee7&9)(H0K$a!$yZ$^|TB_SbWG?yci+iHSL%PKcSjICHp%8Nr6kZHB@$=|xK-?WUkV+{(Aj{Cv`lpmg{Ip&0;?Boq>@PQ0C~REr zHG8DT4%#p_o=B~{#&`8PXd^8jsrqfjl=p;%4IJXoTxerhWln@C66Pcn^kEz{l-Fs9 z^+O{S11tU;yaQxj>@ckI^*AxDs-oHlx?34qTs0ZL2B9pFw$ z?%WQNe{BcJo701y`~V;5l&OaBE;YN1DZ1a2D8H-fz0+DLK4gy<^xC9C?4Ta8Rt zx;(4(c}LnO>2hA{3mK_p=YmElK~jQ^GV6-nC6?!*TvnlWIn}pYh7J7`-6dHi zVO*3m@pYpd@7m>Y$_qU0(AV_`V`X`|%lyDhDL$us+6N1g3HavQ>qrX3YI!bN%MxFF z8LOOuX3FD<&1Z3&QOcCh26?RF<6{@16gUK_H}XHU9U|&lpsU-)n2q4_w~s*&CTh)$ zNvr%~4A!DuS~H9L$5CHQKBMdFhvDKj@bfrpLN`%9Q+ZJr)#(uBFX{voI`}jEZa56O z@}GD&u8d;r!ZAues4kV#JIcLImMY7$p(%hZ{9vNgqY8WYkLi5GIL!I2bEQh`fxjN( zrMp|NI800zg2Xx)NPyls)Da0U94EQw178K_N=uMy-nb1biLtzUmWV9)m9K&YzG~Pr z_sX@N)AFSt-n(A*^aSU~kP8C&kaTFN8$tUYYb6C8;34BfMut_UK(7h@BJ!6YvBdHe zjGqs7($&}isWpK#5z3ijC)#>>Co*CDQ^v?{9=rv$=vDe?O*+UCCr0n~tdaM6UXxFI zejlvF3b|$^t&H+%v^LbTC05H8@FpZw$(p3JB1m;2tGqr+cEy)H?O=8f24c0+5`;>s zjj=L~xD%vzqP!~|&*B!nq;qSe#p)~r*ME%5e7Hx)by2>x;Ah2ctkN?&9Mog&YuxMx zL(5`ywt)wxel>*S4hGTp$-j_nm7& zL+Rd?gwdNd2EIMDMyj}^uX_dZ-7i(v&OE~_KOSPh%a%EG$df(xRZxpG6Z7*q_>41v ztrswJFB9P^TtC6ZgHm&zqanj$?jjKrqJ5BIBWWMtQUz;#w-y?7(KH)WB#AI$x{rg8 z&9*9NKy5?8eJDTktm;JGDZC_HO)dnyHSWdgm6c>B>=s@!Xa^lk(!O?)Tuqc$ysurf zBi)bGd$nD8(c3;iAcy}o!hpB=Ax#yVXIFN3(+2A*jty|vr*)Wg6-tue_Wv;?aD`Hi z$z$3jx##Xc;15CYY2$6&A9Ki*&c`X3Xs#esB{{W z>+VK<)u?H8DeAMYF6r2N(YxxC5aFEtdag?q+9@1NZepK45%w-Ug;GDP9o-MTRblrY z&8#kGpPpm#YCWkPr!~o4yrSn{CrOYx4!X38UfZb(MLxT3SbGUx6x=j;ng}an+B-^N z)dtr5;~-~V9wg?aK`KGlLoWDDkTE?IgjYzA72ATGcsMAcjlV@{-WSoiYj2K(v;C2C zvAIs{7U*2t7W*=4VzW)>+Tcj@8>Ohu9y>7c(>cfft{fPpHf>731UD@8ImDtXtFB5i zJzq5*P9M>UNhYz1?povF@;4%%@#Mu^T8>vr#(8A{F6a0pNPfmvJ@7A6UeiiUI%U>) zbj5y;ml_YYoe}Ih#r$^6W=?sCFr>KTo0+GsmbFUKD6S}7s(9LS7vnTR8h2%%pyrArgNiJYUBy@Z7`z%4Etqx!Sd zZ{eJM8O}Pusl5zm9r(yo-`a=jFdtfJl9VQizIV1V3dcmk&qWGcxl)dz z1DFvG7VtXYn^A+=^`wIj=&x;)wzV;? zP!OX$?;_|U9ZyPgi(AO_cVu3A?4Eb;(cD|rY6Pa*FMKWTx$w2Ax*DT%S<3#vZ?EmN z6<6wA)OoSLdo7xCpmI9Bh}>Tw4_U8 z;UVx#T&NSiEGgxpd{o_R@>-U`3gvx}wqa=7cu%K=S^dsE&!L>@-m&m!tqg?uQg79xI@@Gzk){B7S*)%+qG?{U#}<_5D6yO~hPj?>(50@tD3wX>r~U zY^RbZl~%FePuP~=hiQnH;fYCDh0zG`Fx-sm7j+K^On(XE8@Uu(nK%DqW)}3v_AW{td2AnHivy0q_7zR)1`?Ocb_lt10?)nXfUjY+1)85Usb0@~ z**s`pl<0$2d(r&MZ#Q~2(zg%H@SRYf_0VtL2R3^0ohW1bqB+AuIW48_odE}HR+1Py zAc^DS^vTZFAe$%n(Z-6S!PfwZ?_%$2Ji~wS7;t9%Y~&l~{SE3tcjN}vaOx!8>WssIC$v9`?2!WLnd68?RS*F zjE?AI-yhM8RpqVAR$bS6LVW?X4v!c>t$4*fGQnZiwJZQGkr|I4?26|2bmWgKy8s>Y zB7M$<7v0`kwn<#2Y0|krI?G){l;)8_DPQ8tV)m&JJO=Oebn!F;^0kUQi#gF&&9w>I$0w zIM!#H^dY`IkM})VU6(;jSaBQ}Ye2iu0Y?tu{SIwv*CBPC=_zee7p<{tT&d-sqj4O7)DeN>2EPB4_0bsM{)w zgWYSWU#=DNwXJf$OeyGoPfTAJU15ey_fF7n(7O2(&B?2@%TX44zyiIJ(mY@t>jqo4BntR#SsqTq>qid-g-)Ksr-(Ii*@759f z85g_6*1|me1T0pr`$=ADfc7oj6NSihgL_vDBF8*H^mEiep?Cl(h2kejV^GMr+h(HJ zx)}ZZ`QkVqbmEH_dvYcEex^HFPO#HH&^$jkG$BuKHSNE%Z+>-?ClbD8jDAa%oeEj( zRHmZ8M8v4s7+~po+6z(%PJQXnG4udAi5GeW!8R5yLc9Bj5tdXp#e(RX%_&6B3DX1$ z1WTex)HEetUbnB%Jj9NjO|XF)RgQ(zh8YpQH(1GZ)b?YB=_;NW7w&ENz-Sm!2E5(S zl29gtZ(5`hG4AFp36_k^xxPEIY=yG)OEnI*fyc)SVJQ*d)5U|r{0;nMH!iyG8d@Ck z{B@~vcAltYb5Hw)9DPoz=O*#Ux+c40ogT8Fj$z9Q^zpaKdn*lXZ^1|3(9(XEO_?tX z3kBKau*271hqY|<>qWtzCZ@uh7LppCv&2h5!tlf?FOj{?btNijt6iQ14;)VZSa*GA z^}3rZpRLnhnQi63DX_!L1#=l7FzHRY5iWJPe55j8u7-_#gG2z#N(Pp0$J8}Bf>jBc)<7@X^9ANUb2`RKZ0jAFU`;*yWSHQvN9kXrUu zHb5(M1lkp5u*aL9BGhIG&ErMygH>{6rBPDan8M?bmWHsW(=n%T*;-ud z^yZXu9;3;OaF(a!DqqvOn`N%Q6Wn)C^DfjP+S!ZVHRbZNl~F$bdz=rkU{vd39`8|n zJ>*=Mm4o?VIXC96klH49%f=Eq26cyA>#}B`H4V2=JEC!YA-^Eiz2%91Ln#ZL)x}Px z{KS%2e!Mf&npii3nlfvQCB#qbfjl5FHSjF)xofy8uGLvZM0Ztny{Z1_K)c4&+AEXw zo`IuW^-n*94@vpP9VONEcSoO#+o)GtyBsfheVlwqz0*LRGE2Xe`Os8uUkSB4iR+!9 zN8(zlhghxfJa_Lq4v0(k8p;d`HbHR@%&xD0N~!NP=J| z>OchU-uB*>ZK|<7?UstZg;Uy=TKOrR`?+LytSu{6)VFQ*{Q?v+*52WNYmd}3qPD!j zPE7DFHzrWO^5Wh$5=Yx89HnhI>z{1w(SERwN*hcU{Hy$t@Gr4%&#!0eKh#$AIZ_t- z1$Uf#i!Rj#X={+Alt%f%43D#ZHOA5T)2O`ckJbs;8D#^bOxT~kW7B%vcyav)K5d?* z8@7RACX{Cnz)J9}1bs$$uH$X^NKNlc)O^u=aV_Ofv&?R%#pfwBx|Fs&>1>d@xP1+* zvV}V9SAm$NfS5wy4cdLJ6E-wNbI^U3mZsi6?)o@j0ZBEwsQ1h}>l2(u*T(Ed@N+^O zkO0lkBr54+9JuvW1M@j5@y$QI8@>fV9237I!`MC0(x5cUo$ewTkr_rOTQ0CG!;wB$i7DhbkkXtzn)dOJ8-`MS2e4$cd#mReJbV|SjEzL_7(`8ZYL&*FT=RjJ>0;&$2RPGcc;VZ0Oze|5VpmK= zQ?7Lzs*-EA{7a5`(5p=3DQ@9FIVMR?pR>LPoE@q6LgM~BBGzQW21Na7hJtxf55&KX z4Fc0Jb3fo0w_5oL^98g^-7$*`_`$jsXFV_dkM{a5uKvcd<;?OWQ~9DUl>Py&K3jh) zvv`RJO5s$tITu$JmUHz>8MfbQ2!#T=@CfBGfs<`NjAc`A?gA!(F0FYNDZS6sjdWS+<4se_E;au?OTB?PJgx`LfSdy_~zV@pvi8VT` zu{&E6>TQ@$x-!TLx-#yGt}eB0pfR)6jdfz@+&`VYbe|Xlg;7mBU1fHxGIM>pyPP?`q(5tWb94XRr1+Cp zlr!RSwMf{Cm3c%hD*j7ws?;AW+>$TN!j6{iqTc%dT%%&^qw6)%75eDf*;@u0C%ay0 z-tU;R@#XI7`eoV|J72#%FUHIL&v3I+T}!*Xb;8&2J2Z50eX1gGQv+!=?yBos=@YL# za+XhygGGevt*n_}Wt0|G#U$}MNJ0iaS>j-QxjSN$24F!wFh=FXBtqk?2-y-TDkmn& zQ^7;5c=kfd?;pWkBz)y~uWZCVY?gY!t&NKXx6G^{)4}<~gYSwvy;giEZbVaRll}tE zi{cy7=b1T?@YL}g>cci|=opD&p|X%OU7K}HBi_MYi*Gl~(#;g{wh?!y0+XS?jm#$I z$)z+V6x!(&L~G?HSP%!73u=QXQIcyJ9A5!ylWVt#Pl|(ekV$w>+{B6^yg;rQwS$`L zN+aQs5%6^I7}^iL0mW}qGRg7cqWJDgp?pco&AjxVR-3+zt~KDV6KgSbaph?En-Q8f zs%MR^Fgl|G@BtE8w0^pN9QB-ZT+4Ju!X19LUR!!Ale`34pX6lty)CP!ak$HV8l{nd z;XhHUJjjR1qZ5|)NVqGa1~DJ=o$IOZrNWu4c4d>7T-^T9YDHqGyuWIU-ti$1q`P~@ zp2j%ReSb63r!mU0`-g29E6io4CUcT=(7ssQRQ>Z*#gnAz;6sx^JtE<J{xm^A*i+h4e`?x|1#l!V|}&AsW;od??c+espLF)u-#Hq1?(39J4p=#(Pi zSH^w`tLKJ6_W=f_N)K?-iUB@VT&o0Utgq8;xcKM+lD}ba-GNx$DX9B~K&Ji*eb6cKFwNpL9rZwWG4Z$ANk#OLn zugvQ!P^`g72Vae~sJuFXmt#XDcf;Vh zSItjGnsw#z_H3VHkbTgy>GQ9k1$}ThII&NRD3M4Q7uHI}V+TJ~4>tFxY4#q~XhMW} zA#i?;Xiu|?%Gqk@F+}J)*3k9vp_&Fiy#3zkTv8YE%01X2r`fO3+m)BScrK2Zca&%9 z5mm4J)_V_^()DX_rN$+Qq|cpff2o<-PV((a!A878&TZwp5l>-PQZ|x;l#^S_8S^i0 zrc^XnnxLYi;VZ_@$R-~PKg@Vvp0usRtcmYxD0@FjSu<;Bgc&h!C>;&6W1nl6c0J;r zdY=Xszt)g8>DEs)gGx$s)Oz{rjm=T(URBhjwHA6cR_xL~5 zq6Y}2Ee&&qkZOnNf(+*_CYskPskqL8e*P` zgf9*Mp+5n%xskX=O2t6|;Su zU__K)HoWiWl~2LGQl_%D+Vu{sI)(FCQv+RUk$KtSXV3=anIA(+@3G%88u&;3dAJr5 z0_A&+3Y)}ruws5Wt-+`WNeLab(O#S2|3fL}{~8lSOZ4kc1zm{I{W)L=E|Ns-8v9gQ zrwLJ(o4Dt&j%h2{!-hXVv-yoByipQ`U~ zgw{CjQ}x($`{2RG?C#e6q^lNjpJkcHRYNhj4wDVqoq_vNLW1o*c zN5WT+7@}{AwTWHaZg(rYo4%(o72_0yBG}@ljd-IunBCO3SO{ea%LS(C@xaorV&i@y z3SGj;e+OxOzZp96Rq>Yb#(DBJ8AjKuj(z^Cr0n6x)%nqH`!XZp$A{)_X>|Qb6}+#1 zWh%dQJ*z0J0rckJt1)>6_7lh}`@TxQSukrtlY#Az4m~q88L?eLPm7ufJ$LPTN;xS$ zJ@ZxVde))__wpMqo_Hmp98_xaNm$tHF-siuYdcCbkUxod8amhFwuc}e4XX_NbD%3$DL*p+o{W~Q0}o84 z6YmTn#w2LU!BYtueA#6yOXAItHRL>LIckl`LI0lfA=4Mlqv6+8U?%FnQx&AMQ7aR* ztHj!{-h4);+UfTrKP;ONvjr<;zWR_!FXcTr8s3IGvt*G*!}pF%bw^B;bGy`xNc%Wk z2{V`TA>D`ceWdRvz=q-hhX=kd4ObDaEL_H1*bETu)vea9 zUkPlhT}yjD0W%%jUzMw{80d>dg{%SYV+QnC7y9C*%t+UkQu~!-;lc4n=)ptL|F6fN zxCQ$6$PhBlY65+{L=)jL#ACL=jKtIy43>=zU)=Y>rMxVn02Ln`pM>YHK(dw zwIyfSysF9URrO|Qa8sDV?dlElbyiwp6Yln?iHptzh^4xW;y^p%h&jY>Q$Oe;|6L2c z;$1iIg%1?PwbLHXG!lMsT&zLVV-IsfI3U(IvDU?^AGvQ|XsJ04N*j~4$@G!%Gvj+} z)2L@a;a*s!Us6SRw#liqb0TWPcrB*iJppUx@zeqyL8Je_Hh42%4Fs!p|~7HKRD=WJ$z)Oif#IRrxWs zXvs0`mNRk2N!AjhY$_zVhp_vA{+XPrN$*q*cQ{pJR+B>sOvhOa5ga>{MROHIbGb+k zaxFxz0_4(fMXo9Cw9;Kw6!(^SmeypHo0Sdo&4cS1T=UIJghAw>d0Exu@?BLMExSD9 zusuV6HuMnnRI!hQ9nrHw5?&UF9#{OUab(FW;8n5au1DGm9+u)5)mQXVd5m75)xEgl zM-PGm>&YgZ8xmJWwv*ir4(7x^PYK~ZCO992G+nrS(#IgyGCk`9+E9rrrBMgBsw`rz z;ce<)i?)PRk^|i-VhG@yhuH9cea>wC_vgCH&uY|H_zGAum#LaiHdiITlrBscu~RWK z=*d5OLk3+(CgU;`A3MuTp=0};e2ZsqNMM|*pnpCjkefZdI5EVb3U@44h3h|;3!RIB z4|hf58&o@Be1i!kanE0T{y|!6UCV7oBjOdF$I6*1eUZUkeEz4Sm}w54y(X4i-1ETm zAp_Y}WteR!?5=te{?#;gNT=q4O3VV)`BOBaf%=yc<%3q(SP-|Ig;<4!K6tDuCx@6F z6F#_9qdOYD7CyOpL>-NWk7G>$->C(TaP)sdy?XH&3WPu^c6`qJfOpGn^6(42VMa||&PobMC(J7h?^5o>txqBlu* zoQs1d-b%XR>uXys3}=!x@n%kO*Aqxl7w(X&Wq6oz4(i#+Mky>=tUm_}$v!fT-y z(IOrj)V$&Q@DfW0AngH#vYRQqq+l*&k2xK>&=k#dB}sF@adkKKVwxFg!MMymPIry_ zBU)H=HrI^$Eb?CYB}d+g&*dghg70(rq(_vOWG3b6KNek8>7GS5d!~CP`eBonc_QIC zk!o3po_<8fg4Ub^Ut+awIy4G4sQ}LlBX41}KMH0^f5(X^loS0uwMP5k`Dz{se}Z@l z8V@J5jD)3#;7hY%<&=&F^;kF1Yao$ODSOFfK-^~AnY*xa(y6OWkl7<@+02y8n} zK+JhkTd99#T?zHatSjk&FXrsl1W#dWIc%xa`?9Vi)e{ab!&Thk1V(FQq5R`1UyiV| zjPMrKmA4zwCO1C>Yoc;!+~kSHnUKHkns<{UiP&-Jz&kKcfj%i$4de6&!=93na3Oqc zAxTtl$5WbFFV&D!uU&e+%)h^fPhGeqas_$-`f3XYbA4t+jYE6aOnozBWoXRzz~xvG z#PCG^87q>V_j~v(P&)!ACpdTcIY1XH^RHjF{@WKiT69#i^M30m^827b4D?16wxmOd zr`*377FBjQKm-k zp2eL}#mQKe)1hyte$X~VS+%Gx+isn%ivf2;6hXbF&!o-uYo(cpjL}Nx5vAk=ox<4$ z)*E{cM)O_=-`uyEtu21g!=O=|zGZuydT3S2}2mud81KdDk@=Nj`Gbm!H(u%y9Kr9ddMb^w%uQ>Z?jq z8EZxzY{offf~ysQ|9jBZvIViYljvTu4^pV|UNrJyt(qq>ySRPW4~8_*ixcHZ(B3%E zQo3sTK}{)NA%HgA@wNbuI>rL89CHyMVd+ji2k~B^V)7-t%1R1}ay{b1Hj`s&VUa&dQkY-c@f6UssIps&TaIkN)6Ff&CLQOanm!G zrYt5x60ZGs62UQzp2-6n%T2^OgcB<`37#{)c$a`^AjY;0F+jv(k}~SzdJQVG5a-vp z3A`5U8B8%~qJ($zh;<|Y-HwzYF@?$NNZxS3kU_n@SQjt3r)_fx9jPjljk9|)NnSXR zk}(pV4G$k`^GXD#WotQcmfZFW(7t!88X&=V7tWtKugX0}k6 zoqq;T;1w604Xy|8(q2rlt_by^{Ab+xui)oGW}H$z+nS(VrrHdm^f$a2 znx2H%_d>SGDo3N5ulW$(fzv;0!f{W#vglWKd#D5SQHZycNG|E7f`#30hO9|9cBCRT zC0*a~W=KrBspF-Pjq7QSvsQHXYHsnfBSNuD@$&45?pj^%o1vLWR^+oK^#@!E!?U|F zhWptj3d27FFNGFBC-7i*e8?2<8YH*fHT>jo8fIGnN2?Xy4BefCDE$!C*D|wQgI?DM z&BW5s>rzU~rWjQhfsgm#_Uzp%xcBcD) zDe1Pnf{u(3pLA8v_5}qeDb~=wvpvl2T?=wg#sf-b_uhU%5a~%QO|Sd&`})>sM5d5J zO@P~PAf3`7z#}u&b=YypAITtBrYUhrS%G;H^x8X1r@hn6JoOjI7&yhHxQF`N3*Eub zwWWTh@T(vl3%+Sv36B#h^T9J;!iZ!`-yIPVmg!cB?~MyZ=Gvm8XO(Fln!7@xXP(@8 z5^z2IJ|YNS>W^c2oHt7I@;i4quJsH1D-@e>!q}v0ylb+yEa0#jvrc{i-A`*MauXN0 zDq17=yT2|-2>e!^ZkuB@b!%~#11*yQ*ixa)5}0eOGSvv}D48Fmsb>3*5?WfbOBZ-8 zi`IZMB2X{%CA*l}-0$8-KW7Q2gX>=Z({RLLpd@^m2+(l9asy^@zrNKn47-B%3Ybn51Oi3P>`CWp9NmKXj;7@ zKvx@9THJ@y$0~1F4h;?Hz*3Tzrx@b2h~O^6Q&sY>vw>>5j85fW3rMZkg`FZ?emE&2yT#5TEcdPJCdxi23s%=vmdL9Tr%$ zz0K{-7lS#<6@0vyJ-j(OQkv*Uv2>)^f)3&3+910-bKQ@u@c|YZ0Jg~~XUMwk4CV=X zz0s~O%P`{U7!RfcV_s$uKM9_O|3leeds)RGzosiXqtne4OJe)lj*`P)3h;Y#NzR}s z{Z(3m{~rZ22E7$b02+o~wy>x9uHgI&S~jDPO}c4t*?|GnLDxQ|ckqV8?uMPWXM%pv zxiEtw#@0HmCn>;O^N#e~g4|E`_48!Uf;SKct97{!GrOm`)_u80x)H6ByR;!zYn9Xd zOU&z{Yx+ytCYRC2CG~b>K-I-~f8JSp=xgawS_^xM?(m-le}CSNcke}O{&5$=3B;u7 zjOq91udjXTyx~#P*SbgXHCjT`MTWW2gI|{<{Gng3Vde>QUjqf~56t#W@B4BMQ<&lU zHT2w9NH%y?zQ;A4Cy&~`)+PM@d_HRUOSFEzr}=CCQD%-8?{y)K@Ts~7=dSEIxagcA zfmf1Lc*t>EB(kk1lb#su#91HPPu(glm&lGXSeCzxkdRHk6cL*QC%MF15Cg3;AL1OT z$R=qk&Us6{4EuNC=6fdKzS}hc^|`>_U?lqcW&9smm!eV{Gzzc` zDktH$0QnV>vMxi*la5sQHe{`1W|IOnV9cPW=crCE@aGE2(F)xkm2!&WAY;-fX0@R? zD#>X($HScw`j&n*_AaiO*Gjc?bpX4 z(0%gPm!-PLkp_bQIIFFknPHmA1hTKr>TYvjNAur+Xof%aYvP8()nfxS%{4;5hPS$! z90OJS@zzjwQrgEAlCGDSX`LUSg~5{_VZC1m*^xp~1?lJA9^Ul=_JgBAiqAtGW~p3K zP>&8cIu~`iT?18LN2Mtndb(<&KZxFN)(k*%b^vM%;D@2XnKZvpfhG^n{K6prdBCFD5y&n7>H)M{h*uEsue2rTQ=v zO^+U?ItZdX2Hy>6S``K{<~TPGej3(wsmd5Q)gzG6iLw!=ba6fLZq|}f$M;&-A+A4q z%5ZVpUfnRhhw%MU545S4uY;2zXJi>k4q^irdOSQdg4Cx1xYYyyv}mNPp{*LcQ7AAR zG$Ef6`32}Sf{{HT6Su^%WWLb02K{DUCDT`$BH@TPE!>s*K1gF^GYK6v($2oMd7<^v&<_!aU3|S!(VT2-h0UL0R zW5UAQ5F=~x(SPPT7yU2f5p|7I5Y`p8?v=$>)&yOVI$PXJz0v)&&uM^%b$5GCBI_h|Eda=TW}r{ zgSvmJ&hX)^C)m=6B5xuXv-uHz8xKD60yMb1@*_wJ=6TuHGu_Pdkip?ka()HkfUAiR z!gVd~C$7|$v3noV)prx&x42H>`tdzPh#;Mb^f0c!e5c$*`Tvu&7G+t$sKLDA)Kdt&Q?DK9^khX5*)4Ag3lICZ`eJd^c=k zk#Sm@9?{Rw8+Y!u*ZLecSH+c0N z>I;hecCHC?V&`_>SRSh{b>06{-M7azai#sA$xK4Hs1a`nh#?UL0}=rhYcC(%MZ2_an|i4m z6rBkH^7}p$u)e$Q`+5KUz_pc8#>h+L9nGn3VUn zb%=-ROUoB{sji_pA03oTJ?JajSn}~}_f^5)&h6kZ(zuMjdRt|tqda{IPB8ELa}Vf1 z(UXPf7nzfVh^Y1eO&*!+V?TjhPkVKPvlm2Wb@S>d~P*lXt?Zudbo_~pwl6JvC8Yw z>;q;v_%ApSycEd`V_O^Uhvstir8v`dk1&DS=79C{7PR`lFXWhdg*Sy#>-ax^Pxw{# z9?hl?zzFYILbPv82`=Ap0fD-!+CH5PL zy*kX|+vC)thrZ(EeZRZ)W_>r+wuRsHJ*EDF68{Cn$hsBdRV9X#U!}x&4l$l{YxWOD zZ6pI{K}b|@QlsD2b*G5be`_f=&b;kP>@vhIbL$OvO&yk`+1DeTK&mLyZ*UIL8{eVC zXUk`PPxI&`#GldnQA0x7v=+)wKJN3~#T{-3@B6yD8mCOBaCN48OwrE#$(}Jq+pH5k zf0wt_Q;hsek&(c9^PTJt#s9c_PX2uz>)t`H9<=5A(6}AZ?Q=aF-_JumU9(S>80zWc zx)@~q#qn{baL;6DmGiz`gr?zpwR^4gg1ZIOZFJ^OV`N}Ac+*=peix!n zS#vz*%x&mfeVFMCnCXP^I$%0W)&G9qW>CdSyu+EhL* zPPEjm$u_;6II2AocO?7RBIq7vp?@Aix@P!YDw@-U>3W>;d`7fOrcs%*>G`0>0a!nn zX~5b3MU_jk}kys{?KIZ1et-WFr;k-qYC2GCTT9x=3?T=G@yY{MY!NomkG=zFRn zqIW2b)Fbpfp$Le;N-1jVd7sT6H&ui3ECNqC`(Vses-wsJ=7T&|edvVJj@Gxw&nFCO zjh<01no!~w{HmScdW*)6!#I;S^QtY-(C%IcUNcY-eAS?%w4m`FDE0GA#U0;dsb_$5 z0lx)2QK|zfKf_r4BmBPdPrI%B6c%0-)`+vxqyhLpbM=k8y@XF&pJQMDMt{dl;?cSw5PFo`g&p-hn*8O<|c<++x)=4srw8 zV%Zk(nTv07@JD+(B_rBT5Jv*(wnu9044rz!v;j}F=bATCS@ow^a;a*!Dpl)V;KCV( zaM3%}?t~tPs_PqX5zu>L(5lZU99U?S3TM4U`9$=DD4%GBI?~}JMB6ycRb*d}dQ$_* zaD#1-OIse0e4xt+r>MB2b(w0ast|ZRM_BWpQC}ax0Y_3aLRXr5skH)_<=a7`e#ZLSs58eRhb!5PU-IACpo{?^{QILIW2yG!fR8eUsS=RME` zaF*48W`2>VPX(83J>*M(^hqcxg!CDD$Kbh8Ip{se3OJ05<0>Md>j^#4#W;%*cOoZ3 zdo1ge4h4*hHRuaFD=3tBrt6W$h04+3?uho(@ItXu+7J*DCbzOt*dIo6J9h!+WCx_m*w{>WR1@A5NOtDmPe9Mg zHvOrE5w4k#!wbnr(-SUnrntF$Q^65=obCuPCBqw{8(zZs4uuU_IKssYGr^zUS5S!4 zhBWAmg!Y3JlituZgf~=K6Wx#v(}7byFS_A~KSTEt)vDnw#5pTR>7zT^R;k?)$zh&o zpJhNyb3Z+og*|KJ9&f*7zb6`;m6+xTJ>&&aU-KLRMfQmMZu?-LMR=I-cx#;`0R$a1 zIZc5UFg$p4=2OeNoarTow3Q@IIC*5;*pYiNpSk7w&;Reh)QHF!BV8wRx; zs?qy8ir)=gt?HQdF4{_%skF^yFU7k({k!mdrjSGJC)|sc^W~5(wYohQUMP4knnj3 z8gO;M9eNGYHZeSDMeel~K{`QS-Xl5vp!H3vb9#(Fv;4002M-~5&9;wb_S`BPwo8n1>?p-F}fiRp$W~)F(YvNh}=%-R9~pv z;>!2Ms6)CizYF#QrkG$lfs-vr_J=JmQMWy5SNjC@SFp5m#Y6S)297%IfR{s#=%#yw z?a3180uT;#LA5=!Uw*W_5Kaq}cx1b~FM1QtE!ipjagGTBG8!%FyhNU6sY|?J1 zs6=gyEFrla^k0T_!@8xfgQ5FW;0m<#UdQ12RFFYpSZI&e3e*Fh^-r?ljK z!V17SF2WOfN_)d&S!r+hFA)yBmfn$x)u{Y>(;siVu&UqLKi*is>iD&z&NpwYU#>Q> zYkzVQ_xI_Yjg7Cp{l^>DRTJHoYvGx%SbqjT41R=y`0!%ewgO%I@B!!CM3uklT-%18 zUiI%dB!-O_UI_J_h;tWSFl>y&`mrq;gtOF&Hp9j?#5m;7Ftdi}SRaj;JEZek zU1@K7^5;tHUc-uf*85tW4bL!J;LUIT!+SsV1I^jJb@TBaeV?0h%sans_pP_1V>{^m zJLox1fObTf=fVrd=TT=sXCH{M3L4@bE=_1Dt<#I>vr=v_q~rKIul1U4De;M(>v1%J zvyrAEBy#5W?geG)Kl_I5dGy3!=3d)sFV!9KrU_{@h7qR5E7nN&AAzXxt%s#u06+Qy z@TZI=m+hq`6_}^a;$+=qGnQ8Xx#qHMzBj~sx^teZ&__==)-}bTFT`}0RvKMIX_PrS zrFq?ZD^0h0PN(!hcPQr}832|TYvz2fpf^&_;(XhI{zW~U^KDhGQgQVnWEc{_k)&S7 z`D&Y~PpOr;J_Zs8e(6_1Bd9$MaCVIjGe)KndkEA-B8{>{2dz0K`&=NzgR(54n(`9P z`yKQQDSj(fFyqs4O$x1s>kw5!Z#|=+$-WW%!kb$@2&GlIBl2E?Bopz_s!`X%`N&G% z#JgkRxbf}Y2s{si1A=OjAf}7+>)vVTn;&$ZpZbX4F)n7k)Y(-~YgiL3L235;N^UUI zvE(1zd(rB97REIg7JoW#R>h`*y~=t;NV7Yvdu!Oa-`8*x{@W`PQhCltFR(H7=-C2j z%EMwGJ*lLInqbN4?$H}3HoRligC51ID$d1I&hUcFvwrP7MR>ne&@dM0ypVp znb*ZN%&%x}@H}8@;3iyHL!%}{qkie7{t1m&H)p_8a0V#-)9zs?1$0Ea>GVqD&JlNm z%KlQRJq_^GNfNFTsBw~I;*c&RcyEF4n=sX z>kvz)!jpr6_6{yE7S)$#bnftmb*SXT%OeAhAZ33CFNWQ0TJ;_cuBk}e2nd@c4O z)XIe7vjw;s%oNp(wS$G8(LX)^72}h)D(Iz%*~r@orAncPM4Rg)XENsBGo6qKvL!cV zLf$SF5_i6d>d!{@0RJXjoaMlQ{? z$D!>JVkS!sJoLP(%%mGDLz6#}UBh@M9Nams`(0W{r2~Valcd{ejL^<8rhrB;yKLU|tE932RQ8q$5>ATv6D;mFZfo(vmLX^0MkkIZh`(cNQ-t#Gt!Gh9X zAlY=Yp`zwM4auAvf!}RJ-=diy^nF?>Yk=CpQfIlT&;2Qw0jvxOcqLnP<6#jaS&_vj z>8P!QhN4Sx*-^7w6nUxL=FwOq&0%z2|5sTzFa=mJyI4@F34Sc=s-h;oZ@msomtFso z&Mxzvb1k$V(gJtKv9>8RyP@p(T3-%M%G(^wSX#3e`!`>e1$|*>Aji&9zH^5VVWtJC z6+LAhbBL+B6>}R>y6CQf3}q;^O$ntmAf1vE;kmZ_3(wO-a; zxP)_q70H%;P*XB=+Hy#zqTV)y_N%nlr2Q)Dj5DJyzUf5i^}p=gaGlFQAJ)^pN;~mq z`DbBsV_tbY#znHYy8!$`nj2q4-xM}_UwV>h53XX`Aywc{W1I|ud}*v$E;1wR9RW#i zEUx3A`@5}xPOgZqzMd26tNq0dz~*7#1&8LMptP%pjk}4RPoamw#@9)ndhkGz126DY zP>o$J?)9ofZs%6iR;Xkj<9QmMZ_|$#>3Ih5dZ_Ol!Sh5s-__f<{w^0fciE_SeE_6g ziUXY!j5)RFXd>jQsEJ(W`&L4fy5gZ7I#7EshKjoCm6M;DJ#S;B9-`h4+K|dhy?uZ< zu+j~knt?uB)C09G^6r_NYX4gMtY>zmW(rYKe^w|d{H<{cV@v@z% zcIKPg#el3~gdCs{x5*0YZE=2&sy135>#VneV&zlBea&XkwDbpOC8TO8uPizIEHhSA zY1*{yC%I&#vAl~Vdm)|Yt4gtNs)Pm10OTT5EWVA-ZyKoIO$}c4I&uHJ6LKH&bNW~Z zr~iwWOZk-tcV^D?w5LV>#JtU_f^^L`>oN~ZbznyX4U=w!>*y*u$n<~oR;4UZ-e4x% z|9Im;yzv0~M!e&c_b8;u?)S{9)ZfLAodFpFm2a>r?Vdd;1-)LrZ<7-eOO+v z-{IUQ*XegGUF%IuncxWZ3nGU;|ep^8! z-iYr_HO3=g-%X*lveN?jFE^+E5#dq2;ad3g=1#XtPdi;bdmR72U<<%Ej;wN{A568M zsQuhYHB6i!Ux6~y-@*0_YkytgudE4dmDeD}*+4w2hF^@)b~NHk`RL5FCgMBL-XATp zcA?$0PG=vsE+!8vvOXA#H9WJqVJC3v_hDbl42c_m3;xIkYmCRRF&a-b9$L9M$Tf14 zVUf%~R9U-)&Oce>T4rKw7WhLu+P%R~ynnL(?`l}1X$S%Uqj#Aj4ofqtbK;)uZj}(sm z^>(Wkd!$6{k-{71#(tpSB~^G6H-3juw5d6*VQSp9|B%KW?2kfe(Eex-Xs~nMJ#}Sx zM*E|*(EjKS-!8#EIrJXwk4o?!eVd!o>@BH#rRj94iI^L&J*8u0T-Svzs-$iw%7k{d?si-xoGmI{l zeGFIn2~D@R$z(;7d~DBEg@1E%RWYH0#O0KHp8YdC@tEn_!OS2GCvS{HtapIUJXQAM@a+G zEpKxkmNjOZDZANbWLt<1S#t1ivQUrnI)>b=!@9d>*&iwX8pLObhh)R?jSa8W4d|@v zif-7NuX3}U2d%L!CD`SLxUGW6pzKOEW495S6Cz}i{~`-dA2+=$3uaoSL!8$)>t0(p zv0?M0)(2jreTj{%N&MFOS`DFcF@A3iH~wQU)_T?@>50yPjUbE2-9;-sXE-2hY@f?I z#|Bv-yF~hE@O@eG%kD3{IWEiMpY{oy8~UNmL|OL}d+YcPv?EazH=$#B7`O?br!t8m zCd#o9b#H{O0nv@nJ#Ka=SlXC(i(jlCWgs~u^*)v;OBJw-sL~2J^W}l5!G#xhOq=${|zKaA5mA;3y8E^3qH5$- zj-x*eU;{o1{qOpLXCE39rmcZrBd;}Ujk-iTMrxh8xMS#YjNaX)&x7W|RqdwIQtQpO z(i)lHbfHGLQMP>V{DDO$FUDjEH!QQO<;vA3M z0CsB1JMFLIq+lwv7rLYsU1X_`-XqFgNH$^iWxn`ie$;3-WCrbR)&SqnAPyI=&f_vY z4pqiB>rjV$jwKggvhXdKW&~bpE8C zafU`byMeq`;agYXD&c+w#|j+<8{u=wy2fa?DtV;a{$7|-Ju1>M-7%}YaOr&CD%E-i zZ}`k%F+?nlFsxr1)z%k&s=RjTdWWhvj2WgaCDhEm(9PwaI6`mJn-t0!{*HC}io!9c z9T0Jw);iI;qeqQF&(LZyF581nBbTT$6#K&a?{-{R%Az}sZ!T?f6#H~hZ!S~ofqvIE z-(gr?3Cr)uN`r*V17^4FGsyF774 z=?X89T=qOU=BfEljagZ;atYqlA0LocBPTNN$|dDBve8-Wi_kvqTNPF#Cz)&H(asv# z;INFTk;k;v$fF!JaeK=4cCoxA8chBC+m#ylv z+30Ea1%QXlBHA88NSBlyfbHN3jaKjJ$8B#qn4Wf~!(a=8_?}(TFOJa;c}*pEPuMA& zy-OklS>m3^QtJEfGRNprd&|S-Jl(y_m(V%1T?c7udeSBJ3w)>SvB-{FtvE~^0m((? z`=q-HBljUlo88+=63F+9+ksthxH$qRx1WNx*&v3Wm{3U)peGo{Otj zZe10M=WTxJWT+G`o`%F1m!-?lrk?jeA|!%x!$!@{zjiLNUByv8W*{H$wNg68mp&6_ zqrIU~@1%RW4t;^HLSLY(^R~Uvqk^OZZ})-kw(I@cJ;TV7&MY z-D=b&Y``WDZe^+cS`SnnLh9jXztvUPfoF)kQmVq>z4lC4AMs1m(CS*I##xS0T#Q}0 zE-c2b&4i{|e1`a*mH%+4c+;apo8zm}cG#`*OXCAHS7rGB0qP(qo#@qm>yli7pS0hg zg9RNaNCWs1AUjbCEhX^U)(CK`RUMwY8(L-Q(M`}}bX@bMkL`3rr^wWSQ)s?0 z2=0ELZZe(6XG2DS8EFSNR2*beRo2$CvH9*7kF;7u*wW#evjU_B10F0%NSmn zp3>=#!8y7Nu9#YktxO-Qb&r`g*gR$`^OXfFaQ>C!HrbMjlI`(!zB1i(kB5z(Bu;NK zh)Y1#jxB-R5#ghYs;6Sm=Y;lQTrTNp{RK6fPDq~vJQ1$+vBRKI`azJde9v`Z9pP2* zroryL_R&}gBijrJSGl+5t51J>hK-^$4dQ8UseOzXF9Nwo`~r>{KSK#3L-r8(PrZr^ z1lAj>ci5#wzJKk?0xx2LJ<7EkyET$SEeJ3p@|KCbWfC7R3)Z74t^i9pL&SSs6U)cJ zCAjKA(eyj(IL4(x79lOdokX$-K*|JKY#uYot)(`bole;wVpm|XGD_t4bDK#Di_Uzx z9l)vB7L-ma)+wp4U%l1Wr*!_**H5e)gi;T1^X1RFJ_7>54*N3KK-kXf=WYdWdNBBs zWZT$LK<%)^r^Z$2p7x$=JX6Z!?O9ElZM&O@&!hCpwJDdh{49}D*q$NqmgIX-bCCH}?PI?UN*hpXQIL*> zJlhua&hx+s4ocl!LW1h7YF|+54?3Li&HEo3)~J39XT@>U0xh>{`gwm}O%uhdv+&S! z)g9vPHNfW$`wI5m%8OvVXo|v3j!|kh#80MK&3{{)=3u`0sZGDWLz~`z>q|IWJ0#d& z57T~CZJ+Ck!xvir*Dr9JA9nHkf%@=2zHwgp2KBd2Wv0Azt4#5?eaUqrXO*_`gk9}< z7|7;b(lY_#d($Zq_2`el%Z?Jk`AK=uojCS^GbegmEz-l6M?hY2u5wC|hc&~1xpleC z+O{8Y?{y~{V#eFzyQH}oCq~Bsdo1LSTa|YS!Aqk&+hLDriYt$7g8fei_&(il16BC| zBy4u9<1%(sL2eaT!OXYW@Aig4o;Te14=-7U6pU@?51dB@%?$xQZ*1|ZO@DQ3Oka7z zjHkTS@r=%8Py(_7?`S+}|MDVx@N#dy`+H2~aTR-Ai8%3uECc?TuLKfN0g0g>IF9WG zx10IqV0L7q0@6+F`Qk-|8qC#*8|v4AVqjwnJ#KLH^2jFX%)WH>o;}cnGnMbGFkzNm zBk09Qnq8~!7DJj;T&nUwYRRY{nKkI=iZ_asy`vY+saM4>fK{K() zs91wm8zA=OZ8*woJa}&5;Dd83fR_&q5X@o;klfli!p6N!>5S{K+xiuLd{L!_P7$a% zlB|dV(#i8Zyg1XvZDOO;wte<(7{UAG?=apTf}L+#OD=k2u-=`pKMWrA$!4;Zt4UY~ z`yAWEM_aMB`E~)}RW&Z=Jhz`D>yMLdO?-8m-GVye)O1F4kDD1`$<;i$Z7O2y=+6|KyoiAc@Ub7FadQ8>?@^q_M(mV;>{BQ z;`ykts)XL%j&~#QF8Cby-k#=#_AQ9H1u@@IQlYc>PmszQP$hxZL;fhT-@2|<6Zut8 zdKocj=3>6Q!`u|II={Edcb=Z$yzf2CHF&qS-tB_r86&re+o#zYqAgT;nC*e2m)N!2 z8rF(=(^?9wh1oWosbJ+JIwByIVT@`a?tKCB{q zOUSzW)aY5As8xi1g3`SgAXvfkk1zl;5c`>Rr6Su+m^3wDP*wxUyZb zYw`fnAsh!moZH0Pc>|XeyAl%pQv;CsAp0doSo&f~HPHygnJ2D&%#hh*u*Q3_Qo4<8HJd;8Be ziJ{AuJ3z6$IK^$XkAxPskhqc(&|gNlav>X14a6g0wL=8r0fT#(K zV)~5XAf#eWlj*wv~CZk8xRk>C>K@-ah$j*xx!#xKDHj>%=P7 zUrcNwTP<>NKpK8Dx+x$X3BL$K zt1V|D?5ARj@*->|Ta=v;6BgO0+9#z#it~!lvZr+URXvfd18iL)Oji{q0)Sc%@hT z$-e=zMlAAeXp@Ft=2aDt;KcsTRzcr4^SB;F-#35Y%~d2PV{@&xvtCN64g5>;?E&Jp z-Pd7%cf3gHO27#lkC0q18}+EycPPpp30_UV;pO&E!-jtu^YB*}Rl}0=nt;gkqx}op z2>bS!_=o=xo0RwR%xS>>$+PdABcS~8xp_9T=sSwubA<4?fv|CUd(7*xyOdZvficYS z6}GEh-%*nFmZa02mI|_EhgoacrrtR$(xxh!lS?)l9R<{vwBmRdH(8aE1xb)Kz4$o& z5AltB@9u%HZdCQy1ze`RLe!xM*gsoWztqLie#B?+N4^!YT?O12ASv~PEAa@ojKWg= zK2WS1>jz_eYD8fl4{RBO-+&$^>_fZtghBsa2O1ldvBtT99-y9%;4<2=Cr^C~ zGEtbFZL}(2cbU5ec1Z`*my>6@>C0DTZs*9#v~+woJx2L1Ng*+xD612qk*2$ojUv3G z#Rn(3IKui!J))#Y8nc-;LmM0OJ$hL%n~gdgxFM?pTK|$u;o(4Ecq>JWsbs!w@*G!Q zJ`YVSoo9P~f28>PPQ=fmoTjCp_AIcnai=^)lM!t$1k_m^bi}zW=e+_r0W2lpi4y;b z7U(BN**7&AE6t`vPif_2@jS3L=9or!RH80%H@>q#KI~Fu>GY{K$iXCsdnl%in4x&r zJ*upBZ*@Y9>8hd^c?I%d7rj}~Yx-WH=8oGd%Hv|Uz!u#gAR5Js5$8?i1~Dd2k_O9{ z?PJR!y_R>`=6Qgq!!V!(K{r=NIwYVcgCX6D;1x{~cNcHd6j@7hZR(krHt(Jvrfg`uHkqTm%KKe3s}2E&>!v5AG96!;tUQULCvIk(;z$=Yu1Sh+v9Br$lqRrv zDr11gvmVk0Y^>}deU1nbr^^F@%%o`34#hZ|jQt+QYZ?%Ku#zxew48w%t+=CVDNzK> z_vB%Hvf3S%nX)mLy%(0VPE4~JMln4LD^aQ&)rkXPc&Js z^~-$O+V5N=cG>+~-?)+UWO!NxPEgdovEj;ZsA06_+JMM|*RZy^;E7#uXJB_q zGMit!70#%AqALTo97%r+l&-DAYs8h`3oFVU&e9w3jquyOeLR&j?(yKvD7_pd$UHm+ zY5v7cDgW8cy=A!;b$>`+&)s?qJB3UqSpwCvPrdyQt!xbY1GgWoRKb4JBYCmc(6jR^>H^2I0WJS)EVL%j!oIB>KrsC+;k1Gye6^mZKn*VIq)&F{S=v7_p zu&bLQXzadwojIBE^WD?y=MXPX`?4Tz#Ede-v);N@EI-o#X~|hGu2XgW!*)HV2NGOV zr>ZLl)^E33iZ8_%CPuvJneyVY_n zzDjj(Y2Mt}*BcH7WS=&m%gnpP=dnPT^m`??Ah7merqdTu^?Sq)t6Gq!%C=n6SpE^m zMvriPX;FBkx#K}g`POc*hMysu!2fiz(cl+C3M};eU}yDBHg1|{gH?5UH8fiN&YIW~ zu!9yuXIR(tOMH~RGpvK_)Lf6x^;1nwWw>?){6J8poP-BrlpJUS z{gDGzr{-Fuk_&Bn@GhsX)>(@>TI1Z;uF{s|h4SL}g@^Kz!vg(#HY$1HIMiv@qEj35 z7oJgNm*iP4X)U`0s;zr2_viLlKM~&)A3QzV{iy38b_OMQ#&w2C2IQmNGBOu+wUmtx z)hpVbZ?RCzC&J>_OBOD-uf`6B>R@6dY!GX5@#UMZScOhi2h{}6+8Y^C6Vzzf2xLazV_g(lN zy!-Eq^7o%=x~$SJxE$4=%iil1fgP$Bw}Jxnc-`W&Ue%P6ybP~d4EIvsu|l4gHIoFj zt}$F6K3$ePSjSmuzW$`_%#x+gI`j0h^k1DM-RaCaDYs;Szo1~rLWWlltzEwc2*b}f zTJ3XkAz9>SzBB%iW?wU~TS*_8&UOBV=grXM1C|{AL*ZM1i#h-(^K6vD$AM4Rv%&&j zF`T*UlskiO-z-Ci4)@@T!3s}m=xK*jd4il$a@t1Z*@j&n^3K^p>1Q0FFCPtr zLSk+Ui9P~pq$9AOYy^IDKSx1-NE8=$K#w1_kekrYL>!awmPE*Cg?Kqr5kCW3kT@MX z#@g|n8;I*W@LqM8+Vj)>h4<)va#&}3c?wL&<{688jj`Z_7>Yi;9;u0r8d*EX{=Rj@ zGbZ<|>)yAjJ)Ax&NgZIBbL=CY$+^b!J0LS|y~e>$fKT`F?i}S_+x$q<{Dep`FChYm z8pBTETx-dk_+K}rj#_PvcW`I6#Xst>#QKgg{a4H)6}PiOi^gVsnbyD1gbBK=%cuGvH<2l)eiP4l;JA*ePNs9oF^bS^TTN;-@`gQ?=J->5oScVU;oO_jvcuq5 z8lv#scEtv)T5P^v{9@Sj3dn>Gf*e5@WJ+&JPr~BY4G(covyTBO-D@eD4xSmEb*jNT zy$RVd0sM603BL&j>YEB5m?XeEvC9Bo7_sIc))mB3E3rO;W}5_isI;#L#RKN@eKfB` zoMJbe%z_=pmLOm5wErWPUCz_#dh3f~X&d{UB2*U0k+$nqTqS1?roh5%=4vaDendJi z>W9-7xp6=;QhH1PN|Y%6LP0QIhNrK1wd>KO4EXZ@h*|-;fdg7 zdvS+jepKRxSm>Fm_>Q*HAb3koC7j~UQ!q=*fxTgV59jNsJ$ib7&U{rw(I-Ke#kE*aQk06+G;E$BJ|O)WM(^1x6#9WEljUABO%j?E>VoQL-$u%hZq2 z*{p!GS>g%NnaU8P!XXbN7xbaC+I|o;#rw{;hVwU^gLdHhu=A#riL#cpk_2CL(21B9 z-&;<-sGkIx@*cA>$u}}GDlt}ULin|wgLzi5UQ`*Qu=fDgeosq&hMW3A^YtY!J}m3+ zrn8ZU<#_l4{22IJ_=Dj`!Pmjp!HdKz%0#nlkc zHCzd}(m_&3KPi;U2#DBKQ2= zz|IFozHX$aqY1ckW8*wK7J_Hv=G&8TCK!ZP%S^1`C{Gs7@i6CL?KT4431^w=LY$DH zXOaBJzF@mI-o{2<^-{jbv{#FI)4W$@sU?yuw6sFGz*JT)opiFEkCw3jSf z@L0){i&l&)WmXQHLl(sZI33(TI0Kv!ZXVonxD{|V#2dd&%gk`Gi2W$uDu!Et>k{~B z@I}V4N8me|iaiJSEZj42Yv7)TTMM@u?rylLHu&n> zd<*U=!iwJT=X&oK_4@OdELpK=Ny&n;Wxsr)q{C53ih6y%*ViE64|pDid-AooKL9_XH#{8o z^j>7|eN?aCuh(B*wybo?;=AtB=g*o|JnJrf&Z1>yB@3R`+pA?y zm6w*?rB7P$)9bUJEG;ctzWlK#7cF{PzpVGOl+Ybs@BK2y%p4}bY2l*abZ~>=V&LN8 z^r|`O3(B5KU$Nrp*_AAa;{T_g(uRA+17pezyJhLHGY_YRYfXbK2)Hyn?*}(y9M|2!OxjNighKl}tv1DA@()F0*`GJUKbk;36qC!u~} z!wvO^wmfXS`K_x_Qf&9uld_r#dk-~R(#|~@!7&3EEJE`9t`=GRUw~E?ezy+c5J6#(|ykl zey8A@XKL^ZSmx?g4LP zy!^Ymzi5xlP0C&J_JU;1Csh6Tjj|{ndpP9Ro>2Unzx^&JRL-2-VT;v`jfc00H~jZq ztIih>d2;5QEbH{A#*Z66VVwE#$0lUVe=KAC#IgxWKS*V_ zdb&AuNfr51pRX!s+BI;YJfJW3UKcN7+TqKY_BRWe_Qn&T@Q)l!o6>vTmnsH+(gHM# zetU@B4jz&>0FpfcLCV zE~;QTvx>;+tyn5&J~*zDMWZ9C(Bx~zPhlL#sd(;S4aN-|r2#Eu#ZOwEp0phK`@=?z zzZaNxZC&X3i@2`ny{7o-x7AbXWYnLzx4JM#pp0fV`+*sm8TW;Lp_{@G>xCbKt2ia(|MaKQ4+yOnh^@MUGt>XmTZjaM_f*b1$LB+3r7=67 z55Cl_bGZ!?d|vD6Lq_9zn^^H(8IUis|uM8?ZWzg5mPnEx8Fxk(^+o6k1ZNH z=k|NUbmqAIe(VF9+S~6ley-bn`~6)H4n2I^y|O-NRTfaJwBXDM;k^cfyX&zx$DWhM z13l}@UY}3Tk0MUr{gl27_?F&5)Q#}_a6ePPx`n+mT~m+}tY_3mZ@sTPOQdHy>Zw?4 G82f*D1G%XH diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index ed93517d80dc..4aaf1e6ef70a 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -52,7 +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_PLUGIN=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/Kconfig b/src/modules/simulation/Kconfig index 249d66e012c7..1e4ef6d39c2e 100644 --- a/src/modules/simulation/Kconfig +++ b/src/modules/simulation/Kconfig @@ -12,7 +12,7 @@ menu "Simulation" select MODULES_SIMULATION_SIMULATOR_MAVLINK select MODULES_SIMULATION_SIMULATOR_SIH select MODULES_SIMULATION_GZ_MSG - select MODULES_SIMULATION_GZ_PLUGIN + select MODULES_SIMULATION_GZ_PLUGINS ---help--- Enable default set of simulation modules rsource "*/Kconfig" diff --git a/src/modules/simulation/gz_plugin/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt similarity index 100% rename from src/modules/simulation/gz_plugin/CMakeLists.txt rename to src/modules/simulation/gz_plugins/CMakeLists.txt diff --git a/src/modules/simulation/gz_plugin/Kconfig b/src/modules/simulation/gz_plugins/Kconfig similarity index 70% rename from src/modules/simulation/gz_plugin/Kconfig rename to src/modules/simulation/gz_plugins/Kconfig index bec6f3cf49a4..7766d1945afb 100644 --- a/src/modules/simulation/gz_plugin/Kconfig +++ b/src/modules/simulation/gz_plugins/Kconfig @@ -1,4 +1,4 @@ -menuconfig MODULES_SIMULATION_GZ_PLUGIN +menuconfig MODULES_SIMULATION_GZ_PLUGINS bool "gz_plugin" default n depends on PLATFORM_POSIX diff --git a/src/modules/simulation/gz_plugin/OpticalFlowSensor.cpp b/src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp similarity index 100% rename from src/modules/simulation/gz_plugin/OpticalFlowSensor.cpp rename to src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp diff --git a/src/modules/simulation/gz_plugin/OpticalFlowSensor.hpp b/src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp similarity index 100% rename from src/modules/simulation/gz_plugin/OpticalFlowSensor.hpp rename to src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp diff --git a/src/modules/simulation/gz_plugin/OpticalFlowSystem.cpp b/src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp similarity index 100% rename from src/modules/simulation/gz_plugin/OpticalFlowSystem.cpp rename to src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp diff --git a/src/modules/simulation/gz_plugin/OpticalFlowSystem.hpp b/src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp similarity index 100% rename from src/modules/simulation/gz_plugin/OpticalFlowSystem.hpp rename to src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp diff --git a/src/modules/simulation/gz_plugin/optical_flow.proto b/src/modules/simulation/gz_plugins/optical_flow.proto similarity index 100% rename from src/modules/simulation/gz_plugin/optical_flow.proto rename to src/modules/simulation/gz_plugins/optical_flow.proto From e1e7a5a8eb3cfe67fb940a783996abb6602d6c8a Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 7 Jan 2025 01:55:46 -0900 Subject: [PATCH 12/16] disable gps, add plugin path --- .../init.d-posix/airframes/4021_gz_x500_flow | 12 ++++++------ src/modules/simulation/gz_bridge/gz_env.sh.in | 3 ++- 2 files changed, 8 insertions(+), 7 deletions(-) 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 index c4d5e04028d9..cd568b3c6be1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow @@ -12,12 +12,12 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_flow} 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 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/src/modules/simulation/gz_bridge/gz_env.sh.in b/src/modules/simulation/gz_bridge/gz_env.sh.in index 810bc8894871..ad3fbd6d643f 100644 --- a/src/modules/simulation/gz_bridge/gz_env.sh.in +++ b/src/modules/simulation/gz_bridge/gz_env.sh.in @@ -2,5 +2,6 @@ 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_RESOURCE_PATH=$GZ_SIM_RESOURCE_PATH:$PX4_GZ_MODELS:$PX4_GZ_WORLDS:$PX4_GZ_PLUGINS From 093dc097e8fb9ca17f5f8e284067139ce9ad38a2 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 7 Jan 2025 14:11:05 -0900 Subject: [PATCH 13/16] cleanup --- src/modules/simulation/gz_bridge/GZBridge.hpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 18bc783c2806..af06774e6c52 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -118,8 +118,6 @@ class GZBridge : public ModuleBase, 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); - - // TODO: change to sensor_msgs::msgs::OpticalFlow void opticalFlowCallback(const sensor_msgs::msgs::OpticalFlow &image_msg); /** @@ -181,12 +179,10 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S 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_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 _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, _node_mutex}; From cc3292e72b9f45a01e6afc10fe3e6653d1357d4f Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 7 Jan 2025 22:09:45 -0900 Subject: [PATCH 14/16] fix plugin path export --- src/modules/simulation/gz_bridge/gz_env.sh.in | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/simulation/gz_bridge/gz_env.sh.in b/src/modules/simulation/gz_bridge/gz_env.sh.in index ad3fbd6d643f..a62ddf0b8b76 100644 --- a/src/modules/simulation/gz_bridge/gz_env.sh.in +++ b/src/modules/simulation/gz_bridge/gz_env.sh.in @@ -4,4 +4,5 @@ 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:$PX4_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 From 2737454ce997262018fbf0a76f7d733e25861a2d Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 7 Jan 2025 23:20:44 -0900 Subject: [PATCH 15/16] gz submodule --- Tools/simulation/gz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gz b/Tools/simulation/gz index a530ba14166b..fe65c0a3df08 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit a530ba14166b756848b285efbba9d49f7a0a546e +Subproject commit fe65c0a3df080e44d8748fdb0a9fd3d0b38c8b49 From e6e2a834a2afad6f2a5e7978a06c95530fb86d57 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 8 Jan 2025 14:55:13 -0900 Subject: [PATCH 16/16] properly add OpticalFlowSystem dependency to gz --- src/modules/simulation/gz_bridge/CMakeLists.txt | 4 ++-- src/modules/simulation/gz_plugins/CMakeLists.txt | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 553ef653255b..82cc2b4ac7b2 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -125,14 +125,14 @@ if(gz-transport_FOUND) COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_only} $ 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} $ WORKING_DIRECTORY ${SITL_WORKING_DIR} USES_TERMINAL - DEPENDS px4 + DEPENDS px4 OpticalFlowSystem ) endif() endforeach() diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt index d490d2223c0f..8d1c00a214d7 100644 --- a/src/modules/simulation/gz_plugins/CMakeLists.txt +++ b/src/modules/simulation/gz_plugins/CMakeLists.txt @@ -39,7 +39,6 @@ target_link_libraries(${PROJECT_NAME} PUBLIC gz-transport12::gz-transport12 PUBLIC ${OpenCV_LIBS} PUBLIC ${OpticalFlow_LIBS} - # PUBLIC ${PROTOBUF_LIBRARIES} ) target_include_directories(${PROJECT_NAME}