AP_DDS: /ap/cmd_vel accepts body-frame messages

[pedro-fuoco]

The aim of this PR is for /ap/cmd_vel to be able to receive body frame messages with the base_link frame_id.
This check is performed first as this should be the most common use case for aerial vehicles per ROS REP 147

[pedro-fuoco]

Should body-frame be "default" ?
Since it is backed up by ROS REP 147 I think we could assume that a message that doesn't have a frame_id is body frame.
@Ryanf55

[pedro-fuoco]

Should body-frame be "default" ? Since it is backed up by ROS REP 147 I think we could assume that a message that doesn't have a frame_id is body frame. @Ryanf55

We discussed over discord and it is probably best not to do so. ROS packages performing control should provide the frame_id to be ROS compliant. This also forces users to be conscious about the frame they are using beforehand

[Ryanf55]

Should body-frame be "default" ? Since it is backed up by ROS REP 147 I think we could assume that a message that doesn't have a frame_id is body frame. @Ryanf55

I'm hesitant to make assumptions unless we actually need to ( if we had to use Twist without the stamp). Since nav2 sets the frame ID, let's enforce it.

[pedro-fuoco]

Tested both with both of the possible frame_ids.
For map, I used the following ROS 2 CLI command:

ros2 topic pub /ap/cmd_vel geometry_msgs/msg/TwistStamped '{header: {frame_id: "map"}, twist: {linear: {y: 1}}}

For base_link, I used this:

ros2 topic pub /ap/cmd_vel geometry_msgs/msg/TwistStamped '{header: {frame_id: "base_link"}, twist: {linear: {x: 1}}}'

Both worked as expected.