mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
compute cost in liftSolution as well
This commit is contained in:
parent
939f0b7256
commit
0e38730fd4
@ -201,10 +201,13 @@ class SubTrajectory;
|
||||
using CostTerm = std::function<double(const SubTrajectory&, std::string&)>;
|
||||
|
||||
class StagePrivate;
|
||||
class ContainerBasePrivate;
|
||||
/// abstract base class for solutions (primitive and sequences)
|
||||
class SolutionBase
|
||||
{
|
||||
friend StagePrivate; // for set[Start|End]StateUnsafe
|
||||
friend ContainerBasePrivate;
|
||||
|
||||
public:
|
||||
virtual ~SolutionBase() = default;
|
||||
|
||||
|
||||
@ -120,6 +120,10 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa
|
||||
|
||||
void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
|
||||
const InterfaceState* internal_to) {
|
||||
solution->setCreator(me());
|
||||
|
||||
computeCost(*internal_from, *internal_to, *solution);
|
||||
|
||||
if (!storeSolution(solution))
|
||||
return;
|
||||
|
||||
@ -139,8 +143,8 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I
|
||||
InterfaceState* external_to = find_or_create_external(internal_to, created_to);
|
||||
|
||||
// connect solution to start/end state
|
||||
solution->setStartState(*external_from);
|
||||
solution->setEndState(*external_to);
|
||||
solution->setStartStateUnsafe(*external_from);
|
||||
solution->setEndStateUnsafe(*external_to);
|
||||
|
||||
// spawn created states in external interfaces
|
||||
if (created_from)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user