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);
|
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);
|
||||||
};
|
};
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -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();
|
||||||
|
|||||||
20
src/task.cpp
20
src/task.cpp
@ -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();
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user