Controllers failing on using asyncExecute()

asked 2021-05-19 11:35:43 -0600

shoriya gravatar image

updated 2021-05-20 00:01:51 -0600

jayess gravatar image

Hello All,

I am trying to stop a trajectory while monitoring its status by subscribing to "/execute_trajectory/result" topic. But it would not execute after the first first goal pose. I am not sure what the issue is. Could some please help.

Bellow is a snippet of my code

void planning(moveit::planning_interface::MoveGroupInterface& group, ros::NodeHandle& node_handle)
{ 
  const robot_state::JointModelGroup* joint_model_group = group.getCurrentState()->getJointModelGroup("manipulator");

  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  moveit_msgs::MotionPlanRequest response;

  moveit::core::RobotStatePtr current_state;
  std::vector<double> joint_group_positions;
  bool success;

  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  current_state = group.getCurrentState();
  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

  //Pick up loaction
  geometry_msgs::Pose pickup_pose;
  pickup_pose.orientation.w = 1;
  pickup_pose.orientation.x = 0;
  pickup_pose.orientation.y = 0;
  pickup_pose.orientation.z = 0;

  pickup_pose.position.x = 0.198236761301;
  pickup_pose.position.y = -0.113148163157;
  pickup_pose.position.z = 0.0858897188011;


  group.setPoseTarget(pickup_pose);

  group.setStartStateToCurrentState();
  group.setApproximateJointValueTarget(pickup_pose,"Link6");

   success = (group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);  
  group.asyncExecute(my_plan);  
  while(execute_status != 3)
  {
    if(execute_status==3)
    {ROS_INFO("execute status %i",execute_status);}

  } 
  ROS_INFO("execute status %i",execute_status);
  ROS_INFO("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%POSITION-1 DONE%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"); 

  current_state = group.getCurrentState();
  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

  //Pick up loaction

  pickup_pose.orientation.w = 1;
  pickup_pose.orientation.x = 0;
  pickup_pose.orientation.y = 0;
  pickup_pose.orientation.z = 0;

  pickup_pose.position.x = 0.198231888501;
  pickup_pose.position.y = 0.0937851806414;
  pickup_pose.position.z = 0.08588867484;


  group.setPoseTarget(pickup_pose);

  group.setStartStateToCurrentState();
  group.setApproximateJointValueTarget(pickup_pose,"Link6");

  success = (group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);  
  group.asyncExecute(my_plan);  
  while(execute_status != 3)
  {
    if(execute_status==3)
    {ROS_INFO("execute status %i",execute_status);}

  } 
    ROS_INFO("execute status %i",execute_status);
   ROS_INFO("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%POSITION-2 DONE%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"); 

  group.setNamedTarget("home");

  success = (group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);  
  group.asyncExecute(my_plan);
  while(execute_status != 3)
  {
    if(execute_status==3)
    {ROS_INFO("execute status %i",execute_status);}

  } 
  ROS_INFO("execute status %i",execute_status);
  ROS_INFO("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%POSITION-3 DONE%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"); 

  current_state = group.getCurrentState();
  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

  //Pick up loaction

  pickup_pose.orientation.w = 1;
  pickup_pose.orientation.x = 0;
  pickup_pose.orientation.y = 0;
  pickup_pose.orientation.z = 0;

  pickup_pose.position.x = 0.198231888501;
  pickup_pose.position.y = 0.0937851806414;
  pickup_pose.position.z = 0.08588867484;

  group.setPoseTarget(pickup_pose);

  group.setStartStateToCurrentState();
  group.setApproximateJointValueTarget(pickup_pose,"Link6");

  success = (group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);  
  group.asyncExecute(my_plan);  
  while(execute_status != 3)
  {
    if(execute_status==3)
    {ROS_INFO("execute status %i",execute_status);}

  } 
  ROS_INFO("execute status %i",execute_status);
  ROS_INFO("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%POSITION-4 DONE%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"); 


}

image description

edit retag flag offensive close merge delete