Hola,
Estoy haciendo un control basado en la visión y la cámara está en el dron hacia abajo.
Entonces, la estimación de la posición está en el marco del cuerpo.
Encontré que el tema / mavros / setpoint_velocity / cmd_vel se puede usar para cambiar la velocidad del dron, pero parece que está en el marco NED.

¿Hay un tema como / mavros / setpoint_velocity / cmd_vel pero usa el marco del cuerpo?

¿O necesito convertir mi punto de ajuste de velocidad del marco de la carrocería al marco NED para implementarlo?

Intenté encontrar algo que pudiera usar y encontré 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 esta es la mejor solución que pude encontrar.
No estoy seguro de si el Yaw es solo de IMU o de la fusión de IMU y magnético.
¿Tiene algún comentario o idea al respecto?

Versión y plataforma MAVROS

Mavros: 0.17.3
ROS: Índigo
Ubuntu: 14.04

Tipo y versión de piloto automático

APM: helicóptero V3.4-dev (fe724032)

Registros de nodo

bshang2 @ mechatronics-990 : ~ / simulation / ardupilot / ArduCopter $ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_mark.launch
... iniciando sesión en /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/roslaunch-mechatronics-990-28079.log
Comprobando el uso del disco en el directorio de registro. Esto puede tardar un rato.
Presione Ctrl-C para interrumpir
Finalizado la comprobación del uso del disco del archivo de registro. El uso es <1 GB.

inició el servidor roslaunch http: // mechatronics-990 : 45006 /

RESUMEN

BORRAR PARAMETROS

• / mavros /

PARAMETROS

• / mavros / cmd / use_comp_id_system_control: Falso
• / 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: Falso
• / mavros / image / frame_id: px4flow
• / mavros / imu / velocidad_angular_stdev: 0.000349065850399
• / mavros / imu / frame_id: fcu
• / mavros / imu / linear_acceleration_stdev: 0.0003
• / mavros / imu / magnetic_stdev: 0.0
• / mavros / imu / orientación_stdev: 1.0
• / mavros / posición_local / frame_id: fcu
• / mavros / posición_local / tf / child_frame_id: base_link
• / mavros / posición_local / tf / frame_id: mundo
• / mavros / posición_local / tf / send: True
• / mavros / mission / pull_after_gcs: Verdadero
• / mavros / mocap / use_pose: True
• / mavros / mocap / use_tf: Falso
• / 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 / área_de_seguridad / p1 / x: 1.0
• / mavros / área_de_seguridad / p1 / y: 1.0
• / mavros / área_de_seguridad / p1 / z: 1.0
• / mavros / área_seguridad / p2 / x: -1.0
• / mavros / área_de_seguridad / p2 / y: -1.0
• / mavros / área_seguridad / p2 / z: -1.0
• / mavros / setpoint_accel / send_force: Falso
• / mavros / setpoint_attitude / reverse_throttle: Falso
• / mavros / setpoint_attitude / tf / child_frame_id: actitud
• / 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: Falso
• / mavros / sys / disable_diag: Falso
• / mavros / sys / voltaje_min: 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 / vibración / frame_id: vibración
• / mavros / vision_pose / tf / child_frame_id: visión
• / 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: Falso
• / robot_description:
• / rosdistro: índigo
• / rosversion: 1.11.21
• / tf_prefix:
• / use_sim_time: verdadero

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

nuevo maestro de inicio automático
proceso [maestro]: iniciado con pid [28094]
ROS_MASTER_URI = http: // localhost : 11311

