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()
