search

mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
902 stars 993 forks source link

LPE version in 1.6.1 does not work #736

Closed SandW closed 7 years ago

SandW commented 7 years ago

We are working on visual hovering indoor using 6 optitrack cameras. However, it does not work with the following configuration.

Issue details

I can see the data stream in the mocap node and the data was received by the mavros correctly.

[ INFO] [1495755189.973277609]: FCU: [lpe] mocap position init: 0.11, 0.18, 0.20 m [ INFO] [1495755190.162792328]: FCU: [lpe] mocap timeout

However, the terminal kept sending timeout during the flight and posctrl was rejected. The drone doesnot fly correctly.

MAVROS version and platform, Autopilot type and version

Odroid side: 14.04+mavros+mocap_optitrack+wifi;

Flight controller side: px4 stack 1.6.1 lpe+921600@telem2+Lpe_fusion is set to use vision position and vision yaw and othersare unchecked + ATT_EXT_HDG_M is set to motion capture;

PC side: motive tracker is broadcasting the rigid body frames through wired ethernet connection.

References

  1. https://dev.px4.io/en/ros/external_position_estimation.html
  2. https://gitter.im/mavlink/mavros search @SandW, @mzahana and @cjc159_twitter
vooon commented 7 years ago

Is Header.stamp updates? And perhaps better to ask at http://discuss.px4.io/ as you have problem with firmware.

Also check /diagnostics.

SandW commented 7 years ago

Thanks @vooon ! I cannot post topic to the discussion forum as a newbie. Where can I find Header.stamp?

vooon commented 7 years ago

It is ROS standard message, normally called header.

TSC21 commented 7 years ago

@SandW can you please paste the copy of rostopic echo -n1 /diagnostics output please?

SandW commented 7 years ago

px4.launch:

