9. Service Server & Client 작성해보기 – python

🛎️

9. Service Server & Client 작성해보기 – python

지난 시간에 이야기한 바와 같이 service server를 생성하는 방법부터 시작해보겠습니다.

이번 시간의 예제를 실행시켜 보겠습니다.

Turning tinybot

---

$ roslaunch gcamp_gazebo gazebo_world.launch 
# gazebo 시뮬레이션이 완전히 켜질때까지 기다려 주세요!!

# 새 터미널을 엽니다.
$ rosrun service_tutorial robot_turning_srv.py

# 또 다른 터미널을 엽니다.
$ rosrun service_tutorial robot_turning_client.py
[INFO] [1610111418.391926, 0.000000]: ==== Robot Turning Server Started ====
> Type turning time duration: 

이번 예제는, 사용자로부터 time, velocity input을 받도록 되어 있으니, 적절한 값을 입력해 주시면 됩니다.

• 여기서의 속도는 회전하는 각속도를 의미하며, 단위는 radian입니다.

• 시간은 앞선 각속도로 회전하는 시간을 의미하며, 단위는 초입니다.

로봇이 원하는 시간, 원하는 속도로 움직였나요??

---

시간과 속도를 잘 조절해서 90도, 180도 등 원하는 각도만큼 로봇이 회전하도록 해보세요 🙂

그럼, 코드를 분석해 보도록 하겠습니다!! client, server 순으로 분석해 보겠습니다.

robot_turning_client.py

---

#! /usr/bin/env python

"""
basic rospy service example

referenced from wiki.ros.org

url : http://wiki.ros.org/ROS/Tutorials/WritingServiceClient%28python%29 
"""

import sys
import rospy
from service_tutorial.srv import ControlTurningMessage, ControlTurningMessageRequest

rospy.init_node("robot_turning_client")
rospy.loginfo("==== Robot Turning Server Started ====")

rospy.wait_for_service("/control_robot_angle")
service_client = rospy.ServiceProxy("/control_robot_angle", ControlTurningMessage)

request_srv = ControlTurningMessageRequest()

while not rospy.is_shutdown():
    try:
        t = input("> Type turning time duration: ")
        vel = input("> Type turning angular velocity: ")

        if vel > 1.5707:
            raise ArithmeticError("Velocity too high !!")

        request_srv.time_duration = t
        request_srv.angular_vel = vel
        break
    except ArithmeticError as e:
        rospy.logerr(e)
    except Exception as e:
        rospy.logerr("Not a number type number plz !!")

result = service_client(request_srv)

print(result)

• topic에서 message import를 해주었던 것과 같이 service에서 사용되는 srv를 import 해줍니다.

# 이전 topic message에서는 이렇게 사용했죠?
from sensor_msgs.msg import LaserScan 

# service는 이렇게 .srv로 import 합니다.
from service_tutorial.srv import ControlTurningMessage, ControlTurningMessageRequest

client가 request를 해야 하는데, 그 대상이 되는 server가 없다면 곤란할 것입니다.

이를 위해 wait_for_service를 통해 server가 동작중인지 확인하고, ServiceProxy를 생성합니다.

rospy.wait_for_service('/control_robot_angle')

• result 부에는 server로 부터 되돌아온 response가 담겨 있습니다. 해당 예제에서는 단순히 성공 여부만 판단하였지만, 추가 코드를 통해 더 많은 기능을 구현할 수 있겠죠?

service_client = rospy.ServiceProxy('/control_robot_angle', ControlTurningMessage)

(...)

result = service_client(request_srv)

print(result)

나머지 부분은 사용자로부터 입력을 받고, 예외처리를 하는 부분입니다.

지금 강의를 듣는 시점에서는 모두 문제없이 의미있는 숫자를 입력하겠지만, 내가 만든 코드를 다른 사람이 사용하는 경우, 온갖 예외상황들이 발생할 수 있을 것입니다. 이를 위해, 파이썬의 try/except문을 사용하였습니다.

while not rospy.is_shutdown():
    try:
        t = input('> Type turning time duration: ')
        vel = input('> Type turning angular velocity: ')

        if vel > 1.5707:    
            raise ArithmeticError("Velocity too high !!")

        request_msg.time_duration = t
        request_msg.angular_vel = vel
        break
    except ArithmeticError as e:
        rospy.logerr(e)
    except Exception as e:
        rospy.logerr("Not a number type number plz !!")

