Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Rework execution #459

Draft
wants to merge 8 commits into
base: master
Choose a base branch
from
Draft
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Add SolutionBase::visitSubTrajectories()
rhaschke committed Mar 10, 2023
commit fda0f2f92e6a8baab53a0d6db0ac6f13a27cea4d
14 changes: 11 additions & 3 deletions core/include/moveit/task_constructor/storage.h
Original file line number Diff line number Diff line change
@@ -265,6 +265,11 @@ class CostTerm;
class StagePrivate;
class ContainerBasePrivate;
struct TmpSolutionContext;

MOVEIT_CLASS_FORWARD(SolutionBase);
MOVEIT_CLASS_FORWARD(SubTrajectory);
MOVEIT_CLASS_FORWARD(SolutionSequence);

/// abstract base class for solutions (primitive and sequences)
class SolutionBase
{
@@ -311,6 +316,8 @@ class SolutionBase
auto& markers() { return markers_; }
const auto& markers() const { return markers_; }

virtual void visitSubTrajectories(const std::function<void(const SubTrajectory&)>& f) const = 0;

/// append this solution to Solution msg
virtual void fillMessage(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection = nullptr) const = 0;
@@ -340,7 +347,6 @@ class SolutionBase
const InterfaceState* start_ = nullptr;
const InterfaceState* end_ = nullptr;
};
MOVEIT_CLASS_FORWARD(SolutionBase);

/// SubTrajectory connects interface states of ComputeStages
class SubTrajectory : public SolutionBase
@@ -354,6 +360,7 @@ class SubTrajectory : public SolutionBase
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; }

void visitSubTrajectories(const std::function<void(const SubTrajectory&)>& f) const override;
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override;

double computeCost(const CostTerm& cost, std::string& comment) const override;
@@ -368,7 +375,6 @@ class SubTrajectory : public SolutionBase
// actual trajectory, might be empty
robot_trajectory::RobotTrajectoryConstPtr trajectory_;
};
MOVEIT_CLASS_FORWARD(SubTrajectory);

/** Sequence of individual sub solutions
*
@@ -386,6 +392,7 @@ class SolutionSequence : public SolutionBase

void push_back(const SolutionBase& solution);

void visitSubTrajectories(const std::function<void(const SubTrajectory&)>& f) const override;
/// append all subsolutions to solution
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override;

@@ -400,7 +407,6 @@ class SolutionSequence : public SolutionBase
/// series of sub solutions
container_type subsolutions_;
};
MOVEIT_CLASS_FORWARD(SolutionSequence);

/** Wrap an existing solution
*
@@ -418,6 +424,8 @@ class WrappedSolution : public SolutionBase
: SolutionBase(creator, cost), wrapped_(wrapped) {}
explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped)
: WrappedSolution(creator, wrapped, wrapped->cost()) {}

void visitSubTrajectories(const std::function<void(const SubTrajectory&)>& f) const override;
void fillMessage(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection = nullptr) const override;

13 changes: 13 additions & 0 deletions core/src/storage.cpp
Original file line number Diff line number Diff line change
@@ -216,6 +216,10 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, In
std::copy(markers.begin(), markers.end(), info.markers.begin());
}

void SubTrajectory::visitSubTrajectories(const std::function<void(const SubTrajectory&)>& f) const {
f(*this);
}

void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const {
msg.sub_trajectory.emplace_back();
moveit_task_constructor_msgs::SubTrajectory& t = msg.sub_trajectory.back();
@@ -235,6 +239,11 @@ void SolutionSequence::push_back(const SolutionBase& solution) {
subsolutions_.push_back(&solution);
}

void SolutionSequence::visitSubTrajectories(const std::function<void(const SubTrajectory&)>& f) const {
for (const SolutionBase* s : subsolutions_)
s->visitSubTrajectories(f);
}

void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const {
moveit_task_constructor_msgs::SubSolution sub_msg;
SolutionBase::fillInfo(sub_msg.info, introspection);
@@ -274,6 +283,10 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co
return f(*this, comment);
}

void WrappedSolution::visitSubTrajectories(const std::function<void(const SubTrajectory&)>& f) const {
wrapped_->visitSubTrajectories(f);
}

void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection) const {
wrapped_->fillMessage(solution, introspection);