Commit Graph

50 Commits

Author SHA1 Message Date
Robert Haschke
bcacda8507 implement ParallelContainer's pruneInterface() 2018-02-25 13:22:26 +01:00
eirtech
0dc4f5c53e ParallelContainer's requiredInterface() 2018-02-25 13:22:26 +01:00
Robert Haschke
a2fd5fb7b8 more constexpr InterfaceFlags 2018-02-25 11:59:51 +01:00
Robert Haschke
a24039846a Merge branches 'analyze-property-errors' and 'fixes' 2018-02-24 13:45:51 +01:00
Robert Haschke
e9d1742337 ContainerBase::exposePropertiesOfChild 2018-02-24 13:42:18 +01:00
Robert Haschke
6444229877 Stage::reportPropertyError 2018-02-24 13:42:18 +01:00
Robert Haschke
cf54024379 fix derivation of propagation direction from connect stage 2018-02-24 13:42:12 +01:00
Robert Haschke
6a3456f961 unittest ContainerBasePrivate::position() 2018-02-24 13:42:12 +01:00
Robert Haschke
9cd6efe46f Stage::init(PlanningScene) -> Stage::init(RobotModel) 2018-02-24 13:37:50 +01:00
Robert Haschke
cf76352d2d Merge branches 'fix-containers' and 'fix-priority-updates' 2018-02-20 08:07:48 +01:00
Robert Haschke
ba75f7c835 fixed remaining unittest 2018-02-20 08:06:23 +01:00
Robert Haschke
e368fd9948 avoid accidental overwrite of InterfaceState 2018-02-19 09:05:56 +01:00
Robert Haschke
373d7f204f update priorities of all interface states along a (partial) solution path
if a parallel container is involved somewhere in the middle, it will
again access these states, e.g. planning alternative solutions
2018-02-19 09:05:56 +01:00
Robert Haschke
1eacea6b35 reset InterfaceState::owner_ if state is removed from Interface 2018-02-19 09:05:56 +01:00
Robert Haschke
35f1540369 recursive interface auto-detection
trigger auto-detection from top (task) level:
only there we now for sure the accepted interface
2018-02-19 01:05:56 +01:00
Robert Haschke
f45a4e3e2c improve validation
- postpone pruning / interface auto-detection to top task-level
  only the task state knows for sure, that it requires its wrapped child to push to both ends
- perform connectivity validation only after pruning
  only then, we the interfaces are completely determined
2018-02-19 01:05:56 +01:00
Robert Haschke
a523333ef6 basic auto-detection of interfaces for propagating stages
works if propagation direction can be derived from a generator or
connecting stage within the sequence

start-to-end propagation through whole serial container still fails
2018-02-18 21:23:12 +01:00
Robert Haschke
1a0b9b36ee fix connection creation
Establishing the interface connections, we face a chicken-egg-problem:
To establish a connection, a predecessors/successors pull interface is
assigned to the current's stage push interface.
However, propagating stages (in auto-detection mode) can only create
their pull interfaces if the corresponding, opposite-side push interface
is present already (because that's the mechanism to determine the supported
propagation directions).

Hence, we need to resolve this by performing two sweeps:
- initialization, assuming both propagation directions should be supported,
  thus generating both pull interfaces, i.e. providing the egg
- stripping down the interfaces to the actual context
  This context is provided by two stages pushing from both ends
  into a (potentially long) sequence of propagating stages (tbd).

Contributions of this PR:
- PropagatingEitherWay: explicitly distinguish AUTO from BOTHWAYS interface
  AUTO: auto-derive interface from provided push interfaces
  BOTHWAYS: explicitly require both directions
