head+actPolicy+rviz - dingdongdengdong/astra_ws GitHub Wiki

로봇 헤드 제어 부

핵심 구성 요소:

1. head_controller.py: 실제 로봇 헤드 하드웨어(모터)와 직접 통신하고 제어하는 저수준 컨트롤러입니다. 시리얼 통신을 통해 명령을 전달하고, 엔코더 값 등을 읽어옵니다.
2. head_node.py: ROS2 노드로서, head_controller.py와 ROS2 시스템 간의 인터페이스 역할을 합니다. 다른 ROS2 노드로부터 제어 명령을 받아 head_controller.py에 전달하고, head_controller.py로부터 받은 헤드의 상태 정보를 /joint_states 토픽으로 발행합니다.
3. URDF (Unified Robot Description Format): 로봇의 물리적인 구조(링크, 조인트, 외형 등)를 XML 형식으로 기술한 파일입니다. RViz2나 Gazebo와 같은 시뮬레이션 및 시각화 도구에서 로봇 모델을 표시하는 데 사용됩니다. (astra_description 패키지에 관련 파일이 있을 것입니다.)
4. robot_state_publisher: URDF 파일과 /joint_states 토픽을 입력으로 받아, 각 조인트의 현재 상태에 따른 로봇 각 부분의 3D 좌표 변환 정보(TF, Transform)를 발행하는 ROS2 노드입니다.
5. RViz2: ROS2에서 사용되는 3D 시각화 도구입니다. TF 정보와 로봇 모델(URDF)을 사용하여 로봇의 현재 자세를 시각적으로 보여주고, 다양한 센서 데이터(카메라, 라이다 등)를 함께 표시할 수 있습니다.

구현 및 실행 흐름 다이어그램:

graph TD
    %% Define overall graph direction
    direction TB

    %% Subgraph for ROS2 Control Stack
    subgraph "ROS2 Control Stack"
        direction LR
        A["External Control\n(Teleop/MoveIt)"] -->|"JointCommand"| B[head_node.py]
        B -->|"Set Position/Torque"| C[head_controller.py]
        C -->|"Joint States (Raw)"| B
        B -->|"/joint_states\n(sensor_msgs/JointState)"| D[robot_state_publisher]
        E["URDF\n(astra_description)"] --> D
        D -->|"/tf\n(tf2_msgs/TFMessage)"| F[RViz2]
        E --> F
    end

    %% Subgraph for Hardware Interaction
    subgraph "Physical/Simulated Hardware"
        direction LR
        C <-->|"Motor Control/Feedback"| G["Robot Head\n(Motors/Encoders)"]
    end

    %% Subgraph for Visualization
    subgraph "Visualization"
        direction LR
        F -->|"Displays Robot Model"| H[User Interface]
    end

    %% Styling for visual clarity
    classDef rosNode fill:#D6EAF8,stroke:#2E86C1,stroke-width:2px
    classDef hardware fill:#FDEDEC,stroke:#CB4335,stroke-width:2px
    classDef viz fill:#D5F5E3,stroke:#28B463,stroke-width:2px
    classDef config fill:#E5E7E9,stroke:#566573,stroke-width:2px

    class A,B,C,D rosNode
    class G hardware
    class F,H viz
    class E config

흐름 설명:

1. 명령 발생 (A -> B):

   • 사용자 입력(키보드, 조이스틱 등)을 처리하는 원격 조작 노드(teleop_node.py 등)나, 복잡한 경로 계획을 수행하는 MoveIt!과 같은 상위 레벨의 제어 노드(A)가 로봇 헤드의 목표 관절 위치를 담은 astra_controller_interfaces.msg.JointCommand 메시지를 /joint_command 토픽으로 발행합니다.
   • (또는 토크 활성화/비활성화를 위한 std_msgs.msg.UInt8 메시지가 /torque_enable 토픽으로 발행될 수 있습니다.)

2. ROS2 노드에서의 명령 처리 (B -> C):

   • head_node.py(B)는 /joint_command 토픽을 구독하고 있다가 새로운 명령 메시지를 수신합니다.
   • 수신된 메시지로부터 목표 관절 위치 값(pan, tilt 각도)을 추출합니다.
   • HeadController 클래스의 set_pos() 메서드를 호출하여 head_controller.py(C)에 목표 위치를 전달합니다.
   • 만약 /torque_enable 토픽으로 메시지를 받으면, HeadController의 set_torque() 메서드를 호출합니다.

