Oi,
Estou fazendo um controle baseado na visão e a câmera está voltada para baixo no drone.
Portanto, a estimativa da posição está na estrutura corporal.
Descobri que o tópico / mavros / setpoint_velocity / cmd_vel pode ser usado para alterar a velocidade do drone, mas parece que está no quadro do NED.

Existe um tópico como / mavros / setpoint_velocity / cmd_vel, mas usar moldura corporal?

Ou preciso converter meu ponto de ajuste de velocidade da estrutura do corpo para a estrutura NED para implementá-lo?

Tentei encontrar algo que possa usar e encontrei este código. 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);

Parece que essa é a melhor solução que consegui encontrar.
Não tenho certeza se o Yaw é apenas de IMU ou da fusão de IMU e magnético.
Você tem algum comentário ou ideia sobre isso?

Versão e plataforma MAVROS

Mavros: 0.17.3
ROS: Indigo
Ubuntu: 14.04

Tipo e versão do piloto automático

APM: Copter V3.4-dev (fe724032)

Registros de nó

bshang2 @ mechatronics-990 : ~ / simulação / ardupilot / ArduCopter $ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_mark.launch
... registrando em /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/roslaunch-mechatronics-990-28079.log
Verificando o diretório de log para uso do disco. Isso pode demorar um pouco.
Pressione Ctrl-C para interromper
Concluída a verificação do uso do disco do arquivo de log. O uso é <1 GB.

servidor roslaunch iniciado http: // mechatronics-990 : 45006 /

RESUMO

PARÂMETROS CLAROS

• / mavros /

PARÂMETROS

• / 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 / 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: False
• / 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 / magnet_stdev: 0,0
• / mavros / imu /idance_stdev: 1.0
• / mavros / local_position / frame_id: fcu
• / mavros / local_position / tf / child_frame_id: base_link
• / mavros / local_position / tf / frame_id: world
• / mavros / local_position / tf / send: Verdadeiro
• / mavros / mission / pull_after_gcs: True
• / mavros / mocap / use_pose: Verdadeiro
• / mavros / mocap / use_tf: False
• / mavros / plugin_blacklist: ['atuator_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: False
• / mavros / setpoint_attitude / reverse_throttle: False
• / mavros / setpoint_attitude / tf / child_frame_id: atitude
• / mavros / setpoint_attitude / tf / frame_id: local_origin
• / mavros / setpoint_attitude / tf / listen: False
• / 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 / listen: False
• / mavros / setpoint_position / tf / rate_limit: 50.0
• / 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 / vibration / frame_id: vibração
• / mavros / vision_pose / tf / child_frame_id: vision
• / mavros / vision_pose / tf / frame_id: local_origin
• / mavros / vision_pose / tf / listen: False
• / mavros / vision_pose / tf / rate_limit: 10.0
• / mavros / vision_speed / listen_twist: False
• / robot_description:
• / rosdistro: indigo
• / rosversion: 1.11.21
• / tf_prefix:
• / use_sim_time: Verdadeiro

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

auto-iniciar novo mestre
processo [master]: iniciado com pid [28094]
ROS_MASTER_URI = http: // localhost : 11311

configuração / run_id para ca9213bc-89bf-11e7-b657-1803733b65bb
processo [rosout-1]: iniciado com pid [28107]
iniciou o serviço principal [/ rosout]
processo [mavros-2]: iniciado com pid [28126]
processo [spawn_erlecopter-3]: iniciado com pid [28133]
processo [gazebo-4]: iniciado com pid [28134]
processo [gazebo_gui-5]: iniciado com pid [28151]
script spawn_model iniciado
processo [gazebo-4] morreu [pid 28134, código de saída 255, cmd / home / bshang2 / simulação / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / gzserver -u -e ode / home / bshang2 / simulação / 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-11e7-b657-1803733b65bb / gazebo].
arquivo de log: /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/gazebo-4 .log[INFO] [WallTime: 1503684250.172508] [0,000000] Carregando modelo xml do parâmetro ros[INFO] [WallTime: 1503684250.176886] [0,000000] Aguardando serviço / gazebo / spawn_urdf_modelprocesso [gazebo_gui-5] morreu [pid 28151, código de saída 255, cmd / home / bshang2 / simulação / 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].arquivo de registro: /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/gazebo_gui-5 .log
^ C [spawn_erlecopter-3] matando na saída
[mavros-2] matando na saída
Traceback (última chamada mais recente):
Arquivo "/ home / bshang2 / simulação / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / spawn_model", linha 301, em
sm.callSpawnService ()
Arquivo "/ home / bshang2 / simulação / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / spawn_model", linha 267, em callSpawnService
proposição_inicial, self.reference_frame, self.gazebo_namespace)
Arquivo "/home/bshang2/simulation/ros_catkin_ws/src/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/gazebo_interface.py", linha 28, em spawn_urdf_model_client
rospy.wait_for_service (gazebo_namespace + '/ spawn_urdf_model')
Arquivo "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", linha 159, em wait_for_service
aumentar ROSInterruptException ("desligamento rospy")
rospy.exceptions.ROSInterruptException: rospy shutdown
^ C [rosout-1] matando na saída
^ C [master] matando na saída
desligando monitor de processamento ...
... desligando o monitor de processamento completo
feito
bshang2 @ mechatronics-990 : ~ / simulação / ardupilot / ArduCopter $ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_mark.launch
... registrando em /home/bshang2/.ros/log/e483c856-89bf-11e7-bb61-1803733b65bb/roslaunch-mechatronics-990-28603.log
Verificando o diretório de log para uso do disco. Isso pode demorar um pouco.
Pressione Ctrl-C para interromper
Concluída a verificação do uso do disco do arquivo de log. O uso é <1 GB.

