Commit Graph

147 Commits

Author SHA1 Message Date
v4hn
b7f80939ef add more asserts to onNewSolution
Strictly speaking there is no need for the invariant,
but if something violates it a stage computed something it wasn't meant to.
2021-03-22 14:42:23 +01:00
v4hn
be1c8292b5 add more (commented-out) debug statements
Also print generator symbol for completeness.
2021-03-22 14:42:23 +01:00
v4hn
a82b88355e streamline trait parameters
state asked for a reference, but trajectories for a pointer.
2021-03-22 14:42:23 +01:00
v4hn
bca01e8aa7 rename Status values
Start and End are already used for an entirely different concept,
so if anyone ever wants to read this code, we should use new terms instead.

Because the source state is the disabled state that *failed* to extend,
triggering the whole subtree to be disabled, I went for the new terms
DISABLED and DISABLED_FAILED.
2021-03-22 14:42:23 +01:00
Robert Haschke
c0100ff775 Remove debug output 2021-03-22 14:42:23 +01:00
Robert Haschke
d3864596b6 Replace Priority::enabled() with status()
The key to pruning in the Connecting stage was the following:
- Don't remove states during pruning, but only disable them.
  They might become re-enabled due to further input.
- Distinguish START and END sides of a disabled solution tree to break their symmetry.
  The START side from where we started disabling, can be re-enabled by a new partner state in
  Connecting, the END side must not. This was important as, otherwise, the states would simply
  get re-enabled immediately. The END side only gets re-enabled if the START side actually
  connects the whole solution branch.
2021-03-22 14:42:23 +01:00
Robert Haschke
f229743024 Debug evolution of interfaces 2021-03-22 14:42:23 +01:00
Robert Haschke
604a0da1e4 Actually implement pruning and re-enabling of states 2021-03-22 14:42:23 +01:00
Robert Haschke
19484cec64 Restore pruning
If a stage fails to find a solution, this often implies that further planning
on the open end(s) of connected InterfaceStates is not needed anymore.
Thus the InterfaceStates along all connected solution paths will be marked as disabled.
They are not removed from the pending state lists though, because they might get
reactivated by solutions found in future.

To this end, we introduced the method ContainerBase::onNewFailure().
2021-03-22 14:42:23 +01:00
Robert Haschke
2a09daa42f InterfaceState::Priority::enabled()
To allow pruning, we need to enable and disable InterfaceStates to be considered for further
planning. In the past, we just indicated the disabled status with infinite costs.
However, because we might need to re-enable states (with previous state),
we need to separate these two concepts.
2021-03-22 14:42:23 +01:00
Robert Haschke
a6fa45214f Remove return value from Container::insert
adapted Container::add

Logical consequence of removing the ROS_ERROR output in `setParent`
2021-02-24 12:22:38 +01:00
Robert Haschke
f2d2cf10f0 Fallback: Correctly end iterating children
Fixes #234
2021-02-17 21:17:05 +01:00
Robert Haschke
9f18219993 minor cleanups
* unify usage of pimpl()
* fix StatePair constructors
* improve/add comments
* test_container: reset MOCK_ID for each test to facilitate identification of stages
2020-12-06 23:55:58 +01:00
Robert Haschke
a6a23d3775 Remove set[Start|End]StateUnsafe
These methods were introduced to temporarily set meaningful states for cost calculation
w/o connecting the solution to these temporary states (to solve a chicken-egg problem).
This commit provides TmpInterfaceStateProvider as an alternative approach to this problem.

This essentially reverts 53c0964618.
2020-12-06 04:11:35 +01:00
Robert Haschke
c8e75ab205 Simplify SolutionCollector 2020-12-06 04:11:35 +01:00
Robert Haschke
5d4cf640ef Fix issues #182 + #218 2020-12-06 04:10:47 +01:00
Robert Haschke
f05b08fa8a SerialContainer: Simplify solution traversal
Compute depth + accumulated cost of a partial solution sequence using InterfaceState::Priority.
2020-12-06 04:09:59 +01:00
Robert Haschke
66fdff9711 Simplify state priority updates
Only need to update the end points of a partial solution.
2020-12-06 04:06:08 +01:00
Robert Haschke
d3b878a31c Fix issues with Merger stage 2020-09-23 08:00:42 +02:00
Robert Haschke
88c217e9a6 Report exceptions (e.g. config issues) 2020-09-23 07:59:39 +02:00
Robert Haschke
7499f7b297 Report merge failures 2020-09-23 07:59:39 +02:00
Robert Haschke
14fc3d2586 Reject failures early 2020-09-23 07:59:39 +02:00
Robert Haschke
8152994e52 Fix interface resolution 2020-09-22 10:57:49 +02:00
Robert Haschke
4089d5eefa Container::remove(): return removed Stage 2020-09-22 10:56:11 +02:00
v4hn
45e6cdb47d Revert "add CostAggregator for SerialContainer"
This reverts commit dc7ce9bdfac97eb468f5a850adcb27cc118b5fd7.

It turns out multiple places in SerialContainer's cost inference
expect 0.0 as the neutral element (which is why std::min and multiply did not work).
While these additional issues can be fixed, it would make the interface much less elegant.

We should consider adding it back if an actual use-case is there to discuss.
2020-09-21 22:52:19 +02:00
v4hn
8ae1060b4d add CostAggregator for SerialContainer
It can be useful to change the default addition to other operators.

