graph TD
    subgraph User_Input ["User Input"]
        direction LR
        UI["Web Browser UI / Local Camera"] --> CamInput{"Camera Feed + User Actions"};
    end

    subgraph Non_ROS_Vision_Processing ["Non-ROS Vision Processing"]
        direction LR
        CamInput --> ATeleopProc["astra_teleop/process.py - ArUco Detection and Pose Estimation"];
        ATeleopProc --> ProcessedVisionData{"Processed Vision Data Marker Poses)"};

        subgraph Web_Interface ["non_ros_src/astra_teleop_web"]
            direction LR
            CamInputWeb["Web Client index.html, index.js, opencv.js"] -->|Marker Data, Pedal via WebRTC| WebServer["webserver.py"];
            WebServer --> Teleoperator["teleoprator.py"];
            Teleoperator -->|Uses| ATeleopProc;
            Teleoperator --> WebArmGoal{"Arm Goal Pose"};
        end
    end

    subgraph ROS2_System ["ROS2 System (ros_src)"]
        direction TB

        subgraph ROS_Input_Nodes ["ROS Input Nodes"]
            direction LR
            ProcessedVisionData --> TeleopNode["astra_controller/teleop_node.py"];
            WebArmGoal --> TeleopWebNode["astra_controller/teleop_web_node.py"];
        end

        subgraph ROS_Controllers ["ROS Controllers"]
            direction LR
            TeleopNode -->|Target Pose| AstraCtrl["astra_controller/astra_controller.py"];
            TeleopWebNode -->|Target Pose| AstraCtrl;
            AstraCtrl --> ArmCtrl["astra_controller/arm_controller.py"];
            AstraCtrl --> OtherCtrls["(Head, Gripper, Base Ctrls)"];
            ArmCtrl -->|Joint Commands| SimOrHW{"Simulated/Real Robot"};
            OtherCtrls -->|Joint Commands| SimOrHW;
        end

        subgraph Simulation_Setup ["Simulation - MoveIt povr Gazebo/RViz"]
            direction TB
            LaunchSim["astra_moveit_config/launch/demo.launch.py"] --> RVizGazebo["RViz / Gazebo"];
            RobotModel["astra_description (URDF)"] --> RVizGazebo;
            ROS2CtrlCfgs["astra_moveit_config/config/ros2_controllers.yaml"] --> SimCtrlManager["Simulated Controller Manager (e.g., FakeControllerManager or Gazebo Controllers)"];
            SimCtrlManager --> SimOrHW;
        end

        subgraph Main_Launch_File ["Main Launch"]
             StartLaunch["astra_controller/launch/start.launch.py"] --> AstraCtrl;
             StartLaunch --> TeleopNode;
             StartLaunch --> TeleopWebNode;
             StartLaunch -->|Can Include| LaunchSim;
        end
    end

    style User_Input fill:#E6E6FA,stroke:#333,stroke-width:2px
    style CamInput fill:#ADD8E6,stroke:#333,stroke-width:1px
    style Non_ROS_Vision_Processing fill:#FFFACD,stroke:#333,stroke-width:2px
    style ATeleopProc fill:#FAFAD2,stroke:#333,stroke-width:1px
    style ProcessedVisionData fill:#FFEFD5,stroke:#333,stroke-width:1px
    style Web Trinface fill:#F0FFF0,stroke:#333,stroke-width:1.5px
    style CamInputWeb fill:#98FB98,stroke:#333,stroke-width:1px
    style WebServer fill:#AFEEEE,stroke:#333,stroke-width:1px
    style Teleoperator fill:#AFEEEE,stroke:#333,stroke-width:1px
    style WebArmGoal fill:#FFE4E1,stroke:#333,stroke-width:1px
    style ROS2_System fill:#D3D3D3,stroke:#000,stroke-width:2px
    style ROS_Input_Nodes fill:#F5DEB3,stroke:#333,stroke-width:1.5px
    style TeleopNode fill:#FFEBCD,stroke:#333,stroke-width:1px
    style TeleopWebNode fill:#FFEBCD,stroke:#333,stroke-width:1px
    style ROS_Controllers fill:#E0FFFF,stroke:#333,stroke-width:1.5px
    style AstraCtrl fill:#B0E0E6,stroke:#333,stroke-width:1px
    style ArmCtrl fill:#B0E0E6,stroke:#333,stroke-width:1px
    style OtherCtrls fill:#B0E0E6,stroke:#333,stroke-width:1px
    style SimOrHW fill:#FFDAB9,stroke:#333,stroke-width:1px
    style Simulation_Setup fill:#F0E68C,stroke:#333,stroke-width:1.5px
    style LaunchSim fill:#FFF8DC,stroke:#333,stroke-width:1px
    style RVizGazebo fill:#F5F5DC,stroke:#333,stroke-width:1px
    style RobotModel fill:#F5F5DC,stroke:#333,stroke-width:1px
    style ROS2CtrlCfgs fill:#F5F5DC,stroke:#333,stroke-width:1px
    style SimCtrlManager fill:#F5F5DC,stroke:#333,stroke-width:1px
    style Main_Launch_File fill:#FFE4B5,stroke:#333,stroke-width:1.5px
    style StartLaunch fill:#FFDEAD,stroke:#333,stroke-width:1px

