ROS2: port mavros launch

[vooon]

We could try to continue use of xml, but probably launch.py would be preferable.

[Vicidel]

What would need to be done here? Also, is there any solution to run the package without it for short-term testing?

[vooon]

We need to port node lanuch and variants for PX4 and APM. Possibly we may use component loader instead of mavros_node. But currently i had problems with it.

[vooon]

@Vicidel probably i misinterpret your question. It is possible to run ether combined mavros_node or composition nodes without launch. They just helps to manage set of nodes, parameters and remappings.

You can run combined node thus way (params file provides settings, see docs):

ros2 run mavros mavros_node --ros-args --params-file /ws/mavros_param.yaml

[JosepBC]

Hello, I'm working on this, I've succesfully ported node.launch to ROS2, right now doing tests with Pixhawk 4 with ArduPilot but without plugin.yaml and config.yaml as I don't know which plugins/parameters are ported to ROS2. I'll do a PR with all I have in a few hours, someone has the config.yaml and plugins.yaml so I can test everything?

[vooon]

Plugin lists should be the same, except that black/white checked to allow/deny lists.

As for parameters it's harder, because with new parameter protocol each plugin has it's own node. And some of parameters have been renamed, so i suggest you to check sources.

[eliabruni]

Hello, this is blocking our ROS2 development, unfortunately, do you have an ETA for tackling the issue? Thanks!

[JosepBC]

I just ported node.launch as I had to switch back to ros1. If you want I can PR that. But in general, you can use this article to port all the launch files. https://docs.ros.org/en/foxy/How-To-Guides/Launch-files-migration-guide.html

[Rainerino]

I have also manually ported px4.launch and apm.launch, but the parameters don't seem to be loaded correctly

[eliabruni]

thanks for the pointer, @JosepBC.

@Rainerino , would you be so kind to share your code? perhaps we could try to tackle it together?

[Rainerino]

@eliabruni Yeah, of course, I am new to ROS2 and had to face quite a steep learning curve coming from ROS, would love to have some pointers!

