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

썸네일용

이번에는 Ch8. 메시지 정의 만들고 토픽과 서비스에서 다루기에 대해서 작성하고자 한다.

이번에는 메시지를 정의하고 이를 다루는 것에 대해서 작성할 것이다.

 

1. 메시지 정의

(1) 메시지 정의를 위한 별도의 패키지 만들기

앞의 7장에서 패키지를 만들 때 사용한 명령은 다음과 같다.

ros2 pkg create --build-type ament_python --node-name my_first_node my_first_package

이 명령어는 빌드 도구를 ament_python으로 하겠다고 하였는데, 이 경우의 colcon은 새로운 메시지 정의를 만들지 못한다.

그 이유는 CMakeLists.txt가 필요하고, 이 파일은 ament_python이 빌드 도구 일 때 사용 불가능하기 때문이다.

 

따라서 C++로 패키지를 만들기 위해서 다음과 같은 명령어를 워크스페이스의 src 폴더에서 실행한다.

ros2 pkg create --build-type ament_cmake my_first_package_msgs

이후 tree 명령어를 실행 시 다음과 같은 결과가 나온다.

 

(2) 메시지 정의 msg definition 만들기

이제 my_first_package_msgs 폴더에 msg라는 폴더를 만들고, 이후 my_first_package_msgs 폴더를 편집기로 열어 CmdAndPoseVel.msg 라는 파일을 만든다.

이후 다음과 같이 작성을 한다.

float32 cmd_vel_linear
float32 cmd_vel_angular

float32 pose_x
float32 pose_y
float32 linear_vel
float32 angular_vel

이는 해석해보면, cmd_vel 토픽의 linear의 x 성분과 angular의 z 성분을 각각 cmd_vel_linear와 cmd_vel_angular로 받고, turtlesim의 pose 토픽에서 x, y 좌표와 linear_velocity, angular_velocity 값을 각각 pose_x, pose_y, linear_vel, angular_vel로 받을 예정이다.

(3) 새로 정의된 msg를 포함한 패키지 빌드하기

이렇게 새로 정의된 msg를 사용하기 위해서는 2개의 파일에 손을 대야 한다.

 >> CMakeList.txt

 >> package.xml

이렇게 두가지 파일이다.

 

먼저, CMake 파일에서 다음 위치에 다음 문구를 추가한다.

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/CmdAndPoseVel.msg"
)

이 문구는 CmdAndPoseVel.msg 파일을 찾아 메시지로 등록해서 빌드하라는 설정을 Cmake에 추가하는 내용이다.

 

이후 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>

이후 워크 스페이스로 돌아가서 빌드한 뒤, ros2 interface show my_first_package_msgs/msg/CmdAndPoseVel

명령을 통해 인식되는지 확인한다.

ros2 interface show my_first_package_msgs/msg/CmdAndPoseVel

 

이를 통해 제대로 msg가 새롭게 잘 인식된 것을 확인 가능하다.

 

2. 두 개의 토픽 구독하고 하나의 토픽을 발행하기

 < 목표 >

 - turtlesim의 pose 토픽 구독

 - my_publisher 노드의 cmd_vel 토픽 구독

 - 데이터 타입을 사용하는 토픽으로 방금 구독한 두 토픽을 저장, 발행

 

(1) turtlesim이 발행하는 pose 토픽 구동 부분부터 시작하기

워크 스페이스의 src 폴더를 code로 연 뒤, my_first_package 폴더에서 turtle_cmd_and_pose.py로 만들고, 이 파일을 setup.py에 추가한다.

import rclpy as rp
from rclpy.node import Node
from turtlesim.msg import Pose

class CmdAndPose(Node):

    def __init__(self):
        super().__init__('turtle_cmd_pose')
        self.sub_pose = self.create_subscription(Pose, '/turtle1/pose', self.callback_pose, 10)

    def callback_pose(self, msg):
        print(msg)

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

    turtle_cmd_pose_node = CmdAndPose()
    rp.spin(turtle_cmd_pose_node)

    turtle_cmd_pose_node.destroy_node()
    rp.shutdown()

if __name__ == '__main__':
    main()

이후 워크 스페이스로 이동하여 빌드를 수행한 뒤, 한쪽에선 turtlesim_node, 한쪽에선 turtle_cmd_and_pose 노드를 실행한다.

이렇게 잘 실행되는 것을 볼 수 있다.

 

(2) 새로 정의한 CmdAndPoseVel 사용해 보기

