search

PX4 / PX4-Autopilot

PX4 Autopilot Software
https://px4.io
BSD 3-Clause "New" or "Revised" License
8.38k stars 13.46k forks source link

mocap_pose_estimate does not work on px4 firmware version 1.10.1 with LPE running mavros #16928

Open ashirsat opened 3 years ago

ashirsat commented 3 years ago

Describe the bug Unable to get mocap_pose_estimate plugin to work with optitrack mocap message for position hold.

To Reproduce Steps to reproduce the behavior:

  1. I am following the guide for Using Vision or Motion Capture Systems for Position Estimation for enabling position set points with optitrack
  2. First I launch mavros using the following command roslaunch mavros px4.launch fcu_url:=///dev/ttyAMA0:921600 gcs_url:=tcp-l://[gcs-ip]:port_id
  3. Then on a seperate terminal i launch the vrpn_clinet node using the command roslaunch vrpn_client_ros sample.launch server:=<server-ip>
  4. Next on a seperate terminal I use topic_tools to relay /vrpn_client_ros/robot_id/pose to /mavros/mocap/pose topic
  5. However I do not get message saying '[lpe] mocap position init ....' and neither anything else happens after this

Expected behavior after realying the optitrack pose message to mocap/pose topic the LPE should do 'mocap position init to ....'

**Checking log directory for disk usage. This may take a while. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://ubuntu:39903/

SUMMARY

CLEAR PARAMETERS

PARAMETERS

NODES / mavros (mavros/mavros_node)

auto-starting new master process[master]: started with pid [2910] ROS_MASTER_URI=http://localhost:11311

