How to resolve FCU lpe mocap timeout

[pchatrath]

I am using Optitrack Mocap system for getting position data. I run mavros and get position updates. However I get FCU: [lpe] mocap timeout xy timeout sonar timeout msgs due to which local position keeps on initializing again and again.

I tried streaming mocap data at 240FPS/300FPS i get same error. The frequency of /mavros/mocap/topic is around 350Hz. What am I missing?

[mhkabir]

what's the link you're using to the flight controller?

[pchatrath]

MavROS ttyUSB0@921600 with mavros_extra mocap_position_estimate

I do roslaunch mavros px4.launch fcu_url:=/dev/ttyUSB0:921600

[mhkabir]

Can you check if your timestamping is correct on the mocap data?

[pchatrath]

Does correct mean secs and nsecs are changing/increasing?

[mhkabir]

They need to be correctly time stamped with reference to the ROS clock. Mavros should handle the sync between the flight controller and ROS.

[pchatrath]

How do I verify if the timestamp are correct with reference to ROS clock?

And I connect my odroid to my computer by correspondingly setting ROS_MASTER_URL (in odroid). I do not synchronize their clock. Can this be the reason?

[pchatrath]

@mhkabir If I connect my laptop to pixhawk then everything works fine. If I connect my odroid to computer by correspondingly changing ROS_MASTER_URI then timestamp set screwed up /get mocap timeout. I installed chrony for time synchronization between pc and odroid. Mentioned below is /mavros/mocap/pose They look fine to me. What do you think

/mavros/mocap/pose

header: 
  seq: 197441
  stamp: 
    secs: 1469984950
    nsecs: 313846227
  frame_id: world
pose: 
  position: 
    x: -4.08432912827
    y: 3.941188097
    z: 0.978130221367
  orientation: 
    x: 0.0128829134628
    y: -0.00318198255263
    z: 0.537988424301
    w: 0.842847764492
---
header: 
  seq: 197442
  stamp: 
    secs: 1469984950
    nsecs: 316906183
  frame_id: world
pose: 
  position: 
    x: -4.08432912827
    y: 3.941188097
    z: 0.978130221367
  orientation: 
    x: 0.0128829134628
    y: -0.00318198255263
    z: 0.537988424301
    w: 0.842847764492
---
^Cheader: 
  seq: 197443
  stamp: 
    secs: 1469984950
    nsecs: 323517177
  frame_id: world
pose: 
  position: 
    x: -4.08432912827
    y: 3.941188097
    z: 0.978130221367
  orientation: 
    x: 0.0128829134628
    y: -0.00318198255263
    z: 0.537988424301
    w: 0.842847764492
---
header: 
  seq: 197444
  stamp: 
    secs: 1469984950
    nsecs: 329445312
  frame_id: world
pose: 
  position: 
    x: -4.08432912827
    y: 3.941188097
    z: 0.978130221367
  orientation: 
    x: 0.0128829134628
    y: -0.00318198255263
    z: 0.537988424301
    w: 0.842847764492
---
header: 
  seq: 197445
  stamp: 
    secs: 1469984950
    nsecs: 330793048
  frame_id: world
pose: 
  position: 
    x: -4.08432912827
    y: 3.941188097
    z: 0.978130221367
  orientation: 
    x: 0.0128829134628
    y: -0.00318198255263
    z: 0.537988424301
    w: 0.842847764492
---
header: 
  seq: 197446
  stamp: 
    secs: 1469984950
    nsecs: 338968971
  frame_id: world
pose: 
  position: 
    x: -4.08432912827
    y: 3.941188097
    z: 0.978130221367
  orientation: 
    x: 0.0128829134628
    y: -0.00318198255263
    z: 0.537988424301
    w: 0.842847764492
---
header: 
  seq: 197447
  stamp: 
    secs: 1469984950
    nsecs: 340281861
  frame_id: world
pose: 
  position: 
    x: -4.08432912827
    y: 3.941188097
    z: 0.978130221367
  orientation: 
    x: 0.0128829134628
    y: -0.00318198255263
    z: 0.537988424301
    w: 0.842847764492
---
header: 
  seq: 197448
  stamp: 
    secs: 1469984950
    nsecs: 343733489
  frame_id: world
pose: 
  position: 
    x: -4.08432912827
    y: 3.941188097
    z: 0.978130221367
  orientation: 
    x: 0.0128829134628
    y: -0.00318198255263
    z: 0.537988424301
    w: 0.842847764492
