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
|
||||
{
|
||||
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) {}
|
||||
explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped, double cost)
|
||||
explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped, double cost)
|
||||
: SolutionBase(creator, cost), wrapped_(wrapped) {}
|
||||
explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped)
|
||||
explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped)
|
||||
: WrappedSolution(creator, wrapped, wrapped->cost()) {}
|
||||
void fillMessage(moveit_task_constructor_msgs::Solution& solution,
|
||||
Introspection* introspection = nullptr) const override;
|
||||
@ -244,7 +244,7 @@ class MergerPrivate : public ParallelContainerBasePrivate
|
||||
|
||||
moveit::core::JointModelGroupPtr jmg_merged_;
|
||||
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
|
||||
std::map<InterfaceState*, ChildSolutionMap> source_state_to_solutions_;
|
||||
|
||||
|
||||
@ -220,8 +220,8 @@ public:
|
||||
const_cast<InterfaceState&>(state).addIncoming(this);
|
||||
}
|
||||
|
||||
inline const StagePrivate* creator() const { return creator_; }
|
||||
void setCreator(StagePrivate* creator);
|
||||
inline const Stage* creator() const { return creator_; }
|
||||
void setCreator(Stage* creator);
|
||||
|
||||
inline double cost() const { return cost_; }
|
||||
void setCost(double cost);
|
||||
@ -243,12 +243,12 @@ public:
|
||||
bool operator<(const SolutionBase& other) const { return this->cost_ < other.cost_; }
|
||||
|
||||
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)) {}
|
||||
|
||||
private:
|
||||
// back-pointer to creating stage, allows to access sub-solutions
|
||||
StagePrivate* creator_;
|
||||
Stage* creator_;
|
||||
// associated cost
|
||||
double cost_;
|
||||
// comment for this solution, e.g. explanation of failure
|
||||
@ -293,7 +293,7 @@ public:
|
||||
using container_type = std::vector<const 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)) {}
|
||||
|
||||
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
|
||||
// However, as the Merging-Connect's solution may be composed of several subsolutions, we need to disregard those
|
||||
size_t len = trace.size();
|
||||
const StagePrivate* prev_creator = nullptr;
|
||||
const Stage* prev_creator = nullptr;
|
||||
for (const auto& s : trace) {
|
||||
if (s->creator() == prev_creator)
|
||||
--len;
|
||||
@ -328,13 +328,13 @@ void updateStateCosts(const SolutionSequence::container_type& partial_solution_p
|
||||
|
||||
void SerialContainer::onNewSolution(const SolutionBase& current) {
|
||||
auto impl = pimpl();
|
||||
const StagePrivate* creator = current.creator();
|
||||
const Stage* creator = current.creator();
|
||||
auto& children = impl->children();
|
||||
|
||||
// find number of stages before and after creator stage
|
||||
size_t num_before = 0, num_after = 0;
|
||||
for (auto it = children.begin(), end = children.end(); it != end; ++it, ++num_before)
|
||||
if ((*it)->pimpl() == creator)
|
||||
if (&(**it) == creator)
|
||||
break;
|
||||
assert(num_before < children.size()); // creator should be one of our children
|
||||
num_after = children.size() - 1 - num_before;
|
||||
@ -370,7 +370,7 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
|
||||
// insert outgoing solutions in normal order
|
||||
solution.insert(solution.end(), out.first.begin(), out.first.end());
|
||||
// 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) {
|
||||
// update state priorities along the whole partial solution path
|
||||
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) {
|
||||
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());
|
||||
}
|
||||
|
||||
|
||||
@ -125,7 +125,7 @@ void StagePrivate::validateConnectivity() const {
|
||||
}
|
||||
|
||||
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
|
||||
solution->setCreator(this);
|
||||
solution->setCreator(me());
|
||||
if (introspection_)
|
||||
introspection_->registerSolution(*solution);
|
||||
|
||||
|
||||
@ -195,7 +195,7 @@ Connect::makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConst
|
||||
planning_scene::PlanningSceneConstPtr end_ps = *++scene_it;
|
||||
|
||||
auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(sub));
|
||||
inserted->setCreator(pimpl_);
|
||||
inserted->setCreator(this);
|
||||
// push back solution pointer
|
||||
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
|
||||
creator_ = creator;
|
||||
}
|
||||
@ -150,7 +150,7 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, In
|
||||
info.cost = this->cost();
|
||||
info.comment = this->comment();
|
||||
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();
|
||||
info.markers.resize(markers.size());
|
||||
|
||||
Loading…
Reference in New Issue
Block a user