3. 하드웨어 제어 (C <-> G):

   • head_controller.py(C)는 set_pos() 또는 set_torque() 메서드가 호출되면, 해당 명령을 로봇 헤드 모터(G)가 이해할 수 있는 시리얼 프로토콜 형식으로 변환합니다.
   • 변환된 데이터를 시리얼 포트를 통해 실제 로봇 헤드 모터 컨트롤러에 전송합니다.
   • 동시에, recv_thread는 헤드 모터 컨트롤러로부터 현재 엔코더 값(raw data)과 같은 피드백 데이터를 지속적으로 수신합니다.

4. 상태 피드백 (C -> B):

   • head_controller.py(C)는 수신된 raw 엔코더 값을 SI 단위(라디안)로 변환하고, 이를 이용해 현재 관절 위치, 속도, 추정 토크를 계산합니다.
   • 이 계산된 상태 정보는 head_node.py(B)에 등록된 state_cb 콜백 함수를 통해 전달됩니다.

5. 조인트 상태 발행 (B -> D):

   • head_node.py(B)의 state_cb 콜백 함수는 head_controller.py로부터 받은 현재 관절 상태(위치, 속도, 추정 토크)를 sensor_msgs.msg.JointState 메시지 형식으로 채웁니다.
   • 이 JointState 메시지를 /joint_states 토픽으로 발행합니다. 이 토픽에는 로봇의 모든 조인트("joint_head_pan", "joint_head_tilt")의 현재 상태가 포함됩니다.

6. TF 정보 발행 (D -> F):

   • robot_state_publisher 노드(D)는 로봇의 URDF 모델(astra_description 패키지 내)과 /joint_states 토픽을 구독합니다.
   • /joint_states 토픽으로부터 각 조인트의 현재 각도 값을 받으면, URDF에 정의된 로봇의 각 링크(link)들 사이의 상대적인 3D 공간 변환 관계(TF)를 계산합니다.
   • 계산된 TF 정보는 /tf 토픽으로 발행됩니다. 이 정보는 로봇의 각 부분이 현재 어떤 자세로 위치해 있는지를 나타냅니다.

7. RViz2에서의 시각화 (E, F -> H):

   • RViz2(F)는 사용자가 설정한 내용에 따라 다양한 토픽을 시각화합니다.
   • RobotModel Display: URDF(E)를 로드하고, /tf 토픽(D로부터 발행됨)을 사용하여 각 링크를 올바른 위치와 자세로 3D 공간에 표시합니다. 이를 통해 사용자는 로봇 헤드가 실제로 어떻게 움직이는지 화면(H)을 통해 실시간으로 확인할 수 있습니다.
   • TF Display: /tf 정보를 직접 시각화하여 각 좌표계(frame)들이 어떻게 연결되어 있고 움직이는지 확인할 수 있습니다.
   • (필요에 따라 Grid, Camera Image 등 다른 Display 타입도 추가하여 활용할 수 있습니다.)

실행 단계 (터미널 명령어 예시):

실제 로봇 하드웨어를 연결한 경우와 시뮬레이션만 하는 경우에 따라 단계가 달라질 수 있습니다. 여기서는 제공된 코드가 실제 하드웨어 제어를 목표로 하므로, 실제 하드웨어 연결을 가정하고 RViz2에서 시각화하는 단계를 중심으로 설명합니다.

1. ROS2 환경 설정:

   source /opt/ros/<your_ros_distro>/setup.bash
   source <path_to_your_astra_ws>/install/setup.bash

2. head_controller와 통신할 하드웨어 연결 및 권한 확인:

   • 로봇 헤드 컨트롤 보드가 PC와 시리얼로 연결되어 있고, head_node.py에 지정된 장치 경로(device 파라미터, 예: /dev/tty_head)가 올바른지 확인합니다.
   • 필요시 해당 장치에 접근 권한을 부여합니다 (sudo chmod 666 /dev/tty_head 등).

3. head_node 실행:

   • head_node.py는 head_controller.py를 내부적으로 사용합니다.
   • 보통 ROS2 launch 파일을 통해 실행합니다. astra_controller 패키지 내에 head_node를 실행하는 launch 파일이 있는지 확인하거나, 직접 실행할 수 있습니다.
   • 만약 launch 파일이 있다면 (예: astra_controller/launch/head.launch.py):

     ros2 launch astra_controller head.launch.py

   • 직접 실행한다면 (파라미터 설정 필요):

     ros2 run astra_controller head_node --ros-args -p device:=/dev/tty_head

   • 이 노드가 실행되면 /joint_states 토픽이 발행되기 시작합니다.