이제 pose 구독이 잘 되는 것을 확인했으므로, 이제 CmdAndPoseVel이라는 데이터 정의를 사용하기 위해서 각각의 데이터 정의를 이제 사용할 데이터 정의에 저장해야 하기 때문에 토픽 데이터 정의를 import 한다.

코드는 다음과 같고, 던더 init에 CmdAndVelPose를 객체화하는 코드를 포함한다.

from my_first_package_msgs.msg import CmdAndPoseVel

self.cmd_pose = CmdAndPoseVel()

이후 callback_pose 함수의 내용을 turtlesim의 pose를 받아서 CmdAndPoseVel에서 해당하는 속성만 바꾸어 저장하도록 다음과 같은 코드를 작성한다.

    def callback_pose(self, msg):
        self.cmd_pose.pose_x = msg.x
        self.cmd_pose.pose_y = msg.y
        self.cmd_pose.linear_vel = msg.linear_velocity
        self.cmd_pose.angular_vel = msg.angular_velocity
        print(self.cmd_pose)

전체 코드는 다음과 같다.

import rclpy as rp
from rclpy.node import Node
from turtlesim.msg import Pose
from my_first_package_msgs.msg import CmdAndPoseVel

class CmdAndPose(Node):

    def __init__(self):
        super().__init__('turtle_cmd_pose')
        self.sub_pose = self.create_subscription(Pose, '/turtle1/pose', self.callback_pose, 10)
        self.cmd_pose = CmdAndPoseVel()

    def callback_pose(self, msg):
        self.cmd_pose.pose_x = msg.x
        self.cmd_pose.pose_y = msg.y
        self.cmd_pose.linear_vel = msg.linear_velocity
        self.cmd_pose.angular_vel = msg.angular_velocity
        print(self.cmd_pose)

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

    turtle_cmd_pose_node = CmdAndPose()
    rp.spin(turtle_cmd_pose_node)

    turtle_cmd_pose_node.destroy_node()
    rp.shutdown()

if __name__ == '__main__':
    main()

이후 워크 스페이스를 빌드하는데, 하나의 패키지만 빌드하기 위해서 다음 명령어를 실행한다.

colcon build --packages-select my_first_package

이후 다시 turtlesim 노드와 cmd_and_pose를 실행하면 cmd_pose라는 메시지에 특정 데이터들이 잘 저장되는 것을 확인할 수 있다.

(3) cmd_vel 토픽도 구독해 보기

위 상황에서 또 하나의 토픽을 구독하기 위해서 create_subscription을 한번 더 아래와 같이 사용하였고, 콜백 함수를 callback_cmd라고 하였다.

self.sub_cmdvel = self.create_subscription(Twist, '/turtle1/cmd_vel', self.callback_cmd, 10)

여기서 사용하는 메시지 정의는 Twist였고 이를 아래와 같이 지정하였다.

from geometry__msgs.msg import Twist

콜백함수 callback_cmd는 다음과 같이 만들고, cmd_pose에 vel_linear, vel_angular를 각각 cmd_vel, linear의 x, angular의 z를 저장하도록 한다.

    def callback_cmd(self, msg):
        self.cmd_pose.cmd_vel_linear = msg.linear.x
        self.cmd_pose.cmd_vel_angular = msg.angular.z

import rclpy as rp
from rclpy.node import Node
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from my_first_package_msgs.msg import CmdAndPoseVel

class CmdAndPose(Node):

    def __init__(self):
        super().__init__('turtle_cmd_pose')
        self.sub_pose = self.create_subscription(Pose, '/turtle1/pose', self.callback_pose, 10)
        self.sub_cmdvel = self.create_subscription(Twist, '/turtle1/cmd_vel', self.callback_cmd, 10)
        self.cmd_pose = CmdAndPoseVel()

    def callback_pose(self, msg):
        self.cmd_pose.pose_x = msg.x
        self.cmd_pose.pose_y = msg.y
        self.cmd_pose.linear_vel = msg.linear_velocity
        self.cmd_pose.angular_vel = msg.angular_velocity

    def callback_cmd(self, msg):
        self.cmd_pose.cmd_vel_linear = msg.linear.x
        self.cmd_pose.cmd_vel_angular = msg.angular.z

        print(self.cmd_pose)

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

    turtle_cmd_pose_node = CmdAndPose()
    rp.spin(turtle_cmd_pose_node)

    turtle_cmd_pose_node.destroy_node()
    rp.shutdown()

