Mavros: 在Ardupilot固件中使用fake_gps插件和vision_pose_estimation插件

发行细节

我想通过使用车载摄像头进行姿势估计来在室内控制无人机（ SkyViper ）。 实际上，我正在寻找一个包含许多ArUco标记的场景，并使用opencv的aruco检测和姿势估计算法来估计无人机的姿势。 长话短说，我正在使用视觉估计无人机的6维姿势。 现在，我打算将此数据发送到FCU，以便在室内以引导模式控制无人机。 使用arducopter固件，显然我可以做两件事：
一种。 使用fake_pgs插件。
b。 使用vision_pose_estimate插件
但是我都遇到了问题。 我想描述一下我为上述两种方法所做的事情，以便读者在遇到错误的情况下为我提供帮助。

一种。 使用fake_gps

1. 在我的apm_config.yaml中添加了fake_gps插件。
2. 编辑了apm_pluginlists.yaml，这是fake_gps部分的外观：

# fake_gps
fake_gps:
  # select data source
  use_mocap: true       # ~mocap/pose
  mocap_transform: true   # ~mocap/tf instead of pose
  use_vision: false       # ~vision (pose)
  # origin (default: Zürich)
  geo_origin:
    lat: 30.6197674          # latitude [degrees]
    lon:  -96.3414215          # longitude [degrees]
    alt: 99.9674829152          # altitude (height over the WGS-84 ellipsoid) [meters]
  eph: 2.0
  epv: 2.0
  satellites_visible: 10   # virtual number of visible satellites
  fix_type: 3             # type of GPS fix (default: 3D)
  tf:
    listen: false
    send: false           # send TF?
    frame_id: "map"       # TF frame_id
    child_frame_id: "Quadrotor_pose" # TF child_frame_id
    rate_limit: 10.0      # TF rate
  gps_rate: 5.0           # GPS data publishing rate

1. 我正在以约4 Hz的频率在rostopic / mavros / fake_gps / mocap / tf上发布机器人姿势（我可以将其增加到30 Hz，但我认为gps_rate参数会限制它，顺便说一句我可以增加gps_rate吗？）
2. 我回声/ mavros / global_position / global我看不到任何有意义的输出，

任何人都可以在这里帮助我，以使此功能正常工作吗？ 我还有什么需要做的吗？ 我必须设置或取消设置的任何参数吗？

b。 使用vision_pose

1. 在我的apm_config.yaml中添加了fake_gps插件。
2. 编辑了apm_pluginlists.yaml，这是vision_pose部分的外观：

# vision_pose_estimate
vision_pose:
  tf:
    listen: false           # enable tf listener (disable topic subscribers)
    frame_id: "map"
    child_frame_id: "vision_estimate"
    rate_limit: 10.0

1. 我在rostopic / mavros / vision_pose / pose上发布机器人姿势
2. 我在vision_position_estimate.cpp中进行了一些修改，并使用VISION_POSITION_DELTA而不是VISION_POSITION_ESTIMATE 。
3. 我在上面的代码中设置了适当的标志，以查看是否执行了该代码并打印了这些标志，因此，我得出结论说，mavros正在将数据发送到FCU，但FCU端可能没有接收到信号。

在此花费了大量时间，非常感谢您的帮助！！
PS我能够在PX4固件中执行vision_position_estimate，还没有在那里尝试过fake_gps，但是由于某些限制，我想在Arducopter固件上进行这项工作。

MAVROS版本和平台

马夫罗斯：0.18
ROS：动能
的Ubuntu的：16.04

自动驾驶仪的类型和版本

[X] ArduPilot
[] PX4

版本：3.4.6

节点日志

started roslaunch server http://subodh-desktop:41065/

SUMMARY
========

CLEAR PARAMETERS
 * /mavros/

