Commit Graph

534 Commits

Author SHA1 Message Date
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
v4hn
04a8afb16c melodic compatibility 2020-09-21 22:52:19 +02:00
v4hn
5d0fd57754 initialize default CostTerm in constructor
It seems to be too fragile to rely on `init` being called.
On the other hand we should really enforce this anyway. -.-
2020-09-21 22:52:19 +02:00
Robert Haschke
687fcb6a6d Update comments to solution's computeCost() methods 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
cce7380f9d Clearance: provide better interface for different modes
Interface::Direction should always designate a valid direction.
2020-09-21 22:52:19 +02:00
v4hn
f63d5b6892 Make transform in cost::Clearance configurable
In theory this can be done with a PassThrough with a modifying transform,
but this specific mapping is intrinsic to the definition of Clearance as a cost.
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