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 execute_with_feedback(goal_handle):
try:
goal_handle.publish_feedback('different feedback type')
except TypeError:
feedback = Fibonacci.Feedback()
feedback.sequence = [1, 1, 2, 3]
goal_handle.publish_feedback(feedback)
goal_handle.succeed()
return Fibonacci.Result()
def execute_callback(self, goal_handle):
feedback = Fibonacci.Feedback()
feedback.sequence = [0, 1]
for i in range(1, goal_handle.request.order):
feedback.sequence.append(feedback.sequence[i] + feedback.sequence[i-1])
goal_handle.publish_feedback(feedback)
goal_handle.succeed()
result = Fibonacci.Result()
result.sequence = feedback.sequence
return result
def test_send_goal_async_no_server(self):
ac = ActionClient(self.node, Fibonacci, 'not_fibonacci')
try:
future = ac.send_goal_async(Fibonacci.Goal())
self.timed_spin(2.0)
self.assertFalse(future.done())
finally:
ac.destroy()
def test_send_goal_async(self):
ac = ActionClient(self.node, Fibonacci, 'fibonacci')
try:
self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
future = ac.send_goal_async(Fibonacci.Goal())
rclpy.spin_until_future_complete(self.node, future, self.executor)
self.assertTrue(future.done())
goal_handle = future.result()
self.assertTrue(goal_handle.accepted)
finally:
ac.destroy()
def test_send_goal_multiple(self):
ac = ActionClient(self.node,
Fibonacci,
'fibonacci',
callback_group=ReentrantCallbackGroup())
executor = MultiThreadedExecutor(context=self.context)
try:
self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
future_0 = ac.send_goal_async(Fibonacci.Goal())
future_1 = ac.send_goal_async(Fibonacci.Goal())
future_2 = ac.send_goal_async(Fibonacci.Goal())
rclpy.spin_until_future_complete(self.node, future_0, executor)
rclpy.spin_until_future_complete(self.node, future_1, executor)
rclpy.spin_until_future_complete(self.node, future_2, executor)
self.assertTrue(future_0.done())
self.assertTrue(future_1.done())
self.assertTrue(future_2.done())
self.assertTrue(future_0.result().accepted)
self.assertTrue(future_1.result().accepted)
self.assertTrue(future_2.result().accepted)
finally:
ac.destroy()
def generate_fibonacci_goal_tests():
tests = []
order = 10
goal = Fibonacci.Goal()
goal.order = order
valid_result_sequence = [0, 1]
for i in range(1, goal.order):
valid_result_sequence.append(valid_result_sequence[i] +
valid_result_sequence[i - 1])
test = ActionClientTest(goal)
def is_feedback_valid(feedback):
if len(feedback.sequence) > (order + 1):
print('Feedback sequence is greater than goal order',
file=sys.stderr)
return False
for i in range(0, len(feedback.sequence)):
if feedback.sequence[i] != valid_result_sequence[i]:
print(
'Feedback sequence not correct, expected {} but got {} for order {}'
.format(valid_result_sequence[i], feedback.sequence[i],
order),
file=sys.stderr)
return False
return True
def is_result_valid(result):
if len(result.sequence) != (order + 1):
print('Result sequence does not equal goal order', file=sys.stderr)
return False
for i in range(0, len(result.sequence)):
if result.sequence[i] != valid_result_sequence[i]:
print(
'Result sequence not correct, expected {} but got {} for order {}'
.format(valid_result_sequence[i], result.sequence[i],
order),
file=sys.stderr)
return False
return True
test.is_feedback_valid = is_feedback_valid
test.is_result_valid = is_result_valid
tests.append(test)
return tests
def execute_goal(goal_handle):
goal = goal_handle.request
feedback = Fibonacci.Feedback()
feedback.sequence = [0, 1]
for i in range(1, goal.order):
if not rclpy.ok():
goal_handle.set_aborted()
return Fibonacci.Result()
# Check if the goal was canceled
if goal_handle.is_cancel_requested:
goal_handle.set_canceled()
result = Fibonacci.Result()
result.sequence = feedback.sequence
print('Goal was canceled')
return result
# Update the sequence.
feedback.sequence.append(feedback.sequence[i] +
feedback.sequence[i - 1])
# Publish feedback
goal_handle.publish_feedback(feedback)
print('Publishing feedback')
# 10 Hz update rate
time.sleep(0.1)
# Send final result
result = Fibonacci.Result()
result.sequence = feedback.sequence
goal_handle.set_succeeded()
print('Goal succeeded')
return result
def test_get_result_async(self):
ac = ActionClient(self.node, Fibonacci, 'fibonacci')
try:
self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
# Send a goal
goal_future = ac.send_goal_async(Fibonacci.Goal())
rclpy.spin_until_future_complete(self.node, goal_future, self.executor)
self.assertTrue(goal_future.done())
goal_handle = goal_future.result()
# Get the goal result
result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(self.node, result_future, self.executor)
self.assertTrue(result_future.done())
finally:
ac.destroy()
def test_send_goal_async_with_feedback_after_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 after goal has been accepted
self.mock_action_server.publish_feedback(goal_uuid)
self.timed_spin(1.0)
self.assertNotEqual(self.feedback, None)
finally:
ac.destroy()
def test_send_cancel_async(self):
ac = ActionClient(self.node, Fibonacci, 'fibonacci')
try:
self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
# Send a goal
goal_future = ac.send_goal_async(Fibonacci.Goal())
rclpy.spin_until_future_complete(self.node, goal_future, self.executor)
self.assertTrue(goal_future.done())
goal_handle = goal_future.result()
# Cancel the goal
cancel_future = goal_handle.cancel_goal_async()
rclpy.spin_until_future_complete(self.node, cancel_future, self.executor)
self.assertTrue(cancel_future.done())
self.assertEqual(
cancel_future.result().goals_canceling[0].goal_id,
goal_handle.goal_id)
finally:
ac.destroy()
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()
def publish_feedback(self, goal_id):
feedback = Fibonacci.Feedback()
feedback.action_goal_id = goal_id
self.feedback_pub.publish(feedback)
def action_callback(self, goal_handle):
goal_handle.succeed()
return Fibonacci.Result()
def execute_callback(goal_handle):
# Wait, to give the opportunity to cancel
time.sleep(3.0)
self.assertFalse(goal_handle.is_cancel_requested)
goal_handle.canceled()
return Fibonacci.Result()
def execute_callback(gh):
# The goal should already be in state CANCELING
self.assertTrue(gh.is_cancel_requested)
gh.canceled()
return Fibonacci.Result()
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.abort()
return result
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
def execute_goal_callback(self, goal_handle):
goal_handle.succeed()
return Fibonacci.Result()
