mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
restore online publishing of new solutions
This commit is contained in:
parent
6d10f7e835
commit
2d5797d741
@ -3,6 +3,9 @@
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <moveit_msgs/DisplayTrajectory.h>
|
||||
|
||||
#include <ros/publisher.h>
|
||||
#include <set>
|
||||
|
||||
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<SubTrajectory*> published;
|
||||
const Task &task;
|
||||
ros::Publisher pub;
|
||||
|
||||
public:
|
||||
NewSolutionPublisher(const Task &task, const std::string &topic = "task_plan");
|
||||
void publish();
|
||||
};
|
||||
|
||||
} }
|
||||
|
||||
@ -42,7 +42,7 @@ public:
|
||||
|
||||
bool plan();
|
||||
|
||||
bool processSolutions(SolutionCallback& processor) const;
|
||||
bool processSolutions(const SolutionCallback &processor) const;
|
||||
|
||||
const moveit::core::RobotState &getCurrentRobotState() const;
|
||||
|
||||
|
||||
@ -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<moveit_msgs::DisplayTrajectory>(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<SubTrajectory*>& 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);
|
||||
}
|
||||
|
||||
} }
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
#include <moveit_task_constructor/task.h>
|
||||
#include <moveit_task_constructor/subtask.h>
|
||||
#include <moveit_task_constructor/debug.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
@ -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<SubTrajectory*>& 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<SubTrajectory*> trace;
|
||||
trace.reserve(nr_of_trajectories);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user