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

[arthurlovekin]

Hi all, I think this is a great PR idea as I am currently using ROS2 Jazzy and am running into issues with TwistStamped vs Twist messages. However, I build the code from the PR (https://github.com/FIBO-Engineer/twist_mux/tree/stamped-support) and am having many issues:

1. When I run twist_mux_launch.py I get the error

 - TypeError: 'bool' object is not iterable
 - InvalidFrontendLaunchFileError: The launch file may have a syntax error, or its format is unknown

This can be fixed by changing default_value=False, to default_value='False', on line 57 of twist_mux_launch.py

2. When I run ros2 launch twist_mux twist_mux.launch, I get the error:

[ERROR] [launch]: Caught exception in launch (see debug for traceback): Caught multiple exceptions when trying to load file of format [launch]:
 - VisitError: Error trying to process rule "substitution":
Unknown substitution: find
 - SyntaxError: invalid syntax (twist_mux.launch, line 1)

3. I don't think the 'use_sim_time' launch argument works. I would like to be able to set a parameter called 'use_sim_time' to determine whether the timestamper should use the local computer time to generate the timestamp (as you would do when running a physical robot), or to use the /clock topic (as you would do when running a simulated robot in Gazebo). The file twist_mux_launch.py seems to expose this parameter, but I see no reference to that parameter in the source code, indicating that it probably has no effect.

4. I don't think the 'output_stamped' launch argument works. The "output_stamped" parameter is used in twist_mux_launch.py, but in twist_marker.cpp the only parameter declared is "use_stamped." I would like a way to make the output of twist_mux a TwistStamped as opposed to a Twist. However, neither of the following commands give me TwistStamped outputs: ros2 launch twist_mux twist_mux_launch.py output_stamped:='true' and ros2 launch twist_mux twist_mux.launch use_stamped:='true'

I am considering reworking this PR or creating a new one in either twist_mux or teleop_twist_joy which would allow the user to set a parameter that could add stamps to unstamped twists. It seems to me that the time-stamping could be done here in twist_mux, or it could be done in teleop_twist_joy (or both). Which should I do? Is this a useful contribution to the project?

[bmagyar]

hi @5730289021-NN , I've just merged #55 and was about to resolve the conflict on your branch but I don't have permissions. There's a part where you unticked the "allow maintainers to edit branch" or used different default settings. Please either resolve the conflict or add permissions so I can fix it up.

[5730289021-NN]

@bmagyar I don't see "Allow maintainers to edit branch" on this PR page.
It might be due to GitHub Organization PR bug.

To mitigate this issue, I've granted you direct access to that repo.
https://github.com/FIBO-Engineer/twist_mux/invitations

This should fix the issue.