PinkLAB - 민형기 ROS2 응용 실습 7일차 - 커스텀 액션 서버

안상훈·2024년 8월 23일

PinkLAB ros2

ROS2 실습

목록 보기

11/13

본 블로그 포스팅은 https://www.youtube.com/@pinklab_studio/playlists
에서 현재 강의 중에 있는

ROS2 무작정 따라하기강의의 내용을 필자가 다시 복기하여 기록하는 내용에 관한 것이다.

1. 커스텀 액션 메세지 만들기

이전 포스트 PinkLAB - 민형기 ROS2 응용 실습 6일차 - 커스텀 서비스 클라이언트에 이어 이번에는 사용자 정의 액션메세지를 생성하고자 한다.

1.1 액션메세지패키지 - my_trd_pkg_msgs생성

강의에서는 기존 패키지에 신규 메세지 파일을 추가하는 식으로 작업이 진행되나 패키지 만드는 명령어는 복기할 필요성이 있기에 필자는 계속 패키지 신설하는 식으로 코드를 작성하고자 한다.

이전과 동일하게 ros2_workspace/src경로로 이동한 후 신규패키지 작성을 수행한다.

$ cd ros2_workspace/src #각자의 ros2작업공간 내 'src'폴더로 이동

$ ros2 pkg create --build-type ament_cmake my_trd_pkg_msgs --license Apache-2.0
#신규패키지 [my_trd_pkg_msgs] 생성

신규 패키지 my_trd_pkg_msgs 생성 후에는 액션 메세지를 관리하는 action폴더를 생성 후 해당 폴더 내 ActTurtle.action 파일을 작성한다.

$ mkdir action #신규패키지 [my_trd_pkg_msgs] 내 에서 생성해야함

액션ActTurtle.action는 총 3가지 메세지 범주로 구분되나
서비스 + 토픽 = 액션으로 보면 간단하게 이해할 수 있으며, 항목별로는 아래와 같다.

1) 액션 Request = 서비스 Request
2) 액션 Result = 서비스 Response
2) 액션 Feedback = 토픽

액션은 목표(goal)이란 항목이 존재하고 해당 goal의 설정(Request)와 goal을 수행하기까지의 상태변화(Feedback)
최종 goal의 수행 결과(성공, 실패 등등..)(Result)에 따라서 살짝 의미가 다르기에 하위 메서드 범주의 명명이 다르게 표현된다

아무튼 각 메세지 범주별로 하위 메세지 항목, 타입을 위 사진처럼 지정한다.

1.2 빌드 옵션 및 의존성 설정

1) 빌드 옵션 설정: CmakeList.txt
2) 빌드 의존성 설정 : Package.xml

Package.xml 작업내용

<!--추가해야 할 항목-->
  <build_depend>rosidl_default_generators</build_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

  <depend>action_msgs</depend>
<!--추가해야 할 항목-->

빌드 의존성 설정은 커스텀 메세지 작성시 필수로 설정하는
rosidl_default_generators, rosidl_default_runtime, rosidl_interface_packages는 대충 이해가 가는데

여기에 action_msgs를 하나 더 추가의존한다.
이는 액션메세지의 Request항목을 통해 goal항목이 설정되면 이것에 대한 메타데이터 goal IDs가 새로이 정의되며, 이때 해당 메타데이터를 관리하는 빌드패키지가 action_msgs라 보면된다.

CmakeList.txt 작업내용

# 추가해야할 항목
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "action/ActTurtle.action"
)
# 추가해야할 항목

다음으로 신설한 액션 파일ActTurtle.action 파일의 경로를 위 코드로 추가하여 빌드 옵션설정을 완료한다.

패키지 빌드

신규 패키지 my_trd_pkg_msgs에 액션 메세지 ActTurtle.action이 새로이 정의되었으니 아래 코드를 수행하여 패키지 빌드+등록을 진행 후 검증까지 완료하자

$ colcon build --packages-select my_trd_pkg_msgs
	#신규 설계한 [my_trd_pkg_msgs] 패키지 빌드
$ r2pkgsetup #설계한 패키지를 등록

$ ros2 interface show my_trd_pkg_msgs/action/ActTurtle
	#신규패키지[my_trd_pkg_msgs] 내 커스텀 메세지 [ActTurtle] 구성 출력

2. 커스텀 액션 서버 실습

작업 개요는 위 사진처럼 액션은 총 3가지 하위 메세지 항목이 각 단계별로 입력/출력 메세지를 정의하며, 추가로 터틀봇 제어를 위한 토픽을 발행한다.

순서로 보면 아래와 같다.

1) 목표설정 : 액션Request 항목으로 cmd_num을 받아서 Goal설정

