Dynamic tf_prefix for mavros frame ids

[mhl787156]

Currently the frame id's for the Transformations given by mavros are hard coded by the single px4 config file (https://github.com/rob-clarke/mavros/blob/master/mavros/launch/px4_config.yaml). This is fine for flying a single vehicle through TF using the simple offboard.

However this breaks for multiple vehicles as all of the transformations for the different vehicles clash. A fix has been made from simple offboard here which adds VEHICLE_NAMESPACE as a prefix to the frame ids which are then broadcast.

However I do not know how this will interact with the mavros instance which broadcasts (?) its own frame_ids. Therefore a fix is needed to dynamically modify a px4_config.yaml with the correct prefixed frame ids.

[mhl787156]

@rob-clarke do you think it should be as simple as replacing any frame_id or child_frame_id with <vehicle_namespace>/<frame> (except for the global frames of map)?

In which case my proposal is that in mavros_setup.sh we first create a px4_config_namespaced.yaml with frame_ids replaced. Then either we:

1. Provide our own node.launch where we load in the local config params
2. Copy px4_config_namespaced.yaml into the mavros installation launch folder to replace the original px4_config.yaml

What do you think?

px4_config.yaml

# 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: false
    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: true         # ~mocap/pose
  mocap_transform: true   # ~mocap/tf instead of pose
  use_vision: false       # ~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: false
    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: true  # ~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: false           # enable tf listener (disable topic subscribers)
    frame_id: "odom"
    child_frame_id: "vision_estimate"
    rate_limit: 10.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: "odom"             # 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:

[rob-clarke]

1. Provide our own node.launch where we load in the local config params

We should just be able to use the existing launch file and modify the "config_yaml" argument. Though at the moment we're using the px4.launch file instead so there would be some more more arguments to fill in.

An alternative may be to overwrite the parameters later in the launch file (or in a different launch file), though I'm not sure what the order of execution looks like in ROS2 in that respect, i.e. if the parameters would already be loaded by MAVROS before that point.

[rob-clarke]

Otherwise yes, it should just be replacing the names of the vehicle-specific frames.

[mhl787156]

We should just be able to use the existing launch file and modify the "config_yaml" argument. Though at the moment we're using the px4.launch file instead so there would be some more more arguments to fill in.

Yeah we currently call px4.launch directly. Unfortunately px4.launxh doesnt expose the config_yaml as an argument (https://github.com/rob-clarke/mavros/blob/master/mavros/launch/px4.launch), but it uses it itself.

Do you know if the following will load our modified config yaml or if it would be overwritten? Afaik all of this is still running in ROS1 I think!

<include file="$(find mavros)/launch/$(arg firmware).launch" ns="$(arg vehicle_namespace)">
    <arg name="fcu_url" value="$(arg fcu_url)?ids=$(arg system_id),240"/>
    <arg name="gcs_url" value="$(arg gcs_url)"/>
    <arg name="tgt_system" value="$(arg target_system)"/>
    <!-- load blacklist, config -->
    <rosparam command="load" file="$(arg pluginlists_yaml)" />
    <rosparam command="load" file="$(arg our_config_yaml)" />
</include>

[mhl787156]

We should just be able to use the existing launch file and modify the "config_yaml" argument

There is also the question of when to do this modification... if it's done in mavros_setup.sh we need to work out some mechanism to avoid double writes if a user opens up more than one session (reminder to myself more than anything).

Make a copy of px4_config and modify the copy the first time mavros_setup.sh is run. Then submit that as config_yaml parameter