Action 실습 — Goal부터 취소와 실제 로봇까지

Action Client                         Action Server
     │ Goal(distance=1.0)                  │
     ├────────────────────────────────────>│
     │ Goal accepted                       │
     │<────────────────────────────────────┤
     │ Feedback(remaining=0.7)             │
     │<────────────────────────────────────┤
     │ Feedback(remaining=0.3)             │
     │<────────────────────────────────────┤
     │ Cancel request                      │
     ├────────────────────────────────────>│
     │ Result(canceled)                    │
     │<────────────────────────────────────┤

Goal 요청 → ACCEPTED → EXECUTING ─┬→ SUCCEEDED
                                  ├→ CANCELING → CANCELED
                                  └→ ABORTED
Goal 요청 → REJECTED

ros2 node info /fibonacci_action_server
ros2 action list -t

source /opt/ros/$ROS_DISTRO/setup.bash
printenv ROS_DISTRO
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws

source ~/ros2_ws/install/setup.bash

Terminal A: Action Server
Terminal B: Action Client 또는 CLI
Terminal C: Graph·Topic·Log 관찰

ros2 run action_tutorials_py fibonacci_action_server

ros2 action list
ros2 action list -t
ros2 action type /fibonacci
ros2 interface show action_tutorials_interfaces/action/Fibonacci

ros2 action send_goal /fibonacci \
  action_tutorials_interfaces/action/Fibonacci \
  "{order: 10}" --feedback

ros2 interface proto action_tutorials_interfaces/action/Fibonacci
ros2 action info /fibonacci

ros2 action send_goal /fibonacci \
  action_tutorials_interfaces/action/Fibonacci \
  "{order: 1000}" --feedback

# Goal
float32 distance_m
float32 max_speed_mps
---
# Result
float32 traveled_m
string message
---
# Feedback
float32 traveled_m
float32 remaining_m

int32 OK=0
int32 BLOCKED=1
int32 ODOMETRY_LOST=2
int32 error_code
string message

cd ~/ros2_ws/src
ros2 pkg create lab_interfaces --build-type ament_cmake
mkdir -p lab_interfaces/action

find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "action/DriveDistance.action"
)

ament_export_dependencies(rosidl_default_runtime)
ament_package()

<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

cd ~/ros2_ws
colcon build --packages-select lab_interfaces --symlink-install
source install/setup.bash
ros2 interface show lab_interfaces/action/DriveDistance

cd ~/ros2_ws/src
ros2 pkg create drive_action_py \
  --build-type ament_python \
  --dependencies rclpy lab_interfaces

entry_points={
    'console_scripts': [
        'drive_server = drive_action_py.drive_server:main',
        'drive_client = drive_action_py.drive_client:main',
        'cancel_client = drive_action_py.cancel_client:main',
    ],
},

# drive_action_py/drive_server.py
import asyncio
import math

import rclpy
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.node import Node

from lab_interfaces.action import DriveDistance


class DriveDistanceServer(Node):
    def __init__(self):
        super().__init__('drive_distance_server')
        self._server = ActionServer(
            self,
            DriveDistance,
            'drive_distance',
            execute_callback=self.execute_callback,
            goal_callback=self.goal_callback,
            cancel_callback=self.cancel_callback,
        )

    def goal_callback(self, goal_request):
        distance = float(goal_request.distance_m)
        speed = float(goal_request.max_speed_mps)
        if not math.isfinite(distance) or not math.isfinite(speed):
            self.get_logger().warning('NaN 또는 Infinity Goal 거절')
            return GoalResponse.REJECT
        if abs(distance) > 5.0 or not 0.02 <= speed <= 0.5:
            self.get_logger().warning('허용 범위를 벗어난 Goal 거절')
            return GoalResponse.REJECT
        return GoalResponse.ACCEPT

    def cancel_callback(self, goal_handle):
        self.get_logger().info('Cancel 요청 수락')
        return CancelResponse.ACCEPT

    async def execute_callback(self, goal_handle):
        goal = goal_handle.request
        direction = 1.0 if goal.distance_m >= 0.0 else -1.0
        traveled = 0.0
        dt = 0.1
        feedback = DriveDistance.Feedback()

        while traveled < abs(goal.distance_m):
            if goal_handle.is_cancel_requested:
                goal_handle.canceled()
                return self.make_result(
                    direction * traveled, '사용자 요청으로 취소됨')

            await asyncio.sleep(dt)
            traveled = min(
                traveled + float(goal.max_speed_mps) * dt,
                abs(float(goal.distance_m)),
            )
            feedback.traveled_m = direction * traveled
            feedback.remaining_m = max(abs(goal.distance_m) - traveled, 0.0)
            goal_handle.publish_feedback(feedback)

        goal_handle.succeed()
        return self.make_result(direction * traveled, '목표 거리 도달')

    @staticmethod
    def make_result(traveled, message):
        result = DriveDistance.Result()
        result.traveled_m = float(traveled)
        result.message = message
        return result

    def destroy_node(self):
        self._server.destroy()
        super().destroy_node()


