How to detect new obstacles

edit

This is generally in my experience the place to use the local costmap.

The global costmap is used by the global planner to generate a path from point a to point b that avoids the known obstacles in the static map that you have probably previously generated. Generally the global map is not updated during run time (unless you are using SLAM) and therefore cannot help you account for new obstacles.

The local planner, the thing that usually generates the actually velocity commands sent to the base controller, uses the local costmap. The local costmap characterizes the environment immediately around the robot (10m*10m) and is updated continuously during run time by a laserscan or other depth sensor (realsense-kinect-radar etc). Because it is continuously updated during run time, it will incorporate new obstacles, and the local planner will generate commands that stay away from obstacles in the local costmap (read: not the global costmap).

So the local planner has the job of staying close to the global plan it was given, avoiding obstacles that it sees in the local costmap, and navigating to its own "carrot style" goal. The proper balance of these things will provide a functional behavior.

You should look look at the config file here and in particular the costmap_local.yaml which uses the costmap_2d::ObstacleLayer with the laserscan as its input.

I am not exactly sure if these things generalize to the TEB planner exactly, but this is my experience with the other ROS nav stack planners.

Going with your strategy, you could perhaps get that information from the obstacle layer of say, the local costmap. This link might help you to access a particular plugin like obstacle_layer. I haven't done so yet, but i believe obstacles would be reflected in this layer's data and would possibly give a more specific dataset to compare in the pre and post obstacle scenarios.

Alternatively, since you are already using TEB, you might be familiar with the costmap converter. You could use that (costmap converter package) to base your node and issue messages when any obstacle populate it.