Optitrack data didn't used in EKF

mengchaoheng commented 1 year ago

Describe the bug

I use optitrack as an external position reference, but it seems that ekf is not using position data.

To Reproduce

follow the guide and https://risc.readthedocs.io/1-indoor-flight.html

In Motive, choose View > Data Streaming from menu bar. Check the boxes Broadcast Frame Data in OptiTrack Streaming Engine and VRPN Streaming Engine sections. Create a rigid body by selecting markers of interest. In Advanced Network Options section change Up Axis to Z Up.

on my companion computer, run

$ roslaunch vrpn_client_ros sample.launch server:=192.168.3.252

I can receive Mocap data under topic /vrpn_client_node/<rigid_body_name>/pose by run rostopic echo /vrpn_client_node/RigidBody/pose

---
header: 
  seq: 4975
  stamp: 
    secs: 1682086599
    nsecs: 613445830
  frame_id: "world"
pose: 
  position: 
    x: 1.1339868307113647
    y: -2.063505172729492
    z: 0.5767601728439331
  orientation: 
    x: 0.03174411132931709
    y: 0.01097777858376503
    z: 0.011162354610860348
    w: 0.9993734955787659
---

Remap the pose topic from Mocap /vrpn_client_node/
/pose directly to /mavros/vision_pose/pose.
```
$ rosrun topic_tools relay /vrpn_client_node/RigidBody/pose /mavros/vision_pose/pose
```

run rostopic echo /mavros/vision_pose/pose , (Not run at the same time as the command that prints /vrpn_client_node/RigidBody/pose.), so the update time interval is about 20ms?

---
header: 
  seq: 92417
  stamp: 
    secs: 1682086448
    nsecs: 761981303
  frame_id: "world"
pose: 
  position: 
    x: 1.6821959018707275
    y: -1.878477931022644
    z: 0.8759177327156067
  orientation: 
    x: 5.24241550010629e-05
    y: -0.00011159150744788349
    z: -1.5280322259059176e-05
    w: 1.0
---
header: 
  seq: 92418
  stamp: 
    secs: 1682086448
    nsecs: 782298890
  frame_id: "world"
pose: 
  position: 
    x: 1.6821929216384888
    y: -1.8784681558609009
    z: 0.8759211897850037
  orientation: 
    x: 5.0888331315945834e-05
    y: -7.606665894854814e-05
    z: -1.709804928395897e-05
    w: 1.0
---
header: 
  seq: 92419
  stamp: 
    secs: 1682086448
    nsecs: 791980543
  frame_id: "world"
pose: 
  position: 
    x: 1.6821929216384888
    y: -1.8784756660461426
    z: 0.8759207725524902
  orientation: 
    x: 6.052733078831807e-05
    y: -9.11090464796871e-05
    z: -1.5816982568139792e-07
    w: 1.0
---

and in the log, we can see the vehicle_visual_odometry_0 data is

run mavros


$ roslaunch mavros px4.launch
... logging to /home/parallels/.ros/log/ecea82c8-e04e-11ed-bca1-99848a3fac42/roslaunch-ubuntu-linux-20-04-desktop-200778.log
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://localhost:38635/

SUMMARY
========

CLEAR PARAMETERS
 * /mavros/

