Commit Graph

1005 Commits

Author SHA1 Message Date
Robert Haschke
a0bc0602e8 Enable tests
Adapt test results FallbacksFixturePropagate.computeFirstSuccessfulStagePerSolutionOnly
due to 2e63c154aa:

The order of computations has changed, because we lock the processed state
as soon as it is forwarded to the first fallback child.

In this case, after processing GEN1 und FWD1 once, we have the two states with costs 2, 4 in the queue.
The first one, i.e. with cost 2 is forwarded to the child FWD2, which fails.
In the next cycle, although we have new states in the queue (1, 2, 3, 4), we stick with state "2"
and forward it two FWD3, which adds costs 210, resulting in 212.

With previous code, the Fallback container switched to state "1", forwarded to FWD2.
2021-11-24 15:07:21 +01:00
Robert Haschke
2e63c154aa Reintroduce pending state 2021-11-22 01:39:45 +01:00
Robert Haschke
5c235ab580 debugging helper function 2021-11-22 01:39:45 +01:00
Robert Haschke
dcb6857f36 Simplify computePropagate()
- Drop variable current_external_state_
- Instead encode the info that the external state wasn't yet forwarded to any child via stage = children().cend()
- If all children have exhausted their solutions for this state, it is removed from the pending list
2021-11-22 01:39:45 +01:00
Robert Haschke
c33b1967bc Handle updates on external states 2021-11-22 01:36:34 +01:00
Robert Haschke
e5b20ac11f Fix pruning
Pruning - if acting on the external state - needs to pass the current stage (this).
2021-11-22 01:36:34 +01:00
Robert Haschke
6653c4853a Rename: computeFromExternal -> computePropagate 2021-11-22 01:36:34 +01:00
Robert Haschke
b783173b27 GENERATE: return correct canCompute() result as early as possible
Moving to next child generator only in compute() requires an extra call
to canCompute() to notice the failure of the next generator(s).
2021-11-22 01:35:34 +01:00
v4hn
b6ac5b09ba add demo illustrating useful fallbacks behavior 2021-11-15 09:29:09 +01:00
v4hn
887da5b094 fix fallbacks logic
Setting up a demo for
Fallbacks({CartesianPath,PTP,RRTConnect})
I found the logic did not work as expected yet.

- process last job spec as well
- ignore failures when looking for a solution
- add more debug output
2021-11-15 09:29:09 +01:00
v4hn
22809c04a5 order external states 2021-11-15 09:29:09 +01:00
v4hn
9a583ab006 run only one compute step per call
Note that while this ensures other stages outside the Fallbacks container
can compute as well, it does not solve the problem internally.
A new incoming state will only ever be considered once
the current stage cannot compute any more.

