asynchronous execute and stop command

edit

Hi all,

I am using MoveGroupInterface to make simple motion plans for a ur5 robotic arm. I have some queries concerning different motion commands, in particular move, execute, and asyncExecute. I understand that move, and execute commands are blocking meaning that a planned trajectory will complete its execution and it cannot be interrupted using a stop command. On the other hand, I could do something like the following:

// initial pause before starting the movement
 sleep(5.0);
move_group.asyncExecute(my_plan);
sleep(2);
move_group.stop();

The above code will stop the movement after 2 seconds into the motion. Such a functionality is useful so that based on external inputs, the motion can be stopped and a new target could be sent. This is particularly useful in case of dealing with dynamic obstacles.

Now, I have written a separate node, which keeps on publishing random integer values in the range (1, 100) on a topic called /stop_command_control. In my motion planning node, I subscribe to that topic and I have a callback where I can receive and inspect that value just published. My initial idea was that whenever I have the received number in a certain range such as (43, 67), I will issue a stop command. This exercise was to emulate an event-triggered behavior.

This is how my code looks.

int SimpleMotionPlanning::control_number=0;

    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);
      control_number = val.data;
    }

then, I modify my motion control code as follows:

 // initial pause before starting the movement
  sleep(5.0);
  move_group.asyncExecute(my_plan);

//  sleep(2);

  ROS_INFO("Received  %d", control_number);
  if (((control_number-43) <= (67-43)))
    move_group.stop();

But, I have observed that this does not take effect and the motion always executes to completion, even when the received random number is in the given range.

Please can you advice me, how to issue the stop command based on certain input received.

thanks,

Zahid