[ROS2] Gazebo Classic - Set/Get Entity Pose by Calling ROS2 Service

 

Ubuntu 24.04 LTS 및 ROS2 Rolling 이상 버전부터는 Gazebo Classic이 공식적으로 지원이 중단되긴 하지만 (EOL), 아직까지는 gzsim(ignition)보다 Gazebo Classic이 손에 익은 지라 애용하는 중 ^^;; (아직까지 ROS1을 사용하는 어찌보면 대단한 로봇 업체들도 있는걸 보면 오래된 툴을 사용하는게 마냥 나쁜 건 아닐지도?)

 

이번 포스팅에서는 Gazebo 상에 spawn된 entity의 Pose를 ROS2 Service 호출을 통해 Get/Set할 수 있는 방법에 대해 소개해보도록 하겠다

1. world 파일(sdf)에 플러그인 추가 (libgazebo_ros_state.so)

Gazebo상에 spawn된 entity들의 pose 정보를 가져오거나 값을 수정하기 위해서는 libgazebo_ros_state 플러그인(plugin)을 수동으로 추가해줘야 한다

gzserver 실행 시 일반적으로 sdf 태그가 root인 xml 포맷의 world 파일을 로드하게 되는데, 이 world 파일에 아래와 같이 플러그인을 추가해주자

<?xml version="1.0"?>
<sdf version="1.7">
    <world name="my_world">
        <plugin name="gazebo_ros_state" filename="libgazebo_ros_state.so">
            <ros>
                <namespace>/gazebo</namespace>
            </ros>
            <update_rate>1.0</update_rate>
        </plugin>
        
        <!-- world 구현, 아래는 예시 -->
        
        <include>
            <uri>model://sun</uri>
        </include>
        
        <include>
            <uri>model://ground_plane</uri>
        </include>        
        
    </world>
</sdf>

※ Tip: 플러그인의 <ros> - <namespace> 태그를 통해 플러그인 추가 후 가제보 실행 시 추가되는 ROS 서비스들(/get_entity_state, /set_entity_state) 이름 앞에 붙는 네임스페이스를 지정할 수 있다 (/gazebo 문자열을 추천)

2. gazebo server 및 client 실행

터미널에서 아래와 같이 순차적으로 gzserver, gzclient를 실행시켜주자

# launch gzserver
$ ros2 launch gazebo_ros gzserver.launch.py world:={world파일 절대경로}

# launch gzclient
$ ros2 launch gazebo_ros gzclient.launch.py gui_required:=true

 

※ Tip: python기반의 launch 파일을 작성할 경우 아래와 같이 간단하게 구현할 수도 있다

from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription

ld = LaunchDescription()

world_path = '~/default.world'  # just example!

# launch gzserver
gazebo_ros_path = get_package_share_directory('gazebo_ros')
launch_gzserver = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
        os.path.join(gazebo_ros_path, 'launch', 'gzserver.launch.py')
    ),
    launch_arguments={
        'init': 'true',
        'factory': 'true',
        'world': world_path,
        'server_required': 'true',
        'use_namespace': 'True',
    }.items()
)
ld.add_action(launch_gzserver)

# launch gzclient
launch_gzclient = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
        os.path.join(gazebo_ros_path, 'launch', 'gzclient.launch.py')
    ),
    launch_arguments={
        'gui_required': 'true'
    }.items()
)
ld.add_action(launch_gzclient)

3. ROS2 Service 생성 확인 (get_entity_state, set_entity_state)

$ ros2 service list | grep entity

플러그인을 로드하지 않으면 생성되지 않는 'get_entity_state'와 'set_entity_state' 서비스가 추가된 것을 확인할 수 있다

4. ROS2 Service Call 구현 예시 (rclpy)

이번 챕터에서 언급되는 서비스들의 이름 앞에 붙는 /gazebo 네임스페이스는 앞서 wolrd sdf 파일에 추가한 플러그인에서 설정된 값이다 (<namespace> 태그 참고)

4.1. Get Entity State

4.1.1. Interface

get_entity_state 서비스의 인터페이스를 알아보자

$ ros2 service type /gazebo/get_entity_state

인터페이스명: gazebo_msgs/srv/GetEntityState 

