Привет,
Я делаю управление на основе зрения, а камера на дроне смотрит вниз.
Таким образом, оценка положения находится в кадре тела.
Я обнаружил, что тему / mavros / setpoint_velocity / cmd_vel можно использовать для изменения скорости дрона, но, похоже, это в кадре NED.

Есть ли такая тема, как / mavros / setpoint_velocity / cmd_vel, но использовать рамку тела?

Или мне нужно преобразовать мою уставку скорости из корпуса в кадр NED, чтобы реализовать это?

Я попытался найти что-то, что можно было бы использовать, и нашел этот код. 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);

Кажется, это лучшее решение, которое я смог найти.
Я не уверен, происходит ли рыскание только от IMU или от слияния IMU и магнитного поля.
Есть ли у вас какие-нибудь комментарии или идеи по этому поводу?

Версия и платформа МАВРОС

Маврос: 0,17,3
ROS: индиго
Ubuntu: 14.04

Тип и версия автопилота

APM: коптер V3.4-dev (fe724032)

Журналы узлов

bshang2 @ mechatronics-990 : ~ / Simulation / ardupilot / ArduCopter $ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_mark.launch
... вход в /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/roslaunch-mechatronics-990-28079.log
Проверка каталога журнала на предмет использования диска. Это может занять некоторое время.
Нажмите Ctrl-C, чтобы прервать
Завершена проверка использования диска для файла журнала. Использование <1 ГБ.

запущен сервер roslaunch http: // mechatronics

РЕЗЮМЕ

ЯСНЫЕ ПАРАМЕТРЫ

• / mavros /

ПАРАМЕТРЫ

