Turtlesim에 새로운 서비스 만들기

저작권: 쿼드(QUAD) 드론연구소 https://smartstore.naver.com/maponarooo

이번 시간에는 이전 시간에 만든 자율주행 Turtlesim 노드에 거북이의 이동경로의 색깔을 변경하는 서비스를 추가해 보도록 하겠습니다.

자동 변경을 위해 화면의 반을 경계로 거북이가 왼쪽에 있을땐 빨간색, 오른쪽에 있을땐 초록색으로 변경 하도록 하겠습니다.

아래의 경로로 이동하여 code 를 실행 합니다.

cd ~/ros2_ws/src/my_robot_controller/my_robot_controller
code .

경로의 색깔을 변경하기 위해 지난 시간에 배웠던 /turtle1/set_pen 서비스를 이용 하겠습니다.

노드에 서비스를 추가하는 전형적인 패턴이므로 꼭 숙지!!!

    def call_set_pen_service(self, r, g, b, width, off):
        client = self.create_client(SetPen, "/turtle1/set_pen")
        while not client.wait_for_service(1.0):
            self.get_logger().warn("Waiting for service...")

        request = SetPen.Request()
        request.r = r
        request.g = g
        request.b = b
        request.width = width
        request.off = off

        future = client.call_async(request)
        future.add_done_callback(partial(self.callback_set_pen))

    def callback_set_pen(self, future):
        try:
            response = future.result()
        except Exception as e:
            self.get_logger().error("Service call failed: %r" % (e,))

다음으로 자동 실행을 위해 pose_callback(self, pose: Pose) 함수에 코드를 추가 합니다.

    def pose_callback(self, pose: Pose):
        cmd = Twist()

        if pose.x > 9.0 or pose.x < 2.0 or pose.y > 9.0 or pose.y < 2.0:
            cmd.linear.x = 1.0
            cmd.angular.z = 1.0
        else:
            cmd.linear.x = 5.0
            cmd.angular.z = 0.0

        self.cmd_vel_publisher_.publish(cmd)
        
        # 화면의 반을 경계로 색깔 변경
        if pose.x > 5.5 and self.previous_x_ <= 5.5:
            self.previous_x_ = pose.x
            self.get_logger().info("Set color to Red!")
            self.call_set_pen_service(255, 0, 0, 3, 0)
        elif pose.x <= 5.5 and self.previous_x_> 5.5:
            self.previous_x_ = pose.x
            self.get_logger().info("Set color to Green!")
            self.call_set_pen_service(0, 255, 0, 3, 0)

전체 코드는 다음과 같습니다.

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from turtlesim.srv import SetPen
from functools import partial

class TurtleControllerNode(Node):

    def __init__(self):
        super().__init__("turtle_controller")
        self.previous_x_ = 0
        self.cmd_vel_publisher_ = self.create_publisher(Twist, "/turtle1/cmd_vel", 10)
        self.pose_subscriber_ = self.create_subscription(Pose, "/turtle1/pose", self.pose_callback, 10)
        self.get_logger().info("Turtle controller node has been started.")
        
    def pose_callback(self, pose: Pose):
        cmd = Twist()

        if pose.x > 9.0 or pose.x < 2.0 or pose.y > 9.0 or pose.y < 2.0:
            cmd.linear.x = 1.0
            cmd.angular.z = 1.0
        else:
            cmd.linear.x = 5.0
            cmd.angular.z = 0.0

        self.cmd_vel_publisher_.publish(cmd)

        if pose.x > 5.5 and self.previous_x_ <= 5.5:
            self.previous_x_ = pose.x
            self.get_logger().info("Set color to Red!")
            self.call_set_pen_service(255, 0, 0, 3, 0)
        elif pose.x <= 5.5 and self.previous_x_> 5.5:
            self.previous_x_ = pose.x
            self.get_logger().info("Set color to Green!")
            self.call_set_pen_service(0, 255, 0, 3, 0)

    def call_set_pen_service(self, r, g, b, width, off):
        client = self.create_client(SetPen, "/turtle1/set_pen")
        while not client.wait_for_service(1.0):
            self.get_logger().warn("Waiting for service...")

        request = SetPen.Request()
        request.r = r
        request.g = g
        request.b = b
        request.width = width
        request.off = off

        future = client.call_async(request)
        future.add_done_callback(partial(self.callback_set_pen))

    def callback_set_pen(self, future):
        try:
            response = future.result()
        except Exception as e:
            self.get_logger().error("Service call failed: %r" % (e,))

def main(args=None):

    rclpy.init(args=args)

    node = TurtleControllerNode()
    rclpy.spin(node)

    rclpy.shutdown()

이제 실행해 보도록 하겠습니다.

turtlesim_node를 실행 합니다.

$ ros2 run turtlesim turtlesim_node

수정한 turtle_controller 노드를 실행 합니다.

$ ros2 run my_robot_controller turtle_controller 

[INFO] [1717842129.616883358] [turtle_controller]: Turtle controller node has been started.

[INFO] [1717842129.617821394] [turtle_controller]: Set color to Red!

[INFO] [1717842134.301539873] [turtle_controller]: Set color to Green!

[INFO] [1717842138.859944427] [turtle_controller]: Set color to Red!

[INFO] [1717842143.890289944] [turtle_controller]: Set color to Green!

[INFO] [1717842148.757529816] [turtle_controller]: Set color to Red!

[INFO] [1717842153.592940163] [turtle_controller]: Set color to Green!

[INFO] [1717842158.066656510] [turtle_controller]: Set color to Red!

[INFO] [1717842162.910421023] [turtle_controller]: Set color to Green!

이상으로 ROS2 중급과정 강좌를 마칩니다. 수고하셨습니다.