Range sensor layer can't transform from odom to range_sensor_link

[arslogavitabrevis]

Required Info:

• Operating System: Ubuntu 20.04

• ROS2 Version: Galactic binaries

• Version or commit hash: 1.08

• DDS implementation: Default

Steps to reproduce issue

1. Add some range sensor to the gazebo model
2. Add a RangeSensorLayer plugin to the local costmap
3. Run the simulation
4. You should get the messages "Range sensor layer can't transform from odom to range_sensor_link" sometime in the terminal.

Expected behavior

It should have a similar behavior than the "ObstacleLayer" pluging when dealing with the update rate of the odom->base_footprint frame instead.

Actual behavior

It seem that the range messages that can't have a transform from odom to range_sensor_link are discarded.

Additional information

I add to the range_sensor_layer.cpp a fallback canTransform check for the latest tranform in the range sensor layer for my test and it did seem to work well, but I know this is probably not a good solution as the transforms can be wrong if they are too old.

if (!tf_->canTransform(
        in.header.frame_id, global_frame_, tf2_ros::fromMsg(in.header.stamp)))
{
  if (!tf_->canTransform(
          in.header.frame_id, global_frame_, rclcpp::Time()))
  {
    RCLCPP_INFO(
        logger_, "Range sensor layer can't transform from %s to %s",
        global_frame_.c_str(), in.header.frame_id.c_str());
    return;
  }
}

[SteveMacenski]

You should print now() time and in.header.stamp and let me know what that says. My guess is that they're on different clocks or in.header.stamp is not properly populated (e.g. 0).

If in.header.stamp is 0, then you need to update your range sensor driver to provide a valid timestamp on the capture time of the data. This is important for time synchronization.

If the times are on different clocks, that means 1 thinks its on simulation time and the other is on wall time, in that case, you need to correct the use of is_sim_time in the appropriate nodes configurations depending on what you're doing

[arslogavitabrevis]

I forgot to mention that the range sensor messages are sent by some node of the pluging libgazebo_ros_ray_sensor.so. I check the parameter of /local_costmap node and range_sensor node and both have use_sim_time to true.

There is a difference between the stamp and now ans this difference is getting bigger over time
[controller_server-1] [INFO] [1654116828.990940974] [local_costmap.local_costmap]: Range sensor layer can't transform from odom to front_left_ultrasonic_link. Now: 4.034000, Stamp: 2.014000
...
[controller_server-1] [INFO] [1654117329.582256570] [local_costmap.local_costmap]: Range sensor layer can't transform from odom to front_left_ultrasonic_link. Now: 57.732000, Stamp: 28.876000

[SteveMacenski]

That is an odd problem but I'm not sure how I/Nav2 can help with that. It sounds like there's an issue with how the sonar data is coming out of the simulator or something to do with clocks not being the same (?). Doesn't seem the part of Nav2 to resolve but you're welcome to file tickets with the simulator plugin repository or look at the source code yourself about why the delay.

[arslogavitabrevis]

Our robot is also using some lidar scanner which use the same gazebo pluging (libgazebo_ros_ray_sensor.so). The sensor_msgs/LaserScan are also connected to the local_costmap throught the nav2_costmap_2d::ObstacleLayer and to the global_costmap throught the nav2_costmap_2d::ObstacleLayer and nav2_costmap_2d::VoxelLayer.

I do not have a warning with those layer. It seem that the transform are handled differently in the range_sensor_layer and in the obstacle_layer. The obstacle_layer is more complex, but as I understand it is transforming the laserscan to a point cloud using the laser_geometry package, but I don't see where the transformation is done. That why I was thinking that it may have something to do with the range_sensor_layer.

[SteveMacenski]

Does everything work fine after canTransform check -- e.g. the transform(...) calls in the lines below it do fine? That's strange, because transform is using the same timestamps as canTransform, and canTransform is even used internal to transform(). I suspect because there is no transform timeout applied to canTransform that it fails immediately due to some buffer bug.

Can you try to modify the canTransform call to use the timeout? https://github.com/ros2/geometry2/blob/ros2/tf2_ros/include/tf2_ros/buffer.h#L170 like we do with transform?

If that doesn't work (I think it should), here's what we can do. I tracked down internal to transform() and found that it will throws an exception if it fails. So if without this check works, we can then remove https://github.com/ros-planning/navigation2/blob/main/nav2_costmap_2d/plugins/range_sensor_layer.cpp#L295-L302 and wrap the transformations in a timeout or a try/catch so that failures don't cause a crash and then fail the update if it cannot be made.

[SteveMacenski]

Any feedback?

[arslogavitabrevis]

I did not made the test you suggested with the timeout yet. I should be able to do it before the end of the week. Thanks you for following with me.

[SteveMacenski]

Hi,

Any update?

[arslogavitabrevis]

I modified the can transform call based on https://github.com/ros2/geometry2/blob/ros2/tf2_ros/include/tf2_ros/buffer.h#L170 the log is gone and we see that the range sensor are used in the local costmap.

I used the transform tolerance as the duration in the can_transform as this duration us used in the call to transform later. In my test the transform_tolerance was set to 0.3s.

I can try to create a pull request with the changes.

Thanks you following with me on this request

[SteveMacenski]

Please! I look forward to it!

[SteveMacenski]

Whoops, wrong button

[arslogavitabrevis]

Il did a pull request to the galactic branch as I tested it using this branch. The mergify bot told me that I should do it to the main branch. Should I create a new pull request to the main branch? #3053

[SteveMacenski]

Yes please! All changes go from main -> backport elsewhere so that it can be contained in all future distributions