Bug report

Required Info:

• Operating System:
  • Docker 22.04 Ubuntu
• ROS2 Version:
  • rolling
• Version or commit hash:
  • 7813fee
• DDS implementation:
  • rmw_cyclonedds_cpp

Steps to reproduce issue

I am using a rectangular robot with two laser scanner. Config as follows:

collision_monitor:
  ros__parameters:
    base_frame_id: "base_footprint"
    odom_frame_id: "odom"
    cmd_vel_in_topic: "cmd_vel_raw"
    cmd_vel_out_topic: "cmd_vel"
    transform_tolerance: 0.5
    source_timeout: 5.0
    stop_pub_timeout: 2.0
    polygons: ["FootprintApproach"]
    FootprintApproach:
      type: "polygon"
      action_type: "approach"
      footprint_topic: "/local_costmap/published_footprint"
      time_before_collision: 2.0
      simulation_time_step: 0.02
      max_points: 1
      visualize: True
    observation_sources: ["scan_front", "scan_back"]
    scan_front:
      type: "scan"
      topic: "/scan/front"
    scan_back:
      type: "scan"
      topic: "/scan/back"

I am publishing 0.3 rad/sec twist to cmd_vel_raw topic in both gif below and footprint is from FootprintApproach

Expected behavior

As time_before_collision is 2.0 sec, it should detect collision for obstacles under 34 degree in rotation wrt current pose. It should slow down to avoid collsion.

Actual behavior

Collision is not detected (in the log), and robot continued to rotate and collide with obstacle (In gif, it is a medium sized block)
In first gif below, first I try to collide head on, but the collision is detected and robot slows down. As next I try to rotate in place, but robot collides with block (log don't show any collision warning).

In this gif, same behavior but with different angle of approach.

Additional information

Approach model works perfectly in head on collision but not when rotating in place. I went through the source code but can not find anything strange which might be causing this. I tried longer time_before_collision and using more finer time step, but still same result in rotation.