configuración / run_id en ca9213bc-89bf-11e7-b657-1803733b65bb
proceso [rosout-1]: comenzó con pid [28107]
inició el servicio principal [/ rosout]
proceso [mavros-2]: comenzó con pid [28126]
proceso [spawn_erlecopter-3]: comenzó con pid [28133]
proceso [gazebo-4]: comenzó con pid [28134]
proceso [gazebo_gui-5]: comenzó con pid [28151]
Se inició el script spawn_model
El proceso de [gazebo-4] ha muerto [pid 28134, código de salida 255, cmd / home / bshang2 / simulation / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / gzserver -u -e ode / home / bshang2 / simulation / 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-1803733b65 / gazebo].
archivo de registro: /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/gazebo-4 .log[INFO] [WallTime: 1503684250.172508] [0.000000] Cargando modelo xml desde el parámetro ros[INFO] [WallTime: 1503684250.176886] [0.000000] Esperando servicio / gazebo / spawn_urdf_modelEl proceso de [gazebo_gui-5] ha muerto [pid 28151, código de salida 255, cmd / home / bshang2 / simulation / 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].archivo de registro: /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/gazebo_gui-5 .log
^ C [spawn_erlecopter-3] matando al salir
[mavros-2] matando al salir
Rastreo (llamadas recientes más última):
Archivo "/ home / bshang2 / simulation / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / spawn_model", línea 301, en
sm.callSpawnService ()
Archivo "/ home / bshang2 / simulation / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / spawn_model", línea 267, en callSpawnService
propósito_inicial, self.reference_frame, self.gazebo_namespace)
Archivo "/home/bshang2/simulation/ros_catkin_ws/src/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/gazebo_interface.py", línea 28, en spawn_urdf_model_client
rospy.wait_for_service (gazebo_namespace + '/ spawn_urdf_model')
Archivo "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", línea 159, en wait_for_service
elevar ROSInterruptException ("cierre rospy")
rospy.exceptions.ROSInterruptException: cierre de rospy
^ C [rosout-1] matando al salir
^ C [maestro] matando al salir
apagar el monitor de procesamiento ...
... apagando el monitor de procesamiento completo
hecho
bshang2 @ mechatronics-990 : ~ / simulation / ardupilot / ArduCopter $ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_mark.launch
... iniciando sesión en /home/bshang2/.ros/log/e483c856-89bf-11e7-bb61-1803733b65bb/roslaunch-mechatronics-990-28603.log
Comprobando el uso del disco en el directorio de registro. Esto puede tardar un rato.
Presione Ctrl-C para interrumpir
Finalizado la comprobación del uso del disco del archivo de registro. El uso es <1 GB.

inició el servidor roslaunch http: // mechatronics-990 : 41224 /

RESUMEN

BORRAR PARAMETROS

• / mavros /

PARAMETROS

• / mavros / cmd / use_comp_id_system_control: Falso
• / 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: Falso
• / mavros / image / frame_id: px4flow
• / mavros / imu / velocidad_angular_stdev: 0.000349065850399
• / mavros / imu / frame_id: fcu
• / mavros / imu / linear_acceleration_stdev: 0.0003
• / mavros / imu / magnetic_stdev: 0.0
• / mavros / imu / orientación_stdev: 1.0
• / mavros / posición_local / frame_id: fcu
• / mavros / posición_local / tf / child_frame_id: base_link
• / mavros / posición_local / tf / frame_id: mundo
• / mavros / posición_local / tf / send: True
• / mavros / mission / pull_after_gcs: Verdadero
• / mavros / mocap / use_pose: True
• / mavros / mocap / use_tf: Falso
• / 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 / área_de_seguridad / p1 / x: 1.0
• / mavros / área_de_seguridad / p1 / y: 1.0
• / mavros / área_de_seguridad / p1 / z: 1.0
• / mavros / área_seguridad / p2 / x: -1.0
• / mavros / área_de_seguridad / p2 / y: -1.0
• / mavros / área_seguridad / p2 / z: -1.0
• / mavros / setpoint_accel / send_force: Falso
• / mavros / setpoint_attitude / reverse_throttle: Falso
• / mavros / setpoint_attitude / tf / child_frame_id: actitud
• / 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: Falso
• / mavros / sys / disable_diag: Falso
• / mavros / sys / voltaje_min: 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 / vibración / frame_id: vibración
• / mavros / vision_pose / tf / child_frame_id: visión
• / 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: Falso
• / robot_description:
• / rosdistro: índigo
• / rosversion: 1.11.21
• / tf_prefix:
• / use_sim_time: verdadero

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

