mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Task: allow multiple solution callbacks
This commit is contained in:
parent
97bed73a9a
commit
b944450736
@ -14,11 +14,11 @@ MOVEIT_CLASS_FORWARD(SolutionBase)
|
||||
void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true);
|
||||
|
||||
class NewSolutionPublisher {
|
||||
ros::Publisher pub_;
|
||||
ros::Publisher pub_;
|
||||
|
||||
public:
|
||||
NewSolutionPublisher(const std::string &topic = "task_plan");
|
||||
void operator()(const SolutionBase &s);
|
||||
NewSolutionPublisher(const std::string &topic = "task_plan");
|
||||
void operator()(const SolutionBase &s);
|
||||
};
|
||||
|
||||
} }
|
||||
|
||||
@ -32,8 +32,10 @@ public:
|
||||
using SerialContainer::clear;
|
||||
|
||||
typedef std::function<void(const SolutionBase &s)> SolutionCallback;
|
||||
/// set function called for every newly found solution
|
||||
SolutionCallback setSolutionCallback(const SolutionCallback &cb);
|
||||
typedef std::list<SolutionCallback> SolutionCallbackList;
|
||||
/// add function to be called for every newly found solution
|
||||
SolutionCallbackList::const_iterator add(SolutionCallback &&cb);
|
||||
void erase(SolutionCallbackList::const_iterator which);
|
||||
|
||||
bool plan();
|
||||
void printState();
|
||||
|
||||
20
src/task.cpp
20
src/task.cpp
@ -18,7 +18,7 @@ class TaskPrivate : public SerialContainerPrivate {
|
||||
|
||||
robot_model_loader::RobotModelLoaderPtr rml_;
|
||||
planning_scene::PlanningSceneConstPtr scene_; // initial scene
|
||||
Task::SolutionCallback cb_; // function called for each new solution
|
||||
std::list<Task::SolutionCallback> callbacks_; // functions called for each new solution
|
||||
|
||||
public:
|
||||
TaskPrivate(Task* me, const std::string &name)
|
||||
@ -68,7 +68,8 @@ public:
|
||||
void TaskPrivate::onNewSolution(SolutionBase &s)
|
||||
{
|
||||
SerialContainerPrivate::onNewSolution(s);
|
||||
if (cb_) cb_(s);
|
||||
for (const auto& cb : callbacks_)
|
||||
cb(s);
|
||||
}
|
||||
|
||||
|
||||
@ -106,17 +107,22 @@ void Task::add(pointer &&stage) {
|
||||
throw std::runtime_error(std::string("insertion failed for stage: ") + stage->name());
|
||||
}
|
||||
|
||||
Task::SolutionCallback Task::setSolutionCallback(const Task::SolutionCallback &cb)
|
||||
Task::SolutionCallbackList::const_iterator Task::add(SolutionCallback &&cb)
|
||||
{
|
||||
auto& callbacks = pimpl()->callbacks_;
|
||||
callbacks.emplace_back(std::move(cb));
|
||||
return --callbacks.cend();
|
||||
}
|
||||
|
||||
void Task::erase(SolutionCallbackList::const_iterator which)
|
||||
{
|
||||
auto impl = pimpl();
|
||||
SolutionCallback old = impl->cb_;
|
||||
impl->cb_ = cb;
|
||||
return old;
|
||||
pimpl()->callbacks_.erase(which);
|
||||
}
|
||||
|
||||
bool Task::plan(){
|
||||
auto impl = pimpl();
|
||||
setSolutionCallback(NewSolutionPublisher());
|
||||
add(NewSolutionPublisher());
|
||||
|
||||
reset();
|
||||
impl->initScene();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user