Commit Graph

123 Commits

Author SHA1 Message Date
v4hn
0e40b464ec support TYPED_TEST_SUITE
Get rid of deprecation warning if new variant exists.
2021-06-04 14:19:52 +02:00
v4hn
9d53987e53 satisfy clang-tidy & -Werror -Wall -Wextra 2021-06-04 14:19:52 +02:00
Robert Haschke
437cc550f2 Add missing semicolons after cleanup of MoveIt 2021-05-29 00:38:43 +02:00
v4hn
059a939203 Fix test: correctly reset Interface 2021-05-28 00:20:07 +02:00
v4hn
58b1a020b5 Fix memory leak in unit test 2021-05-28 00:19:45 +02:00
v4hn
7eb8fad57e remove unused helper method 2021-05-12 19:21:19 +02:00
v4hn
f7015ee021 fix pick_ur5 test
TAMS' models changed and often produces less solutions.
2021-05-12 19:21:19 +02:00
Robert Haschke
cfcb1f274e Fix more clang-tidy issues 2021-04-24 02:12:24 +02:00
Robert Haschke
4fa8c10f44 clang-tidy auto-fixes 2021-04-24 02:03:24 +02:00
Robert Haschke
889b61529b Fix trailing white space 2021-04-23 23:49:50 +02:00
Robert Haschke
dc8cd34b86 Simplify tests 2021-04-01 00:21:07 +02:00
v4hn
4e09d78239 Implement pruning inside-to-outside of a container
- Remove public onNewFailure() interface
  Moved to ContainerBasePrivate to reuse logic for serial and parallel containers.
- Add tests
2021-04-01 00:19:15 +02:00
v4hn
106c138ef5 Implement pruning inside serial container
By inefficient inverse lookup.

Also add disabled test for the inverted inference (failure inside should prune outside)
2021-03-30 15:19:49 +02:00
v4hn
3e5e1da429 add (failing) simple pruning test
to validate propagators do not compute disabled solutions.
It's also good to have a simpler test around than 2 connects.
2021-03-22 14:42:23 +01:00
v4hn
9cf3c01b9a pruning tests: restructure tests 2021-03-22 14:42:23 +01:00
v4hn
f8f885966e add failing pruning test for branching propagator 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
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
7c6eb1c0f3 Extend unit tests: PruningForward + PruningBackward 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
6c18e2b482 test comparison of ConnectingPrivate's StatePairs 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
5de9ce7203 Perform clang-format check via github action from pre-commit.com 2021-03-15 14:29:19 +01:00
v4hn
3d8faea079 add test to validate correct solution state in computeCost
This is somewhat cumbersome because of the additional internal/external
layer introduced through the container.
But it's still better than leaving this unverified.
2021-03-05 14:38:29 +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
f99d03386a
Use RobotModelBuilder to simplify tests (#225)
* Simplify RobotModel definition using RobotModelBuilder
* Silent RobotModel errors once in models.cpp
2020-12-30 11:07:57 +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
0925031a81 Propagating: disable infeasible state pruning
Because stages might get re-enabled later, we must not throw them away.
Remove dropFailedStarts(), dropFailedEnds().
Extend Pruning unittest.
2020-12-06 23:55:10 +01:00
Robert Haschke
c73c0c3555 Interface::updatePriority(): only allow decrease 2020-12-06 04:06:08 +01:00
Robert Haschke
fe189cd692 Test Interface::updatePriority() 2020-12-06 04:06:08 +01:00
Robert Haschke
5fd98ce307 SerialContainer tests 2020-12-06 04:06:08 +01:00
Robert Haschke
cb0f20cad7 Improve task unittests 2020-10-24 03:22:46 +02:00
Robert Haschke
198f8f5389 clearer naming of cost variables 2020-09-21 22:52:19 +02:00
v4hn
62586c2688 rework CostTerm's from support flags to vtable
requested in review.

Without support for custom aggregators, which we dropped again
after finding more flaws with it, I agree that this is the nicer solution.

On the downside, it converts the interfaces from copyable objects
to another round of shared_ptrs.

I added shortcuts for lambda costs to keep support for
`stage->setCostTerm([](auto&& s){ return 42; })`
without the additional
`stage->setCostTerm(LambdaCostTerm{ [](auto&& s){ return 42; } } )`
2020-09-21 22:52:19 +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
cdc7cb04fc refactor Forward -> PassThrough
based on review feedback
2020-09-21 22:52:19 +02:00
v4hn
05932ad5f1 enable implicit construction of CostTerm from lambdas
without the need for explicit casts to the correct signature.
2020-09-21 22:52:19 +02:00
v4hn
034a155315 provide CostTerm class for all CostTerms
This pattern allows cost::Constant to override the hierarchical cost computation
for the SerialContainer and avoid traversing the graph.

I implemented the CostTerm::supports() pattern over a full double visitor pattern
with overloads for each SolutionBase specialization, because the SerialContainer
needs to know whether cost aggregation of the subsolutions should take place or whether
the SolutionSequence should be forwarded to the CostTerm.
This would not be possible with a `virtual double operator()(const SolutionSequence&)`
callback in CostTerm.

Alternatively, implementing the hierarchical aggregation in the default implementation
of this operator would be possible as well, but breaks intuition:
- the corresponding methods to handle `SubTrajectory` and `WrappedSolution` *have to*
  default to not touching the solution's cost at all so it is inappropriate to have
  the default implementation for the Sequence do something else
- The SerialContainer also aggregates costs outside the `computeCost()` interface
  (in multiple places in `onNewSolution()` to aggregate costs along partial paths)
  and thus moving the hierarchical aggregation to the CostTerm methods requires
  the aggregator to be shared between the Container and the CostTerm,

The only shortcoming of the implemented approach, by contrast, is that user implementations
that want to handle WrappedSolution or SolutionSequence differently have to ensure
the supports_ flags are set correctly. Notice that most custom CostTerms will only
ever access SubTrajectories and this case is simplified with the provided CostTerm constructors.
2020-09-21 22:50:51 +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
28b98e989c improve cost term tests
- add tests for nested containers
- add descriptions to checks
- refactor helper to support different containers
2020-09-21 22:50:45 +02:00
v4hn
dcee7c408f add basic tests for using cost terms 2020-09-21 22:31:52 +02:00
v4hn
1a2eafa256 fix stage test mock
base class init needs to be called although this didn't pose problems
until now.
2020-09-17 23:00:12 +02:00
Michael Görner
ee6c50ad31
Apply clang-format-10 (#199) 2020-08-20 11:56:53 +02:00
Michael Görner
47f052b93d
adapt tests to new moveit_resources layout (#200) 2020-08-20 11:04:18 +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
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
b762079478 clang-tidy: readability-named-parameters 2020-06-11 11:03:50 +02:00
v4hn
3fe4436ad7 clang-tidy: modernize-use-override 2020-06-11 11:03:50 +02:00