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

Add function to Introspection class to retrieve all solutions #510

Closed
wants to merge 1 commit into from
Closed

Add function to Introspection class to retrieve all solutions #510

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
4 changes: 4 additions & 0 deletions core/include/moveit/task_constructor/introspection.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,10 @@ class Introspection
/// fill task state message for publishing the current task state
moveit_task_constructor_msgs::msg::TaskStatistics&
fillTaskStatistics(moveit_task_constructor_msgs::msg::TaskStatistics& msg);

/// Function to get a map between solution id and solutions.
std::map<uint32_t, moveit_task_constructor_msgs::msg::Solution> getAllSolutions();

/// publish the current state of task
void publishTaskState();

Expand Down
18 changes: 14 additions & 4 deletions core/src/introspection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -161,7 +161,7 @@ class IntrospectionPrivate

/// mapping from stages to their id
std::map<const StagePrivate*, moveit_task_constructor_msgs::msg::StageStatistics::_id_type> stage_to_id_map_;
boost::bimap<uint32_t, const SolutionBase*> id_solution_bimap_;
std::map<uint32_t, const SolutionBase*> id_solution_bimap_;
};

Introspection::Introspection(const TaskPrivate* task) : impl(new IntrospectionPrivate(task, this)) {}
Expand Down Expand Up @@ -213,9 +213,19 @@ void Introspection::publishAllSolutions(bool wait) {
};
}

std::map<uint32_t, moveit_task_constructor_msgs::msg::Solution> Introspection::getAllSolutions() {
std::map<uint32_t, moveit_task_constructor_msgs::msg::Solution> solution_id_map = {};
for (const auto& [solution_id, solution_base] : impl->id_solution_bimap_) {
moveit_task_constructor_msgs::msg::Solution solution;
fillSolution(solution, *solution_base);
solution_id_map[solution_id] = solution;
}
return solution_id_map;
}

const SolutionBase* Introspection::solutionFromId(uint id) const {
auto it = impl->id_solution_bimap_.left.find(id);
if (it == impl->id_solution_bimap_.left.end())
auto it = impl->id_solution_bimap_.find(id);
if (it == impl->id_solution_bimap_.end())
return nullptr;
return it->second;
}
Expand All @@ -241,7 +251,7 @@ uint32_t Introspection::stageId(const Stage* const s) const {
}

uint32_t Introspection::solutionId(const SolutionBase& s) {
auto result = impl->id_solution_bimap_.left.insert(std::make_pair(1 + impl->id_solution_bimap_.size(), &s));
auto result = impl->id_solution_bimap_.insert(std::make_pair(1 + impl->id_solution_bimap_.size(), &s));
return result.first->first;
}

Expand Down
Loading