mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Solution: export public creator
It's a public interface, returning private pointer is not neat.
This commit is contained in:
parent
3aa0cfc1a2
commit
7609f0b1e2
@ -195,11 +195,11 @@ PIMPL_FUNCTIONS(SerialContainer)
|
|||||||
class WrappedSolution : public SolutionBase
|
class WrappedSolution : public SolutionBase
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped, double cost, std::string comment)
|
explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped, double cost, std::string comment)
|
||||||
: SolutionBase(creator, cost, std::move(comment)), wrapped_(wrapped) {}
|
: SolutionBase(creator, cost, std::move(comment)), wrapped_(wrapped) {}
|
||||||
explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped, double cost)
|
explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped, double cost)
|
||||||
: SolutionBase(creator, cost), wrapped_(wrapped) {}
|
: SolutionBase(creator, cost), wrapped_(wrapped) {}
|
||||||
explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped)
|
explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped)
|
||||||
: WrappedSolution(creator, wrapped, wrapped->cost()) {}
|
: WrappedSolution(creator, wrapped, wrapped->cost()) {}
|
||||||
void fillMessage(moveit_task_constructor_msgs::Solution& solution,
|
void fillMessage(moveit_task_constructor_msgs::Solution& solution,
|
||||||
Introspection* introspection = nullptr) const override;
|
Introspection* introspection = nullptr) const override;
|
||||||
@ -244,7 +244,7 @@ class MergerPrivate : public ParallelContainerBasePrivate
|
|||||||
|
|
||||||
moveit::core::JointModelGroupPtr jmg_merged_;
|
moveit::core::JointModelGroupPtr jmg_merged_;
|
||||||
using ChildSolutionList = std::vector<const SubTrajectory*>;
|
using ChildSolutionList = std::vector<const SubTrajectory*>;
|
||||||
using ChildSolutionMap = std::map<const StagePrivate*, ChildSolutionList>;
|
using ChildSolutionMap = std::map<const Stage*, ChildSolutionList>;
|
||||||
// map from external source state (iterator) to all corresponding children's solutions
|
// map from external source state (iterator) to all corresponding children's solutions
|
||||||
std::map<InterfaceState*, ChildSolutionMap> source_state_to_solutions_;
|
std::map<InterfaceState*, ChildSolutionMap> source_state_to_solutions_;
|
||||||
|
|
||||||
|
|||||||
@ -220,8 +220,8 @@ public:
|
|||||||
const_cast<InterfaceState&>(state).addIncoming(this);
|
const_cast<InterfaceState&>(state).addIncoming(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
inline const StagePrivate* creator() const { return creator_; }
|
inline const Stage* creator() const { return creator_; }
|
||||||
void setCreator(StagePrivate* creator);
|
void setCreator(Stage* creator);
|
||||||
|
|
||||||
inline double cost() const { return cost_; }
|
inline double cost() const { return cost_; }
|
||||||
void setCost(double cost);
|
void setCost(double cost);
|
||||||
@ -243,12 +243,12 @@ public:
|
|||||||
bool operator<(const SolutionBase& other) const { return this->cost_ < other.cost_; }
|
bool operator<(const SolutionBase& other) const { return this->cost_ < other.cost_; }
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
SolutionBase(StagePrivate* creator = nullptr, double cost = 0.0, std::string comment = "")
|
SolutionBase(Stage* creator = nullptr, double cost = 0.0, std::string comment = "")
|
||||||
: creator_(creator), cost_(cost), comment_(std::move(comment)) {}
|
: creator_(creator), cost_(cost), comment_(std::move(comment)) {}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// back-pointer to creating stage, allows to access sub-solutions
|
// back-pointer to creating stage, allows to access sub-solutions
|
||||||
StagePrivate* creator_;
|
Stage* creator_;
|
||||||
// associated cost
|
// associated cost
|
||||||
double cost_;
|
double cost_;
|
||||||
// comment for this solution, e.g. explanation of failure
|
// comment for this solution, e.g. explanation of failure
|
||||||
@ -293,7 +293,7 @@ public:
|
|||||||
using container_type = std::vector<const SolutionBase*>;
|
using container_type = std::vector<const SolutionBase*>;
|
||||||
|
|
||||||
explicit SolutionSequence() : SolutionBase() {}
|
explicit SolutionSequence() : SolutionBase() {}
|
||||||
SolutionSequence(container_type&& subsolutions, double cost = 0.0, StagePrivate* creator = nullptr)
|
SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr)
|
||||||
: SolutionBase(creator, cost), subsolutions_(std::move(subsolutions)) {}
|
: SolutionBase(creator, cost), subsolutions_(std::move(subsolutions)) {}
|
||||||
|
|
||||||
void push_back(const SolutionBase& solution);
|
void push_back(const SolutionBase& solution);
|
||||||
|
|||||||
@ -292,7 +292,7 @@ struct SolutionCollector
|
|||||||
// Traced path should not extend past container boundaries, i.e. trace.size() <= max_depth
|
// Traced path should not extend past container boundaries, i.e. trace.size() <= max_depth
|
||||||
// However, as the Merging-Connect's solution may be composed of several subsolutions, we need to disregard those
|
// However, as the Merging-Connect's solution may be composed of several subsolutions, we need to disregard those
|
||||||
size_t len = trace.size();
|
size_t len = trace.size();
|
||||||
const StagePrivate* prev_creator = nullptr;
|
const Stage* prev_creator = nullptr;
|
||||||
for (const auto& s : trace) {
|
for (const auto& s : trace) {
|
||||||
if (s->creator() == prev_creator)
|
if (s->creator() == prev_creator)
|
||||||
--len;
|
--len;
|
||||||
@ -328,13 +328,13 @@ void updateStateCosts(const SolutionSequence::container_type& partial_solution_p
|
|||||||
|
|
||||||
void SerialContainer::onNewSolution(const SolutionBase& current) {
|
void SerialContainer::onNewSolution(const SolutionBase& current) {
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
const StagePrivate* creator = current.creator();
|
const Stage* creator = current.creator();
|
||||||
auto& children = impl->children();
|
auto& children = impl->children();
|
||||||
|
|
||||||
// find number of stages before and after creator stage
|
// find number of stages before and after creator stage
|
||||||
size_t num_before = 0, num_after = 0;
|
size_t num_before = 0, num_after = 0;
|
||||||
for (auto it = children.begin(), end = children.end(); it != end; ++it, ++num_before)
|
for (auto it = children.begin(), end = children.end(); it != end; ++it, ++num_before)
|
||||||
if ((*it)->pimpl() == creator)
|
if (&(**it) == creator)
|
||||||
break;
|
break;
|
||||||
assert(num_before < children.size()); // creator should be one of our children
|
assert(num_before < children.size()); // creator should be one of our children
|
||||||
num_after = children.size() - 1 - num_before;
|
num_after = children.size() - 1 - num_before;
|
||||||
@ -370,7 +370,7 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
|
|||||||
// insert outgoing solutions in normal order
|
// insert outgoing solutions in normal order
|
||||||
solution.insert(solution.end(), out.first.begin(), out.first.end());
|
solution.insert(solution.end(), out.first.begin(), out.first.end());
|
||||||
// store solution in sorted list
|
// store solution in sorted list
|
||||||
sorted.insert(std::make_shared<SolutionSequence>(std::move(solution), prio.cost(), impl));
|
sorted.insert(std::make_shared<SolutionSequence>(std::move(solution), prio.cost(), this));
|
||||||
} else if (prio.depth() > 1) {
|
} else if (prio.depth() > 1) {
|
||||||
// update state priorities along the whole partial solution path
|
// update state priorities along the whole partial solution path
|
||||||
updateStateCosts(in.first, prio);
|
updateStateCosts(in.first, prio);
|
||||||
@ -650,7 +650,7 @@ ParallelContainerBase::ParallelContainerBase(const std::string& name)
|
|||||||
|
|
||||||
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
|
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
impl->liftSolution(std::make_shared<WrappedSolution>(impl, &solution, cost, std::move(comment)), solution.start(),
|
impl->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)), solution.start(),
|
||||||
solution.end());
|
solution.end());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -125,7 +125,7 @@ void StagePrivate::validateConnectivity() const {
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
|
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
|
||||||
solution->setCreator(this);
|
solution->setCreator(me());
|
||||||
if (introspection_)
|
if (introspection_)
|
||||||
introspection_->registerSolution(*solution);
|
introspection_->registerSolution(*solution);
|
||||||
|
|
||||||
|
|||||||
@ -195,7 +195,7 @@ Connect::makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConst
|
|||||||
planning_scene::PlanningSceneConstPtr end_ps = *++scene_it;
|
planning_scene::PlanningSceneConstPtr end_ps = *++scene_it;
|
||||||
|
|
||||||
auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(sub));
|
auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(sub));
|
||||||
inserted->setCreator(pimpl_);
|
inserted->setCreator(this);
|
||||||
// push back solution pointer
|
// push back solution pointer
|
||||||
sub_solutions.push_back(&*inserted);
|
sub_solutions.push_back(&*inserted);
|
||||||
|
|
||||||
|
|||||||
@ -136,7 +136,7 @@ void Interface::updatePriority(InterfaceState* state, const InterfaceState::Prio
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void SolutionBase::setCreator(StagePrivate* creator) {
|
void SolutionBase::setCreator(Stage* creator) {
|
||||||
assert(creator_ == nullptr || creator_ == creator); // creator must only set once
|
assert(creator_ == nullptr || creator_ == creator); // creator must only set once
|
||||||
creator_ = creator;
|
creator_ = creator;
|
||||||
}
|
}
|
||||||
@ -150,7 +150,7 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, In
|
|||||||
info.cost = this->cost();
|
info.cost = this->cost();
|
||||||
info.comment = this->comment();
|
info.comment = this->comment();
|
||||||
const Introspection* ci = introspection;
|
const Introspection* ci = introspection;
|
||||||
info.stage_id = ci ? ci->stageId(this->creator()->me()) : 0;
|
info.stage_id = ci ? ci->stageId(this->creator()) : 0;
|
||||||
|
|
||||||
const auto& markers = this->markers();
|
const auto& markers = this->markers();
|
||||||
info.markers.resize(markers.size());
|
info.markers.resize(markers.size());
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user