[wip] gz optical flow

ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow

@@ -0,0 +1,18 @@
+#!/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 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

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/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

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -43,6 +43,8 @@
 #include <iostream>
 #include <string>
 
+#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";

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>
@@ -116,6 +117,7 @@ class GZBridge : public ModuleBase<GZBridge>, 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<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_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);