Use current time to check if observation is out of date

[marting87]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#545
Primary OS tested on	Ubuntu
Robotic platform tested on	prototype

Issue:

Points remain in obstacle layer after we stopped publishing a point cloud on regular basis. Settings to reproduce:

      obstacle_layer:
        enabled: true
        observation_sources: pointcloud
        pointcloud:
          topic: "blabla"
          max_obstacle_height: 2.0
          data_type: "PointCloud2"
          expected_update_rate: 0.0
          observation_persistence: 1.0
          clearing: true
          marking: true

According to observation_persistence: 1.0 the point clouds used by the obstacle layers should time-out after 1 second.

Analysis:

• last_updated_ is updated on every new cloud that needs to be added to the buffer.
• purgeStaleObservations checks the cloud's time stamp with last_updated_.
• When we stop publishing a point cloud last_updated_ will never be updated.
• Hence the observations will not time-out and remain in the observation_list_.
• This may result in the following warning, when the robot drives away from the clouds in observation_list_:

Sensor origin at (x, y) is out of map bounds. The costmap cannot raytrace for it.

---

Description of contribution in a few bullet points

• Fixed it by replacing last_updated_ with nh_->now() in purgeStaleObservations

Description of documentation updates required from your changes

• None

Future work that may be required in bullet points

• None

[marting87]

Related: https://answers.ros.org/question/354255/costmap-not-clearing-obstacles/

[codecov]

Codecov Report

Merging #1872 into master will decrease coverage by 0.85%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##           master    #1872      +/-   ##
==========================================
- Coverage   70.14%   69.29%   -0.86%     
==========================================
  Files         218      218              
  Lines       10586    10586              
==========================================
- Hits         7426     7336      -90     
- Misses       3160     3250      +90     

Impacted Files	Coverage Δ
nav2_costmap_2d/src/observation_buffer.cpp	63.10% <0.00%> (ø)
...re/include/dwb_core/illegal_trajectory_tracker.hpp	40.00% <0.00%> (-60.00%)	⬇️
...roller/dwb_core/src/illegal_trajectory_tracker.cpp	25.00% <0.00%> (-55.00%)	⬇️
nav2_map_server/src/map_server/main.cpp	63.63% <0.00%> (-36.37%)	⬇️
...map_2d/include/nav2_costmap_2d/inflation_layer.hpp	91.66% <0.00%> (-8.34%)	⬇️
nav2_navfn_planner/src/navfn_planner.cpp	82.09% <0.00%> (-6.80%)	⬇️
nav2_costmap_2d/src/clear_costmap_service.cpp	17.89% <0.00%> (-6.32%)	⬇️
..._dwb_controller/dwb_core/src/dwb_local_planner.cpp	77.32% <0.00%> (-4.97%)	⬇️
...tree/include/nav2_behavior_tree/bt_action_node.hpp	79.26% <0.00%> (-4.88%)	⬇️
nav2_controller/src/nav2_controller.cpp	79.38% <0.00%> (-3.10%)	⬇️
... and 7 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 69977cd...9819cf3. Read the comment docs.

[SteveMacenski]

@DLu can you take a look at this? I could go either way on it.

[DLu]

That makes sense to me.

[SteveMacenski]

OK, I wasn't sure.

[SteveMacenski]

Thanks for the contribution @marting87