diff --git a/include/moveit_task_constructor/debug.h b/include/moveit_task_constructor/debug.h index 7ce96872..7f429de3 100644 --- a/include/moveit_task_constructor/debug.h +++ b/include/moveit_task_constructor/debug.h @@ -17,8 +17,8 @@ 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_; + std::set published_; + const Task &task_; ros::Publisher pub_; public: diff --git a/src/debug.cpp b/src/debug.cpp index 7554c4c8..8335a457 100644 --- a/src/debug.cpp +++ b/src/debug.cpp @@ -10,7 +10,7 @@ namespace moveit { namespace task_constructor { bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, std::vector& 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& 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;