def test_multi_goal_accept(self): executor = MultiThreadedExecutor(context=self.context) action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=self.execute_goal_callback, callback_group=ReentrantCallbackGroup(), handle_accepted_callback=lambda gh: None, ) goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) future0 = self.mock_action_client.send_goal(goal_msg) goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) future1 = self.mock_action_client.send_goal(goal_msg) goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) future2 = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, future0, executor) rclpy.spin_until_future_complete(self.node, future1, executor) rclpy.spin_until_future_complete(self.node, future2, executor) self.assertTrue(future0.result().accepted) self.assertTrue(future1.result().accepted) self.assertTrue(future2.result().accepted) action_server.destroy()
def test_send_goal_async_with_feedback_for_another_goal(self): ac = ActionClient(self.node, Fibonacci, 'fibonacci') try: self.assertTrue(ac.wait_for_server(timeout_sec=2.0)) # Send a goal and then publish feedback first_goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) future = ac.send_goal_async( Fibonacci.Goal(), feedback_callback=self.feedback_callback, goal_uuid=first_goal_uuid) rclpy.spin_until_future_complete(self.node, future, self.executor) # Send another goal, but without a feedback callback second_goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) future = ac.send_goal_async(Fibonacci.Goal(), goal_uuid=second_goal_uuid) rclpy.spin_until_future_complete(self.node, future, self.executor) # Publish feedback for the second goal self.mock_action_server.publish_feedback(second_goal_uuid) self.timed_spin(1.0) self.assertEqual(self.feedback, None) # Publish feedback for the first goal (with callback) self.mock_action_server.publish_feedback(first_goal_uuid) self.timed_spin(1.0) self.assertNotEqual(self.feedback, None) finally: ac.destroy()
def test_expire_goals_multi(self): # 1 second timeout action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=self.execute_goal_callback, result_timeout=1, ) # Send multiple goals goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) self.mock_action_client.send_goal(goal_msg) goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) self.mock_action_client.send_goal(goal_msg) goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) self.mock_action_client.send_goal(goal_msg) self.timed_spin(0.5) self.assertEqual(3, len(action_server._goal_handles)) # After two seconds the internal goal handles should be destroyed self.timed_spin(2.1) self.assertEqual(0, len(action_server._goal_handles)) action_server.destroy()
def test_execute_no_terminal_state(self): def execute_callback(goal_handle): # Do not set the goal handles state result = Fibonacci.Result() result.sequence.extend([1, 1, 2, 3, 5]) return result action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=execute_callback, ) goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = goal_uuid goal_future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, goal_future, self.executor) goal_handle = goal_future.result() self.assertTrue(goal_handle.accepted) get_result_future = self.mock_action_client.get_result(goal_uuid) rclpy.spin_until_future_complete(self.node, get_result_future, self.executor) result_response = get_result_future.result() # Goal status should default to STATUS_ABORTED self.assertEqual(result_response.status, GoalStatus.STATUS_ABORTED) self.assertEqual(result_response.result.sequence.tolist(), [1, 1, 2, 3, 5]) action_server.destroy()
def test_single_goal_reject(self): goal_order = 10 def goal_callback(goal): nonlocal goal_order self.assertEqual(goal.order, goal_order) return GoalResponse.REJECT def handle_accepted_callback(goal_handle): # Since the goal is rejected, we don't expect this function to be called self.assertFalse(True) action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=self.execute_goal_callback, goal_callback=goal_callback, handle_accepted_callback=handle_accepted_callback, ) goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) goal_msg.goal.order = goal_order future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, future, self.executor) self.assertFalse(future.result().accepted) action_server.destroy()
def test_feedback(self): def execute_with_feedback(goal_handle): feedback = Fibonacci.Feedback() feedback.sequence = [1, 1, 2, 3] goal_handle.publish_feedback(feedback) goal_handle.succeed() return Fibonacci.Result() action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=execute_with_feedback, ) goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) goal_future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, goal_future, self.executor) self.assertIsNotNone(self.mock_action_client.feedback_msg) self.assertEqual( [1, 1, 2, 3], self.mock_action_client.feedback_msg.feedback.sequence.tolist()) action_server.destroy()
def test_execute_raises_exception(self): def execute_callback(goal_handle): # User callback raises raise RuntimeError('test user callback raises') action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=execute_callback, ) goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = goal_uuid goal_future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, goal_future, self.executor) goal_handle = goal_future.result() self.assertTrue(goal_handle.accepted) get_result_future = self.mock_action_client.get_result(goal_uuid) rclpy.spin_until_future_complete(self.node, get_result_future, self.executor) result_response = get_result_future.result() # Goal status should default to STATUS_ABORTED self.assertEqual(result_response.status, GoalStatus.STATUS_ABORTED) self.assertEqual(result_response.result.sequence.tolist(), []) action_server.destroy()
def test_execute_succeed(self): def execute_callback(goal_handle): self.assertEqual(goal_handle.status, GoalStatus.STATUS_EXECUTING) result = Fibonacci.Result() result.sequence.extend([1, 1, 2, 3, 5]) goal_handle.succeed() return result action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=execute_callback, ) goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = goal_uuid goal_future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, goal_future, self.executor) goal_handle = goal_future.result() self.assertTrue(goal_handle.accepted) get_result_future = self.mock_action_client.get_result(goal_uuid) rclpy.spin_until_future_complete(self.node, get_result_future, self.executor) result_response = get_result_future.result() self.assertEqual(result_response.status, GoalStatus.STATUS_SUCCEEDED) self.assertEqual(result_response.result.sequence.tolist(), [1, 1, 2, 3, 5]) action_server.destroy()
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from unique_identifier_msgs.msg import UUID self.goal_id = kwargs.get('goal_id', UUID()) from turtlesim.action._rotate_absolute import RotateAbsolute_Goal self.goal = kwargs.get('goal', RotateAbsolute_Goal())
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from unique_identifier_msgs.msg import UUID self.goal_id = kwargs.get('goal_id', UUID()) from example_interfaces.action._fibonacci import Fibonacci_Goal self.goal = kwargs.get('goal', Fibonacci_Goal())
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from unique_identifier_msgs.msg import UUID self.goal_id = kwargs.get('goal_id', UUID()) from action_tutorials.action._fibonacci import Fibonacci_Feedback self.feedback = kwargs.get('feedback', Fibonacci_Feedback())
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from unique_identifier_msgs.msg import UUID self.goal_id = kwargs.get('goal_id', UUID()) from hydrophone_streamer_pkg.action._hydrophoneraw import Hydrophoneraw_Goal self.goal = kwargs.get('goal', Hydrophoneraw_Goal())
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from unique_identifier_msgs.msg import UUID self.goal_id = kwargs.get('goal_id', UUID()) from move_base_msgs.action._move_base import MoveBase_Goal self.goal = kwargs.get('goal', MoveBase_Goal())
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from unique_identifier_msgs.msg import UUID self.goal_id = kwargs.get('goal_id', UUID()) from builtin_interfaces.msg import Time self.stamp = kwargs.get('stamp', Time())
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from unique_identifier_msgs.msg import UUID self.goal_id = kwargs.get('goal_id', UUID()) from teleop_tools_msgs.action._increment import Increment_Feedback self.feedback = kwargs.get('feedback', Increment_Feedback())
def test_send_goal_async_with_feedback_for_not_a_goal(self): ac = ActionClient(self.node, Fibonacci, 'fibonacci') try: self.assertTrue(ac.wait_for_server(timeout_sec=2.0)) # Send a goal and then publish feedback goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) future = ac.send_goal_async( Fibonacci.Goal(), feedback_callback=self.feedback_callback, goal_uuid=goal_uuid) rclpy.spin_until_future_complete(self.node, future, self.executor) # Publish feedback for a non-existent goal ID self.mock_action_server.publish_feedback(UUID(uuid=list(uuid.uuid4().bytes))) self.timed_spin(1.0) self.assertEqual(self.feedback, None) finally: ac.destroy()
def __init__(self, obstacle_msg, top_down, measurement_noise_cov, error_cov_post, process_noise_cov): '''Initialize with an Obstacle msg and an assigned id''' self.msg = obstacle_msg uuid_msg = UUID() uuid_msg.uuid = list(uuid.uuid4().bytes) self.msg.uuid = uuid_msg position = np.array([[ obstacle_msg.position.x, obstacle_msg.position.y, obstacle_msg.position.z ]]).T # shape 3*1 velocity = np.array([[ obstacle_msg.velocity.x, obstacle_msg.velocity.y, obstacle_msg.velocity.z ]]).T # check aganist state space dimension, top_down or not if top_down: measurement_noise_cov[2] = 0. error_cov_post[2] = 0. error_cov_post[5] = 0. process_noise_cov[2] = 0. # setup kalman filter self.kalman = cv2.KalmanFilter( 6, 3) # 3d by default, 6d state space and 3d observation space self.kalman.measurementMatrix = np.array( [[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0]], np.float32) self.kalman.measurementNoiseCov = np.diag( measurement_noise_cov).astype(np.float32) self.kalman.statePost = np.concatenate([position, velocity]).astype(np.float32) self.kalman.errorCovPost = np.diag(error_cov_post).astype(np.float32) self.dying = 0 self.top_down = top_down self.process_noise_cov = process_noise_cov
def test_cancel_goal_accept(self): def execute_callback(goal_handle): # Wait, to give the opportunity to cancel time.sleep(3.0) self.assertTrue(goal_handle.is_cancel_requested) goal_handle.canceled() return Fibonacci.Result() def cancel_callback(request): return CancelResponse.ACCEPT executor = MultiThreadedExecutor(context=self.context) action_server = ActionServer( self.node, Fibonacci, 'fibonacci', callback_group=ReentrantCallbackGroup(), execute_callback=execute_callback, handle_accepted_callback=lambda gh: None, cancel_callback=cancel_callback, ) goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) goal_order = 10 goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = goal_uuid goal_msg.goal.order = goal_order goal_future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, goal_future, executor) goal_handle = goal_future.result() self.assertTrue(goal_handle.accepted) cancel_srv = CancelGoal.Request() cancel_srv.goal_info.goal_id = goal_uuid cancel_srv.goal_info.stamp.sec = 0 cancel_srv.goal_info.stamp.nanosec = 0 cancel_future = self.mock_action_client.cancel_goal(cancel_srv) rclpy.spin_until_future_complete(self.node, cancel_future, executor) cancel_result = cancel_future.result() self.assertEqual(len(cancel_result.goals_canceling), 1) assert all( cancel_result.goals_canceling[0].goal_id.uuid == goal_uuid.uuid) action_server.destroy() executor.shutdown()
def test_goal_callback_invalid_return(self): def goal_callback(goal): return 'Invalid return type' action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=self.execute_goal_callback, goal_callback=goal_callback, handle_accepted_callback=lambda gh: None, ) goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, future, self.executor) # An invalid return type in the goal callback should translate to a rejected goal self.assertFalse(future.result().accepted) action_server.destroy()
def test_expire_goals_one(self): # 1 second timeout action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=self.execute_goal_callback, result_timeout=1, ) goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) goal_future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, goal_future, self.executor) self.assertEqual(1, len(action_server._goal_handles)) # After two seconds the internal goal handle should be destroyed self.timed_spin(2.1) self.assertEqual(0, len(action_server._goal_handles)) action_server.destroy()
def test_send_goal_async_with_feedback_before_goal(self): ac = ActionClient(self.node, Fibonacci, 'fibonacci') try: self.assertTrue(ac.wait_for_server(timeout_sec=2.0)) # Publish feedback before goal has been accepted goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) self.mock_action_server.publish_feedback(goal_uuid) self.timed_spin(1.0) self.assertEqual(self.feedback, None) # Send a goal and then publish feedback future = ac.send_goal_async( Fibonacci.Goal(), feedback_callback=self.feedback_callback, goal_uuid=goal_uuid) rclpy.spin_until_future_complete(self.node, future, self.executor) # Check the feedback was not received self.assertEqual(self.feedback, None) finally: ac.destroy()
def test_single_goal_accept(self): goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) goal_order = 10 def goal_callback(goal): nonlocal goal_order self.assertEqual(goal.order, goal_order) return GoalResponse.ACCEPT handle_accepted_callback_triggered = False def handle_accepted_callback(goal_handle): nonlocal goal_order nonlocal goal_uuid nonlocal handle_accepted_callback_triggered handle_accepted_callback_triggered = True self.assertEqual(goal_handle.status, GoalStatus.STATUS_ACCEPTED) self.assertEqual(goal_handle.goal_id, goal_uuid) self.assertEqual(goal_handle.request.order, goal_order) action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=self.execute_goal_callback, goal_callback=goal_callback, handle_accepted_callback=handle_accepted_callback, ) goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = goal_uuid goal_msg.goal.order = goal_order future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, future, self.executor) self.assertTrue(future.result().accepted) self.assertTrue(handle_accepted_callback_triggered) action_server.destroy()
def test_duplicate_goal(self): executor = MultiThreadedExecutor(context=self.context) action_server = ActionServer( self.node, Fibonacci, 'fibonacci', execute_callback=self.execute_goal_callback, callback_group=ReentrantCallbackGroup(), handle_accepted_callback=lambda gh: None, ) # Send a goal with the same ID twice goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) future0 = self.mock_action_client.send_goal(goal_msg) future1 = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, future0, executor) rclpy.spin_until_future_complete(self.node, future1, executor) # Exactly one of the goals should be accepted self.assertNotEqual(future0.result().accepted, future1.result().accepted) action_server.destroy()
def _generate_random_uuid(self): return UUID(uuid=list(uuid.uuid4().bytes))
def test_three_point_set(self): # test data uuids = [ 'da7c242f-2efe-5175-9961-49cc621b80b9', '812f1c08-a34b-5a21-92b9-18b2b0cf4950', '6f0606f6-a776-5940-b5ea-5e889b32c712' ] latitudes = [30.3840168, 30.3857290, 30.3866750] longitudes = [-97.7282100, -97.7316754, -97.7270967] eastings = [622191.124, 621856.023, 622294.785] northings = [3362024.764, 3362210.790, 3362320.569] points = [] for i in range(len(uuids)): latlon = GeoPoint(latitude=latitudes[i], longitude=longitudes[i]) points.append( WayPoint(id=UUID(uuid=list(uuid.UUID(uuids[i]).bytes)), position=latlon)) # test iterator wupts = WuPointSet(points) i = 0 for w in wupts: uuid_msg = UUID(uuid=list(uuid.UUID(uuids[i]).bytes)) self.assertEqual(str(uuid.UUID(bytes=bytes(w.uuid()), version=5)), uuids[i]) self.assertEqual( str( uuid.UUID(bytes=bytes(wupts[str(uuid_msg.uuid)].uuid()), version=5)), uuids[i]) self.assertAlmostEqual(w.utm.easting, eastings[i], places=3) self.assertAlmostEqual(w.utm.northing, northings[i], places=3) point_xy = w.toPointXY() self.assertAlmostEqual(point_xy.x, eastings[i], places=3) self.assertAlmostEqual(point_xy.y, northings[i], places=3) self.assertAlmostEqual(point_xy.z, 0.0, places=3) i += 1 self.assertEqual(i, 3) self.assertEqual(len(wupts), 3) bogus = '00000000-c433-5c42-be2e-fbd97ddff9ac' self.assertFalse(bogus in wupts) self.assertEqual(wupts.get(bogus), None) uu = str(UUID(uuid=list(uuid.UUID(uuids[1]).bytes)).uuid) self.assertTrue(uu in wupts) wpt = wupts[uu] self.assertEqual(str(wpt.uuid()), uu) self.assertNotEqual(wupts.get(uu), None) self.assertEqual(str(wupts.get(uu).uuid()), uu) # test index() function for i in range(len(uuids)): wpuuid = str(UUID(uuid=list(uuid.UUID(uuids[i]).bytes)).uuid) self.assertEqual(wupts.index(wpuuid), i) self.assertEqual( str(uuid.UUID(bytes=bytes(wupts.points[i].id.uuid), version=5)), uuids[i])
def test_cancel_defered_goal(self): server_goal_handle = None def handle_accepted_callback(gh): nonlocal server_goal_handle server_goal_handle = gh def cancel_callback(request): return CancelResponse.ACCEPT def execute_callback(gh): # The goal should already be in state CANCELING self.assertTrue(gh.is_cancel_requested) gh.canceled() return Fibonacci.Result() action_server = ActionServer( self.node, Fibonacci, 'fibonacci', callback_group=ReentrantCallbackGroup(), execute_callback=execute_callback, handle_accepted_callback=handle_accepted_callback, cancel_callback=cancel_callback, ) goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = goal_uuid goal_future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, goal_future, self.executor) send_goal_response = goal_future.result() self.assertTrue(send_goal_response.accepted) self.assertIsNotNone(server_goal_handle) self.assertEqual(server_goal_handle.status, GoalStatus.STATUS_ACCEPTED) # Cancel the goal, before execution cancel_srv = CancelGoal.Request() cancel_srv.goal_info.goal_id = goal_uuid cancel_srv.goal_info.stamp.sec = 0 cancel_srv.goal_info.stamp.nanosec = 0 cancel_future = self.mock_action_client.cancel_goal(cancel_srv) rclpy.spin_until_future_complete(self.node, cancel_future, self.executor) cancel_result = cancel_future.result() self.assertEqual(len(cancel_result.goals_canceling), 1) self.assertEqual(server_goal_handle.status, GoalStatus.STATUS_CANCELING) # Execute the goal server_goal_handle.execute() # Get the result and exepect it to have canceled status get_result_future = self.mock_action_client.get_result(goal_uuid) rclpy.spin_until_future_complete(self.node, get_result_future, self.executor) result = get_result_future.result() self.assertEqual(result.status, GoalStatus.STATUS_CANCELED) self.assertEqual(server_goal_handle.status, GoalStatus.STATUS_CANCELED) action_server.destroy()