diff --git a/moveit_studio_py/examples/start_stop_async.py b/moveit_studio_py/examples/start_stop_async.py index 8291a06..4f19918 100755 --- a/moveit_studio_py/examples/start_stop_async.py +++ b/moveit_studio_py/examples/start_stop_async.py @@ -38,6 +38,7 @@ from moveit_msgs.msg import MoveItErrorCodes from moveit_studio_py.objective_manager import ObjectiveManager +done = False def done_cb(future: rclpy.task.Future) -> None: """ @@ -51,6 +52,8 @@ def done_cb(future: rclpy.task.Future) -> None: print("Objective executed successfully!") else: print(f"MoveItErrorCode Value: {result.error_code.val}") + global done + done = True def main(): @@ -74,14 +77,18 @@ def main(): objective_manager = ObjectiveManager() print(f"Starting {args.objective_name}.") + global done + done = False objective_manager.start_objective( args.objective_name, blocking=False, async_callback=done_cb ) + cancel_time = time.time() + args.wait_time + while not done and time.time() < cancel_time: + time.sleep(1) - time.sleep(args.wait_time) - - print(f"Stopping {args.objective_name}.") - objective_manager.stop_objective() + if not done: + print(f"Stopping {args.objective_name}.") + objective_manager.stop_objective() rclpy.shutdown()