We have no way of telling a child to compute for *a specific state* for now.
So once we copied a state to its interface we have to let it compute until
all possibilities are exhausted to detect whether or not it could generate a solution for it.
If we wouldn't do so, there were no way of knowing when to fall back
to the next child as long as the stage can still compute on *any* copied solution.
2021-11-15 09:29:09 +01:00
v4hn
5a9cfc50ea cleanup: get rid of superfluous parameter 2021-11-15 09:29:09 +01:00
v4hn
aa733fcf5f simplify onNewFailure
give an elaborate reason for an empty overload that doesn't call the parent.
2021-11-15 09:29:09 +01:00
v4hn
3d5cecd753 fallback generator can run a single job per compute call 2021-11-15 09:29:09 +01:00
v4hn
8719b1c3d6 Implement state-wise Fallbacks
Keep the previous logic around for Generator stages.
Note that this only makes sense for *pure* Generators and not for MonitoringGenerator,
because for the latter we would expect monitored solutions to be passed individually
(similar to pruning).
2021-11-15 09:29:09 +01:00
v4hn
f32e747645 add hook to ParallelContainerBase to customize state propagation 2021-11-15 09:29:09 +01:00
Michael Görner
79869b856c
Merge pull request #304 from v4hn/pr-move-to-tests
Add MoveTo tests & make them pass
2021-11-12 23:14:57 +01:00
v4hn
3b835986e3 refactor logic to handle ik_frame
fallbacks and verification.
2021-11-12 14:02:46 +01:00
v4hn
f1fc447e3b establish utils namespace
leaves us a place to put free helper functions
2021-11-10 14:36:58 +01:00
v4hn
48959c6806 add compat header to cmake
previous oversight
2021-11-10 14:35:25 +01:00
v4hn
0f53db5f12 simplify parameter-free lambdas
I just didn't know the syntax was allowed
2021-11-10 13:33:26 +01:00
Robert Haschke
01d0cf9a13 Extend mtc_add_test() macro to handle rostest as well 2021-11-10 13:14:04 +01:00
v4hn
ef27a6eb22 ComputeIK supports attached-object ik frame 2021-11-10 13:14:04 +01:00
v4hn
e1216aa8ab MoveTo supports attached objects&subframes for ik frame 2021-11-10 13:14:04 +01:00
v4hn
aee76fee5e add move MoveTo tests
(partially disabled because broken)
2021-11-10 13:12:21 +01:00
v4hn
86fe752d43 InterpolationPlanner: implement simple IK-based solver for pose targets 2021-11-10 13:12:21 +01:00
v4hn
ef86799f27 add some tests for MoveTo 2021-11-10 13:12:21 +01:00
v4hn
dfe746b9bb FixedState: add optional scene in constructor 2021-11-10 13:12:21 +01:00
v4hn
e60a9793f1 fix test helper
never unload the plugin loader before the plugins (IK plugins here).
We don't have unrelated loaders in gtest executables, so the static should be fine.
2021-11-10 13:12:21 +01:00
Robert Haschke
d6f68f9254 Simplify code
We know that trajectory at least comprises the start state.
Thus, we don't need the sanity checks.
2021-11-10 12:11:39 +01:00
Robert Haschke
71fab0fbce Simplify allowCollisions(object, JMG) 2021-11-10 12:11:39 +01:00
Robert Haschke
aa54de0447
Gracefully handle execution_type (#307)
In moveit_resources 0.8.1 the arg execution_type was renamed to fake_execution_type.
To support both, Melodic and Noetic versions of moveit_resources, provide both name variants.
Additionally, disable  roslaunch's checking via pass_all_args="true".
2021-11-10 10:00:13 +01:00
Robert Haschke
9ca8f5a1e3 Improve CI
- Allow downloading of pre-commit + clang-tidy fixes
- Use clang compiler for clang-tidy job
- Adapt to improved lcov-action
2021-10-22 23:28:43 +02:00
Michael Görner
7ec874572c move to tf2_eigen everywhere (#301) 2021-10-18 23:34:29 +02:00
v4hn
55e30e26ce split off pruning tests
yes, most pruning happen along children of a serial container,
but children for many tests comprise a lot of other containers as well.

- migrated pruning tests from Connect to ConnectMockup (as the concrete implementation
is not relevant for them)

- added missing header to stage_mockups.h
2021-10-04 23:06:13 +02:00
v4hn
9428c83ba5 consolidate test base 2021-10-04 23:06:13 +02:00
Michael Görner
8b2ac94609
define core tests through macros (#299)
Maybe it makes sense to define this in an exported config,
but then why bother until someone needs it.
2021-10-04 22:27:34 +02:00
Michael Görner
e89c68ced0
Merge pull request #294 from v4hn/pr-fallbacks-split-tests
new set of fallback tests
2021-09-20 23:13:10 +02:00
Robert Haschke
9da3a8055e Optimize setStatus(): only escalate to parent interface at boundaries of a partial solution
In all other cases internalToExternalMap().find(s) will fail anyway.
2021-09-20 23:07:00 +02:00
v4hn
47d20984ae simplify test specification
before anyone starts copying from here...
2021-09-20 23:07:00 +02:00
v4hn
81ec8bb3aa demos: improve CMakeLists
- build pick_place_task as proper library as people (e.g. moveit_benchmark_suite) reuse it
2021-09-20 23:07:00 +02:00
v4hn
35560d897d DISABLED -> PRUNED
This makes the semantics much clearer as states can only be disabled by pruning.
2021-09-20 23:07:00 +02:00
v4hn
d143dd7076 simplify exception handling
This could have been done already back when `runCompute` was introduced.
Wrapping the calls in try/catch comes from the previous implementation directly
calling `compute()`.
2021-09-20 23:07:00 +02:00
v4hn
1e10aaccfd Connecting: add another ROS_DEBUG hint
These can facilitate debugging a lot for little overhead.
2021-09-20 23:07:00 +02:00
v4hn
40b00c61d2 DISABLED_FAILED -> FAILED
Failed states are *not* disabled, they just failed connecting (for now).
2021-09-20 23:07:00 +02:00
v4hn
3b9f6ee519 add todo 2021-09-20 23:07:00 +02:00
v4hn
dbfa7e2b8a disable currently failing tests
They will be enabled when the corresponding functionality is merged.
2021-09-20 15:11:13 +02:00
Robert Haschke
35dc037365 Add more fallback tests 2021-09-20 14:52:15 +02:00
Robert Haschke
4e0f295d1e Replace std::list<double>() with PredefinedCosts() 2021-09-20 14:50:13 +02:00