[ INFO] [1498106814.436805653]: FCU URL: /dev/ttyUSB0:921600 [ INFO] [1498106814.437457066]: serial0: device: /dev/ttyUSB0 @ 921600 bps [ INFO] [1498106814.439729511]: GCS bridge disabled [ INFO] [1498106814.615226658]: Plugin 3dr_radio loaded and initialized [ INFO] [1498106814.628204749]: Plugin actuator_control loaded and initialized [ INFO] [1498106814.633419885]: Plugin altitude loaded and initialized [ INFO] [1498106815.229380130]: Plugin cam_imu_sync loaded and initialized [ INFO] [1498106815.252056623]: Plugin command loaded and initialized [ INFO] [1498106815.252297247]: Plugin distance_sensor blacklisted [ INFO] [1498106815.284015854]: Plugin ftp loaded and initialized [ INFO] [1498106815.319096025]: Plugin global_position loaded and initialized [ INFO] [1498106815.322166341]: Plugin hil_controls loaded and initialized [ INFO] [1498106815.322376631]: Plugin image_pub blacklisted [ INFO] [1498106815.355205024]: Plugin imu_pub loaded and initialized [ INFO] [1498106815.373844832]: IMU: High resolution IMU detected! [ INFO] [1498106815.374523745]: IMU: Attitude quaternion IMU detected! [ INFO] [1498106815.383502693]: Plugin local_position loaded and initialized [ INFO] [1498106815.386566591]: Plugin manual_control loaded and initialized [ INFO] [1498106815.407237971]: Plugin mocap_pose_estimate loaded and initialized [ INFO] [1498106815.418638572]: Plugin param loaded and initialized [ INFO] [1498106815.442804848]: Plugin px4flow loaded and initialized [ INFO] [1498106815.460668744]: Plugin rc_io loaded and initialized [ INFO] [1498106815.461000492]: Plugin safety_area blacklisted [ INFO] [1498106815.477814603]: Plugin setpoint_accel loaded and initialized [ INFO] [1498106815.534282275]: Plugin setpoint_attitude loaded and initialized [ INFO] [1498106815.550377139]: Listen to position setpoint transform local_origin -> setpoint [ INFO] [1498106815.550980177]: Plugin setpoint_position loaded and initialized [ INFO] [1498106815.596244706]: Plugin setpoint_raw loaded and initialized [ INFO] [1498106815.609151381]: Plugin setpoint_velocity loaded and initialized [ INFO] [1498106815.636874887]: Plugin sys_status loaded and initialized [ INFO] [1498106815.651956382]: Plugin sys_time loaded and initialized [ INFO] [1498106815.657729099]: Plugin vfr_hud loaded and initialized [ INFO] [1498106815.658007889]: Plugin vibration blacklisted [ INFO] [1498106815.698114364]: Plugin vision_pose_estimate loaded and initialized [ INFO] [1498106815.714650435]: Plugin vision_speed_estimate loaded and initialized [ INFO] [1498106815.732623913]: Plugin waypoint loaded and initialized [ INFO] [1498106815.732926912]: Autostarting mavlink via USB on PX4 [ INFO] [1498106815.733108411]: Built-in SIMD instructions: None [ INFO] [1498106815.733229702]: Built-in MAVLink package version: 2016.5.20 [ INFO] [1498106815.733455992]: Built-in MAVLink dialect: ardupilotmega [ INFO] [1498106815.733635283]: MAVROS started. MY ID 1.240, TARGET ID 1.1 [ WARN] [1498106815.755849195]: TM: Clock skew detected (-1498106255.784842014 s). Hard syncing clocks. [ INFO] [1498108772.709479960]: VER: 1.1: Capabilities 0x00000000000024ef [ INFO] [1498108772.709706584]: VER: 1.1: Flight software: 010601ff (b4b8a03d10429c85) [ INFO] [1498108772.709859416]: VER: 1.1: Middleware software: 010601ff (b4b8a03d10429c85) [ INFO] [1498108772.710033082]: VER: 1.1: OS software: 071000ff (0000000000000000) [ INFO] [1498108772.710150498]: VER: 1.1: Board hardware: 00000011 [ INFO] [1498108772.710271331]: VER: 1.1: VID/PID: 26ac:0011 [ INFO] [1498108772.710388122]: VER: 1.1: UID: 3034511138353334 [ WARN] [1498108772.710617329]: CMD: Unexpected command 520, result 0 [ INFO] [1498109226.406630331]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ INFO] [1498109226.456784873]: FCU: [lpe] reinit P (1, 1) negative [ INFO] [1498109228.209286646]: FCU: [lpe] mocap timeout [ INFO] [1498109230.611550392]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ INFO] [1498109234.357245497]: FCU: [lpe] mocap timeout [ INFO] [1498109237.757345694]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ INFO] [1498109238.256972082]: FCU: [lpe] mocap timeout [ INFO] [1498109239.705721537]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ INFO] [1498109241.905954874]: FCU: [lpe] mocap timeout [ INFO] [1498109243.554815999]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ INFO] [1498109245.107062436]: FCU: [lpe] mocap timeout [ INFO] [1498109247.058304637]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ INFO] [1498109248.858044677]: FCU: [lpe] mocap timeout [ INFO] [1498109251.455771121]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ INFO] [1498109252.254385938]: FCU: [lpe] mocap timeout [ INFO] [1498109255.356879739]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ INFO] [1498109255.556873452]: FCU: [lpe] mocap timeout [ INFO] [1498109257.605863335]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ INFO] [1498109258.906855815]: FCU: [lpe] mocap timeout [ INFO] [1498109260.655982191]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ INFO] [1498109261.354848921]: FCU: [lpe] mocap timeout [ INFO] [1498109262.906703318]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ INFO] [1498109263.203952924]: FCU: [lpe] mocap timeout [ERROR] [1498109263.753833270]: FCU: REJECT AUTO HOLD [ INFO] [1498109263.805857884]: RC_CHANNELS message detected! [ INFO] [1498109264.354821694]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ INFO] [1498109266.503556539]: FCU: [lpe] mocap timeout [ INFO] [1498109267.605436468]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ INFO] [1498109267.805678179]: FCU: [lpe] mocap timeout [ INFO] [1498109268.505314363]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ERROR] [1498109268.707692103]: FCU: REJECT AUTO HOLD [ERROR] [1498109270.055424520]: FCU: REJECT AUTO HOLD [ERROR] [1498109270.755174703]: FCU: REJECT AUTO HOLD [ERROR] [1498109271.652786945]: FCU: REJECT AUTO HOLD [ INFO] [1498109272.503649041]: FCU: [lpe] mocap timeout [ERROR] [1498109272.905909370]: FCU: REJECT AUTO HOLD [ERROR] [1498109273.703396402]: FCU: REJECT AUTO HOLD [ INFO] [1498109274.255531776]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ INFO] [1498109276.705769660]: FCU: [lpe] mocap timeout [ERROR] [1498109277.307022707]: FCU: REJECT AUTO HOLD [ INFO] [1498109277.354876638]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ERROR] [1498109278.305015115]: FCU: REJECT AUTO HOLD [ INFO] [1498109280.504610081]: FCU: [lpe] mocap timeout [ INFO] [1498109282.454493457]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ INFO] [1498109283.907476804]: FCU: [lpe] mocap timeout [ERROR] [1498109284.356329279]: FCU: REJECT AUTO HOLD [ INFO] [1498109284.556079784]: FCU: [lpe] mocap position init: -0.21, 0.11, -0.12 m [ERROR] [1498109285.154713222]: FCU: REJECT AUTO HOLD

