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
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
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
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
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
Robert Haschke
08dc19f7b3
Fix cost calculation
...
The default CostTerm should rely on precomputed costs. Only, if a CostTerm is explicitly defined
for a particular stage, it should modify costs.
2020-12-06 04:06:08 +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
5fd98ce307
SerialContainer tests
2020-12-06 04:06:08 +01:00
Robert Haschke
fe189cd692
Test Interface::updatePriority()
2020-12-06 04:06:08 +01:00
Robert Haschke
bfa2a6070d
MoveTo: Publish failed planning attempts
2020-10-30 00:44:51 +01:00
Robert Haschke
ae843a784e
Merge PR #214 (fix various issues) and #215 (rework rviz introspection)
2020-10-24 04:45:53 +02:00
Robert Haschke
cf5031e89d
Create task-specific ServiceClients to fetch solutions
...
This finally allows to have multiple tasks publishing in the same namespace.
2020-10-24 04:43:30 +02:00
Robert Haschke
b91d87c6eb
IntroSpection: indicateReset() on disable
2020-10-24 04:43:30 +02:00
Robert Haschke
8b1c66c4b7
Allow naming a Task
...
which just names the top-level container
2020-10-24 04:43:30 +02:00
Robert Haschke
c471879b08
Rename process_id -> task_id
2020-10-24 04:43:30 +02:00
Robert Haschke
b071a059f9
Rename TaskPrivate::id -> TaskPrivate::ns
...
TaskPrivate's id_ actually served as a namespace parameter.
2020-10-24 04:43:30 +02:00
Robert Haschke
3a7a4eb953
Connect: Fix segfault when accessing solutions from multiple plan() calls
...
Connect::init() was resetting its dynamically created JMG merged_jmg_,
thus invalidating all previous solutions. Only reset the JMG in reset().
2020-10-24 03:23:19 +02:00
Robert Haschke
cb0f20cad7
Improve task unittests
2020-10-24 03:22:46 +02:00
Jafar Abdi
be270cb574
Task: Add ability to set timeout ( #213 )
...
Task::setTimeout will allow setting an overall timeout.
2020-10-24 03:22:05 +02: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
Robert Haschke
198f8f5389
clearer naming of cost variables
2020-09-21 22:52:19 +02:00