- SerialContainer: (better, but not yet perfect) validation of connectivity
- ParallelContainer: determine interface from what children offer
2018-02-18 21:23:12 +01:00
Robert Haschke
a4feb705d0 cleanup operator<<(ostream, *)
- basic implementation for StagePrivate
- implementation for Stage calls this
- implementation for ContainerBase recursively calls this
- implementation for Task added
2018-02-18 13:58:09 +01:00
Robert Haschke
2ea463fff4 replace std::cout with ros console 2018-02-16 16:01:05 +01:00
Robert Haschke
acffd39091 SerialContainer: fix priority propagation
- traverse all (also partial) solution paths
- and update priority at both ends
- remove pending state pairs if cost increased to infinity
2018-02-16 16:01:05 +01:00
Robert Haschke
449ccf55c6 merge WrapperBase + Wrapper into WrapperBase
Wrapper is not restricted to generator-type stage anymore.
2018-02-12 23:39:46 +01:00
Robert Haschke
032a4e23c1 derive WrapperBase from ParallelContainerBase 2018-02-12 23:39:46 +01:00
Robert Haschke
1ab2d596fb ContainerBasePrivate::liftSolution() 2018-02-12 23:39:46 +01:00
Robert Haschke
919be24d16 implement ParallelContainerBase 2018-02-12 23:39:46 +01:00
Robert Haschke
a3a8f1a9d2 improved directional (forward/backward) access to interfaces
- TraverseDirection -> Interface::Direction
- StagePrivate: pullInterface(), pushInterface()
- trajectories<dir>(SolutionBase) -> SolutionBase::trajectories<dir>()
2018-02-12 23:39:46 +01:00
Robert Haschke
73f509c3ba simplify usage of ContainerBasePrivate::copyState() 2018-02-12 23:39:46 +01:00
Robert Haschke
eaaaf07214 exploit default argument for Interface constructor 2018-02-12 23:39:46 +01:00
Robert Haschke
cf71d2f0d2 unify use of buffer interfaces in containers
All containers need to buffer their children's sendBackward/sendForward states.
2018-02-12 23:39:46 +01:00
Robert Haschke
3a4dc6755b Merge branches 'fix-remote-task-model', 'wip-cost-ordering' and 'wip-properties' 2018-02-12 23:35:01 +01:00
Robert Haschke
c3eead0115 allow only a single inititialization source 2018-02-12 22:45:15 +01:00
Robert Haschke
5c43d700a8 update state priorities in a container-specific fashion
Reorder interface list on priority updates.
This requires the InterfaceState to store a pointer to the owning Interface.
2018-02-12 22:44:18 +01:00
Robert Haschke
facd6438c9 Interface: order states by priority
Priority is (depth of solutions, accumulated cost along trace).
2018-02-12 22:44:18 +01:00
Robert Haschke
d0351698a6 single map internal_to_external
There is no need to distinguish between starts and ends when mapping
states, because start/end states need to be disjoint sets.
2018-02-12 22:44:18 +01:00
Robert Haschke
e6af069139 SerialContainer: traverse solutions w/o stopping stage
Always traverse from current solution to the start/end of a complete
path and only call the callback once for the whole trace.
2018-02-12 22:44:18 +01:00
Robert Haschke
244a7d147b sort serial solutions before insertion 2018-02-12 22:36:58 +01:00
Robert Haschke
29577515a0 cost ordering for solutions 2018-02-12 22:36:56 +01:00
Robert Haschke
2184450bd6 count all failures for statistics 2018-02-12 22:36:12 +01:00
Robert Haschke
97bb6ef2c6 allow handling of failures
- store "failure" solutions to facilitate debugging
- Introspection assigns solution IDs as soon as they are created in a stage
  Thus, solution IDs represent their creation order.
  In contrast, the order of publishing (in StageStatistics) should
  represent the cost order.
- Storing failures is disabled if Introspection is not available.
2018-02-12 22:35:58 +01:00
Robert Haschke
57cd11e3a9 Wrapper::compute: return true if new solutions are found 2018-02-12 22:33:39 +01:00
Robert Haschke
5c1cad14df Merge branches 'fixes', 'wip-drag-n-drop', 'wip-ikstage' and 'wip-marker-vis'
resolved conflicts:
core/include/moveit/task_constructor/container.h
core/src/container.cpp
core/src/stages/generate_grasp_pose.cpp
visualization/motion_planning_tasks/src/task_display.cpp
visualization/motion_planning_tasks/src/task_panel_p.h
2018-02-04 21:36:01 +01:00
Robert Haschke
b6959170db derive WrapperBase from ContainerBase
While ParallelContainers can directly promote their child solutions as
their own, a Wrapper needs to modify those solutions. Hence, Wrappers
should be derived directly from Container.
2018-02-04 09:01:45 +01:00
Robert Haschke
e4ad7a0753 removed Stage::validate()
... only was checking for implies(a, a) which is always true
2018-02-04 09:01:45 +01:00
Robert Haschke
104e52eb48 promise to not modify solutions 2018-02-04 09:01:45 +01:00
Robert Haschke
e9363919e8 Merge branch 'wip-properties' 2018-02-04 08:59:35 +01:00
Robert Haschke
c9e3be08d0 fix initialization order of properties
First from INTERFACE, second from PARENT.

INTERFACE initialization only makes sense for Propagating stages.
Connecting stages should ensure that interfaces define identical
properties which is not possible with boost::any.
2018-02-04 08:56:11 +01:00
v4hn
d7719e721f add full license information
so dull... but for the matter of completeness
2017-12-12 21:24:32 +01:00
Robert Haschke
957b10a7a8 don't export SerialContainer as pluginlib class
... it's imported as builtin
2017-11-20 00:07:27 +01:00
Robert Haschke
df074b48a5 cleanup TODO 2017-11-12 21:42:48 +01:00
Robert Haschke
9c5ddd3f6d split repo into different ROS packages: msgs, core, visualization 2017-11-12 21:42:48 +01:00