[ROS2] EP16. 일정 거리 이동시키는 액션 서버 구현하기

---

• OS : Ubuntu 22.04
• ROS 2 Humble
• Gazebo 11

---

1. 저장한 거리만큼 거북이 보내기

1. 코드 수정

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

import time
import math

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

class TurtleSub_Action(TurtlesimSubscriber):
    def __init__(self):
        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,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_pose = math.sqrt((self.current_pose.x - self.previous_pose.x)**2 + (self.current_pose.y - self.previous_pose.y)**2) 

        self.previous_pose = self.current_pose

        return diff_pose


    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.1)

            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.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(ac)
    executor.add_node(sub)

    try:
        executor.spin()

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

if __name__ == '__main__':
    main()

• Pose는 터틀심이 발행하는 Pose 토픽을 구독하려면 알아야하는 데이터 타입
• Twist는 cmd-vel 토픽이 사용하는 데이터 타입
• DistTurtle은 만드려고 하는 액션 서버가 사용하는 데이터 타입
• main 함수는 멀티쓰레드

2. 빌드 후 환경 부르기

• colcon build
• ros2study

3. 액션 서버 실행

4. 액션 클라이언트 설정

• send_goal

5. rqt_graph 확인

• /dist_turtle/_action/feedback 토픽이 움직이는 것을 확인