Integration of ArduPilot and VIO tracking camera (Part 4): non-ROS bridge to MAVLink in Python

Blog
, ,
81

OK
Here are the curves from Sept 19 fly (corresponding to the pictures and video showned on this date in this thread):

image
image1147×662 16.1 KB

Zooming deeper , we can see noise - I consider normal - but no glitches as you have
image

82

Hi @anbello, thank you for testing out the system!
Besides what @ppoirier has suggested, you can also check out the confidence level in each of your test, just to make sure the T265 worked well in the non-ROS tests and the issue was the script.

83

@LuckyBird and @ppoirier follows another test in circle and console out from the script (during circle confidence always HIGH):

Screenshot from 2020-03-08 18-02-29
Screenshot from 2020-03-08 18-02-291920×1080 306 KB 

ubuntu@anbelloros:~/catkin_ws/src/vision_to_mavros/scripts$ ./t265_to_mavlink.py 
INFO: Using default connection_string /dev/ttyAMA0
INFO: Using default connection_baudrate 921600
INFO: Using default vision_msg_hz 14
INFO: Using default confidence_msg_hz 1
INFO: Using camera position offset: Enabled, x y z is 0.11 -0.01 0.08
INFO: Using compass: Disabled
INFO: Using default scale factor 1.0
INFO: Using default camera orientation 0
INFO: Connecting to Realsense camera.
INFO: Realsense connected.
INFO: Connecting to vehicle.
INFO: Vehicle connected.
INFO: Sending VISION_POSITION_ESTIMATE messages to FCU.
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  Medium
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
WARNING:autopilot:Radio Failsafe - Disarming
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
CRITICAL:autopilot:PreArm: Radio failsafe on
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
INFO: Tracking confidence:  High
^CINFO: KeyboardInterrupt has been caught. Cleaning up...
INFO: Tracking confidence:  High
INFO: Realsense pipeline and vehicle object closed.
ubuntu@anbelloros:~/catkin_ws/src/vision_to_mavros/scripts$
84

That looks so much like a communication problem … it is puzzling that MavRos is ok :thinking:

Can you try reducing BaudRate to 115200 ?

85

Just tested with baudrate 115200, same problem :frowning:

86

From the log of my latest flight, the script seems to work fine.

@anbello :

  • Do you use the same output rate for non-ROS and ROS (vision_msg_hz in t265_to_mavlink.py script and param output_rate in t265_tf_to_mavros.launch file)?
  • Maybe you can have a handheld test for non-ROS with LOG_DISARMED enabled and see if there are any drops like in real flight?
87

@LuckyBird

I used same rate for non-ROS and ROS, tested 30, 14, 10 Hz and I always have problem only on non-ROS.

I will try handheld test for non-ROS with LOG_DISARMED later and let you know.

88

@LuckyBird follows the plot from handheld test for non-ROS, it seems worse when there is more yaw, anyway the confidence was always High.

Screenshot from 2020-03-09 18-26-46
Screenshot from 2020-03-09 18-26-461920×1080 354 KB

89

Hi @ppoirier

Referring to post 74 and 76: I tested the VIO non-ROS script today. As per your suggestions I decreased the vision_msg_hz to 20hz. I took off in altitude hold and switched to loiter and it worked beautifully. When I took off in the loiter mode, the drone wasn’t as stable as before and started drifting away. I was also getting DCM Roll/Pitch inconsistent by 85 deg and EKF2 Roll/Pitch inconsistent by 21 deg error. Could this be an camera orientation issue? Note: The camera is downfacing with around 30 deg tilt for better yaw estimation.

Thanks

1 Like
90

Hello,
Yes that is a strong probability as the transform matrix is expecting orthogonal coordinates, you could look at the logs and compare visual RPY with IMU

91

Hi @anbello, enabling the debug option can help us inspect the raw data and transformed data within the script:

  • Run with ./t265_to_mavlink.py --debug_enable.
  • By default only the current data will be printed out for readability. To see all previous data, comment out this line.
  • You can also save the output to a log file, for example: ./t265_to_mavlink.py --debug_enable | tee non_ROS_log.txt
1 Like
92

HI @LuckyBird here the output with --debug_enable
non_ROS_log.zip (376.6 KB)

93

Hi @anbello, the plots of the recorded data look ok.

image
image2773×1968 535 KB

You can compare them to the VISP plot of flight log. It seems to me the most plausible explanation is the communication.

94

Thanks for the anlysys @LuckyBird from the following graph is evident that, as you said, is a problem of communication. Top half of the graph: output from your script, bottom half: data from log. This is one of the test with less errors that I saw, normally there are more (as you can see form my other posts).

visp1
visp11600×1130 257 KB

But I don’t understand why I don’t have this problem with ROS, in the two situation I have the same communication channel: serial from RPi3 to Pixracer. The only difference I can think is that with mavros the serial com is implemented in C++ while with your script in python.

95

Looking at it , it seems that the glitches are happening the circle phase.
@anbello could you try with your externally generated python circle script ?

96

Here are some suggestions that I can think of for the software side of things:

  • You can monitor the CPU load when running the script to see if there are any sudden spikes for some reason.
  • The main differences in terms of functionality between the script and the ROS node are: the script fetches data continuously and it also sends the confidence message. You can try disabling the confidence message by comment out this line (although unlikely) or skipping a few frames after every successful read (add a counter before every new fetch, for example).
  • Another difference is the body offset option (although also unlikely), you can disable them and see if it helps.

Basically we can throw all of the variables that make the two implementations differ from one another against the wall and see which one sticks :))

97

There should be some strange interaction between main task and scheduled jobs, with this modified version that doesn’t use scheduler lib I have no error and also less CPU load from 80% to 60% (for all 4 cores).

For now I only tested handheld, I will do a real test flight ASAP.

Follows plot:

Screenshot from 2020-03-12 19-14-22
Screenshot from 2020-03-12 19-14-221920×1080 262 KB

2 Likes
98

@anbello Glad to know you got it working. On my setup with RPi3 the CPU load never got more than 40% so the issue did not occur. I suspect some messages got dropped by the scheduler whenever the CPU load is high.

If you are still interested in debugging the original script, I made a new version with debug message of elapsed time between consecutive messages and when a message is missed here in this branch.

99

Thanks @LuckyBird I will test your script with debug message but I would like to know why so big difference in CPU load, we are using same SBC (RPi3), which OS do you use?

A thought about what happen in the script:
I am not an expert in multi threading programming but I fear that there could be some problems in using H_aeroRef_aeroBody as global variable that is written in the scheduled job and then read in main task.
[Edit]
It is for this thought that I eliminate the scheduler to use an “old school” method using timer. Moreover in this way I have less CPU load, I am curious if this is true also for you, could you test the script without scheduler on your system and report CPU load?

100

Now thinking back at my unsuccessful integration on the RPI Zero, I hit the same issue and decided to move forward with the Bananapi Zero.

Thanks @anbello for testing and pushing the project and I invite @rmackay9 to take a look at this as he is presently working on implementing as well

1 Like