1주차 프로젝트 개요 및 구현 흐름

목표: 카메라 비전 처리(non_ros_src/astra_teleop 로직)를 통해 얻은 로봇 팔 움직임 데이터를 teleop_node로 전달하고, 이 데이터를 기반으로 MoveIt 시뮬레이션 환경의 Astra 로봇 팔을 움직이는 기본 시스템을 구축합니다. 동시에 페달 입력을 받아 하부 움직임 및 그리퍼 제어를 위한 파이프라인을 연결합니다.

핵심 기술: ROS2 노드 통신 (Topics), MoveIt 시뮬레이션 환경 구성, Python 기반 ROS2 노드 수정 및 개발, Launch 파일 활용, 비전 처리 연동

goalPos노드 흐름(예시).

graph TD
    %% Define nodes and main flow
    A[Source Node: Teleop] -->|Sends Goal Pose| B{IK Node: ik_node.py}
    B -->|"Joint Commands"| E[dry_run_node: Simulation]
    E -->|"Simulated Joint State"| F[MoveIt & RViz: Visualization]
    E -->|"Joint State Feedback"| C[MoveIt - move_group Node]

    %% Highlight critical data paths
    A -->|"goal_pose: PoseStamped"| B
    B -->|"JointCommand: astra_controller"| E
    E -->|"JointState: sensor_msgs"| F
    E -->|"JointState: sensor_msgs"| C

    %% Styling for clarity and readability
    classDef node fill:#f0f9ff,stroke:#333,stroke-width:2px
    classDef highlight fill:#fff3cd,stroke:#d4a017,stroke-width:2px
    class A,B,C,E,F node
    class E,C highlight

    %% Style feedback loop and goal_pose path
    linkStyle 3,5 stroke:#d4a017,stroke-width:2px

    %% Comments for additional clarity
    %% Source Node (Teleop) sends goal_pose to IK Node.
    %% IK Node computes joint commands using inverse kinematics.
    %% dry_run_node simulates joint motion and publishes states.
    %% MoveIt uses feedback to plan motion; RViz visualizes results.

구현 흐름도 :

graph LR
    A[Camera Input] --> B[Vision Processing\nnon_ros_src/astra_teleop logic]
    I[Pedal Input] --> J[Pedal Input Node]

    subgraph astra_controller
        C[teleop_node]
        D[moveit_relay_node]
        K[Base/Lift/Gripper Nodes]
    end

    B -->|Processed Arm Movement Data| C
    J -->|Pedal Data| C
    C -->|Arm Commands\nsensor_msgs/msg/JointState| D
    C -->|Base/Gripper Commands| K
    D -->|/joint_trajectory_controller/joint_trajectory| E[MoveIt / ros2_control\nSimulated Arm]
    K -->|Commands| F[ros2_control\nSimulated Base/Gripper]
    E -->|/joint_states Arm| G[Robot State Publisher\nJoint State Publisher]
    F -->|/joint_states Base/Gripper| G
    G --> H[RViz\nVisualization]
    E -->|/joint_states Feedback| C
    F -->|/joint_states Feedback| C

기술 설명

