Solution: export public creator

It's a public interface, returning private pointer is not neat.
This commit is contained in:
v4hn 2020-06-29 23:50:01 +02:00
parent 3aa0cfc1a2
commit 7609f0b1e2
6 changed files with 18 additions and 18 deletions

View File

@ -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_;

View File

@ -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);

View File

@ -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());
}

View File

@ -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);

View File

@ -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);

View File

@ -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());