Commit Graph

563 Commits

Author SHA1 Message Date
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
v4hn
64e2440c53 simplify LinkMotion computation
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
1689f6cd95 introduce new trivial stage Forward
This can be used together with a custom CostTerm to modify costs of a solution.
2020-09-21 22:31:52 +02:00
v4hn
0e38730fd4 compute cost in liftSolution as well 2020-09-21 22:31:52 +02:00
v4hn
939f0b7256 setCostTermShort(nullptr) must not set a valid function 2020-09-21 22:31:52 +02:00
v4hn
a0975e3afc avoid ambiguity of setCostTerm(nullptr) 2020-09-21 22:31:52 +02:00
v4hn
d68ab383b6 allow setCostTerm for containers
implement visitor pattern for cost computation on solutions

- compute cost terms for solution subtrees instead of only for SubTrajectory

- allows users to set cost terms for containers
2020-09-21 22:31:24 +02:00
v4hn
039b0a2c27 move more overhead to computeCost 2020-09-21 22:31:23 +02:00
v4hn
e53d943b75 remove CostTransform
If required, this can hopefully be implemented through Wrapper stages soon.
2020-09-21 22:31:23 +02:00
v4hn
53c0964618 cost computation: provide valid interface states for Solution
The CostTerm's should get only a single solution that is well-setup
with its InterfaceStates. That's impossible though because these
states are stored in different places depending on the cost.

To avoid this, set stub states for cost computation
and change them to the real states later on.

This is motivated by the Clearance cost which can act on an InterfaceState only.
2020-09-21 22:31:23 +02:00
v4hn
bbf7d415f6 make cost operators const 2020-09-21 22:28:36 +02:00
v4hn
1f8feaaf0a refactor addCost->computeCost 2020-09-21 22:26:25 +02:00
v4hn
d2bd0a0037 generalize Clearance cost
- can now be used to estimate cost for either interface state or the trajectory
- Introduced Interface::Direction NONE as a way of pointing to the trajectory in contrast to START or END
2020-09-21 22:26:25 +02:00
v4hn
7a00ac646e add missing license 2020-09-21 22:24:44 +02:00
v4hn
14c885621f remove Cost suffix for elements in mtc::cost:: 2020-09-21 22:24:44 +02:00
v4hn
9a96a7e4f6 implement LinkMotionCost 2020-09-21 22:24:44 +02:00
v4hn
6dbc742f20 implement correct PathLengthCost
The previous implementation depends on the dynamics limits of the robot,
which might be interesting in some cases, but shouldn't be a default anywhere.
2020-09-21 22:24:44 +02:00
v4hn
a92753dafc add cumulative distance to ClearanceCost 2020-09-21 22:24:44 +02:00
v4hn
18ed51d7f3 turn Clearance cost into parameterized struct
There is a lot of variations for how to compute clearance.
2020-09-21 22:24:44 +02:00
v4hn
cf0e4e3b01 establish struct cost pattern
for costs with multiple parameters, this option-style is more useful
than verbose getter/setter. There is nothing to hide here.
2020-09-21 22:24:07 +02:00
v4hn
f9ba302728 enable costs to (optionally) return a comment
With more complicated costs (such as the ClearanceCost), it's useful to get comments.
2020-09-21 22:22:37 +02:00
v4hn
a15204b40b allow wrappers to use CostTerm 2020-09-21 22:22:37 +02:00
v4hn
56268cb6cc first attempts on ClearanceCost 2020-09-21 22:22:37 +02:00
v4hn
9060404037 improve comments 2020-09-21 22:22:37 +02:00
v4hn
f3fec26f5a add CostTransform
CostTerms only apply to primitive solutions and generalizing them
to Containers would make them quite unintuitive (and adds overhead).

Instead CostTransform can be used in any container to scale, crop, square
the cost of the solutions.

I initially thought about adding scaling factors, but then again,
other transforms are of interest just as well.
2020-09-21 22:22:37 +02:00
v4hn
f79625d1bd fully restrict CostTerms to primitive solutions
simplify implementations.
2020-09-21 22:22:37 +02:00
v4hn
ffc62c2d11 add explicit default CostTerms to most basic stages
Simplifies code and makes the costs of each stage explicit.
2020-09-21 22:22:37 +02:00
v4hn
9eae554d89 provide Constant and PathLength cost terms 2020-09-21 22:22:37 +02:00
v4hn
9337743de2 add CostTerm interface
basic stages can now be configured with arbitrary cost terms.
2020-09-21 18:02:49 +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
v4hn
c510c6b5b6 WrappedSolution: expose child solution
SolutionSequence allows to access children already,
but WrappedSolution blocked access to the full hierarchy.
2020-08-21 14:41:22 +02:00
v4hn
4a6c389aee expose WrappedSolution together with other Solution types
Moved out of private header.

