[gz] optical flow support

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

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

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

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

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -43,6 +43,9 @@
 #include <iostream>
 #include <string>
 
+#include "camera/gz_camera.hpp"
+#include <lib/drivers/device/Device.hpp> // 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<float>(scan.range_min());
+	msg.max_distance = static_cast<float>(scan.range_max());
+	msg.current_distance = static_cast<float>(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";

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -59,6 +59,8 @@
 #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 <uORB/topics/distance_sensor.h>
 
 #include <gz/math.hh>
 #include <gz/msgs.hh>
@@ -109,6 +111,8 @@ class GZBridge : public ModuleBase<GZBridge>, 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<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)};
+	uORB::PublicationMulti<distance_sensor_s>     _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};

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=<INSTALL_DIR>
+    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()

src/modules/simulation/gz_bridge/camera/gz_camera.cpp

@@ -0,0 +1,35 @@
+#include "gz_camera.hpp"
+
+#include <gz/msgs.hh>
+#include <gz/math.hh>
+#include <gz/transport.hh>
+
+// #include <opencv2/opencv.hpp>
+#include "flow_opencv.hpp"
+#include <iostream>
+
+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;
+}

src/modules/simulation/gz_bridge/camera/gz_camera.hpp

@@ -0,0 +1,6 @@
+#pragma once
+
+#include <gz/msgs/image.pb.h>
+
+int calculate_flow(const gz::msgs::Image &image_msg, uint64_t sim_time, int &integration_time, float &flow_x,
+		   float &flow_y);