Dynamic polygons in Collision Monitor are not being corrected for different frames

[AlexeyMerzlyakov]

When we are using dynamic polygon, read from input PolygonStamped topic once, the Collision Monitor won't update its polygon correctly. During the first read, it converts all vertices from input PolygonStamped frame to the base_frame_id_, but later doesn't do it anymore.

This leads the CM to behave incorrectly, e.g. for the case when action polygon is defined in the global map frame (not in base_frame):

Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04
• ROS2 Version:
  • ROS2 rolling built from sources (20230110)
• Version or commit hash:
  • be597dc
• DDS implementation:
  • CycloneDDS

Steps to reproduce issue

• Enable Collision Monitor with dynamic polygon read from input polygon_sub_topic
• Send PolygonStamped message once, e.g.

ros2 topic pub /area_stop geometry_msgs/msg/PolygonStamped "{header: {frame_id: "map"}, polygon: {points: [{x: 0.0, y: 0.5}, {x: -3.0, y: 0.5}, {x: -3.0, y: -5.5}, {x: 0.0, y: -5.5}]}}"

• Move robot and ensure that polygon in global frame is moving along with robot (robot suddenly stops w/o any reason)

Expected behavior

Collision Monitor corrects the poly_ vertices to the latest TF state

Actual behavior

Collision Monitor converts only once the poly_ vertices to the base_frame, but does not correct them during the further operation