Task: allow multiple solution callbacks

This commit is contained in:
Robert Haschke 2017-10-20 05:52:23 +02:00
parent 97bed73a9a
commit b944450736
3 changed files with 20 additions and 12 deletions

View File

@ -14,11 +14,11 @@ MOVEIT_CLASS_FORWARD(SolutionBase)
void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true); void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true);
class NewSolutionPublisher { class NewSolutionPublisher {
ros::Publisher pub_; ros::Publisher pub_;
public: public:
NewSolutionPublisher(const std::string &topic = "task_plan"); NewSolutionPublisher(const std::string &topic = "task_plan");
void operator()(const SolutionBase &s); void operator()(const SolutionBase &s);
}; };
} } } }

View File

@ -32,8 +32,10 @@ public:
using SerialContainer::clear; using SerialContainer::clear;
typedef std::function<void(const SolutionBase &s)> SolutionCallback; typedef std::function<void(const SolutionBase &s)> SolutionCallback;
/// set function called for every newly found solution typedef std::list<SolutionCallback> SolutionCallbackList;
SolutionCallback setSolutionCallback(const SolutionCallback &cb); /// add function to be called for every newly found solution
SolutionCallbackList::const_iterator add(SolutionCallback &&cb);
void erase(SolutionCallbackList::const_iterator which);
bool plan(); bool plan();
void printState(); void printState();

View File

@ -18,7 +18,7 @@ class TaskPrivate : public SerialContainerPrivate {
robot_model_loader::RobotModelLoaderPtr rml_; robot_model_loader::RobotModelLoaderPtr rml_;
planning_scene::PlanningSceneConstPtr scene_; // initial scene 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: public:
TaskPrivate(Task* me, const std::string &name) TaskPrivate(Task* me, const std::string &name)
@ -68,7 +68,8 @@ public:
void TaskPrivate::onNewSolution(SolutionBase &s) void TaskPrivate::onNewSolution(SolutionBase &s)
{ {
SerialContainerPrivate::onNewSolution(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()); 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(); auto impl = pimpl();
SolutionCallback old = impl->cb_; pimpl()->callbacks_.erase(which);
impl->cb_ = cb;
return old;
} }
bool Task::plan(){ bool Task::plan(){
auto impl = pimpl(); auto impl = pimpl();
setSolutionCallback(NewSolutionPublisher()); add(NewSolutionPublisher());
reset(); reset();
impl->initScene(); impl->initScene();