Hi I have an issue sending vehicle states specifically attitude to px4 over mavros. I am using ros melodic.
I am using iris quad in HIL mode with an RP-Lidar to perform hector SLAM.
I have a hector slam node that is publishing vehicle pose onto the default /slam_out_pose in the ROS ENU convention.
I then have a node that takes the xy position and attitude quarternion from SLAM and then altitude data from a separate sensor and publishes them onto /mavros/vision_pose/pose.
I can see data in the mavros/local_position/pose that agrees with what is published on vision_pose.
The attitude output from SLAM is a unit quarternion as i would expect to begin. However when i use qground to inspect the attitude on PX4 side i get a weird value of [0.707 0 0 0.707] suggesting the vehicle is rolled by 90 degrees.
Something is wrong with the mavros coordinate transform and I’m pretty sure it stems from my tf tree.
The documentation for set up is shown at this link: Using Vision or Motion Capture Systems for Position Estimation | PX4 User Guide
Below are my px4_config.yaml and tf tree.
Ive been trying to figure this out for a week and its driving me insane haha.
# Common configuration for PX4 autopilot
#
# node:
startup_px4_usb_quirk: true
# --- system plugins ---
# sys_status & sys_time connection options
conn:
heartbeat_rate: 1.0 # send hertbeat rate in Hertz
timeout: 10.0 # hertbeat timeout in seconds
timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0)
system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0)
# sys_status
sys:
min_voltage: 10.0 # diagnostics min voltage
disable_diag: false # disable all sys_status diagnostics, except heartbeat
# sys_time
time:
time_ref_source: "fcu" # time_reference source
timesync_mode: MAVLINK
timesync_avg_alpha: 0.6 # timesync averaging factor
# --- mavros plugins (alphabetical order) ---
# 3dr_radio
tdr_radio:
low_rssi: 40 # raw rssi lower level for diagnostics
# actuator_control
# None
# command
cmd:
use_comp_id_system_control: false # quirk for some old FCUs
# dummy
# None
# ftp
# None
# global_position
global_position:
frame_id: "map" # origin frame
child_frame_id: "base_link" # body-fixed frame
rot_covariance: 99999.0 # covariance for attitude?
gps_uere: 1.0 # User Equivalent Range Error (UERE) of GPS sensor (m)
use_relative_alt: true # use relative altitude for local coordinates
tf:
send: false # send TF?
frame_id: "map" # TF frame_id
global_frame_id: "earth" # TF earth frame_id
child_frame_id: "base_link" # TF child_frame_id
# imu_pub
imu:
frame_id: "base_link"
# need find actual values
linear_acceleration_stdev: 0.0003
angular_velocity_stdev: 0.0003490659 // 0.02 degrees
orientation_stdev: 1.0
magnetic_stdev: 0.0
# local_position
local_position:
frame_id: "map"
tf:
send: true #send transformation from map to base link
frame_id: "map"
child_frame_id: "base_link"
send_fcu: false
# param
# None, used for FCU params
# rc_io
# None
# safety_area
safety_area:
p1: {x: 1.0, y: 1.0, z: 1.0}
p2: {x: -1.0, y: -1.0, z: -1.0}
# setpoint_accel
setpoint_accel:
send_force: false
# setpoint_attitude
setpoint_attitude:
reverse_thrust: false # allow reversed thrust
use_quaternion: false # enable PoseStamped topic subscriber
tf:
listen: false # enable tf listener (disable topic subscribers)
frame_id: "map"
child_frame_id: "target_attitude"
rate_limit: 50.0
setpoint_raw:
thrust_scaling: 1.0 # used in setpoint_raw attitude callback.
# Note: PX4 expects normalized thrust values between 0 and 1, which means that
# the scaling needs to be unitary and the inputs should be 0..1 as well.
# setpoint_position
setpoint_position:
tf:
listen: false # enable tf listener (disable topic subscribers)
frame_id: "map"
child_frame_id: "target_position"
rate_limit: 50.0
mav_frame: LOCAL_NED
# setpoint_velocity
setpoint_velocity:
mav_frame: LOCAL_NED
# vfr_hud
# None
# waypoint
mission:
pull_after_gcs: true # update mission if gcs updates
use_mission_item_int: true # use the MISSION_ITEM_INT message instead of MISSION_ITEM
# for uploading waypoints to FCU
# --- mavros extras plugins (same order) ---
# adsb
# None
# debug_value
# None
# distance_sensor
## Currently available orientations:
# Check http://wiki.ros.org/mavros/Enumerations
##
distance_sensor:
hrlv_ez4_pub:
id: 0
frame_id: "hrlv_ez4_sonar"
orientation: PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing
field_of_view: 0.0 # XXX TODO
send_tf: true
sensor_position: {x: 0.0, y: 0.0, z: -0.1}
lidarlite_pub:
id: 1
frame_id: "lidarlite_laser"
orientation: PITCH_270
field_of_view: 0.0 # XXX TODO
send_tf: true
sensor_position: {x: 0.0, y: 0.0, z: -0.1}
sonar_1_sub:
subscriber: true
id: 2
orientation: PITCH_270
horizontal_fov_ratio: 1.0 # horizontal_fov = horizontal_fov_ratio * msg.field_of_view
vertical_fov_ratio: 1.0 # vertical_fov = vertical_fov_ratio * msg.field_of_view
# custom_orientation: # Used for orientation == CUSTOM
# roll: 0
# pitch: 270
# yaw: 0
laser_1_sub:
subscriber: true
id: 3
orientation: PITCH_270
# image_pub
image:
frame_id: "px4flow"
# fake_gps
fake_gps:
# select data source
use_mocap: false # ~mocap/pose
mocap_transform: false # ~mocap/tf instead of pose
use_vision: true # ~vision (pose)
# origin (default: Zürich)
geo_origin:
lat: 47.3667 # latitude [degrees]
lon: 8.5500 # longitude [degrees]
alt: 408.0 # altitude (height over the WGS-84 ellipsoid) [meters]
eph: 2.0
epv: 2.0
satellites_visible: 5 # virtual number of visible satellites
fix_type: 3 # type of GPS fix (default: 3D)
tf:
listen: true
send: false # send TF?
frame_id: "map" # TF frame_id
child_frame_id: "fix" # TF child_frame_id
rate_limit: 10.0 # TF rate
gps_rate: 5.0 # GPS data publishing rate
# landing_target
landing_target:
listen_lt: false
mav_frame: "LOCAL_NED"
land_target_type: "VISION_FIDUCIAL"
image:
width: 640 # [pixels]
height: 480
camera:
fov_x: 2.0071286398 # default: 115 [degrees]
fov_y: 2.0071286398
tf:
send: true
listen: false
frame_id: "landing_target"
child_frame_id: "camera_center"
rate_limit: 10.0
target_size: {x: 0.3, y: 0.3}
# mocap_pose_estimate
mocap:
# select mocap source
use_tf: false # ~mocap/tf
use_pose: false # ~mocap/pose
# odom
odometry:
fcu:
odom_parent_id_des: "map" # desired parent frame rotation of the FCU's odometry
odom_child_id_des: "base_link" # desired child frame rotation of the FCU's odometry
# px4flow
px4flow:
frame_id: "px4flow"
ranger_fov: 0.118682 # 6.8 degrees at 5 meters, 31 degrees at 1 meter
ranger_min_range: 0.3 # meters
ranger_max_range: 5.0 # meters
# vision_pose_estimate
vision_pose:
tf:
listen: true # enable tf listener (disable topic subscribers)
frame_id: "map"
child_frame_id: "base_link"
rate_limit: 30.0
# vision_speed_estimate
vision_speed:
listen_twist: true # enable listen to twist topic, else listen to vec3d topic
twist_cov: true # enable listen to twist with covariance topic
# vibration
vibration:
frame_id: "base_link"
# wheel_odometry
wheel_odometry:
count: 2 # number of wheels to compute odometry
use_rpm: false # use wheel's RPM instead of cumulative distance to compute odometry
wheel0: {x: 0.0, y: -0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m)
wheel1: {x: 0.0, y: 0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m)
send_raw: true # send wheel's RPM and cumulative distance (~/wheel_odometry/rpm, ~/wheel_odometry/distance)
send_twist: false # send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry
frame_id: "map" # origin frame
child_frame_id: "base_link" # body-fixed frame
vel_error: 0.1 # wheel velocity measurement error 1-std (m/s)
tf:
send: false
frame_id: "odom"
child_frame_id: "base_link"
# vim:set ts=2 sw=2 et:
