mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Add Generator::spawn(from, to, trajectory) variant (#546)
This commit is contained in:
parent
55c4b52b13
commit
560aa0a476
@ -365,6 +365,7 @@ public:
|
|||||||
|
|
||||||
virtual bool canCompute() const = 0;
|
virtual bool canCompute() const = 0;
|
||||||
virtual void compute() = 0;
|
virtual void compute() = 0;
|
||||||
|
void spawn(InterfaceState&& from, InterfaceState&& to, SubTrajectory&& trajectory);
|
||||||
void spawn(InterfaceState&& state, SubTrajectory&& trajectory);
|
void spawn(InterfaceState&& state, SubTrajectory&& trajectory);
|
||||||
void spawn(InterfaceState&& state, double cost) {
|
void spawn(InterfaceState&& state, double cost) {
|
||||||
SubTrajectory trajectory;
|
SubTrajectory trajectory;
|
||||||
|
|||||||
@ -136,6 +136,7 @@ public:
|
|||||||
void sendBackward(InterfaceState&& from, const InterfaceState& to, const SolutionBasePtr& solution);
|
void sendBackward(InterfaceState&& from, const InterfaceState& to, const SolutionBasePtr& solution);
|
||||||
template <Interface::Direction>
|
template <Interface::Direction>
|
||||||
inline void send(const InterfaceState& start, InterfaceState&& end, const SolutionBasePtr& solution);
|
inline void send(const InterfaceState& start, InterfaceState&& end, const SolutionBasePtr& solution);
|
||||||
|
void spawn(InterfaceState&& from, InterfaceState&& to, const SolutionBasePtr& solution);
|
||||||
void spawn(InterfaceState&& state, const SolutionBasePtr& solution);
|
void spawn(InterfaceState&& state, const SolutionBasePtr& solution);
|
||||||
void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution);
|
void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution);
|
||||||
|
|
||||||
|
|||||||
@ -219,28 +219,32 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
|
|||||||
newSolution(solution);
|
newSolution(solution);
|
||||||
}
|
}
|
||||||
|
|
||||||
void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) {
|
void StagePrivate::spawn(InterfaceState&& from, InterfaceState&& to, const SolutionBasePtr& solution) {
|
||||||
assert(prevEnds() && nextStarts());
|
assert(prevEnds() && nextStarts());
|
||||||
|
|
||||||
computeCost(state, state, *solution);
|
computeCost(from, to, *solution);
|
||||||
|
|
||||||
if (!storeSolution(solution, nullptr, nullptr))
|
if (!storeSolution(solution, nullptr, nullptr))
|
||||||
return; // solution dropped
|
return; // solution dropped
|
||||||
|
|
||||||
auto from = states_.insert(states_.end(), InterfaceState(state)); // copy
|
auto from_it = states_.insert(states_.end(), std::move(from));
|
||||||
auto to = states_.insert(states_.end(), std::move(state));
|
auto to_it = states_.insert(states_.end(), std::move(to));
|
||||||
|
|
||||||
solution->setStartState(*from);
|
solution->setStartState(*from_it);
|
||||||
solution->setEndState(*to);
|
solution->setEndState(*to_it);
|
||||||
|
|
||||||
if (!solution->isFailure()) {
|
if (!solution->isFailure()) {
|
||||||
prevEnds()->add(*from);
|
prevEnds()->add(*from_it);
|
||||||
nextStarts()->add(*to);
|
nextStarts()->add(*to_it);
|
||||||
}
|
}
|
||||||
|
|
||||||
newSolution(solution);
|
newSolution(solution);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) {
|
||||||
|
spawn(InterfaceState(state), std::move(state), solution);
|
||||||
|
}
|
||||||
|
|
||||||
void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) {
|
void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) {
|
||||||
computeCost(from, to, *solution);
|
computeCost(from, to, *solution);
|
||||||
|
|
||||||
@ -688,6 +692,10 @@ void GeneratorPrivate::compute() {
|
|||||||
Generator::Generator(GeneratorPrivate* impl) : ComputeBase(impl) {}
|
Generator::Generator(GeneratorPrivate* impl) : ComputeBase(impl) {}
|
||||||
Generator::Generator(const std::string& name) : Generator(new GeneratorPrivate(this, name)) {}
|
Generator::Generator(const std::string& name) : Generator(new GeneratorPrivate(this, name)) {}
|
||||||
|
|
||||||
|
void Generator::spawn(InterfaceState&& from, InterfaceState&& to, SubTrajectory&& t) {
|
||||||
|
pimpl()->spawn(std::move(from), std::move(to), std::make_shared<SubTrajectory>(std::move(t)));
|
||||||
|
}
|
||||||
|
|
||||||
void Generator::spawn(InterfaceState&& state, SubTrajectory&& t) {
|
void Generator::spawn(InterfaceState&& state, SubTrajectory&& t) {
|
||||||
pimpl()->spawn(std::move(state), std::make_shared<SubTrajectory>(std::move(t)));
|
pimpl()->spawn(std::move(state), std::make_shared<SubTrajectory>(std::move(t)));
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user