fix publishing plans

This commit is contained in:
Robert Haschke 2017-09-27 22:46:03 -07:00
parent 2dfc2f395e
commit a5b7f0c2f3
2 changed files with 5 additions and 17 deletions

View File

@ -59,8 +59,6 @@ protected:
robot_model_loader::RobotModelLoaderPtr rml_; robot_model_loader::RobotModelLoaderPtr rml_;
planning_pipeline::PlanningPipelinePtr planner_; planning_pipeline::PlanningPipelinePtr planner_;
ros::Publisher pub;
}; };
} } } }

View File

@ -18,8 +18,6 @@ moveit::task_constructor::Task::Task(){
ros::NodeHandle h; ros::NodeHandle h;
pub = h.advertise<moveit_msgs::DisplayTrajectory>("task_plan", 30);
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene"); ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
client.waitForExistence(); client.waitForExistence();
@ -72,7 +70,6 @@ bool moveit::task_constructor::Task::plan(){
} }
if(computed){ if(computed){
printState(); printState();
publishPlans();
} }
} }
return false; return false;
@ -142,31 +139,25 @@ bool moveit::task_constructor::Task::processSolutions(moveit::task_constructor::
namespace { namespace {
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, std::vector<moveit::task_constructor::SubTrajectory*>& solution){ bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, std::vector<moveit::task_constructor::SubTrajectory*>& solution){
bool all_flagged= true; dt.trajectory.clear();
for(auto& t : solution){
all_flagged= all_flagged && t->flag;
}
if( all_flagged ){
return true;
}
for(auto& t : solution){ for(auto& t : solution){
if(t->trajectory){ if(t->trajectory){
dt.trajectory.emplace_back(); dt.trajectory.emplace_back();
t->trajectory->getRobotTrajectoryMsg(dt.trajectory.back()); t->trajectory->getRobotTrajectoryMsg(dt.trajectory.back());
} }
t->flag= true;
} }
std::cout << "publishing solution" << std::endl; std::cout << "publishing solution" << std::endl;
pub.publish(dt); pub.publish(dt);
std::cout << "Press <Enter> to continue ..." << std::endl;
getchar();
return true; return true;
} }
} }
void moveit::task_constructor::Task::publishPlans(){ void moveit::task_constructor::Task::publishPlans(){
ros::NodeHandle n;
ros::Publisher pub = n.advertise<moveit_msgs::DisplayTrajectory>("task_plan", 1, true);
moveit_msgs::DisplayTrajectory dt; moveit_msgs::DisplayTrajectory dt;
robot_state::robotStateToRobotStateMsg(scene_->getCurrentState(), dt.trajectory_start); robot_state::robotStateToRobotStateMsg(scene_->getCurrentState(), dt.trajectory_start);
dt.model_id= scene_->getRobotModel()->getName(); dt.model_id= scene_->getRobotModel()->getName();
@ -175,5 +166,4 @@ void moveit::task_constructor::Task::publishPlans(){
&publishSolution, pub, dt, std::placeholders::_1); &publishSolution, pub, dt, std::placeholders::_1);
processSolutions(processor); processSolutions(processor);
} }