Initial commit

dev_ws/src/mission_manager/config/mission_manager.yaml

@@ -0,0 +1,17 @@
+mission_manager:
+  ros__parameters:
+    autostart: false
+    auto_arm_complete_timeout_sec: 8.0
+    platform_side: "left"
+    blue_zone_label: "blue"
+    green_zone_label: "green"
+    approach_speed_mps: 0.18
+    slope_speed_mps: 0.32
+    lateral_speed_mps: 0.12
+    turn_speed_radps: 0.65
+    yaw_tolerance_deg: 5.0
+    ultrasonic_stop_distance_m: 0.35
+    signboard_stop_distance_m: 0.70
+    board_hold_sec: 3.0
+    alignment_tolerance: 0.12
+    max_wait_signal_board_sec: 30.0

dev_ws/src/mission_manager/launch/mission_manager.launch.py

@@ -0,0 +1,21 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    config = os.path.join(
+        get_package_share_directory('mission_manager'),
+        'config',
+        'mission_manager.yaml'
+    )
+    return LaunchDescription([
+        Node(
+            package='mission_manager',
+            executable='mission_manager',
+            name='mission_manager',
+            output='screen',
+            parameters=[config],
+        )
+    ])

dev_ws/src/mission_manager/mission_manager/mission_manager_node.py