PARAMETERS
 * /mavros/cmd/use_comp_id_system_control: False
 * /mavros/conn/heartbeat_rate: 1.0
 * /mavros/conn/system_time_rate: 0.0
 * /mavros/conn/timeout: 10.0
 * /mavros/conn/timesync_rate: 0.0
 * /mavros/distance_sensor/rangefinder_pub/field_of_view: 0.0
 * /mavros/distance_sensor/rangefinder_pub/frame_id: lidar
 * /mavros/distance_sensor/rangefinder_pub/id: 0
 * /mavros/distance_sensor/rangefinder_pub/send_tf: False
 * /mavros/distance_sensor/rangefinder_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/rangefinder_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/rangefinder_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/rangefinder_sub/id: 1
 * /mavros/distance_sensor/rangefinder_sub/orientation: PITCH_270
 * /mavros/distance_sensor/rangefinder_sub/subscriber: True
 * /mavros/fake_gps/eph: 2.0
 * /mavros/fake_gps/epv: 2.0
 * /mavros/fake_gps/fix_type: 3
 * /mavros/fake_gps/geo_origin/alt: 99.9674829152
 * /mavros/fake_gps/geo_origin/lat: 30.6197674
 * /mavros/fake_gps/geo_origin/lon: -96.3414215
 * /mavros/fake_gps/gps_rate: 5.0
 * /mavros/fake_gps/mocap_transform: True
 * /mavros/fake_gps/satellites_visible: 10
 * /mavros/fake_gps/tf/child_frame_id: Quadrotor_pose
 * /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/ttyACM0:57600
 * /mavros/gcs_url: 
 * /mavros/global_position/child_frame_id: base_link
 * /mavros/global_position/frame_id: map
 * /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.000349065850399
 * /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/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/mocap/use_pose: False
 * /mavros/mocap/use_tf: True
 * /mavros/odometry/estimator_type: 3
 * /mavros/odometry/frame_tf/desired_frame: ned
 * /mavros/plugin_blacklist: ['actuator_contro...
 * /mavros/plugin_whitelist: ['fake_gps', 'vis...
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.118682389136
 * /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_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: NONE
 * /mavros/vibration/frame_id: base_link
 * /mavros/vision_pose/tf/child_frame_id: Quadrotor_pose
 * /mavros/vision_pose/tf/frame_id: map
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: False
 * /rosdistro: kinetic
 * /rosversion: 1.12.12

NODES
  /
    mavros (mavros/mavros_node)

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

setting /run_id to af2a9730-1e56-11e8-86d2-001fc69c99e7
process[rosout-1]: started with pid [26943]
started core service [/rosout]
process[mavros-2]: started with pid [26961]
[ INFO] [1520021829.553095960]: FCU URL: /dev/ttyACM0:57600
[ INFO] [1520021829.554915572]: serial0: device: /dev/ttyACM0 @ 57600 bps
[ INFO] [1520021829.555552991]: GCS bridge disabled
[ INFO] [1520021829.567142748]: Plugin 3dr_radio loaded
[ INFO] [1520021829.569320914]: Plugin 3dr_radio initialized
[ INFO] [1520021829.569349388]: Plugin actuator_control blacklisted
[ INFO] [1520021829.571809744]: Plugin adsb loaded
[ INFO] [1520021829.575769648]: Plugin adsb initialized
[ INFO] [1520021829.575794473]: Plugin altitude blacklisted
[ INFO] [1520021829.575892237]: Plugin cam_imu_sync loaded
[ INFO] [1520021829.576626653]: Plugin cam_imu_sync initialized
[ INFO] [1520021829.576772377]: Plugin command loaded
[ INFO] [1520021829.582684066]: Plugin command initialized
[ INFO] [1520021829.582712464]: Plugin debug_value blacklisted
[ INFO] [1520021829.582886299]: Plugin distance_sensor loaded
[ INFO] [1520021829.592604674]: Plugin distance_sensor initialized
[ INFO] [1520021829.592748090]: Plugin fake_gps loaded
[ INFO] [1520021829.609103603]: Plugin fake_gps initialized
[ INFO] [1520021829.609128115]: Plugin ftp blacklisted
[ INFO] [1520021829.609259829]: Plugin global_position loaded
[ INFO] [1520021829.628580110]: Plugin global_position initialized
[ INFO] [1520021829.628612736]: Plugin hil blacklisted
[ INFO] [1520021829.628766952]: Plugin home_position loaded
[ INFO] [1520021829.633001155]: Plugin home_position initialized
[ INFO] [1520021829.633140461]: Plugin imu loaded
[ INFO] [1520021829.640533947]: Plugin imu initialized
[ INFO] [1520021829.640671494]: Plugin local_position loaded
[ INFO] [1520021829.646783905]: Plugin local_position initialized
[ INFO] [1520021829.646948100]: Plugin manual_control loaded
[ INFO] [1520021829.650807807]: Plugin manual_control initialized
[ INFO] [1520021829.650943083]: Plugin mocap_pose_estimate loaded
[ INFO] [1520021829.655518964]: Plugin mocap_pose_estimate initialized
[ INFO] [1520021829.655642737]: Plugin obstacle_distance loaded
[ INFO] [1520021829.659014223]: Plugin obstacle_distance initialized
[ INFO] [1520021829.659130864]: Plugin odom loaded
[ INFO] [1520021829.663794739]: Plugin odom initialized
[ INFO] [1520021829.664012214]: Plugin param loaded
[ INFO] [1520021829.667288846]: Plugin param initialized
[ INFO] [1520021829.667312025]: Plugin px4flow blacklisted
[ INFO] [1520021829.667423654]: Plugin rangefinder loaded
[ INFO] [1520021829.668137088]: Plugin rangefinder initialized
[ INFO] [1520021829.668297342]: Plugin rc_io loaded
[ INFO] [1520021829.672569622]: Plugin rc_io initialized
[ INFO] [1520021829.672594510]: Plugin safety_area blacklisted
[ INFO] [1520021829.672725799]: Plugin setpoint_accel loaded
[ INFO] [1520021829.676462007]: Plugin setpoint_accel initialized
[ INFO] [1520021829.676710869]: Plugin setpoint_attitude loaded
[ INFO] [1520021829.689281332]: Plugin setpoint_attitude initialized
[ INFO] [1520021829.689452779]: Plugin setpoint_position loaded
[ INFO] [1520021829.705853198]: Plugin setpoint_position initialized
[ INFO] [1520021829.706010896]: Plugin setpoint_raw loaded
[ INFO] [1520021829.716862466]: Plugin setpoint_raw initialized
[ INFO] [1520021829.717029302]: Plugin setpoint_velocity loaded
[ INFO] [1520021829.723754613]: Plugin setpoint_velocity initialized
[ INFO] [1520021829.723998780]: Plugin sys_status loaded
[ INFO] [1520021829.730640327]: Plugin sys_status initialized
[ INFO] [1520021829.730800888]: Plugin sys_time loaded
[ INFO] [1520021829.734651825]: TM: Timesync mode: NONE
[ INFO] [1520021829.735319038]: Plugin sys_time initialized
[ INFO] [1520021829.735450495]: Plugin vfr_hud loaded
[ INFO] [1520021829.736911489]: Plugin vfr_hud initialized
[ INFO] [1520021829.736935514]: Plugin vibration blacklisted
[ INFO] [1520021829.737033502]: Plugin vision_pose_estimate loaded
[ INFO] [1520021829.746346315]: Plugin vision_pose_estimate initialized
[ INFO] [1520021829.746515765]: Plugin vision_speed_estimate loaded
[ INFO] [1520021829.750239950]: Plugin vision_speed_estimate initialized
[ INFO] [1520021829.750419761]: Plugin waypoint loaded
[ INFO] [1520021829.755638602]: Plugin waypoint initialized
[ INFO] [1520021829.755726057]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1520021829.755751539]: Built-in MAVLink package version: 2018.2.2
[ INFO] [1520021829.755767668]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1520021829.755795346]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1520021829.849907257]: CON: Got HEARTBEAT, connected. FCU: ArduPilotMega / ArduCopter
[ INFO] [1520021830.072218261]: IMU: Raw IMU message used.
[ INFO] [1520021830.075098601]: RC_CHANNELS message detected!
[ WARN] [1520021830.075901836]: TM: Wrong FCU time.
[ WARN] [1520021830.572875238]: GP: No GPS fix
[ INFO] [1520021830.859501557]: VER: 1.1: Capabilities         0x0000000000003bcf
[ INFO] [1520021830.859610755]: VER: 1.1: Flight software:     030406ff (e707341bde6b667d8c965992)
[ INFO] [1520021830.859692086]: VER: 1.1: Middleware software: 00000000 (de6b667d8c965992)
[ INFO] [1520021830.859811288]: VER: 1.1: OS software:         00000000 (8c965992)
[ INFO] [1520021830.859894423]: VER: 1.1: Board hardware:      00000000
[ INFO] [1520021830.859991754]: VER: 1.1: VID/PID:             0000:0000
[ INFO] [1520021830.860091685]: VER: 1.1: UID:                 0000000000000000
[ WARN] [1520021830.860457816]: CMD: Unexpected command 520, result 0
[ERROR] [1520021833.850124648]: FCU: PreArm: RC not calibrated
[ERROR] [1520021833.850282817]: FCU: PreArm: Compass offsets too high
[ INFO] [1520021839.850783561]: HP: requesting home position
[ INFO] [1520021839.853771718]: FCU: APM:Copter V3.4.6 (e707341b)
[ INFO] [1520021839.854040516]: FCU: PX4: de6b667d NuttX: 8c965992
[ INFO] [1520021839.854183948]: FCU: Frame: QUAD
[ INFO] [1520021839.854286161]: FCU: PX4v2 0034002F 31334705 39343031
[ INFO] [1520021840.469965330]: PR: parameters list received
[ INFO] [1520021844.857490824]: WP: mission received
[ INFO] [1520021849.851115121]: HP: requesting home position
[ INFO] [1520021859.850746668]: HP: requesting home position
[ WARN] [1520021860.684726116]: GP: No GPS fix
[ERROR] [1520021863.861805572]: FCU: PreArm: RC not calibrated
[ERROR] [1520021863.861953300]: FCU: PreArm: Compass offsets too high