nuevo maestro de inicio automático
proceso [maestro]: iniciado con pid [28618]
ROS_MASTER_URI = http: // localhost : 11311

configuración / run_id en e483c856-89bf-11e7-bb61-1803733b65bb
proceso [rosout-1]: comenzó con pid [28631]
inició el servicio principal [/ rosout]
proceso [mavros-2]: comenzó con pid [28656]
proceso [spawn_erlecopter-3]: comenzó con pid [28657]
proceso [gazebo-4]: comenzó con pid [28674]
proceso [gazebo_gui-5]: comenzó con pid [28678]
Se inició el script spawn_model
[INFO] [WallTime: 1503684293.653120] [0.000000] Cargando modelo xml desde el parámetro ros
[INFO] [WallTime: 1503684293.657344] [0.000000] Esperando servicio / gazebo / spawn_urdf_model
[INFO] [WallTime: 1503684294.260952] [0.047500] Servicio de llamadas / gazebo / spawn_urdf_model
[INFO] [WallTime: 1503684294.855628] [0.445000] Estado de generación: Modelo de generación: Modelo generado con éxito
El proceso [spawn_erlecopter-3] ha finalizado limpiamente
archivo de registro: /home/bshang2/.ros/log/e483c856-89bf-11e7-bb61-1803733b65bb/spawn_erlecopter-3*.log

Diagnósticos

bshang2 @ mechatronics-990 : ~ / simulación / ardupilot / ArduCopter $ rostopic echo -n1 / diagnostics
encabezamiento:
seq: 6456
sello:
segundos: 6817
nseg: 500000000
frame_id: ''
estado:

• 
  nivel: 0
  nombre: mavros: conexión FCU
  mensaje: conectado
  hardware_id: tcp: //127.0.0.1 : 5760
  valores:
  
  
  
  • 
    
    tecla: Paquetes recibidos:
    
    valor: 24635
  
  
  • 
    
    clave: Paquetes descartados:
    
    valor: 0
  
  
  • 
    
    clave: Desbordamiento del búfer:
    
    valor: 0
  
  
  • 
    
    clave: Analizar errores:
    
    valor: 0
  
  
  • 
    
    tecla: Número de secuencia de Rx:
    
    valor: 58
  
  
  • 
    
    tecla: Número de secuencia de Tx:
    
    valor: 190
  
  
  • 
    
    clave: bytes totales de Rx:
    
    valor: 19770100
  
  
  • 
    
    clave: Tx bytes totales:
    
    valor: 169295
  
  
  • 
    
    tecla: Velocidad Rx:
    
    valor: inf
  
  
  • 
    
    tecla: Velocidad Tx:
    
    valor: inf
  
  
• 
  nivel: 0
  nombre: mavros: puente GCS
  mensaje: conectado
  hardware_id: tcp: //127.0.0.1 : 5760
  valores:
  
  
  
  • 
    
    tecla: Paquetes recibidos:
    
    valor: 3059
  
  
  • 
    
    clave: Paquetes descartados:
    
    valor: 0
  
  
  • 
    
    clave: Desbordamiento del búfer:
    
    valor: 0
  
  
  • 
    
    clave: Analizar errores:
    
    valor: 0
  
  
  • 
    
    tecla: Número de secuencia de Rx:
    
    valor: 243
  
  
  • 
    
    tecla: Número de secuencia de Tx:
    
    valor: 0
  
  
  • 
    
    clave: bytes totales de Rx:
    
    valor: 51809
  
  
  • 
    
    clave: Tx bytes totales:
    
    valor: 19769923
  
  
  • 
    
    tecla: Velocidad Rx:
    
    valor: inf
  
  
  • 
    
    tecla: Velocidad Tx:
    
    valor: inf
  
  
• 
  nivel: 0
  nombre: mavros: GPS
  mensaje: corrección 3D
  hardware_id: tcp: //127.0.0.1 : 5760
  valores:
  
  
  
  • 
    
    tecla: Satélites visibles
    
    valor: 10
  
  
  • 
    
    clave: tipo de arreglo
    
    valor: 3
  
  
  • 
    
    clave: EPH (m)
    
    valor: 1.21
  
  
  • 
    
    clave: EPV (m)
    
    valor: 2.00
  
  
