The preempt_request_ flag was only checked at the top-level task container before each compute iteration.
As a single sweep might take a while, we should check the flag before computing each stage.
Calling preempt() before plan() is able to reset the preempt_requested_ flag causes the preemption request to get lost. To avoid this issue, we allow a) manual resetting of the request and b) reset the request before leaving plan().
When min_distance == max_distance > 0.0, the minimal distance might be missed due to numerical errors.
To avoid this, deactivate the minimal distance check and run the full distance as given by max_distance.
- Unify move_to.launch.py and test_task_model.launch.py
- Rename them to test.launch.py as they are independent of the executable
- Move Node creation to the fixture constructor
- Replace Task::setRobotModel(loadModel()) with Task::loadRobotModel()
... to illustrate that MoveRelative's min-max constraint fails with PipelinePlanners (e.g. Pilz) returning a partially invalid trajectory: MTC does not truncate the trajectory to its valid part and thus fails, even if the valid part fits the given min-max range. This logic is only supported for the CartesianPath planner for now.
Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Looks like Rolling's MoveIt uses a more relaxed goal constraint threshold than Humble.
For this reason, all end states reached by Connect solutions of pick+place demo are rejected.
This commit relaxes the max_distance threshold of Connect accordingly.
This stage can be used to add arbitrary user-defined properties w/o modifying the PlanningScene state or adding a trajectory.
Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
The cost of a newly created Priority does not need to be finite.
An example occurs in the Pick+Place demo: the CurrentState solution
is filtered by an applicibility test, which may set the cost to infinity
while lifting the solution to the wrapper stage.
In this case, new InterfaceStates are created from the infinite cost solution.
Consequently, the state should be marked as PRUNED.