mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Interface: order states by priority
Priority is (depth of solutions, accumulated cost along trace).
This commit is contained in:
parent
d0351698a6
commit
facd6438c9
@ -139,9 +139,9 @@ protected:
|
|||||||
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
||||||
|
|
||||||
/// callback for new start states (received externally)
|
/// callback for new start states (received externally)
|
||||||
virtual void onNewStartState(const InterfaceState &external) = 0;
|
virtual void onNewStartState(Interface::iterator external, bool updated) = 0;
|
||||||
/// callback for new end states (received externally)
|
/// callback for new end states (received externally)
|
||||||
virtual void onNewEndState(const InterfaceState &external) = 0;
|
virtual void onNewEndState(Interface::iterator external, bool updated) = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -98,13 +98,13 @@ protected:
|
|||||||
{}
|
{}
|
||||||
|
|
||||||
/// copy external_state to a child's interface and remember the link in internal_to map
|
/// copy external_state to a child's interface and remember the link in internal_to map
|
||||||
void copyState(InterfaceState &external, Stage &child, bool to_start);
|
void copyState(Interface::iterator external, Stage &child, bool to_start, bool updated);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
container_type children_;
|
container_type children_;
|
||||||
|
|
||||||
// map start/end states of children (internal) to corresponding states in our external interfaces
|
// map start/end states of children (internal) to corresponding states in our external interfaces
|
||||||
std::map<const InterfaceState*, InterfaceState*> internal_to_external_;
|
std::map<const InterfaceState*, Interface::iterator> internal_to_external_;
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(ContainerBase)
|
PIMPL_FUNCTIONS(ContainerBase)
|
||||||
|
|
||||||
|
|||||||
@ -50,6 +50,8 @@ public:
|
|||||||
|
|
||||||
reference top() { return c.front(); }
|
reference top() { return c.front(); }
|
||||||
const_reference top() const { return c.front(); }
|
const_reference top() const { return c.front(); }
|
||||||
|
reference pop() { reference result = top(); c.pop_front(); return result; }
|
||||||
|
|
||||||
reference front() { return c.front(); }
|
reference front() { return c.front(); }
|
||||||
const_reference front() const { return c.front(); }
|
const_reference front() const { return c.front(); }
|
||||||
reference back() { return c.back(); }
|
reference back() { return c.back(); }
|
||||||
@ -68,6 +70,9 @@ public:
|
|||||||
const_reverse_iterator crbegin() const { return c.rbegin(); }
|
const_reverse_iterator crbegin() const { return c.rbegin(); }
|
||||||
const_reverse_iterator crend() const { return c.rend(); }
|
const_reverse_iterator crend() const { return c.rend(); }
|
||||||
|
|
||||||
|
/// explicitly sort container, useful if many items have changed their value
|
||||||
|
void sort() { c.sort(comp); }
|
||||||
|
|
||||||
iterator insert(const value_type& item) {
|
iterator insert(const value_type& item) {
|
||||||
iterator at = std::upper_bound(c.begin(), c.end(), item, comp);
|
iterator at = std::upper_bound(c.begin(), c.end(), item, comp);
|
||||||
return c.insert(at, item);
|
return c.insert(at, item);
|
||||||
@ -79,7 +84,7 @@ public:
|
|||||||
|
|
||||||
iterator erase(const_iterator pos) { return c.erase(pos); }
|
iterator erase(const_iterator pos) { return c.erase(pos); }
|
||||||
|
|
||||||
/// update sort position of iterator after changes
|
/// update sort position of a single item after changes
|
||||||
iterator update(iterator &it) {
|
iterator update(iterator &it) {
|
||||||
container_type temp;
|
container_type temp;
|
||||||
temp.splice(temp.end(), c, it); // move it from c to temp
|
temp.splice(temp.end(), c, it); // move it from c to temp
|
||||||
@ -87,12 +92,26 @@ public:
|
|||||||
c.splice(at, temp, it);
|
c.splice(at, temp, it);
|
||||||
return it;
|
return it;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// move element pos from this to other container, inserting before other_pos
|
||||||
|
iterator moveTo(iterator pos, container_type& other, iterator other_pos) {
|
||||||
|
other.splice(other_pos, c, pos);
|
||||||
|
return pos;
|
||||||
|
}
|
||||||
|
/// move element pos from other container into this one (sorted)
|
||||||
|
iterator moveFrom(iterator pos, container_type& other) {
|
||||||
|
iterator at = std::upper_bound(begin(), end(), *pos, comp);
|
||||||
|
c.splice(at, other, pos);
|
||||||
|
return pos;
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
namespace detail {
|
namespace detail {
|
||||||
|
|
||||||
template <typename ValueType, typename CostType>
|
template <typename ValueType, typename CostType>
|
||||||
struct ItemCostPair : std::pair<ValueType, CostType> {
|
struct ItemCostPair : std::pair<ValueType, CostType> {
|
||||||
|
typedef CostType cost_type;
|
||||||
|
|
||||||
ItemCostPair(const std::pair<ValueType, CostType>& other)
|
ItemCostPair(const std::pair<ValueType, CostType>& other)
|
||||||
: std::pair<ValueType, CostType>(other) {}
|
: std::pair<ValueType, CostType>(other) {}
|
||||||
ItemCostPair(std::pair<ValueType, CostType>&& other)
|
ItemCostPair(std::pair<ValueType, CostType>&& other)
|
||||||
@ -101,7 +120,7 @@ struct ItemCostPair : std::pair<ValueType, CostType> {
|
|||||||
inline ValueType& value() { return this->first; }
|
inline ValueType& value() { return this->first; }
|
||||||
inline const ValueType& value() const { return this->first; }
|
inline const ValueType& value() const { return this->first; }
|
||||||
|
|
||||||
inline const CostType& cost() const { return this->second; }
|
inline CostType cost() const { return this->second; }
|
||||||
|
|
||||||
// comparison only considers cost
|
// comparison only considers cost
|
||||||
constexpr bool operator<(const ItemCostPair& other) const {
|
constexpr bool operator<(const ItemCostPair& other) const {
|
||||||
@ -109,7 +128,7 @@ struct ItemCostPair : std::pair<ValueType, CostType> {
|
|||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
} // namespace detail
|
||||||
|
|
||||||
template <typename ValueType, typename CostType = double,
|
template <typename ValueType, typename CostType = double,
|
||||||
typename Compare = std::less<detail::ItemCostPair<ValueType, CostType>>>
|
typename Compare = std::less<detail::ItemCostPair<ValueType, CostType>>>
|
||||||
@ -117,7 +136,10 @@ class cost_ordered : public ordered<detail::ItemCostPair<ValueType, CostType>, C
|
|||||||
{
|
{
|
||||||
typedef ordered<detail::ItemCostPair<ValueType, CostType>, Compare> base_type;
|
typedef ordered<detail::ItemCostPair<ValueType, CostType>, Compare> base_type;
|
||||||
public:
|
public:
|
||||||
auto insert(const ValueType& value, const CostType& cost) {
|
auto insert(const ValueType& value, const CostType cost) {
|
||||||
return base_type::insert(std::make_pair(value, cost));
|
return base_type::insert(std::make_pair(value, cost));
|
||||||
}
|
}
|
||||||
|
auto insert(ValueType&& value, const CostType cost) {
|
||||||
|
return base_type::insert(std::make_pair(std::move(value), cost));
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|||||||
@ -203,7 +203,9 @@ public:
|
|||||||
enum Direction { FORWARD = 0x01, BACKWARD = 0x02, ANYWAY = FORWARD | BACKWARD};
|
enum Direction { FORWARD = 0x01, BACKWARD = 0x02, ANYWAY = FORWARD | BACKWARD};
|
||||||
void restrictDirection(Direction dir);
|
void restrictDirection(Direction dir);
|
||||||
|
|
||||||
|
virtual void reset() override;
|
||||||
virtual void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
virtual void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
||||||
|
|
||||||
virtual bool computeForward(const InterfaceState& from) = 0;
|
virtual bool computeForward(const InterfaceState& from) = 0;
|
||||||
virtual bool computeBackward(const InterfaceState& to) = 0;
|
virtual bool computeBackward(const InterfaceState& to) = 0;
|
||||||
|
|
||||||
@ -285,6 +287,8 @@ public:
|
|||||||
PRIVATE_CLASS(Connecting)
|
PRIVATE_CLASS(Connecting)
|
||||||
Connecting(const std::string& name);
|
Connecting(const std::string& name);
|
||||||
|
|
||||||
|
virtual void reset() override;
|
||||||
|
|
||||||
virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0;
|
virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0;
|
||||||
void connect(const InterfaceState& from, const InterfaceState& to,
|
void connect(const InterfaceState& from, const InterfaceState& to,
|
||||||
SubTrajectory&& trajectory);
|
SubTrajectory&& trajectory);
|
||||||
|
|||||||
@ -146,6 +146,7 @@ PIMPL_FUNCTIONS(ComputeBase)
|
|||||||
|
|
||||||
class PropagatingEitherWayPrivate : public ComputeBasePrivate {
|
class PropagatingEitherWayPrivate : public ComputeBasePrivate {
|
||||||
friend class PropagatingEitherWay;
|
friend class PropagatingEitherWay;
|
||||||
|
std::list<Interface::value_type> processed; // already processed states
|
||||||
|
|
||||||
public:
|
public:
|
||||||
PropagatingEitherWay::Direction dir;
|
PropagatingEitherWay::Direction dir;
|
||||||
@ -166,14 +167,11 @@ public:
|
|||||||
const InterfaceState &fetchEndState();
|
const InterfaceState &fetchEndState();
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// get informed when new start or end state becomes available
|
|
||||||
void newStartState(const std::list<InterfaceState>::iterator& it);
|
|
||||||
void newEndState(const std::list<InterfaceState>::iterator& it);
|
|
||||||
// initialize properties from parent and/or state
|
// initialize properties from parent and/or state
|
||||||
void initProperties(const InterfaceState &state);
|
void initProperties(const InterfaceState &state);
|
||||||
|
// drop states corresponding to failed (infinite-cost) trajectories
|
||||||
Interface::const_iterator next_start_state_;
|
void dropFailedStarts(Interface::iterator state);
|
||||||
Interface::const_iterator next_end_state_;
|
void dropFailedEnds(Interface::iterator state);
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(PropagatingEitherWay)
|
PIMPL_FUNCTIONS(PropagatingEitherWay)
|
||||||
|
|
||||||
@ -209,6 +207,12 @@ PIMPL_FUNCTIONS(Generator)
|
|||||||
class ConnectingPrivate : public ComputeBasePrivate {
|
class ConnectingPrivate : public ComputeBasePrivate {
|
||||||
friend class Connecting;
|
friend class Connecting;
|
||||||
|
|
||||||
|
typedef std::pair<Interface::const_iterator, Interface::const_iterator> StatePair;
|
||||||
|
struct StatePairLess {
|
||||||
|
bool operator()(const StatePair& x, const StatePair& y) const {
|
||||||
|
return x.first->priority() + x.second->priority() < y.first->priority() + y.second->priority();
|
||||||
|
}
|
||||||
|
};
|
||||||
public:
|
public:
|
||||||
inline ConnectingPrivate(Connecting *me, const std::string &name);
|
inline ConnectingPrivate(Connecting *me, const std::string &name);
|
||||||
|
|
||||||
@ -220,12 +224,13 @@ public:
|
|||||||
|
|
||||||
private:
|
private:
|
||||||
// get informed when new start or end state becomes available
|
// get informed when new start or end state becomes available
|
||||||
void newStartState(const std::list<InterfaceState>::iterator& it);
|
void newStartState(Interface::iterator it, bool updated);
|
||||||
void newEndState(const std::list<InterfaceState>::iterator& it);
|
void newEndState(Interface::iterator it, bool updated);
|
||||||
// initialize properties from parent and/or interface states
|
// initialize properties from parent and/or interface states
|
||||||
void initProperties(const InterfaceState &start, const InterfaceState &end);
|
void initProperties(const InterfaceState &start, const InterfaceState &end);
|
||||||
|
|
||||||
std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
|
// ordered list of pending state pairs
|
||||||
|
ordered<StatePair, StatePairLess> pending;
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(Connecting)
|
PIMPL_FUNCTIONS(Connecting)
|
||||||
|
|
||||||
|
|||||||
@ -41,6 +41,7 @@
|
|||||||
|
|
||||||
#include <moveit/macros/class_forward.h>
|
#include <moveit/macros/class_forward.h>
|
||||||
#include <moveit/task_constructor/properties.h>
|
#include <moveit/task_constructor/properties.h>
|
||||||
|
#include <moveit/task_constructor/cost_queue.h>
|
||||||
#include <moveit_task_constructor_msgs/Solution.h>
|
#include <moveit_task_constructor_msgs/Solution.h>
|
||||||
#include <visualization_msgs/MarkerArray.h>
|
#include <visualization_msgs/MarkerArray.h>
|
||||||
|
|
||||||
@ -74,8 +75,31 @@ MOVEIT_CLASS_FORWARD(Introspection)
|
|||||||
*/
|
*/
|
||||||
class InterfaceState {
|
class InterfaceState {
|
||||||
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase
|
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase
|
||||||
|
friend class Interface; // Interface is allowed to change priority
|
||||||
public:
|
public:
|
||||||
// TODO turn this into priority queue
|
/** InterfaceStates are ordered according to two values:
|
||||||
|
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
|
||||||
|
* Preference ordering considers high-depth first and within same depth, minimal cost paths.
|
||||||
|
*/
|
||||||
|
struct Priority : public std::pair<unsigned int, double> {
|
||||||
|
Priority() : Priority(0, 0.0) {}
|
||||||
|
Priority(unsigned int depth, double cost)
|
||||||
|
: std::pair<unsigned int, double>(depth, cost) {}
|
||||||
|
|
||||||
|
inline unsigned int depth() const { return this->first; }
|
||||||
|
inline double cost() const { return this->second; }
|
||||||
|
|
||||||
|
Priority operator+(const Priority& other) const {
|
||||||
|
return Priority(this->depth() + other.depth(),
|
||||||
|
this->cost() + other.cost());
|
||||||
|
}
|
||||||
|
inline bool operator<(const Priority& other) const {
|
||||||
|
if (this->depth() == other.depth())
|
||||||
|
return this->cost() < other.cost();
|
||||||
|
else
|
||||||
|
return this->depth() > other.depth();
|
||||||
|
}
|
||||||
|
};
|
||||||
typedef std::deque<SolutionBase*> Solutions;
|
typedef std::deque<SolutionBase*> Solutions;
|
||||||
|
|
||||||
/// create an InterfaceState from a planning scene
|
/// create an InterfaceState from a planning scene
|
||||||
@ -91,6 +115,12 @@ public:
|
|||||||
PropertyMap& properties() { return properties_; }
|
PropertyMap& properties() { return properties_; }
|
||||||
const PropertyMap& properties() const { return properties_; }
|
const PropertyMap& properties() const { return properties_; }
|
||||||
|
|
||||||
|
/// states are ordered by priority
|
||||||
|
inline bool operator<(const InterfaceState& other) const {
|
||||||
|
return this->priority_ < other.priority_;
|
||||||
|
}
|
||||||
|
inline const Priority& priority() const { return priority_; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// these methods should be only called by SolutionBase::set[Start|End]State()
|
// these methods should be only called by SolutionBase::set[Start|End]State()
|
||||||
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
|
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
|
||||||
@ -101,50 +131,21 @@ private:
|
|||||||
PropertyMap properties_;
|
PropertyMap properties_;
|
||||||
Solutions incoming_trajectories_;
|
Solutions incoming_trajectories_;
|
||||||
Solutions outgoing_trajectories_;
|
Solutions outgoing_trajectories_;
|
||||||
|
Priority priority_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
/** Interface provides a list of InterfaceStates available as input for a stage.
|
/** Interface provides a cost-sorted list of InterfaceStates available as input for a stage. */
|
||||||
*
|
class Interface : public ordered<InterfaceState> {
|
||||||
* This is essentially an adaptor to a container class, to allow for notification
|
|
||||||
* of the interface's owner when new states become available
|
|
||||||
*/
|
|
||||||
class Interface : protected std::list<InterfaceState> {
|
|
||||||
public:
|
public:
|
||||||
typedef std::list<InterfaceState> container_type;
|
typedef std::function<void(iterator it, bool updated)> NotifyFunction;
|
||||||
typedef std::function<void(const container_type::iterator&)> NotifyFunction;
|
|
||||||
Interface(const NotifyFunction ¬ify = NotifyFunction());
|
Interface(const NotifyFunction ¬ify = NotifyFunction());
|
||||||
|
|
||||||
// add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state
|
// add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state
|
||||||
// and finally run the notify callback
|
iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing);
|
||||||
container_type::iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing);
|
|
||||||
|
|
||||||
// clone an existing InterfaceState, but without its incoming/outgoing connections
|
// clone an existing InterfaceState, but without its incoming/outgoing connections
|
||||||
container_type::iterator clone(const InterfaceState &state);
|
iterator clone(const InterfaceState &state);
|
||||||
|
|
||||||
using container_type::value_type;
|
|
||||||
using container_type::reference;
|
|
||||||
using container_type::const_reference;
|
|
||||||
|
|
||||||
using container_type::iterator;
|
|
||||||
using container_type::const_iterator;
|
|
||||||
using container_type::reverse_iterator;
|
|
||||||
using container_type::const_reverse_iterator;
|
|
||||||
|
|
||||||
using container_type::empty;
|
|
||||||
using container_type::size;
|
|
||||||
using container_type::clear;
|
|
||||||
using container_type::front;
|
|
||||||
using container_type::back;
|
|
||||||
|
|
||||||
using container_type::begin;
|
|
||||||
using container_type::cbegin;
|
|
||||||
using container_type::end;
|
|
||||||
using container_type::cend;
|
|
||||||
using container_type::rbegin;
|
|
||||||
using container_type::crbegin;
|
|
||||||
using container_type::rend;
|
|
||||||
using container_type::crend;
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const NotifyFunction notify_;
|
const NotifyFunction notify_;
|
||||||
|
|||||||
@ -43,6 +43,9 @@
|
|||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <boost/range/adaptor/reversed.hpp>
|
#include <boost/range/adaptor/reversed.hpp>
|
||||||
|
#include <functional>
|
||||||
|
|
||||||
|
using namespace std::placeholders;
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
@ -87,14 +90,14 @@ bool ContainerBasePrivate::compute()
|
|||||||
return static_cast<ContainerBase*>(me_)->compute();
|
return static_cast<ContainerBase*>(me_)->compute();
|
||||||
}
|
}
|
||||||
|
|
||||||
void ContainerBasePrivate::copyState(InterfaceState &external,
|
void ContainerBasePrivate::copyState(Interface::iterator external,
|
||||||
Stage &child, bool to_start) {
|
Stage &child, bool to_start, bool updated) {
|
||||||
if (to_start) {
|
if (to_start) {
|
||||||
InterfaceState& internal = *child.pimpl()->starts()->clone(external);
|
InterfaceState& internal = *child.pimpl()->starts()->clone(*external);
|
||||||
internal_to_external_.insert(std::make_pair(&internal, &external));
|
internal_to_external_.insert(std::make_pair(&internal, external));
|
||||||
} else {
|
} else {
|
||||||
InterfaceState& internal = *child.pimpl()->ends()->clone(external);
|
InterfaceState& internal = *child.pimpl()->ends()->clone(*external);
|
||||||
internal_to_external_.insert(std::make_pair(&internal, &external));
|
internal_to_external_.insert(std::make_pair(&internal, external));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -283,8 +286,8 @@ void SerialContainerPrivate::storeNewSolution(SerialSolution &&s)
|
|||||||
solution.setStartState(*it->second);
|
solution.setStartState(*it->second);
|
||||||
} else {
|
} else {
|
||||||
// spawn a new state in previous stage
|
// spawn a new state in previous stage
|
||||||
InterfaceState& external = *prevEnds()->add(InterfaceState(*internal_from), NULL, &solution);
|
Interface::iterator external = prevEnds()->add(InterfaceState(*internal_from), NULL, &solution);
|
||||||
internal_to_external_.insert(std::make_pair(internal_from, &external));
|
internal_to_external_.insert(std::make_pair(internal_from, external));
|
||||||
}
|
}
|
||||||
|
|
||||||
// add solution to existing or new end state
|
// add solution to existing or new end state
|
||||||
@ -294,8 +297,8 @@ void SerialContainerPrivate::storeNewSolution(SerialSolution &&s)
|
|||||||
solution.setEndState(*it->second);
|
solution.setEndState(*it->second);
|
||||||
} else {
|
} else {
|
||||||
// spawn a new state in next stage
|
// spawn a new state in next stage
|
||||||
InterfaceState& external = *nextStarts()->add(InterfaceState(*internal_to), &solution, NULL);
|
Interface::iterator external = nextStarts()->add(InterfaceState(*internal_to), &solution, NULL);
|
||||||
internal_to_external_.insert(std::make_pair(internal_to, &external));
|
internal_to_external_.insert(std::make_pair(internal_to, external));
|
||||||
}
|
}
|
||||||
|
|
||||||
// perform default stage action on new solution
|
// perform default stage action on new solution
|
||||||
@ -363,16 +366,16 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
|
|||||||
// initialize starts_ and ends_ interfaces
|
// initialize starts_ and ends_ interfaces
|
||||||
Stage* child = start->get();
|
Stage* child = start->get();
|
||||||
if (child->pimpl()->starts())
|
if (child->pimpl()->starts())
|
||||||
impl->starts_.reset(new Interface([impl, child](const Interface::iterator& external){
|
impl->starts_.reset(new Interface([impl, child](Interface::iterator external, bool updated){
|
||||||
// new external state in our starts_ interface is copied to first child
|
// new external state in our starts_ interface is copied to first child
|
||||||
impl->copyState(*external, *child, true);
|
impl->copyState(external, *child, true, updated);
|
||||||
}));
|
}));
|
||||||
|
|
||||||
child = last->get();
|
child = last->get();
|
||||||
if (child->pimpl()->ends())
|
if (child->pimpl()->ends())
|
||||||
impl->ends_.reset(new Interface([impl, child](const Interface::iterator& external){
|
impl->ends_.reset(new Interface([impl, child](Interface::iterator external, bool updated){
|
||||||
// new external state in our ends_ interface is copied to last child
|
// new external state in our ends_ interface is copied to last child
|
||||||
impl->copyState(*external, *child, false);
|
impl->copyState(external, *child, false, updated);
|
||||||
}));
|
}));
|
||||||
|
|
||||||
// validate connectivity of this
|
// validate connectivity of this
|
||||||
@ -466,12 +469,8 @@ void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg,
|
|||||||
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase *me, const std::string &name)
|
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase *me, const std::string &name)
|
||||||
: ContainerBasePrivate(me, name)
|
: ContainerBasePrivate(me, name)
|
||||||
{
|
{
|
||||||
starts_.reset(new Interface([me](const Interface::iterator& external){
|
starts_.reset(new Interface(std::bind(&ParallelContainerBase::onNewStartState, me, _1, _2)));
|
||||||
me->onNewStartState(*external);
|
ends_.reset(new Interface(std::bind(&ParallelContainerBase::onNewEndState, me, _1, _2)));
|
||||||
}));
|
|
||||||
ends_.reset(new Interface([me](const Interface::iterator& external){
|
|
||||||
me->onNewEndState(*external);
|
|
||||||
}));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -42,6 +42,7 @@
|
|||||||
#include <iomanip>
|
#include <iomanip>
|
||||||
#include <ros/console.h>
|
#include <ros/console.h>
|
||||||
|
|
||||||
|
using namespace std::placeholders;
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
void InitStageException::push_back(const Stage &stage, const std::string &msg)
|
void InitStageException::push_back(const Stage &stage, const std::string &msg)
|
||||||
@ -257,32 +258,35 @@ PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *m
|
|||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
|
||||||
|
// move infinite-cost states to processed list
|
||||||
|
if (std::isinf(state->priority().cost()))
|
||||||
|
starts_->moveTo(state, processed, processed.end());
|
||||||
|
}
|
||||||
|
void PropagatingEitherWayPrivate::dropFailedEnds(Interface::iterator state) {
|
||||||
|
// move infinite-cost states to processed list
|
||||||
|
if (std::isinf(state->priority().cost()))
|
||||||
|
ends_->moveTo(state, processed, processed.end());
|
||||||
|
}
|
||||||
|
|
||||||
inline bool PropagatingEitherWayPrivate::hasStartState() const{
|
inline bool PropagatingEitherWayPrivate::hasStartState() const{
|
||||||
return next_start_state_ != starts_->end();
|
return starts_ && !starts_->empty();
|
||||||
}
|
}
|
||||||
|
|
||||||
const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){
|
const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){
|
||||||
if (!hasStartState())
|
assert(hasStartState());
|
||||||
throw std::runtime_error("no new state for beginning available");
|
// move state to processed list
|
||||||
|
return *starts_->moveTo(starts_->begin(), processed, processed.end());
|
||||||
const InterfaceState& state= *next_start_state_;
|
|
||||||
++next_start_state_;
|
|
||||||
|
|
||||||
return state;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
inline bool PropagatingEitherWayPrivate::hasEndState() const{
|
inline bool PropagatingEitherWayPrivate::hasEndState() const{
|
||||||
return next_end_state_ != ends_->end();
|
return ends_ && !ends_->empty();
|
||||||
}
|
}
|
||||||
|
|
||||||
const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
|
const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
|
||||||
if(!hasEndState())
|
assert(hasEndState());
|
||||||
throw std::runtime_error("no new state for ending available");
|
// move state to processed list
|
||||||
|
return *ends_->moveTo(ends_->begin(), processed, processed.end());
|
||||||
const InterfaceState& state= *next_end_state_;
|
|
||||||
++next_end_state_;
|
|
||||||
|
|
||||||
return state;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state)
|
void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state)
|
||||||
@ -324,22 +328,6 @@ bool PropagatingEitherWayPrivate::compute()
|
|||||||
return countFailures(result);
|
return countFailures(result);
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingEitherWayPrivate::newStartState(const Interface::iterator &it)
|
|
||||||
{
|
|
||||||
// we just appended a state to the list, but the iterator doesn't see it anymore
|
|
||||||
// so let's point it at the new one
|
|
||||||
if(next_start_state_ == starts_->end())
|
|
||||||
--next_start_state_;
|
|
||||||
}
|
|
||||||
|
|
||||||
void PropagatingEitherWayPrivate::newEndState(const Interface::iterator &it)
|
|
||||||
{
|
|
||||||
// we just appended a state to the list, but the iterator doesn't see it anymore
|
|
||||||
// so let's point it at the new one
|
|
||||||
if(next_end_state_ == ends_->end())
|
|
||||||
--next_end_state_;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
PropagatingEitherWay::PropagatingEitherWay(const std::string &name)
|
PropagatingEitherWay::PropagatingEitherWay(const std::string &name)
|
||||||
: PropagatingEitherWay(new PropagatingEitherWayPrivate(this, ANYWAY, name))
|
: PropagatingEitherWay(new PropagatingEitherWayPrivate(this, ANYWAY, name))
|
||||||
@ -356,23 +344,17 @@ void PropagatingEitherWay::initInterface()
|
|||||||
{
|
{
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
if (impl->dir & PropagatingEitherWay::FORWARD) {
|
if (impl->dir & PropagatingEitherWay::FORWARD) {
|
||||||
if (!impl->starts_) { // keep existing interface if possible
|
if (!impl->starts_) // keep existing interface if possible
|
||||||
impl->starts_.reset(new Interface([impl](const Interface::iterator& it) { impl->newStartState(it); }));
|
impl->starts_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, impl, _1)));
|
||||||
impl->next_start_state_ = impl->starts_->begin();
|
|
||||||
}
|
|
||||||
} else {
|
} else {
|
||||||
impl->starts_.reset();
|
impl->starts_.reset();
|
||||||
impl->next_start_state_ = Interface::iterator();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (impl->dir & PropagatingEitherWay::BACKWARD) {
|
if (impl->dir & PropagatingEitherWay::BACKWARD) {
|
||||||
if (!impl->ends_) { // keep existing interface if possible
|
if (!impl->ends_) // keep existing interface if possible
|
||||||
impl->ends_.reset(new Interface([impl](const Interface::iterator& it) { impl->newEndState(it); }));
|
impl->ends_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, impl, _1)));
|
||||||
impl->next_end_state_ = impl->ends_->end();
|
|
||||||
}
|
|
||||||
} else {
|
} else {
|
||||||
impl->ends_.reset();
|
impl->ends_.reset();
|
||||||
impl->next_end_state_ = Interface::iterator();
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -386,6 +368,12 @@ void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir
|
|||||||
initInterface();
|
initInterface();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void PropagatingEitherWay::reset()
|
||||||
|
{
|
||||||
|
pimpl()->processed.clear();
|
||||||
|
ComputeBase::reset();
|
||||||
|
}
|
||||||
|
|
||||||
void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene)
|
void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||||
{
|
{
|
||||||
ComputeBase::init(scene);
|
ComputeBase::init(scene);
|
||||||
@ -393,14 +381,10 @@ void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &sce
|
|||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
|
|
||||||
// after being connected, restrict actual interface directions
|
// after being connected, restrict actual interface directions
|
||||||
if (!impl->nextStarts()) {
|
if (!impl->nextStarts())
|
||||||
impl->starts_.reset();
|
impl->starts_.reset();
|
||||||
impl->next_start_state_ = Interface::iterator();
|
if (!impl->prevEnds())
|
||||||
}
|
|
||||||
if (!impl->prevEnds()) {
|
|
||||||
impl->ends_.reset();
|
impl->ends_.reset();
|
||||||
impl->next_end_state_ = Interface::iterator();
|
|
||||||
}
|
|
||||||
if (!impl->isConnected())
|
if (!impl->isConnected())
|
||||||
throw InitStageException(*this, "can neither send forwards nor backwards");
|
throw InitStageException(*this, "can neither send forwards nor backwards");
|
||||||
}
|
}
|
||||||
@ -433,7 +417,6 @@ PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward *me, con
|
|||||||
{
|
{
|
||||||
// indicate, that we don't accept new states from ends_ interface
|
// indicate, that we don't accept new states from ends_ interface
|
||||||
ends_.reset();
|
ends_.reset();
|
||||||
next_end_state_ = Interface::iterator();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -452,7 +435,6 @@ PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward *me,
|
|||||||
{
|
{
|
||||||
// indicate, that we don't accept new states from starts_ interface
|
// indicate, that we don't accept new states from starts_ interface
|
||||||
starts_.reset();
|
starts_.reset();
|
||||||
next_start_state_ = Interface::iterator();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -510,23 +492,40 @@ void Generator::spawn(InterfaceState&& state, SubTrajectory&& t)
|
|||||||
ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
|
ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
|
||||||
: ComputeBasePrivate(me, name)
|
: ComputeBasePrivate(me, name)
|
||||||
{
|
{
|
||||||
starts_.reset(new Interface([this](const Interface::iterator& it) { this->newStartState(it); }));
|
starts_.reset(new Interface(std::bind(&ConnectingPrivate::newStartState, this, _1, _2)));
|
||||||
ends_.reset(new Interface([this](const Interface::iterator& it) { this->newEndState(it); }));
|
ends_.reset(new Interface(std::bind(&ConnectingPrivate::newEndState, this, _1, _2)));
|
||||||
it_pairs_ = std::make_pair(starts_->begin(), ends_->begin());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConnectingPrivate::newStartState(const Interface::iterator& it)
|
void ConnectingPrivate::newStartState(Interface::iterator it, bool updated)
|
||||||
{
|
{
|
||||||
// TODO: need to handle the pairs iterator
|
if (!std::isfinite(it->priority().cost()))
|
||||||
if(it_pairs_.first == starts_->end())
|
return;
|
||||||
--it_pairs_.first;
|
if (updated) {
|
||||||
|
// many pairs might be affected: sort
|
||||||
|
pending.sort();
|
||||||
|
} else { // new state: insert all pairs with other interface
|
||||||
|
for (auto oit = ends_->begin(), oend = ends_->end(); oit != oend; ++oit) {
|
||||||
|
if (!std::isfinite(oit->priority().cost()))
|
||||||
|
break;
|
||||||
|
pending.insert(std::make_pair(it, oit));
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConnectingPrivate::newEndState(const Interface::iterator& it)
|
void ConnectingPrivate::newEndState(Interface::iterator it, bool updated)
|
||||||
{
|
{
|
||||||
// TODO: need to handle the pairs iterator properly
|
if (!std::isfinite(it->priority().cost()))
|
||||||
if(it_pairs_.second == ends_->end())
|
return;
|
||||||
--it_pairs_.second;
|
if (updated) {
|
||||||
|
// many pairs might be affected: sort
|
||||||
|
pending.sort();
|
||||||
|
} else { // new state: insert all pairs with other interface
|
||||||
|
for (auto oit = starts_->begin(), oend = starts_->end(); oit != oend; ++oit) {
|
||||||
|
if (!std::isfinite(oit->priority().cost()))
|
||||||
|
break;
|
||||||
|
pending.insert(std::make_pair(oit, it));
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConnectingPrivate::initProperties(const InterfaceState &start, const InterfaceState &end)
|
void ConnectingPrivate::initProperties(const InterfaceState &start, const InterfaceState &end)
|
||||||
@ -538,15 +537,13 @@ void ConnectingPrivate::initProperties(const InterfaceState &start, const Interf
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool ConnectingPrivate::canCompute() const{
|
bool ConnectingPrivate::canCompute() const{
|
||||||
// TODO: implement this properly
|
return !pending.empty();
|
||||||
return it_pairs_.first != starts_->end() &&
|
|
||||||
it_pairs_.second != ends_->end();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ConnectingPrivate::compute() {
|
bool ConnectingPrivate::compute() {
|
||||||
// TODO: implement this properly
|
const StatePair& top = pending.pop();
|
||||||
const InterfaceState& from = *it_pairs_.first;
|
const InterfaceState& from = *top.first;
|
||||||
const InterfaceState& to = *(it_pairs_.second++);
|
const InterfaceState& to = *top.second;
|
||||||
initProperties(from, to);
|
initProperties(from, to);
|
||||||
return countFailures(static_cast<Connecting*>(me_)->compute(from, to));
|
return countFailures(static_cast<Connecting*>(me_)->compute(from, to));
|
||||||
}
|
}
|
||||||
@ -557,6 +554,12 @@ Connecting::Connecting(const std::string &name)
|
|||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void Connecting::reset()
|
||||||
|
{
|
||||||
|
pimpl()->pending.clear();
|
||||||
|
ComputeBase::reset();
|
||||||
|
}
|
||||||
|
|
||||||
void Connecting::connect(const InterfaceState& from, const InterfaceState& to,
|
void Connecting::connect(const InterfaceState& from, const InterfaceState& to,
|
||||||
SubTrajectory&& t) {
|
SubTrajectory&& t) {
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
|
|||||||
@ -59,29 +59,37 @@ Interface::Interface(const Interface::NotifyFunction ¬ify)
|
|||||||
: notify_(notify)
|
: notify_(notify)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
|
// Function used by sendForward()/sendBackward()/spawn() to create a new interface state
|
||||||
Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing) {
|
Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing) {
|
||||||
if (!state.incomingTrajectories().empty() || !state.outgoingTrajectories().empty())
|
if (!state.incomingTrajectories().empty() || !state.outgoingTrajectories().empty())
|
||||||
throw std::runtime_error("expecting empty incoming/outgoing trajectories");
|
throw std::runtime_error("expecting empty incoming/outgoing trajectories");
|
||||||
if (!state.scene())
|
if (!state.scene())
|
||||||
throw std::runtime_error("expecting valid planning scene");
|
throw std::runtime_error("expecting valid planning scene");
|
||||||
|
|
||||||
|
// move state to a list node
|
||||||
|
std::list<InterfaceState> container;
|
||||||
|
auto it = container.insert(container.end(), std::move(state));
|
||||||
|
// configure state: inherit priority from other end's state and add current solution's cost
|
||||||
assert(bool(incoming) ^ bool(outgoing)); // either incoming or outgoing is set
|
assert(bool(incoming) ^ bool(outgoing)); // either incoming or outgoing is set
|
||||||
emplace_back(state);
|
if (incoming) {
|
||||||
iterator back = --end();
|
it->priority_ = InterfaceState::Priority(1, incoming->cost());
|
||||||
// adjust subtrajectories ...
|
incoming->setEndState(*it);
|
||||||
if (incoming) incoming->setEndState(*back);
|
} else if (outgoing) {
|
||||||
if (outgoing) outgoing->setStartState(*back);
|
it->priority_ = InterfaceState::Priority(1, outgoing->cost());
|
||||||
// ... before calling notify callback
|
outgoing->setStartState(*it);
|
||||||
if (notify_) notify_(back);
|
}
|
||||||
return back;
|
// move list node into interface's state list (sorted by priority)
|
||||||
|
moveFrom(it, container);
|
||||||
|
// and finally call notify callback
|
||||||
|
if (notify_) notify_(it, false);
|
||||||
|
return it;
|
||||||
}
|
}
|
||||||
|
|
||||||
Interface::iterator Interface::clone(const InterfaceState &state)
|
Interface::iterator Interface::clone(const InterfaceState &state)
|
||||||
{
|
{
|
||||||
emplace_back(InterfaceState(state));
|
iterator it = insert(InterfaceState(state));
|
||||||
iterator back = --end();
|
if (notify_) notify_(it, false);
|
||||||
if (notify_) notify_(back);
|
return it;
|
||||||
return back;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user