alexistm / aero-optical-flow-ros

Optical flow for the aero on ROS to publish the images as well

Aero optical flow with ROS

This package computes the Optical flow from the Intel aero downfacing camera and publish the full image to ROS (for other processing to be done).

It is meant to replace the original service https://github.com/intel-aero/aero-optical-flow to have a ROS interaction with the module.

The official module should be disabled: sudo systemctl aero-optical-flow disable; sudo systemctl aero-optical-flow stop

This project is still in development and did not go through cleaning yet. The node is firing many messages due to a dependency and is statically configured with defines.

Build Steps

mkdir -p ~/Workspace/src
cd ~/Workspace/src
git clone --recursive http://github.com/AlexisTM/aero-optical-flow-ros
# or clone and then use git submodule update --init --recursive
cd ~/Workspace
catkin build
# or catkin_make
source ~/Workspace/devel/setup.bash
rosrun aero_optical_flow aero-optical-flow _compute_vio:=true # Default, enable vio
# rosrun aero_optical_flow aero-optical-flow _compute_vio:=false

Result

The node will connect itself to the PX4 firmware using a TCP link at 127.0.0.1:5670 which is sent from mavlink-router service already provided on the aero.

The image will be cropped (128x128) and the Optical Flow will be computed and sent via the MavLink link.

The raw image is then cropped of 10 pixels (=> 630x470) on each side (because there is noise) then sent to ROS on the topic camera/image_raw

Todoes

• Manage communications via mavros instead of raw TCP (to allow UDP, serial, RTSP like Mavros)
• Improve exposure and gain PIDs
• Rosify the node => Params instead of #define
• Properly list dependencies

How to run it on Intel (R) Aero RTF Drone

• install Aero image v1.6 or newer (https://github.com/intel-aero/meta-intel-aero/wiki/02-Initial-Setup#flashing) using the official kernel (untested on others): 4.4.76-aero-1.2 (usb OTG drivers fails on newer (>4.10) drivers
• build and install the latest stable PX4 (make aerofc-v1_default upload)
• install a lidar, they recommend the LeddarOne(https://docs.px4.io/en/flight_controller/intel_aero.html) that can be connected to the telemetry port, I use the Terraranger
• in QGroundControl change this parameters:
  • EKF2_AID_MASK = 3 (use GPS + use optical flow)
  • EKF2_HGT_MODE = 2 (range sensor)

aero-optical-flow-ros's Issues

fatal error: mavlink.h: No such file or directory

Hi AlexisTM,
I cloned your repository and tried to build it, but errors occurred.
"fatal error: mavlink.h: No such file or directory".
I didn't find mavlink.h in aero-optical-ros/src/, could you tell me how to solve this?

Thanks in advance.

Exposure PID controller

We were noticing odd exposure behavior with our camera (rapid flickering between high and low exposure) so I decided to look at the source to see what was going on.

I think there might be some mistakes in this code. It seems that the gain starts at 127 and never changes, so it only ever enters the if condition (because of the if (gain > 1.0 || ...)), and never the else condition. Even more strangely, despite the fact that it never enters the else condition (which is the part that modifies the exposure), the exposure continues goes up until it overflows uint16_t. Also, you're assigning float literals to a uint16_t data types, which I also don't understand. Is this code correct?

aero-optical-flow-ros/src/mainloop.cpp

Lines 223 to 275 in 9b71d44

	/* PID-controller */
	float msv_error = EXPOSURE_MSV_TARGET - msv;
	float msv_error_d = msv_error - _msv_error_old;
	_msv_error_int += msv_error;
	float exposure = _camera->exposure_get();
	float gain = _camera->gain_get();
	exposure += (EXPOSURE_P_GAIN * msv_error) + (EXPOSURE_I_GAIN * _msv_error_int) + (EXPOSURE_D_GAIN * msv_error_d);
	// adjust the gain if exposure is saturated
	if (gain > 1.0f || (exposure > EXPOSURE_ABOSULUTE_MAX_VALUE-1 && _camera->exposure_get() > EXPOSURE_ABOSULUTE_MAX_VALUE-1)) {
	// calculate new gain value based on MSV
	gain += (GAIN_P_GAIN*msv_error) + (GAIN_I_GAIN*_msv_error_int) + (GAIN_D_GAIN*msv_error_d);
	if (gain > GAIN_ABOSULUTE_MAX_VALUE) {
	gain = GAIN_ABOSULUTE_MAX_VALUE;
	} else if (gain < 1.0f) {
	gain = 1.0f;
	}
	/* set new gain value if bigger than threshold */
	if (fabs(gain - _camera->gain_get()) > GAIN_CHANGE_THRESHOLD || (gain < 2.0f && _camera->gain_get() > 1.0f) ||
	(gain > GAIN_ABOSULUTE_MAX_VALUE-1 && _camera->gain_get() < GAIN_ABOSULUTE_MAX_VALUE)) {
	#if DEBUG_LEVEL
	DEBUG("gain set %u", (uint16_t)gain);
	#endif
	_camera->gain_set(gain);
	}
	} else { // adjust exposure
	if (exposure > EXPOSURE_ABOSULUTE_MAX_VALUE) {
	exposure = EXPOSURE_ABOSULUTE_MAX_VALUE;
	} else if (exposure < 1.0f) {
	exposure = 1.0f;
	}
	/* set new exposure value if bigger than threshold */
	if (fabs(exposure - _camera->exposure_get()) > EXPOSURE_CHANGE_THRESHOLD || (exposure < 2.0f && _camera->exposure_get() > 1.0f) ||
	(exposure > EXPOSURE_ABOSULUTE_MAX_VALUE-1 && _camera->exposure_get() < EXPOSURE_ABOSULUTE_MAX_VALUE)) {
	#if DEBUG_LEVEL
	DEBUG("exposure set %u", (uint16_t)exposure);
	#endif
	_camera->exposure_set(exposure);
	}
	}
	_msv_error_old = msv_error;
	/* update exposure at 5Hz */
	_next_exposure_update_timestap = timestamp_us + (USEC_PER_SEC / 5);

Exposure PID loop

aero-optical-flow-ros/src/mainloop.cpp

Line 223 in 9b71d44

/* PID-controller */