こんにちは、
私は視覚ベースの制御を行っており、カメラはドローンに下向きになっています。
したがって、位置推定は体のフレーム内にあります。
トピック/ 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と磁気の融合によるものなのかはわかりません。
これについてコメントやアイデアはありますか？

MAVROSのバージョンとプラットフォーム

マブロス：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を押して中断します
ログファイルのディスク使用量のチェックが完了しました。 使用量は1GB未満です。

roslaunchサーバーを開始しましたhttp：// mechatronics

概要

明確なパラメータ

• / mavros /

パラメーター

• / mavros / cmd / use_comp_id_system_control：False
• / mavros / conn / heartbeat_rate：1.0
• / mavros / conn / system_time_rate：0.0
• / mavros / conn /タイムアウト：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 / Magnetic_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：world
• / mavros / local_position / tf / send：True
• / mavros / Mission / pull_after_gcs：True
• / 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 / 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：態度
• / 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 /バイブレーション/ frame_id：バイブレーション
• / 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：インディゴ
• / rosversion：1.11.21
• / tf_prefix：
• / use_sim_time：True

ノード
/
ガゼボ（gazebo_ros / gzserver）
gazebo_gui（gazebo_ros / gzclient）
mavros（mavros / mavros_node）
spawn_erlecopter（gazebo_ros / spawn_model）

新しいマスターの自動起動
process [master]：pid [28094]で開始
ROS_MASTER_URI = http：// localhost ：11311

/ run_idをca9213bc-89bf-11e7-b657-1803733b65bbに設定します
process [rosout-1]：pid [28107]で開始
コアサービスを開始しました[/ rosout]
process [mavros-2]：pidで開始[28126]
プロセス[spawn_erlecopter-3]：pidで開始[28133]
プロセス[ガゼボ-4]：pidで開始[28134]
process [gazebo_gui-5]：pidで開始[28151]
spawn_modelスクリプトが開始されました
[gazebo-4]プロセスが終了しました[pid28134、終了コード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-1803733b65bb /gazebo-4.log]。
ログファイル：/home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/gazebo-4 .log[INFO] [WallTime：1503684250.172508] [0.000000] rosパラメーターからモデルxmlを読み込んでいます[情報] [WallTime：1503684250.176886] [0.000000]サービスを待機しています/ gazebo / spawn_urdf_model[gazebo_gui-5]プロセスが終了しました[pid28151、終了コード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]。ログファイル：/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
initial_pose、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 shutdown"）を発生させます
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を押して中断します
ログファイルのディスク使用量のチェックが完了しました。 使用量は1GB未満です。

roslaunchサーバーを開始しましたhttp：// mechatronics

概要

明確なパラメータ

• / mavros /

パラメーター

• / mavros / cmd / use_comp_id_system_control：False
• / mavros / conn / heartbeat_rate：1.0
• / mavros / conn / system_time_rate：0.0
• / mavros / conn /タイムアウト：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 / Magnetic_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：world
• / mavros / local_position / tf / send：True
• / mavros / Mission / pull_after_gcs：True
• / 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 / 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：態度
• / 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 /バイブレーション/ frame_id：バイブレーション
• / 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：インディゴ
• / rosversion：1.11.21
• / tf_prefix：
• / use_sim_time：True

ノード
/
ガゼボ（gazebo_ros / gzserver）
gazebo_gui（gazebo_ros / gzclient）
mavros（mavros / mavros_node）
spawn_erlecopter（gazebo_ros / spawn_model）

新しいマスターの自動起動
process [master]：pid [28618]で開始
ROS_MASTER_URI = http：// localhost ：11311

