Support both geometry_msgs::TwistStamped and geometry_msgs::Twist as inputs and output

[5730289021-NN]

This PR provides the versatile way for one who'd like to select whether each inputs or output should be stamped. It is similar to #50

For input, if you'd like to subscribe to a stamped topic, you can define via stamped

In this YAML config file, joystick will subscribe to geometry_msgs::Twist topic and navigation will subscribe to geometry_msgs::TwistStamped topic, see stamped

twist_mux:
  ros__parameters:
    topics:
      joystick:
        topic   : joy_vel
        timeout : 0.5
        priority: 70
        stamped : false
      navigation:
        topic   : nav_vel
        timeout : 0.5
        priority: 40
        stamped : true

For output, output_stamped parameter is used to define whether you'd like to output stamped topic. A lean version of a launch file would probably be

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    default_config_locks = os.path.join(get_package_share_directory('twist_mux'),
                                        'config', 'twist_mux_locks.yaml')
    default_config_topics = os.path.join(get_package_share_directory('twist_mux'),
                                         'config', 'twist_mux_topics.yaml')

    return LaunchDescription([
        DeclareLaunchArgument(
            'config_locks',
            default_value=default_config_locks,
            description='Default locks config file'),
        DeclareLaunchArgument(
            'config_topics',
            default_value=default_config_topics,
            description='Default topics config file'),
        DeclareLaunchArgument(
            'cmd_vel_out',
            default_value='cmd_vel',
            description='cmd vel output topic'),
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='False',
            description='Use simulation time'),
        DeclareLaunchArgument(
            'output_stamped',
            default_value='False',
            description='Output as geometry_msgs/TwistStamped instead of geometry_msgs/Twist'),
        Node(
            package='twist_mux',
            executable='twist_mux',
            output='screen',
            remappings={('/cmd_vel_out', LaunchConfiguration('cmd_vel_out'))},
            parameters=[
                {'use_sim_time': LaunchConfiguration('use_sim_time'),
                 'output_stamped': LaunchConfiguration('output_stamped')},
                LaunchConfiguration('config_locks'),
                LaunchConfiguration('config_topics')]
        )
    ])

[ottojo]

I just tested this and it works well for my use case, the ability to specify stamped/unstamped for each input and output separately is really helpful for incremental migration of components!

[bmagyar]

hey @5730289021-NN , great PR! I've merged #50 this morning as a first step toward TwistStamped support but I think it'd be useful to incorporate your PR as well.

First, I'd like to state for the record that a simple launch file example is one that isn't written in Python ;)

I'd like to soon change things such that the default would be to use stamped messages and opt-in for non-stamped. With merging #50 I caused a lot of conflicts on this PR... would you mind rebasing please?

[5730289021-NN]

@bmagyar I've squashed my commits and rebased on top of the upstream.
Note: For those who would like to check the original commits, they can check them out here: https://github.com/FIBO-Engineer/twist_mux/tree/stamped-support-backup