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

cnpcshangbo · 2017-08-25T18:52:03Z

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: <?xml version="1....
/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]
[ INFO] [1503684249.765286316]: FCU URL: tcp://127.0.0.1:5760
[ INFO] [1503684249.765864009]: tcp0: Server address: 127.0.0.1:5760
[ INFO] [1503684249.766140880]: GCS URL: udp://127.0.0.1@127.0.0.1:14550
[ INFO] [1503684249.766327994]: udp1: Bind address: 127.0.0.1:14555
[ INFO] [1503684249.766384820]: udp1: Remote address: 127.0.0.1:14550
[ INFO] [1503684249.816486192]: Plugin 3dr_radio loaded and initialized
[ INFO] [1503684249.816691790]: Plugin actuator_control blacklisted
[ INFO] [1503684249.816709978]: Plugin altitude blacklisted
[ INFO] [1503684249.883071398]: Plugin cam_imu_sync loaded and initialized
[ INFO] [1503684249.903105017]: Plugin command loaded and initialized
[ INFO] [1503684249.903249482]: Plugin distance_sensor blacklisted
[ INFO] [1503684249.903266326]: Plugin ftp blacklisted
[ INFO] [1503684249.924388528]: Plugin global_position loaded and initialized
[ INFO] [1503684249.924549872]: Plugin image_pub blacklisted
[ INFO] [1503684249.941592560]: Plugin imu_pub loaded and initialized
[ INFO] [1503684249.955208241]: Plugin local_position loaded and initialized
[ INFO] [1503684249.956367782]: Plugin manual_control loaded and initialized
[ INFO] [1503684249.956511794]: Plugin mocap_pose_estimate blacklisted
[ INFO] [1503684249.961173388]: Plugin param loaded and initialized
[ INFO] [1503684249.961293345]: Plugin px4flow blacklisted
[ INFO] [1503684249.983169639]: Plugin rc_io loaded and initialized
[ INFO] [1503684249.983295772]: Plugin safety_area blacklisted
[ INFO] [1503684250.015071242]: Plugin setpoint_accel loaded and initialized
[ INFO] [1503684250.015143442]: Plugin setpoint_attitude blacklisted
[ INFO] [1503684250.035009570]: Plugin setpoint_position loaded and initialized
spawn_model script started
[ INFO] [1503684250.049028815]: Plugin setpoint_raw loaded and initialized
[ INFO] [1503684250.052631329]: Plugin setpoint_velocity loaded and initialized
[ INFO] [1503684250.071711536]: Plugin sys_status loaded and initialized
[ INFO] [1503684250.078658041]: Plugin sys_time loaded and initialized
[ INFO] [1503684250.080268204]: Plugin vfr_hud loaded and initialized
[ INFO] [1503684250.080390651]: Plugin vibration blacklisted
[ INFO] [1503684250.080409746]: Plugin vision_pose_estimate blacklisted
[ INFO] [1503684250.080424743]: Plugin vision_speed_estimate blacklisted
[ INFO] [1503684250.084729079]: Plugin waypoint loaded and initialized
[ INFO] [1503684250.084852807]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1503684250.084872142]: Built-in MAVLink package version: 2016.5.20
[ INFO] [1503684250.084889546]: Built-in MAVLink dialect: ardupilotmega
[ INFO] [1503684250.084903251]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[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: <?xml version="1....
/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]
[ INFO] [1503684293.270923068]: FCU URL: tcp://127.0.0.1:5760
[ INFO] [1503684293.271522355]: tcp0: Server address: 127.0.0.1:5760
[ INFO] [1503684293.271839293]: GCS URL: udp://127.0.0.1@127.0.0.1:14550
[ INFO] [1503684293.272026985]: udp1: Bind address: 127.0.0.1:14555
[ INFO] [1503684293.272084067]: udp1: Remote address: 127.0.0.1:14550
[ INFO] [1503684293.316437121]: Plugin 3dr_radio loaded and initialized
[ INFO] [1503684293.316655124]: Plugin actuator_control blacklisted
[ INFO] [1503684293.316674160]: Plugin altitude blacklisted
[ INFO] [1503684293.368046663]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1503684293.368413553]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1503684293.369102627]: Plugin cam_imu_sync loaded and initialized
[ INFO] [1503684293.375509896]: Plugin command loaded and initialized
[ INFO] [1503684293.375654308]: Plugin distance_sensor blacklisted
[ INFO] [1503684293.375671107]: Plugin ftp blacklisted
[ INFO] [1503684293.387643967]: Plugin global_position loaded and initialized
[ INFO] [1503684293.387786887]: Plugin image_pub blacklisted
[ INFO] [1503684293.398038357]: Plugin imu_pub loaded and initialized
[ INFO] [1503684293.405953305]: Plugin local_position loaded and initialized
[ INFO] [1503684293.407028686]: Plugin manual_control loaded and initialized
[ INFO] [1503684293.407163774]: Plugin mocap_pose_estimate blacklisted
[ INFO] [1503684293.410446205]: Plugin param loaded and initialized
[ INFO] [1503684293.410566347]: Plugin px4flow blacklisted
[ INFO] [1503684293.416021722]: Plugin rc_io loaded and initialized
[ INFO] [1503684293.416164233]: Plugin safety_area blacklisted
[ INFO] [1503684293.420494818]: Plugin setpoint_accel loaded and initialized
[ INFO] [1503684293.420548823]: Plugin setpoint_attitude blacklisted
[ INFO] [1503684293.427379607]: Plugin setpoint_position loaded and initialized
[ INFO] [1503684293.437025470]: Plugin setpoint_raw loaded and initialized
[ INFO] [1503684293.440323065]: Plugin setpoint_velocity loaded and initialized
[ INFO] [1503684293.448539748]: Plugin sys_status loaded and initialized
[ INFO] [1503684293.454509017]: Plugin sys_time loaded and initialized
[ INFO] [1503684293.455937215]: Plugin vfr_hud loaded and initialized
[ INFO] [1503684293.456035918]: Plugin vibration blacklisted
[ INFO] [1503684293.456057968]: Plugin vision_pose_estimate blacklisted
[ INFO] [1503684293.456072602]: Plugin vision_speed_estimate blacklisted
[ INFO] [1503684293.460453738]: Plugin waypoint loaded and initialized
[ INFO] [1503684293.460584429]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1503684293.460606805]: Built-in MAVLink package version: 2016.5.20
[ INFO] [1503684293.460621936]: Built-in MAVLink dialect: ardupilotmega
[ INFO] [1503684293.460636416]: MAVROS started. MY ID 1.240, TARGET ID 1.1
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] [1503684294.118622116]: Model name: erlecopter
[ INFO] [1503684294.118667420]: Nb motor servos: 4
[ INFO] [1503684294.139894667]: ARI: Services declared !
[ INFO] [1503684294.139947494]: ARI: Binding to listening port from ArduPilot...

