async def execute_callback(self, goal_handle):
"""Executes a goal."""
self._node.get_logger().info('Executing goal...')
# Append the seeds for the Fibonacci sequence
feedback_msg = Fibonacci.Feedback()
feedback_msg.sequence = [0, 1]
# Start executing the action
for i in range(1, goal_handle.request.order):
if goal_handle.is_cancel_requested:
goal_handle.canceled()
self._node.get_logger().info('Goal canceled')
return Fibonacci.Result()
# Update Fibonacci sequence
feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i-1])
self._node.get_logger().info('Publishing feedback: {0}'.format(feedback_msg.sequence))
# Publish the feedback
goal_handle.publish_feedback(feedback_msg)
# Sleep for demonstration purposes
#time.sleep(0.5)
goal_handle.succeed()
# Populate result message
result = Fibonacci.Result()
result.sequence = feedback_msg.sequence
self._node.get_logger().info('Returning result: {0}'.format(result.sequence))
return result
async def execute_callback(self, goal_handle):
# アクションの実行
self.get_logger().info('executing...')
# フィボナッチ数列の初期値0, 1を設定
msg = Fibonacci.Feedback()
msg.sequence = [0, 1]
# フィボナッチ数列を一つずつ追加
for i in range(1, goal_handle.request.order):
if goal_handle.is_cancel_requested:
# アクションのキャンセル
goal_handle.set_canceled()
self.get_logger().info('goal canceled')
return Fibonacci.Result()
# フィボナッチ数列の更新
msg.sequence.append(msg.sequence[i] + msg.sequence[i - 1])
self.get_logger().info('feedback: {0}'.format(msg.sequence))
# アクションのフィードバックの送信
goal_handle.publish_feedback(msg)
# 1秒待機（重たい処理の代わり）
time.sleep(1)
# アクションの実行結果の送信
goal_handle.set_succeeded()
result = Fibonacci.Result()
result.sequence = msg.sequence
self.get_logger().info('result: {0}'.format(result.sequence))
return result
async def execute_callback(self, goal_handle):
# this is where the actual virtual rail execution code goes
# Start executing the action
while True:
start_time = current_time()
# check if the goal has been cancelled
if goal_handle.is_cancel_requested:
goal_handle.canceled()
result = Fibonacci.Result()
result.success = False
return Fibonacci.Result()
if path_complete():
goal_handle.succeed()
result = Fibonacci.Result()
result.success = True
return Fibonacci.Result()
# Publish a pose to the position controller
msg=Pose()
pose.data = calc_position_setpoint(time, percent_complete)
self.abs_pose_publisher.publish(msg)
# Publish the feedback, could be current setpoint, percent complete, whatever we need to display in the GUI
goal_handle.publish_feedback(feedback_msg)
# Sleep for loop time
while (current_time() - start_time < loop_time):
ros.spin_once() # this is te asynchronous bit, allows other call backs to run sunch as cancelling the goal, rejecting incoming goals, basically Python asyncio
def execute_callback(self, goal_handle):
"""Executes a goal."""
try:
self.get_logger().info('Executing goal...')
# Append the seeds for the Fibonacci sequence
feedback_msg = Fibonacci.Feedback()
feedback_msg.sequence = [0, 1]
# Start executing the action
for i in range(1, goal_handle.request.order):
if goal_handle.is_cancel_requested:
goal_handle.canceled()
self.get_logger().info('Goal canceled')
return Fibonacci.Result()
# Update Fibonacci sequence
feedback_msg.sequence.append(feedback_msg.sequence[i] +
feedback_msg.sequence[i - 1])
self.get_logger().info('Publishing feedback: {0}'.format(
feedback_msg.sequence))
# Publish the feedback
goal_handle.publish_feedback(feedback_msg)
# Sleep for demonstration purposes
time.sleep(1)
goal_handle.succeed()
# Populate result message
result = Fibonacci.Result()
result.sequence = feedback_msg.sequence
self.get_logger().info('Returning result: {0}'.format(
result.sequence))
return result
finally:
with self._goal_queue_lock:
try:
# Start execution of the next goal in the queue.
self._current_goal = self._goal_queue.popleft()
self.get_logger().info('Next goal pulled from the queue')
self._current_goal.execute()
except IndexError:
# No goal in the queue.
self._current_goal = None
async def send_goal(self):
self.get_logger().info('Waiting for action server...')
self._action_client.wait_for_server()
goal_msg = Fibonacci.Goal()
goal_msg.order = 10
self.get_logger().info('Sending goal request...')
goal_handle = await self._action_client.send_goal_async(
goal_msg, feedback_callback=self.feedback_callback)
if not goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
return
self.get_logger().info('Goal accepted :)')
res = await goal_handle.get_result_async()
result = res.result
status = res.status
if status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info('Goal succeeded! Result: {0}'.format(
result.sequence))
else:
self.get_logger().info(
'Goal failed with status: {0}'.format(status))
return result, status
def send_goal(self):
self.get_logger().info('waiting...')
self._action_client.wait_for_server()
goal_msg = Fibonacci.Goal()
goal_msg.order = 10
self._send_goal_future = self._action_client.send_goal_async(
goal_msg, feedback_callback=self.feedback_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def execute_callback(self, goal_handle):
"""Execute the goal."""
self.get_logger().info('Executing goal...')
# Append the seeds for the Fibonacci sequence
feedback_msg = Fibonacci.Feedback()
feedback_msg.sequence = [0, 1]
# Start executing the action
for i in range(1, goal_handle.request.order):
# If goal is flagged as no longer active (ie. another goal was accepted),
# then stop executing
if not goal_handle.is_active:
self.get_logger().info('Goal aborted')
return Fibonacci.Result()
if goal_handle.is_cancel_requested:
goal_handle.canceled()
self.get_logger().info('Goal canceled')
return Fibonacci.Result()
# Update Fibonacci sequence
feedback_msg.sequence.append(feedback_msg.sequence[i] +
feedback_msg.sequence[i - 1])
self.get_logger().info('Publishing feedback: {0}'.format(
feedback_msg.sequence))
# Publish the feedback
goal_handle.publish_feedback(feedback_msg)
# Sleep for demonstration purposes
time.sleep(1)
goal_handle.succeed()
# Populate result message
result = Fibonacci.Result()
result.sequence = feedback_msg.sequence
self.get_logger().info('Returning result: {0}'.format(result.sequence))
return result
def send_goal(self):
self.get_logger().info('waiting...')
self._action_client.wait_for_server()
# 10要素のフィボナッチ数列を標値に設定
goal_msg = Fibonacci.Goal()
goal_msg.order = 10
# アクションの非同期実行
# （フィードバックと実行結果の受信コールバック関数も設定）
self._send_goal_future = self._action_client.send_goal_async(
goal_msg, feedback_callback=self.feedback_callback)
self._send_goal_future.add_done_callback(
self.goal_response_callback)
async def execute_callback(self, goal_handle):
self.get_logger().info('executing...')
msg = Fibonacci.Feedback()
msg.sequence = [0, 1]
for i in range(1, goal_handle.request.order):
if goal_handle.is_cancel_requested:
goal_handle.canceled()
self.get_logger().info('goal_canceled')
return Fibonacci.Result()
msg.sequence.append(msg.sequence[i] + msg.sequence[i - 1])
self.get_logger().info('feedback:{}'.format(msg.sequence))
goal_handle.publish_feedback(msg)
time.sleep(1) # dummy job
goal_handle.set_succeeded()
result = Fibonacci.Result()
result.sequence = msg.sequence
self.get_logger().info('result:{}'.format(result.sequence))
return result
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_action_client')
action_client = ActionClient(node, Fibonacci, 'fibonacci')
node.get_logger().info('Waiting for action server...')
action_client.wait_for_server()
goal_msg = Fibonacci.Goal()
goal_msg.order = 10
node.get_logger().info('Sending goal request...')
send_goal_future = action_client.send_goal_async(
goal_msg,
feedback_callback=lambda feedback: feedback_cb(node.get_logger(),
feedback))
rclpy.spin_until_future_complete(node, send_goal_future)
goal_handle = send_goal_future.result()
if not goal_handle.accepted:
node.get_logger().info('Goal rejected :(')
action_client.destroy()
node.destroy_node()
rclpy.shutdown()
return
node.get_logger().info('Goal accepted :)')
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, get_result_future)
result = get_result_future.result().result
status = get_result_future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
node.get_logger().info('Goal succeeded! Result: {0}'.format(
result.sequence))
else:
node.get_logger().info(
'Goal failed with status code: {0}'.format(status))
action_client.destroy()
node.destroy_node()
rclpy.shutdown()
def send_goal(self, order):
# self.get_logger().info('Waiting for action server '+self._server_name)
print('Waiting for action server ' + self._server_name)
self._action_client.wait_for_server()
goal_msg = Fibonacci.Goal()
goal_msg.order = order
# self.get_logger().info('Sending goal request to '+self._server_name)
print('Sending goal request to ' + self._server_name)
self._send_goal_future = self._action_client.send_goal_async(
goal_msg, feedback_callback=self.feedback_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def send_goal(self):
self.logger.info('waiting...')
while not self._cli.wait_for_server(timeout_sec=1.0):
self.logger.info("Waiting for \"fibonacci\" server...")
# 10要素のフィボナッチ数列を標値に設定
goal_msg = Fibonacci.Goal()
goal_msg.order = 10
# アクションの非同期実行
# （フィードバックと実行結果の受信コールバック関数も設定）
goal = goal_msg
feedback_callback = self._feedback_callback
self._goal_future = self._cli.send_goal_async(goal, feedback_callback)
callback = self._goal_response_callback
self._goal_future.add_done_callback(callback)
