Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[WIP] Quick access of stages by id/index #626

Closed
wants to merge 1 commit into from
Closed

[WIP] Quick access of stages by id/index #626

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions core/include/moveit/task_constructor/task.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,6 +98,9 @@ class Task : protected WrapperBase
void insert(Stage::pointer&& stage, int before = -1) override;
void clear() final;

/// creates a flat buffer of stages in breath-first search order
void fillFlatBufferStages();

/// enable introspection publishing for use with rviz
void enableIntrospection(bool enable = true);
Introspection& introspection();
Expand Down Expand Up @@ -150,6 +153,7 @@ class Task : protected WrapperBase
/// access stage tree
ContainerBase* stages();
const ContainerBase* stages() const;
const ContainerBase* stages(uint32_t stage_id) const;

/// properties access
PropertyMap& properties();
Expand All @@ -165,6 +169,8 @@ class Task : protected WrapperBase

private:
using WrapperBase::init;

std::vector<const ContainerBase*> bfs_stages_; // breadth-first search
};

inline std::ostream& operator<<(std::ostream& os, const Task& task) {
Expand Down
1 change: 1 addition & 0 deletions core/include/moveit/task_constructor/task_p.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ class TaskPrivate : public WrapperBasePrivate

const std::string& ns() const { return ns_; }
const ContainerBase* stages() const;
const ContainerBase* stages(uint32_t stage_id) const;

private:
std::string ns_;
Expand Down
16 changes: 16 additions & 0 deletions core/src/task.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,13 @@ const ContainerBase* TaskPrivate::stages() const {
return children().empty() ? nullptr : static_cast<ContainerBase*>(children().front().get());
}

const ContainerBase* Task::stages(uint32_t stage_id) const {
if (stage_id < bfs_stages_.size()) {
return bfs_stages_[stage_id];
}
return nullptr;
}

Task::Task(const std::string& ns, bool introspection, ContainerBase::pointer&& container)
: WrapperBase(new TaskPrivate(this, ns), std::move(container)) {
setPruning(false);
Expand Down Expand Up @@ -143,6 +150,15 @@ void Task::loadRobotModel(const std::string& robot_description) {
throw Exception("Task failed to construct RobotModel");
}

void Task::fillFlatBufferStages() {
bfs_stages_.clear();
auto stage_cb = [&](const moveit::task_constructor::Stage& stage, unsigned int depth) -> bool {
bfs_stages_.push_back(static_cast<const ContainerBase*>(&stage));
return true;
};
stages()->traverseRecursively(stage_cb);
}

void Task::add(Stage::pointer&& stage) {
stages()->add(std::move(stage));
}
Expand Down
Loading