Hai,
Saya melakukan kontrol berbasis penglihatan dan kamera pada drone menghadap ke bawah.
Jadi estimasi posisinya ada di body frame.
Saya menemukan topic / mavros / setpoint_velocity / cmd_vel dapat digunakan untuk mengubah kecepatan drone tetapi tampaknya itu dalam bingkai NED.

Apakah ada topik seperti / mavros / setpoint_velocity / cmd_vel tetapi menggunakan body frame?

Atau saya perlu mengubah setpoint kecepatan saya dari bingkai tubuh ke bingkai NED untuk menerapkannya?

Saya mencoba menemukan sesuatu yang dapat saya gunakan dan saya menemukan kode ini. https://github.com/weiweikong/px4_velocity_control_keyboard/blob/master/src/px4_velocity_control_node.cpp

vs_body_axis.twist.linear.x = vs.twist.linear.x * cos(uavYawENU) + vs.twist.linear.y * sin(uavYawENU);
vs_body_axis.twist.linear.y = vs.twist.linear.x * sin(uavYawENU) - vs.twist.linear.y * cos(uavYawENU);

Sepertinya ini adalah solusi terbaik yang bisa saya temukan.
Saya tidak yakin apakah Yaw hanya dari IMU atau dari fusi IMU dan magnet.
Apakah Anda punya komentar atau ide tentang ini?

Versi dan platform MAVROS

Mavros: 0.17.3
ROS: Indigo
Ubuntu: 14.04

Jenis dan versi autopilot

APM: Copter V3.4-dev (fe724032)

Log node

bshang2 @ mechatronics-990 : ~ / simulasi / ardupilot / ArduCopter $ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_mark.launch
... masuk ke /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/roslaunch-mechatronics-990-28079.log
Memeriksa direktori log untuk penggunaan disk. Ini mungkin membutuhkan waktu beberapa saat.
Tekan Ctrl-C untuk menginterupsi
Selesai memeriksa penggunaan disk file log. Penggunaan <1GB.

memulai server roslaunch http: // mechatronics-990 : 45006 /

RINGKASAN

HAPUS PARAMETER

• / mavros /

PARAMETER