PARAMETERS
 * /mavros/camera/frame_id: base_link
 * /mavros/cmd/use_comp_id_system_control: False
 * /mavros/conn/heartbeat_rate: 1.0
 * /mavros/conn/system_time_rate: 1.0
 * /mavros/conn/timeout: 10.0
 * /mavros/conn/timesync_rate: 10.0
 * /mavros/distance_sensor/hrlv_ez4_pub/field_of_view: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/frame_id: hrlv_ez4_sonar
 * /mavros/distance_sensor/hrlv_ez4_pub/id: 0
 * /mavros/distance_sensor/hrlv_ez4_pub/orientation: PITCH_270
 * /mavros/distance_sensor/hrlv_ez4_pub/send_tf: True
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/laser_1_sub/id: 3
 * /mavros/distance_sensor/laser_1_sub/orientation: PITCH_270
 * /mavros/distance_sensor/laser_1_sub/subscriber: True
 * /mavros/distance_sensor/lidarlite_pub/field_of_view: 0.0
 * /mavros/distance_sensor/lidarlite_pub/frame_id: lidarlite_laser
 * /mavros/distance_sensor/lidarlite_pub/id: 1
 * /mavros/distance_sensor/lidarlite_pub/orientation: PITCH_270
 * /mavros/distance_sensor/lidarlite_pub/send_tf: True
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/sonar_1_sub/horizontal_fov_ratio: 1.0
 * /mavros/distance_sensor/sonar_1_sub/id: 2
 * /mavros/distance_sensor/sonar_1_sub/orientation: PITCH_270
 * /mavros/distance_sensor/sonar_1_sub/subscriber: True
 * /mavros/distance_sensor/sonar_1_sub/vertical_fov_ratio: 1.0
 * /mavros/fake_gps/eph: 2.0
 * /mavros/fake_gps/epv: 2.0
 * /mavros/fake_gps/fix_type: 3
 * /mavros/fake_gps/geo_origin/alt: 408.0
 * /mavros/fake_gps/geo_origin/lat: 47.3667
 * /mavros/fake_gps/geo_origin/lon: 8.55
 * /mavros/fake_gps/gps_rate: 5.0
 * /mavros/fake_gps/mocap_transform: True
 * /mavros/fake_gps/satellites_visible: 5
 * /mavros/fake_gps/tf/child_frame_id: fix
 * /mavros/fake_gps/tf/frame_id: map
 * /mavros/fake_gps/tf/listen: False
 * /mavros/fake_gps/tf/rate_limit: 10.0
 * /mavros/fake_gps/tf/send: False
 * /mavros/fake_gps/use_mocap: True
 * /mavros/fake_gps/use_vision: False
 * /mavros/fcu_protocol: v2.0
 * /mavros/fcu_url: /dev/ttyUSB0:921600
 * /mavros/gcs_url: udp://@192.168.3.169
 * /mavros/global_position/child_frame_id: base_link
 * /mavros/global_position/frame_id: map
 * /mavros/global_position/gps_uere: 1.0
 * /mavros/global_position/rot_covariance: 99999.0
 * /mavros/global_position/tf/child_frame_id: base_link
 * /mavros/global_position/tf/frame_id: map
 * /mavros/global_position/tf/global_frame_id: earth
 * /mavros/global_position/tf/send: False
 * /mavros/global_position/use_relative_alt: True
 * /mavros/image/frame_id: px4flow
 * /mavros/imu/angular_velocity_stdev: 0.0003490659 // 0...
 * /mavros/imu/frame_id: base_link
 * /mavros/imu/linear_acceleration_stdev: 0.0003
 * /mavros/imu/magnetic_stdev: 0.0
 * /mavros/imu/orientation_stdev: 1.0
 * /mavros/landing_target/camera/fov_x: 2.0071286398
 * /mavros/landing_target/camera/fov_y: 2.0071286398
 * /mavros/landing_target/image/height: 480
 * /mavros/landing_target/image/width: 640
 * /mavros/landing_target/land_target_type: VISION_FIDUCIAL
 * /mavros/landing_target/listen_lt: False
 * /mavros/landing_target/mav_frame: LOCAL_NED
 * /mavros/landing_target/target_size/x: 0.3
 * /mavros/landing_target/target_size/y: 0.3
 * /mavros/landing_target/tf/child_frame_id: camera_center
 * /mavros/landing_target/tf/frame_id: landing_target
 * /mavros/landing_target/tf/listen: False
 * /mavros/landing_target/tf/rate_limit: 10.0
 * /mavros/landing_target/tf/send: True
 * /mavros/local_position/frame_id: map
 * /mavros/local_position/tf/child_frame_id: base_link
 * /mavros/local_position/tf/frame_id: map
 * /mavros/local_position/tf/send: False
 * /mavros/local_position/tf/send_fcu: False
 * /mavros/mission/pull_after_gcs: True
 * /mavros/mission/use_mission_item_int: True
 * /mavros/mocap/use_pose: True
 * /mavros/mocap/use_tf: False
 * /mavros/mount/debounce_s: 4.0
 * /mavros/mount/err_threshold_deg: 10.0
 * /mavros/mount/negate_measured_pitch: False
 * /mavros/mount/negate_measured_roll: False
 * /mavros/mount/negate_measured_yaw: False
 * /mavros/odometry/fcu/odom_child_id_des: base_link
 * /mavros/odometry/fcu/odom_parent_id_des: map
 * /mavros/plugin_blacklist: ['safety_area', '...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.118682
 * /mavros/px4flow/ranger_max_range: 5.0
 * /mavros/px4flow/ranger_min_range: 0.3
 * /mavros/safety_area/p1/x: 1.0
 * /mavros/safety_area/p1/y: 1.0
 * /mavros/safety_area/p1/z: 1.0
 * /mavros/safety_area/p2/x: -1.0
 * /mavros/safety_area/p2/y: -1.0
 * /mavros/safety_area/p2/z: -1.0
 * /mavros/setpoint_accel/send_force: False
 * /mavros/setpoint_attitude/reverse_thrust: False
 * /mavros/setpoint_attitude/tf/child_frame_id: target_attitude
 * /mavros/setpoint_attitude/tf/frame_id: map
 * /mavros/setpoint_attitude/tf/listen: False
 * /mavros/setpoint_attitude/tf/rate_limit: 50.0
 * /mavros/setpoint_attitude/use_quaternion: False
 * /mavros/setpoint_position/mav_frame: LOCAL_NED
 * /mavros/setpoint_position/tf/child_frame_id: target_position
 * /mavros/setpoint_position/tf/frame_id: map
 * /mavros/setpoint_position/tf/listen: False
 * /mavros/setpoint_position/tf/rate_limit: 50.0
 * /mavros/setpoint_raw/thrust_scaling: 1.0
 * /mavros/setpoint_velocity/mav_frame: LOCAL_NED
 * /mavros/startup_px4_usb_quirk: False
 * /mavros/sys/disable_diag: False
 * /mavros/sys/min_voltage: 10.0
 * /mavros/target_component_id: 1
 * /mavros/target_system_id: 1
 * /mavros/tdr_radio/low_rssi: 40
 * /mavros/time/time_ref_source: fcu
 * /mavros/time/timesync_avg_alpha: 0.6
 * /mavros/time/timesync_mode: MAVLINK
 * /mavros/vibration/frame_id: base_link
 * /mavros/vision_pose/tf/child_frame_id: vision_estimate
 * /mavros/vision_pose/tf/frame_id: odom
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: True
 * /mavros/vision_speed/twist_cov: True
 * /mavros/wheel_odometry/child_frame_id: base_link
 * /mavros/wheel_odometry/count: 2
 * /mavros/wheel_odometry/frame_id: odom
 * /mavros/wheel_odometry/send_raw: True
 * /mavros/wheel_odometry/send_twist: False
 * /mavros/wheel_odometry/tf/child_frame_id: base_link
 * /mavros/wheel_odometry/tf/frame_id: odom
 * /mavros/wheel_odometry/tf/send: False
 * /mavros/wheel_odometry/use_rpm: False
 * /mavros/wheel_odometry/vel_error: 0.1
 * /mavros/wheel_odometry/wheel0/radius: 0.05
 * /mavros/wheel_odometry/wheel0/x: 0.0
 * /mavros/wheel_odometry/wheel0/y: -0.15
 * /mavros/wheel_odometry/wheel1/radius: 0.05
 * /mavros/wheel_odometry/wheel1/x: 0.0
 * /mavros/wheel_odometry/wheel1/y: 0.15
 * /rosdistro: noetic
 * /rosversion: 1.15.14

NODES
  /
    mavros (mavros/mavros_node)

ROS_MASTER_URI=http://localhost:11311

process[mavros-1]: started with pid [200795]
[ INFO] [1682086520.296394532]: FCU URL: /dev/ttyUSB0:921600
[ INFO] [1682086520.297794449]: serial0: device: /dev/ttyUSB0 @ 921600 bps
[ INFO] [1682086520.313562407]: GCS URL: udp://@192.168.3.169
[ INFO] [1682086520.313869555]: udp1: Bind address: 0.0.0.0:14555
[ INFO] [1682086520.313905401]: udp1: Remote address: 192.168.3.169:14550
[ INFO] [1682086520.322512592]: Plugin 3dr_radio loaded
[ INFO] [1682086520.323607639]: Plugin 3dr_radio initialized
[ INFO] [1682086520.323682642]: Plugin actuator_control loaded
[ INFO] [1682086520.325633070]: Plugin actuator_control initialized
[ INFO] [1682086520.330150500]: Plugin adsb loaded
[ INFO] [1682086520.331708547]: Plugin adsb initialized
[ INFO] [1682086520.331823394]: Plugin altitude loaded
[ INFO] [1682086520.332357142]: Plugin altitude initialized
[ INFO] [1682086520.332424580]: Plugin cam_imu_sync loaded
[ INFO] [1682086520.332712902]: Plugin cam_imu_sync initialized
[ INFO] [1682086520.332780083]: Plugin camera loaded
[ INFO] [1682086520.333060754]: Plugin camera initialized
[ INFO] [1682086520.333138637]: Plugin cellular_status loaded
[ INFO] [1682086520.334165773]: Plugin cellular_status initialized
[ INFO] [1682086520.334269746]: Plugin command loaded
[ INFO] [1682086520.337549257]: Plugin command initialized
[ INFO] [1682086520.337630793]: Plugin companion_process_status loaded
[ INFO] [1682086520.338883584]: Plugin companion_process_status initialized
[ INFO] [1682086520.338961553]: Plugin debug_value loaded
[ INFO] [1682086520.341125643]: Plugin debug_value initialized
[ INFO] [1682086520.341148037]: Plugin distance_sensor blacklisted
[ INFO] [1682086520.341230820]: Plugin esc_status loaded
[ INFO] [1682086520.342040554]: Plugin esc_status initialized
[ INFO] [1682086520.342163610]: Plugin esc_telemetry loaded
[ INFO] [1682086520.342632714]: Plugin esc_telemetry initialized
[ INFO] [1682086520.342764668]: Plugin fake_gps loaded
[ INFO] [1682086520.349407934]: Plugin fake_gps initialized
[ INFO] [1682086520.349575047]: Plugin ftp loaded
[ INFO] [1682086520.353590235]: Plugin ftp initialized
[ INFO] [1682086520.353709853]: Plugin geofence loaded
[ INFO] [1682086520.355815273]: Plugin geofence initialized
[ INFO] [1682086520.355969965]: Plugin global_position loaded
[ INFO] [1682086520.363373708]: Plugin global_position initialized
[ INFO] [1682086520.363485074]: Plugin gps_input loaded
[ INFO] [1682086520.364874030]: Plugin gps_input initialized
[ INFO] [1682086520.364960897]: Plugin gps_rtk loaded
[ INFO] [1682086520.366327847]: Plugin gps_rtk initialized
[ INFO] [1682086520.366489071]: Plugin gps_status loaded
[ INFO] [1682086520.367674724]: Plugin gps_status initialized
[ INFO] [1682086520.367764556]: Plugin guided_target loaded
[ INFO] [1682086520.369660569]: Plugin guided_target initialized
[ INFO] [1682086520.369758782]: Plugin hil loaded
[ INFO] [1682086520.375381188]: Plugin hil initialized
[ INFO] [1682086520.375497626]: Plugin home_position loaded
[ INFO] [1682086520.377134673]: Plugin home_position initialized
[ INFO] [1682086520.377312231]: Plugin imu loaded
[ INFO] [1682086520.380540680]: Plugin imu initialized
[ INFO] [1682086520.380629652]: Plugin keyboard_command loaded
[ INFO] [1682086520.382015385]: Plugin keyboard_command initialized
[ INFO] [1682086520.382147511]: Plugin landing_target loaded
[ INFO] [1682086520.389228031]: Plugin landing_target initialized
[ INFO] [1682086520.389344125]: Plugin local_position loaded
[ INFO] [1682086520.392058683]: Plugin local_position initialized
[ INFO] [1682086520.392156553]: Plugin log_transfer loaded
[ INFO] [1682086520.394344283]: Plugin log_transfer initialized
[ INFO] [1682086520.394443958]: Plugin mag_calibration_status loaded
[ INFO] [1682086520.395048454]: Plugin mag_calibration_status initialized
[ INFO] [1682086520.395138844]: Plugin manual_control loaded
[ INFO] [1682086520.396730847]: Plugin manual_control initialized
[ INFO] [1682086520.396810235]: Plugin mocap_pose_estimate loaded
[ INFO] [1682086520.398326976]: Plugin mocap_pose_estimate initialized
[ INFO] [1682086520.398407567]: Plugin mount_control loaded
[ WARN] [1682086520.400640041]: Could not retrive negate_measured_roll parameter value, using default (0)
[ WARN] [1682086520.400818803]: Could not retrive negate_measured_pitch parameter value, using default (0)
[ WARN] [1682086520.401010630]: Could not retrive negate_measured_yaw parameter value, using default (0)
[ WARN] [1682086520.401585899]: Could not retrive debounce_s parameter value, using default (4.000000)
[ WARN] [1682086520.401760405]: Could not retrive err_threshold_deg parameter value, using default (10.000000)
[ INFO] [1682086520.401812026]: Plugin mount_control initialized
[ INFO] [1682086520.401896743]: Plugin nav_controller_output loaded
[ INFO] [1682086520.402223491]: Plugin nav_controller_output initialized
[ INFO] [1682086520.402294798]: Plugin obstacle_distance loaded
[ INFO] [1682086520.403531255]: Plugin obstacle_distance initialized
[ INFO] [1682086520.403606430]: Plugin odom loaded
[ INFO] [1682086520.405627649]: Plugin odom initialized
[ INFO] [1682086520.405707810]: Plugin onboard_computer_status loaded
[ INFO] [1682086520.406724071]: Plugin onboard_computer_status initialized
[ INFO] [1682086520.406863161]: Plugin param loaded
[ INFO] [1682086520.408710260]: Plugin param initialized
[ INFO] [1682086520.408859278]: Plugin play_tune loaded
[ INFO] [1682086520.410137987]: Plugin play_tune initialized
[ INFO] [1682086520.410257605]: Plugin px4flow loaded
[ INFO] [1682086520.414057024]: Plugin px4flow initialized
[ INFO] [1682086520.414194264]: Plugin rallypoint loaded
[ INFO] [1682086520.415857574]: Plugin rallypoint initialized
[ INFO] [1682086520.415894409]: Plugin rangefinder blacklisted
[ INFO] [1682086520.416043900]: Plugin rc_io loaded
[ INFO] [1682086520.418113001]: Plugin rc_io initialized
[ INFO] [1682086520.418160023]: Plugin safety_area blacklisted
[ INFO] [1682086520.418268681]: Plugin setpoint_accel loaded
[ INFO] [1682086520.419665030]: Plugin setpoint_accel initialized
[ INFO] [1682086520.419853849]: Plugin setpoint_attitude loaded
[ INFO] [1682086520.425026622]: Plugin setpoint_attitude initialized
[ INFO] [1682086520.425162789]: Plugin setpoint_position loaded
[ INFO] [1682086520.432297337]: Plugin setpoint_position initialized
[ INFO] [1682086520.432422371]: Plugin setpoint_raw loaded
[ INFO] [1682086520.437363429]: Plugin setpoint_raw initialized
[ INFO] [1682086520.437490354]: Plugin setpoint_trajectory loaded
[ INFO] [1682086520.439999459]: Plugin setpoint_trajectory initialized
[ INFO] [1682086520.440247765]: Plugin setpoint_velocity loaded
[ INFO] [1682086520.444032699]: Plugin setpoint_velocity initialized
[ INFO] [1682086520.444457015]: Plugin sys_status loaded
[ INFO] [1682086520.451360107]: Plugin sys_status initialized
[ INFO] [1682086520.451515787]: Plugin sys_time loaded
[ INFO] [1682086520.453720065]: TM: Timesync mode: MAVLINK
[ INFO] [1682086520.453915116]: TM: Not publishing sim time
[ INFO] [1682086520.454415854]: Plugin sys_time initialized
[ INFO] [1682086520.454514369]: Plugin terrain loaded
[ INFO] [1682086520.454810771]: Plugin terrain initialized
[ INFO] [1682086520.454879929]: Plugin trajectory loaded
[ INFO] [1682086520.457155213]: Plugin trajectory initialized
[ INFO] [1682086520.457236363]: Plugin tunnel loaded
[ INFO] [1682086520.458721083]: Plugin tunnel initialized
[ INFO] [1682086520.458839713]: Plugin vfr_hud loaded
[ INFO] [1682086520.459211204]: Plugin vfr_hud initialized
[ INFO] [1682086520.459237724]: Plugin vibration blacklisted
[ INFO] [1682086520.459315091]: Plugin vision_pose_estimate loaded
[ INFO] [1682086520.462900246]: Plugin vision_pose_estimate initialized
[ INFO] [1682086520.462993301]: Plugin vision_speed_estimate loaded
[ INFO] [1682086520.464535660]: Plugin vision_speed_estimate initialized
[ INFO] [1682086520.464629575]: Plugin waypoint loaded
[ INFO] [1682086520.467631983]: Plugin waypoint initialized
[ INFO] [1682086520.467666368]: Plugin wheel_odometry blacklisted
[ INFO] [1682086520.467749581]: Plugin wind_estimation loaded
[ INFO] [1682086520.468246708]: Plugin wind_estimation initialized
[ INFO] [1682086520.468526950]: Built-in SIMD instructions: ARM NEON
[ INFO] [1682086520.468562581]: Built-in MAVLink package version: 2022.12.30
[ INFO] [1682086520.468587081]: Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all cubepilot development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta
[ INFO] [1682086520.468620994]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1682086520.468657227]: udp1: Remote address: 192.168.3.169:14550
[ INFO] [1682086520.469185259]: IMU: High resolution IMU detected!
[ INFO] [1682086520.469452348]: IMU: Attitude quaternion IMU detected!
[ INFO] [1682086520.665288795]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[ INFO] [1682086520.676228736]: IMU: High resolution IMU detected!
[ INFO] [1682086520.676465179]: IMU: Attitude quaternion IMU detected!
[ INFO] [1682086520.893204662]: FCU: Manual control regained after 12.5s
[ INFO] [1682086520.945622551]: FCU: Data link regained
[ INFO] [1682086521.460505511]: FCU: Onboard controller regained
[ INFO] [1682086521.671006917]: GF: Using MISSION_ITEM_INT
[ INFO] [1682086521.671065845]: RP: Using MISSION_ITEM_INT
[ INFO] [1682086521.671090473]: WP: Using MISSION_ITEM_INT
[ INFO] [1682086521.671106721]: VER: 1.1: Capabilities         0x000000000000e4ef
[ INFO] [1682086521.671130618]: VER: 1.1: Flight software:     010c0300 (c6186ac04a000000)
[ INFO] [1682086521.671146866]: VER: 1.1: Middleware software: 010c0300 (c6186ac04a000000)
[ INFO] [1682086521.671157998]: VER: 1.1: OS software:         080200ff (bf660cba2af81f05)
[ INFO] [1682086521.671179274]: VER: 1.1: Board hardware:      00000032
[ INFO] [1682086521.671199174]: VER: 1.1: VID/PID:             26ac:0032
[ INFO] [1682086521.671223244]: VER: 1.1: UID:                 3138511635333837
[ WARN] [1682086521.673975325]: CMD: Unexpected command 520, result 0
[ INFO] [1682086530.666204872]: HP: requesting home position
[ WARN] [1682086532.803352081]: Detected jump back in time of 1.20831s. Clearing TF buffer.
[ INFO] [1682086535.670007956]: GF: mission received
[ INFO] [1682086535.670779771]: RP: mission received
[ INFO] [1682086535.671012800]: WP: mission received
[ INFO] [1682086540.666352436]: HP: requesting home position
[ INFO] [1682086550.666470368]: HP: requesting home position
[ INFO] [1682086560.666625094]: HP: requesting home position
[ INFO] [1682086570.667340168]: HP: requesting home position