• Camera Input: 로봇에 장착된 카메라로부터 입력 영상을 획득합니다.
• Vision Processing (non_ros_src/astra_teleop): 카메라 영상을 분석하여 손의 위치, 제스처 등을 인식하고, 로봇 팔 움직임 데이터(예: 목표 조인트 각도, 엔드이펙터 자세)로 변환합니다. 이 로직은 non_ros_src/astra_teleop 폴더에 포함되며, 처리된 데이터는 teleop_node의 주 입력입니다.
• Pedal Input Node: 페달 장치로부터 입력을 읽어 로봇 하부 움직임 및 그리퍼 제어에 사용하며, teleop_node로 전달됩니다. (시스템에 따라 teleop_node에 통합 가능)
• teleop_node (src/astra_controller): rclpy.node.Node("teleop_node")로 정의된 핵심 노드입니다. **비전 처리 결과(로봇 팔 움직임 데이터)**와 페달 입력을 수신하여, sensor_msgs/msg/JointState 메시지로 변환해 /leader_arm/commands 토픽에 발행합니다. 페달 입력은 하부 및 그리퍼 제어 명령으로 변환되어 해당 토픽에 발행됩니다.
• moveit_relay_node (src/astra_controller): /leader_arm/commands 토픽에서 JointState 메시지를 수신하여 MoveIt의 /joint_trajectory_controller/joint_trajectory 토픽으로 전달, 시뮬레이션 로봇 팔을 움직입니다.
• Base/Lift/Gripper Nodes (src/astra_controller): 페달 입력 기반 명령을 수신하여 하부 움직임 및 그리퍼를 제어하며, ros2_control 인터페이스와 상호작용합니다.
• MoveIt / ros2_control (Simulated Robot): MoveIt은 로봇 모델과 조인트 상태를 관리하고, ros2_control의 FakeSystem 인터페이스를 통해 움직임을 시뮬레이션합니다. 명령을 받아 조인트 상태를 업데이트하고 /joint_states 토픽에 발행합니다.
• Robot State Publisher & Joint State Publisher: /joint_states 토픽을 구독하여 URDF/XACRO 기반으로 로봇의 3D 자세(tf 변환)를 계산 및 발행합니다.
• RViz (Visualization): /joint_states 및 tf 데이터를 구독하여 로봇 모델을 3D로 시각화합니다.

1주차 과제 상세 해결 가이드

과제 1.1: MoveIt 시뮬레이션 환경 실행 및 검증

목표

astra_moveit_config 패키지를 사용해 MoveIt 시뮬레이션 환경(로봇 팔, 하부, 그리퍼 포함)을 실행하고, RViz에서 Astra 로봇 모델을 확인합니다.

기술 설명

astra_moveit_config 패키지는 URDF/XACRO 모델, SRDF, MoveIt 설정, 시뮬레이션 및 RViz 실행용 Launch 파일을 포함합니다. demo.launch.py는 로봇 상태 발행, 조인트 상태 발행, MoveIt 노드, RViz를 시작합니다. 하부와 그리퍼가 URDF 및 MoveIt/ros2_control에 반영되면 RViz에서 전체 모델을 확인할 수 있습니다.

실행 단계

1. ROS2 작업 공간의 src 폴더로 이동하여 astra_moveit_config 패키지를 확인합니다.
2. 작업 공간 루트(예: colcon_ws)에서 colcon build --packages-select astra_moveit_config를 실행합니다.
3. source install/setup.bash로 환경을 설정합니다.
4. ros2 launch astra_moveit_config demo.launch.py로 시뮬레이션 환경을 시작합니다.
5. RViz에서 Astra 로봇 모델이 표시되는지 확인합니다.
6. RViz의 Displays 패널에서 RobotModel 상태를 점검합니다.
7. MoveIt MotionPlanning 플러그인으로 로봇 팔, 하부, 그리퍼의 계획 및 실행을 테스트합니다.

핵심 파일/개념

• astra_moveit_config/launch/demo.launch.py
• URDF/SRDF
• robot_state_publisher
• joint_state_publisher
• move_group node
• RViz

예상 결과

RViz에 Astra 로봇 모델이 정상 표시되고, MoveIt Planning으로 각 부분을 움직일 수 있습니다.

문제 해결 팁

• 터미널 로그를 확인합니다.
• rqt_graph, ros2 topic list, ros2 node list로 문제를 진단합니다.

