search

mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
900 stars 994 forks source link

EKF measurement timeout when publishing pose to /mavros/vision_pose/pose #782

Closed Roman- closed 7 years ago

Roman- commented 7 years ago

Hello, I am using a Pixhawk with PX4 firmware and gazebo simulator, ekf2 as the state/pose estimator. I want to use aruco fiducial markers for vision pose estimation. My ROS node publishes estimated coordinates to /mavros/vision_pose/pose topic. Here's an example of the message:

header: 
  seq: 25595
  stamp: 
    secs: 633445690
    nsecs: 321158808
  frame_id: fcu
pose: 
  position: 
    x: -5.8058299648
    y: 0.165404601647
    z: 4.50232891986
  orientation: 
    x: 0.00249907606208
    y: 0.000708453209019
    z: 0.0138610026572
    w: -0.999900600617

For testing vision-based navigation in the gazebo simulator, I do the following:

Here is an example of two messages from these two topics obtained in about the same time:

header:  
  seq: 25595
  stamp: 
    secs: 633445690
    nsecs: 321158808
  frame_id: fcu
pose: 
  position: 
    x: -5.8058299648
    y: 0.165404601647
    z: 4.50232891986
  orientation: 
    x: 0.00249907606208
    y: 0.000708453209019
    z: 0.0138610026572
    w: -0.999900600617
header: 
  seq: 252486
  stamp: 
    secs: 633445690
    nsecs: 419158808
  frame_id: fcu
pose: 
  position: 
    x: -6.03800153732
    y: 0.445443034172
    z: 4.81723451614
  orientation: 
    x: 0.00267396630329
    y: 0.000531808934511
    z: 0.0139412081099
    w: -0.999899125477

I expect the copter to hold its position based on the pose estimated by vision. However, it stays still for about 3 seconds, then it shifts slightly, and then I see these messages:

WARN  [commander_tests] Failsafe enabled: no local position
INFO  [tone_alarm] fast_bat
WARN  [lib__ecl] EKF measurement timeout - stopping navigation

I have tried different publishing rates: 10hz, 20hz, and as the vision data come (~10-12 hz, the rate is not consistent but the estimated pose is always different). The result is always the same. px4 version: 1.6.5 mavros version: 0.19.0 Ros Kinetic, Ubuntu 16.04 LTS

Am I doing anything wrong?

TSC21 commented 7 years ago

I'm not aware of the current status and stability of vision support on efk2, but the problem can be on that side. Have you tried to use LPE instead and check the results?

Roman- commented 7 years ago

@TSC21 Thanks for the suggestion. I have tried the LPE, here's the output when I enable vision fusion by setting LPE_FUSION to 145:

WARN  [mavlink] [timesync] Hard setting offset.
INFO  [local_position_estimator] [lpe] vision position init: -1.26 -6.74 -3.89 m std  0.01  0.03  0.01 m
WARN  [local_position_estimator] [lpe] vision position timeout 
INFO  [local_position_estimator] [lpe] vision position init: -1.32 -6.78 -3.86 m std  0.03  0.00  0.01 m
WARN  [mavlink] [timesync] Hard setting offset.
WARN  [local_position_estimator] [lpe] vision position timeout 
WARN  [mavlink] [timesync] Hard setting offset.
INFO  [local_position_estimator] [lpe] vision position init: -1.37 -6.82 -3.90 m std  0.00  0.00  0.00 m
WARN  [local_position_estimator] [lpe] vision position timeout 
WARN  [mavlink] [timesync] Hard setting offset.
INFO  [local_position_estimator] [lpe] vision position init: -1.29 -6.75 -3.87 m std  0.05  0.02  0.02 m
WARN  [local_position_estimator] [lpe] vision position timeout 
WARN  [mavlink] [timesync] Hard setting offset.
INFO  [local_position_estimator] [lpe] vision position init: -1.27 -6.71 -3.89 m std  0.02  0.02  0.06 m
WARN  [local_position_estimator] [lpe] vision position timeout 
INFO  [local_position_estimator] [lpe] vision position init: -1.33 -6.76 -3.96 m std  0.04  0.03  -nan m
WARN  [local_position_estimator] [lpe] vision position timeout 
WARN  [mavlink] [timesync] Hard setting offset.
WARN  [mavlink] [timesync] Hard setting offset.
INFO  [local_position_estimator] [lpe] vision position init: -1.25 -6.73 -3.79 m std  0.02  0.01  0.02 m
WARN  [local_position_estimator] [lpe] vision position timeout 
WARN  [mavlink] [timesync] Hard setting offset.
INFO  [local_position_estimator] [lpe] vision position init: -1.30 -6.72 -3.88 m std  0.00  0.00  0.06 m
WARN  [mavlink] [timesync] Hard setting offset.
WARN  [local_position_estimator] [lpe] vision position timeout 
WARN  [mavlink] [timesync] Hard setting offset.
INFO  [local_position_estimator] [lpe] vision position init: -1.28 -6.71 -3.86 m std  0.02  0.02  0.01 m
WARN  [local_position_estimator] [lpe] vision position timeout

The experiment conditions remain the same - both /mavros/local_position/pose and /mavros/vision_pose/pose messages has very close position data, close timestamps and increasing sequence number in headers. The /mavros/vision_pose/pose messages rate is high enough (>30hz; although I have increased the rate artificially, so a few consecutive messages might have the same position values - not sure if that can cause this problem). The 'Hard setting offset' messages keep appearing with or without vision fusion - the problem might also be related with that. Any thoughts?

TSC21 commented 7 years ago

you are having constant timeouts. Is your computer handling the simulation well? Can you show the output of top command on your linux terminal?

TSC21 commented 7 years ago

For this, please try using the latest sitl_gazebo.

TSC21 commented 7 years ago

@Roman- updates?

AlexisTM commented 7 years ago

@Roman- When I was using vision, I had had time-out except for an update rate of 7Hz.

Roman- commented 7 years ago

Hey, sorry for the delay. I guess the problem is indeed caused by low machine performance (I used my laptop). I will need some more time for deploying and testing it on more powerful computer, for now I think you can close this issue.

TSC21 commented 7 years ago

@Roman- you can try to increase the vision timeout on LPE - https://github.com/PX4/Firmware/blob/9b5de235534859cbf43304f17c58fd965cefdde9/src/modules/local_position_estimator/sensors/vision.cpp#L15 - let's say to VISION_TIMEOUT = 1000000; // 1second. Let us know if you are able to solve this in the mean time.