ROS2 3일차(4) Action 프로그래밍

Action Server 작성

Goal Response, Feedback, Result Response를 구현(cancel 추가)

 

fibonacci_action_server.py

#!/usr/bin/env/ python3

# Copyright 2019 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the 'License');
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an 'AS IS' BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

# https://docs.ros.org/en/foxy/Tutorials/Actions/Writing-a-Py-Action-Server-Client.html#id4

import time

from custom_interfaces.action import Fibonacci

import rclpy
from rclpy.action import ActionServer, GoalResponse
from rclpy.node import Node


class FibonacciActionServer(Node):

    def __init__(self):
        super().__init__('fibonacci_action_server')
        self.action_server = ActionServer(
            self,
            Fibonacci,
            'fibonacci',
            self.execute_callback,
            goal_callback=self.goal_callback,
        )

        self.get_logger().info('=== Fibonacci Action Server Started ====')

    async def execute_callback(self, goal_handle):
        self.get_logger().info('Executing goal...')

        feedback_msg = Fibonacci.Feedback()
        feedback_msg.partial_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()

            feedback_msg.partial_sequence.append(
                feedback_msg.partial_sequence[i] + feedback_msg.partial_sequence[i - 1]
            )

            self.get_logger().info(f'Feedback: {feedback_msg.partial_sequence}')
            goal_handle.publish_feedback(feedback_msg)
            time.sleep(1)

        goal_handle.succeed()
        self.get_logger().warn('==== Succeed ====')

        result = Fibonacci.Result()
        result.sequence = feedback_msg.partial_sequence
        return result

    def goal_callback(self, goal_request):
        """Accept or reject a client request to begin an action."""
        # This server allows multiple goals in parallel
        self.get_logger().info('Received goal request')
        return GoalResponse.ACCEPT


def main(args=None):
    rclpy.init(args=args)

    fibonacci_action_server = FibonacciActionServer()
    rclpy.spin(fibonacci_action_server)

    fibonacci_action_server.destroy()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

 

import

import time

import rclpy
from rclpy.action import ActionServer, GoalResponse
from rclpy.node import Node

from custom_interfaces.action import Fibonacci

다른 방식들과는 다르게 action은 rclpy.action을 import 해야 함

더불어, GoalResponse라는 것도 import

rclpy.action은 Action의 여러 상태들을 고유한 숫자로 1대1 대응

ACCEPT = 2

REJECT = 1

 

GoalResponse에 따라 어떠한 특정 로직을 구현하고 싶다면 숫자 1을 쓰거나 GoalResponse.REJECT를 쓰면 됨

 

FibonacciActionServer 클래스 내부

class FibonacciActionServer(Node):

    def __init__(self):
        super().__init__("fibonacci_action_server")
        # Action Server를 생성합니다.
        self.action_server = ActionServer(
            self, Fibonacci, "fibonacci", 
            # 각 상황에 대한 callback을 지정합니다.
            # Goal Response가 오면, 우선 goal_callback을 실행시킨 뒤
            # execute_callback으로 넘어가게 됩니다.
            self.execute_callback,
            goal_callback=self.goal_callback)

        self.get_logger().info("=== Fibonacci Action Server Started ====")

    # goal_callback 이후의 진입 callback, 
    # 실제 Feedback과 Result를 처리하는 로직을 담고 있습니다.
    def execute_callback(self, goal_handle):
        self.get_logger().info("Executing goal...")

        # Feedback action을 준비합니다.
        feedback_msg = Fibonacci.Feedback()
        feedback_msg.partial_sequence = [0, 1]
				
        # 지금의 경우 Request 숫자만큼의 피보나치 수열을 계산해야 합니다.
        for i in range(1, goal_handle.request.order):

            # 실질적인 피보나치 로직
            feedback_msg.partial_sequence.append(
                feedback_msg.partial_sequence[i] + feedback_msg.partial_sequence[i - 1]
            )

            # feedback publish가 이루어지는 부분입니다.
            print(f"Feedback: {feedback_msg.partial_sequence}")
            goal_handle.publish_feedback(feedback_msg)
            time.sleep(1)

        goal_handle.succeed()
        self.get_logger().warn("==== Succeed ====")

        # 모든 계산을 마치고, result를 되돌려주는 부분입니다.
        result = Fibonacci.Result()
        result.sequence = feedback_msg.partial_sequence
        return result

    # Goal Request 시 가장 처음 진입하게 되는 callback입니다.
    def goal_callback(self, goal_request):
        """Accept or reject a client request to begin an action."""
        self.get_logger().info('Received goal request')

        # 도저히 불가능한 Request가 왔다면, 여기에서 판단하여 REJECT합니다.
        # 아래 ACCEPT => REJECT로 바꾼 뒤, 다시 실행시켜보세요!!
        return GoalResponse.ACCEPT

 