• / mavros / cmd / use_comp_id_system_control: Salah
• / mavros / conn / heartbeat_rate: 1.0
• / mavros / samb / system_time_rate: 0.0
• / mavros / samb / batas waktu: 10.0
• / mavros / conn / timesync_rate: 0,0
• / mavros / fcu_url: tcp: //127.0.0.1 : 5760
• / mavros / gcs_url: udp: //127.0.0.1@1 ...
• / mavros / global_position / frame_id: fcu
• / mavros / global_position / rot_covariance: 99999.0
• / mavros / global_position / tf / child_frame_id: fcu_utm
• / mavros / global_position / tf / frame_id: local_origin
• / mavros / global_position / tf / send: Salah
• / mavros / image / frame_id: px4flow
• / mavros / imu / angular_velocity_stdev: 0,000349065850399
• / mavros / imu / frame_id: fcu
• / mavros / imu / linear_acceleration_stdev: 0,0003
• / mavros / imu / magnetic_stdev: 0.0
• / mavros / imu / orientasi_stdev: 1.0
• / mavros / local_position / frame_id: fcu
• / mavros / local_position / tf / child_frame_id: base_link
• / mavros / local_position / tf / frame_id: dunia
• / mavros / local_position / tf / send: Benar
• / mavros / mission / pull_after_gcs: Benar
• / mavros / mocap / use_pose: Benar
• / mavros / mocap / use_tf: Salah
• / mavros / plugin_blacklist: ['actuator_contro ...
• / mavros / plugin_whitelist: []
• / mavros / px4flow / frame_id: px4flow
• / mavros / px4flow / ranger_fov: 0.0
• / 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: Salah
• / mavros / setpoint_attitude / reverse_throttle: Salah
• / mavros / setpoint_attitude / tf / child_frame_id: sikap
• / mavros / setpoint_attitude / tf / frame_id: local_origin
• / mavros / setpoint_attitude / tf / dengarkan: Salah
• / mavros / setpoint_attitude / tf / rate_limit: 10.0
• / mavros / setpoint_position / tf / child_frame_id: setpoint
• / mavros / setpoint_position / tf / frame_id: local_origin
• / mavros / setpoint_position / tf / dengar: Salah
• / mavros / setpoint_position / tf / rate_limit: 50.0
• / mavros / startup_px4_usb_quirk: Salah
• / mavros / sys / disable_diag: Salah
• / mavros / sys / min_voltage: 10.0
• / mavros / target_component_id: 1
• / mavros / target_system_id: 1
• / mavros / tdr_radio / low_rssi: 40
• / mavros / waktu / waktu_ref_source: fcu
• / mavros / time / timesync_avg_alpha: 0,6
• / mavros / vibrasi / frame_id: getaran
• / mavros / vision_pose / tf / child_frame_id: visi
• / mavros / vision_pose / tf / frame_id: local_origin
• / mavros / vision_pose / tf / dengar: Salah
• / mavros / vision_pose / tf / rate_limit: 10.0
• / mavros / vision_speed / listen_twist: Salah
• / robot_description:
• / rosdistro: nila
• / rosversion: 1.11.21
• / tf_prefix:
• / use_sim_time: Benar

NODA
/
gazebo (gazebo_ros / gzserver)
gazebo_gui (gazebo_ros / gzclient)
mavros (mavros / mavros_node)
spawn_erlecopter (gazebo_ros / spawn_model)

mulai otomatis master baru
proses [master]: dimulai dengan pid [28094]
ROS_MASTER_URI = http: // localhost : 11311

pengaturan / run_id ke ca9213bc-89bf-11e7-b657-1803733b65bb
proses [rosout-1]: dimulai dengan pid [28107]
memulai layanan inti [/ rosout]
proses [mavros-2]: dimulai dengan pid [28126]
proses [spawn_erlecopter-3]: dimulai dengan pid [28133]
proses [gazebo-4]: dimulai dengan pid [28134]
proses [gazebo_gui-5]: dimulai dengan pid [28151]
skrip spawn_model dimulai
Proses [gazebo-4] telah mati [pid 28134, kode keluar 255, cmd / home / bshang2 / simulasi / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / gzserver -u -e ode / home / bshang2 / simulasi / ros_catkin_ws / src /ardupilot_sitl_gazebo_plugin/ardupilot_sitl_gazebo_plugin/worlds/mark_world/mark.world __name: = gazebo __log: = / home / bshang2 / .ros / log / ca9213bc-89bf-11bloge7-b657-1803733b65bb].
file log: /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/gazebo-4 .log[INFO] [WallTime: 1503684250.172508] [0,000000] Memuat model xml dari parameter ros[INFO] [WallTime: 1503684250.176886] [0,000000] Menunggu layanan / gazebo / spawn_urdf_modelProses [gazebo_gui-5] telah mati [pid 28151, kode keluar 255, cmd / home / bshang2 / simulasi / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / gzclient __name: = gazebo_gui __log: = / home / bshang2 / .ros / log / ca9213bc-89bf-11e7-b657-1803733b65bb / gazebo_gui-5.log].file log: /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/gazebo_gui-5 .log
^ C [spawn_erlecopter-3] membunuh saat keluar
[mavros-2] membunuh saat keluar
Traceback (panggilan terakhir terakhir):
File "/ home / bshang2 / Simulation / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / spawn_model", baris 301, di
sm.callSpawnService ()
File "/ home / bshang2 / Simulation / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / spawn_model", baris 267, di callSpawnService
initial_pose, self.reference_frame, self.gazebo_namespace)
File "/home/bshang2/simulation/ros_catkin_ws/src/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/gazebo_interface.py", baris 28, di spawn_urdf_model_client
rospy.wait_for_service (gazebo_namespace + '/ spawn_urdf_model')
File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", baris 159, di wait_for_service
naikkan ROSInterruptException ("rospy shutdown")
rospy.exceptions.ROSInterruptException: rospy shutdown
^ C [rosout-1] membunuh saat keluar
^ C [master] membunuh saat keluar
mematikan monitor pemrosesan ...
... mematikan monitor pemrosesan selesai
selesai
bshang2 @ mechatronics-990 : ~ / simulasi / ardupilot / ArduCopter $ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_mark.launch
... masuk ke /home/bshang2/.ros/log/e483c856-89bf-11e7-bb61-1803733b65bb/roslaunch-mechatronics-990-28603.log
Memeriksa direktori log untuk penggunaan disk. Ini mungkin membutuhkan waktu beberapa saat.
Tekan Ctrl-C untuk menginterupsi
Selesai memeriksa penggunaan disk file log. Penggunaan <1GB.