setting /run_id to fa78062e-758c-11eb-a216-e84e0673eb2a process[rosout-1]: started with pid [2921] started core service [/rosout] process[mavros-2]: started with pid [2927] [ INFO] [1614053398.799276436]: FCU URL: ///dev/ttyAMA0:921600 [ INFO] [1614053398.812181279]: serial0: device: ///dev/ttyAMA0 @ 921600 bps [ INFO] [1614053398.813823779]: GCS URL: tcp-l://gcs-ip:5760 [ INFO] [1614053398.814552165]: tcp1: Bind address: gcs-ip:5760 [ INFO] [1614053398.990358675]: Plugin 3dr_radio loaded [ INFO] [1614053399.000776852]: Plugin 3dr_radio initialized [ INFO] [1614053399.001395290]: Plugin actuator_control loaded [ INFO] [1614053399.013946331]: Plugin actuator_control initialized [ INFO] [1614053399.091422113]: Plugin adsb loaded [ INFO] [1614053399.108670081]: Plugin adsb initialized [ INFO] [1614053399.109346436]: Plugin altitude loaded [ INFO] [1614053399.113788519]: Plugin altitude initialized [ INFO] [1614053399.114446227]: Plugin cam_imu_sync loaded [ INFO] [1614053399.117326592]: Plugin cam_imu_sync initialized [ INFO] [1614053399.117984300]: Plugin command loaded [ INFO] [1614053399.144344092]: Plugin command initialized [ INFO] [1614053399.145001800]: Plugin companion_process_status loaded [ INFO] [1614053399.155147008]: Plugin companion_process_status initialized [ INFO] [1614053399.155730290]: Plugin debug_value loaded [ INFO] [1614053399.175057790]: Plugin debug_value initialized [ INFO] [1614053399.175230081]: Plugin distance_sensor blacklisted [ INFO] [1614053399.175738936]: Plugin esc_status loaded [ INFO] [1614053399.180887738]: Plugin esc_status initialized [ INFO] [1614053399.181056644]: Plugin fake_gps blacklisted [ INFO] [1614053399.181786540]: Plugin ftp loaded [ INFO] [1614053399.211375290]: Plugin ftp initialized [ INFO] [1614053399.212099300]: Plugin global_position loaded [ INFO] [1614053399.278165081]: Plugin global_position initialized [ INFO] [1614053399.278864561]: Plugin gps_rtk loaded [ INFO] [1614053399.291020186]: Plugin gps_rtk initialized [ INFO] [1614053399.291614977]: Plugin gps_status loaded [ INFO] [1614053399.302287425]: Plugin gps_status initialized [ INFO] [1614053399.303055967]: Plugin hil loaded [ INFO] [1614053399.355022008]: Plugin hil initialized [ INFO] [1614053399.355722373]: Plugin home_position loaded [ INFO] [1614053399.370624144]: Plugin home_position initialized [ INFO] [1614053399.371334352]: Plugin imu loaded [ INFO] [1614053399.402958623]: Plugin imu initialized [ INFO] [1614053399.403597633]: Plugin landing_target loaded [ INFO] [1614053399.460096071]: Plugin landing_target initialized [ INFO] [1614053399.460885133]: Plugin local_position loaded [ INFO] [1614053399.489345550]: Plugin local_position initialized [ INFO] [1614053399.489967477]: Plugin log_transfer loaded [ INFO] [1614053399.502546696]: Plugin log_transfer initialized [ INFO] [1614053399.503234352]: Plugin manual_control loaded [ INFO] [1614053399.515923988]: Plugin manual_control initialized [ INFO] [1614053399.516578206]: Plugin mocap_pose_estimate loaded [ INFO] [1614053399.529313310]: Plugin mocap_pose_estimate initialized [ INFO] [1614053399.529920342]: Plugin mount_control loaded [ INFO] [1614053399.545143988]: Plugin mount_control initialized [ INFO] [1614053399.545753206]: Plugin obstacle_distance loaded [ INFO] [1614053399.556897581]: Plugin obstacle_distance initialized [ INFO] [1614053399.557563935]: Plugin odom loaded [ INFO] [1614053399.575600915]: Plugin odom initialized [ INFO] [1614053399.576203727]: Plugin onboard_computer_status loaded [ INFO] [1614053399.586406644]: Plugin onboard_computer_status initialized [ INFO] [1614053399.587129560]: Plugin param loaded [ INFO] [1614053399.599845758]: Plugin param initialized [ INFO] [1614053399.600541904]: Plugin play_tune loaded [ INFO] [1614053399.611009612]: Plugin play_tune initialized [ INFO] [1614053399.611715862]: Plugin px4flow loaded [ INFO] [1614053399.641531071]: Plugin px4flow initialized [ INFO] [1614053399.641749821]: Plugin rangefinder blacklisted [ INFO] [1614053399.642391019]: Plugin rc_io loaded [ INFO] [1614053399.657013571]: Plugin rc_io initialized [ INFO] [1614053399.657234300]: Plugin safety_area blacklisted [ INFO] [1614053399.657886227]: Plugin setpoint_accel loaded [ INFO] [1614053399.670537425]: Plugin setpoint_accel initialized [ INFO] [1614053399.671292790]: Plugin setpoint_attitude loaded [ INFO] [1614053399.708248571]: Plugin setpoint_attitude initialized [ INFO] [1614053399.709047894]: Plugin setpoint_position loaded [ INFO] [1614053399.771762217]: Plugin setpoint_position initialized [ INFO] [1614053399.772663414]: Plugin setpoint_raw loaded [ INFO] [1614053399.809052321]: Plugin setpoint_raw initialized [ INFO] [1614053399.809788467]: Plugin setpoint_trajectory loaded [ INFO] [1614053399.830338623]: Plugin setpoint_trajectory initialized [ INFO] [1614053399.831071019]: Plugin setpoint_velocity loaded [ INFO] [1614053399.854124873]: Plugin setpoint_velocity initialized [ INFO] [1614053399.854934873]: Plugin sys_status loaded [ INFO] [1614053399.898600758]: Plugin sys_status initialized [ INFO] [1614053399.899299092]: Plugin sys_time loaded [ INFO] [1614053399.920986435]: TM: Timesync mode: MAVLINK [ INFO] [1614053399.926591800]: Plugin sys_time initialized [ INFO] [1614053399.927231644]: Plugin trajectory loaded [ INFO] [1614053399.949060237]: Plugin trajectory initialized [ INFO] [1614053399.949865081]: Plugin vfr_hud loaded [ INFO] [1614053399.952920185]: Plugin vfr_hud initialized [ INFO] [1614053399.953117425]: Plugin vibration blacklisted [ INFO] [1614053399.953771592]: Plugin vision_pose_estimate loaded [ INFO] [1614053399.983849144]: Plugin vision_pose_estimate initialized [ INFO] [1614053399.984632217]: Plugin vision_speed_estimate loaded [ INFO] [1614053400.000762216]: Plugin vision_speed_estimate initialized [ INFO] [1614053400.001480966]: Plugin waypoint loaded [ INFO] [1614053400.023235237]: Plugin waypoint initialized [ INFO] [1614053400.023465602]: Plugin wheel_odometry blacklisted [ INFO] [1614053400.024076696]: Plugin wind_estimation loaded [ INFO] [1614053400.027144352]: Plugin wind_estimation initialized [ INFO] [1614053400.027575498]: Autostarting mavlink via USB on PX4 [ INFO] [1614053400.027897894]: Built-in SIMD instructions: None [ INFO] [1614053400.028073727]: Built-in MAVLink package version: 2020.11.11 [ INFO] [1614053400.028330862]: Known MAVLink dialects: common ardupilotmega ASLUAV all autoquad icarous matrixpilot paparazzi standard uAvionix ualberta [ INFO] [1614053400.028600758]: MAVROS started. MY ID 1.240, TARGET ID 1.1 [ INFO] [1614053400.035670550]: IMU: High resolution IMU detected! [ INFO] [1614053400.036255758]: IMU: Attitude quaternion IMU detected! [ INFO] [1614053400.486167060]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot [ INFO] [1614053400.494504560]: IMU: High resolution IMU detected! [ INFO] [1614053400.498253987]: IMU: Attitude quaternion IMU detected! [ERROR] [1614053400.876154820]: FCU: Mag #1 fail: STALE! [ERROR] [1614053400.925605862]: FCU: Mag #2 fail: STALE! [ INFO] [1614053401.503866799]: WP: Using MISSION_ITEM_INT [ INFO] [1614053401.504174091]: VER: 1.1: Capabilities 0x000000000000e4ef [ INFO] [1614053401.504625914]: VER: 1.1: Flight software: 010a01ff (e0f016c2b3000000) [ INFO] [1614053401.504864247]: VER: 1.1: Middleware software: 010a01ff (e0f016c2b3000000) [ INFO] [1614053401.505040706]: VER: 1.1: OS software: 071d00ff (427238133be2b0ec) [ INFO] [1614053401.505199924]: VER: 1.1: Board hardware: 00000011 [ INFO] [1614053401.505343518]: VER: 1.1: VID/PID: 26ac:0011 [ INFO] [1614053401.505465445]: VER: 1.1: UID: 3036511135323931 [ WARN] [1614053401.505726383]: CMD: Unexpected command 520, result 0 [ INFO] [1614053410.486802525]: HP: requesting home position [ INFO] [1614053415.491673773]: WP: mission received [ INFO] [1614053420.486753146]: HP: requesting home position ** Any suggestion to resolve this will be appreciated

