무작정 공부하기 - 혼자 공부하는 로봇 SW 따라하기 (9)

썸네일용

 

이번에는 Ch9. 액션 익숙해지기 에 대해서 써보고자 한다.

앞의 글에서 액션을 터미널 명령을 이용하여 사용해 보았다. 이제 간단히 python으로 ROS 액션 클라이언트를 다뤄보고 액션 서버를 만들어보고자 한다.

액션의 목표는 다음 그림과 같이 서비스로 중간 상태는 토픽으로 전달하는 형태이다.

출처 : https://design.ros2.org/articles/actions.html

 

1. 액션 정의 만들기

(1) 액션 정의를 만들기 위한 준비

새롭게 액션을 시작하기 위해서는 액션 정의를 만들어야 한다.

따라서 새로운 정의를 만들기 위한 패키지인 my_first_package_msgs에 새 액션을 추가하고자 한다.

이를 위해서 my_first_package_msgs 폴더에 action이라는 폴더를 만들었다.

 

(2) 액션 정의 만들기

이번 액션에서 받아야 할 정보는 다음과 같다.

---

입력 : 선 속도, 각 속도, 이동할 거리

결과 : turtlesim.x, turtlesim.y, 자세, 이동한 거리

피드백 : 남은 거리

---

이러한 과정을 위해서 action 폴더에 DistTurtle.action 이라고 파일은 생성하고 다음과 같이 입력한다.

 

이 과정을 통해서 액션 정의가 작성되었다.

 

(3) 액션 정의 빌드하기

Cmake 파일에 현재 CmdAndPoseVel과 MultiSpawn이 등록된 상황에서 action/DistTurtle.action이라는 부분을 추가한다.

이후 package.xml 파일에 다음과 같이 action_msgs 를 추가한다.

이후 빌드를 진행한 뒤 제대로 되었는지 확인하기 위해서 interface show 명령을 이용해서 확인한다.

 

2. 간단한 액션 서버로 개념 보기

(1) 간단히 결과를 보여주는 액션 서버

간단하게 액션 서버를 만들어서 액션이 어떻게 동작하는 지 알아보고자 한다.

my_first_package 폴더에 dist_turtle_action_server.py 라는 파일을 만들고 다음과 같이 작성한다.

import rclpy as rp
from rclpy.action import ActionServer
from rclpy.node import Node

from my_first_package_msgs.action import DistTurtle


class DistTurtleServer(Node):

    def __init__(self):
        super().__init__('dist_turtle_action_server')
        self._action_server = ActionServer(
            self,
            DistTurtle,
            'dist_turtle',
            self.execute_callback)

    def execute_callback(self, goal_handle):
        goal_handle.succeed()
        result = DistTurtle.Result()
        return result


def main(args=None):
    rp.init(args=args)
    dist_turtle_action_server = DistTurtleServer()
    rp.spin(dist_turtle_action_server)


if __name__ == '__main__':
    main()

 

코드 내용은 다음과 같다.

rclpy, ActionServer, Node, DistTurtle을 import 하고, Node 클래스의 속성을 상속하는 DistTurtleServer라는 클래스를 만들었다.

이후 ActionServer을 선언하고, 액션 정의를 DistTurtle로 지정하고 그 이름을 dist_turtle이라고 지정한다.

이후 목표가 설정되면 실행해야 하는 execute_callback 함수도 지정해서 작성한다.

 

목표가 설정되면 goal_handle이라는 액션의 상태를 저장하는 메소드가 있어 여기에 succeed()라고 하면 액션이 성공된 상태가 된다.

이후 단순히 이를 성공한 것으로 바로 지정하였다.

 

이후 setup.py에 다음 내용을 추가한다.

이후 워크스페이스의 빌드 이후 dist_turtle_action_server을 실행한다.

 

다음과 같이 결과가 나타나면 잘 동작한다는 것을 알 수 있다.

 

(2) feedback을 액션 서버에 추가해 보기

앞에서 정한 feedback 부분을 추가하여 중간 과정을 토픽으로 발행 할 수 있도록 하고자 한다.

 

따라서 다음과 같이 코드를 수정하였다.

        feedback_msg = DistTurtle.Feedback()
        for n in range(0, 10):
            feedback_msg.remained_dist = float(n)
            goal_handle.publish_feedback(feedback_msg)
            time.sleep(0.5)

전체 코드는 다음과 같다.

import rclpy as rp
from rclpy.action import ActionServer
from rclpy.node import Node
import time

from my_first_package_msgs.action import DistTurtle


class DistTurtleServer(Node):

    def __init__(self):
        super().__init__('dist_turtle_action_server')
        self._action_server = ActionServer(
            self,
            DistTurtle,
            'dist_turtle',
            self.execute_callback)

    def execute_callback(self, goal_handle):
        feedback_msg = DistTurtle.Feedback()
        for n in range(0, 10):
            feedback_msg.remained_dist = float(n)
            goal_handle.publish_feedback(feedback_msg)
            time.sleep(0.5)

        goal_handle.succeed()
        result = DistTurtle.Result()
        return result