4. robot_state_publisher 실행:

   • 로봇의 URDF 파일을 로드하고 /joint_states를 받아 TF를 발행합니다. 이 또한 launch 파일로 관리되는 것이 일반적입니다. astra_description 또는 astra_moveit_config 같은 패키지에 rsp.launch.py (Robot State Publisher의 약자) 또는 유사한 이름의 launch 파일이 있을 수 있습니다.
   • 예시 (실제 파일명은 다를 수 있음):

     ros2 launch astra_description robot_state_publisher.launch.py

     또는 astra_moveit_config에 포함된 rsp.launch.py를 사용할 수도 있습니다.

     ros2 launch astra_moveit_config rsp.launch.py

5. RViz2 실행:

   • 새 터미널을 열고 RViz2를 실행합니다.

     rviz2

   • RViz2가 실행되면 다음 설정을 진행합니다:
     • Global Options: Fixed Frame을 로봇의 base_link (또는 odom 등 기준이 되는 프레임)로 설정합니다. URDF에 정의된 프레임 이름을 사용해야 합니다.
     • Add Display: 왼쪽 하단의 "Add" 버튼을 클릭합니다.
       • By display type 탭에서 RobotModel을 선택하고 OK를 누릅니다.
         • Description Source가 Topic으로 되어있는지 확인하고, Description Topic이 /robot_description (또는 robot_state_publisher가 발행하는 로봇 모델 토픽명)으로 설정되었는지 확인합니다.
         • TF Prefix는 일반적으로 비워둡니다.
       • By display type 탭에서 TF를 선택하고 OK를 누릅니다 (선택 사항, TF 시각화).
         • Show Names, Show Axes, Show Arrows 등을 취향에 맞게 설정합니다.

6. (선택 사항) 헤드 제어 명령 발행 노드 실행:

   • 헤드를 실제로 움직여보려면 제어 명령을 발행하는 노드가 필요합니다.
   • 간단한 테스트를 위해 ros2 topic pub 명령어를 사용하거나, 제공된 teleop_node.py 또는 다른 제어 프로그램을 실행할 수 있습니다.
   • 예시 (ros2 topic pub 사용 - pan 0.5 라디안, tilt 0.2 라디안으로 이동):

     ros2 topic pub --once /joint_command astra_controller_interfaces/msg/JointCommand '{name: ["joint_head_pan", "joint_head_tilt"], position_cmd: [0.5, 0.2]}'

   • teleop_node.py가 있다면:

     ros2 run astra_controller teleop_node

     (실행 방법은 해당 노드의 구현에 따라 다를 수 있습니다.)

주의사항:

• URDF 경로 및 내용: robot_state_publisher가 올바른 URDF 파일을 참조하도록 설정해야 합니다. URDF 파일 내의 조인트 이름(joint_head_pan, joint_head_tilt)은 head_node.py에서 /joint_states로 발행하는 조인트 이름과 정확히 일치해야 합니다.
• Launch 파일 활용: 일반적으로 위에서 언급된 노드들은 개별적으로 실행하기보다는, 하나의 통합된 launch 파일을 통해 함께 실행됩니다. astra_ws/src/astra_controller/launch/start.launch.py 와 같은 파일이 전체 시스템을 실행하는 역할을 할 수 있으니 확인해보시는 것이 좋습니다. 특히 demo.launch.py나 moveit_rviz.launch.py 같은 파일은 RViz2와 함께 필요한 모든 것을 실행하도록 구성되어 있을 가능성이 높습니다.
• 시뮬레이션 (Gazebo 등): 만약 실제 하드웨어 없이 Gazebo와 같은 시뮬레이터에서 로봇을 구동하려면, ros2_control과 Gazebo 플러그인을 설정하여 시뮬레이션 환경 내에서 /joint_states를 발행하고 /joint_command를 받아 모터를 움직이도록 구성해야 합니다. 이는 별도의 설정과 launch 파일이 필요합니다. 제공된 코드에는 ros2_control 관련 설정이 astra_moveit_config/config/ros2_controllers.yaml 등에 있을 수 있습니다.

이 설명을 바탕으로 제공된 코드를 빌드하고 실행하여 RViz2에서 로봇 헤드가 제어되는 모습을 확인하실 수 있을 것입니다. 각 패키지의 launch 폴더와 config 폴더를 자세히 살펴보시면 실제 실행에 필요한 구체적인 파일들과 설정을 찾으실 수 있습니다.