ashirsat commented 3 years ago

@TSC21 any suggestions on this?

TSC21 commented 3 years ago

@ashirsat is there any reason you are using LPE? We are not actively maintaining LPE anymore, so I would suggest you use EKF2 instead and send the mocap data through the odom or vision_pose_estimate plugins.

ashirsat commented 3 years ago

@TSC21 It was an easier estimator to work with and for debugging. No other reason. Also I tried vision_pose_estimate I still have an offset of 90 deg when I work with optitrack system with respect to the NED frame. The heading direction reported by px4 and verified by QGC is east when I pointing North( +x axis of the optitrack system). So I do not know if the transformations are done correctly with the vision_pose topic on the mavros side. I have followed the guide mentioned in the initial bug report.

TSC21 commented 3 years ago

@TSC21 It was an easier estimator to work with and for debugging. No other reason. Also I tried vision_pose_estimate I still have an offset of 90 deg when I work with optitrack system with respect to the NED frame. The heading direction reported by px4 and verified by QGC is east when I pointing North( +x axis of the optitrack system). So I do not know if the transformations are done correctly with the vision_pose topic on the mavros side. I have followed the guide mentioned in the initial bug report.

That's probably because the optitrack system is providing the orientation in a left-handed reference or not in a different frame of reference than the expected. Try to swap the rotations in https://github.com/mavlink/mavros/blob/master/mavros_extras/src/plugins/vision_pose_estimate.cpp#L100-L101

ashirsat commented 3 years ago

@TSC21 Is this code a part of the binary installation with ros-melodic-mavros-extras?

TSC21 commented 3 years ago

@TSC21 Is this code a part of the binary installation with ros-melodic-mavros-extras?

What code?

ashirsat commented 3 years ago

@TSC21 Is this code a part of the binary installation with ros-melodic-mavros-extras?

What code?

The lines that you linked in the previous response?

ashirsat commented 3 years ago

@TSC21 It was an easier estimator to work with and for debugging. No other reason. Also I tried vision_pose_estimate I still have an offset of 90 deg when I work with optitrack system with respect to the NED frame. The heading direction reported by px4 and verified by QGC is east when I pointing North( +x axis of the optitrack system). So I do not know if the transformations are done correctly with the vision_pose topic on the mavros side. I have followed the guide mentioned in the initial bug report.

That's probably because the optitrack system is providing the orientation in a left-handed reference or not in a different frame of reference than the expected. Try to swap the rotations in https://github.com/mavlink/mavros/blob/master/mavros_extras/src/plugins/vision_pose_estimate.cpp#L100-L101

Also I don't think it is a left hand co-ordinate system. If I follow the guide I think I get +x forward +y left of pixhawk and +z upwards.