roslibrust old_tf_to_new_tf

[lucasw]

Make a fork of https://github.com/arjo129/rustros_tf use roslibrust instead of rosrust.

Have a config file read in so multiple transforms can be done in one node.

Also should be possible to use from outside.

[lucasw]

Fork https://github.com/smilerobotics/tf_rosrust instead? It has a lot of updates.

[lucasw]

Try building both, start with the original:

ROS_PACKAGE_PATH=`rospack find geometry_msgs` cargo build

Modify it to use my fork of rosrust: https://github.com/lucasw/rosrust/tree/noetic_aggregated

Have it built, but it doesn't build the example, no way to test it immediately? Could get it building but try the other one first.

[lucasw]

Run the example (tried it in tf_rosrust and it should work in the other one too)

ROS_PACKAGE_PATH=`rospack find geometry_msgs` cargo run --example listener
rosrun tf2_ros static_transform_publisher 0.5 0 0 0 0 0 camera base_link
...
Err(CouldNotFindTransform("camera", "base_link", {}))
Ok(TransformStamped { header: Header { seq: 1, stamp: Time { sec: 0, nsec: 0 }, frame_id: "camera" }, child_frame_id: "base_link", transform: Transform { translation: Vector3 { x: 0.5, y: 0.0, z: 0.0 }, rotation: Quaternion { x: 0.0, y: 0.0, z: 0.0, w: 1.0 } } })
...

It shouldn't be hard to make an echo binary

[lucasw]

tf_rosrust has two different implementations of tf, one for r2r and the other for rosrust. Could add a third by copy pasting the rosrust one, but it would be nice if there was a common library backing all of them (though that would require an internal time and duration struct instead of using the native ones).

These same files appear in tf_r2r and tf_rosrust:

lib.rs
tf_broadcaster.rs
tf_buffer.rs
tf_error.rs
tf_graph_node.rs
tf_individual_transform_chain.rs
tf_listener.rs
transforms.rs

For now do the copy paste thing and get it to work, and by doing that will learn what code can be moved into a library (just called tf or maybe tf_ros).

[lucasw]

cd tf_rosrust
ROS_PACKAGE_PATH=`rospack find geometry_msgs`:`rospack find tf2_msgs`:`rospack find tf2_msgs`:`rospack find actionlib_msgs` cargo build

[lucasw]

sec vs secs:

pub struct Duration {
    pub sec: i32,
    pub nsec: i32,
}

https://github.com/Carter12s/roslibrust/blob/master/roslibrust_codegen/src/integral_types.rs#L44-L47

and

pub struct Time {
    // Note: rosbridge appears to accept secs and nsecs in for time without issue?
    // Not sure we should actually rely on this behavior, but ok for now...

    // This alias is required for ros2 where field has been renamed
    #[serde(alias = "sec")]
    pub secs: u32,
    // This alias is required for ros2 where field has been renamed
    #[serde(alias = "nanosec")]
    pub nsecs: u32,
}

https://github.com/Carter12s/roslibrust/blob/master/roslibrust_codegen/src/integral_types.rs#L7-L17

[lucasw]

Need to modify roslibrust to provide addition and subtraction and cmp for Time and Duration

ROS_PACKAGE_PATH=`rospack find geometry_msgs`:`rospack find tf2_msgs`:`rospack find std_msgs`:`rospack find actionlib_msgs` cargo build -p roslibrust

[lucasw]

Have a basic rust tf echo working with roslibrust: https://github.com/lucasw/rustros_tf/blob/roslibrust/tf_roslibrust/src/bin/echo.rs - but it needs testing, also had to correct how it was looking up the most recent transform.

Can copy echo.rs here and make an old_tf_to_new_tf node, can rosbag record it and visualize in rviz with a variety of dynamic + static tf inputs to see it's working (but still need automated tests).

[lucasw]

Upon testing it more there are some errors, multiple stops in a chain don't transform correctly.

Also saw an instance of it locking up- need to track down where that can occur and eliminate. Right before the lockup it reports a really old transform:

...
At time 1721772382.569, (current time 1721772382.594, 0.025s old)
frame map -> tf2
- Translation: [-3.155 2.024 0.173]
- Rotation: [0.000 0.628 -0.000 0.778]
At time 1721772382.819, (current time 1721772407.705, 24.886s old)
frame map -> tf2
- Translation: [-3.155 2.024 0.173]
- Rotation: [0.000 0.628 -0.000 0.778]

If something in the lookup goes slow it'll prevent any new transforms from arriving, then it goes to 100% catching up? Would it recover if I waited?

There appears to be a flood of static and dynamic incoming tfm messages that causes the lockup- where are they coming from?

[lucasw]

The increased cpu use over the C++ node is something to pay attention to- is it because my tokio select loop is inefficient or subscribes and publishes are just slower?

See if doing more publishes and higher tf input rate causes it to slow down more relative to the C++.

roslaunch rviz_interactive_tf demo.launch

rosrun tf2_ros static_transform_publisher 0 0 1.0 0 0 0 tf1 base_link

ROS_PACKAGE_PATH=`rospack find geometry_msgs`:`rospack find tf2_msgs`:`rospack find std_msgs`:`rospack find actionlib_msgs` cargo run  _lookup_child:=base_link

rosrun tf_demo old_tf_to_new_tf _lookup_child:=base_link _broadcast_child:=tmp2

[lucasw]

The tf lookup takes around 230 us, not doing that or the publish cuts the cpu usage in half. Not doing the publish cuts down the cpu usage by a third.

Maybe if I wasn't awaiting the publish inside the update? I could pass the TFMessage into a channel or similar then publish it in the outer tokio select loop?

[lucasw]

Test 1000 Hz /tf:

rosrun tf static_transform_publisher 0 0 0 0 0 0 map test 1

old_tf_to_new_tf.rs takes 15% cpu, cpp takes half/third of that.

[lucasw]

Tried changing the system to find the closest transforms to the desired lookup time but it didn't result in any speedup, need to understand what's taking so long- could it be the matrix transforms if it wasn't the lookup?

-> Forgot I wasn't using release mode, see how much better that is... it's a lot better, on par with on maybe slightly slower than the C++.

Also can't leave all the same named executables lying around when wanting to rosrun:

[rosrun] You have chosen a non-unique executable, please pick one of the following:
1) /home/lucasw/ros/ros1/tc_catkin_ws/devel/lib/tf_demo/old_tf_to_new_tf          4) /home/lucasw/ros/ros1/tc_catkin_ws/src/lucasw/tf_demo/target/release/old_tf_to_new_tf
2) /home/lucasw/ros/ros1/tc_catkin_ws/src/lucasw/tf_demo/target/release/old_tf_to_new_tf  5) /home/lucasw/ros/ros1/tc_catkin_ws/src/lucasw/tf_demo/target/debug/old_tf_to_new_tf
3) /home/lucasw/ros/ros1/tc_catkin_ws/src/lucasw/tf_demo/target/debug/old_tf_to_new_tf

The 'old tf to new tf' name isn't that correct, maybe 'tf_reparent', or 'tf_to_tf', and don't rename the existing python or C++ nodes?

[lucasw]

Calling this done for now, can make a new issue to bring in more features from the C++ and python version, and to be able to publish many transforms from many lookups from one process (can make that a different executable and make the common parts reusable).