Salut,
Je fais un contrôle basé sur la vision et la caméra est sur le drone face vers le bas.
Donc, l'estimation de la position est dans le corps.
J'ai trouvé que le sujet / mavros / setpoint_velocity / cmd_vel peut être utilisé pour changer la vitesse du drone, mais il semble qu'il soit dans le cadre NED.

Existe-t-il un sujet comme / mavros / setpoint_velocity / cmd_vel mais utilise le cadre du corps?

Ou j'ai besoin de convertir ma consigne de vitesse du cadre de carrosserie en cadre NED pour l'implémenter?

J'ai essayé de trouver quelque chose que je peux utiliser et j'ai trouvé ce code. 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);

Il semble que ce soit la meilleure solution que j'ai pu trouver.
Je ne sais pas si le lacet provient uniquement de l'IMU ou de la fusion de l'IMU et du magnétique.
Avez-vous des commentaires ou des idées à ce sujet?

Version et plateforme MAVROS

Mavros: 0,17,3
ROS: Indigo
Ubuntu: 14.04

Type et version du pilote automatique

APM: Copter V3.4-dev (fe724032)

Journaux des nœuds

bshang2 @ mécatronique-990 : ~ / simulation / ardupilot / ArduCopter $ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_mark.launch
... connexion à /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/roslaunch-mechatronics-990-28079.log
Vérification du répertoire du journal pour l'utilisation du disque. Cela peut prendre un peu de temps.
Appuyez sur Ctrl-C pour interrompre
Vérification de l'utilisation du disque du fichier journal terminé. L'utilisation est <1 Go.

a démarré le serveur roslaunch http: // mécatronique-990 : 45006 /

RÉSUMÉ

EFFACER LES PARAMÈTRES

• / mavros /

PARAMÈTRES

• / 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: Faux
• / 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 / orientation_stdev: 1.0
• / mavros / local_position / frame_id: fcu
• / mavros / local_position / tf / child_frame_id: base_link
• / mavros / local_position / tf / frame_id: monde
• / mavros / local_position / tf / send: Vrai
• / mavros / mission / pull_after_gcs: Vrai
• / mavros / mocap / use_pose: Vrai
• / mavros / mocap / use_tf: Faux
• / 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: False
• / mavros / setpoint_attitude / reverse_throttle: Faux
• / mavros / setpoint_attitude / tf / child_frame_id: attitude
• / mavros / setpoint_attitude / tf / frame_id: local_origin
• / mavros / setpoint_attitude / tf / listen: Faux
• / 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: Faux
• / mavros / setpoint_position / tf / rate_limit: 50,0
• / mavros / startup_px4_usb_quirk: Faux
• / mavros / sys / disable_diag: False
• / mavros / sys / min_voltage: 10,0
• / mavros / id_composant_cible: 1
• / mavros / id_système_cible: 1
• / mavros / tdr_radio / low_rssi: 40
• / mavros / time / time_ref_source: fcu
• / mavros / time / timesync_avg_alpha: 0.6
• / mavros / vibration / frame_id: vibration
• / mavros / vision_pose / tf / child_frame_id: vision
• / mavros / vision_pose / tf / frame_id: local_origin
• / mavros / vision_pose / tf / listen: Faux
• / mavros / vision_pose / tf / rate_limit: 10.0
• / mavros / vision_speed / listen_twist: Faux
• / robot_description:
• / rosdistro: indigo
• / rosversion: 1.11.21
• / tf_prefix:
• / use_sim_time: Vrai

NOEUDS
/
gazebo (gazebo_ros / gzserver)
gazebo_gui (gazebo_ros / gzclient)
mavros (mavros / noeud_mavros)
spawn_erlecopter (gazebo_ros / spawn_model)

démarrage automatique d'un nouveau maître
processus [master]: démarré avec pid [28094]
ROS_MASTER_URI = http: // localhost : 11311