memulai server roslaunch http: // mechatronics-990 : 41224 /

RINGKASAN

HAPUS PARAMETER

• / mavros /

PARAMETER

• / mavros / cmd / use_comp_id_system_control: Salah
• / mavros / conn / heartbeat_rate: 1.0
• / mavros / samb / system_time_rate: 0.0
• / mavros / samb / batas waktu: 10.0
• / mavros / conn / timesync_rate: 0,0
• / mavros / fcu_url: tcp: //127.0.0.1 : 5760
• / mavros / gcs_url: udp: //127.0.0.1@1 ...
• / mavros / global_position / frame_id: fcu
• / mavros / global_position / rot_covariance: 99999.0
• / mavros / global_position / tf / child_frame_id: fcu_utm
• / mavros / global_position / tf / frame_id: local_origin
• / mavros / global_position / tf / send: Salah
• / mavros / image / frame_id: px4flow
• / mavros / imu / angular_velocity_stdev: 0,000349065850399
• / mavros / imu / frame_id: fcu
• / mavros / imu / linear_acceleration_stdev: 0,0003
• / mavros / imu / magnetic_stdev: 0.0
• / mavros / imu / orientasi_stdev: 1.0
• / mavros / local_position / frame_id: fcu
• / mavros / local_position / tf / child_frame_id: base_link
• / mavros / local_position / tf / frame_id: dunia
• / mavros / local_position / tf / send: Benar
• / mavros / mission / pull_after_gcs: Benar
• / mavros / mocap / use_pose: Benar
• / mavros / mocap / use_tf: Salah
• / mavros / plugin_blacklist: ['actuator_contro ...
• / mavros / plugin_whitelist: []
• / mavros / px4flow / frame_id: px4flow
• / mavros / px4flow / ranger_fov: 0.0
• / 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: Salah
• / mavros / setpoint_attitude / reverse_throttle: Salah
• / mavros / setpoint_attitude / tf / child_frame_id: sikap
• / mavros / setpoint_attitude / tf / frame_id: local_origin
• / mavros / setpoint_attitude / tf / dengarkan: Salah
• / mavros / setpoint_attitude / tf / rate_limit: 10.0
• / mavros / setpoint_position / tf / child_frame_id: setpoint
• / mavros / setpoint_position / tf / frame_id: local_origin
• / mavros / setpoint_position / tf / dengar: Salah
• / mavros / setpoint_position / tf / rate_limit: 50.0
• / mavros / startup_px4_usb_quirk: Salah
• / mavros / sys / disable_diag: Salah
• / mavros / sys / min_voltage: 10.0
• / mavros / target_component_id: 1
• / mavros / target_system_id: 1
• / mavros / tdr_radio / low_rssi: 40
• / mavros / waktu / waktu_ref_source: fcu
• / mavros / time / timesync_avg_alpha: 0,6
• / mavros / vibrasi / frame_id: getaran
• / mavros / vision_pose / tf / child_frame_id: visi
• / mavros / vision_pose / tf / frame_id: local_origin
• / mavros / vision_pose / tf / dengar: Salah
• / mavros / vision_pose / tf / rate_limit: 10.0
• / mavros / vision_speed / listen_twist: Salah
• / robot_description:
• / rosdistro: nila
• / rosversion: 1.11.21
• / tf_prefix:
• / use_sim_time: Benar

NODA
/
gazebo (gazebo_ros / gzserver)
gazebo_gui (gazebo_ros / gzclient)
mavros (mavros / mavros_node)
spawn_erlecopter (gazebo_ros / spawn_model)

mulai otomatis master baru
proses [master]: dimulai dengan pid [28618]
ROS_MASTER_URI = http: // localhost : 11311

pengaturan / run_id ke e483c856-89bf-11e7-bb61-1803733b65bb
proses [rosout-1]: dimulai dengan pid [28631]
memulai layanan inti [/ rosout]
proses [mavros-2]: dimulai dengan pid [28656]
proses [spawn_erlecopter-3]: dimulai dengan pid [28657]
proses [gazebo-4]: dimulai dengan pid [28674]
proses [gazebo_gui-5]: dimulai dengan pid [28678]
skrip spawn_model dimulai
[INFO] [WallTime: 1503684293.653120] [0,000000] Memuat model xml dari parameter ros
[INFO] [WallTime: 1503684293.657344] [0,000000] Menunggu layanan / gazebo / spawn_urdf_model
[INFO] [Waktu Dinding: 1503684294.260952] [0,047500] Layanan panggilan / gazebo / spawn_urdf_model
[INFO] [WallTime: 1503684294.855628] [0,445000] Status spawn: SpawnModel: Model berhasil muncul
Proses [spawn_erlecopter-3] telah selesai dengan bersih
file log: /home/bshang2/.ros/log/e483c856-89bf-11e7-bb61-1803733b65bb/spawn_erlecopter-3*.log

Diagnostik

bshang2 @ mechatronics-990 : ~ / simulasi / ardupilot / ArduCopter $ rostopic echo -n1 / diagnostik
tajuk:
seq: 6456
stempel:
detik: 6817
nsecs: 500000000
frame_id: ''
status:

• 
  level: 0
  name: mavros: koneksi FCU
  pesan: terhubung
  hardware_id: tcp: //127.0.0.1 : 5760
  nilai:
  
  
  
  • 
    
    kunci: Paket yang diterima:
    
    nilai: 24635
  
  
  • 
    
    kunci: Paket yang dijatuhkan:
    
    nilai: 0
  
  
  • 
    
    kunci: Buffer overruns:
    
    nilai: 0
  
  
  • 
    
    kunci: Parse error:
    
    nilai: 0
  
  
  • 
    
    kunci: nomor urut Rx:
    
    nilai: 58
  
  
  • 
    
    kunci: Nomor urut Tx:
    
    nilai: 190
  
  
  • 
    
    kunci: Rx total byte:
    
    nilai: 19770100
  
  
  • 
    
    key: Tx total byte:
    
    nilai: 169295
  
  
  • 
    
    kunci: Kecepatan rx:
    
    nilai: inf
  
  
  • 
    
    kunci: kecepatan Tx:
    
    nilai: inf
  
  
• 
  level: 0
  nama: mavros: jembatan GCS
  pesan: terhubung
  hardware_id: tcp: //127.0.0.1 : 5760
  nilai:
  
  
  
  • 
    
    kunci: Paket yang diterima:
    
    nilai: 3059
  
  
  • 
    
    kunci: Paket yang dijatuhkan:
    
    nilai: 0
  
  
  • 
    
    kunci: Buffer overruns:
    
    nilai: 0
  
  
  • 
    
    kunci: Parse error:
    
    nilai: 0
  
  
  • 
    
    kunci: nomor urut Rx:
    
    nilai: 243
  
  
  • 
    
    kunci: Nomor urut Tx:
    
    nilai: 0
  
  
  • 
    
    kunci: Rx total byte:
    
    nilai: 51809
  
  
  • 
    
    key: Tx total byte:
    
    nilai: 19769923
  
  
  • 
    
    kunci: Kecepatan rx:
    
    nilai: inf
  
  
  • 
    
    kunci: kecepatan Tx:
    
    nilai: inf
  
  
• 
  level: 0
  nama: mavros: GPS
  pesan: Perbaikan 3D
  hardware_id: tcp: //127.0.0.1 : 5760
  nilai:
  
  
  
  • 
    
    kunci: Satelit terlihat
    
    nilai: 10
  
  
  • 
    
    kunci: Jenis perbaikan
    
    nilai: 3
  
  
  • 
    
    kunci: EPH (m)
    
    nilai: 1,21
  
  
  • 
    
    kunci: EPV (m)
    
    nilai: 2.00
  
  
• 
  level: 0
  nama: mavros: Detak Jantung
  pesan: Normal
  hardware_id: tcp: //127.0.0.1 : 5760
  nilai:
  
  
  
  • 
    
    kunci: Detak jantung sejak startup
    
    nilai: 6817
  
  
  • 
    
    kunci: Frekuensi (Hz)
    
    nilai: 0,952381
  
  
  • 
    
    kunci: Jenis kendaraan
    
    nilai: Quadrotor
  
  
  • 
    
    kunci: Jenis autopilot
    
    nilai: ArduPilotMega
  
  
  • 
    
    kunci: Mode
    
    nilai: GUIDED
  
  
  • 
    
    kunci: Status sistem
    
    nilai: Aktif
  
  
• 
  level: 0
  nama: mavros: Sistem
  pesan: Normal
  hardware_id: tcp: //127.0.0.1 : 5760
  nilai:
  
  
  
  • 
    
    kunci: Sensor hadir
    
    nilai: 0x0061FD2F
  
  
  • 
    
    kunci: Sensor diaktifkan
    
    nilai: 0x0061FD2F
  
  
  • 
    
    Kunci: Sensor helth
    
    nilai: 0x0061FD2F
  
  
  • 
    
    Kunci: Sensor 3D Gyro
    
    nilai: Ok
  
  
  • 
    
    Kunci: Sensor 3D Accel
    
    nilai: Ok
  
  
  • 
    
    Kunci: Sensor 3D Mag
    
    nilai: Ok
  
  
  • 
    
    Kunci: Sensor Abs Pressure
    
    nilai: Ok
  
  
  • 
    
    Kunci: Sensor GPS
    
    nilai: Ok
  
  
  • 
    
    kunci: Posisi Laser Sensor
    
    nilai: Ok
  
  
  • 
    
    kunci: Sensor Ang Rate Control
    
    nilai: Ok
  
  
  • 
    
    Kunci: Sensor Sikap Stab
    
    nilai: Ok
  
  
  • 
    
    kunci: Posisi Sensor Yaw
    
    nilai: Ok
  
  
  • 
    
    Kunci: Kontrol Sensor Z / Alt
    
    nilai: Ok
  
  
  • 
    
    Kunci: Kontrol Pos Sensor X / Y
    
    nilai: Ok
  
  
  • 
    
    kunci: Keluaran Motor Sensor
    
    nilai: Ok
  
  
  • 
    
    Kunci: Penerima RC Sensor
    
    nilai: Ok
  
  
  • 
    
    kunci: kesehatan AHRS
    
    nilai: Ok
  
  
  • 
    
    kunci: Kesehatan medan
    
    nilai: Ok
  
  
  • 
    
    kunci: Beban CPU (%)
    
    nilai: 0.0
  
  
  • 
    
    kunci: Tingkat penurunan (%)
    
    nilai: 0.0
  
  
  • 
    
    key: Error comm
    
    nilai: 0
  
  
  • 
    
    kunci: Kesalahan menghitung # 1
    
    nilai: 0
  
  
  • 
    
    kunci: Kesalahan menghitung # 2
    
    nilai: 0
  
  
  • 
    
    kunci: Kesalahan menghitung # 3
    
    nilai: 0
  
  
  • 
    
    kunci: Kesalahan menghitung # 4
    
    nilai: 0
  
  
• 
  tingkat 1
  nama: mavros: Baterai
  pesan: Tegangan rendah
  hardware_id: tcp: //127.0.0.1 : 5760
  nilai:
  
  
  
  • 
    
    kunci: Voltase
    
    nilai: 0,00
  
  
  • 
    
    kunci: Arus
    
    nilai: 0.0
  
  
  • 
    
    kunci: Sisa
  
  

nilai: 100.0

Periksa ID

bshang2 @ mechatronics-990 : ~ / simulasi / ardupilot / ArduCopter $ rosrun mavros checkid
BAIK. Saya mendapat pesan dari 1: 1.

Menerima 1231 pesan, dari 1 alamat
sys: daftar
1: 1 0, 1, 2, 136, 152, 150, 24, 27, 29, 30, 32, 33, 162, 163, 164, 165, 42, 173, 178, 62, 193, 74, 35, 36 , 241, 116