Melodic collision minimum

[mlautman]

These are the necessary changes to get Descartes to correctly collision check.

This PR should replace #230

This PR is the result of a collaboration between PickNik Robotics and Carbon Robotics

[mlautman]

@shaun-edwards @Jmeyer1292

[jrgnicho]

@mlautman reading the comments in #230 I do agree with the original authors in their concerns about having a planning scene monitor which could change the state while planning is taking place. I feel that the right approach would be to provide a method to mutate the planning scene for the purposes of collision checking (if there isn't one already). Obviously this means that the calling code would have to fetch the planning scene when necessary but I think this is a fair trade off and allows to do offline path planning with various planning scenes states that differ from the current one.

[gavanderhoorn]

@mlautman: just about every commit included in this PR is attributed to mikeblautman@github.com. Was that intentional? Do you work at Github now?

[mlautman]

having a planning scene monitor which could change the state while planning is taking place.

The planning scene monitor does provide a lock for reading and writing to the underlying planning scene which I use in both the isInCollision method (std::unique_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_));) and the setState method (planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_)->setCurrentState(state);).

While it is technically possible to circumvent the lock by getting a direct reference to the planning scene, I don't see that as a real concern as the shared planning scene monitor is only used by the move_group node and now Descartes.

[mlautman]

@mlautman: just about every commit included in this PR is attributed to mikeblautman@github.com. Was that intentional? Do you work at Github now?

I'm not sure I totally understand what happened but I'm excited to be getting so much recognition :P

[jrgnicho]

@mlautman I'm still hesitant on whether embedding the planning scene monitor into the planner is a viable approach. Would't it be sufficient to provide an accessor method that allows mutating the internal PlanningScene instance instead?

Furthermore, the initial PR comment states "These are the necessary changes to get Descartes to correctly collision check." What was incorrect about the collision checking prior to this change?

[mlautman]

@mlautman I'm still hesitant on whether embedding the planning scene monitor into the planner is a viable approach. Would't it be sufficient to provide an accessor method that allows mutating the internal PlanningScene instance instead?

@jrgnicho The PSM affords us strictly more functionality than the planning scene. The PSM takes care of a lot of things such as locking the planning scene for read/write and keeping it up to date. Adding set/get method would eventually result in re-writing the PSM.

Furthermore, the initial PR comment states "These are the necessary changes to get Descartes to correctly collision check." What was incorrect about the collision checking prior to this change?

@jrgnicho The collision checking as is only checks self collisions. In order to get collisions with attached bodies, world objects etc.. you need to use a monitored planning scene which will automatically apply diffs.

I'm not sure I understand your concerns about the planning scene changing in the middle of a plan. Either the planning scene represents the world or it doesn't. Using a local planning scene simply ignores everything going on outside of the robot.

[mlautman]

Ping @jrgnicho @Jmeyer1292

[jrgnicho]

The collision checking as is only checks self collisions. In order to get collisions with attached bodies, world objects etc.. you need to use a monitored planning scene which will automatically apply diffs.

@mlautman I understand that this functionality would be useful in a scenario where you only care about planning for the current context/environment however this approach does not fit well in an offline planning application where it's not necessary to sync the local planning scene to the current environment.

If feel that keeping the planning scene up-to-date might be outside the jurisdiction of the planner. In my opinion, a higher level application could do that and pass an up-to-date instance of the Planning Scene to the Descartes planner which is how move_group operates if I remember correctly.

[mlautman]

I understand that this functionality would be useful in a scenario where you only care about planning for the current context/environment however this approach does not fit well in an offline planning application where it's not necessary to sync the local planning scene to the current environment.

I am not sure that we are on the same page regarding the planning scene monitor. Using a planning scene monitor gives you all of the functionality for offline planning that you currently have with a planning scene, but it also gives you the option to do online planning with an up to date planning scene.

If feel that keeping the planning scene up-to-date might be outside the jurisdiction of the planner.

I agree. None of these changes put the responsibility for keeping the planning scene up to date on the planner.

(The only code that actually changes the planning scene is the setState method which is never called from within Descartes. For that method, I simply recreated the functionality in the melodic-devel branch with the planning scene monitor.)

In my opinion, a higher level application could do that and pass an up-to-date instance of the Planning Scene to the Descartes planner which is how move_group operates if I remember correctly.

I agree. That is why I wrote this code to do exactly that. The move group node sets up a planning scene monitor which is placed in the move_group context and passed to all of the move group capabilities. The higher level application is entirely responsible for ensuring that the planning scene in the planning scene monitor is up to date. These changes simply allow you to pass in the monitor itself so that the user has the option to do online planning. Another user could just as easily pass initialize the MoveitStateAdapter with just the robot_description and never worry about external nodes changing the planning scene.

I really don't understand the push-back here. I don't see these changes as controversial yet, we've been debating them for weeks. The code will behave just like the old code but with the additional option of online planning.

[jrgnicho]

This function gets called repeatedly by Descartes for every waypoint during the graph building stage (which can take minutes even) and given this setup there's a chance that the PlanningScene instance held within the PSM changes as the graph is built resulting in nodes that are evaluated with different planning scenes. While that mode of operation may be desirable for dynamic planning applications it does not apply for situations where one just wants to do offline planning on a static scene.

I agree that the current collision capability is lacking in that it only accounts for the robot model in isolation and having the ability to include other environment objects( attached geometries, occupancy grids, .etc) is much needed but I think in the context of offline planning the scene should remain static.

Could we maybe setup a way to add a custom collision callback that overrides a default collision function? The default could do collision checking against a static scene, while in a custom cb one could include a pointer to an active PSM that performs collision checks against an up-to-date environment.