From 5636ddacab9aacb08f55dabd489ddbb95afef7f0 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Fri, 20 Dec 2024 14:31:36 -0900 Subject: [PATCH 1/6] 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 2/6] 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 3/6] 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 4/6] 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 5/6] 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 6/6] 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