💡

참고로, 1.5707이 어떠한 숫자냐면, pi (3.141)의 절반이 되는 숫자입니다. 파이썬의 math 라이브러리를 가져와도 좋지만, 자주 사용되는 숫자라 외워두면 편합니다.

이번에는 service server 코드를 살펴보겠습니다!

robot_turning_srv.py

---

#! /usr/bin/env python

import time
import rospy
from service_tutorial.srv import ControlTurningMessage, ControlTurningMessageResponse
from geometry_msgs.msg import Twist

def callback(request):

    print(request)
    
    # publish to cmd_vel
    cmd_vel = Twist()
    cmd_vel.angular.z = request.angular_vel
    start_time = time.time()

    print('\nRobot Turning...')
    while time.time() - start_time < request.time_duration:
        velocity_publisher.publish(cmd_vel)
    
    print("Done ...")
    cmd_vel.angular.z = 0.0
    velocity_publisher.publish(cmd_vel)

    response = ControlTurningMessageResponse()
    response.success = True

    return response

rospy.init_node('robot_turning_server')
rospy.loginfo("==== Robot Turning Server Started ====")

service_server = rospy.Service('/control_robot_angle', ControlTurningMessage, callback)
velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=1 )

rospy.spin()

이전 코드와 비교해서, import 부분에 차이가 보입니다.

# client
from service_tutorial.srv import ControlTurningMessage, ControlTurningMessageRequest

# server
from service_tutorial.srv import ControlTurningMessage, ControlTurningMessageResponse

• ControlTurningMessage srv 형식에 대해 조회해볼까요?

$ rossrv info ControlTurningMessage
[service_tutorial/ControlTurningMessage]:
uint32 time_duration
float64 angular_vel
---
bool success

[cpp_service_tutorial/ControlTurningMessage]:
uint32 time_duration
float64 angular_vel
---
bool success

이것을 파이썬 코드에서 사용하는 경우는 [MessageName]Request, [MessageName]Response 이런 형태를 갖게 됩니다. import 한 뒤에는 적절한 값을 채워줘야 하겠지요

    response = ControlTurningMessageResponse()
    response.success = True
		
		return response

ControlTurningMessage는 어디에서 나온 것일까요? srv들을 조회하려면, rossrv 커맨드를 사용합니다.

$ rossrv list
control_msgs/QueryCalibrationState
control_msgs/QueryTrajectoryState
control_toolbox/SetPidGains
controller_manager_msgs/ListControllerTypes
controller_manager_msgs/ListControllers
controller_manager_msgs/LoadController
controller_manager_msgs/ReloadControllerLibraries
controller_manager_msgs/SwitchController
controller_manager_msgs/UnloadController
diagnostic_msgs/AddDiagnostics
diagnostic_msgs/SelfTest
dynamic_reconfigure/Reconfigure
(...)

지금은 예제를 위해 제가 만들어둔 srv를 사용하고 있으며, 다음 위치에서 확인이 가능합니다.

$ cd ~/gcamp_ws/src/gcamp_ros_basic/service_tutorial/srv
$ ls -al
ControlTurningMessage.srv
$ cat ControlTurningMessage.srv
uint32 time_duration
float64 angular_vel
---
bool success

최종적으로, service_tutorial에서는 다음과 같은 세가지 srv를 사용할 수 있습니다.

• ControlTurningMessage

• ControlTurningMessageRequest

• ControlTurningMessageResponse

• 코드로 돌아와서, import 부를 다시 정리하면, client에게 response는 불가하니 request만 import 하면 될 것이며, 다음과 같은 이름으로 가져옵니다.

from service_tutorial.srv import ControlTurningMessage, ControlTurningMessageResponse

• 이제는 친숙한 init_node, loginfo가 보이고, Service Server class를 생성하는 부분이 보입니다.

rospy.init_node('robot_turning_server')
rospy.loginfo("==== Robot Turning Server Started ====")

service_server = rospy.Service('/control_robot_angle', ControlTurningMessage, callback)
velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=1 )

rospy.spin()

• rospy.Sevice('service-server-name', 'srv', 'callback')

이처럼, 기본 정보들과 함께 초기화를 해준 뒤, 요청이 들어올 때마다 콜백이 실행됩니다.

