Vision estimate

[LorenzMeier]

This PR adds support for vision position estimates coming in via MAVLink. They are published and the INAV estimator subscribes and reads them.

NOTE: Set the parameter CBRK_NO_VISION to 0 to disable the circuit breaker for vision estimates (right now we default to vision input OFF).

None of this is tested, in particular the INAV changes might need a few printfs and testing until the runtime results make sense.

[ggregory8]

As I mentioned on px4users mailing list when I view the local_position_ned.x in QGC the value drifts exponentially when I send it a constant vision_position_estimate.

I think I found the problem that it is running the inertial_filter_predict() but not the inertial_filter_correct() due to the if (use_gps_xy) condition.
I changed line 784 to
bool use_gps_xy = (ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W) || vision_valid;
and it seems to be working now.

What is the best way to debug the PX4 firmware. Can we printf to the NSH console? I found if I stopped an app in NSH and then started it again without backgrounding it I can get warnx messages. Is this the best way?

[LorenzMeier]

@DrTon This is something for you 8)

[LorenzMeier]

@ggregory8 The warnx() message go to the current console, which is by default on SERIAL4/5 (pin 4 (TX), pin 5 (RX), pin 6 (GND)). See the PX4 wiki (search for wiring) for details. If you stop the app via USB console and restart it, it will send its output to the USB console. That's a viable option if you don't mind the restart. We mostly use an FTDI on the serial console to also be able to watch the boot log.

[LorenzMeier]

@ggregory8 Can you send a pull request for your change?

https://help.github.com/articles/using-pull-requests

[ggregory8]

@LorenzMeier I can't use Serial5 because the pins 4&5 are broken, I was hoping to switch the default console to Serial4 until I get a new connector. I guess the stop/start option will do until then, also the mavlink_log_info() function, while not ideal, works for debugging :)

I will do the PR but unfortunately I have no time at the moment. I won't be able to get back to this for a few days. If it is still outstanding by then I will do it.

Thanks

[ggregory8]

I have finally made my first pull request:
#1065

[LorenzMeier]

@DrTon Could you please review the INAV changes? Its ok if they are hacky on the vision side, but I want to be sure we don't ruin the GPS-only performance with these changes.

[DrTon]

This is very hacky and bad way. Why not to add corr_vision and use is separately? You also will be able to set weights different from GPS and will not touch GPS-related code, so we will be sure that it works.

[LorenzMeier]

Could you maybe push a patch to resolve this? Not knowing the INAV internals that's kind of hard to do for me.

[LorenzMeier]

@DrTon Could you please provide some additional guidance / feedback / patches here? I would like to move forward with the integration and a lot of people would be unblocked by this.

[DrTon]

@LorenzMeier, I think first need to fix issues that I found: add corr_vision and use is separately instead of using corr_gps. Unfortunately, I have no time for this now, maybe later. @ggregory8, can you do this?

[LorenzMeier]

@mhkabir98 Is this something you could pick up? The goal would be to accept vision estimates as soon as possible in the master branch so you can code ahead in ROS. I'm aware that you plan to to all estimation within ROS, however, doing the estimation in the autopilot does have the benefit of a) continue to work (given GPS) if the Linux computer (during testing) fails, b) it will immediately work and give you faster results,

and most importantly c) has high-rate IMU feedback which will make it much more robust and less laggy (as there is no communication latency of the sensors involved).

[BEmran]

Hi,
I wrote a plugin which sends a message to the VISION_POSITION_ESTIMATE using this functions:

mavlink_msg_vision_position_estimate_pack(0, 0, &msg,usec,x,y,z,0,0,0);
uas->mav_link->send_message(&msg);

what I noticed is the follwing, the LOCAL_POSITION_NED follows the x and y data I sent exactly. However, the z value of the LOCAL_POSITION_NED doesn't change or follow what ever I sent. It only changes when the IMU data change (the pix4/Quadrotor moves).

[ggregory8]

@LorenzMeier I can look into doing this if it is still outstanding in a weeks time. I have been extremely busy so haven't had time to spend on this like I wanted to and I have a few weeks off work so will have heaps of time :)

[ggregory8]

@TSC21 @mhkabir
What issues are you facing exactly with using the vision_estimate branch with mavros so I can have a look at these?
I know of:

• Separate vision corrections from GPS corrections (doesn't actually create a problem for our vision only testing)
• When vision estimate stops the Z estimate in PX4 increments to infinity
• When vision estimate is regained, it sometimes does not bring the location position back to vision estimate (potentially due to the local estimates being so far off)

[TSC21]

@ggregory8 for now I confirm only those and another: position lock doesn't go off if the vision estimation is not received. Also, we will probably have to tweak better the weights of the sensors in the estimator so to have a better pose estimation. When I receive my px4flow cable I'll be doing also some POSCTL with it + the vision estimation.

[mhkabir]

@ggregory8 Loss of vision estimate is not detected after merging the last master. Previously, a vision timeout message was sent, but now it doesn't detect loss of estimate. It continues to think that there is a valid position even if mavlink messages are cut off after sending only initially.

[ggregory8]

Thanks guys. I have updated to the lastest vision_estimate branch.

@mhkabir the timeout is still working for me. If I block the camera I have to wait for mavros to stop sending messages (normally about 5s, we will need to change this in the vision_position plugin) then I get a timeout message in PX4 (500ms after the last recieved vision estimate). So there is quite a delay all up.

Is mavros definitely not sending a vision estimate? I added a printf here to confirm.

[TSC21]

I think this is a matter of understanding the weights of each sensor in the inav estimator and getting the right values to them. Since GPS/Flow fusion was already working, I think Vision/Flow should also work. If @DrTon could give us a flowchart of the algorithm, it would be helpful also.

@vooon: your participation can be considered almost "demanding" :P since you are responsible for the mavros bridge.

[LorenzMeier]

@TSC21 I tried your VM, but there are multiple issues with it, and I would appreciate if you could fix them for the next revision:

• Did you export it properly? Because my VirtualBox version refuses to import it. If you did export it correctly, you should obtain an .ova file (see the File -> Export Appliance menu)
• Please ZIP it, do not use RAR. Its not really more efficient and in contrast to ZIP none of the systems supports it by default. RAR is not an useful exchange format.
• Please try the import process (which would partially have uncovered these issues) and provide instructions how to 1) import 2) run 3) within the VM (please provide the login and password there as well): How to run a set of ROS packages to talk to the autopilot

