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..cd568b3c6be1 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow @@ -0,0 +1,23 @@ +#!/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" +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/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/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 230450cc817d..fe65c0a3df08 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 230450cc817dd7675612ed5ec72ee59b6989d367 +Subproject commit fe65c0a3df080e44d8748fdb0a9fd3d0b38c8b49 diff --git a/boards/ark/fpv/extras/ark_fpv_bootloader.bin b/boards/ark/fpv/extras/ark_fpv_bootloader.bin deleted file mode 100755 index 9ca59954b374..000000000000 Binary files a/boards/ark/fpv/extras/ark_fpv_bootloader.bin and /dev/null differ diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 1584f93ef647..4aaf1e6ef70a 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_PLUGINS=y CONFIG_MODULES_SIMULATION_SENSOR_AGP_SIM=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y diff --git a/src/modules/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); } diff --git a/src/modules/simulation/Kconfig b/src/modules/simulation/Kconfig index a417092ed94b..1e4ef6d39c2e 100644 --- a/src/modules/simulation/Kconfig +++ b/src/modules/simulation/Kconfig @@ -11,6 +11,8 @@ menu "Simulation" select MODULES_SIMULATION_SENSOR_MAG_SIM select MODULES_SIMULATION_SIMULATOR_MAVLINK select MODULES_SIMULATION_SIMULATOR_SIH + select MODULES_SIMULATION_GZ_MSG + select MODULES_SIMULATION_GZ_PLUGINS ---help--- Enable default set of simulation modules rsource "*/Kconfig" diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index e57c6020451b..82cc2b4ac7b2 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -71,6 +71,9 @@ if(gz-transport_FOUND) module.yaml ) + # TODO: this doesn't feel right + target_link_libraries(modules__simulation__gz_bridge PUBLIC gz_px4_messages) + px4_add_git_submodule(TARGET git_gz PATH "${PX4_SOURCE_DIR}/Tools/simulation/gz") include(ExternalProject) ExternalProject_Add(gz @@ -122,14 +125,14 @@ if(gz-transport_FOUND) COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_only} $ WORKING_DIRECTORY ${SITL_WORKING_DIR} USES_TERMINAL - DEPENDS px4 + DEPENDS px4 OpticalFlowSystem ) else() add_custom_target(gz_${model_only}_${world_name} COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_only} PX4_GZ_WORLD=${world_name} $ WORKING_DIRECTORY ${SITL_WORKING_DIR} USES_TERMINAL - DEPENDS px4 + DEPENDS px4 OpticalFlowSystem ) endif() endforeach() diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 2dc1ef13e4f4..167a3bfde729 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -253,6 +253,14 @@ int GZBridge::init() return PX4_ERROR; } + // Camera: + std::string flow_topic = "/optical_flow"; + + if (!_node.Subscribe(flow_topic, &GZBridge::opticalFlowCallback, this)) { + PX4_ERR("failed to subscribe to %s", flow_topic.c_str()); + return PX4_ERROR; + } + if (!_mixing_interface_esc.init(_model_name)) { PX4_ERR("failed to init ESC output"); return PX4_ERROR; @@ -277,6 +285,46 @@ int GZBridge::init() return OK; } +// TODO: change to sensor_msgs::msgs::OpticalFlow +void GZBridge::opticalFlowCallback(const sensor_msgs::msgs::OpticalFlow &flow) +{ + + // Construct SensorOpticalFlow message + sensor_optical_flow_s msg = {}; + + msg.timestamp = hrt_absolute_time(); + msg.timestamp_sample = flow.time_usec(); + msg.pixel_flow[0] = flow.integrated_x(); + msg.pixel_flow[1] = flow.integrated_y(); + msg.quality = flow.quality(); + msg.integration_timespan_us = flow.integration_time_us(); + + // Static data + device::Device::DeviceId id; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.bus = 0; + id.devid_s.address = 0; + id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM; + msg.device_id = id.devid; + + // values taken from PAW3902 + msg.mode = sensor_optical_flow_s::MODE_LOWLIGHT; + msg.max_flow_rate = 7.4f; + // msg.min_ground_distance = 0.08f; + msg.min_ground_distance = 0.f; + msg.max_ground_distance = 30.f; + msg.error_count = 0; + + // No delta angle + // No distance + + // This means that delta angle will come from vehicle gyro + // Distance will come from vehicle distance sensor + + // 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..af06774e6c52 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 @@ -75,6 +76,7 @@ #include #include #include +#include using namespace time_literals; @@ -116,6 +118,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 opticalFlowCallback(const sensor_msgs::msgs::OpticalFlow &image_msg); /** * @brief Call Entityfactory service @@ -176,11 +179,11 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; - - uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; - uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; - uORB::PublicationMulti _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; + uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; + uORB::PublicationMulti _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; + uORB::PublicationMulti _optical_flow_pub{ORB_ID(sensor_optical_flow)}; GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex}; GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex}; diff --git a/src/modules/simulation/gz_bridge/gz_env.sh.in b/src/modules/simulation/gz_bridge/gz_env.sh.in index 810bc8894871..a62ddf0b8b76 100644 --- a/src/modules/simulation/gz_bridge/gz_env.sh.in +++ b/src/modules/simulation/gz_bridge/gz_env.sh.in @@ -2,5 +2,7 @@ export PX4_GZ_MODELS=@PX4_SOURCE_DIR@/Tools/simulation/gz/models export PX4_GZ_WORLDS=@PX4_SOURCE_DIR@/Tools/simulation/gz/worlds +export PX4_GZ_PLUGINS=@PX4_BINARY_DIR@/src/modules/simulation/gz_plugins/ export GZ_SIM_RESOURCE_PATH=$GZ_SIM_RESOURCE_PATH:$PX4_GZ_MODELS:$PX4_GZ_WORLDS +export GZ_SIM_SYSTEM_PLUGIN_PATH=$GZ_SIM_SYSTEM_PLUGIN_PATH:$PX4_GZ_PLUGINS diff --git a/src/modules/simulation/gz_msg/CMakeLists.txt b/src/modules/simulation/gz_msg/CMakeLists.txt new file mode 100644 index 000000000000..3f0488a5d24e --- /dev/null +++ b/src/modules/simulation/gz_msg/CMakeLists.txt @@ -0,0 +1,24 @@ +find_package(Protobuf REQUIRED) + +PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS + optical_flow.proto +) + +# add_library(gz_px4_messages STATIC +# ${PROTO_SRCS} +# ${PROTO_HDRS} +# ) + +px4_add_library(gz_px4_messages + ${PROTO_SRCS} + ${PROTO_HDRS} +) + +target_include_directories(gz_px4_messages + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} +) + +target_link_libraries(gz_px4_messages + PUBLIC ${PROTOBUF_LIBRARIES} +) diff --git a/src/modules/simulation/gz_msg/Kconfig b/src/modules/simulation/gz_msg/Kconfig new file mode 100644 index 000000000000..9acc0542d314 --- /dev/null +++ b/src/modules/simulation/gz_msg/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_SIMULATION_GZ_MSG + bool "gz_msg" + default n + depends on PLATFORM_POSIX + ---help--- + Enable support for gz_msg diff --git a/src/modules/simulation/gz_msg/optical_flow.proto b/src/modules/simulation/gz_msg/optical_flow.proto new file mode 100644 index 000000000000..b922b81b434d --- /dev/null +++ b/src/modules/simulation/gz_msg/optical_flow.proto @@ -0,0 +1,20 @@ +// msgs/optical_flow.proto +syntax = "proto3"; +package sensor_msgs.msgs; + +message OpticalFlow { + // Timestamp (microseconds, since system start) + int64 time_usec = 1; + + // Integration time + uint32 integration_time_us = 2; + + // Integrated x-axis flow (rad) + float integrated_x = 3; + + // Integrated y-axis flow (rad) + float integrated_y = 4; + + // Quality of optical flow measurement (0: bad, 255: maximum quality) + float quality = 5; +} diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt new file mode 100644 index 000000000000..8d1c00a214d7 --- /dev/null +++ b/src/modules/simulation/gz_plugins/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.11.0 FATAL_ERROR) +find_package(gz-cmake3 REQUIRED) + +project(OpticalFlowSystem) + +gz_find_package(gz-plugin2 REQUIRED COMPONENTS register) +gz_find_package(gz-sim8 REQUIRED) +gz_find_package(gz-sensors8 REQUIRED) +gz_find_package(gz-transport12 REQUIRED) +find_package(OpenCV REQUIRED) + +include(ExternalProject) + +ExternalProject_Add(OpticalFlow + GIT_REPOSITORY https://github.com/PX4/PX4-OpticalFlow.git + GIT_TAG master + PREFIX ${CMAKE_BINARY_DIR}/OpticalFlow + INSTALL_DIR ${CMAKE_BINARY_DIR}/OpticalFlow/install + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX= + BUILD_BYPRODUCTS ${CMAKE_BINARY_DIR}/OpticalFlow/install/lib/libOpticalFlow.so +) + +ExternalProject_Get_Property(OpticalFlow install_dir) + +set(OpticalFlow_INCLUDE_DIRS ${install_dir}/include) +set(OpticalFlow_LIBS ${install_dir}/lib/libOpticalFlow.so) + +add_library(${PROJECT_NAME} SHARED + OpticalFlowSensor.cpp + OpticalFlowSystem.cpp +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC gz_px4_messages + PUBLIC gz-sensors8::gz-sensors8 + PUBLIC gz-rendering8::gz-rendering8 + PUBLIC gz-plugin2::gz-plugin2 + PUBLIC gz-sim8::gz-sim8 + PUBLIC gz-transport12::gz-transport12 + PUBLIC ${OpenCV_LIBS} + PUBLIC ${OpticalFlow_LIBS} +) + +target_include_directories(${PROJECT_NAME} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC ${OpenCV_INCLUDE_DIRS} + PUBLIC ${OpticalFlow_INCLUDE_DIRS} +) + +add_dependencies(${PROJECT_NAME} OpticalFlow) diff --git a/src/modules/simulation/gz_plugins/Kconfig b/src/modules/simulation/gz_plugins/Kconfig new file mode 100644 index 000000000000..7766d1945afb --- /dev/null +++ b/src/modules/simulation/gz_plugins/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_SIMULATION_GZ_PLUGINS + bool "gz_plugin" + default n + depends on PLATFORM_POSIX + ---help--- + Enable support for gz_plugin diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp b/src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp new file mode 100644 index 000000000000..8249d80ff97e --- /dev/null +++ b/src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp @@ -0,0 +1,126 @@ +#include +#include +#include + +#include "OpticalFlowSensor.hpp" +#include "optical_flow.pb.h" + +using namespace custom; + +bool OpticalFlowSensor::Load(const sdf::Sensor &_sdf) +{ + auto type = gz::sensors::customType(_sdf); + if ("optical_flow" != type) { + gzerr << "Trying to load [optical_flow] sensor, but got type [" << type << "] instead." << std::endl; + return false; + } + + gz::sensors::Sensor::Load(_sdf); + + _publisher = _node.Advertise(this->Topic()); + gzdbg << "Advertising optical flow data on: " << this->Topic() << std::endl; + + // Get camera topic from our sensor config + auto elem = _sdf.Element(); + auto opticalFlowElem = elem->GetElement("gz:optical_flow"); + auto camera_topic = opticalFlowElem->Get("camera_topic"); + + std::string topic; + int image_width = 0; + int image_height = 0; + int update_rate = 0; + float hfov = 0; + + // Get FOV from the actual camera sensor's config + auto sensorElem = elem->GetParent()->GetElement("sensor"); + while (sensorElem) { + if (sensorElem->Get("name") == "flow_camera") { + + auto cameraElem = sensorElem->GetElement("camera"); + update_rate = sensorElem->GetElement("update_rate")->Get(); + hfov = cameraElem->GetElement("horizontal_fov")->Get(); + + auto imageElem = cameraElem->GetElement("image"); + image_width = imageElem->GetElement("width")->Get(); + image_height = imageElem->GetElement("height")->Get(); + break; + } + sensorElem = sensorElem->GetNextElement("sensor"); + } + + gzdbg << "image_width: " << image_width << std::endl; + gzdbg << "image_height: " << image_height << std::endl; + gzdbg << "update_rate: " << update_rate << std::endl; + gzdbg << "hfov: " << hfov << std::endl; + + // Subscribe to camera + gzdbg << "Subscribing to camera topic: " << camera_topic << std::endl; + if (!_node.Subscribe(camera_topic, &OpticalFlowSensor::OnImage, this)) { + gzerr << "Failed to subscribe to camera topic: " << camera_topic << std::endl; + return false; + } + + // TODO: get from sdf + float focal_length = (image_width / 2.0f) / tan(hfov / 2.0f); + + // Create OpticalFlow + _optical_flow = std::make_shared(focal_length, focal_length, update_rate, image_width, image_height); + + return true; +} + +void OpticalFlowSensor::OnImage(const gz::msgs::Image &image_msg) +{ + if (image_msg.width() == 0 || image_msg.height() == 0) { + gzerr << "Invalid image dimensions" << std::endl; + return; + } + + if (image_msg.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8) { + cv::Mat temp(image_msg.height(), image_msg.width(), CV_8UC3); + std::memcpy(temp.data, image_msg.data().c_str(), image_msg.data().size()); + cv::cvtColor(temp, _last_image_gray, cv::COLOR_RGB2GRAY); + + } else if (image_msg.pixel_format_type() == gz::msgs::PixelFormatType::L_INT8) { + std::memcpy(_last_image_gray.data, image_msg.data().c_str(), image_msg.data().size()); + + } else { + gzerr << "Unsupported image format" << std::endl; + return; + } + + // Store current timestamp for integration time calculation + uint32_t current_timestamp = (image_msg.header().stamp().sec() * 1000000ULL + + image_msg.header().stamp().nsec() / 1000ULL) & 0xFFFFFFFF; + + if (_last_image_timestamp != 0) { + _integration_time_us = (current_timestamp - _last_image_timestamp) & 0xFFFFFFFF; + } + + _last_image_timestamp = current_timestamp; + _new_image_available = true; +} + +bool OpticalFlowSensor::Update(const std::chrono::steady_clock::duration &_now) +{ + if (!_new_image_available) { + return true; + } + + sensor_msgs::msgs::OpticalFlow msg; + msg.set_time_usec(_last_image_timestamp); + + int quality = _optical_flow->calcFlow(_last_image_gray.data, _last_image_timestamp, _integration_time_us, _flow_x, _flow_y); + + msg.set_integrated_x(_flow_x); + msg.set_integrated_y(_flow_y); + msg.set_integration_time_us(_integration_time_us); + msg.set_quality(quality); + + if (!_publisher.Publish(msg)) { + gzwarn << "Failed to publish optical flow message" << std::endl; + } + + _new_image_available = false; + return true; +} diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp b/src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp new file mode 100644 index 000000000000..ae62daf2532c --- /dev/null +++ b/src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "flow_opencv.hpp" + +namespace custom +{ +class OpticalFlowSensor : public gz::sensors::Sensor +{ +public: + virtual bool Load(const sdf::Sensor &_sdf) override; + virtual bool Update(const std::chrono::steady_clock::duration &_now) override; + +private: + void OnImage(const gz::msgs::Image &_msg); + + gz::transport::Node _node; + gz::transport::Node::Publisher _publisher; + + // Flow + std::shared_ptr _optical_flow {nullptr}; + float _flow_x {0.0f}; + float _flow_y {0.0f}; + int _integration_time_us; + + // Camera + double _horizontal_fov {0.0}; + double _vertical_fov {0.0}; + + cv::Mat _last_image_gray; + uint32_t _last_image_timestamp {0}; + bool _new_image_available {false}; +}; + +} // end namespace custom diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp b/src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp new file mode 100644 index 000000000000..47eda76b6f00 --- /dev/null +++ b/src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp @@ -0,0 +1,91 @@ +// OpticalFlowSystem.cc +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "OpticalFlowSensor.hpp" +#include "OpticalFlowSystem.hpp" + +using namespace custom; + +void OpticalFlowSystem::PreUpdate(const gz::sim::UpdateInfo &, gz::sim::EntityComponentManager &_ecm) +{ + _ecm.EachNew( + [&](const gz::sim::Entity & _entity, + const gz::sim::components::CustomSensor * _custom, + const gz::sim::components::ParentEntity * _parent)->bool + { + auto sensorScopedName = gz::sim::removeParentScope(gz::sim::scopedName(_entity, _ecm, "::", false), "::"); + + sdf::Sensor data = _custom->Data(); + data.SetName(sensorScopedName); + + if (data.Topic().empty()) { + std::string topic = scopedName(_entity, _ecm) + "/optical_flow"; + data.SetTopic(topic); + } + + gz::sensors::SensorFactory sensorFactory; + auto sensor = sensorFactory.CreateSensor(data); + + if (sensor == nullptr) { + gzerr << "Failed to create optical flow sensor [" << sensorScopedName << "]" << std::endl; + return false; + } + + auto parentName = _ecm.Component(_parent->Data())->Data(); + + sensor->SetParent(parentName); + + _ecm.CreateComponent(_entity, gz::sim::components::SensorTopic(sensor->Topic())); + + this->entitySensorMap.insert(std::make_pair(_entity, std::move(sensor))); + + gzdbg << "OpticalFlowSystem PreUpdate" << std::endl; + + return true; + }); +} + +void OpticalFlowSystem::PostUpdate(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm) +{ + if (!_info.paused) { + for (auto &[entity, sensor] : this->entitySensorMap) { + sensor->Update(_info.simTime); + } + } + + this->RemoveSensorEntities(_ecm); +} + +void OpticalFlowSystem::RemoveSensorEntities(const gz::sim::EntityComponentManager &_ecm) +{ + _ecm.EachRemoved( + [&](const gz::sim::Entity & _entity, + const gz::sim::components::CustomSensor *)->bool + { + if (this->entitySensorMap.erase(_entity) == 0) { + gzerr << "Internal error, missing optical flow sensor for entity [" + << _entity << "]" << std::endl; + } + return true; + }); +} + +GZ_ADD_PLUGIN(OpticalFlowSystem, gz::sim::System, + OpticalFlowSystem::ISystemPreUpdate, + OpticalFlowSystem::ISystemPostUpdate +) + +GZ_ADD_PLUGIN_ALIAS(OpticalFlowSystem, "custom::OpticalFlowSystem") diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp b/src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp new file mode 100644 index 000000000000..3f868c6eeb65 --- /dev/null +++ b/src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp @@ -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_plugins/optical_flow.proto b/src/modules/simulation/gz_plugins/optical_flow.proto new file mode 100644 index 000000000000..b7012cc42d00 --- /dev/null +++ b/src/modules/simulation/gz_plugins/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; +}