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 {
|
enum InterfaceFlag {
|
||||||
READS_INPUT = 0x01,
|
READS_INPUT = 0x01,
|
||||||
READS_OUTPUT = 0x02,
|
READS_OUTPUT = 0x02,
|
||||||
READS_MASK = READS_INPUT | READS_OUTPUT,
|
|
||||||
WRITES_NEXT_INPUT = 0x04,
|
WRITES_NEXT_INPUT = 0x04,
|
||||||
WRITES_PREV_OUTPUT = 0x08,
|
WRITES_PREV_OUTPUT = 0x08,
|
||||||
WRITES_UNKNOWN = 0x10,
|
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;
|
typedef Flags<InterfaceFlag> InterfaceFlags;
|
||||||
InterfaceFlags interfaceFlags() const;
|
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 {
|
bool SerialContainerPrivate::canInsert(const ContainerBasePrivate::value_type &subtask, ContainerBasePrivate::const_iterator before) const {
|
||||||
return ContainerBasePrivate::canInsert(*subtask);
|
return ContainerBasePrivate::canInsert(*subtask);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -46,6 +46,7 @@ public:
|
|||||||
output_.reset(new Interface(Interface::NotifyFunction()));
|
output_.reset(new Interface(Interface::NotifyFunction()));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
SubTask::InterfaceFlags interfaceFlags() const override;
|
||||||
bool canInsert(const value_type& subtask, const_iterator before) const;
|
bool canInsert(const value_type& subtask, const_iterator before) const;
|
||||||
|
|
||||||
const SubTaskPrivate* prev_(const SubTaskPrivate* child) const override;
|
const SubTaskPrivate* prev_(const SubTaskPrivate* child) const override;
|
||||||
|
|||||||
@ -13,8 +13,8 @@ SubTask::SubTask(SubTaskPrivate *impl)
|
|||||||
SubTask::InterfaceFlags SubTask::interfaceFlags() const
|
SubTask::InterfaceFlags SubTask::interfaceFlags() const
|
||||||
{
|
{
|
||||||
SubTask::InterfaceFlags result = pimpl_->interfaceFlags();
|
SubTask::InterfaceFlags result = pimpl_->interfaceFlags();
|
||||||
result &= ~SubTask::InterfaceFlags(SubTask::READS_MASK);
|
result &= ~SubTask::InterfaceFlags(SubTask::OWN_IF_MASK);
|
||||||
result |= pimpl_->SubTaskPrivate::interfaceFlags();
|
result |= pimpl_->deducedInterfaceFlags();
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -31,7 +31,20 @@ std::ostream& operator<<(std::ostream &os, const SubTask& stage) {
|
|||||||
os << *stage.pimpl_func();
|
os << *stage.pimpl_func();
|
||||||
return os;
|
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) {
|
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
|
||||||
|
|
||||||
// inputs
|
// inputs
|
||||||
for (const InterfacePtr &i : {stage.prevOutput(), stage.input_}) {
|
for (const InterfacePtr &i : {stage.prevOutput(), stage.input_}) {
|
||||||
os << std::setw(3);
|
os << std::setw(3);
|
||||||
@ -39,7 +52,9 @@ std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
|
|||||||
else os << "-";
|
else os << "-";
|
||||||
}
|
}
|
||||||
// trajectories
|
// 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
|
// outputs
|
||||||
for (const InterfacePtr &i : {stage.output_, stage.nextInput()}) {
|
for (const InterfacePtr &i : {stage.output_, stage.nextInput()}) {
|
||||||
os << std::setw(3);
|
os << std::setw(3);
|
||||||
@ -74,21 +89,23 @@ SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajec
|
|||||||
return trajectories_.back();
|
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;
|
SubTask::InterfaceFlags f;
|
||||||
if (input_) f |= SubTask::READS_INPUT;
|
if (input_) f |= SubTask::READS_INPUT;
|
||||||
if (output_) f |= SubTask::READS_OUTPUT;
|
if (output_) f |= SubTask::READS_OUTPUT;
|
||||||
|
if (prevOutput()) f |= SubTask::WRITES_PREV_OUTPUT;
|
||||||
|
if (nextInput()) f |= SubTask::WRITES_NEXT_INPUT;
|
||||||
return f;
|
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;
|
std::cout << "sending state to start" << std::endl;
|
||||||
nextInput()->add(ps, &trajectory, NULL);
|
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;
|
std::cout << "sending state to end" << std::endl;
|
||||||
prevOutput()->add(ps, NULL, &trajectory);
|
prevOutput()->add(ps, NULL, &trajectory);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -23,7 +23,8 @@ public:
|
|||||||
|
|
||||||
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
|
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 sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
|
||||||
void sendBackward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
|
void sendBackward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user