def main(args=None):
    rclpy.init(args=args)
    node = DriveDistanceServer()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

입력 검증 → Goal 수락 → 반복 실행 → Feedback
                         ├ Cancel 확인 → canceled() → Result
                         ├ 정상 완료 → succeed() → Result
                         └ 실행 불가 → abort() → Result

# drive_action_py/drive_client.py
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

from lab_interfaces.action import DriveDistance


class DriveDistanceClient(Node):
    def __init__(self):
        super().__init__('drive_distance_client')
        self._client = ActionClient(
            self, DriveDistance, 'drive_distance')

    def send_goal(self, distance, speed):
        if not self._client.wait_for_server(timeout_sec=5.0):
            self.get_logger().error('Action Server 발견 Timeout')
            rclpy.shutdown()
            return

        goal = DriveDistance.Goal()
        goal.distance_m = float(distance)
        goal.max_speed_mps = float(speed)

        future = self._client.send_goal_async(
            goal, feedback_callback=self.feedback_callback)
        future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        try:
            self._goal_handle = future.result()
        except Exception as exc:
            self.get_logger().error(f'Goal 전송 실패: {exc}')
            rclpy.shutdown()
            return

        if not self._goal_handle.accepted:
            self.get_logger().warning('Goal이 거절되었습니다.')
            rclpy.shutdown()
            return

        self.get_logger().info('Goal 수락됨')
        result_future = self._goal_handle.get_result_async()
        result_future.add_done_callback(self.result_callback)

    def feedback_callback(self, message):
        feedback = message.feedback
        self.get_logger().info(
            f'이동={feedback.traveled_m:.2f} m, '
            f'남음={feedback.remaining_m:.2f} m')

    def result_callback(self, future):
        try:
            wrapped = future.result()
            result = wrapped.result
            self.get_logger().info(
                f'status={wrapped.status}, '
                f'traveled={result.traveled_m:.2f}, '
                f'message={result.message}')
        except Exception as exc:
            self.get_logger().error(f'Result 수신 실패: {exc}')
        finally:
            rclpy.shutdown()


def main(args=None):
    rclpy.init(args=args)
    node = DriveDistanceClient()
    node.send_goal(distance=1.0, speed=0.2)
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()


if __name__ == '__main__':
    main()

Server 발견 → send_goal_async
                  ↓
            Goal 수락 여부
              ├ 거절 → 종료
              └ 수락 → Feedback 반복 + get_result_async
                                             ↓
                                        최종 Status·Result

cd ~/ros2_ws
colcon build --packages-select lab_interfaces drive_action_py --symlink-install
source install/setup.bash

source ~/ros2_ws/install/setup.bash
ros2 run drive_action_py drive_server

source ~/ros2_ws/install/setup.bash
ros2 run drive_action_py drive_client

ros2 action send_goal /drive_distance \
  lab_interfaces/action/DriveDistance \
  "{distance_m: 1.0, max_speed_mps: 0.2}" --feedback

ros2 action send_goal /drive_distance \
  lab_interfaces/action/DriveDistance \
  "{distance_m: 100.0, max_speed_mps: 9.0}" --feedback

# drive_action_py/cancel_client.py
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

from lab_interfaces.action import DriveDistance