诊断程序

header: 
  seq: 59
  stamp: 
    secs: 1520021901
    nsecs: 257080897
  frame_id: ''
status: 
  - 
    level: 0
    name: "mavros: FCU connection"
    message: "connected"
    hardware_id: "/dev/ttyACM0:57600"
    values: 
      - 
        key: "Received packets:"
        value: "6240"
      - 
        key: "Dropped packets:"
        value: "0"
      - 
        key: "Buffer overruns:"
        value: "0"
      - 
        key: "Parse errors:"
        value: "0"
      - 
        key: "Rx sequence number:"
        value: "211"
      - 
        key: "Tx sequence number:"
        value: "82"
      - 
        key: "Rx total bytes:"
        value: "182353"
      - 
        key: "Tx total bytes:"
        value: "1883"
      - 
        key: "Rx speed:"
        value: "2106.000000"
      - 
        key: "Tx speed:"
        value: "21.000000"
  - 
    level: 2
    name: "mavros: GPS"
    message: "No satellites"
    hardware_id: "/dev/ttyACM0:57600"
    values: 
      - 
        key: "Satellites visible"
        value: "0"
      - 
        key: "Fix type"
        value: "0"
      - 
        key: "EPH (m)"
        value: "0.00"
      - 
        key: "EPV (m)"
        value: "0.00"
  - 
    level: 0
    name: "mavros: Heartbeat"
    message: "Normal"
    hardware_id: "/dev/ttyACM0:57600"
    values: 
      - 
        key: "Heartbeats since startup"
        value: "72"
      - 
        key: "Frequency (Hz)"
        value: "1.037047"
      - 
        key: "Vehicle type"
        value: "Quadrotor"
      - 
        key: "Autopilot type"
        value: "ArduPilotMega / ArduCopter"
      - 
        key: "Mode"
        value: "STABILIZE"
      - 
        key: "System status"
        value: "Standby"
  - 
    level: 0
    name: "mavros: System"
    message: "Normal"
    hardware_id: "/dev/ttyACM0:57600"
    values: 
      - 
        key: "Sensor present"
        value: "0x0160FC0F"
      - 
        key: "Sensor enabled"
        value: "0x00601C0F"
      - 
        key: "Sensor helth"
        value: "0x0160FC0F"
      - 
        key: "3D gyro"
        value: "Ok"
      - 
        key: "3D accelerometer"
        value: "Ok"
      - 
        key: "3D magnetometer"
        value: "Ok"
      - 
        key: "absolute pressure"
        value: "Ok"
      - 
        key: "3D angular rate control"
        value: "Ok"
      - 
        key: "attitude stabilization"
        value: "Ok"
      - 
        key: "yaw position"
        value: "Ok"
      - 
        key: "AHRS subsystem health"
        value: "Ok"
      - 
        key: "Terrain subsystem health"
        value: "Ok"
      - 
        key: "CPU Load (%)"
        value: "30.1"
      - 
        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: 1
    name: "mavros: Battery"
    message: "Low voltage"
    hardware_id: "/dev/ttyACM0:57600"
    values: 
      - 
        key: "Voltage"
        value: "0.00"
      - 
        key: "Current"
        value: "-0.0"
      - 
        key: "Remaining"
        value: "-1.0"
---

支票编号

OK. I got messages from 1:1.

---
Received 1131 messages, from 1 addresses
sys:comp   list of messages
  1:1     0, 1, 2, 193, 152, 150, 24, 27, 29, 30, 33, 163, 36, 165, 42, 178, 182, 62, 65, 74, 77, 241, 116, 125