2) Goal을 달성하기 위한 작업 중 중간보고(Feedback) : 노드의 중간 상태보고는 액션 토픽이라 볼 수 있는 Feedback항목과 Topic 두가지 항목을 동시에 발행하며, Topic을 통해 Turtlebot이 제어된다.

3) Goal달성 여부에 따른 Result : 설정한 Goal을 달성하면 succeed()메서드를 통해서 액션 서버 상태를 SUCCEEDED으로 변경한 뒤 이에 따른 Result데이터인 Turtlebot의
추정 위치/자세 정보를 출력한다.

위 작업을 수행하는 my_dist_action_node노드 코드 파일인 my_action1.py는 아래와 같이 작성한다.

라이브러리 import

import rclpy as rp #ros2용 파이썬 라이브러리
from rclpy.node import Node #노드 작성시 필요 라이브러리
# Action기능을 쓰려면 별도의 ActionServer, ActionClient
# 라이브러리를 import 해야함
from rclpy.action import ActionServer

# 임의 커스텀 액션 메세지 : my_trd_pkg_msgs의 액션 커스텀 메세지
from my_trd_pkg_msgs.action import ActTurtle

# 터틀봇을 제어하기 위한 메세지
from geometry_msgs.msg import Twist
import random, time, math

main구문

def main(args=None):
    rp.init(args=args) #ros2 노드 초기화, cmd명령 받을 수 있음
    my_action_srv_node = ActionSrvNode() #액션 서버 클래스 객채화
    rp.spin(my_action_srv_node) #프로그램이 종료될때까지 노드 유지

    #키보드 인터럽트(ctrl+c)들어오면 아래 명령 수행됨
    my_action_srv_node.destroy_node() #노드 삭제 및 노드 기능 종료
    rp.shutdown()

if __name__ == '__main__':
    main() #메인 함수 구동

노드 구동방식(설계도) $\rightarrow$ 클래스 설계 부

class ActionSrvNode(Node): #액션서버 기능 설계
    def __init__(self):
        super().__init__("dist_turtle_act_srv_node") #노드명 기입
        self.action_srv = ActionServer(
            node=self, #액션서버가 구동되는 노드 지정이 가능함
            action_type=ActTurtle,
            action_name='act_dist_turtle',
            execute_callback=self.action_srv_callback
        )

        #터틀봇 제어 퍼블리셔
        self.turtle_pub = self.create_publisher(
            msg_type=Twist,
            topic='/turtle1/cmd_vel',
            qos_profile=10
        )
        #터틀봇의 위치/자세값 (x, y, theta순)
        self.pos_att = [5.544445, 5.544445, 0.0]
        #터틀봇의 초기 위치/자세값을 입력

    def dead_reckoning(self, pos_att, cmd_vel_msg, delta_time):
        delta_x = cmd_vel_msg.linear.x * math.cos(pos_att[2]) * delta_time #x 변화량
        delta_y = cmd_vel_msg.angular.z * math.sin(pos_att[2]) * delta_time #y 변화량
        delta_theta = cmd_vel_msg.angular.z * delta_time #theta변화량

        # 위치/자세 업데이트
        pos_att[0] += delta_x
        pos_att[1] += delta_y
        pos_att[2] += delta_theta

        # theta를 -π에서 π 사이의 값으로 정규화
        pos_att[2] = math.atan2(math.sin(pos_att[2]), math.cos(pos_att[2]))

        return pos_att
    
    
    # 액션 서버의 콜백 함수는 지속적+동기적으로 호출되는 
    # 'execute_callback'타입의 콜백함수를 사용한다.
    # 이때 해당 콜백함수가 `Goal`대해 작업을 어떻게 수행중인지? 를 확인하는
    # 핸들러 객체를 인자값으로 받아, 해당 객체에 작업 수행 정보를 리턴하며,
    # 이 액션 작업 수행 정보를 담는 핸들러는 `goal_handle` 이다.
    # `goal_handle`핸들러는 목표('Goal')에 도달하기 위해 기능 수행 중 : EXECUTE 
    # 목표('Goal')에 도달함 : SUCCEED
    # 목표('Goal')가 취소되거나 중단됨 : CANCELED, ABORTED
    # 등과 같은 상태정보를 리턴하고, 위 상태로 변경하는
    # 클래스 함수 : abort(), canceled(), destroy(), succeed() 등을 제공함
    # 목표('Goal')에 도달하기 위해 어떤 기능을 수행하는지를 확인하는
    # Feedback메세지 발행 클래스 함수는 publish_feedback(feedback_msg) 임
    def action_srv_callback(self, goal_handle):
        #해당 액션 서버에서 사용하는 모든 메세지 타입에 대해 인스턴스화
        feedback_msg = ActTurtle.Feedback()
        cmd_turtle_msg = Twist()
        result_msg = ActTurtle.Result()

        # cmd_turtle_msg의 발행주기를 관장하는 시간주기 변수 설정
        delta_time_sec = 1.0
        
        for i in range(goal_handle.request.cmd_num):
            feedback_msg.count = i + 1
            # Feedback에 대한 로그정보(Print) 수행
            self.get_logger().info(f"수행횟수 : {feedback_msg.count}")
            #현재의 수행횟수(feedback)를 발행
            goal_handle.publish_feedback(feedback_msg)

            #터틀봇 제어메세지(cmd_turtle_msg)에 데이터값 기입
            cmd_turtle_msg.linear.x = random.random() * 2
            cmd_turtle_msg.angular.z = random.random() * 2.5

            # 기입한 속도/방향각 정보로 추측항법 구동 -> 위치/자세 업데이트
            self.pos_att = self.dead_reckoning(self.pos_att, 
                                               cmd_turtle_msg,
                                               delta_time_sec)
            #제어메세지 발행하기
            self.turtle_pub.publish(cmd_turtle_msg)
            #제어메세지 발행 간격 정의
            time.sleep(delta_time_sec)

        #액션 서버 콜백함수의 상태를 'goal'달성 -> SUCCEED상태로 변경함
        goal_handle.succeed() 

        # Result_msg에 터틀봇의 위치/자세 정보 기입
        result_msg.pos_x = self.pos_att[0]
        result_msg.pos_y = self.pos_att[1]
        result_msg.pos_theta = self.pos_att[2]

        return result_msg

