Lazier TSRPlanner refactor

[mkoval]

This supersedes #321 by making both IK generation and collision checking lazy.

The algorithm now looks like this:

1. Generate a set of num_candidates IK solutions without collision checking.
2. Rank the set of candidates by a user-provided score function.
3. Iteratively collision check the candidates in rank order until chunk_size are collision free.
4. If necessary, repeat (1)-(3) until the chunk is full or we reach a user-defined time limit.
5. Attempt to PlanToConfigurations to the chunk.
6. Repeat (1)-(5) num_attempts times.

Note that the sampling timeout is shared across all attempts. We may try fewer than num_attempts planning queries if we are unable to generate collision free samples quickly enough.

[cdellin]

@gilwoolee has been having trouble testing the previous PR (#321) -- it seems to cause hangs on the tableclearing demo, related to locking. (Originally raised in this comment).

I just tested this (otherwise glorious) refactor, and it exhibits the same behavior. Here's an example:

$ rosrun table_clearing run.py --planner restart --nobowl --noglass --perception-sim
...
[INFO] [hgpc.actions.base:base.py:42]:__enter__: Validate precondition: None
[WARNING] [prpy.base.robot:robot.py:352]:PostProcessPath: Post-processing smooth paths is not supported. Using the default post-processing logic; this may significantly change the geometric path.
[INFO] [hgpc.actions.base:base.py:47]:__exit__: Validate postcondition: None
openravepy.interfaces.BaseManipulation: __del__, failed to lock environment for BaseManipulation.__del__!
[WARNING] [openravepy.interfaces.BaseManipulation:BaseManipulation.py:47]:__del__: failed to lock environment for BaseManipulation.__del__!
openravepy.interfaces.TaskManipulation: __del__, failed to lock environment for TaskManipulation.__del__!
[WARNING] [openravepy.interfaces.TaskManipulation:TaskManipulation.py:53]:__del__: failed to lock environment for TaskManipulation.__del__!
openravepy.inversekinematics: __del__, failed to lock environment for InverseKinematicsModel.__del__!
[WARNING] [openravepy.inversekinematics:inversekinematics.py:280]:__del__: failed to lock environment for InverseKinematicsModel.__del__!
openravepy.inversekinematics: __del__, failed to lock environment for InverseKinematicsModel.__del__!
[WARNING] [openravepy.inversekinematics:inversekinematics.py:280]:__del__: failed to lock environment for InverseKinematicsModel.__del__!
^\Quit

It then just hangs there indefinitely. The original code before both versions of this refactor finishes successfully.

Any ideas where to look? Could this be related to cloning in the wrapper somehow?

[cdellin]

OK, I found a workaround for the class of bug from last night. (Issue about potentially deeper issue in #324). Workaround committed!

[mkoval]

I addressed both of @gilwoolee's issues and plan to merge once Travis is happy.