réglage / run_id sur ca9213bc-89bf-11e7-b657-1803733b65bb
processus [rosout-1]: démarré avec pid [28107]
a démarré le service principal [/ rosout]
processus [mavros-2]: démarré avec pid [28126]
processus [spawn_erlecopter-3]: démarré avec pid [28133]
processus [gazebo-4]: démarré avec pid [28134]
processus [gazebo_gui-5]: démarré avec pid [28151]
Le script spawn_model a démarré
[gazebo-4] processus est mort [pid 28134, code de sortie 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-1803733eb65-11e7-b657-1803733eb65-bb65 / gazbo].
fichier journal: /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/gazebo-4 .log[INFO] [WallTime: 1503684250.172508] [0.000000] Chargement du modèle xml à partir du paramètre ros[INFO] [WallTime: 1503684250.176886] [0.000000] En attente de service / gazebo / spawn_urdf_model[gazebo_gui-5] processus est mort [pid 28151, code de sortie 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].fichier journal: /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/gazebo_gui-5 .log
^ C [spawn_erlecopter-3] tuant à la sortie
[mavros-2] tuant à la sortie
Traceback (dernier appel le plus récent):
Fichier "/ home / bshang2 / simulation / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / spawn_model", ligne 301, dans
sm.callSpawnService ()
Fichier "/ home / bshang2 / simulation / ros_catkin_ws / src / gazebo_ros_pkgs / gazebo_ros / scripts / spawn_model", ligne 267, dans callSpawnService
initial_pose, self.reference_frame, self.gazebo_namespace)
Fichier "/home/bshang2/simulation/ros_catkin_ws/src/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/gazebo_interface.py", ligne 28, dans spawn_urdf_model_client
rospy.wait_for_service (gazebo_namespace + '/ spawn_urdf_model')
Fichier "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", ligne 159, dans wait_for_service
lever ROSInterruptException ("arrêt de rospy")
rospy.exceptions.ROSInterruptException: arrêt de rospy
^ C [rosout-1] tuant à la sortie
^ C [maître] tuant à la sortie
arrêt du moniteur de traitement ...
... arrêt du moniteur de traitement terminé
Fini
bshang2 @ mécatronique-990 : ~ / simulation / ardupilot / ArduCopter $ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_mark.launch
... connexion à /home/bshang2/.ros/log/e483c856-89bf-11e7-bb61-1803733b65bb/roslaunch-mechatronics-990-28603.log
Vérification du répertoire du journal pour l'utilisation du disque. Cela peut prendre un peu de temps.
Appuyez sur Ctrl-C pour interrompre
Vérification de l'utilisation du disque du fichier journal terminé. L'utilisation est <1 Go.

a démarré le serveur roslaunch http: // mécatronique-990 : 41224 /

RÉSUMÉ

EFFACER LES PARAMÈTRES

• / mavros /

PARAMÈTRES

• / 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: Faux
• / 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 / orientation_stdev: 1.0
• / mavros / local_position / frame_id: fcu
• / mavros / local_position / tf / child_frame_id: base_link
• / mavros / local_position / tf / frame_id: monde
• / mavros / local_position / tf / send: Vrai
• / mavros / mission / pull_after_gcs: Vrai
• / mavros / mocap / use_pose: Vrai
• / mavros / mocap / use_tf: Faux
• / 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: False
• / mavros / setpoint_attitude / reverse_throttle: Faux
• / mavros / setpoint_attitude / tf / child_frame_id: attitude
• / mavros / setpoint_attitude / tf / frame_id: local_origin
• / mavros / setpoint_attitude / tf / listen: Faux
• / 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: Faux
• / mavros / setpoint_position / tf / rate_limit: 50,0
• / mavros / startup_px4_usb_quirk: Faux
• / mavros / sys / disable_diag: False
• / mavros / sys / min_voltage: 10,0
• / mavros / id_composant_cible: 1
• / mavros / id_système_cible: 1
• / mavros / tdr_radio / low_rssi: 40
• / mavros / time / time_ref_source: fcu
• / mavros / time / timesync_avg_alpha: 0.6
• / mavros / vibration / frame_id: vibration
• / mavros / vision_pose / tf / child_frame_id: vision
• / mavros / vision_pose / tf / frame_id: local_origin
• / mavros / vision_pose / tf / listen: Faux
• / mavros / vision_pose / tf / rate_limit: 10.0
• / mavros / vision_speed / listen_twist: Faux
• / robot_description:
• / rosdistro: indigo
• / rosversion: 1.11.21
• / tf_prefix:
• / use_sim_time: Vrai