과제 1.2: 비전 처리, 텔레오퍼레이션, MoveIt Relay 노드 분석

목표

non_ros_src/astra_teleop, teleop_node.py, moveit_relay_node.py, 하부/그리퍼 노드 코드를 분석하여 역할, 통신 인터페이스, 비전 처리 결과의 teleop_node 전달 방식을 파악합니다.

기술 설명

비전 처리 결과인 로봇 팔 움직임 데이터가 teleop_node로 전달되어 ROS2 명령으로 변환되는 데이터 흐름을 이해합니다. moveit_relay_node 및 하부/그리퍼 노드의 기대 메시지도 확인합니다.

실행 단계

1. non_ros_src/astra_teleop/의 Python 파일(예: cam.py, process.py)을 분석하여 로봇 팔 움직임 데이터 생성 및 전달 방식(파일, 네트워크, 함수 호출 등)을 파악합니다.
2. src/astra_controller/teleop_node.py를 분석:
   • 노드 이름("teleop_node") 확인.
   • 비전 처리 결과 및 페달 입력 수신 로직 파악.
   • /leader_arm/commands 토픽과 sensor_msgs/msg/JointState 발행 확인.
   • 하부/그리퍼 명령 토픽 및 메시지 타입 확인.
   • 데이터 변환 로직 분석.
3. src/astra_controller/moveit_relay_node.py를 분석:
   • /leader_arm/commands 토픽 및 JointState 구독 확인.
   • JointState 메시지가 /joint_trajectory_controller/joint_trajectory로 전달되는 방식과 사용 필드(position, velocity, effort) 확인.
4. base_node.py, lift_node.py를 분석:
   • 각 노드 역할(하부, 리프트/그리퍼) 파악.
   • 구독 토픽 및 메시지 타입(예: geometry_msgs/msg/Twist, std_msgs/msg/Float64) 확인.

핵심 파일/개념

• non_ros_src/astra_teleop/
• src/astra_controller/teleop_node.py
• src/astra_controller/moveit_relay_node.py
• base_node.py, lift_node.py
• sensor_msgs/msg/JointState
• geometry_msgs/msg/Twist
• Topics

예상 결과

로봇 팔 움직임 데이터의 생성 및 전달 방식, teleop_node의 데이터 수신 및 명령 변환 과정을 이해합니다.

문제 해결 팁

• 비전 처리와 teleop_node 간 데이터 전달 방식을 명확히 추적합니다.
• 코드 주석을 활용합니다.

과제 1.3: 비전 데이터 기반 로봇 팔 명령 발행 노드 구현 및 페달 제어 연결

목표

teleop_node.py를 수정하여 **비전 처리 결과(로봇 팔 움직임 데이터)**를 sensor_msgs/msg/JointState로 변환해 /leader_arm/commands 토픽에 발행하고, 페달 입력으로 하부/그리퍼 명령을 발행합니다. 초기에는 특정 조인트 하나만 제어합니다.

기술 설명

비전 데이터와 페달 입력을 받아 ROS2 메시지로 변환하여 각 제어 노드에 발행합니다. 비전 데이터는 JointState의 position 또는 velocity로, 페달 입력은 Twist 및 그리퍼 메시지로 매핑됩니다.

실행 단계

1. src/astra_controller/teleop_node.py를 엽니다.
2. 메시지 타입 임포트: sensor_msgs.msg.JointState, geometry_msgs.msg.Twist, std_msgs.msg.Float64.
3. 비전 처리 결과 및 페달 입력 수신 로직을 구현(과제 1.2 기반).
4. 로봇 팔 제어:
   • /leader_arm/commands 토픽에 JointState 발행 설정.
   • 비전 데이터(예: 손 높이)를 특정 조인트(예: joint1)의 목표 값으로 변환.
   • JointState 메시지 생성(header.stamp, name, position/velocity).
   • 메시지 발행.
5. 하부/그리퍼 제어:
   • /cmd_vel, /gripper_command 토픽에 메시지 발행 설정.
   • 페달 입력을 속도 및 그리퍼 명령으로 변환.
   • 메시지 발행.
6. create_timer()로 주기적 실행 설정.
7. colcon build --packages-select astra_controller로 빌드.

핵심 파일/개념