rostopic echo -n1 /diagnostics ：

header: seq: 569 stamp: secs: 1498109516 nsecs: 222290303 frame_id: '' status:

rosgraph

screenshot from 2017-06-22 13 31 02

TSC21 commented 7 years ago

You are getting consecutive timeouts on the mocap streaming. At frequency are you publishing the mocap data? The minimum frequency on the LPE side is 5hz.

SandW commented 7 years ago

How can I observe the mocap frequency?

TSC21 commented 7 years ago

rostopic hz /mavros/mocap/pose or rosrun tf tf_monitor <mocap_parent_frame> <mocap_child_frame>, depending on what you are currently subscribing? Are you using tf or pose msg?

TSC21 commented 7 years ago

@SandW updates?

SandW commented 7 years ago

Going to update soon.

SandW commented 7 years ago

rostopic hz /mavros/mocap/pose

subscribed to [/mavros/mocap/pose] average rate: 125.414 min: 0.000s max: 0.091s std dev: 0.02305s window: 106 average rate: 118.973 min: 0.000s max: 0.094s std dev: 0.02347s window: 198 average rate: 89.548 min: 0.000s max: 0.596s std dev: 0.04730s window: 203 average rate: 61.912 min: 0.000s max: 1.325s std dev: 0.09536s window: 242 average rate: 73.699 min: 0.000s max: 1.325s std dev: 0.07955s window: 357 average rate: 81.603 min: 0.000s max: 1.325s std dev: 0.06861s window: 482 average rate: 86.358 min: 0.000s max: 1.325s std dev: 0.06221s window: 596 average rate: 88.721 min: 0.000s max: 1.325s std dev: 0.05811s window: 691 average rate: 82.445 min: 0.000s max: 1.325s std dev: 0.05788s window: 735 average rate: 80.307 min: 0.000s max: 1.325s std dev: 0.05765s window: 759 average rate: 110.930 min: 0.000s max: 0.124s std dev: 0.02129s window: 6337 average rate: 111.096 min: 0.000s max: 0.124s std dev: 0.02113s window: 6467 average rate: 110.913 min: 0.000s max: 0.124s std dev: 0.02108s window: 6563 average rate: 110.871 min: 0.000s max: 0.124s std dev: 0.02110s window: 6674 average rate: 110.941 min: 0.000s max: 0.124s std dev: 0.02104s window: 6790 average rate: 110.969 min: 0.000s max: 0.124s std dev: 0.02109s window: 6907 average rate: 110.956 min: 0.000s max: 0.124s std dev: 0.02115s window: 7018 average rate: 110.925 min: 0.000s max: 0.124s std dev: 0.02116s window: 7127 average rate: 110.638 min: 0.000s max: 0.124s std dev: 0.02117s window: 7215 average rate: 110.437 min: 0.000s max: 0.124s std dev: 0.02123s window: 7315 average rate: 110.324 min: 0.000s max: 0.124s std dev: 0.02128s window: 7420 average rate: 110.095 min: 0.000s max: 0.124s std dev: 0.02135s window: 7517 average rate: 110.055 min: 0.000s max: 0.124s std dev: 0.02139s window: 7628 average rate: 109.947 min: 0.000s max: 0.124s std dev: 0.02143s window: 7732 average rate: 109.813 min: 0.000s max: 0.124s std dev: 0.02143s window: 7828 average rate: 109.329 min: 0.000s max: 0.124s std dev: 0.02151s window: 7902 average rate: 108.966 min: 0.000s max: 0.124s std dev: 0.02160s window: 7986 average rate: 108.623 min: 0.000s max: 0.124s std dev: 0.02167s window: 8072 average rate: 108.469 min: 0.000s max: 0.124s std dev: 0.02174s window: 8172 average rate: 108.200 min: 0.000s max: 0.124s std dev: 0.02182s window: 8262 average rate: 108.161 min: 0.000s max: 0.124s std dev: 0.02176s window: 8371 average rate: 108.040 min: 0.000s max: 0.124s std dev: 0.02178s window: 8471 average rate: 108.120 min: 0.000s max: 0.124s std dev: 0.02180s window: 8586 average rate: 108.110 min: 0.000s max: 0.124s std dev: 0.02184s window: 8687 average rate: 108.191 min: 0.000s max: 0.124s std dev: 0.02188s window: 8804 average rate: 108.010 min: 0.000s max: 0.124s std dev: 0.02186s window: 8900 average rate: 108.140 min: 0.000s max: 0.124s std dev: 0.02189s window: 9021 average rate: 108.139 min: 0.000s max: 0.124s std dev: 0.02194s window: 9132 average rate: 108.178 min: 0.000s max: 0.124s std dev: 0.02190s window: 9246 average rate: 108.262 min: 0.000s max: 0.124s std dev: 0.02194s window: 9364 average rate: 108.264 min: 0.000s max: 0.124s std dev: 0.02198s window: 9474 average rate: 108.327 min: 0.000s max: 0.124s std dev: 0.02201s window: 9580 average rate: 108.237 min: 0.000s max: 0.124s std dev: 0.02206s window: 9683 average rate: 108.310 min: 0.000s max: 0.124s std dev: 0.02209s window: 9800 average rate: 108.406 min: 0.000s max: 0.124s std dev: 0.02213s window: 9920 average rate: 108.482 min: 0.000s max: 0.124s std dev: 0.02216s window: 10038 average rate: 108.423 min: 0.000s max: 0.124s std dev: 0.02220s window: 10144 average rate: 108.249 min: 0.000s max: 0.124s std dev: 0.02226s window: 10238 average rate: 108.271 min: 0.000s max: 0.124s std dev: 0.02229s window: 10351 average rate: 108.337 min: 0.000s max: 0.124s std dev: 0.02232s window: 10468 average rate: 108.300 min: 0.000s max: 0.124s std dev: 0.02235s window: 10574 average rate: 108.379 min: 0.000s max: 0.124s std dev: 0.02237s window: 10683 average rate: 108.442 min: 0.000s max: 0.124s std dev: 0.02240s window: 10800 average rate: 108.506 min: 0.000s max: 0.124s std dev: 0.02243s window: 10917 average rate: 108.608 min: 0.000s max: 0.124s std dev: 0.02245s window: 11039 average rate: 108.550 min: 0.000s max: 0.124s std dev: 0.02247s window: 11144 average rate: 108.559 min: 0.000s max: 0.124s std dev: 0.02250s window: 11256 average rate: 108.436 min: 0.000s max: 0.124s std dev: 0.02254s window: 11354 average rate: 108.519 min: 0.000s max: 0.124s std dev: 0.02256s window: 11474

TSC21 commented 7 years ago

Seems to me that the problem may be between the fcu and where you are running mavros. Does your wifi link is stable? Have you tried to increase the timeout time?

SandW commented 7 years ago

Where can I increase the timeout time?

TSC21 commented 7 years ago

https://github.com/PX4/Firmware/blob/master/src/modules/local_position_estimator/sensors/mocap.cpp#L10

You can try another thing - throttle the msgs to a lower rate (say 10hz), using http://wiki.ros.org/roscpp/Overview/Time#Sleeping_and_Rates.

Also check the wifi link bandwidth.

TSC21 commented 7 years ago

@SandW updates?

TSC21 commented 7 years ago

Closing now. If the problem persists let us know.