Users can access these objects, so they should know about the type.
2020-08-21 14:41:22 +02:00
v4hn
7609f0b1e2 Solution: export public creator
It's a public interface, returning private pointer is not neat.
2020-08-21 14:41:22 +02:00
v4hn
3aa0cfc1a2 InterfaceState: provide constructor with initial Priority
most useful to prepare a valid InterfaceState in tests
2020-08-21 14:41:22 +02:00
v4hn
6e0e323a58 PredicateFilter: actually use declared property 2020-08-20 15:29:45 +02:00
v4hn
310ca3ef15 MoveTo: support map<string,double> joint target
There was a corresponding setGoal method already, but no implementation...
2020-08-20 15:20:29 +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
62813f33f7 fix c&p error in documentation 2020-08-11 20:43:34 +02:00
v4hn
62d894199f expose Stage API to get the matching introspection ID
This was implicitly requested in #192 .
2020-08-11 20:43:34 +02:00
v4hn
6bebe3f3ed do not mix pimpl() and impl for no reason 2020-08-11 20:43:34 +02:00
v4hn
03c7f55fcc fix unused-parameter warning 2020-08-11 20:43:34 +02:00
v4hn
e861d311e0 fix comment typo 2020-08-11 20:43:34 +02:00
v4hn
d031f1b1e3 send a single reset message when introspection is enabled
My previous patches accidentally disabled *all* (3) reset messages,
instead of keeping exactly one. This patch sends exactly one empty description
(reset) message every time an introspection instance is constructed for a task.

Notice the additional increase of the description queue_size from 1 to 2 to avoid
directly dropping the reset message in favor of the new description likely to be send
shortly afterwards.
2020-07-22 12:57:34 +02:00
v4hn
7ba73306ca introspection *can* be disabled
Otherwise a new task will always setup the publisher,
even if introspection is disabled afterwards.

It is a good idea to keep introspection on, but there should be a way to initialize the C++ classes without ROS communication.
2020-07-22 12:57:34 +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
21f3d95128 fix autocomplete typo 2020-07-22 12:57:34 +02:00
Robert Haschke
0902d432ce Improve code readability 2020-07-01 22:13:21 +02:00
Robert Haschke
ee7ebb3b96
SolutionMsg: always fill start_scene (#175)
So far, the start_scene field of a SolutionMsg was only filled by  Introspection::fillSolution(),
but not yet by Task::execute().

Addendum(v4hn): The previous approach was actually reasonable too (although the scene should have been marked as `is_diff`) for solutions sent for execution, but keeping the full start_scene around can facilitate debugging from recorded data.
2020-06-18 11:07:05 +02:00
Robert Haschke
1610f5ab1d MoveRelative: provide failure message 2020-06-13 22:48:48 +02:00
v4hn
15707673ed clang-tidy: performance-*
I NOLINTed the noexcept move constructor for Task for now because
the constructor *can* indeed throw exceptions.
2020-06-11 11:03:50 +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
fa041ed2e6 clang-format: readability-container-size-empty 2020-06-11 11:03:50 +02:00
v4hn
b762079478 clang-tidy: readability-named-parameters 2020-06-11 11:03:50 +02:00
v4hn
5da2df4c61 clang-tidy: modernize-use-nullptr 2020-06-11 11:03:50 +02:00
v4hn
3fe4436ad7 clang-tidy: modernize-use-override 2020-06-11 11:03:50 +02:00
v4hn
36166348bc clang-tidy: use using over typedef
$ run-clang-tidy.py -header-filter='.*' -checks='modernize-use-using' -fix

add .clang-tidy file
2020-06-11 11:03:50 +02:00
v4hn
c662311a78 grant access to subsolutions of a sequence
SubTrajectory allows to access the trajectory,
but for SolutionSequence is was not needed until now.
2020-06-11 11:03:50 +02:00
v4hn
98cac4fd7c add informative comment 2020-06-11 11:03:50 +02:00
v4hn
ebf7b751d2 do not reset on plan
users can reset explicitly as needed.
Reset sends ROS messages and prevents { plan(1); plan(1) } style usage.
2020-06-11 11:03:50 +02:00
v4hn
b9839a3f22 check for valid robot model
Also, no need to reset() unless the model is different and not the first.
2020-06-11 11:03:50 +02:00
v4hn
b27939cb27 fix typo 2020-06-11 11:03:50 +02:00
v4hn
d224097f3f extend comment on unintuitive setter 2020-06-11 11:03:50 +02:00
Jafar Abdi
b3b215a4f7
Add ability to add/remove objects to/from planning scene (#165) 2020-05-05 18:28:54 +02:00
Aris Synodinos
2d99017c17
Fix for GCC5 (Ubuntu 16.04 / Kinetic) call to non-constexpr function (#163)
Co-authored-by: Aris Synodinos <aris.synodinos@unibap.com>
2020-04-22 11:48:08 +02:00
v4hn
72af8bc94b add convenience includes 2020-04-18 21:04:57 +02:00