The simplest example is applying a Constant cost term to a container.
As the tests show, the visitor-based cost computation ends up adding
cost::Constant *for each subtrajectory*.
2020-09-21 22:50:51 +02:00
v4hn
0e38730fd4 compute cost in liftSolution as well 2020-09-21 22:31:52 +02:00
v4hn
4a6c389aee expose WrappedSolution together with other Solution types
Moved out of private header.

Users can access these objects, so they should know about the type.
2020-08-21 14:41:22 +02:00
v4hn
7609f0b1e2 Solution: export public creator
It's a public interface, returning private pointer is not neat.
2020-08-21 14:41:22 +02:00
v4hn
0aff5d56dd replace remaining typedefs by using declaration
I have no idea why these were not picked up by clang-tidy.
2020-07-22 12:57:34 +02:00
v4hn
21f3d95128 fix autocomplete typo 2020-07-22 12:57:34 +02:00
Robert Haschke
0902d432ce Improve code readability 2020-07-01 22:13:21 +02:00
v4hn
15707673ed clang-tidy: performance-*
I NOLINTed the noexcept move constructor for Task for now because
the constructor *can* indeed throw exceptions.
2020-06-11 11:03:50 +02:00
v4hn
8faba159f9 clang-tidy: llvm-namespace-comment 2020-06-11 11:03:50 +02:00
v4hn
2153237643 clang-format: readability-identifier-naming
Probably the most invasive format patch, also changing some internal API.

I deliberately disabled ClassCase and MethodCase checks for the moment
to avoid public API changes in this patch set.
2020-06-11 11:03:50 +02:00
v4hn
b27939cb27 fix typo 2020-06-11 11:03:50 +02:00
Robert Haschke
c4d0ab0636 SerialContainer: Resolve interfaces of all stages 2020-04-10 20:25:54 +02:00
Robert Haschke
499fcfb04b cleanup / renaming
* Rename pruneInterface() -> resolveInterface()
* Rename accepted (interface) -> expected
* Improve exception strings
2020-04-10 20:11:51 +02:00
Robert Haschke
aa732d8c66 simplify internal API 2020-04-10 19:59:27 +02:00
Robert Haschke
e56c772ecb Combine pruneInterface() + validateConnectivity()
Validate interfaces during resolution. No need to separately validate interfaces.
Thus, validateConnectivity() is removed from Task::init().
Functions are kept (but simplified) for unit testing.
2020-04-10 19:59:27 +02:00
Robert Haschke
10260b9dd7 Further simplify interface resolution
* Do not modify explicitly configured direction of Propagator
* Avoid code duplication in pruneInterface() and validateConnectivity()
  Only implement pruneInterface() for stages that actually need to adapt their interface.
2020-04-10 19:59:23 +02:00
Robert Haschke
a98d5ed8ae templated flowSymbol<input/output mask>(flags)
* Enforce at compile time that either input or output flags are considered.
* Use horizontal flow symbols (as in console output).
* Replace redundant direction(flags).
2020-04-10 19:39:32 +02:00
v4hn
b6a5f89307 remove logical flow BOTH / simplify pruning
The PROPAGATE concept BOTH declared the stages *will* propagate solutions in
either direction. ANY, on the other hand, only means the propagation
direction is *not resolved yet* (but will be at planning time).

BOTH was originally described to support a more general control flow
than was eventually decided to support. The four exclusive Stage interfaces
CONNECT, PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS, and GENERATOR
do not allow for BOTH as a valid setup anymore, unless you setup a very
convolved task like `Alternatives(GEN, PROP) - Alternatives(PROP, GEN)`
which would be very complex to inspect. The same functionality can still
be achieved more readable as `Alternatives(Seq(GEN, PROP), Seq(PROP, GEN))`.

The confusion between BOTH (propagator *will* send in both directions) and
ANY (propagator will send in *either* direction, decided during init) led
to a lot of confusion with users and was not fully accounted
throughout the pipeline.

Adjust tests.

Notice the difference between ANY (unresolved propagator) and UNKNOWN
(a container before introspecting its children). propagators still
report UNKNOWN as requiredInterface though to simplify control flow.

The simplification enables a much simpler linear inference of the connective
structure of a task, as the first interface direction is always given.

Additionally, unify the resource setup for static interfaces to run
in the constructor, and for dynamic initialization in `pruneInterface`,
getting rid of partial initializations in `init`.
2020-04-08 13:27:36 +02:00
v4hn
5ae7e54732 treat empty container as init exception 2020-04-08 13:27:21 +02:00
v4hn
87d70a5da1 minor documentation improvements 2020-04-08 13:27:14 +02:00
Robert Haschke
f1acfa2f7e
Merge pull request #151: Various fixes 2020-04-03 11:58:19 +02:00
Robert Haschke
8834ce18f7 simplify trajectory merging
avoid code duplication: reuse merge(std::vector<JointModelGroup*>)
2020-04-03 11:36:05 +02:00
v4hn
7cd82d5c04 resolve clang warnings & suggestions 2020-04-01 15:05:20 +02:00
v4hn
8578a64ac0 correct sign for StageCallback type
backward compatible, the function type implicitly converts from previous int definitions.
2020-04-01 15:05:20 +02:00
v4hn
c046ec605d streamline add/insert interfaces for Task/Container
`add` falls back to `insert` for both structures,
but `add` throws exceptions and does not provide a return value.

`insert` provides standard STL container access.
2020-04-01 15:05:20 +02:00