• / mavros / cmd / use_comp_id_system_control: ложь
• / 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 / 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: ложь
• / mavros / изображение / frame_id: px4flow
• / mavros / imu / angular_velocity_stdev: 0.000349065850399
• / mavros / imu / frame_id: fcu
• / mavros / imu / linear_acceleration_stdev: 0,0003
• / mavros / imu / magnet_stdev: 0,0
• / mavros / imu / direction_stdev: 1.0
• / mavros / local_position / frame_id: fcu
• / mavros / local_position / tf / child_frame_id: базовая_ссылка
• / mavros / local_position / tf / frame_id: мир
• / mavros / local_position / tf / send: Истина
• / mavros / mission / pull_after_gcs: Верно
• / mavros / mocap / use_pose: Верно
• / mavros / mocap / use_tf: Ложь
• / mavros / plugin_blacklist: ['actator_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 / зона_безопасности / p1 / x: 1.0
• / mavros / зона_безопасности / p1 / y: 1.0
• / mavros / зона_безопасности / p1 / z: 1.0
• / mavros / зона_безопасности / p2 / x: -1,0
• / mavros / зона_безопасности / p2 / y: -1,0
• / mavros / зона_безопасности / p2 / z: -1,0
• / mavros / setpoint_accel / send_force: ложь
• / mavros / setpoint_attitude / reverse_throttle: Ложь
• / mavros / setpoint_attitude / tf / child_frame_id: отношение
• / mavros / setpoint_attitude / tf / frame_id: local_origin
• / mavros / setpoint_attitude / tf / listen: Ложь
• / mavros / setpoint_attitude / tf / rate_limit: 10.0
• / mavros / setpoint_position / tf / child_frame_id: установленное значение
• / mavros / setpoint_position / tf / frame_id: local_origin
• / mavros / setpoint_position / tf / listen: Ложь
• / mavros / заданное_положение / tf / rate_limit: 50.0
• / mavros / startup_px4_usb_quirk: Ложь
• / mavros / sys / disable_diag: ложь
• / mavros / sys / min_voltage: 10.0
• / mavros / target_component_id: 1
• / mavros / target_system_id: 1
• / mavros / tdr_radio / low_rssi: 40
• / mavros / время / time_ref_source: fcu
• / mavros / время / timesync_avg_alpha: 0,6
• / mavros / вибрация / frame_id: вибрация
• / mavros / vision_pose / tf / child_frame_id: видение
• / mavros / vision_pose / tf / frame_id: local_origin
• / mavros / vision_pose / tf / listen: ложь
• / mavros / vision_pose / tf / rate_limit: 10.0
• / mavros / vision_speed / listen_twist: ложь
• / robot_description:
• / росдистро: индиго
• / rosversion: 1.11.21
• / tf_prefix:
• / use_sim_time: Верно

УЗЛЫ
/
беседка (gazebo_ros / gzserver)
gazebo_gui (gazebo_ros / gzclient)
mavros (mavros / mavros_node)
spawn_erlecopter (gazebo_ros / spawn_model)

автоматический запуск нового мастера
процесс [master]: запущен с pid [28094]
ROS_MASTER_URI = http: // локальный : 11311

установка / run_id на ca9213bc-89bf-11e7-b657-1803733b65bb
процесс [rosout-1]: запущен с pid [28107]
запущена основная служба [/ rosout]
процесс [mavros-2]: запущен с pid [28126]
процесс [spawn_erlecopter-3]: запущен с pid [28133]
процесс [gazebo-4]: запущен с pid [28134]
процесс [gazebo_gui-5]: запущен с pid [28151]
запущен скрипт spawn_model
[gazebo-4] процесс умер [pid 28134, код выхода 255, cmd / home / bshang2 / simulation / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / gzserver -u -e ode / home / bshang2 / Simulation / ros_catrkin_ws / /ardupilot_sitl_gazebo_plugin/ardupilot_sitl_gazebo_plugin/worlds/mark_world/mark.world __name: = gazebo __log: = / home / bshang2 / .ros / log / ca9213bc-89bf-11e7-b657-180log4.
файл журнала: /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/gazebo-4 .log[INFO] [WallTime: 1503684250.172508] [0.000000] Загрузка модели xml из параметра ros[INFO] [WallTime: 1503684250.176886] [0.000000] Ожидание обслуживания / беседка / spawn_urdf_model[gazebo_gui-5] процесс умер [pid 28151, код выхода 255, cmd / home / bshang2 / Simulation / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / gzclient __name: = gazebo_gui __log: = / home / bshang2 / log / ca9213bc-89bf-11e7-b657-1803733b65bb / gazebo_gui-5.log].файл журнала: /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/gazebo_gui-5 .log
^ C [spawn_erlecopter-3] убийство при выходе
[mavros-2] убийство на выходе
Отслеживание (последний вызов последний):
Файл "/ home / bshang2 / Simulation / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / spawn_model", строка 301, в
sm.callSpawnService ()
Файл "/ home / bshang2 / Simulation / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / spawn_model", строка 267, в callSpawnService
начальная_позиция, self.reference_frame, self.gazebo_namespace)
Файл "/home/bshang2/simulation/ros_catkin_ws/src/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/gazebo_interface.py", строка 28, в spawn_urdf_model_client
rospy.wait_for_service (gazebo_namespace + '/ spawn_urdf_model')
Файл "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", строка 159, в wait_for_service
поднять ROSInterruptException ("быстрое завершение работы")
rospy.exceptions.ROSInterruptException: отключение rospy
^ C [rosout-1] убийство при выходе
^ C [хозяин] убивает при выходе
выключение монитора обработки ...
... завершение работы монитора обработки
Выполнено
bshang2 @ mechatronics-990 : ~ / Simulation / ardupilot / ArduCopter $ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_mark.launch
... вход в /home/bshang2/.ros/log/e483c856-89bf-11e7-bb61-1803733b65bb/roslaunch-mechatronics-990-28603.log
Проверка каталога журнала на предмет использования диска. Это может занять некоторое время.
Нажмите Ctrl-C, чтобы прервать
Завершена проверка использования диска для файла журнала. Использование <1 ГБ.

запущен сервер roslaunch http: // mechatronics

РЕЗЮМЕ

ЯСНЫЕ ПАРАМЕТРЫ

• / mavros /

ПАРАМЕТРЫ

• / mavros / cmd / use_comp_id_system_control: ложь
• / 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 / 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: ложь
• / mavros / изображение / frame_id: px4flow
• / mavros / imu / angular_velocity_stdev: 0.000349065850399
• / mavros / imu / frame_id: fcu
• / mavros / imu / linear_acceleration_stdev: 0,0003
• / mavros / imu / magnet_stdev: 0,0
• / mavros / imu / direction_stdev: 1.0
• / mavros / local_position / frame_id: fcu
• / mavros / local_position / tf / child_frame_id: базовая_ссылка
• / mavros / local_position / tf / frame_id: мир
• / mavros / local_position / tf / send: Истина
• / mavros / mission / pull_after_gcs: Верно
• / mavros / mocap / use_pose: Верно
• / mavros / mocap / use_tf: Ложь
• / mavros / plugin_blacklist: ['actator_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 / зона_безопасности / p1 / x: 1.0
• / mavros / зона_безопасности / p1 / y: 1.0
• / mavros / зона_безопасности / p1 / z: 1.0
• / mavros / зона_безопасности / p2 / x: -1,0
• / mavros / зона_безопасности / p2 / y: -1,0
• / mavros / зона_безопасности / p2 / z: -1,0
• / mavros / setpoint_accel / send_force: ложь
• / mavros / setpoint_attitude / reverse_throttle: Ложь
• / mavros / setpoint_attitude / tf / child_frame_id: отношение
• / mavros / setpoint_attitude / tf / frame_id: local_origin
• / mavros / setpoint_attitude / tf / listen: Ложь
• / mavros / setpoint_attitude / tf / rate_limit: 10.0
• / mavros / setpoint_position / tf / child_frame_id: установленное значение
• / mavros / setpoint_position / tf / frame_id: local_origin
• / mavros / setpoint_position / tf / listen: Ложь
• / mavros / заданное_положение / tf / rate_limit: 50.0
• / mavros / startup_px4_usb_quirk: Ложь
• / mavros / sys / disable_diag: ложь
• / mavros / sys / min_voltage: 10.0
• / mavros / target_component_id: 1
• / mavros / target_system_id: 1
• / mavros / tdr_radio / low_rssi: 40
• / mavros / время / time_ref_source: fcu
• / mavros / время / timesync_avg_alpha: 0,6
• / mavros / вибрация / frame_id: вибрация
• / mavros / vision_pose / tf / child_frame_id: видение
• / mavros / vision_pose / tf / frame_id: local_origin
• / mavros / vision_pose / tf / listen: ложь
• / mavros / vision_pose / tf / rate_limit: 10.0
• / mavros / vision_speed / listen_twist: ложь
• / robot_description:
• / росдистро: индиго
• / rosversion: 1.11.21
• / tf_prefix:
• / use_sim_time: Верно

УЗЛЫ
/
беседка (gazebo_ros / gzserver)
gazebo_gui (gazebo_ros / gzclient)
mavros (mavros / mavros_node)
spawn_erlecopter (gazebo_ros / spawn_model)

автоматический запуск нового мастера
процесс [master]: запущен с pid [28618]
ROS_MASTER_URI = http: // локальный : 11311

установка / run_id на e483c856-89bf-11e7-bb61-1803733b65bb
процесс [rosout-1]: запущен с pid [28631]
запущена основная служба [/ rosout]
процесс [mavros-2]: запущен с pid [28656]
процесс [spawn_erlecopter-3]: запущен с pid [28657]
процесс [gazebo-4]: запущен с pid [28674]
процесс [gazebo_gui-5]: запущен с pid [28678]
запущен скрипт spawn_model
[INFO] [WallTime: 1503684293.653120] [0.000000] Загрузка модели xml из параметра ros
[INFO] [WallTime: 1503684293.657344] [0.000000] Ожидание обслуживания / беседка / spawn_urdf_model
[ИНФОРМАЦИЯ] [WallTime: 1503684294.260952] [0.047500] Служба звонков / беседка / spawn_urdf_model
[INFO] [WallTime: 1503684294.855628] [0.445000] Статус создания: SpawnModel: Успешно созданная модель
[spawn_erlecopter-3] процесс успешно завершен
файл журнала: /home/bshang2/.ros/log/e483c856-89bf-11e7-bb61-1803733b65bb/spawn_erlecopter-3*.log

Диагностика

bshang2 @ mechatronics-990 : ~ / имитация / ardupilot / ArduCopter $ rostopic echo -n1 / диагностика
заголовок:
seq: 6456
печать:
секунд: 6817
нсек: 500000000
frame_id: ''
статус:

• 
  уровень: 0
  имя: mavros: подключение FCU
  сообщение: подключено
  hardware_id: tcp: //127.0.0.1 : 5760
  значения:
  
  
  
  • 
    
    ключ: Полученные пакеты:
    
    значение: 24635
  
  
  • 
    
    ключ: Отброшенные пакеты:
    
    значение: 0
  
  
  • 
    
    клавиша: Переполнение буфера:
    
    значение: 0
  
  
  • 
    
    ключ: Ошибки синтаксического анализа:
    
    значение: 0
  
  
  • 
    
    клавиша: порядковый номер Rx:
    
    значение: 58
  
  
  • 
    
    клавиша: порядковый номер Tx:
    
    значение: 190
  
  
  • 
    
    ключ: Rx всего байтов:
    
    значение: 19770100
  
  
  • 
    
    ключ: Всего байтов Tx:
    
    значение: 169295
  
  
  • 
    
    ключ: скорость Rx:
    
    значение: inf
  
  
  • 
    
    ключ: скорость передачи:
    
    значение: inf
  
  
• 
  уровень: 0
  имя: mavros: мост GCS
  сообщение: подключено
  hardware_id: tcp: //127.0.0.1 : 5760
  значения:
  
  
  
  • 
    
    ключ: Полученные пакеты:
    
    значение: 3059
  
  
  • 
    
    ключ: Отброшенные пакеты:
    
    значение: 0
  
  
  • 
    
    клавиша: Переполнение буфера:
    
    значение: 0
  
  
  • 
    
    ключ: Ошибки синтаксического анализа:
    
    значение: 0
  
  
  • 
    
    клавиша: порядковый номер Rx:
    
    значение: 243
  
  
  • 
    
    клавиша: порядковый номер Tx:
    
    значение: 0
  
  
  • 
    
    ключ: Rx всего байтов:
    
    значение: 51809
  
  
  • 
    
    ключ: Всего байтов Tx:
    
    значение: 19769923
  
  
  • 
    
    ключ: скорость Rx:
    
    значение: inf
  
  
  • 
    
    ключ: скорость передачи:
    
    значение: inf
  
  
• 
  уровень: 0
  имя: mavros: GPS
  сообщение: 3D fix
  hardware_id: tcp: //127.0.0.1 : 5760
  значения:
  
  
  
  • 
    
    клавиша: спутники видны
    
    значение: 10
  
  
  • 
    
    ключ: тип исправления
    
    значение: 3
  
  
  • 
    
    ключ: EPH (м)
    
    значение: 1.21
  
  
  • 
    
    ключ: EPV (м)
    
    значение: 2.00
  
  
• 
  уровень: 0
  имя: mavros: Heartbeat
  сообщение: Нормальный
  hardware_id: tcp: //127.0.0.1 : 5760
  значения:
  
  
  
  • 
    
    ключ: Сердцебиение с момента запуска
    
    значение: 6817
  
  
  • 
    
    клавиша: частота (Гц)
    
    значение: 0.952381
  
  
  • 
    
    ключ: Тип транспортного средства
    
    значение: Quadrotor
  
  
  • 
    
    ключ: Тип автопилота
    
    значение: ArduPilotMega
  
  
  • 
    
    ключ: режим
    
    значение: GUIDED
  
  
  • 
    
    клавиша: Статус системы
    
    значение: Активный
  
  
• 
  уровень: 0
  имя: mavros: Система
  сообщение: Нормальный
  hardware_id: tcp: //127.0.0.1 : 5760
  значения:
  
  
  
  • 
    
    клавиша: Датчик присутствует
    
    значение: 0x0061FD2F
  
  
  • 
    
    клавиша: датчик включен
    
    значение: 0x0061FD2F
  
  
  • 
    
    ключ: датчик helth
    
    значение: 0x0061FD2F
  
  
  • 
    
    ключ: Датчик 3D гироскопа
    
    значение: ОК
  
  
  • 
    
    Ключ: Датчик 3D-ускорения
    
    значение: ОК
  
  
  • 
    
    ключ: Датчик 3D Mag
    
    значение: ОК
  
  
  • 
    
    Ключ: Датчик давления абс.
    
    значение: ОК
  
  
  • 
    
    ключ: Датчик GPS
    
    значение: ОК
  
  
  • 
    
    Ключ: Датчик положения лазера
    
    значение: ОК
  
  
  • 
    
    Клавиша: Контроль угловой скорости датчика
    
    значение: ОК
  
  
  • 
    
    ключ: Sensor Attitude Stab
    
    значение: ОК
  
  
  • 
    
    Клавиша: Датчик положения рысканья
    
    значение: ОК
  
  
  • 
    
    Клавиша: Датчик Z / Alt Control
    
    значение: ОК
  
  
  • 
    
    Клавиша: Датчик положения X / Y
    
    значение: ОК
  
  
  • 
    
    Клавиша: Выходы двигателя датчика
    
    значение: ОК
  
  
  • 
    
    ключ: датчик RC приемник
    
    значение: ОК
  
  
  • 
    
    ключ: здоровье AHRS
    
    значение: ОК
  
  
  • 
    
    ключ: здоровье местности
    
    значение: ОК
  
  
  • 
    
    ключ: загрузка процессора (%)
    
    значение: 0,0
  
  
  • 
    
    ключ: шанс выпадения (%)
    
    значение: 0,0
  
  
  • 
    
    ключ: Ошибки связи
    
    значение: 0
  
  
  • 
    
    ключ: количество ошибок # 1
    
    значение: 0
  
  
  • 
    
    ключ: количество ошибок # 2
    
    значение: 0
  
  
  • 
    
    ключ: количество ошибок # 3
    
    значение: 0
  
  
  • 
    
    ключ: количество ошибок # 4
    
    значение: 0
  
  
• 
  1-й уровень
  имя: mavros: Аккумулятор
  сообщение: Низкое напряжение
  hardware_id: tcp: //127.0.0.1 : 5760
  значения:
  
  
  
  • 
    
    ключ: напряжение
    
    значение: 0.00
  
  
  • 
    
    ключ: Текущий
    
    значение: 0,0
  
  
  • 
    
    ключ: Остающийся
  
  

значение: 100.0

Проверить ID

bshang2 @ mechatronics-990 : ~ / имитация / ardupilot / ArduCopter $ rosrun mavros checkid
ОК. Получал сообщения от 1: 1.

Получено 1231 сообщение с 1 адреса
sys: составить список сообщений
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