Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Melodic collision minimum #237

Open
wants to merge 8 commits into
base: melodic-devel
Choose a base branch
from

Conversation

mlautman
Copy link

@mlautman mlautman commented Mar 25, 2019

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
Copy link
Author

@shaun-edwards @Jmeyer1292

@jrgnicho
Copy link
Contributor

@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
Copy link
Member

@mlautman: just about every commit included in this PR is attributed to [email protected]. Was that intentional? Do you work at Github now?

@mlautman
Copy link
Author

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
Copy link
Author

@mlautman: just about every commit included in this PR is attributed to [email protected]. 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
Copy link
Contributor

jrgnicho commented Apr 1, 2019

@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
Copy link
Author

mlautman commented Apr 3, 2019

@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
Copy link
Author

mlautman commented Apr 9, 2019

Ping @jrgnicho @Jmeyer1292

@jrgnicho
Copy link
Contributor

jrgnicho commented Apr 9, 2019

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
Copy link
Author

mlautman commented Apr 9, 2019

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.

// Check if we successfully got a locked planning scene
if (!(*ls))
{
CONSOLE_BRIDGE_logError("MoveitStateAdapter.isInCollision: Failed to secure a locked planning scene. Has the planing scene monitor been correctly initialized?");
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants