mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
fix publishing plans
This commit is contained in:
parent
2dfc2f395e
commit
a5b7f0c2f3
@ -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;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
20
src/task.cpp
20
src/task.cpp
@ -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);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user