Action이 해야 하는 기능

• Goal Response => goal_callback
• 중간 결과를 Feedback => publish_feedback
• 최종 Result Response => Fibonacci.Result()
• Feedback을 보내면서 내부 로직실행 => execute_callback()
• goal_handle을 통해 주로 작업이 이루어짐

이에 따라 Action Server는 다음과 같이 생성

ActionServer

self._action_server = ActionServer(
    self, <action-type>, "<action-name>",
    <execute_callback>,
    <goal_callback>)
    
self._action_server = ActionServer(
    self, Fibonacci, "fibonacci",
    self.execute_callback,
    goal_callback=self.goal_callback)

 

Action Server 작성 - cancel ver.

기존의 Action Server에 다음과 같은 기능 추가

• 여러 client request에도 대응 가능 => MultiThreadedExecutor
• Goal cancel 가능 => CancelResponse

fibonacci_action_server_cancel.py

# !/usr/bin/env/ python3
#
# Copyright 2019 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the 'License');
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an 'AS IS' BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

# Also Referenced ROS Documents
# https://docs.ros.org/en/foxy/Tutorials/Actions/Writing-a-Py-Action-Server-Client.html#id4

import time

from custom_interfaces.action import Fibonacci

import rclpy
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node

from custom_interfaces.action import Fibonacci


class FibonacciActionServer(Node):
    def __init__(self):
        super().__init__("fibonacci_action_server")
        self.action_server = ActionServer(
            self,
            Fibonacci,
            'fibonacci',
						# 새로운 용어가 등장하였습니다. 하단에 레퍼런스를 걸어두었답니다 :)
            callback_group=ReentrantCallbackGroup(),
            execute_callback=self.execute_callback,
            goal_callback=self.goal_callback,
						# cancel_callback이 추가되었습니다.
            cancel_callback=self.cancel_callback)

        self.get_logger().info("=== Fibonacci Action Server Started ====")

    async def execute_callback(self, goal_handle):
        self.get_logger().info("Executing goal...")

        feedback_msg = Fibonacci.Feedback()
        feedback_msg.partial_sequence = [0, 1]

        for i in range(1, goal_handle.request.order):
						# 실행 중간 cancel이 탐지되면 지금까지의 계산결과를 return 합니다.
            if goal_handle.is_cancel_requested:
                goal_handle.canceled()
                self.get_logger().info('Goal canceled')
                return Fibonacci.Result()

            feedback_msg.partial_sequence.append(
                feedback_msg.partial_sequence[i] + feedback_msg.partial_sequence[i - 1]
            )

            print(f"Feedback: {feedback_msg.partial_sequence}")
            goal_handle.publish_feedback(feedback_msg)
            time.sleep(1)

        goal_handle.succeed()
        self.get_logger().warn("==== Succeed ====")

        result = Fibonacci.Result()
        result.sequence = feedback_msg.partial_sequence
        return result

    def goal_callback(self, goal_request):
        """Accept or reject a client request to begin an action."""
        # This server allows multiple goals in parallel
        self.get_logger().info('Received goal request')
        return GoalResponse.ACCEPT
		
		# 하단에 CancelResponse에 대한 레퍼런스를 걸어두었습니다.
    def cancel_callback(self, goal_handle):
        """Accept or reject a client request to cancel an action."""
        self.get_logger().info('Received cancel request')
        return CancelResponse.ACCEPT


def main(args=None):
    rclpy.init(args=args)

    fibonacci_action_server = FibonacciActionServer()

		# MultiThreadedExecutor는 다음과 같이 사용합니다.
    executor = MultiThreadedExecutor()
    rclpy.spin(fibonacci_action_server, executor=executor)

    fibonacci_action_server.destroy()
    rclpy.shutdown()


if __name__ == "__main__":
    main()

 

import (CancelResponse 추가)

import time

from custom_interfaces.action import Fibonacci

import rclpy
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node

 

Cancel의 상세 구현

• cancel_callback를 구현하고, Action Server 생성 시 이를 매개변수로 추가

self.action_server = ActionServer(
        self,
        Fibonacci,
        "fibonacci",
        callback_group=ReentrantCallbackGroup(),
        execute_callback=self.execute_callback,
        goal_callback=self.goal_callback,
        cancel_callback=self.cancel_callback,
    )