if __name__ == '__main__':
    main()

코드 전문은 다음과 같다.

이후 빌드를 재수행 하고, 터미널을 열어서 turtlesim과 my_publisher, turtle_cmd_and_pose를 실행했다.

이렇게 turtle이 돌고 그 위치를 CmdAndPoseVel의 정의된 데이터에 저장하는 것을 확인할 수 있다.

 

(4) 두 개의 토픽을 구독한 결과를 발행해 보기

현재 만들어진 my_publisher 노드는 0.5초마다 한 번씩 cmd_vel을 발행한다.

이번에는 cmd_and_pose라는 이름의 토픽으로 1초에 한번 발행하도록 하고자 한다.

self.publisher = self.create_publisher(CmdAndPoseVel, '/cmd_and_pose', 10)

이후 일정 간격을 유지하도록 타이머를 timer_callback이 실행되도록 설정하였다.

self.timer = self.create_timer(self.timer_period, self.timer_callback)

그 timer_callback 함수에서 토픽을 발행하도록 다음과 같이 설정하였다.

def timer_callback(self):
    self.publisher.publish(self.cmd_pose)

완성된 코드는 다음과 같다.

import rclpy as rp
from rclpy.node import Node

from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from my_first_package_msgs.msg import CmdAndPoseVel

class CmdAndPose(Node):

    def __init__(self):
        super().__init__('turtle_cmd_pose')
        self.sub_pose = self.create_subscription(Pose, '/turtle1/pose', self.callback_pose, 10)
        self.sub_cmdvel = self.create_subscription(Twist, '/turtle1/cmd_vel', self.callback_cmd, 10)
        self.timer_period = 1.0
        self.publisher = self.create_publisher(CmdAndPoseVel, '/cmd_and_pose', 10)
        self.timer = self.create_timer(self.timer_period, self.timer_callback)
        self.cmd_pose = CmdAndPoseVel()

    def callback_pose(self, msg):
        self.cmd_pose.pose_x = msg.x
        self.cmd_pose.pose_y = msg.y
        self.cmd_pose.linear_vel = msg.linear_velocity
        self.cmd_pose.angular_vel = msg.angular_velocity

    def callback_cmd(self, msg):
        self.cmd_pose.cmd_vel_linear = msg.linear.x
        self.cmd_pose.cmd_vel_angular = msg.angular.z

    def timer_callback(self):
        self.publisher.publish(self.cmd_pose)


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

    turtle_cmd_pose_node = CmdAndPose()
    rp.spin(turtle_cmd_pose_node)

    turtle_cmd_pose_node.destroy_node()
    rp.shutdown()

if __name__ == '__main__':
    main()

위 코드를 다시 빌드하고, 4개의 터미널에서 각각 turtlesim, my_publisher, turtle_cmd_and_pose, rqt_graph를 열었다.

다음과 같은 모습을 볼 수 있으며, 하나의 터미널을 새로 열어 cmd_and_pose를 관찰하고자 topic echo 명령으로 열어보면 다음과 같다.

 

3. 서비스 정의 Service definition 만들어 보기

이번에는 사용자에게 입력받은 수만큼 turtle을 원형으로 배치하는 서비스를 만들어보고자 한다.

수행할 목표는 다음과 같다.

 

 < 목표 >

 - 사용자가 서비스 정의 만들기

 - 서비스 정의를 사용하는 서비스 서버 만들기

 - 서비스 서버에서 클라이언트 만들어 보기

 - 사용자가 요청한 수만큼의 turtle을 생성해서 원형으로 배치하기

(1) 서비스 정의 만들고 빌드하기

서비스 정의를 다루기 위해서 my_first_package_msgs 폴더에 srv 폴더를 만들고 그 안에 MultiSpawn.srv 파일을 만들었다.

이후 내부에 다음과 같이 입력하였다.

int64 num
---
float64[] x
float64[] y
float64[] theta

이 뜻은 int64의 형태로 num 데이터를 받고, float64[]의 형태로 x, y, theta 값을 출력한다는 뜻이다.

 

이후, 아래 사진처럼 srv/MultiSpawn.srv라는 부분을 추가해 주고, package.xml이 다음과 같은지 확인한다.

이후 빌드한 후 ros2 interface show 명령으로 서비스 정의를 조회해 보았다.

 

다음과 같이 정상적으로 출력되었다.

 

(2) 서비스 서버 만들기

이번에는 my_first_package 패키지에 my_service_server.py 파일을 만든다.