💡

여기서, 혼란스러울 수 있는 점이 ControlTurningMessage를 인자로 넣어주어야 한다는 점입니다.

rospy.Service('/control_robot_angle', ControlTurningMessageResponse, callback)

server는 response만 사용하니까 ControlTurningMessageResponse를 넣어야 하지 않나? 하는 생각으로 실수할 수 있습니다.

• 다시 코드로 돌아와서, 본 예제가 각속도, 시간을 request 받아 로봇을 움직이는 것이었기에 cmd_vel로 publish하는 publisher도 만들었습니다.

velocity_publisher = rospy.Publisher("/cmd_vel", Twist, queue_size=1)

• 다음은 callback입니다. client로부터 request가 올 때마다 실행될 부분으로, request srv에서 각속도와 시간을 가져와 cmd_vel로 publish하는 부분입니다.

def callback(request):

    print(request)
    
    # publish to cmd_vel
    cmd_vel = Twist()
    cmd_vel.angular.z = request.angular_vel
    start_time = time.time()

    print('\nRobot Turning...')
    while time.time() - start_time < request.time_duration:
        velocity_publisher.publish(cmd_vel)
    
    print("Done ...")
    cmd_vel.angular.z = 0.0
    velocity_publisher.publish(cmd_vel)

    response = ControlTurningMessageResponse()
    response.success = True

    return response

마지막으로, 모든 작업이 완료되었다면, client에게 성공했다는 response를 보내야 할 것입니다.

현재는 타입이 bool이기 때문에 True를 넣어주었습니다.

gazebo services

---

우리가 roslaunch를 통해 gazebo를 실행시키는 것과, 일반적으로 gazebo를 실행시킬 때에는 차이가 있습니다.

roslaunch로 실행시키는 경우에는 gazebo_ros를 통해 rosnode들도 같이 실행된다는 점입니다. 그래서, 모델을 불러오고, 제거하고, 위치를 이동시키고, 중력 상수를 바꾸는 등의 수많은 service들이 같이 실행됩니다.

그 중에서, 이번 시간에 배운 모델 spawn을 포함하여, 모델 제거, 시뮬레이션 초기화를 request하는 service client들을 예시로 제공하니, gazebo를 통해 더 많은 응용과 연습을 해보시길 바랍니다. 🤩🤩

#! /usr/bin/env python

"""
Fuctions for Gazebo model Spawn and Deletion
created by kimsooyoung : https://github.com/kimsooyoung
"""

import rospy
import rospkg

from std_srvs.srv import Empty
from geometry_msgs.msg import Pose
from gazebo_msgs.srv import SpawnModel, DeleteModel

def GazeboSpawnModel( name, pkg_name, pose, namespace='', frame='world'):
    # model_name
    model_name = name

    # model_xml
    rospack = rospkg.RosPack()
    model_path = rospack.get_path(pkg_name)+'/models/'

    with open (model_path + model_name + '.urdf', 'r') as xml_file:
        model_xml = xml_file.read().replace('\n', '')

    # robot_namespace
    robot_namespace = namespace

    # initial_pose
    initial_pose = Pose()
    initial_pose.position = pose.position

    # z rotation -pi/2 to Quaternion
    initial_pose.orientation = pose.orientation

    # reference_frame
    reference_frame = frame

    # service call
    spawn_model_prox = rospy.ServiceProxy('gazebo/spawn_urdf_model', SpawnModel)
    result = spawn_model_prox(
        model_name, model_xml,
        robot_namespace, initial_pose,
        reference_frame) 

    ''' result fromat
    bool success
    string status_message
    '''

    print(result.status_message)
    

def GazeboDeleteModel(name):

    '''
    string model_name
    ---
    bool success
    string status_message
    '''

    delete_srv = DeleteModel()

    model_name = name

    spawn_model_prox = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
    result = spawn_model_prox(model_name)

    print(result.status_message)


def GazeboResetSimulation():
    spawn_model_prox = rospy.ServiceProxy('gazebo/reset_simulation', Empty)
    result = spawn_model_prox()

    rospy.loginfo('Gazebo Simulation Reset')

rospy.init_node("gazebo_spawn_handler")

지금까지 ROS의 service에 대해 배워보았습니다. 다음으로 action에 대해 배워보겠습니다!! 👨🏻‍💻