Expected behavior

The state of the state estimator(EKF) should directly have a simple coordinate transformation relationship with the external visual position data.

Log Files and Screenshots

The log is here

As shown by the Visual Odometry position data, I pick up the plane, go east, go back near the origin, then go north, return to the origin and put down the plane. The data released by optitrack has been set correctly. At this time, the yaw angle is close to 75 degrees, so roughly the local position data should be exchanged with the x and y axes of the Visual Odometry position data (of course, the z axis is opposite). But the log shows that this is not the case. bokeh_plot bokeh_plot (1) bokeh_plot (2) bokeh_plot (3)

Drone (please complete the following information):

fmu-v5, v1.12.3, CUAV V5+ .
mavros 1.15, ros noetic, ubuntu20, vrpn_client_ros on kinetic-devel branch (I couldn't find a noetic version, so that shouldn't be a problem.)
everything is install by guiden

mengchaoheng commented 1 year ago

sample.launch of vrpn_client_ros

<launch>

  <arg name="server" default="localhost"/>

  <node pkg="vrpn_client_ros" type="vrpn_client_node" name="vrpn_client_node" output="screen">
    <rosparam subst_value="true">
      server: $(arg server)
      port: 3883

      update_frequency: 100.0
      frame_id: world

      # Use the VRPN server's time, or the client's ROS time.
      use_server_time: false
      broadcast_tf: true

      # Must either specify refresh frequency > 0.0, or a list of trackers to create
      refresh_tracker_frequency: 1.0
      #trackers:
      #- FirstTracker
      #- SecondTracker
    </rosparam>
  </node>

