Hallo,
Ich mache eine visionsbasierte Steuerung und die Kamera befindet sich auf der Drohne und zeigt nach unten.
Die Positionsschätzung erfolgt also im Körperrahmen.
Ich fand, dass das Thema / mavros / setpoint_velocity / cmd_vel verwendet werden kann, um die Geschwindigkeit der Drohne zu ändern, aber es scheint, dass es sich im NED-Frame befindet.

Gibt es ein Thema wie / mavros / setpoint_velocity / cmd_vel, aber Body Frame verwenden?

Oder muss ich meinen Geschwindigkeitssollwert vom Body-Frame in den NED-Frame konvertieren, um ihn zu implementieren?

Ich habe versucht, etwas zu finden, das ich verwenden kann, und ich habe diesen Code gefunden. 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);

Es scheint, dass dies die beste Lösung ist, die ich finden konnte.
Ich bin nicht sicher, ob das Gieren nur von der IMU oder von der Fusion von IMU und Magnet stammt.
Haben Sie Kommentare oder Ideen dazu?

MAVROS Version und Plattform

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

Typ und Version des Autopiloten

APM: Copter V3.4-dev (fe724032)

Knotenprotokolle

bshang2 @ mechatronics-990 : ~ / simulations / ardupilot / ArduCopter $ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_mark.launch
... Protokollierung in /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/roslaunch-mechatronics-990-28079.log
Überprüfen des Protokollverzeichnisses auf Festplattennutzung. Das kann eine Weile dauern.
Drücken Sie Strg-C, um zu unterbrechen
Überprüfung der Datenträgerverwendung der Protokolldatei abgeschlossen. Die Nutzung beträgt <1 GB.

Roslaunch Server gestartet http: // mechatronics-990 : 45006 /

ZUSAMMENFASSUNG

KLARE PARAMETER

• / mavros /

PARAMETER

• / 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 / angle_velocity_stdev: 0,000349065850399
• / mavros / imu / frame_id: fcu
• / mavros / imu / linear_acceleration_stdev: 0,0003
• / mavros / imu / magnet_stdev: 0.0
• / mavros / imu / Orientierung_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: True
• / mavros / mission / pull_after_gcs: Richtig
• / mavros / mocap / use_pose: True
• / mavros / mocap / use_tf: False
• / 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 / security_area / p1 / x: 1.0
• / mavros / security_area / p1 / y: 1.0
• / mavros / security_area / p1 / z: 1.0
• / mavros / security_area / p2 / x: -1,0
• / mavros / security_area / p2 / y: -1,0
• / mavros / security_area / p2 / z: -1,0
• / mavros / setpoint_accel / send_force: False
• / mavros / setpoint_attitude / reverse_throttle: False
• / mavros / setpoint_attitude / tf / child_frame_id: Haltung
• / 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: Vibration
• / 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: Falsch
• / robot_description:
• / rosdistro: Indigo
• / rosversion: 1.11.21
• / tf_prefix:
• / use_sim_time: True

NODES
/.
Pavillon (Pavillon_ros / gzserver)
Pavillon_gui (Pavillon_ros / gzclient)
mavros (mavros / mavros_node)
spawn_erlecopter (Pavillon_ros / spawn_model)

Neustart neuer Master
Prozess [Master]: gestartet mit pid [28094]
ROS_MASTER_URI = http: // localhost : 11311

