Hi,
I am doing vision-based control and the camera is on the drone facing down.
So the position estimation is in body frame.
I found the topic /mavros/setpoint_velocity/cmd_vel can be used to change the drone's speed but it seems it is in NED frame.

Is there a topic like /mavros/setpoint_velocity/cmd_vel but use body frame?

Or I need to convert my speed setpoint from body frame to NED frame to implement it?

I tried to find something that I can use and I found this 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);

It seems this is the best solution I could find.
I am not sure if the Yaw is from IMU only or from fusion of IMU and magnetic.
Do you have some comments or ideas about this?

MAVROS version and platform

Mavros: 0.17.3
ROS: Indigo
Ubuntu: 14.04

Autopilot type and version

APM:Copter V3.4-dev (fe724032)

Node logs

bshang2@mechatronics-990:~/simulation/ardupilot/ArduCopter$ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_mark.launch
... logging to /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/roslaunch-mechatronics-990-28079.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://mechatronics-990:45006/

SUMMARY

CLEAR PARAMETERS

• /mavros/

PARAMETERS

• /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/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: attitude
• /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: False
• /robot_description:
• /rosdistro: indigo
• /rosversion: 1.11.21
• /tf_prefix:
• /use_sim_time: True

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

auto-starting new master
process[master]: started with pid [28094]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to ca9213bc-89bf-11e7-b657-1803733b65bb
process[rosout-1]: started with pid [28107]
started core service [/rosout]
process[mavros-2]: started with pid [28126]
process[spawn_erlecopter-3]: started with pid [28133]
process[gazebo-4]: started with pid [28134]
process[gazebo_gui-5]: started with pid [28151]
spawn_model script started
[gazebo-4] process has died [pid 28134, exit code 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].
log file: /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/gazebo-4.log
[INFO] [WallTime: 1503684250.172508] [0.000000] Loading model xml from ros parameter
[INFO] [WallTime: 1503684250.176886] [0.000000] Waiting for service /gazebo/spawn_urdf_model
[gazebo_gui-5] process has died [pid 28151, exit code 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].
log file: /home/bshang2/.ros/log/ca9213bc-89bf-11e7-b657-1803733b65bb/gazebo_gui-5.log
^C[spawn_erlecopter-3] killing on exit
[mavros-2] killing on exit
Traceback (most recent call last):
File "/home/bshang2/simulation/ros_catkin_ws/src/gazebo_ros_pkgs/gazebo_ros/scripts/spawn_model", line 301, in
sm.callSpawnService()
File "/home/bshang2/simulation/ros_catkin_ws/src/gazebo_ros_pkgs/gazebo_ros/scripts/spawn_model", line 267, in callSpawnService
initial_pose, self.reference_frame, self.gazebo_namespace)
File "/home/bshang2/simulation/ros_catkin_ws/src/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/gazebo_interface.py", line 28, in spawn_urdf_model_client
rospy.wait_for_service(gazebo_namespace+'/spawn_urdf_model')
File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 159, in wait_for_service
raise ROSInterruptException("rospy shutdown")
rospy.exceptions.ROSInterruptException: rospy shutdown
^C[rosout-1] killing on exit
^C[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done
bshang2@mechatronics-990:~/simulation/ardupilot/ArduCopter$ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_mark.launch
... logging to /home/bshang2/.ros/log/e483c856-89bf-11e7-bb61-1803733b65bb/roslaunch-mechatronics-990-28603.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://mechatronics-990:41224/

SUMMARY

CLEAR PARAMETERS

• /mavros/

PARAMETERS

• /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/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: attitude
• /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: False
• /robot_description:
• /rosdistro: indigo
• /rosversion: 1.11.21
• /tf_prefix:
• /use_sim_time: True

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

auto-starting new master
process[master]: started with pid [28618]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to e483c856-89bf-11e7-bb61-1803733b65bb
process[rosout-1]: started with pid [28631]
started core service [/rosout]
process[mavros-2]: started with pid [28656]
process[spawn_erlecopter-3]: started with pid [28657]
process[gazebo-4]: started with pid [28674]
process[gazebo_gui-5]: started with pid [28678]
spawn_model script started
[INFO] [WallTime: 1503684293.653120] [0.000000] Loading model xml from ros parameter
[INFO] [WallTime: 1503684293.657344] [0.000000] Waiting for service /gazebo/spawn_urdf_model
[INFO] [WallTime: 1503684294.260952] [0.047500] Calling service /gazebo/spawn_urdf_model
[INFO] [WallTime: 1503684294.855628] [0.445000] Spawn status: SpawnModel: Successfully spawned model
[spawn_erlecopter-3] process has finished cleanly
log file: /home/bshang2/.ros/log/e483c856-89bf-11e7-bb61-1803733b65bb/spawn_erlecopter-3*.log

Diagnostics

bshang2@mechatronics-990:~/simulation/ardupilot/ArduCopter$ rostopic echo -n1 /diagnostics
header:
seq: 6456
stamp:
secs: 6817
nsecs: 500000000
frame_id: ''
status:

• 
  level: 0
  name: mavros: FCU connection
  message: connected
  hardware_id: tcp://127.0.0.1:5760
  values:
  
  
  
  • 
    
    key: Received packets:
    
    value: 24635
  
  
  • 
    
    key: Dropped packets:
    
    value: 0
  
  
  • 
    
    key: Buffer overruns:
    
    value: 0
  
  
  • 
    
    key: Parse errors:
    
    value: 0
  
  
  • 
    
    key: Rx sequence number:
    
    value: 58
  
  
  • 
    
    key: Tx sequence number:
    
    value: 190
  
  
  • 
    
    key: Rx total bytes:
    
    value: 19770100
  
  
  • 
    
    key: Tx total bytes:
    
    value: 169295
  
  
  • 
    
    key: Rx speed:
    
    value: inf
  
  
  • 
    
    key: Tx speed:
    
    value: inf
  
  
• 
  level: 0
  name: mavros: GCS bridge
  message: connected
  hardware_id: tcp://127.0.0.1:5760
  values:
  
  
  
  • 
    
    key: Received packets:
    
    value: 3059
  
  
  • 
    
    key: Dropped packets:
    
    value: 0
  
  
  • 
    
    key: Buffer overruns:
    
    value: 0
  
  
  • 
    
    key: Parse errors:
    
    value: 0
  
  
  • 
    
    key: Rx sequence number:
    
    value: 243
  
  
  • 
    
    key: Tx sequence number:
    
    value: 0
  
  
  • 
    
    key: Rx total bytes:
    
    value: 51809
  
  
  • 
    
    key: Tx total bytes:
    
    value: 19769923
  
  
  • 
    
    key: Rx speed:
    
    value: inf
  
  
  • 
    
    key: Tx speed:
    
    value: inf
  
  
• 
  level: 0
  name: mavros: GPS
  message: 3D fix
  hardware_id: tcp://127.0.0.1:5760
  values:
  
  
  
  • 
    
    key: Satellites visible
    
    value: 10
  
  
  • 
    
    key: Fix type
    
    value: 3
  
  
  • 
    
    key: EPH (m)
    
    value: 1.21
  
  
  • 
    
    key: EPV (m)
    
    value: 2.00
  
  
• 
  level: 0
  name: mavros: Heartbeat
  message: Normal
  hardware_id: tcp://127.0.0.1:5760
  values:
  
  
  
  • 
    
    key: Heartbeats since startup
    
    value: 6817
  
  
  • 
    
    key: Frequency (Hz)
    
    value: 0.952381
  
  
  • 
    
    key: Vehicle type
    
    value: Quadrotor
  
  
  • 
    
    key: Autopilot type
    
    value: ArduPilotMega
  
  
  • 
    
    key: Mode
    
    value: GUIDED
  
  
  • 
    
    key: System status
    
    value: Active
  
  
• 
  level: 0
  name: mavros: System
  message: Normal
  hardware_id: tcp://127.0.0.1:5760
  values:
  
  
  
  • 
    
    key: Sensor present
    
    value: 0x0061FD2F
  
  
  • 
    
    key: Sensor enabled
    
    value: 0x0061FD2F
  
  
  • 
    
    key: Sensor helth
    
    value: 0x0061FD2F
  
  
  • 
    
    key: Sensor 3D Gyro
    
    value: Ok
  
  
  • 
    
    key: Sensor 3D Accel
    
    value: Ok
  
  
  • 
    
    key: Sensor 3D Mag
    
    value: Ok
  
  
  • 
    
    key: Sensor Abs Pressure
    
    value: Ok
  
  
  • 
    
    key: Sensor GPS
    
    value: Ok
  
  
  • 
    
    key: Sensor Laser Position
    
    value: Ok
  
  
  • 
    
    key: Sensor Ang Rate Control
    
    value: Ok
  
  
  • 
    
    key: Sensor Attitude Stab
    
    value: Ok
  
  
  • 
    
    key: Sensor Yaw Position
    
    value: Ok
  
  
  • 
    
    key: Sensor Z/Alt Control
    
    value: Ok
  
  
  • 
    
    key: Sensor X/Y Pos Control
    
    value: Ok
  
  
  • 
    
    key: Sensor Motor Outputs
    
    value: Ok
  
  
  • 
    
    key: Sensor RC Receiver
    
    value: Ok
  
  
  • 
    
    key: AHRS health
    
    value: Ok
  
  
  • 
    
    key: Terrain health
    
    value: Ok
  
  
  • 
    
    key: CPU Load (%)
    
    value: 0.0
  
  
  • 
    
    key: Drop rate (%)
    
    value: 0.0
  
  
  • 
    
    key: Errors comm
    
    value: 0
  
  
  • 
    
    key: Errors count #1
    
    value: 0
  
  
  • 
    
    key: Errors count #2
    
    value: 0
  
  
  • 
    
    key: Errors count #3
    
    value: 0
  
  
  • 
    
    key: Errors count #4
    
    value: 0
  
  
• 
  level: 1
  name: mavros: Battery
  message: Low voltage
  hardware_id: tcp://127.0.0.1:5760
  values:
  
  
  
  • 
    
    key: Voltage
    
    value: 0.00
  
  
  • 
    
    key: Current
    
    value: 0.0
  
  
  • 
    
    key: Remaining
  
  

value: 100.0

Check ID

bshang2@mechatronics-990:~/simulation/ardupilot/ArduCopter$ rosrun mavros checkid
OK. I got messages from 1:1.

Received 1231 messages, from 1 addresses
sys:comp list of 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