def main(args=None):
    rp.init(args=args)
    dist_turtle_action_server = DistTurtleServer()
    rp.spin(dist_turtle_action_server)


if __name__ == '__main__':
    main()

이를 다시 빌드하고 앞과 같은 과정을 거치면 다음과 같이 출력된다.

여기서 액션 명령은 feedback 옵션을 줘야 한다.

 

 

3. ROS2 Multi Thread 기초

현재 진행하고 있는 것은 turtlesim의 움직인 거리를 측정하고, 사용자의 지시에 따라 turtlesim을 구동시키기도 해야 한다.

따라서 이를 위해서는 액션 서버에서 토픽을 발행하고, 구독해야 한다.

 

이때 토픽의 발행은 excute_callback 함수에 넣으면 되지만, 토픽을 구독하기 위해서는 발행할 때마다 실행하는 callback 함수를 하나 더 만들어야 하는데, 앞의 코드처럼 하면 잘 되지 않는다.

그 이유는 현재 callback 함수인 excute_callback이 실행되면 다른 callback 함수는 실행되지 않기 때문이다.

 

따라서 이 문제를 해결하기 위해서 멀티스레드(multi thread)를 사용하고자 한다.

 

따라서 멀티스레드를 익혀보고자 한다.

다음과 같이 my_first_package/my_multi_thread.py 에 작성해 보았다.

import rclpy as rp
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node

from my_first_package.my_publisher import TurtlesimPublisher
from my_first_package.my_subscriber import TurtlesimSubscriber


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

    sub = TurtlesimSubscriber()
    pub = TurtlesimPublisher()

    executor = MultiThreadedExecutor()

    executor.add_node(sub)
    executor.add_node(pub)

    try:
        executor.spin()

    finally:
        executor.shutdown()
#        sub.destroy_node()
        pub.destroy_node()
        rp.shutdown()


if __name__ == '__main__':
    main()

이 코드는 앞에서 만든 Subscriber와 Publisher 코드를 불러와서, 두 클래스를 sub, pub으로 받고, 멀티스레드를 위해서 excutor라는 변수에 MultiThreadedExecutor를 객체화시켰다.

 

이후 add_node 명령으로 TurtlesimPublisher와 TurtlesimSubscriber를 추가하였다.

 

이제 이를 setup.py의 entry_point에 추가하고, 빌드 후 turtlesim과 my_multi_thread를 실행하면 다음과 같은 모습을 볼 수 있다.

 

4. 지정한 거리만큼 이동하는 액션 서버 만들기

이제 목표한 내용을 할 준비가 끝났으므로 하나하나 만들어보고자 한다.

 

< 목표 >

 - 사용자가 turtle에게 속도 명령을 인가한 것을 전달

 - 사용자가 지정한 이동 거리를 계산하여 해당 이동 거리만큼 이동하고 멈추기

 - 이동 거리는 pose 토픽을 구독하여 일정 시간 간격마다 진행한 거리를 계산

 

(1) 전체 코드

import rclpy as rp
from rclpy.action import ActionServer
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node

from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from my_first_package_msgs.action import DistTurtle
from my_first_package.my_subscriber import TurtlesimSubscriber

import math
import time

class TurtleSub_Action(TurtlesimSubscriber):
    def __init__(self, ac_server):
        super().__init__()
        self.ac_server = ac_server

    def callback(self, msg):
        self.ac_server.current_pose = msg

class DistTurtleServer(Node):
    def __init__(self):
        super().__init__('dist_turtle_action_server')
        self.total_dist = 0
        self.is_first_time = True
        self.current_pose = Pose()
        self.previous_pose = Pose()
        self.publisher = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)
        self._action_server = ActionServer(self, DistTurtle, 'dist_turtle', self.execute_callback)

    def calc_diff_pose(self):
        if self.is_first_time:
            self.previous_pose.x = self.current_pose.x
            self.previous_pose.y = self.current_pose.y
            self.is_first_time = False

        diff_dist = math.sqrt((self.current_pose.x - self.previous_pose.x)**2 + (self.previous_pose.y - self.current_pose.y)**2)

        self.previous_pose = self.current_pose

        return diff_dist

    def execute_callback(self, goal_handle):
        feedback_msg = DistTurtle.Feedback()

        msg = Twist()
        msg.linear.x = goal_handle.request.linear_x
        msg.angular.z = goal_handle.request.angular_z

        while True:
            self.total_dist += self.calc_diff_pose()
            feedback_msg.remained_dist = goal_handle.request.dist - self.total_dist
            goal_handle.publish_feedback(feedback_msg)
            self.publisher.publish(msg)
            time.sleep(0.01)

            if feedback_msg.remained_dist < 0.2:
                break

        goal_handle.succeed()
        result = DistTurtle.Result()

        result.pos_x = self.current_pose.x
        result.pos_y = self.current_pose.y
        result.pos_theta = self.current_pose.theta
        result.result_dist = self.total_dist

        self.total_dist = 0
        self.is_first_time = True

        return result


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

    executor = MultiThreadedExecutor()

    ac = DistTurtleServer()
    sub = TurtleSub_Action(ac_server = ac)

    executor.add_node(sub)
    executor.add_node(ac)

    try:
        executor.spin()

    finally:
        executor.shutdown()
        sub.destroy_node()
        ac.destroy_node()
        rp.shutdown()