그리고 다음과 같이 작성한다.

from my_first_package_msgs.srv import MultiSpawn

import rclpy as rp
from rclpy.node import Node


class MultiSpawning(Node):

    def __init__(self):
        super().__init__('multi_spawn')
        self.server = self.create_service(MultiSpawn, 'multi_spawn', self.callback_service)

    def callback_service(self, request, response):
        print('request : ', request)

        response.x = [1., 2., 3.]
        response.y = [10., 20.]
        response.theta = [100., 200., 300.]

        return response

def main(args=None):
    rp.init(args=args)
    multi_spawn = MultiSpawning()
    rp.spin(multi_spawn)
    rp.shutdown()

if __name__ == '__main__':
    main()

 

이후 serup.py에 다음과 같이 추가한다.

이후 빌드를 진행한 뒤, my_service_server을 열고 다른 터미널에서 서비스 목록을 조회하면 다음과 같이 잘 나오는 것을 볼 수 있다.

이후 다음 명령을 통해 multi_spawn 서비스를 요청하고 이를 print 하는 모습을 볼 수 있다.

 

(3) 서비스 서버 코드 안에 클라이언트 코드 만들기

이제 코드에서 turtlesim의 위치를 이동시키기 위해서 다음과 같은 부분을 작성한다.

from turtlesim.srv import TeleportAbsolute

이후 MultiSpawning 클래스의 던더 init 부분에 TeleportAbsolute 정의를 사용하는 turtle1/teleport_absolute 서비스 클라이언트를 만드는 다음 코드를 추가한다.

self.teleport = self.create_client(TeleportAbsolute, '/turtle1/teleport_absolute')

이후 서비스 클라이언트에서는 서비스 정의를 객체화하는 것이 필요하기 때문에 다음과 같은 코드를 추가한다.

self.req_teleport = TeleportAbsolute.Request()

이후 multi_spawn 서비스 서버의 callback_service 함수에 teleport_absolute 서비스의 클라이언트를 호출하는 다음 코드로 바꿔준다.

    def callback_service(self, request, response):
        self.req_teleport.x = float(1)
        self.teleport.call_async(self.req_teleport)

        return response

전체 코드는 다음과 같다.

from my_first_package_msgs.srv import MultiSpawn
from turtlesim.srv import TeleportAbsolute

import rclpy as rp
from rclpy.node import Node


class MultiSpawning(Node):

    def __init__(self):
        super().__init__('multi_spawn')
        self.server = self.create_service(MultiSpawn, 'multi_spawn', self.callback_service)
        self.teleport = self.create_client(TeleportAbsolute, '/turtle1/teleport_absolute')
        self.req_teleport = TeleportAbsolute.Request()

    def callback_service(self, request, response):
        self.req_teleport.x = float(1)
        self.teleport.call_async(self.req_teleport)

        return response

def main(args=None):
    rp.init(args=args)
    multi_spawn = MultiSpawning()
    rp.spin(multi_spawn)
    rp.shutdown()

if __name__ == '__main__':
    main()

이후 빌드하고, turtlesim과 my_service_server을 실행한 뒤, 다음 서비스를 요청하여 서비스 서버 코드 안에 클라이언트 코드를 넣을 수 있음을 확인한다.

ros2 service call /multi_spawn my_first_package_msgs/srv/MultiSpawn  "{num: 1}"

 

(4) 여러 거북이를 원 모양으로 배치하기

목적에 맞게 거북이를 배치하고자 한다.

이렇게 할 때 메인코드에 바로 작성하면 매번 빌드해야 하기에 jupyter notebook을 통해서 진행하고자 한다.

 

그리고 home의 python 폴더에 Calc Position 파일을 만들고 진행하고자 한다.

---

다음과 같이 작성하여 개수가 주어졌을 때 각 원 사이의 각도를 구할 수 있다.

import numpy as np

n = 3
to_degree = 180/np.pi

gap_theta = 2*np.pi / n
gap_theta * to_degree

그 뒤에 list 형으로 원의 각도를 저장하기 위해서 다음과 같이 작성한다.

theta = [gap_theta*n for n in range(n)]
[each * to_degree for each in theta]

이는 gap_theta를 이용하여 원의 개수 n 만큼의 각도를 미리 생성해서 list에 저장하고, 0부터 n-1만큼 숫자를 생성하고 이를 gap_theta에 곱해서 리스트형에 저장하였다.

 