NOEUDS
/
gazebo (gazebo_ros / gzserver)
gazebo_gui (gazebo_ros / gzclient)
mavros (mavros / noeud_mavros)
spawn_erlecopter (gazebo_ros / spawn_model)

démarrage automatique d'un nouveau maître
process [master]: démarré avec pid [28618]
ROS_MASTER_URI = http: // localhost : 11311

réglage / run_id sur e483c856-89bf-11e7-bb61-1803733b65bb
processus [rosout-1]: démarré avec pid [28631]
a démarré le service principal [/ rosout]
processus [mavros-2]: démarré avec pid [28656]
processus [spawn_erlecopter-3]: démarré avec pid [28657]
processus [gazebo-4]: démarré avec pid [28674]
processus [gazebo_gui-5]: démarré avec pid [28678]
Le script spawn_model a démarré
[INFO] [WallTime: 1503684293.653120] [0.000000] Chargement du modèle xml à partir du paramètre ros
[INFO] [WallTime: 1503684293.657344] [0.000000] En attente de service / gazebo / spawn_urdf_model
[INFO] [WallTime: 1503684294.260952] [0.047500] Service d'appel / gazebo / spawn_urdf_model
[INFO] [WallTime: 1503684294.855628] [0.445000] Statut d'apparition: SpawnModel: modèle généré avec succès
Le processus [spawn_erlecopter-3] s'est terminé proprement
fichier journal: /home/bshang2/.ros/log/e483c856-89bf-11e7-bb61-1803733b65bb/spawn_erlecopter-3*.log

Diagnostique

bshang2 @ mécatronique-990 : ~ / simulation / ardupilot / ArduCopter $ rostopic echo -n1 / diagnostics
entête:
seq: 6456
timbre:
secs: 6817
nsecs: 500000000
frame_id: ''
statut:

• 
  niveau: 0
  nom: mavros: connexion FCU
  message: connecté
  id_matériel :
  valeurs:
  
  
  
  • 
    
    clé: Paquets reçus:
    
    valeur: 24635
  
  
  • 
    
    clé: paquets abandonnés:
    
    valeur: 0
  
  
  • 
    
    clé: dépassements de tampon:
    
    valeur: 0
  
  
  • 
    
    clé: erreurs d'analyse:
    
    valeur: 0
  
  
  • 
    
    Touche: numéro de séquence Rx:
    
    valeur: 58
  
  
  • 
    
    Touche: numéro de séquence Tx:
    
    valeur: 190
  
  
  • 
    
    clé: octets totaux Rx:
    
    valeur: 19770100
  
  
  • 
    
    clé: octets totaux Tx:
    
    valeur: 169295
  
  
  • 
    
    clé: vitesse Rx:
    
    valeur: inf
  
  
  • 
    
    clé: vitesse Tx:
    
    valeur: inf
  
  
• 
  niveau: 0
  nom: mavros: pont GCS
  message: connecté
  id_matériel :
  valeurs:
  
  
  
  • 
    
    clé: Paquets reçus:
    
    valeur: 3059
  
  
  • 
    
    clé: paquets abandonnés:
    
    valeur: 0
  
  
  • 
    
    clé: dépassements de tampon:
    
    valeur: 0
  
  
  • 
    
    clé: erreurs d'analyse:
    
    valeur: 0
  
  
  • 
    
    Touche: numéro de séquence Rx:
    
    valeur: 243
  
  
  • 
    
    Touche: numéro de séquence Tx:
    
    valeur: 0
  
  
  • 
    
    clé: octets totaux Rx:
    
    valeur: 51809
  
  
  • 
    
    clé: octets totaux Tx:
    
    valeur: 19769923
  
  
  • 
    
    clé: vitesse Rx:
    
    valeur: inf
  
  
  • 
    
    clé: vitesse Tx:
    
    valeur: inf
  
  
