mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branch 'master' into fix
This commit is contained in:
commit
7641e004d1
@ -17,9 +17,9 @@ 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 {
|
class NewSolutionPublisher {
|
||||||
std::set<const SubTrajectory*> published;
|
std::set<const SubTrajectory*> published_;
|
||||||
const Task &task;
|
const Task &task_;
|
||||||
ros::Publisher pub;
|
ros::Publisher pub_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
NewSolutionPublisher(const Task &task, const std::string &topic = "task_plan");
|
NewSolutionPublisher(const Task &task, const std::string &topic = "task_plan");
|
||||||
|
|||||||
@ -41,22 +41,22 @@ void publishAllPlans(const Task &task, const std::string &topic, bool wait){
|
|||||||
|
|
||||||
|
|
||||||
NewSolutionPublisher::NewSolutionPublisher(const Task &task, const std::string &topic)
|
NewSolutionPublisher::NewSolutionPublisher(const Task &task, const std::string &topic)
|
||||||
: task(task)
|
: task_(task)
|
||||||
{
|
{
|
||||||
ros::NodeHandle n;
|
ros::NodeHandle n;
|
||||||
pub = n.advertise<moveit_msgs::DisplayTrajectory>(topic, 1, true);
|
pub_ = n.advertise<moveit_msgs::DisplayTrajectory>(topic, 1, true);
|
||||||
}
|
}
|
||||||
|
|
||||||
void NewSolutionPublisher::publish()
|
void NewSolutionPublisher::publish()
|
||||||
{
|
{
|
||||||
moveit_msgs::DisplayTrajectory dt;
|
moveit_msgs::DisplayTrajectory dt;
|
||||||
robot_state::robotStateToRobotStateMsg(task.getCurrentRobotState(), dt.trajectory_start);
|
robot_state::robotStateToRobotStateMsg(task_.getCurrentRobotState(), dt.trajectory_start);
|
||||||
dt.model_id = task.getCurrentRobotState().getRobotModel()->getName();
|
dt.model_id = task_.getCurrentRobotState().getRobotModel()->getName();
|
||||||
|
|
||||||
Task::SolutionCallback processor = [this,&dt](std::vector<SubTrajectory*>& solution) {
|
Task::SolutionCallback processor = [this,&dt](std::vector<SubTrajectory*>& solution) {
|
||||||
bool all_published = true;
|
bool all_published = true;
|
||||||
for(const SubTrajectory* t : solution){
|
for(const SubTrajectory* t : solution){
|
||||||
auto result = published.insert(t);
|
auto result = published_.insert(t);
|
||||||
// if t was not yet published, the insertion yields result.second == true
|
// if t was not yet published, the insertion yields result.second == true
|
||||||
all_published &= !result.second;
|
all_published &= !result.second;
|
||||||
}
|
}
|
||||||
@ -64,9 +64,9 @@ void NewSolutionPublisher::publish()
|
|||||||
if(all_published)
|
if(all_published)
|
||||||
return true;
|
return true;
|
||||||
|
|
||||||
return publishSolution(pub, dt, solution, false);
|
return publishSolution(pub_, dt, solution, false);
|
||||||
};
|
};
|
||||||
task.processSolutions(processor);
|
task_.processSolutions(processor);
|
||||||
}
|
}
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user