그리고 삼각함수를 이용해서 x, y 좌표를 구하였다.

r = 3

x = [r*np.cos(th) for th in theta]
y = [r*np.sin(th) for th in theta]

그리고 이를 scatter 함수를 이용해 그려보았다.

import matplotlib.pyplot as plt

plt.scatter(x, y)
plt.axis('equal')
plt.show()

이후 원을 배치하는 코드를 함수로 작성하였다.

def calc_position(n, r):
    gap_theta = 2*np.pi / n
    theta = [gap_theta*n for n in range(n)]
    x = [r*np.cos(th) for th in theta]
    y = [r*np.sin(th) for th in theta]

    return x, y, theta

지금까지의 결과를 그래프로 확인하기 위해서 다음 코드를 작성하였다.

def draw_pos(x, y):
    plt.scatter(x, y)
    plt.axis('equal')
    plt.show()

이후 테스트를 한 결과로, 잘 구현되었음을 알 수 있다.

X, Y, theta = calc_position(4, 3)
draw_pos(X, Y)

X, Y, theta = calc_position(15,3)
draw_pos(X, Y)

 

(5) 여러 거북이를 배치하는 서비스 서버 구현

이제 위에 있던 코드들을 원래 사용하던 서비스 서버에 넣기 위해서

다음과 같이 Spawn을 import 하고, numpy까지 import 한다.

from turtlesim.srv import Spawn
import numpy as np

그리고 spawn 서비스를 이용하기 위해서 다음과 같은 내용을 던더 init에 추가한다.

self.spawn = self.create_client(Spawn, '/spawn')

또한 화면의 센터값이 5.54 임을 고려하여 center 값 보정을 진행한다.

self.center_x = 5.54
self.center_y = 5.54

그러고 나서 spawn 서비스를 사용하기 위해서 Spawn 서비스 정의를 초기화하기 위해 다음 내용을 추가한 뒤, calc_position 함수를 삽입하되, self를 인자로 받도록 한다.

self.req_spawn = Spawn.Request()

 

모두 적용한 코드는 다음과 같다.

from my_first_package_msgs.srv import MultiSpawn
from turtlesim.srv import TeleportAbsolute
from turtlesim.srv import Spawn

import rclpy as rp
import numpy as np
from rclpy.node import Node


class MultiSpawning(Node):

    def __init__(self):
        super().__init__('multi_spawn')
        self.server = self.create_service(MultiSpawn, 'multi_spawn', self.callback_service)
        self.teleport = self.create_client(TeleportAbsolute, '/turtle1/teleport_absolute')
        self.spawn = self.create_client(Spawn, '/spawn')
        self.req_teleport = TeleportAbsolute.Request()
        self.req_spawn = Spawn.Request()
        self.center_x = 5.54
        self.center_y = 5.54


    def calc_position(self, n, r):
        gap_theta = 2*np.pi / n
        theta = [gap_theta*n for n in range(n)]
        x = [r*np.cos(th) for th in theta]
        y = [r*np.sin(th) for th in theta]

        return x, y, theta

    def callback_service(self, request, response):
        x, y, theta = self.calc_position(request.num, 3)

        for n in range(len(theta)):
            self.req_spawn.x = x[n] + self.center_x
            self.req_spawn.y = y[n] + self.center_y
            self.req_spawn.theta = theta[n]
            self.spawn.call_async(self.req_spawn)

        response.x = x
        response.y = y
        response.theta = theta

        return response

def main(args=None):
    rp.init(args=args)
    multi_spawn = MultiSpawning()
    rp.spin(multi_spawn)
    rp.shutdown()

if __name__ == '__main__':
    main()

 

(6) 여러 거북이를 등장시키기

이후 빌드하고, turtlesim, my_service_server을 열고 서비스에 {num: 9}로 서비스를 요청한다.

다음과 같이 성공적으로 되는 것을 확인할 수 있다.

 

4. 마무리

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

또한 이 과정에서 사용한 파일을 다음과 같이 올려본다.

 

MultiSpawn.srv

0.00MB

my_service_server [v1].py

0.00MB

my_service_server [v2].py

0.00MB

my_service_server [v3_fianl].py

0.00MB

turtle_cmd_and_pose [v1].py

0.00MB

turtle_cmd_and_pose [v2].py

0.00MB

turtle_cmd_and_pose [v3].py

0.00MB

turtle_cmd_and_pose [v4_final].py

0.00MB

Calc Position.ipynb

0.03MB