• src/astra_controller/teleop_node.py
• 비전/페달 입력 수신
• sensor_msgs/msg/JointState
• /leader_arm/commands
• ROS2 Publisher/Subscriber/Timer

예상 결과

teleop_node가 비전 데이터 및 페달 입력을 받아 명령을 발행하며, ros2 topic echo로 메시지를 확인할 수 있습니다.

문제 해결 팁

• 비전 데이터 형식이 teleop_node 기대 형식과 일치하는지 확인.
• 토픽 이름 및 메시지 타입 점검.
• 간단한 매핑으로 시작.

과제 1.4: 모든 구성 요소 통합 실행 및 기본 동작 확인

목표

카메라 드라이버, 비전 처리, 페달 입력, teleop_node, moveit_relay_node, 하부/그리퍼 노드, MoveIt 시뮬레이션을 Launch 파일로 실행하고, 비전 입력과 페달 조작에 따라 RViz 로봇이 움직이는지 확인합니다.

기술 설명

ROS2 Launch로 모든 노드와 MoveIt 시뮬레이션을 시작합니다. teleop_node는 비전/페달 입력을 명령으로 변환하고, moveit_relay_node 및 하부/그리퍼 노드는 이를 시뮬레이션 로봇에 반영합니다.

실행 단계

1. astra_controller/launch/teleop_vision_sim.launch.py를 생성.
2. 모듈 임포트:

   import os
   from ament_index_python.packages import get_package_share_directory
   from launch import LaunchDescription
   from launch_ros.actions import Node
   from launch.actions import IncludeLaunchDescription
   from launch.launch_description_sources import PythonLaunchDescriptionSource
   

3. generate_launch_description() 정의.
4. 카메라 드라이버 노드 추가:

   camera_node = Node(
       package='usb_cam',
       executable='usb_cam_node',
       name='camera_driver',
       output='screen'
   )
   

5. 비전 처리 노드 추가(독립적인 경우).
6. 페달 입력 노드 추가(독립적인 경우).
7. teleop_node 추가:

   teleop_node = Node(
       package='astra_controller',
       executable='teleop_node',
       name='teleop_main',
       output='screen'
   )
   

8. moveit_relay_node 추가:

   moveit_relay_node = Node(
       package='astra_controller',
       executable='moveit_relay_node',
       name='moveit_relay',
       output='screen',
       parameters=[{
           'command_topic_name': '/leader_arm/commands',
           'controller_topic_name': '/joint_trajectory_controller/joint_trajectory',
           'joint_names': ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6']
       }]
   )
   

9. 하부/그리퍼 노드 추가:

   base_node = Node(
       package='astra_controller',
       executable='base_node',
       name='base_controller',
       output='screen'
   )
   lift_node = Node(
       package='astra_controller',
       executable='lift_node',
       name='lift_controller',
       output='screen'
   )
   

10. MoveIt 시뮬레이션 포함:

    moveit_launch_file = os.path.join(
        get_package_share_directory('astra_moveit_config'),
        'launch',
        'demo.launch.py'
    )
    moveit_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(moveit_launch_file),
        launch_arguments={'use_sim_time': 'true'}.items()
    )
    

11. LaunchDescription에 추가:

    ld = LaunchDescription([
        camera_node,
        teleop_node,
        moveit_relay_node,
        base_node,
        lift_node,
        moveit_launch
    ])
    return ld
    

12. setup.py에 Launch 파일 추가.
13. colcon build --packages-select astra_controller astra_moveit_config usb_cam.
14. source install/setup.bash.
15. ros2 launch astra_controller teleop_vision_sim.launch.py 실행.
16. RViz에서 비전 입력과 페달 조작에 따른 로봇 움직임 확인.

핵심 파일/개념

• astra_controller/launch/teleop_vision_sim.launch.py
• launch_ros.actions.Node
• launch.actions.IncludeLaunchDescription
• 카메라/비전/페달 노드
• teleop_node, moveit_relay_node
• astra_moveit_config/launch/demo.launch.py

예상 결과

Launch 파일로 시스템이 실행되며, 비전 입력과 페달 조작에 따라 RViz 로봇이 움직입니다.

문제 해결 팁

• 실행 로그 확인.
• rqt_graph, ros2 topic echo로 메시지 점검.
• 카메라 입력 및 비전 데이터 전달 확인