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);
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);
};
} }

View File

@ -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();

View File

@ -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();