</launch>

mengchaoheng commented 1 year ago

px4.launch of mavros

<launch>
    <!-- vim: set ft=xml noet : -->
    <!-- example launch script for PX4 based FCU's udp://:14540@127.0.0.1:14557  udp://@192.168.101.60-->

    <arg name="fcu_url" default="/dev/ttyUSB0:921600" />
    <arg name="gcs_url" default="udp://@192.168.3.169 " />
    <arg name="tgt_system" default="1" />
    <arg name="tgt_component" default="1" />
    <arg name="log_output" default="screen" />
    <arg name="fcu_protocol" default="v2.0" />
    <arg name="respawn_mavros" default="false" />

    <include file="$(find mavros)/launch/node.launch">
        <arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
        <arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />

        <arg name="fcu_url" value="$(arg fcu_url)" />
        <arg name="gcs_url" value="$(arg gcs_url)" />
        <arg name="tgt_system" value="$(arg tgt_system)" />
        <arg name="tgt_component" value="$(arg tgt_component)" />
        <arg name="log_output" value="$(arg log_output)" />
        <arg name="fcu_protocol" value="$(arg fcu_protocol)" />
        <arg name="respawn_mavros" default="$(arg respawn_mavros)" />
    </include>
</launch>

mengchaoheng commented 1 year ago

