diff --git a/include/moveit_task_constructor/debug.h b/include/moveit_task_constructor/debug.h index 0d1d4e4f..1be8293a 100644 --- a/include/moveit_task_constructor/debug.h +++ b/include/moveit_task_constructor/debug.h @@ -3,6 +3,9 @@ #include #include +#include +#include + namespace moveit { namespace task_constructor { MOVEIT_CLASS_FORWARD(Task) @@ -13,4 +16,14 @@ bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true); +class NewSolutionPublisher { + std::set published; + const Task &task; + ros::Publisher pub; + +public: + NewSolutionPublisher(const Task &task, const std::string &topic = "task_plan"); + void publish(); +}; + } } diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index d7a12b94..8c0a14e8 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -42,7 +42,7 @@ public: bool plan(); - bool processSolutions(SolutionCallback& processor) const; + bool processSolutions(const SolutionCallback &processor) const; const moveit::core::RobotState &getCurrentRobotState() const; diff --git a/src/debug.cpp b/src/debug.cpp index 963e9cc9..bcc8ee56 100644 --- a/src/debug.cpp +++ b/src/debug.cpp @@ -39,4 +39,34 @@ void publishAllPlans(const Task &task, const std::string &topic, bool wait){ task.processSolutions(processor); } + +NewSolutionPublisher::NewSolutionPublisher(const Task &task, const std::string &topic) + : task(task) +{ + ros::NodeHandle n; + pub = n.advertise(topic, 1, true); +} + +void NewSolutionPublisher::publish() +{ + moveit_msgs::DisplayTrajectory dt; + robot_state::robotStateToRobotStateMsg(task.getCurrentRobotState(), dt.trajectory_start); + dt.model_id = task.getCurrentRobotState().getRobotModel()->getName(); + + auto processor = [this,&dt](std::vector& solution) -> bool { + bool all_published = true; + for(auto *t : solution){ + auto result = published.insert(t); + // if t was not yet published, the insertion yields result.second == true + all_published &= !result.second; + } + + if(all_published) + return true; + + return publishSolution(pub, dt, solution, false); + }; + task.processSolutions(processor); +} + } } diff --git a/src/task.cpp b/src/task.cpp index 394b92ac..b9437f6c 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -1,5 +1,6 @@ #include #include +#include #include @@ -56,6 +57,7 @@ void Task::clear(){ } bool Task::plan(){ + NewSolutionPublisher debug(*this); bool computed= true; while(ros::ok() && computed){ @@ -69,6 +71,7 @@ bool Task::plan(){ std::cout << (success ? "succeeded" : "failed") << std::endl; } if(computed){ + debug.publish(); printState(); } } @@ -106,7 +109,7 @@ namespace { bool traverseFullTrajectories( SubTrajectory& start, int nr_of_trajectories, - Task::SolutionCallback& cb, + const Task::SolutionCallback& cb, std::vector& trace) { bool ret= true; @@ -131,7 +134,7 @@ bool traverseFullTrajectories( } } -bool Task::processSolutions(Task::SolutionCallback& processor) const { +bool Task::processSolutions(const Task::SolutionCallback& processor) const { const size_t nr_of_trajectories= subtasks_.size(); std::vector trace; trace.reserve(nr_of_trajectories);