mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
fancy print output based on interfaceFlags()
This commit is contained in:
parent
ce7d570459
commit
2bf66dc125
@ -45,10 +45,14 @@ public:
|
||||
enum InterfaceFlag {
|
||||
READS_INPUT = 0x01,
|
||||
READS_OUTPUT = 0x02,
|
||||
READS_MASK = READS_INPUT | READS_OUTPUT,
|
||||
WRITES_NEXT_INPUT = 0x04,
|
||||
WRITES_PREV_OUTPUT = 0x08,
|
||||
WRITES_UNKNOWN = 0x10,
|
||||
|
||||
OWN_IF_MASK = READS_INPUT | READS_OUTPUT,
|
||||
EXT_IF_MASK = WRITES_NEXT_INPUT | WRITES_PREV_OUTPUT,
|
||||
INPUT_IF_MASK = READS_INPUT | WRITES_PREV_OUTPUT | WRITES_UNKNOWN,
|
||||
OUTPUT_IF_MASK = READS_OUTPUT | WRITES_NEXT_INPUT | WRITES_UNKNOWN,
|
||||
};
|
||||
typedef Flags<InterfaceFlag> InterfaceFlags;
|
||||
InterfaceFlags interfaceFlags() const;
|
||||
|
||||
@ -65,6 +65,15 @@ bool ContainerBase::traverseStages(const ContainerBase::StageCallback &processor
|
||||
}
|
||||
|
||||
|
||||
SubTask::InterfaceFlags SerialContainerPrivate::interfaceFlags() const
|
||||
{
|
||||
SubTask::InterfaceFlags f;
|
||||
if (!children().size()) return f;
|
||||
f |= children().front()->pimpl_func()->deducedInterfaceFlags() & SubTask::INPUT_IF_MASK;
|
||||
f |= children().back()->pimpl_func()->deducedInterfaceFlags() & SubTask::OUTPUT_IF_MASK;
|
||||
return f;
|
||||
}
|
||||
|
||||
bool SerialContainerPrivate::canInsert(const ContainerBasePrivate::value_type &subtask, ContainerBasePrivate::const_iterator before) const {
|
||||
return ContainerBasePrivate::canInsert(*subtask);
|
||||
}
|
||||
|
||||
@ -46,6 +46,7 @@ public:
|
||||
output_.reset(new Interface(Interface::NotifyFunction()));
|
||||
}
|
||||
|
||||
SubTask::InterfaceFlags interfaceFlags() const override;
|
||||
bool canInsert(const value_type& subtask, const_iterator before) const;
|
||||
|
||||
const SubTaskPrivate* prev_(const SubTaskPrivate* child) const override;
|
||||
|
||||
@ -13,8 +13,8 @@ SubTask::SubTask(SubTaskPrivate *impl)
|
||||
SubTask::InterfaceFlags SubTask::interfaceFlags() const
|
||||
{
|
||||
SubTask::InterfaceFlags result = pimpl_->interfaceFlags();
|
||||
result &= ~SubTask::InterfaceFlags(SubTask::READS_MASK);
|
||||
result |= pimpl_->SubTaskPrivate::interfaceFlags();
|
||||
result &= ~SubTask::InterfaceFlags(SubTask::OWN_IF_MASK);
|
||||
result |= pimpl_->deducedInterfaceFlags();
|
||||
return result;
|
||||
}
|
||||
|
||||
@ -31,7 +31,20 @@ std::ostream& operator<<(std::ostream &os, const SubTask& stage) {
|
||||
os << *stage.pimpl_func();
|
||||
return os;
|
||||
}
|
||||
|
||||
template<SubTask::InterfaceFlag own, SubTask::InterfaceFlag other>
|
||||
const char* direction(const SubTaskPrivate& stage) {
|
||||
SubTask::InterfaceFlags f = stage.deducedInterfaceFlags();
|
||||
bool own_if = f & own;
|
||||
bool other_if = f & other;
|
||||
bool reverse = own & SubTask::INPUT_IF_MASK;
|
||||
if (own_if && other_if) return "<>";
|
||||
if (!own_if && !other_if) return "--";
|
||||
if (other_if ^ reverse) return "->";
|
||||
return "<-";
|
||||
};
|
||||
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
|
||||
|
||||
// inputs
|
||||
for (const InterfacePtr &i : {stage.prevOutput(), stage.input_}) {
|
||||
os << std::setw(3);
|
||||
@ -39,7 +52,9 @@ std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
|
||||
else os << "-";
|
||||
}
|
||||
// trajectories
|
||||
os << " -> " << std::setw(3) << stage.trajectories_.size() << " <- ";
|
||||
os << std::setw(5) << direction<SubTask::READS_INPUT, SubTask::WRITES_PREV_OUTPUT>(stage)
|
||||
<< std::setw(3) << stage.trajectories_.size()
|
||||
<< std::setw(5) << direction<SubTask::READS_OUTPUT, SubTask::WRITES_NEXT_INPUT>(stage);
|
||||
// outputs
|
||||
for (const InterfacePtr &i : {stage.output_, stage.nextInput()}) {
|
||||
os << std::setw(3);
|
||||
@ -74,21 +89,23 @@ SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajec
|
||||
return trajectories_.back();
|
||||
}
|
||||
|
||||
SubTask::InterfaceFlags SubTaskPrivate::interfaceFlags() const
|
||||
// return the interface flags that can be deduced from the interface
|
||||
inline SubTask::InterfaceFlags SubTaskPrivate::deducedInterfaceFlags() const
|
||||
{
|
||||
// the base class provides the read flags
|
||||
SubTask::InterfaceFlags f;
|
||||
if (input_) f |= SubTask::READS_INPUT;
|
||||
if (output_) f |= SubTask::READS_OUTPUT;
|
||||
if (prevOutput()) f |= SubTask::WRITES_PREV_OUTPUT;
|
||||
if (nextInput()) f |= SubTask::WRITES_NEXT_INPUT;
|
||||
return f;
|
||||
}
|
||||
|
||||
void SubTaskPrivate::sendForward(SubTrajectory& trajectory, const planning_scene::PlanningSceneConstPtr& ps){
|
||||
inline void SubTaskPrivate::sendForward(SubTrajectory& trajectory, const planning_scene::PlanningSceneConstPtr& ps){
|
||||
std::cout << "sending state to start" << std::endl;
|
||||
nextInput()->add(ps, &trajectory, NULL);
|
||||
}
|
||||
|
||||
void SubTaskPrivate::sendBackward(SubTrajectory& trajectory, const planning_scene::PlanningSceneConstPtr& ps){
|
||||
inline void SubTaskPrivate::sendBackward(SubTrajectory& trajectory, const planning_scene::PlanningSceneConstPtr& ps){
|
||||
std::cout << "sending state to end" << std::endl;
|
||||
prevOutput()->add(ps, NULL, &trajectory);
|
||||
}
|
||||
|
||||
@ -23,7 +23,8 @@ public:
|
||||
|
||||
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
|
||||
|
||||
virtual SubTask::InterfaceFlags interfaceFlags() const;
|
||||
SubTask::InterfaceFlags deducedInterfaceFlags() const;
|
||||
virtual SubTask::InterfaceFlags interfaceFlags() const = 0;
|
||||
void sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
|
||||
void sendBackward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user