Is it because I didn't do this step?

Align your robot’s forward direction with the the system +x-axis.

Is this aligned to the x-axis of the ned or the x-axis of the mocap system？

mengchaoheng commented 1 year ago

I observed the log and found that the xyz coordinate system of /mavros/vision_pose/pose does not overlap with the real geographic ENU (X East, Y North, Z Up), but just a FLU (X Forward, Y Left) customized by the optitrack system , Z Up).

So the xyz coordinates of vehicle_visual_odometry_0 are not really NED (X North, Y East, Z Down).

Will this cause errors in ekf fusion? You can compare the xyz value of /mavros/vision_pose/pose with the xyz value of vehicle_visual_odometry_0 and find that they exchanged the xy axis and reversed the z axis, which is the credit of mavros. But why is the output of ekf wrong?

mengchaoheng commented 1 year ago

and maybe the problem is visual odometry latency?

DronecodeBot commented 1 year ago

This issue has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/error-in-local-positioning-of-pixhawk4-using-vicon-data/16011/46

mengchaoheng commented 1 year ago

@bresch @bkueng @dagar can you help me please!

ayhamalharbat commented 1 year ago

It might be a stupid thing, but I always run mavros before I run the vrpn client and remap its data to /mavros/vision_pose/pose, because I noticed before that if I ran mavros after, this behavior that you mentioned happened.