@@ -0,0 +1,310 @@
+import math
+from enum import Enum, auto
+from typing import Optional
+
+import rclpy
+from geometry_msgs.msg import PointStamped, Twist
+from nav_msgs.msg import Odometry
+from rclpy.duration import Duration
+from rclpy.node import Node
+from sensor_msgs.msg import Imu, Range
+from std_msgs.msg import Bool, String
+
+
+class MissionState(Enum):
+    IDLE = auto()
+    APPROACH_SLOPE_ENTRY = auto()
+    PICK_AND_PLACE_SPONGE = auto()
+    TURN_LEFT_90 = auto()
+    RUSH_SLOPE = auto()
+    CHECK_SIGNAL_BOARD = auto()
+    MOVE_TO_BLOCK_1 = auto()
+    PICK_BLOCK_1 = auto()
+    PLACE_BLOCK_1 = auto()
+    MOVE_TO_BLOCK_2 = auto()
+    PICK_BLOCK_2 = auto()
+    PLACE_BLOCK_2 = auto()
+    FINISHED = auto()
+
+
+class MissionManager(Node):
+    def __init__(self) -> None:
+        super().__init__('mission_manager')
+
+        self.declare_parameter('autostart', False)
+        self.declare_parameter('auto_arm_complete_timeout_sec', 2.0)
+        self.declare_parameter('platform_side', 'left')
+        self.declare_parameter('blue_zone_label', 'blue')
+        self.declare_parameter('green_zone_label', 'green')
+        self.declare_parameter('approach_speed_mps', 0.18)
+        self.declare_parameter('slope_speed_mps', 0.32)
+        self.declare_parameter('lateral_speed_mps', 0.12)
+        self.declare_parameter('turn_speed_radps', 0.65)
+        self.declare_parameter('yaw_tolerance_deg', 5.0)
+        self.declare_parameter('ultrasonic_stop_distance_m', 0.35)
+        self.declare_parameter('signboard_stop_distance_m', 0.70)
+        self.declare_parameter('board_hold_sec', 3.0)
+        self.declare_parameter('alignment_tolerance', 0.12)
+        self.declare_parameter('max_wait_signal_board_sec', 30.0)
+
+        self.autostart = self.get_parameter('autostart').value
+        self.auto_arm_complete_timeout_sec = float(self.get_parameter('auto_arm_complete_timeout_sec').value)
+        self.platform_side = str(self.get_parameter('platform_side').value)
+        self.blue_zone_label = str(self.get_parameter('blue_zone_label').value)
+        self.green_zone_label = str(self.get_parameter('green_zone_label').value)
+        self.approach_speed_mps = float(self.get_parameter('approach_speed_mps').value)
+        self.slope_speed_mps = float(self.get_parameter('slope_speed_mps').value)
+        self.lateral_speed_mps = float(self.get_parameter('lateral_speed_mps').value)
+        self.turn_speed_radps = float(self.get_parameter('turn_speed_radps').value)
+        self.yaw_tolerance_rad = math.radians(float(self.get_parameter('yaw_tolerance_deg').value))
+        self.ultrasonic_stop_distance_m = float(self.get_parameter('ultrasonic_stop_distance_m').value)
+        self.signboard_stop_distance_m = float(self.get_parameter('signboard_stop_distance_m').value)
+        self.board_hold_sec = float(self.get_parameter('board_hold_sec').value)
+        self.alignment_tolerance = float(self.get_parameter('alignment_tolerance').value)
+        self.max_wait_signal_board_sec = float(self.get_parameter('max_wait_signal_board_sec').value)
+
+        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)
+        self.arm_cmd_pub = self.create_publisher(String, '/arm_command', 10)
+        self.state_pub = self.create_publisher(String, '/mission_manager/state', 10)
+
+        self.create_subscription(Range, '/ultrasonic/front/range', self.on_range, 10)
+        self.create_subscription(Imu, '/imu', self.on_imu, 10)
+        self.create_subscription(Odometry, '/odom', self.on_odom, 10)
+        self.create_subscription(String, '/signal_board/result', self.on_signal_board, 10)
+        self.create_subscription(PointStamped, '/depth_target/point', self.on_depth_target, 10)
+        self.create_subscription(String, '/arm/status', self.on_arm_status, 10)
+        self.create_subscription(Bool, '/mission/start', self.on_start, 10)
+
+        self.timer = self.create_timer(0.05, self.on_timer)
+
+        self.state = MissionState.APPROACH_SLOPE_ENTRY if self.autostart else MissionState.IDLE
+        self.current_range_m: Optional[float] = None
+        self.current_yaw: Optional[float] = None
+        self.current_pose = None
+        self.signal_board_result = 'unknown'
+        self.depth_target: Optional[PointStamped] = None
+        self.arm_busy = False
+        self.pending_arm_done_deadline = None
+        self.turn_start_yaw: Optional[float] = None
+        self.state_enter_time = self.get_clock().now()
+        self.block_pick_count = 0
+        self.sponge_done = False
+        self.signal_idle_seen = False
+
+        self.publish_state('READY' if self.autostart else 'IDLE_WAIT_START')
+
+    def on_range(self, msg: Range) -> None:
+        self.current_range_m = float(msg.range)
+
+    def on_imu(self, msg: Imu) -> None:
+        q = msg.orientation
+        siny_cosp = 2.0 * (q.w * q.z + q.x * q.y)
+        cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
+        self.current_yaw = math.atan2(siny_cosp, cosy_cosp)
+
+    def on_odom(self, msg: Odometry) -> None:
+        self.current_pose = msg.pose.pose
+
+    def on_signal_board(self, msg: String) -> None:
+        self.signal_board_result = msg.data.strip()
+
+    def on_depth_target(self, msg: PointStamped) -> None:
+        self.depth_target = msg
+
+    def on_arm_status(self, msg: String) -> None:
+        data = msg.data.strip().lower()
+        if data in {'busy', 'running'}:
+            self.arm_busy = True
+        elif data in {'done', 'idle', 'ok', 'success'}:
+            self.arm_busy = False
+            self.pending_arm_done_deadline = None
+        elif data in {'failed', 'error'}:
+            self.arm_busy = False
+            self.pending_arm_done_deadline = None
+            self.get_logger().warning('Arm reported failure, mission pauses at current state.')
+
+    def on_start(self, msg: Bool) -> None:
+        if msg.data and self.state == MissionState.IDLE:
+            self.transition_to(MissionState.APPROACH_SLOPE_ENTRY, 'external start signal')
+        elif (not msg.data) and self.state != MissionState.IDLE:
+            self.stop_robot()
+            self.transition_to(MissionState.IDLE, 'external stop signal')
+
+    def publish_state(self, detail: str) -> None:
+        msg = String()
+        msg.data = f'{self.state.name}:{detail}'
+        self.state_pub.publish(msg)
+
+    def transition_to(self, new_state: MissionState, reason: str) -> None:
+        self.stop_robot()
+        self.state = new_state
+        self.state_enter_time = self.get_clock().now()
+        if new_state != MissionState.TURN_LEFT_90:
+            self.turn_start_yaw = None
+        self.publish_state(reason)
+        self.get_logger().info(f'Transition -> {new_state.name}: {reason}')
+
+    def state_elapsed(self) -> float:
+        return (self.get_clock().now() - self.state_enter_time).nanoseconds / 1e9
+
+    def stop_robot(self) -> None:
+        self.cmd_pub.publish(Twist())
+
+    def send_cmd(self, linear_x: float = 0.0, linear_y: float = 0.0, angular_z: float = 0.0) -> None:
+        cmd = Twist()
+        cmd.linear.x = float(linear_x)
+        cmd.linear.y = float(linear_y)
+        cmd.angular.z = float(angular_z)
+        self.cmd_pub.publish(cmd)
+
+    def send_arm_command(self, command: str) -> None:
+        msg = String()
+        msg.data = command
+        self.arm_cmd_pub.publish(msg)
+        self.arm_busy = True
+        self.pending_arm_done_deadline = self.get_clock().now() + Duration(seconds=self.auto_arm_complete_timeout_sec)
+        self.get_logger().info(f'Arm command -> {command}')
+
+    def arm_action_finished(self) -> bool:
+        if not self.arm_busy:
+            return True
+        if self.pending_arm_done_deadline is not None and self.get_clock().now() >= self.pending_arm_done_deadline:
+            self.arm_busy = False
+            self.pending_arm_done_deadline = None
+            self.get_logger().warning('Arm timeout auto-completed for integration testing.')
+            return True
+        return False
+
+    @staticmethod
+    def angle_wrap(angle: float) -> float:
+        while angle > math.pi:
+            angle -= 2.0 * math.pi
+        while angle < -math.pi:
+            angle += 2.0 * math.pi
+        return angle
+
+    def depth_target_aligned(self) -> bool:
+        if self.depth_target is None:
+            return False
+        return abs(self.depth_target.point.y) <= self.alignment_tolerance
+
+    def on_timer(self) -> None:
+        if self.state == MissionState.IDLE:
+            self.stop_robot()
+            return
+
+        if self.state == MissionState.APPROACH_SLOPE_ENTRY:
+            if (not self.sponge_done) and self.depth_target is not None and self.depth_target.point.x <= 0.55:
+                self.stop_robot()
+                self.send_arm_command(f'pick_and_place_platform:{self.platform_side}')
+                self.sponge_done = True
+                self.transition_to(MissionState.PICK_AND_PLACE_SPONGE, 'depth target in pickup window')
+                return
+
+            if self.current_range_m is not None and self.current_range_m <= self.ultrasonic_stop_distance_m:
+                self.transition_to(MissionState.TURN_LEFT_90, 'ultrasonic reached slope-entry stop line')
+                return
+
+            self.send_cmd(linear_x=self.approach_speed_mps)
+            return
+
+        if self.state == MissionState.PICK_AND_PLACE_SPONGE:
+            if self.arm_action_finished():
+                self.transition_to(MissionState.APPROACH_SLOPE_ENTRY, 'sponge placed on side platform')
+            return
+
+        if self.state == MissionState.TURN_LEFT_90:
+            if self.current_yaw is None:
+                self.stop_robot()
+                return
+            if self.turn_start_yaw is None:
+                self.turn_start_yaw = self.current_yaw
+            delta = self.angle_wrap(self.current_yaw - self.turn_start_yaw)
+            target = math.pi / 2.0
+            if abs(target - abs(delta)) <= self.yaw_tolerance_rad:
+                self.transition_to(MissionState.RUSH_SLOPE, 'left turn completed')
+                return
+            self.send_cmd(angular_z=self.turn_speed_radps)
+            return
+
+        if self.state == MissionState.RUSH_SLOPE:
+            if self.current_range_m is not None and self.current_range_m <= self.signboard_stop_distance_m:
+                self.transition_to(MissionState.CHECK_SIGNAL_BOARD, 'arrived at signboard check point')
+                return
+            self.send_cmd(linear_x=self.slope_speed_mps)
+            return
+
+        if self.state == MissionState.CHECK_SIGNAL_BOARD:
+            self.stop_robot()
+            if self.signal_board_result.startswith('idle'):
+                if not self.signal_idle_seen:
+                    self.signal_idle_seen = True
+                    self.state_enter_time = self.get_clock().now()
+                    self.publish_state(f'signal board idle confirmed: {self.signal_board_result}')
+                elif self.state_elapsed() >= self.board_hold_sec:
+                    self.transition_to(MissionState.MOVE_TO_BLOCK_1, 'signal board display hold completed')
+                return
+
+            if self.signal_board_result == 'busy' and self.state_elapsed() >= self.max_wait_signal_board_sec:
+                self.get_logger().warning('Signal board stayed busy too long, mission still waits for manual intervention.')
+                self.state_enter_time = self.get_clock().now()
+            return
+
+        if self.state == MissionState.MOVE_TO_BLOCK_1:
+            if self.depth_target is not None and self.depth_target_aligned() and self.depth_target.point.x <= 0.45:
+                self.send_arm_command('pick_block:1')
+                self.transition_to(MissionState.PICK_BLOCK_1, 'first floor block aligned')
+                return
+            self.send_cmd(linear_y=self.lateral_speed_mps)
+            return
+
+        if self.state == MissionState.PICK_BLOCK_1:
+            if self.arm_action_finished():
+                self.send_arm_command(f'place_block:{self.blue_zone_label}')
+                self.transition_to(MissionState.PLACE_BLOCK_1, 'first pick completed')
+            return
+
+        if self.state == MissionState.PLACE_BLOCK_1:
+            if self.arm_action_finished():
+                self.block_pick_count = 1
+                self.transition_to(MissionState.MOVE_TO_BLOCK_2, 'first block placed to blue zone')
+            return
+
+        if self.state == MissionState.MOVE_TO_BLOCK_2:
+            if self.depth_target is not None and self.depth_target_aligned() and self.depth_target.point.x <= 0.45:
+                self.send_arm_command('pick_block:2')
+                self.transition_to(MissionState.PICK_BLOCK_2, 'second floor block aligned')
+                return
+            self.send_cmd(linear_y=self.lateral_speed_mps)
+            return
+
+        if self.state == MissionState.PICK_BLOCK_2:
+            if self.arm_action_finished():
+                self.send_arm_command(f'place_block:{self.green_zone_label}')
+                self.transition_to(MissionState.PLACE_BLOCK_2, 'second pick completed')
+            return
+
+        if self.state == MissionState.PLACE_BLOCK_2:
+            if self.arm_action_finished():
+                self.block_pick_count = 2
+                self.transition_to(MissionState.FINISHED, 'all competition actions completed')
+            return
+
+        if self.state == MissionState.FINISHED:
+            self.stop_robot()
+            return
+
+
+def main() -> None:
+    rclpy.init()
+    node = MissionManager()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.stop_robot()
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

dev_ws/src/mission_manager/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>mission_manager</name>
+  <version>0.1.0</version>
+  <description>Mission state machine for the RDK X5 logistics competition robot.</description>
+  <maintainer email="openai@example.com">OpenAI</maintainer>
+  <license>Apache-2.0</license>
+
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <exec_depend>ros2launch</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

dev_ws/src/mission_manager/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/mission_manager
+[install]
+install_scripts=$base/lib/mission_manager

dev_ws/src/mission_manager/setup.py

@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'mission_manager'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/launch', ['launch/mission_manager.launch.py']),
+        ('share/' + package_name + '/config', ['config/mission_manager.yaml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='OpenAI',
+    maintainer_email='openai@example.com',
+    description='Mission state machine for RDK X5 logistics robot',
+    license='Apache-2.0',
+    entry_points={
+        'console_scripts': [
+            'mission_manager = mission_manager.mission_manager_node:main',
+        ],
+    },
+)