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

[5730289021-NN]

This PR provide 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, use_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(
            'use_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'),
                 'use_stamped': LaunchConfiguration('use_stamped')},
                LaunchConfiguration('config_locks'),
                LaunchConfiguration('config_topics')]
        )
    ])