diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index 77e1e74c..e4a1beb4 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -59,8 +59,6 @@ protected: robot_model_loader::RobotModelLoaderPtr rml_; planning_pipeline::PlanningPipelinePtr planner_; - - ros::Publisher pub; }; } } diff --git a/src/task.cpp b/src/task.cpp index 4a76847c..3ccdfe80 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -18,8 +18,6 @@ moveit::task_constructor::Task::Task(){ ros::NodeHandle h; - pub = h.advertise("task_plan", 30); - ros::ServiceClient client = h.serviceClient("get_planning_scene"); client.waitForExistence(); @@ -72,7 +70,6 @@ bool moveit::task_constructor::Task::plan(){ } if(computed){ printState(); - publishPlans(); } } return false; @@ -142,31 +139,25 @@ bool moveit::task_constructor::Task::processSolutions(moveit::task_constructor:: namespace { bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, std::vector& solution){ - bool all_flagged= true; - for(auto& t : solution){ - all_flagged= all_flagged && t->flag; - } - - if( all_flagged ){ - return true; - } - + dt.trajectory.clear(); for(auto& t : solution){ if(t->trajectory){ dt.trajectory.emplace_back(); t->trajectory->getRobotTrajectoryMsg(dt.trajectory.back()); } - t->flag= true; } std::cout << "publishing solution" << std::endl; pub.publish(dt); - + std::cout << "Press to continue ..." << std::endl; + getchar(); return true; } } void moveit::task_constructor::Task::publishPlans(){ + ros::NodeHandle n; + ros::Publisher pub = n.advertise("task_plan", 1, true); moveit_msgs::DisplayTrajectory dt; robot_state::robotStateToRobotStateMsg(scene_->getCurrentState(), dt.trajectory_start); dt.model_id= scene_->getRobotModel()->getName(); @@ -175,5 +166,4 @@ void moveit::task_constructor::Task::publishPlans(){ &publishSolution, pub, dt, std::placeholders::_1); processSolutions(processor); - }