Dynamically adjust trajectory

[agraussgrimbergen]

Hi, for a project we are using a sensor to measure the distance from the end-effector tool tip to some surface. We are currently generating a rough trajectory to move over this surface. But we would like to execute this trajectory and during execution move closer/further from this object according the data from the distance sensor.

So basically doing small adjustments to a trajectory during execution.

Do you have any advice on how to tackle this problem with MoveIT2? Maybe writing some custom plugins?