---
header: 
  seq: 197449
  stamp: 
    secs: 1469984950
    nsecs: 346705941
  frame_id: world
pose: 
  position: 
    x: -4.08432912827
    y: 3.941188097
    z: 0.978130221367
  orientation: 
    x: 0.0128829134628
    y: -0.00318198255263
    z: 0.537988424301
    w: 0.842847764492
---
header: 
  seq: 197450
  stamp: 
    secs: 1469984950
    nsecs: 355229360
  frame_id: world
pose: 
  position: 
    x: -4.08432912827
    y: 3.941188097
    z: 0.978130221367
  orientation: 
    x: 0.0128829134628
    y: -0.00318198255263
    z: 0.537988424301
    w: 0.842847764492
---
header: 
  seq: 197451
  stamp: 
    secs: 1469984950
    nsecs: 355402654
  frame_id: world
pose: 
  position: 
    x: -4.08432912827
    y: 3.941188097
    z: 0.978130221367
  orientation: 
    x: 0.0128829134628
    y: -0.00318198255263
    z: 0.537988424301
    w: 0.842847764492
---

roslaunch mavros px4.launch fcu_url:=/dev/ttyUSB0:921600 output

[ INFO] [1469985459.704864555]: FCU: [lpe] mocap timeout [ INFO] [1469985459.757863317]: FCU: [lpe] xy timeout [ INFO] [1469985460.332720083]: FCU: [lpe] mocap position init: 4.05, -4.20, -1.26 m [ INFO] [1469985460.383692702]: FCU: [lpe] xy resume [ INFO] [1469985461.535453669]: FCU: [lpe] mocap timeout [ INFO] [1469985461.587335236]: FCU: [lpe] xy timeout [ INFO] [1469985462.009353479]: FCU: [lpe] mocap position init: 4.02, -4.16, -1.08 m [ INFO] [1469985462.061188962]: FCU: [lpe] xy resume [ INFO] [1469985463.165079942]: FCU: [lpe] mocap timeout [ INFO] [1469985463.216946925]: FCU: [lpe] xy timeout [ INFO] [1469985463.843807007]: FCU: [lpe] mocap position init: 4.01, -4.14, -0.98 m [ INFO] [1469985463.895786322]: FCU: [lpe] xy resume [ INFO] [1469985465.355433811]: FCU: [lpe] mocap timeout [ INFO] [1469985465.409466603]: FCU: [lpe] xy timeout [ WARN] [1469985466.259289439]: GP: No GPS fix [ INFO] [1469985466.352184697]: FCU: [lpe] mocap position init: 4.01, -4.14, -0.98 m [ INFO] [1469985466.404160346]: FCU: [lpe] xy resume [ INFO] [1469985466.774025359]: FCU: [lpe] mocap timeout [ INFO] [1469985466.825133559]: FCU: [lpe] xy timeout [ INFO] [1469985467.296018029]: FCU: [lpe] mocap position init: 4.01, -4.14, -0.98 m [ INFO] [1469985467.348028635]: FCU: [lpe] xy resume

[pchatrath]

rostopic echo -n1 /diagnostics output from odroid

header: 
  seq: 1087
  stamp: 
    secs: 1469989787
    nsecs: 109486582
  frame_id: ''