인터페이스에 대해 자세히 알아보자

$ ros2 interface show gazebo_msgs/srv/GetEntityState

---

string name
string reference_frame
---
std_msgs/Header header
        builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
        string frame_id
gazebo_msgs/EntityState state
        string name
        geometry_msgs/Pose pose
                Point position
                        float64 x
                        float64 y
                        float64 z
                Quaternion orientation
                        float64 x 0
                        float64 y 0
                        float64 z 0
                        float64 w 1
        geometry_msgs/Twist twist
                Vector3  linear
                        float64 x
                        float64 y
                        float64 z
                Vector3  angular
                        float64 x
                        float64 y
                        float64 z
        string reference_frame
bool success

---

서비스 호출 시, request에 상태를 알고 싶은 엔티티의 이름(name)과 기준 좌표 프레임(reference_frame)을 문자열로 기입해야 한다 (reference_frame은 입력하지 않을 경우 default값인 'world'가 사용됨)

정상적으로 호출되면 response에 Pose(position + orientation)과 Twist 상태값이 반환된다

※ orientation은 쿼터니언으로 반환됨

4.1.2. rclpy 구현 예시

# get_entity_state_example.py
import rclpy
from rclpy.node import Node
from gazebo_msgs.srv import GetEntityState

class GetEntityStateClientAsync(Node):
    def __init__(self):
        super().__init__('gazebo_get_entity_state_test_client')
        self.cli = self.create_client(GetEntityState, '/gazebo/get_entity_state')
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.req = GetEntityState.Request()

    def send_request(self, name: str, reference_frame: str = "world"):
        self.req.name = name
        self.req.reference_frame = reference_frame
        self.future = self.cli.call_async(self.req)
        rclpy.spin_until_future_complete(self, self.future)
        return self.future.result()

def main(args=None):
    rclpy.init(args=args)

    entity_name = "entity_name"
    reference_frame = "world"

    node = GetEntityStateClientAsync()
    response: GetEntityState.Response = node.send_request(entity_name, reference_frame)
    node.get_logger().info(f'result: {response.success}')
    node.get_logger().info(f'state: {response.state}')
    
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

4.2. Set Entity State

4.2.1. Interface

set_entity_state 서비스의 인터페이스를 알아보자

$ ros2 service type /gazebo/set_entity_state

인터페이스명: gazebo_msgs/srv/SetEntityState 

인터페이스에 대해 자세히 알아보자

$ ros2 interface show gazebo_msgs/srv/SetEntityState

---

gazebo_msgs/EntityState state
        string name
        geometry_msgs/Pose pose
                Point position
                        float64 x
                        float64 y
                        float64 z
                Quaternion orientation
                        float64 x 0
                        float64 y 0
                        float64 z 0
                        float64 w 1
        geometry_msgs/Twist twist
                Vector3  linear
                        float64 x
                        float64 y
                        float64 z
                Vector3  angular
                        float64 x
                        float64 y
                        float64 z
        string reference_frame
---
bool success                    # Return true if setting state was successful.

---

서비스 호출 시, request에 상태를 변경하고 싶은 엔티티의 이름(name)과 기준 좌표 프레임(reference_frame)을 문자열로 기입한 뒤, 변경하고자 하는 Pose(position + orientation)값을 float64 데이터형으로 입력해줘야 한다 (Twist값을 0이 아니게 입력하면 로봇이 계속 translation 혹은 rotation하게 된다)

※ 마찬가지로 orientation은 쿼터니언이므로 회전각이 오일러각으로 주어져있다면 적절하게 변환해줘야 한다

4.2.2. rclpy 구현 예시

import rclpy
from typing import Tuple
from rclpy.node import Node
from gazebo_msgs.srv import SetEntityState
from gazebo_msgs.msg import EntityState
import tf_transformations