Setzen Sie / run_id auf ca9213bc-89bf-11e7-b657-1803733b65bb
Prozess [Rosout-1]: begann mit PID [28107]
Core Service gestartet [/ rosout]
Prozess [mavros-2]: gestartet mit pid [28126]
Prozess [spawn_erlecopter-3]: gestartet mit pid [28133]
Prozess [Pavillon-4]: begann mit PID [28134]
Prozess [Pavillon_gui-5]: begann mit pid [28151]
Das Skript spawn_model wurde gestartet
[Pavillon-4] -Prozess ist gestorben [PID 28134, Exit-Code 255, cmd / home / bshang2 / Simulation / ros_catkin_ws / src / Pavillon_ros_pkgs / Pavillon_ros / Skripte / 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: = Pavillon __log: = / home / bshang2 / .ros / log / ca9213bc-89bf-11e7-b657-1803733b65bb].
Protokolldatei: /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/gazebo-4 .log[INFO] [WallTime: 1503684250.172508] [0.000000] Laden der Modell-XML aus dem ros-Parameter[INFO] [WallTime: 1503684250.176886] [0.000000] Warten auf Service / Pavillon / Spawn_urdf_model[Pavillon_gui-5] Prozess ist gestorben [PID 28151, Exit-Code 255, cmd / home / bshang2 / Simulation / ros_catkin_ws / src / Pavillon_ros_pkgs / Pavillon_ros / Skripte / gzclient __name: = Pavillon_gui __log: = / home / bshang2 / .ros / log / ca9213bc-89bf-11e7-b657-1803733b65bb / Gazebo_gui-5.log].Protokolldatei: /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/gazebo_gui-5 .log
^ C [spawn_erlecopter-3] tötet beim Verlassen
[mavros-2] beim Verlassen töten
Traceback (letzter Anruf zuletzt):
Datei "/ home / bshang2 / Simulation / ros_catkin_ws / src / Gazebo_ros_pkgs / Pavillon_ros / Skripte / Spawn_Model", Zeile 301, in
sm.callSpawnService ()
Datei "/ home / bshang2 / Simulation / ros_catkin_ws / src / Pavillon_ros_pkgs / Pavillon_ros / Skripte / Spawn_Modell", Zeile 267, in callSpawnService
initial_pose, self.reference_frame, self.gazebo_namespace)
Datei "/home/bshang2/simulation/ros_catkin_ws/src/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/gazebo_interface.py", Zeile 28, in spawn_urdf_model_client
rospy.wait_for_service (Pavillon_Namespace + '/ spawn_urdf_model')
Datei "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", Zeile 159, in wait_for_service
ROSInterruptException auslösen ("rospy shutdown")
rospy.exceptions.ROSInterruptException: Herunterfahren von rospy
^ C [Rosout-1] tötet beim Verlassen
^ C [Meister] tötet beim Verlassen
Verarbeitungsmonitor herunterfahren ...
... den Verarbeitungsmonitor vollständig herunterfahren
getan
bshang2 @ mechatronics-990 : ~ / simulations / ardupilot / ArduCopter $ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_mark.launch
... Protokollierung in /home/bshang2/.ros/log/e483c856-89bf-11e7-bb61-1803733b65bb/roslaunch-mechatronics-990-28603.log
Überprüfen des Protokollverzeichnisses auf Festplattennutzung. Das kann eine Weile dauern.
Drücken Sie Strg-C, um zu unterbrechen
Überprüfung der Datenträgerverwendung der Protokolldatei abgeschlossen. Die Nutzung beträgt <1 GB.

Roslaunch Server gestartet http: // mechatronics-990 : 41224 /

ZUSAMMENFASSUNG

KLARE PARAMETER

• / mavros /

PARAMETER

• / 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 / angle_velocity_stdev: 0,000349065850399
• / mavros / imu / frame_id: fcu
• / mavros / imu / linear_acceleration_stdev: 0,0003
• / mavros / imu / magnet_stdev: 0.0
• / mavros / imu / Orientierung_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: True
• / mavros / mission / pull_after_gcs: Richtig
• / mavros / mocap / use_pose: True
• / mavros / mocap / use_tf: False
• / 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 / security_area / p1 / x: 1.0
• / mavros / security_area / p1 / y: 1.0
• / mavros / security_area / p1 / z: 1.0
• / mavros / security_area / p2 / x: -1,0
• / mavros / security_area / p2 / y: -1,0
• / mavros / security_area / p2 / z: -1,0
• / mavros / setpoint_accel / send_force: False
• / mavros / setpoint_attitude / reverse_throttle: False
• / mavros / setpoint_attitude / tf / child_frame_id: Haltung
• / 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: Vibration
• / 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: Falsch
• / robot_description:
• / rosdistro: Indigo
• / rosversion: 1.11.21
• / tf_prefix:
• / use_sim_time: True

NODES
/.
Pavillon (Pavillon_ros / gzserver)
Pavillon_gui (Pavillon_ros / gzclient)
mavros (mavros / mavros_node)
spawn_erlecopter (Pavillon_ros / spawn_model)

Neustart neuer Master
Prozess [Master]: gestartet mit pid [28618]
ROS_MASTER_URI = http: // localhost : 11311