위 액션 노드 설계도class ActionSrvNode(Node)에서 주요 항목별로 설명을 진행하겠다.

첫번째로 ActionServer클래스의 경우,
서비스(서버/클라이언트)나 토픽(퍼블리셔/섭스크라이버)와는 다르게 node 인자값을 받으며, 액션서버가 구동되는 노드를 지정할 수 있다.

이 이유는 ActionServer에서 설정한 execute_callback(콜백함수)는 다른 콜백 함수와 병행하여 구동되지 않는다.

음.. 그러니까 ActionServer에 Request항목이 입력되면 Goal이 설정되는데 이 Goal설정되면서

ServerGoalHandle라는 Goal의 진행상태를 확인하는 핸들러가 생성된다.

문제는 이 핸들러가 간섭하는 콜백함수가 종료될 때까지 다른 종류의 메서드의 수신기능(서비스Reponse, 토픽Subscriber 기능 구동을 막아버린다.)

이것에 대해 이해하려면 ROS2의 실행 관리 기능(Executor)을 좀 이해해야 하는데

https://docs.ros.org/en/foxy/Concepts/About-Executors.html

이거는 읽어보니까 잘 모르겟다;;;

나름의 요약을 하자면

1) 하나의 노드는 기본적으로는 Single Thread Excuator로 동작한다

2) 콜백 함수는 Grouping이 가능하며, 만약 그룹 옵션을 지정하지 않을 시 콜백함수는 Default Callback Group에 할당되고, 이 그룹에 포함된 콜백 함수는 순차실행만 가능하고 병렬 실행은 지원하지 않는다.

여기서 액션 서버의 콜백함수 : execute_callback함수의 특징으로 인해 서비스서버와 토픽 섭스크라이버의 Callback함수와 같이 사용하는것이 불가능한 이유가 있는데 execute_callback는 Syncronous으로 설정한 Goal에 대한 수행상태가 변화하기 전까지 지속적으로 상태체크(Feedback)를 수행한다 보면 된다.

음.. 쉽게 이해를 해보자면 액션 서버의 콜백함수는 동작하는 순간부터 해당 콜백함수의 기능이 종료되기 전까지 while문으로 해당 함수기능을 계속 수행한다 보면 된다.

그래서 나머지 비동기 호출 방식으로 동작하는 서비스서버와 토픽 섭스크라이버의 Callback함수가 구동이 안되는 것이다.

이러한 문제로 인해서 액션서버는 생성시 Node를 지정하는 옵션을 제공하는것이다.

액션서버가 구동되면 콜백함수를 관리하는 Executor를 혼자 다 차지해버리니 MultiThreadExecutor모드로 동작시켜서 여러개 노드로 구동을 하고, 그중 가장 기능이 적은 노드에 액션서버를 할당해라..

이런 의미인 것이다.

Dead Reckoning

그래서 위에서 액션 서버의 execute_callback 특징을 장황하게 왜 설명을 했느냐?

액션 서버가 Goal에 도달하여 작업을 완료(Succeed)상태가 되면 그에 대한 Result 메세지에 담긴 정보를 출력하게 코드를 작성했는데