class CancelingClient(Node):
    def __init__(self):
        super().__init__('canceling_drive_client')
        self.client = ActionClient(self, DriveDistance, 'drive_distance')
        self.goal_handle = None
        self.timer = None

    def start(self):
        if not self.client.wait_for_server(timeout_sec=5.0):
            raise RuntimeError('Action Server를 찾지 못했습니다.')
        goal = DriveDistance.Goal()
        goal.distance_m = 3.0
        goal.max_speed_mps = 0.2
        future = self.client.send_goal_async(goal)
        future.add_done_callback(self.on_goal_response)

    def on_goal_response(self, future):
        self.goal_handle = future.result()
        if not self.goal_handle.accepted:
            self.get_logger().warning('Goal 거절')
            rclpy.shutdown()
            return
        result_future = self.goal_handle.get_result_async()
        result_future.add_done_callback(self.on_result)
        self.timer = self.create_timer(2.0, self.request_cancel)

    def request_cancel(self):
        self.timer.cancel()
        self.get_logger().info('Cancel 요청 전송')
        future = self.goal_handle.cancel_goal_async()
        future.add_done_callback(self.on_cancel_response)

    def on_cancel_response(self, future):
        response = future.result()
        if response.goals_canceling:
            self.get_logger().info('Server가 Cancel 요청을 수락함')
        else:
            self.get_logger().warning('Cancel 요청이 수락되지 않음')

    def on_result(self, future):
        wrapped = future.result()
        self.get_logger().info(
            f'최종 status={wrapped.status}, message={wrapped.result.message}')
        rclpy.shutdown()


def main(args=None):
    rclpy.init(args=args)
    node = CancelingClient()
    node.start()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()


if __name__ == '__main__':
    main()

from threading import Lock
from rclpy.action import GoalResponse

self._goal_lock = Lock()
self._busy = False

def goal_callback(self, request):
    with self._goal_lock:
        if self._busy:
            return GoalResponse.REJECT
        self._busy = True
    return GoalResponse.ACCEPT

새 Goal 검증 → 기존 Goal Cancel 요청 → 실제 Actuator 정지 확인
            → 새 Goal 소유권 부여 → 새 Goal 실행

from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor

self.action_group = ReentrantCallbackGroup()
self.server = ActionServer(
    self,
    DriveDistance,
    'drive_distance',
    execute_callback=self.execute_callback,
    callback_group=self.action_group,
)

executor = MultiThreadedExecutor(num_threads=2)
executor.add_node(node)
executor.spin()

import math

def planar_distance(start_x, start_y, current_x, current_y):
    return math.hypot(current_x - start_x, current_y - start_y)

Action Server ──> /cmd_vel_action ──> Velocity Mux ──> Safety Filter
                                                          │
/odom ─────────> 진행 거리 계산                           ↓
/scan ─────────> 장애물 조건                         Motor Driver
/estop_state ──> 독립 정지 조건

# 개념 예시: 실제 mux Package의 Schema에 맞게 조정
inputs:
  joystick:
    topic: /cmd_vel_joy
    priority: 100
    timeout: 0.3
  navigation:
    topic: /cmd_vel_nav
    priority: 50
    timeout: 0.5
  action:
    topic: /cmd_vel_action
    priority: 40
    timeout: 0.3

if self.estop_active:
    self.publish_stop()
    goal_handle.abort()
    return self.make_result(traveled, '비상정지 상태')

if self.odom_age_sec() > 0.2:
    self.publish_stop()
    goal_handle.abort()
    return self.make_result(traveled, 'Odometry Timeout')

if self.front_clearance_m < self.stop_distance_m:
    self.publish_stop()
    goal_handle.abort()
    return self.make_result(traveled, '전방 장애물')

try:
    await self.control_until_goal(goal_handle)
finally:
    self.publish_stop()

ros2 run drive_action_py drive_server --ros-args -r __ns:=/robot1
ros2 run drive_action_py drive_server --ros-args -r __ns:=/robot2
ros2 action list -t

ros2 run drive_action_py drive_client --ros-args \
  -r drive_distance:=/robot1/drive_distance

ros2 action list -t
ros2 action type /navigate_to_pose
ros2 interface show nav2_msgs/action/NavigateToPose
ros2 action info /navigate_to_pose

ros2 action send_goal /navigate_to_pose \
  nav2_msgs/action/NavigateToPose \
  "{pose: {header: {frame_id: map}, pose: {position: {x: 1.0, y: 0.5, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}}" \
  --feedback

ros2 topic echo /amcl_pose --once
ros2 run tf2_ros tf2_echo map base_link
ros2 topic hz /scan
ros2 topic hz /odom

ros2 action list -t
ros2 action info /drive_distance
ros2 node list
ros2 node info /drive_distance_server
ros2 interface show lab_interfaces/action/DriveDistance
ros2 topic echo /rosout