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/macros/class_forward.h>
|
||||||
#include <moveit_msgs/DisplayTrajectory.h>
|
#include <moveit_msgs/DisplayTrajectory.h>
|
||||||
|
|
||||||
|
#include <ros/publisher.h>
|
||||||
|
#include <set>
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(Task)
|
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);
|
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 plan();
|
||||||
|
|
||||||
bool processSolutions(SolutionCallback& processor) const;
|
bool processSolutions(const SolutionCallback &processor) const;
|
||||||
|
|
||||||
const moveit::core::RobotState &getCurrentRobotState() 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);
|
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/task.h>
|
||||||
#include <moveit_task_constructor/subtask.h>
|
#include <moveit_task_constructor/subtask.h>
|
||||||
|
#include <moveit_task_constructor/debug.h>
|
||||||
|
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
|
|
||||||
@ -56,6 +57,7 @@ void Task::clear(){
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool Task::plan(){
|
bool Task::plan(){
|
||||||
|
NewSolutionPublisher debug(*this);
|
||||||
|
|
||||||
bool computed= true;
|
bool computed= true;
|
||||||
while(ros::ok() && computed){
|
while(ros::ok() && computed){
|
||||||
@ -69,6 +71,7 @@ bool Task::plan(){
|
|||||||
std::cout << (success ? "succeeded" : "failed") << std::endl;
|
std::cout << (success ? "succeeded" : "failed") << std::endl;
|
||||||
}
|
}
|
||||||
if(computed){
|
if(computed){
|
||||||
|
debug.publish();
|
||||||
printState();
|
printState();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -106,7 +109,7 @@ namespace {
|
|||||||
bool traverseFullTrajectories(
|
bool traverseFullTrajectories(
|
||||||
SubTrajectory& start,
|
SubTrajectory& start,
|
||||||
int nr_of_trajectories,
|
int nr_of_trajectories,
|
||||||
Task::SolutionCallback& cb,
|
const Task::SolutionCallback& cb,
|
||||||
std::vector<SubTrajectory*>& trace)
|
std::vector<SubTrajectory*>& trace)
|
||||||
{
|
{
|
||||||
bool ret= true;
|
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();
|
const size_t nr_of_trajectories= subtasks_.size();
|
||||||
std::vector<SubTrajectory*> trace;
|
std::vector<SubTrajectory*> trace;
|
||||||
trace.reserve(nr_of_trajectories);
|
trace.reserve(nr_of_trajectories);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user