def cancel_callback(self, goal_handle):
    """Accept or reject a client request to cancel an action."""
    self.get_logger().info("Received cancel request")

    # Logic

    return CancelResponse.ACCEPT

execute_callback내에서 cancel이 탐지되면 실행하던 것을 멈추고, Result를 반환

    if goal_handle.is_cancel_requested:
        goal_handle.canceled()
        self.get_logger().info("Goal canceled")
        return Fibonacci.Result()

 

MultiThreadedExecutor

다음으로, MultiThreadedExecutor에 대해서 살펴보자

생성한 Node를 실행하는 executor에는 두 종류가 있다.

• singleThreadedExecutor
• MultiThreadedExecutor

이들 중, MultiThreadedExecutor는 rclpy.spin() 실행 시, 사용할 Node와 함께 전달하면 알아서 multithreading을 해준다

fibonacci_action_server = FibonacciActionServer()

executor = MultiThreadedExecutor()
rclpy.spin(fibonacci_action_server, executor=executor)

 

지금의 경우, execute_callback이 async 함수이기 때문에, 여러 client request를 처리할 수 있게 된 것이다.

asyncio의 queue 기능을 사용해서 Node가 자원을 공유하도록 직접 개발할 수도 있다.

 

Action Client 작성

Action Server를 만들었다면, 커멘드 라인을 통해 request가 가능했다

$ ros2 run py_action_pkg fibonacci_action_server
$ ros2 action send_goal fibonacci custom_interfaces/action/Fibonacci "{order: 5}"

하지만 커멘드 라인에서는 cancel request나, 추가 기능을 구현할 수는 없다.

지금부터는 직접 Action Client를 작성해 보겠다.

 

fibonacci_action_client.py

# !/usr/bin/env/ python3
#
# Copyright 2019 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the 'License');
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an 'AS IS' BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

from custom_interfaces.action import Fibonacci

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

class FibonacciActionClient(Node):
    def __init__(self):
        super().__init__("fibonacci_action_client")
        # Server에서 지정한 action 이름과 일치해야 한다는 점에 유의하세요.
        self.action_client = ActionClient(self, Fibonacci, "fibonacci")
        self.get_logger().info("=== Fibonacci Action Client Started ====")

    # client 생성 시 callback으로 묶이는 것이 아니기 때문에, 직접 main에서 호출해야 합니다.
    def send_goal(self, order):
        goal_msg = Fibonacci.Goal()
        goal_msg.order = order

        # 10초간 server를 기다리다가 응답이 없으면 에러를 출력합니다. 
        if self.action_client.wait_for_server(10) is False:
            self.get_logger().error("Server Not exists")

        # goal request가 제대로 보내졌는지 알기 위해 future가 사용됩니다.
        # 더불어, feedback_callback을 묶어 feedback 발생 시 해당 함수로 이동합니다.
        self._send_goal_future = self.action_client.send_goal_async(
            goal_msg, feedback_callback=self.feedback_callback
        )

        # server가 존재한다면, Goal Request의 성공 유무, 
        # 최종 Result에 대한 callback도 필요합니다.
        self._send_goal_future.add_done_callback(self.goal_response_callback)

    # feedback을 받아오고, 지금은 단순히 출력만 합니다.
    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        print(f"Received feedback: {feedback.partial_sequence}")

    # Goal Request에 대한 응답 시 실행될 callback입니다.
    def goal_response_callback(self, future):
        goal_handle = future.result()

        # Goal type에 따라 성공 유무를 판단합니다.
        if not goal_handle.accepted:
            self.get_logger().info("Goal rejected")
            return

        self.get_logger().info("Goal accepted")
				
        # 아직 callback이 남았습니다!
        # 만약 최종 Result 데이터를 다룰 callback을 연동합니다.
        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    # Result callback은 future를 매개변수로 받습니다.
    # future내에서 result에 접근하는 과정에 유의하시기 바랍니다.
    def get_result_callback(self, future):
        result = future.result().result
        self.get_logger().warn(f"Action Done !! Result: {result.sequence}")
        rclpy.shutdown()


def main(args=None):
    rclpy.init(args=args)

    fibonacci_action_client = FibonacciActionClient()
		
    # Client Node 생성 이후 직접 send_goal을 해줍니다. Service와 유사하지요
    # 지금은 딱히 작업이 없지만 Goal Request에 대한 future를 반환하도록 해두었습니다. 
    future = fibonacci_action_client.send_goal(5)

    rclpy.spin(fibonacci_action_client)


if __name__ == "__main__":
    main()