Integrate the T265 root frame into the robot urdf as a new fixed link

[torydebra]

I am opening this to discuss about a common solution between to integrate the T265 camera in the model urdf both in simulation and on real robot.

The T265 has various frames that link the different components (lens, gyro, case, and so on) that results in different transformations to handle.

For now, what we are doing is:

Simulation

The root of the camera frame (*camera_name*_pose_frame) is linked to the pelvis and correspond to the position of the camera mounted on the real robot.
All the (simulated) camera frames are integrate directly in the urdf/xacro so the tf are published directly by xbot

Real Robot

The urdf/xacro of the robot has not the camera frames. It is the official realsense launch that publishes the tf of all the camera frames, and not xbot.
So, we need an additional static_transform_publisher (or not static) to link a robot frame to a camera frame.
What I would like is to have a common solution like adding to the model itself the link between pelvis and camera root so it is xbot that publish this directly and not an additional node.

Problem here is that real (not simulated) realsense T265 publishes the tf starting from a root frame (*camera_name*_odom_frame) that is always fixed: this is the frame where the camera has been switched on.
Indeed the tf published from *camera_name*_odom_frame to *camera_name*_pose_frame corresponds to where the T265 actually is . This info is obviously published also as PoseStamped message

So, we can not link the pelvis to the *camera_name*_pose_frame otherwise *camera_name*_pose_frame will have 2 father links.

Solutions

@aled96 solution (currently in use)

In practice the new root of the robot is the camera root. So the camera pose frame is the father link of pelvis. Hence the only addition here is a static transform publisher that publish from *camera_name*_pose_frame to pelvis

@alaurenzi solution :

Additional node that handles all the camera tf. So the realsense official node will not publish anymore the tf, but it will be this new node that does so (including the camera static ones between lens, gyro, camera case, and so on).
In this case we have more flexibility, so we can keep the pelvis as a son of *camera_name*_odom_frame (ie the fixed world) and as a father of *camera_name*_pose_frame (actual pose of the camera).

Note that this flexibility will be probably necessary when we will use more t265 on the robot, because we will need some algorithm to merge the tracking of the different devices.

---

Note that with d435 this problem is not happening, since it does not track and there is not a fixed frame. For these we can simply add a frame in the robot urdf, and let realsense launch to publish all the other fixed tf internal to the camera
Note2 we should choose a camera name to be sure that the realsense launch file and the urdf model have the same link name for the camera root.

---

Other involved : @liesrock ...

[liesrock]

I believe @alaurenzi solution is to be preferred: can you work on this when you find the time @aled96 and @torydebra ?
In case let's open another branch and when ready/tested let's open a pull request