For cmd_vel topic how to switch from Twist to TwistStamped

[yorobot256]

Is your feature request related to a problem? Please describe.
When I am asking robot to move or spin is using msg/Twist, but I am using jazzy and robot is publishing with TwistStamped

Describe the solution you'd like
A way to switch between Twist or TwistStamped for cmd_vel topic or to force it to TwistStamped.

Describe alternatives you've considered
I tried to force in rai class Ros2PubMessageTool to identify when msg_type is Twist message is used and then to switch to TwistStamped.

def _build_msg(
        self, msg_type: str, msg_args: Dict[str, Any]
    ) -> Tuple[object, Type]:
        # Force replace Twist with TwistStamped
        if msg_type == "geometry_msgs/msg/Twist" or msg_type == "geometry_msgs/Twist":
            self.logger.info("Forcing Twist to TwistStamped conversion.")
            msg_type = "geometry_msgs/msg/TwistStamped"
            msg_args = {
                "header": {
                    "frame_id": "base_link",
                    "stamp": {"sec": 0, "nanosec": 0},
                },
                "twist": msg_args,  # Nest original Twist fields
            }

        # Proceed with building the message
        msg_cls: Type = import_message_from_str(msg_type)
        msg = msg_cls()
        if msg_type == "geometry_msgs/msg/TwistStamped":
            msg.header.stamp = self.node.get_clock().now().to_msg()
        rosidl_runtime_py.set_message.set_message_fields(msg, msg_args)
        return msg, msg_cls

also I have updated nomad.py to publish TwistStamped instead of Twist.

However in logs I still see Twist, and robot doesnt move due to this.
Only Navigate to pose is working as it is not using Twist.

Additional context
Add any other context or screenshots about the feature request here.

[maciejmajek]

Hi. RAI (agent) should automatically adapt to the topic's interface. This looks like the agent did not check the topics and interfaces before publishing. This can sometimes happen, as llms have tendency to overestimate their knowledge of a robot which results in suboptimal performance.

In principle, we try to steer the agent away from using topics such as cmd_vel or really anything similar (time sensitive/time based) as the loop frequency is quite low + relying on llms ability to understand time can cause problems.

I will look into that soon, this kind of things should work out of the box for best zero-shot performance on new robots/platforms.