From 5e77e99aec2ab5a50ec84e177da7fee271adb3c6 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 30 Jul 2024 11:08:59 -0800 Subject: [PATCH] GZ support for optical flow --- .../airframes/4014_gz_x500_mono_cam_down | 16 ++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + Tools/simulation/gz | 2 +- src/modules/logger/logged_topics.cpp | 2 +- .../simulation/gz_bridge/CMakeLists.txt | 6 +- src/modules/simulation/gz_bridge/GZBridge.cpp | 91 +++++++++++++++++++ src/modules/simulation/gz_bridge/GZBridge.hpp | 7 ++ .../gz_bridge/camera/CMakeLists.txt | 83 +++++++++++++++++ .../simulation/gz_bridge/camera/gz_camera.cpp | 35 +++++++ .../simulation/gz_bridge/camera/gz_camera.hpp | 6 ++ 10 files changed, 246 insertions(+), 3 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_x500_mono_cam_down 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/4014_gz_x500_mono_cam_down b/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_x500_mono_cam_down new file mode 100644 index 000000000000..e96a02c0b1f7 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_x500_mono_cam_down @@ -0,0 +1,16 @@ +#!/bin/sh +# +# @name Gazebo x500 mono cam +# +# @type Quadrotor +# + +PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_mono_cam_down} + +. ${R}etc/init.d-posix/airframes/4001_gz_x500 + +param set-default EKF2_OF_CTRL 1 +param set-default EKF2_RNG_A_VMAX 3 + +echo "disabling Sim GPS sats" +param set-default SIM_GPS_USED 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 9235b2e66340..d53330cec1b8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -85,6 +85,7 @@ px4_add_romfs_files( 4011_gz_lawnmower 4012_gz_rover_ackermann 4013_gz_x500_lidar + 4014_gz_x500_mono_cam_down 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 312cd002ff96..dcc4b06f66d5 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 312cd002ff9602644efef58eef93e447c10dcbe8 +Subproject commit dcc4b06f66d5d8eb56295cdaaee0aebbaac0cf55 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index fc187a954d0e..437cd423fde2 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -183,7 +183,7 @@ void LoggedTopics::add_default_topics() add_topic_multi("sensor_gnss_relative", 1000, 1); add_optional_topic_multi("sensor_gyro", 1000, 4); add_topic_multi("sensor_mag", 1000, 4); - add_topic_multi("sensor_optical_flow", 1000, 2); + add_topic_multi("sensor_optical_flow", 0, 2); add_topic_multi("vehicle_imu", 500, 4); add_topic_multi("vehicle_imu_status", 1000, 4); diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 003a712282f9..394ac6af39ce 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 # First look for GZ Harmonic libraries find_package(gz-transport NAMES gz-transport13) @@ -69,6 +71,7 @@ if(gz-transport_FOUND) DEPENDS mixer_module px4_work_queue + gz_camera ${GZ_TRANSPORT_LIB} MODULE_CONFIG module.yaml @@ -92,7 +95,8 @@ if(gz-transport_FOUND) default windy baylands - lawn + lawn + aruco ) # find corresponding airframes diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 3464c628694c..6126c7511689 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -43,6 +43,9 @@ #include #include +#include "camera/gz_camera.hpp" +#include // For DeviceId union + GZBridge::GZBridge(const char *world, const char *name, const char *model, const char *pose_str) : ModuleParams(nullptr), @@ -232,6 +235,22 @@ int GZBridge::init() return PX4_ERROR; } + // Range finder callback + std::string rangefinder_topic = "/rangefinder"; + + if (!_node.Subscribe(rangefinder_topic, &GZBridge::rangeFinderCallback, this)) { + PX4_ERR("failed to subscribe to %s", rangefinder_topic.c_str()); + return PX4_ERROR; + } + + // Video frame callback + 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; @@ -251,6 +270,78 @@ 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::rangeFinderCallback(const gz::msgs::LaserScan &scan) +{ + distance_sensor_s msg = {}; + msg.timestamp = hrt_absolute_time(); + + device::Device::DeviceId id; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.bus = 1; + id.devid_s.address = 1; + id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; + msg.device_id = id.devid; + + msg.min_distance = static_cast(scan.range_min()); + msg.max_distance = static_cast(scan.range_max()); + msg.current_distance = static_cast(scan.ranges()[0]); + msg.variance = 0.0f; + msg.signal_quality = -1; + msg.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + msg.h_fov = 0; + msg.v_fov = 0; + msg.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + + _distance_sensor_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 8a832f7961b2..98da9fba5a6e 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -59,6 +59,8 @@ #include #include #include +#include +#include #include #include @@ -109,6 +111,8 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry); void navSatCallback(const gz::msgs::NavSat &nav_sat); void laserScanCallback(const gz::msgs::LaserScan &scan); + void cameraCallback(const gz::msgs::Image &image_msg); + void rangeFinderCallback(const gz::msgs::LaserScan &scan); /** * @@ -135,6 +139,9 @@ 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)}; + uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor)}; + 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);