• 
  nivel: 0
  name: mavros: Heartbeat
  mensaje: Normal
  hardware_id: tcp: //127.0.0.1 : 5760
  valores:
  
  
  
  • 
    
    clave: Latidos desde el inicio
    
    valor: 6817
  
  
  • 
    
    clave: Frecuencia (Hz)
    
    valor: 0.952381
  
  
  • 
    
    clave: tipo de vehículo
    
    valor: Quadrotor
  
  
  • 
    
    clave: tipo de piloto automático
    
    valor: ArduPilotMega
  
  
  • 
    
    clave: Modo
    
    valor: GUIADO
  
  
  • 
    
    clave: estado del sistema
    
    valor: Activo
  
  
• 
  nivel: 0
  nombre: mavros: Sistema
  mensaje: Normal
  hardware_id: tcp: //127.0.0.1 : 5760
  valores:
  
  
  
  • 
    
    clave: Sensor presente
    
    valor: 0x0061FD2F
  
  
  • 
    
    tecla: Sensor habilitado
    
    valor: 0x0061FD2F
  
  
  • 
    
    clave: Sensor helth
    
    valor: 0x0061FD2F
  
  
  • 
    
    clave: Sensor 3D Gyro
    
    valor: Ok
  
  
  • 
    
    clave: Sensor 3D Accel
    
    valor: Ok
  
  
  • 
    
    clave: Sensor 3D Mag
    
    valor: Ok
  
  
  • 
    
    clave: Sensor Abs Presión
    
    valor: Ok
  
  
  • 
    
    clave: Sensor GPS
    
    valor: Ok
  
  
  • 
    
    clave: Sensor de posición del láser
    
    valor: Ok
  
  
  • 
    
    Tecla: Sensor de control de velocidad de ángulo
    
    valor: Ok
  
  
  • 
    
    Tecla: Puñalada de actitud del sensor
    
    valor: Ok
  
  
  • 
    
    tecla: Posición de guiñada del sensor
    
    valor: Ok
  
  
  • 
    
    tecla: Sensor Z / Control Alt
    
    valor: Ok
  
  
  • 
    
    Tecla: Sensor X / Y Pos Control
    
    valor: Ok
  
  
  • 
    
    tecla: Salidas del motor del sensor
    
    valor: Ok
  
  
  • 
    
    clave: Sensor RC Receptor
    
    valor: Ok
  
  
  • 
    
    clave: salud AHRS
    
    valor: Ok
  
  
  • 
    
    clave: salud del terreno
    
    valor: Ok
  
  
  • 
    
    clave: Carga de CPU (%)
    
    valor: 0.0
  
  
  • 
    
    clave: Tasa de caída (%)
    
    valor: 0.0
  
  
  • 
    
    clave: errores de comunicación
    
    valor: 0
  
  
  • 
    
    clave: Número de errores # 1
    
    valor: 0
  
  
  • 
    
    clave: Número de errores 2
    
    valor: 0
  
  
  • 
    
    clave: Número de errores # 3
    
    valor: 0
  
  
  • 
    
    clave: Número de errores # 4
    
    valor: 0
  
  
• 
  nivel 1
  nombre: mavros: batería
  mensaje: Baja tensión
  hardware_id: tcp: //127.0.0.1 : 5760
  valores:
  
  
  
  • 
    
    clave: voltaje
    
    valor: 0.00
  
  
  • 
    
    clave: actual
    
    valor: 0.0
  
  
  • 
    
    clave: Restante
  
  

valor: 100.0

Verificar ID

bshang2 @ mechatronics-990 : ~ / simulación / ardupilot / ArduCopter $ rosrun mavros checkid
está bien. Recibí mensajes de 1: 1.

1231 mensajes recibidos, de 1 direcciones
sys: lista de mensajes de la compilación
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