[ INFO] [1503684294.139991349]: ARI: SUCCESS in binding to port from ArduPilot

[ INFO] [1503684294.140031062]: ARI: Connecting send port to ArduPilot...

[ INFO] [1503684294.140063208]: ARI: Opened ArduPilot fdm socket

[ INFO] [1503684294.140108275]: ARI: Initialization finished
[ INFO] [1503684294.140480187]: ARI: Starting listening loop for ArduPilot messages
[ INFO] [1503684294.240874709]: ARI: Connected with Ardupilot
[ INFO] [1503684294.241596408]: ARI: Simulation step size is = 0.002500
[ INFO] [1503684294.251601544, 0.027500000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[INFO] [WallTime: 1503684294.260952] [0.047500] Calling service /gazebo/spawn_urdf_model
[ INFO] [1503684294.755607220, 0.445000000]: Camera Plugin (robotNamespace = /), Info: Using the 'robotNamespace' param: '/'
[ INFO] [1503684294.758647818, 0.445000000]: Camera Plugin (robotNamespace = /), Info: Using the 'robotNamespace' param: '/'
[ INFO] [1503684294.761119346, 0.445000000]: Camera Plugin (ns = /) <tf_prefix_>, set to ""
[ INFO] [1503684294.764832978, 0.445000000]: Camera Plugin (ns = /) <tf_prefix_>, set to ""
[ INFO] [1503684294.844909849, 0.445000000]: Laser Plugin (robotNamespace = /), Info: Using the 'robotNamespace' param: '/'
[ INFO] [1503684294.844991467, 0.445000000]: Starting Laser Plugin (ns = /)!
[ INFO] [1503684294.847513684, 0.445000000]: Laser Plugin (ns = /) <tf_prefix_>, set to ""
[INFO] [WallTime: 1503684294.855628] [0.445000] Spawn status: SpawnModel: Successfully spawned model
[ INFO] [1503684294.964623228, 0.445000000]: Physics dynamic reconfigure ready.
[spawn_erlecopter-3] process has finished cleanly
log file: /home/bshang2/.ros/log/e483c856-89bf-11e7-bb61-1803733b65bb/spawn_erlecopter-3*.log
[ INFO] [1503684296.337491654, 2.522500000]: CON: Got HEARTBEAT, connected. FCU: ArduPilotMega
[ INFO] [1503684296.338490573, 2.522500000]: udp1: Remote address: 127.0.0.1:14550
[ WARN] [1503684296.348632231, 2.545000000]: FCU: APM:Copter V3.4-dev (fe724032)
[ WARN] [1503684296.348745018, 2.545000000]: FCU: Frame: QUAD
[ WARN] [1503684296.550695772, 3.020000000]: FCU: Calibrating barometer
[ INFO] [1503684296.764335066, 3.547500000]: VER: 1.1: Capabilities 0x0000000000000000
[ INFO] [1503684296.764380465, 3.547500000]: VER: 1.1: Flight software: 03040000 (fe724032)
[ INFO] [1503684296.764408499, 3.547500000]: VER: 1.1: Middleware software: 00000000 ( )
[ INFO] [1503684296.764429987, 3.547500000]: VER: 1.1: OS software: 00000000 ( )
[ INFO] [1503684296.764452548, 3.547500000]: VER: 1.1: Board hardware: 00000000
[ INFO] [1503684296.764474134, 3.547500000]: VER: 1.1: VID/PID: 0000:0000
[ INFO] [1503684296.764496758, 3.547500000]: VER: 1.1: UID: 0000000000000000
[ WARN] [1503684297.404322882, 5.002500000]: FCU: Initialising APM...
[ WARN] [1503684298.239623583, 7.020000000]: FCU: barometer calibration complete
[ WARN] [1503684300.512804258, 12.525000000]: FCU: APM:Copter V3.4-dev (fe724032)
[ WARN] [1503684300.512935826, 12.525000000]: FCU: Frame: QUAD
[ INFO] [1503684302.577748967, 17.525000000]: WP: mission received
[ INFO] [1503684305.481523641, 24.352500000]: PR: parameters list received
[ WARN] [1503684705.763431138, 978.327500000]: CMD: Unexpected command 11, result 0
[ERROR] [1503684705.804517830, 978.352500000]: FCU: ARMING MOTORS
[ WARN] [1503684705.805724250, 978.355000000]: FCU: Initialising APM...
[ERROR] [1503684708.561672430, 984.995000000]: FCU: ARMING MOTORS
[ WARN] [1503684708.597513790, 985.085000000]: FCU: Initialising APM...
[ERROR] [1503684709.581542897, 987.485000000]: FCU: DISARMING MOTORS
[ WARN] [1503684711.364508932, 991.827500000]: CMD: Unexpected command 22, result 4
[ERROR] [1503684717.531651813, 1006.802500000]: FCU: ARMING MOTORS
[ WARN] [1503684717.532989033, 1006.805000000]: FCU: Initialising APM...
[ WARN] [1503684717.561330955, 1006.877500000]: CMD: Unexpected command 400, result 0
[ WARN] [1503685348.138122431, 2500.825000000]: CMD: Unexpected command 115, result 0
[ WARN] [1503685354.569905210, 2516.342500000]: CMD: Unexpected command 115, result 0
[ WARN] [1503685370.593756643, 2555.272500000]: CMD: Unexpected command 115, result 0
[ WARN] [1503685381.372317453, 2581.505000000]: CMD: Unexpected command 115, result 0
[ WARN] [1503685402.423626155, 2632.007500000]: CMD: Unexpected command 115, result 0
[ WARN] [1503685523.819595670, 2918.257500000]: CMD: Unexpected command 11, result 0
[ WARN] [1503685553.477360205, 2989.897500000]: CMD: Unexpected command 11, result 0
[ WARN] [1503685598.683688100, 3098.957500000]: CMD: Unexpected command 115, result 0
[ WARN] [1503685610.214628138, 3126.885000000]: CMD: Unexpected command 115, result 0
[ WARN] [1503685619.466795587, 3149.237500000]: CMD: Unexpected command 115, result 0
[ERROR] [1503685625.383122086, 3163.642500000]: FCU: ARMING MOTORS
[ WARN] [1503685625.413260459, 3163.715000000]: CMD: Unexpected command 400, result 0
[ WARN] [1503685625.417621559, 3163.725000000]: FCU: Initialising APM...
[ WARN] [1503685631.946502396, 3179.470000000]: CMD: Unexpected command 22, result 4

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 mavconn library. #1
  value: 0
- key: Errors count Pub/sub for Mavlink.msg #2
  value: 0
- key: Errors count Plugin loading functionality. #3
  value: 0
- key: Errors count Diagnostic mesages. #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

The text was updated successfully, but these errors were encountered:

AlexisTM · 2017-08-28T08:35:29Z

Hey there,

You can use the setpoint_raw.

http://wiki.ros.org/mavros#mavros.2BAC8-Plugins.setpoint_raw

On the target setpoint, you can change the coordinate frame.

http://docs.ros.org/api/mavros_msgs/html/msg/PositionTarget.html

TSC21 · 2017-08-28T11:29:17Z

For setpoint_velocity, I will add a fix which will allows choosing between the LOCAL_NED and the BODY_NED, since it currently is defaulted to LOCAL_NED (https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/setpoint_velocity.cpp#L81). In the mean time, you can use setpoint_raw and set the coordinate_frame to FRAME_BODY_NED or MAV_FRAME::BODY_NED (both should work).

cnpcshangbo · 2017-08-31T00:33:32Z

Thanks @AlexisTM and @TSC21 Your reply is helpful. I will try that later.

cnpcshangbo · 2017-09-08T07:05:37Z

@TSC21 @AlexisTM Thanks for your help! I can control the velocity in body frame now. Video is recorded: https://www.youtube.com/watch?v=dyUQfFYX3pk&feature=youtu.be

rostopic pub /mavros/setpoint_raw/local mavros_msgs/PositionTarget '{header: {stamp: now, frame_id: "world"}, coordinate_frame: 8, type_mask: 3527, velocity: {x: 0.1, y: 0, z: 0}}' -r 10

TSC21 · 2017-09-08T08:47:34Z

@cnpcshangbo good to know

hmchung · 2018-08-17T03:09:46Z

hi @cnpcshangbo, thank you for the video.
I post this to help others clearing a confusion that I had.
Even though the frame mask is FRAME_BODY_NED = 8 , the ROS command should still be in ENU . The ENU -> NED conversion is taken care by mavros.
In fact, from the video, the behavior is:

x = 0.1 ---> To the right
y = 0.1 ---> Forward
Obviously it is still in ENU. However, this confusion cost me a week of pulling hair...

cnpcshangbo · 2018-08-19T23:22:55Z

Hi Hoang, Thank you for your clarification. Sorry to hear that it took you so long time. Your efforts help other. Thanks Bo Hoang Minh Chung <notifications@github.com>于2018年8月17日周五上午11:09写道：

…

hi @cnpcshangbo <https://github.com/cnpcshangbo>, thank you for the video. I post this to help others clearing a confusion that I had. Even though the frame mask is "FRAME_BODY_NED = 8", the *ROS command should still be in ENU* . The ENU -> NED conversion will be done by mavros. In fact, from the video, the behavior is: - x = 0.1 ---> To the right - y = 0.1 ---> Forward Obviously it is still in ENU. However, this confusion cost me a week of pulling hair... — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#792 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AEm3NdefOViosyJKxQlf2h0ZweC8lO6uks5uRjP9gaJpZM4PDCb2> .

AlexisTM · 2018-08-20T05:25:03Z

Isn't it supposed to be x forward and y to the left for BODY and x to East, y to North in the map frame @nuno?

hmchung · 2018-08-20T06:30:26Z

So am I correct that we actually have 3 conventions here:

NED: PX4 onboard estimation and control
ENU: standard input format for mavros command messages
NWU: Typical coordinate system in ROS

TSC21 · 2018-08-20T06:32:51Z

No. There's no NWU in ROS.
http://www.ros.org/reps/rep-0103.html

TSC21 · 2018-08-20T06:34:50Z

@AlexisTM north or east should no be used to specify body conventions, but rather orientations like front, left.

hmchung · 2018-08-20T06:46:31Z

Okay. But if I am not wrong, I think we still have 3 conventions?

NED: Onboard control
Right-Forward-Up: How mavros understands raw (velocity) setpoints
Forward-Left-Up: ROS convention

AlexisTM · 2018-08-20T07:21:23Z

@hmchung We are using velocity setpoints within the map frame and it is ENU (ROS convention)

Note that there should be only 2 conventions! NED on the Pixhawk and ENU: anything you do on ROS.

TSC21 · 2018-08-20T08:02:15Z

Forward-Left-Up (FLU) is the ROS convention for the body frame. East-North-Up (ENU) is the ROS convention for the local/word frame.

Ali76farahmand · 2023-12-19T12:08:07Z

For setpoint_velocity, I will add a fix which will allows choosing between the LOCAL_NED and the BODY_NED, since it currently is defaulted to LOCAL_NED (https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/setpoint_velocity.cpp#L81). In the mean time, you can use setpoint_raw and set the coordinate_frame to FRAME_BODY_NED or MAV_FRAME::BODY_NED (both should work).

both of LOCAL_NED and BODY_NED have the same attitude (north, east, down). is there any way to command the drone in FRD (front, right, down) coordinate frame?

vooon added the question label Aug 25, 2017

TSC21 closed this as completed Aug 28, 2017

tomasgareau mentioned this issue Feb 18, 2020

Broken ros convention with velocities in body frame #1382

Open

Scoeerg mentioned this issue Jul 3, 2023

Question: ros2 topic info /mavros/setpoint_velocity/cmd_vel(_unstamped) - Ardupilot Mode? #1870

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

cnpcshangbo commented Aug 25, 2017 •

edited

level: 0
name: mavros: GCS bridge
message: connected
hardware_id: tcp://127.0.0.1:5760
values:

level: 0
name: mavros: GPS
message: 3D fix
hardware_id: tcp://127.0.0.1:5760
values:

level: 0
name: mavros: Heartbeat
message: Normal
hardware_id: tcp://127.0.0.1:5760
values:

level: 0
name: mavros: System
message: Normal
hardware_id: tcp://127.0.0.1:5760
values:

level: 1
name: mavros: Battery
message: Low voltage
hardware_id: tcp://127.0.0.1:5760
values:

AlexisTM commented Aug 28, 2017

TSC21 commented Aug 28, 2017 •

edited

cnpcshangbo commented Aug 31, 2017

cnpcshangbo commented Sep 8, 2017

TSC21 commented Sep 8, 2017

hmchung commented Aug 17, 2018 •

edited

cnpcshangbo commented Aug 19, 2018 via email

AlexisTM commented Aug 20, 2018

hmchung commented Aug 20, 2018 •

edited

TSC21 commented Aug 20, 2018 •

edited

TSC21 commented Aug 20, 2018

hmchung commented Aug 20, 2018

AlexisTM commented Aug 20, 2018

TSC21 commented Aug 20, 2018

Ali76farahmand commented Dec 19, 2023

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

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

Comments

cnpcshangbo commented Aug 25, 2017 • edited

MAVROS version and platform

Autopilot type and version

Node logs

SUMMARY

SUMMARY

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: GCS bridge message: connected hardware_id: tcp://127.0.0.1:5760 values:

level: 0 name: mavros: GPS message: 3D fix hardware_id: tcp://127.0.0.1:5760 values:

level: 0 name: mavros: Heartbeat message: Normal hardware_id: tcp://127.0.0.1:5760 values:

level: 0 name: mavros: System message: Normal hardware_id: tcp://127.0.0.1:5760 values:

level: 1 name: mavros: Battery message: Low voltage hardware_id: tcp://127.0.0.1:5760 values:

Check ID

AlexisTM commented Aug 28, 2017

TSC21 commented Aug 28, 2017 • edited

cnpcshangbo commented Aug 31, 2017

cnpcshangbo commented Sep 8, 2017

TSC21 commented Sep 8, 2017

hmchung commented Aug 17, 2018 • edited

cnpcshangbo commented Aug 19, 2018 via email

AlexisTM commented Aug 20, 2018

hmchung commented Aug 20, 2018 • edited

TSC21 commented Aug 20, 2018 • edited

TSC21 commented Aug 20, 2018

hmchung commented Aug 20, 2018

AlexisTM commented Aug 20, 2018

TSC21 commented Aug 20, 2018

Ali76farahmand commented Dec 19, 2023

cnpcshangbo commented Aug 25, 2017 •

edited

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: GCS bridge
message: connected
hardware_id: tcp://127.0.0.1:5760
values:

level: 0
name: mavros: GPS
message: 3D fix
hardware_id: tcp://127.0.0.1:5760
values:

level: 0
name: mavros: Heartbeat
message: Normal
hardware_id: tcp://127.0.0.1:5760
values:

level: 0
name: mavros: System
message: Normal
hardware_id: tcp://127.0.0.1:5760
values:

level: 1
name: mavros: Battery
message: Low voltage
hardware_id: tcp://127.0.0.1:5760
values:

TSC21 commented Aug 28, 2017 •

edited

hmchung commented Aug 17, 2018 •

edited

hmchung commented Aug 20, 2018 •

edited

TSC21 commented Aug 20, 2018 •

edited