Try it out and let us know.

j-chen-opteran commented 1 year ago

If I remember correctly EKF_AID_MASK has to be set 280 on PX4 <1.13 or visual odometry information to be fused.

Once your system is running, what do you get when you go into the MAVLink shell and type ekf2 status?
Is MAV_ODOM_LP set to 1? If so do you see an Odometry MAVLink message being published by the autopilot? If it is, fusion is happening correctly.

Regarding coordinate frames - MAVROS expects a position and orientation measurement in ENU axes, and perform an internal conversion into NED when turning it into a MAVLink message. It looks to me like you need an additional step in between to transform the FLU axes of your Mocap system into ENU first. If all this is correct, then your vehicle_odometry_aligned message will be in NED axes, and they'll line up pretty much exactly with vehicle_local_position.

You mentioned aligning the +x axis of the robot - this needs to be aligned with the global x-axis of the Mocap system, in whatever coordinate convention that the Mocap system uses. Then you need to figure out how to transform that into ROS ENU. For example if your Mocap system gives out coordinates in FLU, you'll need to apply a +270 degree rotation to positions and rotations about the Z axis first before publishing it into ROS.

mengchaoheng commented 1 year ago

Once your system is running, what do you get when you go into the MAVLink shell and type ekf2 status?

later I will show you