Currently, mavros can run with no problem (I tested most of the nodes manually by echo or request them, and nodes show up on rqt_graph. Connect and echo mavros_node with Ardupilot 4.1.2 and PX4 1.12.3, both work "fine" (some minor but annoying problems, see below) Here are the problems so far:

• param files are not read in the code, all are using default parameters
• this error: bash [mavros_node-1] [ERROR] [1646109751.784878925] [mavros.sys]: VER: autopilot version service timeout [mavros_node-1] [ERROR] [1646109751.786953050] [mavros.sys]: VER: command plugin service call failed!. Doesn't seem to bother mavros, but still...

[Rainerino]

Here are the changes:

• update *.xml to ros2 format
• update *yaml to ros2 format (just add ros__parameters to everything)

(assume you are using simulator)

cd my_ws/src/my_pkg/launch
ros2 launch px4.launch fcu_url:="udp://:14550@"   

(please change include file path. I didn't include these in a project, so the path is hardcoded relative path)

<!-- change this to your own workspace, or mavros -->
<include file="$(find-pkg-prefix my_ws)/launch/node.launch">

px4.launch

<launch>
    <!-- vim: set ft=xml noet : -->
    <!-- example launch script for PX4 based FCU's -->
    <arg name="fcu_url" default="/dev/ttyACM0:57600" />
    <arg name="gcs_url" default="" />
    <arg name="tgt_system" default="1" />
    <arg name="tgt_component" default="1" />
    <arg name="log_output" default="screen" />
    <arg name="fcu_protocol" default="v2.0" />
    <arg name="respawn_mavros" default="false" />

    <!-- <push-ros-namespace namespace=""/> -->
    <include file="$(find-pkg-prefix dy_mavros)/../../src/dy_mavros/launch/node.launch">
        <arg name="fcu_url" value="$(var fcu_url)" />
        <arg name="gcs_url" value="$(var gcs_url)" />
        <arg name="tgt_system" value="$(var tgt_system)" />
        <arg name="tgt_component" value="$(var tgt_component)" />
        <arg name="log_output" value="$(var log_output)" />
        <arg name="fcu_protocol" value="$(var fcu_protocol)" />
        <arg name="respawn_mavros" value="$(var respawn_mavros)" />
        <arg name="config_yaml" value="$(find-pkg-prefix dy_mavros)/../../src/dy_mavros/launch/px4_config.yaml"/>
        <arg name="pluginlists_yaml" value="$(find-pkg-prefix dy_mavros)/../../src/dy_mavros/launch/px4_pluginlists.yaml"/>
    </include>
</launch>

node.launch

<launch>
    <arg name="fcu_url"  />
    <arg name="gcs_url"  />
    <arg name="tgt_system"  />
    <arg name="tgt_component"  />
    <arg name="config_yaml" />
    <arg name="pluginlists_yaml" />
    <arg name="log_output" default="log" />
    <arg name="fcu_protocol" default="v2.0" />
    <arg name="respawn_mavros" default="false" />

    <node pkg="mavros" exec="mavros_node">
    <!-- <node pkg="mavros" exec="mavros_node" name="mavros" required="$(eval not respawn_mavros)" output="$(var log_output)"> -->
        <param name="fcu_url" value="$(var fcu_url)" />
        <param name="gcs_url" value="$(var gcs_url)" />
        <!-- <param name="uas_url" value="dy_px4" /> -->
        <param name="tparamet_system_id" value="$(var tgt_system)" />
        <param name="tparamet_component_id" value="$(var tgt_component)" />
        <param name="fcu_protocol" value="$(var fcu_protocol)" />
        <param from="$(var config_yaml)" />
        <param from="$(var pluginlists_yaml)" />
    </node>
</launch>

px4_config.yaml

# Common configuration for PX4 autopilot
#
# node:
# startup_px4_usb_quirk: true

# --- system plugins ---

# sys_status & sys_time connection options
conn:
  ros__parameters:
    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:
  ros__parameters:
    min_voltage: 10.0   # diagnostics min voltage
    disable_diag: false # disable all sys_status diagnostics, except heartbeat

# sys_time
time:
  ros__parameters:
    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:
  ros__parameters:
    low_rssi: 40  # raw rssi lower level for diagnostics

# actuator_control
# None

# command
cmd:
  ros__parameters:
    use_comp_id_system_control: false # quirk for some old FCUs

# dummy
# None

# ftp
# None

# global_position
global_position:
  ros__parameters:
    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:
      ros__parameters:
        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:
  ros__parameters:
    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:
  ros__parameters:
    frame_id: "map"
    tf:
      ros__parameters:
        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:
  ros__parameters:
    p1: {x:  1.0, y:  1.0, z:  1.0}
    p2: {x: -1.0, y: -1.0, z: -1.0}

# setpoint_accel
setpoint_accel:
  ros__parameters:
    send_force: false

# setpoint_attitude
setpoint_attitude:
  ros__parameters:
    reverse_thrust: false     # allow reversed thrust
    use_quaternion: false     # enable PoseStamped topic subscriber
    tf:
      ros__parameters:
        listen: false           # enable tf listener (disable topic subscribers)
        frame_id: "map"
        child_frame_id: "target_attitude"
        rate_limit: 50.0

setpoint_raw:
  ros__parameters:
    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:
  ros__parameters:
    tf:
      ros__parameters:
        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:
  ros__parameters:
    mav_frame: LOCAL_NED

# vfr_hud
# None

# waypoint
mission:
  ros__parameters:
    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:
  ros__parameters:
    hrlv_ez4_pub:
      ros__parameters:
        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:
      ros__parameters:
        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:
      ros__parameters:
        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:
      ros__parameters:
        subscriber: true
        id: 3
        orientation: PITCH_270

# image_pub
image:
  ros__parameters:
    frame_id: "px4flow"

# fake_gps
fake_gps:
  ros__parameters:
    # 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:
      ros__parameters:
        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:
      ros__parameters:
        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:
  ros__parameters:
    listen_lt: false
    mav_frame: "LOCAL_NED"
    land_target_type: "VISION_FIDUCIAL"
    image:
      ros__parameters:
        width: 640            # [pixels]
        height: 480
    camera:
      ros__parameters:
        fov_x: 2.0071286398   # default: 115 [degrees]
        fov_y: 2.0071286398
    tf:
      ros__parameters:
        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:
  ros__parameters:
    # select mocap source
    use_tf: false   # ~mocap/tf
    use_pose: true  # ~mocap/pose

# odom
odometry:
  ros__parameters:
    fcu:
      ros__parameters:
        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:
  ros__parameters:
    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:
  ros__parameters:
    tf:
      ros__parameters:
        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:
  ros__parameters:
    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:
  ros__parameters:
    frame_id: "base_link"

# wheel_odometry
wheel_odometry:
  ros__parameters:
    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:
      ros__parameters:
        send: false
        frame_id: "odom"
        child_frame_id: "base_link"

# camera
camera:
  ros__parameters:
    frame_id: "base_link"

# vim:set ts=2 sw=2 et:

[vooon]

@Rainerino that yaml wouldn't work, unfortunately it's impossible to make 1-to-1 migration as ROS2 param can only be flat list. So no structures like that:

node:
  tf:
    option: false

In most cases you'd have to check sources of each plugin to get new names.

For example sys_status uses new names and instead of conn/ and sys/ all should be look like that:

/mavros/sys:
  ros_parameters:
    conn_timeout: 10.0
    min_voltage: 10.0
    disable_diag: false
    heartbeat_rate: 1.0
    heartbeat_mav_type: ONBOARD_CONTROLLER

And other thing that each plugin is a sub-node to UAS node. mavros_node uses /mavros for UAS, so plugins would have /mavros/<something> (see initializer list, Plugin(uas_, "<something>")).

[vooon]

And one more thing, distance_sensor uses a hack to workaround flatness and predefinition of params, a config parameter that expects yaml as text, e.g.:

/mavros/distance_sensor:
  ros_parameters:
    base_frame_id: base_link
    config: |-
      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}

[Rainerino]

@vooon Thank you very much for the pointer! I will give it another try, any suggestions on how to test if the parameters are loaded correctly? I would imagine using ros2 param get /mavros/node parameter or even a debugger to manually go through every single one of them?

[vooon]

@Rainerino i think you can dump parameters for each plugin and compare them to source.

Something like that command (not tested):

ros2 node list | grep mavros | xargs -n1 ros2 param dump

[eliabruni]

thanks for sharing @Rainerino ! am glad you already got some good pointers; will give it a try too and see what I can get out of it.

[KhairulM]

Is there currently any workaround for launching apm.launch in ROS2?

[lidorshimoni]

hi, I'm really interested! is there currently any workaround or a working solution?

[lidorshimoni]

Running:

ros2 run mavros mavros_node --ros-args --params-file ros2_ws/src/mavros/mavros/launch/apm_config.yaml Results in:

[ERROR] [1658512363.193936732] [rcl]: Failed to parse global arguments
terminate called after throwing an instance of 'rclcpp::exceptions::RCLInvalidROSArgsError'
  what():  failed to initialize rcl: Couldn't parse params file: '--params-file ros2_ws/src/mavros/mavros/launch/apm_config.yaml'. Error: Cannot have a value before ros__parameters at line 4, at /tmp/binarydeb/ros-foxy-rcl-yaml-param-parser-1.1.13/src/parse.c:623, at /tmp/binarydeb/ros-foxy-rcl-1.1.13/src/rcl/arguments.c:388

I'm currently working on writing a python launch file for this but the params file still needs to change...

[brunden77]

Running:

ros2 run mavros mavros_node --ros-args --params-file ros2_ws/src/mavros/mavros/launch/apm_config.yaml Results in:

[ERROR] [1658512363.193936732] [rcl]: Failed to parse global arguments
terminate called after throwing an instance of 'rclcpp::exceptions::RCLInvalidROSArgsError'
  what():  failed to initialize rcl: Couldn't parse params file: '--params-file ros2_ws/src/mavros/mavros/launch/apm_config.yaml'. Error: Cannot have a value before ros__parameters at line 4, at /tmp/binarydeb/ros-foxy-rcl-yaml-param-parser-1.1.13/src/parse.c:623, at /tmp/binarydeb/ros-foxy-rcl-1.1.13/src/rcl/arguments.c:388

I'm currently working on writing a python launch file for this but the params file still needs to change...

Can confirm I am getting the same error.

[vooon]

Done.

[eMrazSVK]

apm.launch still does not work. Tried clean installs on Jetson and on generic Ubuntu 20.04 Laptop.

dcs_user@dcs-user:~/ros2_ws$ ros2 launch mavros apm.launch fcu_url:=$MAVROS_FCU_URL gcs_url:=$MAVROS_GCS_URL
[INFO] [launch]: All log files can be found below /home/dcs_user/.ros/log/2023-03-02-04-44-13-132924-dcs-user-45864
[INFO] [launch]: Default logging verbosity is set to INFO
Task exception was never retrieved
future: <Task finished name='Task-2' coro=<LaunchService._process_one_event() done, defined at /opt/ros/foxy/lib/python3.8/site-packages/launch/launch_service.py:226> exception=ValueError('$(var log_output) is not a valid standard output config i.e. "screen", "log" or "both"')>
Traceback (most recent call last):
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/launch_service.py", line 228, in _process_one_event
    await self.__process_event(next_event)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/launch_service.py", line 248, in __process_event
    visit_all_entities_and_collect_futures(entity, self.__context))
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  [Previous line repeated 3 more times]
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 38, in visit_all_entities_and_collect_futures
    sub_entities = entity.visit(context)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/action.py", line 108, in visit
    return self.execute(context)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch_ros/actions/node.py", line 453, in execute
    ret = super().execute(context)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/actions/execute_process.py", line 826, in execute
    launch.logging.get_output_loggers(self.__name, self.__output)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/logging/__init__.py", line 409, in get_output_loggers
    output_config = _normalize_output_configuration(output_config)
  File "/opt/ros/foxy/lib/python3.8/site-packages/launch/logging/__init__.py", line 339, in _normalize_output_configuration
    raise ValueError((
ValueError: $(var log_output) is not a valid standard output config i.e. "screen", "log" or "both"

Specifically:

ValueError: $(var log_output) is not a valid standard output config i.e. "screen", "log" or "both"

[eMrazSVK]

https://github.com/mavlink/mavros/pull/1836

[marcusnorgren]

@vooon What's the official status regarding the node.launch file? I am trying to change name of the mavros-node, but I can only do this in the node.launch file (as far as I know), or is there another way of doing this? I am trying to set up so that multiple Mavros-nodes can communicate to multiple drones (PX4) on the same ROS_DOMAIN_ID. I am planning to do this via a rename of the topics (i.e. renaming the namespaces/nodes) to e.g. \ /mavros_1/topic for drone 1 \ (instead of just /mavros/topic as is the default case) /mavros_2/topic for drone 2 \ /mavros_3/topic for drone 3 \

Is it enough to change the node name or is it necessary to change namespace as well?

[vooon]

I think better to use namespaces. Also you can experiment with single-Router - multiple-UAS setup.