[pose control] Header PoseStamped consideration added

[Fireronin]

Based on #22

[michalpelka]

I cannot accept - I set it to move in base_link with identity transform - this should give no movement, however it caused robot movement. It gave unpredictable results when Offset Tag is used.
Here is no tag:
Screencast from 07-25-2024 05:54:33 PM.webm
Here is tag of box entity used:
Screencast from 07-25-2024 05:55:31 PM.webm

[Fireronin]

Rebased and tested

[pijaro]

I can't make it work with my example.

I put two entities on the scene with ROS 2 frames (base_link and target):

I've added the ROS2PoseControl to base_link model, set tracking to true, and published the message:

ros2 topic pub /goal_pose geometry_msgs/msg/PoseStamped "{header:{frame_id: target}, pose:{position:{x: 1.0}}}" -1

It was expected that base_link model would teleport to 1m in front of target, but I got the warning:

[Warning] (ROS2PoseControl) - No entity with tag found .
[Warning] (ROS2PositionControl) - Could not transform map to target, error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist

and nothing happened.

It works when I set the frame to base_link, then it will jump 1m on X axis:

ros2 topic pub /goal_pose geometry_msgs/msg/PoseStamped "{header:{frame_id: base_link}, pose:{position:{x: 1.0}}}" -1

[Fireronin]

Generally, I have written a more detailed documentation https://github.com/RobotecAI/robotec-o3de-tools/blob/kr/pose-controll-better-documentation/doc/UserGuide/PoseControl.md

It will be merged in another pr, but first this needs to be resolved, you are missing transform from map frame to target frame. Therefore If you are saying I want to move this object in front of target frame, component tries to find location in relation to map.

This is because this component is generally made to work in ROS 2 tf positions

[Fireronin]

I see, in my testing I didn't encountered this as correct values were set when I was using goal position and then they persisted even when they were hidden, now I made thise settings visible once more

[pijaro]

TF tree in my example is a valid ROS 2 TF tree and the proposed system should also work.

There is no reason to require map frame to exist.

[pijaro]

As for the reference frame, I don't think this should be a requirement as well.

We have TF API and we can get any transformation in a tree we want. Why do we need to stick to one, specified reference frame? We can get a transform base_link -> target and then use that to move base_link the way we want.

[Fireronin]

Tldr, this will introduce even more feature creep, currently there are like 4 cases for goal messages now it will be 5

[pijaro]

I'm not sure what cases you are referring to exactly.

Let's make it work for all TF trees and with only the essential configuration needed.

If the code is starting to look bloated, then maybe some alternative approaches should be considered.

[pijaro]

Generally, I have written a more detailed documentation https://github.com/RobotecAI/robotec-o3de-tools/blob/kr/pose-controll-better-documentation/doc/UserGuide/PoseControl.md

Is there a reason not to include the readme update here, in this PR? Why splitting it?

[Fireronin]

@michalpelka Can you take a look one more time ?