Feature request | Make collision montior cmd vel direction aware

[nakai-omer]

Feature request

Feature description

Currently any obstacle in the defined polygon will cause the robot to stop, even if the robot is driving away from the obstacle. For example, if the robot is driving forward and someone walks behind the robot, it will stop. It will be nice if the collision monitor will take into account the direction of the cmd vel, and if it is away from the obstacle, it should perform it.

Implementation considerations

polygon->getPointsInside can be changed to check only points which are in the same direction as the velocity command.

[SteveMacenski]

See #3313 and associated PR. Currently being worked through 🙂