status: 
  - 
    level: 0
    name: mavros: FCU connection
    message: connected
    hardware_id: /dev/ttyUSB0:921600
    values: 
      - 
        key: Received packets:
        value: 37618
      - 
        key: Dropped packets:
        value: 0
      - 
        key: Buffer overruns:
        value: 0
      - 
        key: Parse errors:
        value: 0
      - 
        key: Rx sequence number:
        value: 185
      - 
        key: Tx sequence number:
        value: 79
      - 
        key: Rx total bytes:
        value: 22674182
      - 
        key: Tx total bytes:
        value: 12855934
      - 
        key: Rx speed:
        value: 24092.000000
      - 
        key: Tx speed:
        value: 14006.000000
  - 
    level: 2
    name: mavros: GPS
    message: No satellites
    hardware_id: /dev/ttyUSB0:921600
    values: 
      - 
        key: Satellites visible
        value: 0
      - 
        key: Fix type
        value: 0
      - 
        key: EPH (m)
        value: 99.99
      - 
        key: EPV (m)
        value: 99.99
  - 
    level: 0
    name: mavros: Heartbeat
    message: Normal
    hardware_id: /dev/ttyUSB0:921600
    values: 
      - 
        key: Heartbeats since startup
        value: 1454
      - 
        key: Frequency (Hz)
        value: 1.037044
      - 
        key: Vehicle type
        value: Quadrotor
      - 
        key: Autopilot type
        value: PX4
      - 
        key: Mode
        value: ALTCTL
      - 
        key: System status
        value: Standby
  - 
    level: 0
    name: mavros: System
    message: Normal
    hardware_id: /dev/ttyUSB0:921600
    values: 
      - 
        key: Sensor present
        value: 0x00000040
      - 
        key: Sensor enabled
        value: 0x00000040
      - 
        key: Sensor helth
        value: 0x00000040
      - 
        key: Sensor Optical Flow
        value: Ok
      - 
        key: CPU Load (%)
        value: 67.9
      - 
        key: Drop rate (%)
        value: 0.0
      - 
        key: Errors comm
        value: 0
      - 
        key: Errors count #1
        value: 0
      - 
        key: Errors count #2
        value: 0
      - 
        key: Errors count #3
        value: 0
      - 
        key: Errors count #4
        value: 0
  - 
    level: 0
    name: mavros: Battery
    message: Normal
    hardware_id: /dev/ttyUSB0:921600
    values: 
      - 
        key: Voltage
        value: 11.33
      - 
        key: Current
        value: 0.2
      - 
        key: Remaining
        value: 58.0
  - 
    level: 0
    name: mavros: Time Sync
    message: Normal
    hardware_id: /dev/ttyUSB0:921600
    values: 
      - 
        key: Timesyncs since startup
        value: 14472
      - 
        key: Frequency (Hz)
        value: 10.000029
      - 
        key: Last dt (ms)
        value: 0.479080
      - 
        key: Mean dt (ms)
        value: 0.006260
      - 
        key: Last system time (s)
        value: 14255.060253000
      - 
        key: Time offset (s)
        value: 1469975531.952977896

[AlexisTM]

I have the same issue with MOCAP & LPE. The sending script is :

    while (run and not rospy.is_shutdown()):
        msg = PoseStamped()
        msg.header.stamp = rospy.Time.now()
        msg.header.seq = mocap_sent
        msg.pose.position.x = float(position_gps.x)
        msg.pose.position.y = float(position_gps.y)
        msg.pose.position.z = float(laser_altitude)
        msg.pose.orientation.x = imu_quaternion[0]
        msg.pose.orientation.y = imu_quaternion[1]
        msg.pose.orientation.z = imu_quaternion[2]
        msg.pose.orientation.w = imu_quaternion[3]
        mocap_publisher.publish(msg)
        mocap_sent += 1
        rate.sleep()

A fast solution is switching from /mavros/mocap/pose to /mavros/vision_pose/pose.

Don't forget to modify the parameter : "ATT_EXT_HDG_M" to 1 (for Vision heading)

[mhkabir]

Can everyone having issues with the timeout try again with master? We fixed an update rate issue inside LPE recently.

[pchatrath]

@mhkabir sorry don't have access to hardware currently

[mhkabir]

Closing this, I verified today that the code path is fine. Flew with VIO.

If you're still having issues, check your link bandwidth on both ends and the message rates.

[AlexisTM]

Same for me, it works

[andrejpan]

Is there a way that I could see how many mocap messages I receiving at Pixracer shell directly? I am using Firmware 1.4.4. Odroid on the drone through wifi receives 100Hz of /mavros/mocap/pose and gets around 7Hz from Pixracer (/mavros/local_position/pose). I am using UART protocol between Odroid and Pixracer.

[LorenzMeier]

You can use the listener command with any of these messages, e.g. listener vehicle_attitude.

[andrejpan]

I found that I can print more just one message at once with listener vehicle_attitude 1000. Output I can save with screen and then write python script which will grab all timestamps. Is there some other, perhaps easier way to export timestamps? Also, how exactly are defined time stamps?

[andrejpan]

I was checking mocap messages at Pixracer for 2 min and about 20 times I got situation like messages #596 and #597. Message #597 has lower value then #596. Could be this the reason why I getting timeouts? Are this timestamps on Pixracer synced with Mavros or not?