• 
  niveau: 0
  nom: mavros: GPS
  message: correction 3D
  id_matériel :
  valeurs:
  
  
  
  • 
    
    clé: Satellites visibles
    
    valeur: 10
  
  
  • 
    
    clé: type de correction
    
    valeur: 3
  
  
  • 
    
    clé: EPH (m)
    
    valeur: 1.21
  
  
  • 
    
    clé: EPV (m)
    
    valeur: 2,00
  
  
• 
  niveau: 0
  nom: mavros: Heartbeat
  message: Normal
  id_matériel :
  valeurs:
  
  
  
  • 
    
    clé: battements de cœur depuis le démarrage
    
    valeur: 6817
  
  
  • 
    
    Touche: Fréquence (Hz)
    
    valeur: 0,952381
  
  
  • 
    
    clé: type de véhicule
    
    valeur: Quadrotor
  
  
  • 
    
    touche: type de pilote automatique
    
    valeur: ArduPilotMega
  
  
  • 
    
    touche: Mode
    
    valeur: GUIDÉ
  
  
  • 
    
    clé: état du système
    
    valeur: Actif
  
  
• 
  niveau: 0
  nom: mavros: System
  message: Normal
  id_matériel :
  valeurs:
  
  
  
  • 
    
    touche: capteur présent
    
    valeur: 0x0061FD2F
  
  
  • 
    
    touche: capteur activé
    
    valeur: 0x0061FD2F
  
  
  • 
    
    clé: capteur Helth
    
    valeur: 0x0061FD2F
  
  
  • 
    
    clé: Sensor 3D Gyro
    
    valeur: Ok
  
  
  • 
    
    clé: Capteur 3D Accel
    
    valeur: Ok
  
  
  • 
    
    clé: Sensor 3D Mag
    
    valeur: Ok
  
  
  • 
    
    Touche: Pression Abs du capteur
    
    valeur: Ok
  
  
  • 
    
    clé: capteur GPS
    
    valeur: Ok
  
  
  • 
    
    clé: Position du capteur laser
    
    valeur: Ok
  
  
  • 
    
    Touche: Contrôle de la fréquence angulaire du capteur
    
    valeur: Ok
  
  
  • 
    
    clé: Sensor Attitude Stab
    
    valeur: Ok
  
  
  • 
    
    clé: Position de lacet du capteur
    
    valeur: Ok
  
  
  • 
    
    Touche: Sensor Z / Alt Control
    
    valeur: Ok
  
  
  • 
    
    Touche: Commande de position X / Y du capteur
    
    valeur: Ok
  
  
  • 
    
    Touche: Sorties du moteur du capteur
    
    valeur: Ok
  
  
  • 
    
    clé: récepteur de capteur RC
    
    valeur: Ok
  
  
  • 
    
    clé: santé AHRS
    
    valeur: Ok
  
  
  • 
    
    clé: santé du terrain
    
    valeur: Ok
  
  
  • 
    
    clé: Charge CPU (%)
    
    valeur: 0.0
  
  
  • 
    
    touche: Taux de chute (%)
    
    valeur: 0.0
  
  
  • 
    
    clé: Erreurs comm
    
    valeur: 0
  
  
  • 
    
    clé: nombre d'erreurs # 1
    
    valeur: 0
  
  
  • 
    
    clé: nombre d'erreurs # 2
    
    valeur: 0
  
  
  • 
    
    clé: nombre d'erreurs # 3
    
    valeur: 0
  
  
  • 
    
    clé: nombre d'erreurs # 4
    
    valeur: 0
  
  
• 
  niveau 1
  nom: mavros: Batterie
  message: basse tension
  id_matériel :
  valeurs:
  
  
  
  • 
    
    clé: Tension
    
    valeur: 0,00
  
  
  • 
    
    clé: courant
    
    valeur: 0.0
  
  
  • 
    
    clé: Restant
  
  

valeur: 100,0

Vérifier l'ID

bshang2 @ mécatronique-990 : ~ / simulation / ardupilot / ArduCopter $ rosrun mavros checkid
D'ACCORD. J'ai reçu des messages de 1: 1.

Reçu 1231 messages, à partir de 1 adresses
sys: liste des messages
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