mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
commit
bb1fcad44e
@ -17,7 +17,7 @@ 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_;
|
||||
std::set<const SubTrajectory*> published_;
|
||||
const Task &task_;
|
||||
ros::Publisher pub_;
|
||||
|
||||
|
||||
@ -10,7 +10,7 @@ namespace moveit { namespace task_constructor {
|
||||
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
|
||||
std::vector<SubTrajectory*>& solution, bool wait){
|
||||
dt.trajectory.clear();
|
||||
for(SubTrajectory*& t : solution){
|
||||
for(const SubTrajectory* t : solution){
|
||||
if(t->trajectory){
|
||||
dt.trajectory.emplace_back();
|
||||
t->trajectory->getRobotTrajectoryMsg(dt.trajectory.back());
|
||||
@ -55,7 +55,7 @@ void NewSolutionPublisher::publish()
|
||||
|
||||
Task::SolutionCallback processor = [this,&dt](std::vector<SubTrajectory*>& solution) {
|
||||
bool all_published = true;
|
||||
for(SubTrajectory*& t : solution){
|
||||
for(const SubTrajectory* t : solution){
|
||||
auto result = published_.insert(t);
|
||||
// if t was not yet published, the insertion yields result.second == true
|
||||
all_published &= !result.second;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user