diff --git a/include/moveit_task_constructor/debug.h b/include/moveit_task_constructor/debug.h index 656bc03d..57c7ed2b 100644 --- a/include/moveit_task_constructor/debug.h +++ b/include/moveit_task_constructor/debug.h @@ -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); }; } } diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index 9461f22a..c2295360 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -32,8 +32,10 @@ public: using SerialContainer::clear; typedef std::function SolutionCallback; - /// set function called for every newly found solution - SolutionCallback setSolutionCallback(const SolutionCallback &cb); + typedef std::list 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(); diff --git a/src/task.cpp b/src/task.cpp index e679b05c..ead985e7 100644 --- a/src/task.cpp +++ b/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 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();