following stop command, asynExecute does not take effect

edit

Hi,

I am using MoveIt MoveGroupInterface to plan and carry out simple motions, pose goals and joint-space goals. I want to test another functionality. Based on some external event, I want to stop the movement, pause and then resume it. Here is my code.

A publisher that publishes random integers in the range (1, 100) on a topic /stop_command_control .

    ros::Publisher random_number_pub = nh.advertise<std_msgs::Int32>("/stop_command_control",1000);

ros::Rate loop_rate(0.5);
while(ros::ok()){

    std_msgs::Int32 my_number;

    my_number.data = rng.uniformInteger(1, 100);

    ROS_INFO("printing  %d", my_number.data);

    sleep(2);
    random_number_pub.publish(my_number);

    ros::spinOnce();
    loop_rate.sleep();
  }

The subscriber

    random_number_sub = nh.subscribe("/stop_command_control", 1000, &SimpleMotionPlanning::stopHandler, this);

void SimpleMotionPlanning::stopHandler(const std_msgs::Int32 &val) {

  ROS_INFO("Received  %d", val.data);


  if ((val.data <= 67) && (val.data >= 43)) {

    if (control_number < 1) {

      move_group.stop();

      move_group.setStartStateToCurrentState();

      this->goToJointStateExp();

      control_number += 1;
    }
  }
}


void SimpleMotionPlanning::goToJointStateExp() {
  bool success;
  std::vector<double> jointVals;
  double refVals[] =  {-1.8963, -0.5800, -2.2339, -1.8976, 1.5629, 2.0322};

  for (int j=0; j<6; j++) {
    jointVals.push_back(refVals[j]);
  }
  move_group.setJointValueTarget(jointVals);
  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

  if (!success) {
    throw std::runtime_error("No plan found");
  } else {

    ROS_INFO("successfully computed the trajectory to the desired goal configuration");
  }


  // initial pause before starting the movement
//  sleep(5.0);

  // the following two commands cause a blocking motion,
  // you cannot preempt the motion
  // move_group.move();               //blocking

  move_group.asyncMove();
}

I run my program by specifying the original target as given by function goToJointStateExp() or another function that moves the robotic arm to a pose goal. At the same time, I am receiving random inputs on /stop_command_control. This causes the movement to stop whenever the number received is in the range [43, 67]. However, after that it does not plan and execute the motion as specified by a call to this->goToJointStateExp(); in stopHandler callback. I had tested with move_group.asynExecute but the robotic arm does not move after the stop command. Please can someone explain the problem.

thanks, zahid