it builds and publishes, need to figure out build system now

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

@@ -11,10 +11,13 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_flow}
 
 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
+# 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/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

src/modules/simulation/gz_bridge/CMakeLists.txt

@@ -31,8 +31,6 @@
 #
 ############################################################################
 
-# add_subdirectory(camera)
-
 # Find the gz_Transport library
 # Look for GZ Ionic or Harmonic
 find_package(gz-transport NAMES gz-transport14 gz-transport13)
@@ -68,7 +66,6 @@ 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,8 +43,6 @@
 #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),
@@ -256,12 +254,12 @@ int GZBridge::init()
 	}
 
 	// Camera:
-	std::string camera_topic = "/camera";
+	std::string flow_topic = "/optical_flow";
 
-	// if (!_node.Subscribe(camera_topic, &GZBridge::cameraCallback, this)) {
-	// 	PX4_ERR("failed to subscribe to %s", camera_topic.c_str());
-	// 	return PX4_ERROR;
-	// }
+	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");
@@ -287,52 +285,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);
-// }
+// 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[])
 {

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -117,7 +117,9 @@ 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);
+
+	// TODO: change to sensor_msgs::msgs::OpticalFlow
+	void opticalFlowCallback(const gz::msgs::Image &image_msg);
 
 	/**
 	 * @brief Call Entityfactory service

src/modules/simulation/gz_plugin/CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.11.0 FATAL_ERROR)
 
 find_package(gz-cmake3 REQUIRED)
 
-project(OdometerSystem)
+project(OpticalFlowSystem)
 
 gz_find_package(gz-plugin2 REQUIRED COMPONENTS register)
 set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
@@ -13,15 +13,53 @@ set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
 find_package(gz-sensors8 REQUIRED)
 set(GZ_SENSORS_VER ${gz-sensors8_VERSION_MAJOR})
 
-add_subdirectory(sensors)
+find_package(OpenCV REQUIRED)
+
+# add_subdirectory(sensors optical_flow)
+
+PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS optical_flow.proto)
+
+add_library(optical_flow_plugin SHARED
+  ${PROTO_SRCS}
+  ${PROTO_HDRS}
+  OpticalFlow.cc
+)
+
+set(sensors
+  optical_flow_plugin
+)
+
+foreach(sensor ${sensors})
+  target_link_libraries(${sensor}
+    PUBLIC gz-sensors8::gz-sensors8
+    PUBLIC gz-rendering8::gz-rendering8
+    PUBLIC ${OpenCV_LIBS}
+    PUBLIC ${PROTOBUF_LIBRARIES}
+  )
+
+  target_include_directories(${sensor}
+    PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
+    PUBLIC ${OpenCV_INCLUDE_DIRS}
+  )
+
+endforeach()
+
+add_library(${PROJECT_NAME} SHARED
+  OpticalFlowSystem.cc
+)
+
+# add_dependencies(${PROJECT_NAME} optical_flow)
 
-add_library(${PROJECT_NAME} SHARED ${PROJECT_NAME}.cc)
 target_link_libraries(${PROJECT_NAME}
+  PUBLIC ${sensors}
   PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
   PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
   PRIVATE gz-sensors${GZ_SENSORS_VER}::gz-sensors${GZ_SENSORS_VER}
-  PRIVATE odometer
+  PRIVATE ${OpenCV_LIBS}
 )
 
 target_include_directories(${PROJECT_NAME}
-    PUBLIC ${PROJECT_SOURCE_DIR}/sensors)
+    PUBLIC ${PROJECT_SOURCE_DIR}
+    PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
+    PUBLIC ${OpenCV_INCLUDE_DIRS}
+)