ROS & Robotics/사족보행로봇

Unitree Go1을 Gazebo에서 작동시키기 : 2. 카메라 상의 물체를 따라가도록

설치에 이어서 Gazebo 상에서 카메라 토픽을 받아서 주행 커맨드를 작성할 일이 있어서 해당 부분을 만들었다. 구성은 다음과 같다.

1. gazebo 상의 로봇 카메라에서 물체 인식하기

2. camera plane에서 로봇 좌표계로 좌표계 변환하기

3. 얻어진 방향을 기준으로 로봇 제어하기

1. Gazebo 상의 로봇 카메라에서 물체 인식하기

우선 내 맘대로 World를 좀 구성해 줘야 하니까 normal.launch 파일과 sdf 파일을 만들어서 설정해준다.

/unitree_gazebo/launch/normal.launch

    <node pkg="gazebo_ros" type="spawn_model" name="spawn_green_ball" respawn="false" output="screen"
          args="-sdf -file $(find unitree_gazebo)/models/green_ball/model.sdf 
                -model green_ball 
                -x 2.0 -y 0.0 -z 0.1"/>

/unitree_gazebo/material/green_ball/model.sdf

<?xml version="1.0" ?>
<sdf version="1.5">
  <model name="green_ball">
    <static>false</static>
    <link name="link">
      <collision name="collision">
        <geometry>
          <sphere>
            <radius>0.1</radius>
          </sphere>
        </geometry>
      </collision>
      <visual name="visual">
        <geometry>
          <sphere>
            <radius>0.1</radius>
          </sphere>
        </geometry>
        <material>
          <ambient>0 1 0 1</ambient>
          <diffuse>0 1 0 1</diffuse>
          <specular>0.1 0.1 0.1 1</specular>
          <emissive>0 0 0 0</emissive>
        </material>
      </visual>
      <inertial>
        <mass>0.1</mass>
        <inertia>
          <ixx>0.0004</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0004</iyy>
          <iyz>0</iyz>
          <izz>0.0004</izz>
        </inertia>
      </inertial>
    </link>
  </model>
</sdf>

/unitree_gazebo/material/green_ball/model.config

<?xml version="1.0"?>
<model>
  <name>green_ball</name>
  <version>1.0</version>
  <sdf version="1.5">model.sdf</sdf>
  <description>
    A simple green ball for tracking
  </description>
</model>

이렇게 launch 파일을 수정해주고 sdf 파일을 생성하면 다음과 같이 Gazebo world에서 로봇이 따라가고자 하는 공을 확인할 수 있다.

좌 : Gazebo World / 우 : Rviz에서 /camera_face/color/image_raw 토픽 화면

이후 gazebo_recognition 이라는 패키지를 생성한 후, gazebo_recognition_node.py에서 해당 공의 중심점의 위치를 출력하고 이 방향을 로봇 기준으로 찾는 코드를 만들었다. Gazebo 상의 카메라 파라미터는 /camera_info 토픽에서 확인할 수 있다.

2. camera plane에서 로봇 좌표계로 좌표계 변환하기

이런 식으로 이미지에서 객체의 중심을 찾고 나면, 카메라 plane에서 로봇 좌표계로 좌표계 변환을 해 줘야 로봇에서 보는 객체의 방향이 나온다.

이러한 transformation을 하기 위해서 robot.xacro 파일을 살펴보면 다음과 같은 구문이 있다.

<xacro:depthCamera camID="1" name="face">
    <origin rpy="${PI} 0 0" xyz="0.2785 0.0125 0.0167"/>
</xacro:depthCamera>

그리고 tf 관계를 좀 더 자세히 살펴보면 다음과 같이 되어 있다. (/tf를 구독해서 할 수도 있겠지만 내 경우에는 tf가 정상적으로 발행이 안 되고 있었다... )

def setup_transforms(self):
    """
    두 좌표계를 연결하는 변환 설정:
    1. trunk frame (로봇 기준)
    - X: 전방, Y: 좌측, Z: 상방
    
    2. 카메라 좌표계
    - Z: 전방, X: 우측, Y: 하방
    """
    
    # 1. xacro에서 정의된 camera_face frame의 회전 (rpy="${PI} 0 0")
    rot_x_pi = rotation_matrix(np.pi, [1, 0, 0])  # X축 기준 180도 회전
    
    # 2. camera_optical_frame을 만들기 위한 회전
    # ROS의 표준 카메라 광학 프레임 규칙을 따르기 위한 변환
    rot_x_minus_pi_2 = rotation_matrix(-np.pi/2, [1, 0, 0])  # X축 기준 -90도
    rot_z_minus_pi_2 = rotation_matrix(-np.pi/2, [0, 0, 1])  # Z축 기준 -90도
    
    # 3. 카메라와 로봇 좌표계 정렬을 위한 추가 회전
    align_rot_y = rotation_matrix(-np.pi/2, [0, 1, 0])  # Y축 기준 -90도
    align_rot_z = rotation_matrix(np.pi, [0, 0, 1])     # Z축 기준 180도
    align_rot = np.matmul(align_rot_z, align_rot_y)  # 두 회전을 결합
    
    # 모든 회전 변환을 순서대로 결합
    rot_mat = np.matmul(np.matmul(align_rot, rot_z_minus_pi_2), rot_x_minus_pi_2)
    
    # 위치 변환 행렬 (trunk에서 camera_face까지의 이동)
    trans_mat = np.eye(4)  # 4x4 단위 행렬
    trans_mat[0:3, 3] = [0.2785, 0.0125, 0.0167]  # x, y, z 이동
    
    # 최종 변환 행렬 = 이동 * 회전
    self.transform_mat = np.matmul(trans_mat, rot_mat)
    # 역변환 행렬 (카메라 좌표계 -> trunk 좌표계)
    self.inv_transform_mat = np.linalg.inv(self.transform_mat)

이런 식으로 setup_transform 함수를 작성하여 변환 행렬을 만들어준다.

https://youtu.be/zbuqaGT8wAM?si=szW3wdlUl8Sp-8FI