[gz] optical flow support

[dakejahl]

Opening this draft PR for review and testing

gz models branch
https://github.com/dakejahl/PX4-gazebo-models/tree/pr-rangefinder

[dirksavage88]

There is pretty much no drift on the textured world, but even on the lawn world it does drift a bit:

flowtestgz.zip

[dakejahl]

I didn't realize gazebo sitl already had flow so I am going to update this to use the OpticalFlow repo
https://github.com/PX4/PX4-SITL_gazebo-classic/blob/main/src/gazebo_opticalflow_plugin.cpp
https://github.com/PX4/PX4-OpticalFlow

[dakejahl]

@Jaeyoung-Lim I added the PX4/OpticalFlow in my latest commit. The performance is much worse, it just toilet bowls and wanders. Any ideas?

FYI for testing ensure EKF2_OF_CTRL is enabled and SIM_GPS_USED is set to 0 to ensure only flow is used for position hold.

[Jaeyoung-Lim]

@dakejahl I would check the camera orientation if it toilet bowls

[dakejahl]

@Jaeyoung-Lim The orientation looks correct. I think the issue is the noise, I was seeing this in my original implementation when poor quality keypoints are used in the flow average.

[dakejahl]

@Jaeyoung-Lim I added the PX4/OpticalFlow in my latest commit. The performance is much worse, it just toilet bowls and wanders. Any ideas?

The issue was quality was -1 and wrapping to 255. I added a fix and it is a lot better now.

[dirksavage88]

It's working. I think there was an issue with the libOpticalFlow in the cmakelist, it was working after I placed it further down in the camera folder's cmakelist, after add_library

[dakejahl]

I'm not sure why but sometimes when sim is launched PX4 reports

WARN  [health_and_arming_checks] Preflight Fail: ekf2 missing data
WARN  [health_and_arming_checks] Preflight Fail: No connection to the ground control station	
WARN  [health_and_arming_checks] Preflight Fail: ekf2 missing data

And it never recovers

[Jaeyoung-Lim]

I think we should add a sensor plugin for an optical flow sensor, not calculate optical flow inside the gzbridge. Then it is not a bridge anymore

[dakejahl]

hmm yeah I was thinking about that. I need to figure out how to create a gz plugin that is launched with sim since it doesn't exist yet for gz