That would be a baseline to get anyone started. On top of that instructions how to upgrade the various software packages would be great, because that needs to be done regularly.

Thank you for this effort!

[LorenzMeier]

I forgot to add the link - I would appreciate if you could put instructions here:
http://pixhawk.org/dev/ros/desktop

[TSC21]

@LorenzMeier,

1. The VM is for VMWare and not for VirtualBox, as I told you when I sent it to you.
2. Can do that, but it will take much time to load it again to the Cloud; but no problem at all.
3. What do you mean with import process? 1)/2)/3)All the doc is in a readme file on the desktop ;)

The mavros should be updated also. I have to clean the VM again if it has to be sent on .zip, since I'm been using it to develop my project. Readme on the VM must be redone also probably.

[LorenzMeier]

Thanks - I must have missed the comment 8).

1. It runs fine in VMWare 8)
2. I already re-packaged as ZIP (see page)
3. Ok - we only need the instructions how to update it then.

[LorenzMeier]

What is the password for VisionEstimatePX4?

[TSC21]

user: vision
password: px4

[TSC21]

Regarding the update: I was checking the Readme again and, as is, it should be ok. Right know I have some other working pose algorithms. I will probably send them compressed to you also to test in your side.
The only thing that needs update now is mavros. Just clone master (https://github.com/vooon/mavros).

To test the pose estimation pkgs, /mavros/src/plugins/vision_estimate.cpp must be edited, depending what kind of message you are subscribing (can be a /tf or a pose). If doesn't work with /vision subscribed, add prefix / (/vision).

Also, to test the px4flow and gcs_image_bridge plugins, clone mavros_extras (https://github.com/vooon/mavros_extras).

[LorenzMeier]

@TSC21 Sorry that I missed this earlier! The readme is GREAT! I've copied it on this page:
http://pixhawk.org/dev/ros/desktop

Could you maybe extend it with the update instructions, rather than just having them here? Please shape them in a way that we don't have to keep updating the image, but people can just run the appropriate commands to update.

[TSC21]

OK. I will register in the http://pixhawk.org/dev so I can directly edit them.

[TSC21]

Already made some mods in the tutorials. Going to have some more info soon.

[LorenzMeier]

The improvement is already visible. Please note that its not clear from the context in which directory to run commands like git clone xxx and that you might want to explain that git clone is a one-time command and updates can be pulled with git pull origin master.

[vooon]

@TSC21 instead if manual git cloning use wstool (it really save time).

[TSC21]

Did some cleaning now ;)

[TSC21]

@LorenzMeier can you please change the file name of the VM from px4_ros_virtualbox.zip to px4_ros_vmware.zip? Just for convenience 8)

[TSC21]

@LorenzMeier, @ggregory8, @mhkabir,
Is the call going to happen? If yes, at what time, since that appears that it has to happen tomorrow? The options I see are only two: 10:00AM and 4:00PM @ GTC.

And, will it by Skype, Hangout, any other?

Set your thoughts :)

[LorenzMeier]

Yes, final date here!
http://doodle.com/iinyqpeicq4ina9s

Sorry for the latency.

[TSC21]

@LorenzMeier Where's the call going to happen? On Hangouts?
Please add me to the call via < nuno DOT msm DOT 1 AT gmail DOT com > (you already sent me the invitation to my hotmail mail).
Thanks!

[LorenzMeier]

You have been added (see your inbox). Its a G+ hangout.

[TSC21]

Yes it was to my Hotmail inbox. Please send it to my Gmail: < nuno DOT msm DOT 1 AT gmail DOT com >

[vooon]

Please add me too, but i answer in chat.

2014-08-11 12:48 GMT+04:00 TSC21 notifications@github.com:

Yes it was to my Hotmail inbox. Please send it to my Gmail: < nuno DOT msm
DOT 1 AT gmail DOT com >

—
Reply to this email directly or view it on GitHub
#1041 (comment).

[TSC21]

Update: http://pixhawk.org/dev/ros/desktop now has new MAVLINK msg API to setpoints, since mavlink/mavros#97 is done and closed.

[LorenzMeier]

@mhkabir I can't find a reason why the vision estimate should deteriorate flow. I'm merging this now. If you are experiencing problems, please ensure that neither your tracker (e.g. SVO or the multi sensor fusion) nor the ROS bridge send any vision_estimate updates after they have lost track. The symptoms you're describing could be partially explained if they would keep repeating old data.