Is MAV_ODOM_LP set to 1? If so do you see an Odometry MAVLink message being published by the autopilot? If it is, fusion is happening correctly.

Yes! I can see Odometry MAVLink message when MAV_ODOM_LP =1

Regarding coordinate frames - MAVROS expects a position and orientation measurement in ENU axes, and perform an internal conversion into NED when turning it into a MAVLink message. It looks to me like you need an additional step in between to transform the FLU axes of your Mocap system into ENU first. If all this is correct, then your vehicle_odometry_aligned message will be in NED axes, and they'll line up pretty much exactly with vehicle_local_position.

From the logs, vehicle_odometry_aligned messages are indeed in the NED axis, but they have nothing to do with vehicle_local_position, ekf does not enable position fusion.

You mentioned aligning the +x axis of the robot - this needs to be aligned with the global x-axis of the Mocap system, in whatever coordinate convention that the Mocap system uses. Then you need to figure out how to transform that into ROS ENU. For example if your Mocap system gives out coordinates in FLU, you'll need to apply a +270 degree rotation to positions and rotations about the Z axis first before publishing it into ROS.

In fact, the forward direction of my aircraft is not aligned with the x-axis of the mocap system, but the ekf is not enabled, which may not be the reason. My guess is that the startup sequence of vrpn node and mavros node is wrong, which needs to be verified.

mengchaoheng commented 1 year ago

I plot the timestamp of each data by MATLAB：