/ run_idをe483c856-89bf-11e7-bb61-1803733b65bbに設定する
process [rosout-1]：pid [28631]で開始
コアサービスを開始しました[/ rosout]
process [mavros-2]：pidで開始[28656]
プロセス[spawn_erlecopter-3]：pidで開始[28657]
プロセス[ガゼボ-4]：pidで開始[28674]
process [gazebo_gui-5]：pidで開始[28678]
spawn_modelスクリプトが開始されました
[INFO] [WallTime：1503684293.653120] [0.000000] rosパラメーターからモデルxmlを読み込んでいます
[情報] [WallTime：1503684293.657344] [0.000000]サービスを待機しています/ gazebo / spawn_urdf_model
[情報] [WallTime：1503684294.260952] [0.047500]通話サービス/ gazebo / 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 ：〜/ Simulation / ardupilot / ArduCopter $ rostopic echo -n1 / Diagnostics
ヘッダ：
seq：6456
切手：
秒：6817
nsecs：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
  
  
  • 
    
    キー：受信速度：
    
    値：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
  
  
  • 
    
    キー：受信速度：
    
    値：inf
  
  
  • 
    
    キー：送信速度：
    
    値：inf
  
  
• 
  レベル：0
  名前：mavros：GPS
  メッセージ：3D修正
  hardware_id： tcp：//127.0.0.1 ：5760
  値：
  
  
  
  • 
    
    キー：衛星が見える
    
    値：10
  
  
  • 
    
    キー：修正タイプ
    
    値：3
  
  
  • 
    
    キー：EPH（m）
    
    値：1.21
  
  
  • 
    
    キー：EPV（m）
    
    値：2.00
  
  
• 
  レベル：0
  名前：mavros：ハートビート
  メッセージ：通常
  hardware_id： tcp：//127.0.0.1 ：5760
  値：
  
  
  
  • 
    
    キー：起動時からのハートビート
    
    値：6817
  
  
  • 
    
    キー：周波数（Hz）
    
    値：0.952381
  
  
  • 
    
    キー：車両タイプ
    
    値：クワッドローター
  
  
  • 
    
    キー：自動操縦タイプ
    
    値：ArduPilotMega
  
  
  • 
    
    キー：モード
    
    値：GUIDED
  
  
  • 
    
    キー：システムステータス
    
    値：アクティブ
  
  
• 
  レベル：0
  名前：mavros：システム
  メッセージ：通常
  hardware_id： tcp：//127.0.0.1 ：5760
  値：
  
  
  
  • 
    
    キー：センサーが存在します
    
    値：0x0061FD2F
  
  
  • 
    
    キー：センサーが有効
    
    値：0x0061FD2F
  
  
  • 
    
    キー：センサーヘルス
    
    値：0x0061FD2F
  
  
  • 
    
    キー：センサー3Dジャイロ
    
    値：わかりました
  
  
  • 
    
    キー：センサー3Dアクセル
    
    値：わかりました
  
  
  • 
    
    キー：センサー3Dマグ
    
    値：わかりました
  
  
  • 
    
    キー：センサー絶対圧
    
    値：わかりました
  
  
  • 
    
    キー：センサーGPS
    
    値：わかりました
  
  
  • 
    
    キー：センサーレーザー位置
    
    値：わかりました
  
  
  • 
    
    キー：センサー角度レート制御
    
    値：わかりました
  
  
  • 
    
    キー：センサー姿勢スタブ
    
    値：わかりました
  
  
  • 
    
    キー：センサーヨー位置
    
    値：わかりました
  
  
  • 
    
    キー：センサーZ / Altコントロール
    
    値：わかりました
  
  
  • 
    
    キー：センサーX / Y位置制御
    
    値：わかりました
  
  
  • 
    
    キー：センサーモーター出力
    
    値：わかりました
  
  
  • 
    
    キー：センサーRCレシーバー
    
    値：わかりました
  
  
  • 
    
    キー：AHRSの健康
    
    値：わかりました
  
  
  • 
    
    キー：地形の健康
    
    値：わかりました
  
  
  • 
    
    キー：CPU負荷（％）
    
    値：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 ：〜/ Simulation / ardupilot / ArduCopter $ rosrun mavros checkid
OK。 1：1からメッセージが届きました。

1つのアドレスから1231のメッセージを受信しました
sys：compメッセージのリスト
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