Add optical frame publisher

[iche033]

Addresses issue #369

The optical frame publisher subscribes to an image topic, converts the original msg's frame id from the original robot frame (x forward, y left, z up) to a new optical frame (x right, y down, z forward) with _optical suffix string, and republishes the data to a new topic. The original topics and data published to them are untouched to avoid breaking existing code.

e.g. for topics like:

/X3/front/image_raw
/X3/front/camera_info
/X3/front/depth

These new topics will be created :

/X3/front/optical/image_raw
/X3/front/optical/camera_info
/X3/front/optical/depth

Here's an screenshot showing the new depth cloud visualization in rviz and a tf tree on the bottom left of the image.

[iche033]

cc @adlarkin, @Lobotuerk

[Lobotuerk]

I don't think I have an easy way to test this without modifying the image we use, but another easy test is adding a camera plugin in rviz, selecting the new ns/front/optical/image_raw, and seeing if it lines up with the point cloud. Also, the code LGTM

[mjcarroll]

Since each camera should have a corresponding Image and CameraInfo topic, won't this make it so that the TF is published twice?

[iche033]

TF should now be published once per node with changes in e23bb64. But for rgbd cameras, two nodes are created (one node for image_raw and camera_info, and another for depth). I verified that the same tf is indeed published twice in the /tf_static topic but the tf tree and rviz visualization seem to work fine though.

[mjcarroll]

My general comments:

• It's a bit strange to see topics/types passed as args. I don't think it's a huge deal, just not very ROS-y
• We have a whole bunch of duplicated launch files...

Otherwise, I think that there is a question about TF publishing that may need to be addressed (though it may be a non-issue, I'm not confident)

[iche033]

It's a bit strange to see topics/types passed as args. I don't think it's a huge deal, just not very ROS-y

ah ok I got that idea from the ros_ign bridge arguments. How about for topics, I can use <remap> like it's done in subt solutions, and for msg types, I'll scrap the idea of making this node generic since there are really only sensor_msgs/Image and sensor_msgs/CameraInfo to convert?

[angelacmaio]

I wonder if there's a way to avoid breaking changes and avoid advertising a duplicate set of topics. Potential alternative:

• Publish the TF for base_link -> camera_front_optical -> camera_front
• Change the frame id of the existing image topics to camera_front_optical

From my understanding this would be a non-breaking change, as long as solutions look up the TF properly (might be a bad assumption).

[mjcarroll]

I wonder if there's a way to avoid breaking changes and avoid advertising a duplicate set of topics.

This could also be done with a lazy subscription. We can do a check to see if anybody is using the downstream topics and only do work if there is a known consumer.

[iche033]

This could also be done with a lazy subscription. We can do a check to see if anybody is using the downstream topics and only do work if there is a known consumer.

yeah if the concern is performance / extra computations, we can do lazy subscription. But if the problem is the presence of extra set of topics, we'll need to figure out how to update the frame id (probably on the sim or bridge side) before publishing the data to ros topics. Let me know which option is preferred.

[Lobotuerk]

The most straightforward solution is modifying the urdf, but I don't know if you have access to them. Like, you can just remmap stuff inside the gazebo plugin

[iche033]

The most straightforward solution is modifying the urdf, but I don't know if you have access to them. Like, you can just remmap stuff inside the gazebo plugin

The model description comes directly from ignition gazebo, and the frames are auto populated by the ros_ign bridge instead of urdf so unfortunately we won't be able to easily modify frames using urdf.

[iche033]

implemented lazy subscription in e708480. But I found out that the bridge logger always subscribes to it so we don't save much computation with this change.

we discussed about the option for removing duplicate topics and updating the msg frame id. The conclusion was that there shouldn't be a problem if the teams look up TF properly but it's hard to say how the teams are currently consuming the data that come out of existing topics and using the frame ids. So we'll go with the duplicate topics option to be safe.

[adlarkin]

Tested these changes, works for me

[nkoenig]

@iche033 Is this targeting the correct branch?

[iche033]

Is this targeting the correct branch?

I ran into problem testing the frame changes because of missing / intermittent tf data so I had to build on top of the log_heartbeat_pose branch to get the tf fix. Both PRs also touched the vehicle_topics.launch files so building onto top of that branch helped to reduce conflicts.

I can port the changes to cave_feature_release1 branch if needed

[nkoenig]

Is this targeting the correct branch?

I ran into problem testing the frame changes because of missing / intermittent tf data so I had to build on top of the log_heartbeat_pose branch to get the tf fix. Both PRs also touched the vehicle_topics.launch files so building onto top of that branch helped to reduce conflicts.

I can port the changes to cave_feature_release1 branch if needed

That's fine, just wanted to make sure there wasn't a mistake.

[gglaspell]

@iche033 I just tried the husky bot with your fix and it's working great. Now I can test my SLAM module. Thank you for fixing this!

[zbynekwinkler]

DepthCloud for ROBOTIKA_X2_SENSOR_CONFIG_1 is not showing in RViz. OTOH I tried SSCI_X2_SENSOR_CONFIG_1 and SSCI_X4_SENSOR_CONFIG_2 and both show the DepthCloud and it has correct orientation.

Our ROBOTIKA_X2_SENSOR_CONFIG_1 used to show the DepthCloud with wrong orientation but now it is not shown at all. The messages are being sent (and received by rviz), the TF tree seems to be correct (as shown in rviz) but nothing is ever drawn. If I show PointCloud2 message, again it is shown and correct, as it has always been.

[iche033]

I noticed this this as well and found that the problem is related to the naming of camera_info topic. I tried the commit before this change, 0eed307, and I was also not able to to see depth cloud visualization. The ~/front/depth topic requires a matching ~/front/camera_info topic but the only available camera info topic is ~/camera_info without the front prefix (which matches image_raw). Changing all camera topics to have consistent naming fixes the problem but I didn't want to do this as it'll break existing code.

To get the visualization working in rviz , you can republish or remap the camera info topic in your launch file. For testing optical frames, you can republish by doing:

# change the robot name prefix to your robot name
rosrun topic_tools relay /Xa/optical/camera_info /Xa/front/optical/camera_info

You should then be able to see the depth cloud visualization.

[zbynekwinkler]

@iche033 Thanks! That explains it and I've learnt something about ROS again 👍