servidor roslaunch iniciado http: // mechatronics-990 : 41224 /

RESUMO

PARÂMETROS CLAROS

• / mavros /

PARÂMETROS

• / 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 / 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: False
• / 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 / magnet_stdev: 0,0
• / mavros / imu /idance_stdev: 1.0
• / mavros / local_position / frame_id: fcu
• / mavros / local_position / tf / child_frame_id: base_link
• / mavros / local_position / tf / frame_id: world
• / mavros / local_position / tf / send: Verdadeiro
• / mavros / mission / pull_after_gcs: True
• / mavros / mocap / use_pose: Verdadeiro
• / mavros / mocap / use_tf: False
• / mavros / plugin_blacklist: ['atuator_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: False
• / mavros / setpoint_attitude / reverse_throttle: False
• / mavros / setpoint_attitude / tf / child_frame_id: atitude
• / mavros / setpoint_attitude / tf / frame_id: local_origin
• / mavros / setpoint_attitude / tf / listen: False
• / 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 / listen: False
• / mavros / setpoint_position / tf / rate_limit: 50.0
• / 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 / vibration / frame_id: vibração
• / mavros / vision_pose / tf / child_frame_id: vision
• / mavros / vision_pose / tf / frame_id: local_origin
• / mavros / vision_pose / tf / listen: False
• / mavros / vision_pose / tf / rate_limit: 10.0
• / mavros / vision_speed / listen_twist: False
• / robot_description:
• / rosdistro: indigo
• / rosversion: 1.11.21
• / tf_prefix:
• / use_sim_time: Verdadeiro

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

auto-iniciar novo mestre
processo [master]: iniciado com pid [28618]
ROS_MASTER_URI = http: // localhost : 11311

configuração / run_id para e483c856-89bf-11e7-bb61-1803733b65bb
processo [rosout-1]: iniciado com pid [28631]
iniciou o serviço principal [/ rosout]
processo [mavros-2]: iniciado com pid [28656]
processo [spawn_erlecopter-3]: iniciado com pid [28657]
processo [gazebo-4]: iniciado com pid [28674]
processo [gazebo_gui-5]: iniciado com pid [28678]
script spawn_model iniciado
[INFO] [WallTime: 1503684293.653120] [0,000000] Carregando modelo xml do parâmetro ros
[INFO] [WallTime: 1503684293.657344] [0,000000] Aguardando serviço / gazebo / spawn_urdf_model
[INFO] [WallTime: 1503684294.260952] [0,047500] Calling service / gazebo / spawn_urdf_model
[INFO] [WallTime: 1503684294.855628] [0.445000] Status de Spawn: SpawnModel: modelo gerado com sucesso
processo [spawn_erlecopter-3] foi concluído de forma limpa
arquivo de registro: /home/bshang2/.ros/log/e483c856-89bf-11e7-bb61-1803733b65bb/spawn_erlecopter-3*.log

Diagnóstico

bshang2 @ mechatronics-990 : ~ / simulação / ardupilot / ArduCopter $ rostopic echo -n1 / diagnostics
cabeçalho:
seq: 6456
carimbo:
segs: 6817
nsecs: 500000000
frame_id: ''
status:

• 
  nível: 0
  nome: mavros: conexão FCU
  mensagem: conectado
  hardware_id: tcp: //127.0.0.1 : 5760
  valores:
  
  
  
  • 
    
    chave: Pacotes recebidos:
    
    valor: 24635
  
  
  • 
    
    chave: Pacotes perdidos:
    
    valor: 0
  
  
  • 
    
    chave: Estouros de buffer:
    
    valor: 0
  
  
  • 
    
    chave: Erros de análise:
    
    valor: 0
  
  
  • 
    
    chave: número de sequência Rx:
    
    valor: 58
  
  
  • 
    
    chave: número de sequência Tx:
    
    valor: 190
  
  
  • 
    
    chave: Rx total de bytes:
    
    valor: 19770100
  
  
  • 
    
    chave: Tx total bytes:
    
    valor: 169295
  
  
  • 
    
    chave: Velocidade Rx:
    
    valor: inf
  
  
  • 
    
    chave: velocidade Tx:
    
    valor: inf
  
  
• 
  nível: 0
  nome: mavros: ponte GCS
  mensagem: conectado
  hardware_id: tcp: //127.0.0.1 : 5760
  valores:
  
  
  
  • 
    
    chave: Pacotes recebidos:
    
    valor: 3059
  
  
  • 
    
    chave: Pacotes perdidos:
    
    valor: 0
  
  
  • 
    
    chave: Estouros de buffer:
    
    valor: 0
  
  
  • 
    
    chave: Erros de análise:
    
    valor: 0
  
  
  • 
    
    chave: número de sequência Rx:
    
    valor: 243
  
  
  • 
    
    chave: número de sequência Tx:
    
    valor: 0
  
  
  • 
    
    chave: Rx total de bytes:
    
    valor: 51809
  
  
  • 
    
    chave: Tx total bytes:
    
    valor: 19769923
  
  
  • 
    
    chave: Velocidade Rx:
    
    valor: inf
  
  
  • 
    
    chave: velocidade Tx:
    
    valor: inf
  
  
• 
  nível: 0
  nome: mavros: GPS
  mensagem: correção 3D
  hardware_id: tcp: //127.0.0.1 : 5760
  valores:
  
  
  
  • 
    
    chave: Satélites visíveis
    
    valor: 10
  
  
  • 
    
    chave: Tipo de correção
    
    valor: 3
  
  
  • 
    
    chave: EPH (m)
    
    valor: 1,21
  
  
  • 
    
    chave: EPV (m)
    
    valor: 2,00
  
  
• 
  nível: 0
  nome: mavros: pulsação
  mensagem: normal
  hardware_id: tcp: //127.0.0.1 : 5760
  valores:
  
  
  
  • 
    
    chave: batimentos cardíacos desde a inicialização
    
    valor: 6817
  
  
  • 
    
    chave: Frequência (Hz)
    
    valor: 0,952381
  
  
  • 
    
    chave: tipo de veículo
    
    valor: Quadrotor
  
  
  • 
    
    chave: tipo de piloto automático
    
    valor: ArduPilotMega
  
  
  • 
    
    chave: Modo
    
    valor: GUIADO
  
  
  • 
    
    chave: Status do sistema
    
    valor: ativo
  
  
• 
  nível: 0
  nome: mavros: Sistema
  mensagem: normal
  hardware_id: tcp: //127.0.0.1 : 5760
  valores:
  
  
  
  • 
    
    chave: Sensor presente
    
    valor: 0x0061FD2F
  
  
  • 
    
    chave: Sensor habilitado
    
    valor: 0x0061FD2F
  
  
  • 
    
    chave: Sensor helth
    
    valor: 0x0061FD2F
  
  
  • 
    
    chave: Sensor 3D Gyro
    
    valor: ok
  
  
  • 
    
    chave: Sensor 3D Accel
    
    valor: ok
  
  
  • 
    
    chave: Sensor 3D Mag
    
    valor: ok
  
  
  • 
    
    chave: Sensor Abs Pressure
    
    valor: ok
  
  
  • 
    
    chave: Sensor GPS
    
    valor: ok
  
  
  • 
    
    chave: posição do laser do sensor
    
    valor: ok
  
  
  • 
    
    chave: Sensor Ang Rate Control
    
    valor: ok
  
  
  • 
    
    chave: Sensor Atitude Stab
    
    valor: ok
  
  
  • 
    
    chave: Sensor Yaw Position
    
    valor: ok
  
  
  • 
    
    tecla: Sensor Z / Alt Control
    
    valor: ok
  
  
  • 
    
    chave: Sensor X / Y Pos Control
    
    valor: ok
  
  
  • 
    
    chave: Saídas do motor do sensor
    
    valor: ok
  
  
  • 
    
    chave: Sensor RC Receptor
    
    valor: ok
  
  
  • 
    
    chave: saúde AHRS
    
    valor: ok
  
  
  • 
    
    chave: saúde do terreno
    
    valor: ok
  
  
  • 
    
    chave: carga da CPU (%)
    
    valor: 0,0
  
  
  • 
    
    chave: Taxa de queda (%)
    
    valor: 0,0
  
  
  • 
    
    chave: Erros de comunicação
    
    valor: 0
  
  
  • 
    
    chave: contagem de erros # 1
    
    valor: 0
  
  
  • 
    
    chave: contagem de erros # 2
    
    valor: 0
  
  
  • 
    
    chave: contagem de erros # 3
    
    valor: 0
  
  
  • 
    
    chave: contagem de erros # 4
    
    valor: 0
  
  
• 
  nível 1
  nome: mavros: bateria
  mensagem: Baixa tensão
  hardware_id: tcp: //127.0.0.1 : 5760
  valores:
  
  
  
  • 
    
    chave: Tensão
    
    valor: 0,00
  
  
  • 
    
    chave: atual
    
    valor: 0,0
  
  
  • 
    
    chave: Restante
  
  

valor: 100,0

Verificar ID

bshang2 @ mechatronics-990 : ~ / simulação / ardupilot / ArduCopter $ rosrun mavros checkid
OK. Recebi mensagens de 1: 1.

Recebeu 1231 mensagens, de 1 endereço
sys: comp lista de mensagens
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