Commit Graph

934 Commits

Author SHA1 Message Date
Robert Haschke
b82b70ed64 FallbacksPrivate::nextChild()
... factoring out functionality shared between FallbacksPrivateGenerator
and FallbacksPrivatePropagator to switch to next child in nextJob().
2022-01-05 16:37:46 +01:00
Robert Haschke
7af3d8ebd7 Improve readability 2022-01-05 16:37:46 +01:00
Robert Haschke
b2c116edab reset(new Interface()) -> std::make_shared<Interface>() 2022-01-05 16:37:46 +01:00
Robert Haschke
442d39ad3e Improve comments 2022-01-05 16:37:46 +01:00
Robert Haschke
b2056745a8 Generalize connectStageInsideFallbacks
Let's consider the following simple situation, where generators produce solutions in the given order.

GEN           1 3
Fallbacks     |X
GEN           2 4

When passing state 4 to the Fallbacks' connector, it forms pending pairs with both 1 and 3.
Thus, the container needs to check whether 1-4 or 3-4 was processed when receiving a success or failure,
to correctly forward the failed one to the next child.
2022-01-05 16:37:46 +01:00
Robert Haschke
184fab8e0a GeneratePlacePose: add property 'allow_z_flip' 2022-01-04 15:58:19 +01:00
Robert Haschke
9630f4d789 ComputeIK: Improve markers
- always provide eef markers (also in case of success)
- tint failures in red
- use different names for "ik frame" and "target frame" markers
- reduce code duplication
2022-01-04 15:57:34 +01:00
Robert Haschke
91fe791bdb Add comment 2022-01-04 15:12:06 +01:00
Robert Haschke
72510818c8 Export libmoveit_python_tools.so 2022-01-04 15:11:23 +01:00
Jafar Abdi
7dbe0b87e1
Return MoveItErrorCode from task::plan (#319)
... to know whether the plan failed due to timeout, preemption, or actual planning failure
2022-01-02 15:32:37 +01:00
v4hn
84f96ec74c MoveRelative: Interpret direction relative to IKFrame
bugfix
2021-12-20 10:19:52 +01:00
v4hn
191ff253fd add tests for MoveRelative 2021-12-20 10:19:52 +01:00
Robert Haschke
4be448641f Improve debug output
- printChildrenInterfaces(): fix/add usage
- printPendingPairs(): full colorization according to status
2021-11-28 18:28:20 +01:00
JafarAbdi
f21c8ccd35 Port core to ROS2 2021-11-25 23:02:51 +03:00
Robert Haschke
b4a9e2033d
Stage::reset() should reset total_compute_time_ (#310) 2021-11-25 19:47:00 +03:00
Robert Haschke
e296bd7aed Simplify: job_has_solutions_
Just set a flag when we received a full solution
2021-11-25 07:37:28 +01:00
Robert Haschke
7237e81547 Rework FallbacksPrivate*
Further factorize and simplify FallbacksPrivate classes employing ideas from @v4hn.
The key difference between the variants his how they advance to the next job.
Thus, the only virtual method required is nextJob().
2021-11-25 07:36:32 +01:00
Robert Haschke
070c6e9ab6 Disable failing test FallbacksFixtureConnect.connectStageInsideFallbacks
... as we are now missing the implementation for CONNECT interfaces
2021-11-25 07:35:28 +01:00
Robert Haschke
8dd8022ef9 Factorize implementation of FallbacksPrivate into 3 classes 2021-11-24 21:17:49 +01:00
Robert Haschke
e67b3252fc static TaskPrivate::swap() -> ContainerBasePrivate::operator=()
- Enable moving/swapping of other container impls (e.g. Fallbacks)
- Clarify (via move semantics) that content of source impl will be lost
- Get rid of friend declarations
2021-11-24 21:17:49 +01:00
Robert Haschke
c822fd3822 Remove logger configuration
Logger config can be more easily handled via ROSCONSOLE_CONFIG_FILE.
2021-11-24 15:08:20 +01:00
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
Robert Haschke
aef991602f Propagate either STATUS or PRIORITY updates into a container 2021-11-22 00:12:50 +01:00
Robert Haschke
a48b932dce Distinguish STATUS and PRIORITY updates in notify() callbacks
to allow propagating status updates only if the STATUS actually changed.
2021-11-21 15:05:29 +01:00
Robert Haschke
fdd5ff880b templatize: pullInterface(dir) -> pullInterface<dir>()
Also remove unused pushInterface(dir)
2021-11-21 14:15:59 +01:00
Robert Haschke
a31e52dd53 Propagate status across Connecting gap
Not only propagate updates along solution paths, but also bridge
the gap of a `Connecting` stage.

- If a state becomes enabled, re-enable opposite `ARMED` states as well.
- If a state becomes pruned, also prune opposite states if they don't have alternatives.
- Make sure that we don't run into a recursive update loop by disabling notify() callbacks.
2021-11-21 14:15:59 +01:00
Robert Haschke
3648db43ed Never overwrite ARMED with PRUNED 2021-11-21 14:00:50 +01:00
Robert Haschke
9d37495c0b Recombine both variants of Interface::updatePriority()
As only the InterfaceState* variant is actually called,
we can drop the splitting introduced for performance reasons in
29d1e44c5d
2021-11-21 14:00:50 +01:00
Robert Haschke
ffb8598363 Recursively prune new CONNECT state if there is no enabled opposite
This also requires to drop the assertion in SerialContainer::onNewSolution()
that new solutions will have enabled start+end states (a CONNECT stage's solution might not).
2021-11-21 14:00:50 +01:00
Robert Haschke
ab9af6a0fa Recursively re-enable states when matching an ARMED state 2021-11-21 14:00:50 +01:00
Robert Haschke
3c4ef68dbe Rename Interface::Status FAILED -> ARMED
... to better indicate that such a state can be immediately re-enabled.
2021-11-21 14:00:50 +01:00
Robert Haschke
1dda3d14d8 Switch order of function declarations
... to avoid explicit template initialization
2021-11-20 06:24:19 +01:00
Robert Haschke
52dc494525 Fix test Pruning.NoPruningIfAlternativesExist 2021-11-20 06:24:19 +01:00
Robert Haschke
06ae5ddf9c Fix hasPendingOpposites()
- Switch directions: FORWARD <-> BACKWARD to make the function reusable for status propagation.
- We need to ignore the source state when looking for opposite states of the target state.
  Thus add both, source and target state arguments.
2021-11-20 01:24:29 +01:00
Robert Haschke
29d1e44c5d Rework updatePriority() functions
- Centrally distinguish between have owner() or not in InterfaceState::updatePriority()
- Have a separate updateStatus() method to just update the pruning status
- Split Interface::updatePriority() into a method taking the InterfaceState*
  and one taking an Interface::iterator (for efficiency)
- Early return in container.cpp's updateStatePrios()
2021-11-19 23:03:15 +01:00
Robert Haschke
4170a1c93a Drop unused and misleading Direction enums 2021-11-19 23:03:15 +01:00
Robert Haschke
1ddf7dd3f0 Never remove pending CONNECT pairs
Both, failed and pruned states might get re-enabled later!
This also required rework (simplification) of the sorting function for pending pairs.
2021-11-19 23:03:15 +01:00
Robert Haschke
97c2130404 Improve readability 2021-11-19 21:49:44 +01:00
Robert Haschke
718170ab1e Always skip pruning if there exist alternative enabled solutions 2021-11-19 21:49:44 +01:00
Robert Haschke
c617e3353d Disable failing tests 2021-11-19 21:46:09 +01:00
Robert Haschke
011e4be059 Add more pruning tests 2021-11-19 09:42:30 +01:00
Robert Haschke
66e141db7b Fix printChildrenInterfaces() 2021-11-19 09:42:30 +01:00
v4hn
b675876d3b add debug logs to pruning code 2021-11-16 22:52:06 +01:00
JafarAbdi
2d775ffe28 GeneratorMockup: Add solutions_per_compute argument 2021-11-16 22:51:29 +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
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
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
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
Robert Haschke
61496fecd3 Simplify tests by introducing EXPECT_COSTS() 2021-09-20 14:50:03 +02:00
v4hn
61399d6d23 fixup & extend fallback tests 2021-09-20 14:49:01 +02:00
v4hn
a582fe8297 add another non-trivial test for a reliable fallbacks container 2021-09-20 14:42:55 +02:00
v4hn
b61e045f2c add a test for generator-fallbacks 2021-09-20 14:42:48 +02:00
v4hn
f75a498f55 fallbacks: add a test to use fallbacks *per state*
The current implementation will not fall back for each state
independently, but is meant to stay with the first child producing
a solution. For propagators, this is problematic though
as the picked child depends on the (arbitrary) first received state.

Instead, fallbacks should pass each state to each child separately
until one produces a solution for it (or all are exhausted).
2021-09-20 14:42:42 +02:00
v4hn
2bee9d5f66 rearrange fallback test fixtures 2021-09-20 14:42:35 +02:00
v4hn
5d9efc3b2b fallback tests: use fixture 2021-09-20 14:42:26 +02:00
v4hn
001eadaf3d move fallback to separate test 2021-09-20 14:42:20 +02:00
v4hn
14380c5552 add another test to cover both cases for failing children 2021-09-20 14:42:14 +02:00
v4hn
3244ac92b1 disable ConnectStageInsideFallbacks
This should work, but will require more changes.
2021-09-20 14:42:08 +02:00
Robert Haschke
c6cdd1a970 Fix test Fallback.ActiveChildReset 2021-09-20 14:41:51 +02:00
j-kuehn
f6eadb3fc4 add unit tests for Fallbacks container 2021-09-20 14:41:42 +02:00
Robert Haschke
d0ab3c3703
Improve readability of internal-external bimap using tags (#293) 2021-09-16 13:52:34 +02:00
v4hn
06b3df9c30 split assert
so that it becomes obvious which condition triggered it.
2021-09-01 14:53:52 +02:00
v4hn
fa8d21741c print debug message in runCompute
so that it prints from all containers
2021-09-01 14:53:14 +02:00
v4hn
f51f6eb982 address interface changes for object poses in MoveIt
Also include a check for the new object pose field in `Connecting::compatible()`.
2021-08-23 00:33:37 +02:00
v4hn
0273aec83e remove dirty MOVEIT_MASTER-check
... in favor of checking version numbers.
Checking for one header was used for multiple independent things.
In theory we could do exact feature testing instead of using the next release number,
but in practice nobody cares about the individual commits between older releases.
2021-08-23 00:06:42 +02:00
v4hn
6b4eae9d51 ComputeIK: spawn failures with correct states
Otherwise all failures look correct in the introspection.

That was a stupid oversight at some point.
2021-08-21 22:15:21 +02:00
v4hn
ff86217f9a do not modify scene in isTargetPoseColliding
Especially, do not create a custom scene at all.
The method only affects a RobotState.

Fixes https://github.com/ros-planning/moveit_task_constructor/issues/209
2021-08-21 22:15:12 +02:00
v4hn
74d33c4ec0 Merge different mockup implementations
Co-authored-by: Jascha Kühn <57101356+j-kuehn@users.noreply.github.com>
2021-07-07 14:25:42 +02:00
v4hn
4b1f240c21 test_container: correctly initialize robot_model
The local mocks do not care, but leaving a dangling nullptr
leads to segfaults with refactoring/new tests. :-)
2021-07-07 14:25:42 +02:00
v4hn
062c127b86 better API comments for StageCallback 2021-06-15 09:49:05 +02:00
Captain Yoshi
64b65e4eca
GenerateGraspPose: Handle RobotState.msg as pregrasp property (#275)
Co-authored-by: v4hn <me@v4hn.de>
2021-06-15 07:51:18 +02:00
Robert Haschke
ced362f5f6 Merge branch 'master' into wip-python-api 2021-06-11 09:57:55 +02:00
Robert Haschke
244c999514 Skip some python tests on incompatible pybind11 versions
If MoveIt and MTC use incompatible versions of pybind11, the tests
will fail because MoveIt objects like RobotModel or PlanningScene
cannot be passed to MTC objects and vice versa.
2021-06-10 19:31:05 +02:00
Robert Haschke
e3ee75dc16 Fix utf8 encoding 2021-06-10 12:06:22 +02:00
v4hn
0e40b464ec support TYPED_TEST_SUITE
Get rid of deprecation warning if new variant exists.
2021-06-04 14:19:52 +02:00
v4hn
1f51124ae5 add missing virtual destructor to CostTerm
Correctly pointed out by the clang-tidy CI.
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
Michael Görner
84d668b0d5
Split ClassLoader plugin code from main libraries (#271)
to work around https://github.com/ros/class_loader/issues/131
2021-05-29 00:38:02 +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
Robert Haschke
f070247a25 Fix compiler warnings 2021-05-27 22:24:29 +02:00
j-kuehn
67fe4faa30 allow to consider specific joints only in cost terms 2021-05-25 15:47:18 +02:00
Robert Haschke
81fae53009 PyMonitoringGenerator 2021-05-21 12:20:54 +02:00
Robert Haschke
9f7139f376 Fix names of trampoline classes
Using template names T is not a good idea, because this name is used
verbatim for some error reporting, resulting e.g. in:
Tried to call pure virtual function "T::canCompute"
2021-05-21 11:40:12 +02:00
Robert Haschke
0967aa808b Update pybind11 submodule 2021-05-21 01:55:39 +02:00
Robert Haschke
1e7fe85573 smart_holder: conservative mode
... to become compatible with classical pybind11 modules, e.g. the MoveIt packages
2021-05-21 01:55:39 +02:00
Robert Haschke
7d0b6da04f Generator::spawn() 2021-05-21 01:55:39 +02:00
Robert Haschke
a03ea5a4a9 Use pybind11's smart_holder branch 2021-05-20 03:56:47 +02:00
Robert Haschke
defe144b55 Use py:overload_cast<>() 2021-05-20 03:52:32 +02:00
Robert Haschke
fc0abe9dd9 Merge branch 'master' into wip-python-api 2021-05-20 03:49:52 +02:00
Robert Haschke
3c286b2226 Augment license/disclaimer 2021-05-19 23:04:44 +02:00
Robert Haschke
f006ff7a37 Allow casting of PoseStamped from string 2021-05-19 23:04:44 +02:00
Robert Haschke
84ccd679d9 Fix compiler warnings
unused parameters and functions
2021-05-19 21:54:48 +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
j-kuehn
d71c645d83
add visualization for Point goals (#264)
- move visualization from `getPoseGoal` to `compute`
- create target frame from `target_eigen` to allow visualization from Pose and Point goals
2021-05-11 10:53:15 +02:00
j-kuehn
ba0b4f6146
Improve arrow visualization for MoveRelative stage (#255)
Implement visualization as red-green arrow
* overload makeArrow to allow creation with points
* create new function for visualization
* if no plan is found, construct arrow from green cylinder and red arrow
* adjust arrow construction for backward propagators
2021-05-03 21:29:05 +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
Robert Haschke
a204d94a19 Configure namespace package 2021-03-31 00:19:51 +02:00
Robert Haschke
5b4ca1d21a Rename wrapper -> bindings 2021-03-31 00:19:51 +02:00
Robert Haschke
045b358c30 rosdoc_lite configuration 2021-03-31 00:19:51 +02:00
Robert Haschke
3cf92442fb Simplify generation of pybind11 modules
* Install module libs into CATKIN_GLOBAL_PYTHON_DESTINATION (assuming unique names).
  This avoids the need to link them into the source space, because they are found also from devel space.
* Use pybind11's def_submodule() to create the `core` and `stages` submodules,
  everything linked into the same lib
2021-03-31 00:19:51 +02:00
Robert Haschke
d9b7aa37a3 clang-format python wrapping code 2021-03-31 00:19:51 +02:00
cpetersmeier
14e06659d5 Towards inherited classes in Python 2021-03-31 00:19:51 +02:00
v4hn
472a78800d implement bimap for internal_external state map
Get hashing for inverted lookups, but incur
structural overhead.

Whether this is worth it depends on the number of mapped interface states
and the number of pruning/reactivation requests.
2021-03-30 15:19:49 +02:00
Robert Haschke
0e3ec0b6ec fix constness 2021-03-30 15:19:49 +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
Michael Görner
36d9d3e8c0
Rework PipelinePlanner creation (#249)
- Moved Task::createPlanner into PipelinePlanner::create
- Handle mutiple planner pipeline configs as introduced in https://github.com/ros-planning/moveit/pull/2127
2021-03-29 12:00:28 +02:00
Robert Haschke
4ee71881ae
Rework Pruning (#221) 2021-03-23 13:04:38 +01:00
Michael Görner
f2958a4a0b
Explicitly instantiate PropagatingEitherWay::send<> templates (#246)
Otherwise compiler was optimizing (inlining) them away.
2021-03-23 11:37:30 +01:00
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
3b6c13e26b only run propagators on enabled solutions
resolve simple pruning test
2021-03-22 14:42:23 +01: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
be1c8292b5 add more (commented-out) debug statements
Also print generator symbol for completeness.
2021-03-22 14:42:23 +01:00
v4hn
e33962364d resolve PruningMultiForward test
Every InterfaceState along the partial solution has to be disabled
for pruning, not just the ends that are currently relevant.
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
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
9466517f72 operator<< for Interface + Priority
Co-authored by v4hn
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
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
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
cpetersmeier
6ab50fcc40 PythonWrapper: Use collective includes 2021-03-19 18:39:47 +01:00
Robert Haschke
1f5c684909 Simplify wrapper code 2021-03-19 18:39:47 +01:00
Robert Haschke
d7d54d828f Access to container's children 2021-03-19 18:39:47 +01:00
Robert Haschke
fd25a0626a auto-format python code with black 2021-03-19 18:39:44 +01:00
Robert Haschke
10f6b7a58e MoveTo/MoveRel: reduce code duplication
... using templated versions for computeForward + computeBackward
as they essentially perform the same operations.
2021-03-19 18:22:04 +01:00
Robert Haschke
dfffd77eee MoveTo/MoveRel: Report errors via solution comments instead of ROS_WARN 2021-03-19 18:22:04 +01:00
Robert Haschke
5de9ce7203 Perform clang-format check via github action from pre-commit.com 2021-03-15 14:29:19 +01:00
Robert Haschke
0b63c590c5 Replace MoveIt! -> MoveIt 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
v4hn
f4fb8c6f71 fix computeCost interface (again)
Fixes the missing creator pointer introduced in a6a23d3775
2021-03-05 14:38:23 +01:00
v4hn
629d266fbc remove leftover joint counting
Not used anywhere
2021-03-05 14:38:14 +01:00
v4hn
f7a94956c3 output stream formatting for InterfaceState::Priority 2021-03-05 14:38:06 +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
v4hn
4386d0cc60 remove roscpp as an export dependency for the core package
We do use ROS in the background. But there is no need for a public export dependency on it.

This patch also resolves the following catkin_lint issue:

moveit_task_constructor_core: CMakeLists.txt(17): error: package 'roscpp' must be in CATKIN_DEPENDS in catkin_package()
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
Jafar Abdi
8a913d8bf3
getTaskId(): Replace '-' in hostname with '_' (#223) 2020-12-15 20:39:01 +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
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
576a1aa48d Remove default constructor for Priority
It's better to explicitly state the initial value.
2020-12-06 04:11:35 +01:00
Robert Haschke
56ad37d80e Provide move constructor for InterfaceState
The code was tuned to move InterfaceStates around, but there wasn't the matching constructor!
2020-12-06 04:11:35 +01:00
Robert Haschke
bc25c5e3c1 Modernize: require cmake version 3.1.3 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
a9cd4e3281 Connect: Decouple subsolutions of SolutionSequence from external interface states 2020-12-06 04:09:59 +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
11647fd3e5 Connect: mark failures in sub solutions of a sequence 2020-12-06 04:09:59 +01:00
Robert Haschke
c73c0c3555 Interface::updatePriority(): only allow decrease 2020-12-06 04:06:08 +01:00