TOPIC: att_pos_mocap #594
timestamp: 736973939
id: 240
timestamp_received: 736999604
q:   0.6955  -0.0016  -0.0047   0.7185 
x:   0.2773
y:  -0.0059
z:  -0.1777

TOPIC: att_pos_mocap #595
timestamp: 736983282
id: 240
timestamp_received: 737014639
q:   0.6954  -0.0018  -0.0045   0.7186 
x:   0.2773
y:  -0.0059
z:  -0.1777

TOPIC: att_pos_mocap #596
timestamp: 736991631
id: 240
timestamp_received: 737024682
q:   0.6956  -0.0020  -0.0047   0.7184 
x:   0.2773
y:  -0.0059
z:  -0.1777

TOPIC: att_pos_mocap #597
timestamp: 736987706
id: 240
timestamp_received: 737052705
q:   0.6955  -0.0016  -0.0047   0.7185 
x:   0.2773
y:  -0.0059
z:  -0.1777

TOPIC: att_pos_mocap #598
timestamp: 737004312
id: 240
timestamp_received: 737063951
q:   0.6954  -0.0012  -0.0044   0.7186 
x:   0.2772
y:  -0.0059
z:  -0.1777

[blandry]

I have a laptop connected to a pixhawk. Laptop running mavros (master as of right now 10/20) and pixhawk (master as of right now 10/20). Using ROS_INFO I print the delta between timestamps mavros is getting from Vicon. They are in the range of 20000 (corresponding to a rate of 50Hz which is what I am sending - downsampling vicon a bit to give the serial some room). I have disabled a lot of the streams outgoing from the Pixhawk side. Now I still get "mocap timeout" issues and lagy position estimate that sometimes just run away (visualizing through rviz). I think there is still a problem with the LPE that hasn't been fixed. Possibly related to time syncing. I am making myself a cable to get NSH and print out the timestamp on the pixhawk side but would be nice to get pointers if anyone else is having this issue. Next step might be to rip out the LPE all together and using raw vicon measurements in the Frimware as position estimate (directly). FYI I am using the TF code path, not the POSE one.

[blandry]

@andrejpan did you have any luck with this? I get timeouts even if I swap the timestamps to current time in the mavlink receiver. I also printed out the delta time (from pixhawk clock) between messages and it seems alright.

[mhkabir]

@blandry The timeout is set to 0.2 seconds in LPE.

What rate are you sending the mocap poses at? Is the link bandwidth sufficient? Can you verify that the messages come through to PX4 at the same rate as you push them in at the mavros side? Do you see any timesync warnings in the mavros console?

[LorenzMeier]

@blandry Make sure to set serial to 921600 baud. What is your current baud rate?

[blandry]

I don't see any timesync warnings. I am also using a 921600 baud set between Telem2 and a laptop using this guy. I've tried different mocap rates but currently I settled on 50Hz. Due to the nature of the timesync algorithm I will sometimes get synced timestamps from vicon that will be greater than the current time in Pixhawk. Not sure how well this is handled by the LPE so I made the receiver discard them (reverted since it did not seem to fix anything). I will try to log using the actual logger instead of NSH and also make the mavlink receiver just echo the messages to truly vet the serial connection. Otherwise I'm left with uORB (queue sizes?) or LPE.

[mhkabir]

@blandry Can you check what the deltas are here : https://github.com/PX4/Firmware/blob/master/src/modules/local_position_estimator/sensors/mocap.cpp#L106

[blandry]