Setzen von / run_id auf e483c856-89bf-11e7-bb61-1803733b65bb
Prozess [Rosout-1]: begann mit PID [28631]
Core Service gestartet [/ rosout]
Prozess [mavros-2]: gestartet mit pid [28656]
Prozess [spawn_erlecopter-3]: gestartet mit pid [28657]
Prozess [Pavillon-4]: begann mit PID [28674]
Prozess [Pavillon_gui-5]: begann mit pid [28678]
Das Skript spawn_model wurde gestartet
[INFO] [WallTime: 1503684293.653120] [0.000000] Laden der Modell-XML aus dem ros-Parameter
[INFO] [WallTime: 1503684293.657344] [0.000000] Warten auf Service / Pavillon / Spawn_urdf_model
[INFO] [WallTime: 1503684294.260952] [0.047500] Service / Pavillon / spawn_urdf_model aufrufen
[INFO] [WallTime: 1503684294.855628] [0.445000] Spawn-Status: SpawnModel: Erfolgreich gespawntes Modell
Der Prozess [spawn_erlecopter-3] wurde sauber beendet
Protokolldatei: /home/bshang2/.ros/log/e483c856-89bf-11e7-bb61-1803733b65bb/spawn_erlecopter-3*.log

Diagnose

bshang2 @ mechatronics-990 : ~ / simulations / ardupilot / ArduCopter $ rostopic echo -n1 / diagnostics
Header:
seq: 6456
Stempel:
Sekunden: 6817
ns: 500000000
frame_id: ''
Status:

• 
  Stufe: 0
  name: mavros: FCU-Verbindung
  Nachricht: verbunden
  hardware_id: tcp: //127.0.0.1 : 5760
  Werte:
  
  
  
  • 
    
    Schlüssel: Empfangene Pakete:
    
    Wert: 24635
  
  
  • 
    
    Schlüssel: Verworfene Pakete:
    
    Wert: 0
  
  
  • 
    
    Schlüssel: Pufferüberläufe:
    
    Wert: 0
  
  
  • 
    
    Schlüssel: Analysefehler:
    
    Wert: 0
  
  
  • 
    
    Taste: Rx-Sequenznummer:
    
    Wert: 58
  
  
  • 
    
    Taste: Tx-Sequenznummer:
    
    Wert: 190
  
  
  • 
    
    Schlüssel: Rx Gesamtbytes:
    
    Wert: 19770100
  
  
  • 
    
    Schlüssel: Tx Gesamtbytes:
    
    Wert: 169295
  
  
  • 
    
    Taste: Rx Geschwindigkeit:
    
    Wert: inf
  
  
  • 
    
    Taste: Tx Geschwindigkeit:
    
    Wert: inf
  
  
• 
  Stufe: 0
  Name: Mavros: GCS-Brücke
  Nachricht: verbunden
  hardware_id: tcp: //127.0.0.1 : 5760
  Werte:
  
  
  
  • 
    
    Schlüssel: Empfangene Pakete:
    
    Wert: 3059
  
  
  • 
    
    Schlüssel: Verworfene Pakete:
    
    Wert: 0
  
  
  • 
    
    Schlüssel: Pufferüberläufe:
    
    Wert: 0
  
  
  • 
    
    Schlüssel: Analysefehler:
    
    Wert: 0
  
  
  • 
    
    Taste: Rx-Sequenznummer:
    
    Wert: 243
  
  
  • 
    
    Taste: Tx-Sequenznummer:
    
    Wert: 0
  
  
  • 
    
    Schlüssel: Rx Gesamtbytes:
    
    Wert: 51809
  
  
  • 
    
    Schlüssel: Tx Gesamtbytes:
    
    Wert: 19769923
  
  
  • 
    
    Taste: Rx Geschwindigkeit:
    
    Wert: inf
  
  
  • 
    
    Taste: Tx Geschwindigkeit:
    
    Wert: inf
  
  
• 
  Stufe: 0
  Name: Mavros: GPS
  Meldung: 3D Fix
  hardware_id: tcp: //127.0.0.1 : 5760
  Werte:
  
  
  
  • 
    
    Taste: Satelliten sichtbar
    
    Wert: 10
  
  
  • 
    
    Schlüssel: Fixtyp
    
    Wert: 3
  
  
  • 
    
    Schlüssel: EPH (m)
    
    Wert: 1,21
  
  
  • 
    
    Schlüssel: EPV (m)
    
    Wert: 2,00
  
  