if __name__ == '__main__':
    main()

 

(2) main: 멀티스레드 사용

import 해서 앞에서 만든 것들을 가져왔고, main() 문에서 앞에서 진행한 멀티스레드를 적용하였다.

여기서 TurtleSub_Action은 turtlesim의 pose 토픽을 받기 위한 클래스이며, 앞에서 만든 DistTurtleServer를 ac라고 받고 TurtleSub_Action을 객체화시킬 때 입력으로 넣었다. 그 이유는 pose를 토픽으로 받고 이를 액션 서버에 전달하기 위해서이다.

 

(3) TurtleSub_Action: pose 토픽 구독

TurtleSub_Action 클래스는 TurtlesimSubscriber 클래스를 상속받고, ac_server라는 입력을 준비해서 적용할 액션 서버를 지정하였다.

이후 callback 함수를 오버라이딩해서 current_pose라는 변수를 준비하고 그 내용을 msg, 즉 pose 토픽의 내용으로 업데이트하였다.

 

(4) DistTurtleServer: 사용자가 지정한 거리만큼 이동

1) DistTurtleServer/__init__()

던더 init 함수에서는 준비한 변수들과 토픽을 발행하고, 액션 서버를 초기화하기 위한 선언들이 있다.

cmd_vel 발행을 위한 publisher를 준비하였으며, 액션 서버를 준비하였다.

 

turtle이 움직인 거리를 한번에 알 수 없기에 거리를 누적해서 기록할 용도인 변수, total_dist를 선언하였다.

액션 서버에서 반복문이 동작할 때 처음 실행인지 확인하기 위한 변수로 is_first_time을 선언하였다. 이를 통해 첫 계산에서 첫 위치를 이동 거리 계산에 포함시키지 않을 수 있다.

 

2) DistTurtleServer/calc_diff_pose()

앞에서 받은 pose 토픽을 이용해서 거리를 계산하였다.

이후 current_pose에 TurtleSub_Action 클래스가 업데이트를 진행한다.

이를 통해 계속해서 이전 위치가 저장되어 거리 계산이 가능해진다.

 

calc_diff_pose가 첫 실행일 때 시작점의 위치를 보정하는 작업을 거치고, 마지막엔 current_pose를 previous_pose에 저장하고 순간적인 거리값을 반환하였다.

 

3) DistTurtleServer/execute_callback()

사전에 만들어둔 액션 정의의 Request 부분 중 속도 명령에 대한 것이 존재한다.

이 부분은 사용자에게 받아서 그대로 cmd_vel 토픽으로 발행할 것이다.

publisher를 앞에서 선언해서 여기서 발행하고 있다.

여기서 사용된 msg는 Twist를 객체화하고 linear.x와 angular.z에 사용자에게 받은 값들을 각각 지정하였다.

 

while True로 무한 반복되는 코드에선 순간적인 이동 거리를 계산하는 결과를 누적해서 합산하며, 유저가 지정한 거리에서 이 결과를 빼서 remained_dist에 저장하였다.

이를 추후에 발행가능하고, 이를 진행하도록 goal.handle.publish_feedback 명령을 사용하였다.

 

남은 거리가 0.2 이내에 들어오면 반복문을 종료하였다.

이는 항상 거리가 0이 되지 않는 부분을 고려하여 값을 잡은 것이다.

이후 액션을 성공으로 상태를 전환한다.

 

결과 반환을 위해서 result를 선언하고, 마지막 turtlesim의 위치를 기록하여 반환하였다.

그리고 나서 추후 다시 명령이 들어왔을 때 거리를 0부터 따지기 위한 부분 또한 추가하였다.

 

5. 액션 서버 간단히 사용해보기

워크스페이스에서 빌드를 진행하고 turtlesim, dist_turtle_action_server를 실행한 뒤, ros2 action 명령으로 send_goal을 피드백이 있는 형식으로 인가하였다.

그러면 다음과 같은 결과를 볼 수 있다.

 

6. 마무리

이렇게 간단한 액션 제작부터 동작, 멀티스레드를 만들어보았다.

다음에는 Ch10. Parameter 다루기에 대해서 작성하고자 한다.

 

github : https://github.com/MOSW626/ros2_study.git

GitHub - MOSW626/ros2_study

DistTurtle.action

0.00MB

dist_turtle_action_server [v1].py

0.00MB

dist_turtle_action_server [v2].py

0.00MB

dist_turtle_action_server [v3_final].py

0.00MB

my_multi_thread.py

0.00MB