이 Result 메세지에 담고자 하는 정보가 터틀봇의 위치/자세정보인 것이 문제여서 이다.

터틀봇의 위치/자세정보는 오직 Pose 메세지 타입으로 구동되는 '/turtle1/pose' 토픽을 구독(subscription)하는 방식으로 정보를 수집할 수 밖에 없고
이는 토픽섭스크라이버용 callback를 설계해야 하는데
이를 진행하려면 MultiThreadExecutor환경을 설계해야 한다.

지금 실습에서 MultiThreadExecutor까지 만드는건 너무 TMI이니 터틀봇 제어 명령을 다루는 '/turtle1/cmd_vel'을 통해 선속도, 각속도 정보를 입력하고, 이 토픽의 발행주기(delta_time)를 지정할 수 있으니

가장 간단한 dead_reckoning함수를 설계하여, 속도+시간주기 정보를 활용해 위치/자세를 추정하고자 한다.

이것이 설계한 dead_reckoning함수이며, 터틀봇에 적용된 동역학이
$속도*시간주기=이동거리$ 로 매우 단순한 동역하기에 이를 간단하게 구현하여 '/turtle1/cmd_vel'의 발행주기마다 추정한
위치/자세 정보를 업데이트하는 코드를 작성한다.

action_srv_callback

execute_callback타입으로 동작하는 액션서버 콜백 함수 action_srv_callback는

상태정보 메세지 : Feedback
목표 완료 시 Report메세지 : Result
터틀봇 제어 명령을 다루는 '/turtle1/cmd_vel' 메세지 타입 : Twist

3가지 메세지 타입 클래스를 다루는 3가지 객체를 각각 인스턴스화 해야한다.

위 3가지 메세지 타입 객체는 딱히 다른 함수나 클래스에서 사용될 일이 없기에 클래스 변수로 선언하지 않고 함수 내부변수로 인스턴스화 한다.

그리고 위 구문에서 액션의 Goal을 정의하는 Request메세지에 데이터가 기입되면(cmd_num)기입된 발행횟수 만큼

Feedback과 '/turtle1/cmd_vel' 메세지를 발행하게끔 코드를 작성한다.

그리고 발행주기는 가장 마지막 time.sleep 함수로 발행간격을 지정한다.

이때 정석대로라면 timer_callback을 사용해서 발행주기를 관리하는 콜백함수를 설계해야 하지만
앞서 설명했듯이 액션서버 콜백 함수는 현재 코드는 다른 콜백함수의 구동을 어렵게 하고 있으니
time.sleep메서드로 해당 기능을 모사한다.

마지막으로 액션서버의 Goal에 다다르는 작업방식을
위 for문의 구동으로 정의했으니, 액션 서버의 상태를 관리하는 goal_handle객체에 액션 서버 상태정보를 SUCCEED로 변경하는
goal_handle.succeed()클래스 함수를 구동한다.

이를 통해 액션 서버 상태가 SUCCEED되면 Result에 기입된 메세지 정보를 출력이 가능하니

추측항법 구동을 통해 추정한 가장 마지막 터틀봇의 위치/자세정보를 메세지에 기입하여 이를 출력한다.

설계한 my_trd_pkg 내 신규노드 파일 my_action1.py 빌드

신규 노드 파일 my_action1.py의 작성이 완료되었으니 my_dist_action_node라는 노드명을 부여하기 위해 setup.py에 위 사진처럼
코드를 기입한다.

기입 후 cmd창에서 패키지 빌드+등록을 수행한다.

$ colcon build --packages-select my_trd_pkg
	#패키지 [my_trd_pkg]빌드
$ r2pkgsetup # 빌드한 패키지의 노드정보 등록

2.1 커스텀 액셕 서버 구동실습

1) bash 1번 스크립트 : 터틀심 노드 구동

$ humble
$ ros2 run turtlesim turtlesim_node #터틀심 노드 구동

2) bash 2번 스크립트 : 액션 서버 노드 구동하기

$ r2pkgsetup
$ ros2 run my_trd_pkg my_dist_action_node 
	#액션 서버 노드 [my_dist_action_node] 구동하기

3) bash 3번 스크립트 : 액션 서버 노드에 Req기입 $\rightarrow$ Goal 설정

$ ros2 action send_goal /act_dist_turtle my_trd_pkg_msgs/action/ActTurtle "{cmd_num: 10}"
	# 액션 서버 명 [act_dist_turtle]에
    # 커스텀 메세지 타입 [my_trd_pkg_msgs/action/ActTurtle]으로
    # Request 메세지 데이터 : "{cmd_num: 10}" 입력

구동 결과는 아래와 같다.