vehicle_visual_odometry=log.data.vehicle_visual_odometry_0{:,:};
sensor_combined=log.data.sensor_combined_0{:,:};
vehicle_magnetometer=log.data.vehicle_magnetometer_0{:,:};
vehicle_air_data=log.data.vehicle_air_data_0{:,:};
[len1,~]=size(sensor_combined);
[len2,~]=size(vehicle_visual_odometry);
[len3,~]=size(vehicle_magnetometer);
[len4,~]=size(vehicle_air_data);
t1=1:1:len1;
t2=1:1:len2;
t3=1:1:len3;
t4=1:1:len4;
figure,
plot(t1,sensor_combined(:,1),'k-');hold on;
plot(t2,vehicle_visual_odometry(:,1),'r-');hold on;
plot(t3,vehicle_magnetometer(:,1),'b');hold on;
plot(t4,vehicle_air_data(:,1),'k--');hold on;
legend('sensor combined','vehicle visual odometry','vehicle magnetometer','vehicle air data');

Download the log from flight_review, draw the same graph， one is gps , the other is mocap:

so, is the time is error?

mengchaoheng commented 1 year ago

a new log, everything is ok! But I don't know what hanppen.

https://logs.px4.io/plot_app?log=01dd3841-b0ce-4d85-bf79-67a8e1bafb06

mengchaoheng commented 1 year ago

Unfortunately, I have not been able to reproduce this problem, but after careful analysis of the log, I found two problems:

Why does the height and v_z diverge before the ekf converges after the data of the mocap system is transmitted to the pixhawk through mavros?
Why is the time delay beating erratically? Is this normal?

log:

bresch commented 1 year ago

Why does the height and v_z diverge before the ekf converges after the data of the mocap system is transmitted to the pixhawk through mavros?

There is apparently something strange happening with the accel bias when the vision fusion is starting. DeepinScreenshot_select-area_20230425115726

The vision fusion got completely refactored and a lot of bugs were fixed recently, any chance you could try the same tests on the 1.14 release candidate?

mengchaoheng commented 1 year ago

test on v1.14.0-beta2, log is https://logs.px4.io/plot_app?log=faa63906-0a64-4a1d-bf8b-c8030fbc5aed

Problem:

It is also possible to switch to position mode without a correct position estimate.

About the startup sequence: 2.1 Start the ros/mavros node first, and then start the flight control, sometimes it prompts:

[ERROR] [1682506410.410595161]: TM : Time jump detected. Resetting time synchronizer.

2.2 Start the aircraft first, then start ros/mavros, and the following will appear:

[ INFO ] [1682506535.766511963]: RP: mission received
[ERROR] [1682506537.214612200]: FCU: Preflight Fail: Yaw estimate error
[ERROR] [1682506539.297581967]: FCU: Preflight Fail: Yaw estimate error
[ERROR] [1682506542.068707017]: FCU: Preflight Fail: heading estimate not stable
[ERROR] [1682506542.236544900]: FCU: Preflight Fail: Yaw estimate error
[ INFO ] [1682506542.824510012]: HP: requesting home position

or

[ERROR] [1682507013.619805744]: FCU: Preflight Fail: height estimate error
[ERROR] [1682507015.676895252]: FCU: Preflight Fail: height estimate error

2.3 If the mocap data is broken in the middle, it will not be restored automatically. Mavros needs to be restarted, and a prompt appears again:

Preflight Fail: Yaw estimate error

In summary, the best way to use it is to start ros/mavros first and then start the flight control, and ensure that the mocap data is not interrupted.

mengchaoheng commented 1 year ago

The problem with v1.13.3 is the unreasonable visual odometer latency. log is https://logs.px4.io/plot_app?log=3ff36663-37c3-4605-9953-80b82c373c33

mengchaoheng commented 1 year ago

@bresch

bresch commented 1 year ago

In "test on v1.14.0-beta2, log is https://logs.px4.io/plot_app?log=faa63906-0a64-4a1d-bf8b-c8030fbc5aed" it looks like both mag and ev_yaw fusions are active at the same time. This isn't right, I'll investigate a bit.

bresch commented 1 year ago

The mag is enabled because you have MAV_FRAME set to NED and the mag doesn't agree with the attitude sent by the optitrack. If it's really NED and that the mag is wrong, you can disable it using EKF2_MAG_TYPE, otherwise set the MAV_FRAME to FRD (the ekf will then only use the vision data).

DronecodeBot commented 1 year ago

This issue has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-community-q-a-june-28-2023/32873/8

fp018 commented 1 month ago

Hi! I am currently trying to feed visual (mocap) feedback through the uxrce by publishing on the topic /fmu/in/vehicle_visual_odometry. It works with a cube orange flight controller, but I cannot switch in position (local position estimation not valid) with a Pixhawk 6c. Both boards are flashed with the latest release. I am wondering if it could be related to the CONFIG_EKF2_EXTERNAL_VISION not being set to true in the bootloader. Any suggestions? Someone made it work?

Thanks!

PX4 / PX4-Autopilot