[wip] gz plugins

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
+

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

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

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

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);
 }

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,54 @@ int GZBridge::init()
 	return OK;
 }
 
+// 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[])
 {
 	const char *world_name = "default";

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -62,6 +62,7 @@
 #include <uORB/topics/vehicle_odometry.h>
 #include <uORB/topics/wheel_encoders.h>
 #include <uORB/topics/obstacle_distance.h>
+#include <uORB/topics/sensor_optical_flow.h>
 
 #include <gz/math.hh>
 #include <gz/msgs.hh>
@@ -117,6 +118,9 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan);
 	void laserScanCallback(const gz::msgs::LaserScan &scan);
 
+	// TODO: change to sensor_msgs::msgs::OpticalFlow
+	void opticalFlowCallback(const gz::msgs::Image &image_msg);
+
 	/**
 	 * @brief Call Entityfactory service
 	 *
@@ -182,6 +186,8 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	uORB::PublicationMulti<sensor_gyro_s>  _sensor_gyro_pub{ORB_ID(sensor_gyro)};
 	uORB::PublicationMulti<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
 
+	uORB::PublicationMulti<sensor_optical_flow_s> _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};

src/modules/simulation/gz_plugin/CMakeLists.txt

@@ -0,0 +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)
+gz_find_package(gz-sim8 REQUIRED)
+gz_find_package(gz-sensors8 REQUIRED)
+gz_find_package(gz-transport12 REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(Protobuf REQUIRED)
+
+PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS optical_flow.proto)
+
+add_library(${PROJECT_NAME} SHARED
+    OpticalFlow.cpp
+    OpticalFlowSystem.cpp
+    ${PROTO_SRCS}
+    ${PROTO_HDRS}
+)
+
+target_link_libraries(${PROJECT_NAME}
+    PUBLIC gz-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(${PROJECT_NAME}
+    PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
+    PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
+    PUBLIC ${OpenCV_INCLUDE_DIRS}
+)

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

src/modules/simulation/gz_plugin/OpticalFlow.cpp

@@ -0,0 +1,117 @@
+#include <gz/common/Console.hh>
+#include <gz/msgs/Utility.hh>
+#include <gz/sensors/Util.hh>
+
+#include "OpticalFlow.hpp"
+#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<sensor_msgs::msgs::OpticalFlow>(this->Topic());
+
+	auto elem = _sdf.Element();
+	auto opticalFlowElem = elem->GetElement("gz:optical_flow");
+	auto cameraTopic = opticalFlowElem->Get<std::string>("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)
+{
+	// gzdbg << "OpticalFlow::OnImage" << std::endl;
+
+	cv::Mat frame;
+
+	if (_msg.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8) {
+		frame = cv::Mat(_msg.height(), _msg.width(), CV_8UC3, (void *)_msg.data().c_str());
+		cv::cvtColor(frame, frame, cv::COLOR_RGB2GRAY);
+
+	} else if (_msg.pixel_format_type() == gz::msgs::PixelFormatType::L_INT8) {
+		frame = cv::Mat(_msg.height(), _msg.width(), CV_8UC1, (void *)_msg.data().c_str());
+
+	} else {
+		gzerr << "Unsupported image format" << std::endl;
+		return;
+	}
+
+	if (prevFrame.empty()) {
+		frame.copyTo(prevFrame);
+		return;
+	}
+
+	cv::Mat flow;
+	cv::calcOpticalFlowFarneback(prevFrame, frame, flow,
+				     0.5, // Pyramid scale
+				     3,   // Pyramid levels
+				     15,  // Window size
+				     3,   // Iteratpions
+				     5,   // Poly_n
+				     1.2, // Poly_sigma
+				     0);  // Flags
+
+	// Calculate average flow and quality
+	cv::Scalar meanFlow = cv::mean(flow);
+
+	// Update integrated flow (in radians)
+	// TODO: FOV from SDF
+	// Assuming 60° horizontal field of view and 45° vertical field of view
+	const double rad_per_pixel_x = (M_PI / 3.0) / frame.cols;  // 60° in radians / width
+	const double rad_per_pixel_y = (M_PI / 4.0) / frame.rows;  // 45° in radians / height
+
+	this->integrated_x = meanFlow[0] * rad_per_pixel_x;
+	this->integrated_y = meanFlow[1] * rad_per_pixel_y;
+
+	// Calculate quality (0-255)
+	cv::Mat magnitude, angle;
+	cv::cartToPolar(flow.col(0), flow.col(1), magnitude, angle);
+	this->quality = cv::mean(magnitude)[0] * 255.0;
+
+	if (this->quality > 255.0f) {
+		this->quality = 255.0f;
+	}
+
+	frame.copyTo(prevFrame);
+}
+
+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<std::chrono::microseconds>(currentTime - this->lastUpdateTime);
+
+	sensor_msgs::msgs::OpticalFlow msg;
+	msg.set_time_usec(std::chrono::duration_cast<std::chrono::microseconds>(_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;
+}

src/modules/simulation/gz_plugin/OpticalFlow.hpp

@@ -0,0 +1,30 @@
+#pragma once
+
+#include <gz/sensors/Sensor.hh>
+#include <gz/sensors/CameraSensor.hh>
+#include <gz/sensors/SensorTypes.hh>
+#include <gz/transport/Node.hh>
+#include <gz/msgs/image.pb.h>
+#include <opencv2/opencv.hpp>
+
+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