TwistStamped Support

[luis-camero]

Added support for TwistStamped support using parameter use_stamped to maintain support for unstamped Twist messages.

If parameter is enabled, all Twist message topic publishers and subscribers are switched to TwistStamped topics.

[Noel215]

I think https://github.com/ros-teleop/twist_mux/blob/rolling/scripts/joystick_relay.py should be also updated. Otherwise the message type is not compatible:

~$ ros2 topic info /joy_vel_out -v
Type: ['geometry_msgs/msg/Twist', 'geometry_msgs/msg/TwistStamped']

Publisher count: 1

Node name: joystick_relay
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: PUBLISHER
GID: 01.10.a0.41.9e.a5.ed.d1.01.a3.d7.3d.00.00.15.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): KEEP_LAST (1)
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 1

Node name: twist_mux
Node namespace: /
Topic type: geometry_msgs/msg/TwistStamped
Endpoint type: SUBSCRIPTION
GID: 01.10.80.9b.f6.44.06.d9.8a.cc.3e.06.00.00.15.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): KEEP_LAST (1)
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

[Noel215]

Also, creating a new branch is a good idea because rolling and jazzy do not support unstamped velocities anymore. Then, this could be the version for humble and iron, and we can remove all the unstamped velocities for rolling and jazzy.

[Noel215]

I think https://github.com/ros-teleop/twist_mux/blob/rolling/scripts/joystick_relay.py should be also updated. Otherwise the message type is not compatible:

~$ ros2 topic info /joy_vel_out -v
Type: ['geometry_msgs/msg/Twist', 'geometry_msgs/msg/TwistStamped']

Publisher count: 1

Node name: joystick_relay
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: PUBLISHER
GID: 01.10.a0.41.9e.a5.ed.d1.01.a3.d7.3d.00.00.15.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): KEEP_LAST (1)
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 1

Node name: twist_mux
Node namespace: /
Topic type: geometry_msgs/msg/TwistStamped
Endpoint type: SUBSCRIPTION
GID: 01.10.80.9b.f6.44.06.d9.8a.cc.3e.06.00.00.15.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): KEEP_LAST (1)
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Nav2 uses (just) Twist for humble, so the parameter would not work for humble (and maybe iron) . This could happen with other applications also. I'd propose the following:

• humble/iron: Twist subscription + both options for publishing with the parameter. No need of updating joystick_relay here.
• jazzy/rolling: All TwistStamped.

This would include branching off, as I commented before.

[bmagyar]

I've created a branch, please update this PR such that the default behaviour is not to use stamps

[bmagyar]

Generally looks ok but needs a little bit more work. Also could you please update the tests to reflect the new default?

@luis-camero please let us know if you are able to follow-up or if we need to recycle your PR

[luis-camero]

@bmagyar I've made the changes you asked for. I am a bit confused on whether we want the default to be use_stamped:=true or use_stamped:=false, at least in this PR.

[Tacha-S]

I think a similar option is necessary for joystick_relay as well.

[bmagyar]

Thank you!