• 
  Stufe: 0
  Name: Mavros: Herzschlag
  Meldung: Normal
  hardware_id: tcp: //127.0.0.1 : 5760
  Werte:
  
  
  
  • 
    
    Schlüssel: Herzschläge seit dem Start
    
    Wert: 6817
  
  
  • 
    
    Taste: Frequenz (Hz)
    
    Wert: 0,952381
  
  
  • 
    
    Schlüssel: Fahrzeugtyp
    
    Wert: Quadrotor
  
  
  • 
    
    Taste: Autopilot-Typ
    
    Wert: ArduPilotMega
  
  
  • 
    
    Taste: Modus
    
    Wert: GUIDED
  
  
  • 
    
    Schlüssel: Systemstatus
    
    Wert: Aktiv
  
  
• 
  Stufe: 0
  name: mavros: System
  Meldung: Normal
  hardware_id: tcp: //127.0.0.1 : 5760
  Werte:
  
  
  
  • 
    
    Taste: Sensor vorhanden
    
    Wert: 0x0061FD2F
  
  
  • 
    
    Taste: Sensor aktiviert
    
    Wert: 0x0061FD2F
  
  
  • 
    
    Schlüssel: Sensorhelth
    
    Wert: 0x0061FD2F
  
  
  • 
    
    Taste: Sensor 3D Gyro
    
    Wert: Ok
  
  
  • 
    
    Taste: Sensor 3D Accel
    
    Wert: Ok
  
  
  • 
    
    Taste: Sensor 3D Mag
    
    Wert: Ok
  
  
  • 
    
    Taste: Sensor Abs Druck
    
    Wert: Ok
  
  
  • 
    
    Taste: Sensor GPS
    
    Wert: Ok
  
  
  • 
    
    Taste: Position des Sensorlasers
    
    Wert: Ok
  
  
  • 
    
    Taste: Sensor Ang Rate Control
    
    Wert: Ok
  
  
  • 
    
    Taste: Sensor Attitude Stab
    
    Wert: Ok
  
  
  • 
    
    Taste: Gierposition des Sensors
    
    Wert: Ok
  
  
  • 
    
    Taste: Sensor Z / Alt-Steuerung
    
    Wert: Ok
  
  
  • 
    
    Taste: Sensor X / Y Pos Control
    
    Wert: Ok
  
  
  • 
    
    Taste: Sensormotorausgänge
    
    Wert: Ok
  
  
  • 
    
    Taste: Sensor RC Empfänger
    
    Wert: Ok
  
  
  • 
    
    Schlüssel: AHRS Gesundheit
    
    Wert: Ok
  
  
  • 
    
    Schlüssel: Geländegesundheit
    
    Wert: Ok
  
  
  • 
    
    Schlüssel: CPU-Auslastung (%)
    
    Wert: 0.0
  
  
  • 
    
    Schlüssel: Droprate (%)
    
    Wert: 0.0
  
  
  • 
    
    Schlüssel: Fehler comm
    
    Wert: 0
  
  
  • 
    
    Schlüssel: Fehler zählen # 1
    
    Wert: 0
  
  
  • 
    
    Schlüssel: Fehler zählen # 2
    
    Wert: 0
  
  
  • 
    
    Schlüssel: Fehler zählen # 3
    
    Wert: 0
  
  
  • 
    
    Schlüssel: Fehler zählen # 4
    
    Wert: 0
  
  
• 
  Level 1
  Name: Mavros: Batterie
  Meldung: Niederspannung
  hardware_id: tcp: //127.0.0.1 : 5760
  Werte:
  
  
  
  • 
    
    Taste: Spannung
    
    Wert: 0,00
  
  
  • 
    
    Taste: Aktuell
    
    Wert: 0.0
  
  
  • 
    
    Schlüssel: Verbleibend
  
  

Wert: 100,0

ID überprüfen

bshang2 @ mechatronics-990 : ~ / simulations / ardupilot / ArduCopter $ rosrun mavros checkid
OK. Ich habe Nachrichten von 1: 1 erhalten.

1231 Nachrichten von 1 Adressen empfangen
sys: comp Liste der Nachrichten
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