class SetEntityStateClientAsync(Node):
    def __init__(self):
        super().__init__('gazebo_set_entity_state_test_client')
        self.cli = self.create_client(SetEntityState, '/gazebo/set_entity_state')
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.req = SetEntityState.Request()

    def send_request(self, 
                     name: str, 
                     position: Tuple[float, float, float],
                     orientation: Tuple[float, float, float],
                     reference_frame: str = "world"):
        self.req.state.name = name
        self.req.state.pose.position.x = position[0]
        self.req.state.pose.position.y = position[1]
        self.req.state.pose.position.z = position[2]
        quat = tf_transformations.quaternion_from_euler(orientation[0], orientation[1], orientation[2], 'sxyz')
        self.req.state.pose.orientation.x = quat[0]
        self.req.state.pose.orientation.y = quat[1]
        self.req.state.pose.orientation.z = quat[2]
        self.req.state.pose.orientation.w = quat[3]
        """
        self.req.state.twist.linear.x = 0.0
        self.req.state.twist.linear.y = 0.0
        self.req.state.twist.linear.z = 0.0
        self.req.state.twist.angular.x = 0.0
        self.req.state.twist.angular.y = 0.0
        self.req.state.twist.angular.z = 0.0
        """
        self.req.state.reference_frame = reference_frame
        self.future = self.cli.call_async(self.req)
        rclpy.spin_until_future_complete(self, self.future)
        return self.future.result()

def main(args=None):
    rclpy.init(args=args)

    entity_name = "entity_name"
    reference_frame = "world"
    position = (0.0, 0.0, 0.0)
    orientation = (0.0, 0.0, 0.0)

    node = SetEntityStateClientAsync()
    response = node.send_request(entity_name, position, orientation, reference_frame)
    node.get_logger().info(f'result: {response.success}')
    
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Tip) 이 예제에서는 tf_transformation 패키지를 사용해 오일러각을 쿼터니언으로 변경하게 구현했다

(짐벌락을 고려하지 않는다면 roll, pitch, yaw로 구성된 오일러각이 직관적이긴 하다 ㅎㅎ)

5. DEMO

비어있는 gazebo world에 정육면체를 하나 소환한 뒤, 1초 간격으로 위치를 이동시키는 코드를 통해 동작을 확인해보자

gazebo에 정육면체 소환

entity 이름은 'unit_box', 초기 위치는 (0, 0, 0.5)

 

1초에 한번씩 움직이는 기능은 간단하게 아래와 같이 rclpy의 timer 기능을 사용해서 구현해봤다

import rclpy
from typing import Tuple
from rclpy.node import Node
from gazebo_msgs.srv import SetEntityState
from gazebo_msgs.msg import EntityState
import tf_transformations

class SetEntityStateTestClient(Node):
    def __init__(self):
        super().__init__('gazebo_set_entity_state_test_client')
        self.cli = self.create_client(SetEntityState, '/gazebo/set_entity_state')
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.req = SetEntityState.Request()

        self.box_position_list = [
            (0, 0, 0.5),
            (1, 0, 0.5),
            (1, 1, 0.5),
            (0, 1, 0.5)
        ]
        self.box_position_index = 0
        self.timer = self.create_timer(1, self.move_unit_box)

    def send_request(self, 
                     name: str, 
                     position: Tuple[float, float, float],
                     orientation: Tuple[float, float, float],
                     reference_frame: str = "world"):
        self.req.state.name = name
        self.req.state.pose.position.x = float(position[0])
        self.req.state.pose.position.y = float(position[1])
        self.req.state.pose.position.z = float(position[2])
        quat = tf_transformations.quaternion_from_euler(orientation[0], orientation[1], orientation[2], 'sxyz')
        self.req.state.pose.orientation.x = quat[0]
        self.req.state.pose.orientation.y = quat[1]
        self.req.state.pose.orientation.z = quat[2]
        self.req.state.pose.orientation.w = quat[3]
        self.req.state.reference_frame = reference_frame
        self.future = self.cli.call_async(self.req)
        # rclpy.spin_until_future_complete(self, self.future)
        return self.future.result()

    def move_unit_box(self):
        box_position = self.box_position_list[self.box_position_index]
        self.send_request('unit_box', box_position, [0.0, 0.0, 0.0], 'world')
        self.get_logger().info(f'move box to {box_position}')
        self.box_position_index += 1
        if self.box_position_index >= len(self.box_position_list):
            self.box_position_index = 0

def main(args=None):
    rclpy.init(args=args)

    node = SetEntityStateTestClient()
    rclpy.spin(node)
    
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

 

[결과]