For completeness sake this was due to bad network (and an error on my part in the initial diagnostic that didn't take into account the data type of the seconds field). Thanks for your help PX4 team!

[AlexisTM]

@pchatrath @blandry For me, it was resolved by reducing data rate. For unknown reason, when sending too much packets, there will be drops and it causes a timeout. (USB 921600 baudrate) While a frequency of 30 Hz gives timeouts, 7 Hz do not.

@LorenzMeier Is it possible that the USB connexion is "slower" than the hardware serial? I mean : Could the USB add delay on some packets, enough for a timeout (0.2s)?

[negvet]

I had the similar issue: FCU: [lpe] vision position timeout

I am running vision algorithms on development computer and as a onboard computer I am using odroid with mavros. I solved it with clock update using ntp on both ubuntu machines.

[m1577]

I have the same problem with "timeout", i am using ttyUSB0 57600 whith mocap/pose. Could you help me?

[AlexisTM]

@m1577 Could you try things in the comments here?

• 7Hz publishing
• Use realtime tools for your publisher : http://wiki.ros.org/realtime_tools
• Use Higher baudrate (115200 or 921600, the second is the proposed for a companion computer)
• Finally, use a FTDI cable or a hardware serial from your companion computer

[vrjparikh]

Hey, We are using a VICON system + Pixhawk on our Drone. We have Configured the Firmware, and are able to get the mocap data on the local_position/pose topic. Now this data(pose) comes in realtime while using USB connection, but on connecting with a ESP8266 the refresh rate falls down to about 1 Hz, which spoils the entire system. Any ideas on how to boost this data speed. All the packets are being timestamped well on both sides, and VICON clock is in sync with the ROS clock!

[m1577]

@AlexisTM thanks for answering, I am not using a companion computer, I have installed mavros in a ubuntu pc and use the telemetry to communicate with my drone. I am a beginner in ros or mavros and I have some questions:

How do I know the timestamped is correct? How do I know if my optitrack clock is in sync with my ROS clock? How can I be publishing at 7Hz?

I will try using a higher baudrate.

Thanks!!!

[AlexisTM]

@vrjparikh The quick fix would be to increase the timeout of mocap or vision. If you are using LPE, those are vision.cpp and mocap.cpp

For the mocap topic, the default timeout is 0.2s and for the vision it is 0.5s.

@m1577

• To timestamp, use mavros and ros::Time
• Sync the clock, nothing said it is if the driver do not do it
• Use ros::Rate with a buffer

[m1577]

@AlexisTM I tried increase the timeout to 0.5, 0.7 0.9s of mocap but I am getting the same answer. Can you explain me a little more detailed, how can i get to 7 Hz?? I do not know how to use ros::rate and buffer.

Thanks for helping

[AlexisTM]

http://wiki.ros.org/roscpp/Overview/Time#Sleeping_and_Rates

[wwwefwehfrgjnrek]

@AlexisTM I have also experienced similar problems, the frequency of publication of this topic is too low. This is necessary to modify the underlying code?

[AlexisTM]

@m1577 you can use Rate:

ros::Rate rate(7);

while(ros::ok()){
    // do things
    rate.sleep();
}

[AlexisTM]

@wwwefwehfrgjnrek if your data rate is lower than 5Hz for mocap or 2Hz for vision, than you have to either find a way to have faster data or change the implementation to allow lower frequency.

[wwwefwehfrgjnrek]

@AlexisTM Thanks you reply. the frequency of publication of the topic local_position/pose is lower than 1HZ. Other data publication frequency is within the normal range.

[AlexisTM]

Oh, wait, local_position/local, not mocap nor vision?

Then try to use rosrun mavros mavsys rate to change the frequency of the mavlink message.

[AlexisTM]

See : http://wiki.ros.org/mavros#mavsys

[TSC21]

What's the baud rate of your companion link?

[wwwefwehfrgjnrek]

@AlexisTM I use the command to change the rate,and got this message,like that average rate: 9.368 min: 0.041s max: 0.173s std dev: 0.05399s window: 4 average rate: 5.088 min: 0.041s max: 0.488s std dev: 0.15372s window: 6 average rate: 3.533 min: 0.041s max: 0.672s std dev: 0.20998s window: 8 average rate: 2.712 min: 0.041s max: 0.672s std dev: 0.24498s window: 10 average rate: 3.228 min: 0.036s max: 0.672s std dev: 0.23442s window: 15 average rate: 3.495 min: 0.036s max: 0.672s std dev: 0.22413s window: 18 average rate: 3.294 min: 0.036s max: 0.842s std dev: 0.25247s window: 22 average rate: 3.616 min: 0.034s max: 0.842s std dev: 0.24126s window: 27 average rate: 3.299 min: 0.034s max: 0.996s std dev: 0.27296s window: 28 average rate: 3.354 min: 0.034s max: 0.996s std dev: 0.26716s window: 32 average rate: 3.322 min: 0.034s max: 0.996s std dev: 0.26689s window: 35 average rate: 3.381 min: 0.034s max: 0.996s std dev: 0.26344s window: 38 The frequency of publication of the topic local_position/poseis decreasing，it seem have some problem

[wwwefwehfrgjnrek]

@TSC21 /dev/ttyUSB0:921600

[TSC21]

Do you have the same problem in other topics?

[alvikag]

I am facing the same problem on an odroid xu4 for all the topics. Did anyone find a solution to this?