8. rosservice, service client, rossrv
์ด๋ฒ ์๊ฐ์๋ ๋๋ค๋ฅธ node๊ฐ ํต์ ๋ฉ์ปค๋์ฆ์ธ Service์ ๋ํด ๋ฐฐ์๋ณด๊ฒ ์ต๋๋ค.
ํญ์ ๊ทธ๋ฌ๋ ๊ฒ์ฒ๋ผ, ์์ ๋ฅผ ์คํ์์ผ๋ด ์๋ค!!
r2d2 spawn
$ roslaunch gcamp_gazebo gazebo_world.launch
# gazebo ์๋ฎฌ๋ ์ด์
์ด ์์ ํ ์ผ์ง๋๊น์ง ๊ธฐ๋ค๋ ค ์ฃผ์ธ์!!
$ rosrun service_tutorial spawn_model_client.py
์ฌ์ง๊ณผ ๊ฐ์ด r2d2๊ฐ ๋ฑ์ฅํ๋์? ๐ฟ
์ง๊ธ ์ฐ๋ฆฌ๋ gazebo_ros์ service server์๊ฒ r2d2๋ฅผ ๋ฑ์ฅ์์ผ ๋ฌ๋ผ๋
service call
์ ํ ๊ฒ์
๋๋ค!!
ROS Service ๊ฐ๋
์ถ์ฒ : mathworks
๊ทธ๋ผ topic๊ณผ service๋ ์ด๋ค ์ฐจ์ด๊ฐ ์๋ ๊ฑธ๊น์?
- A topic์ด ์คํ๋๋ ๋์, ๋ค๋ฅธ B topic๋ ๊ฐ์ด ์คํ๋ ์ ์์ต๋๋ค. ๋ฐ๋ฉด, service๋ ํ์ฌ ์งํ์ค์ธ service์ request๊ฐ ์ฒ๋ฆฌ๋๋ ๋์, ๋ค๋ฅธ service๋ ๊ธฐ๋ค๋ฆฌ๊ณ ์์ด์ผ ํฉ๋๋ค.
- topic publish๋ฅผ ํ๋ฉด, A node๋, B node๋ subscribe๊ฐ ๊ฐ๋ฅํฉ๋๋ค. ๋ฐ๋ฉด, service๋ request๊ฐ ์จ ๋์์๊ฒ๋ง response๋ฅผ ์ค๋๋ค.
์ด๋ค ์ํฉ์๋ ๋ฌด์กฐ๊ฑด service๋ฅผ ์จ์ผ ํ๋ค, ์ด๋ฐ ๋ฒ์น์ ์์ต๋๋ค. ํ์ง๋ง, ๋ค์๊ณผ ๊ฐ์ด ๊ฐ ๋ฉ์ปค๋์ฆ์ ์ ํฉํ ์ํฉ์ ์๊ฐํด ๋ณผ ์ ์์ ๊ฒ์ ๋๋ค.
- ๋ถ๋ช ํ ์์ฒญ์ ์ฃผ์ฒด๊ฐ ์์ผ๋ฉฐ, ๋น ๋ฅด๊ฒ ๋์์ด ์๋ฃ๋๋ ๊ฒฝ์ฐ์๋ service
- ๋ถํน์ ํ node๊ฐ subscribe์ ๋์์ด ๋๋ฉฐ, ์ง์์ ์ผ๋ก ๋ฐ์ดํฐ์ ์ก์์ ์ด ์ผ์ด๋๋ ๊ฒฝ์ฐ์๋ topic
์ง๋ ์๊ฐ์๋ ์ธ๊ธํ์ง๋ง, ์ ๊ฐ์ธ์ ์ผ๋ก๋ ์ฐธ๊ณ ํ ๋งํ ์คํ์์ค ํ๋ก์ ํธ๋ฅผ ์ดํผ๋ ๊ฒ์ ์ถ์ฒํฉ๋๋ค.
๋๋ถ์ด, ์ข ๋ ์์ธํ ๋น๊ต ๋ถ์์ ๋ํ ๋ ํผ๋ฐ์ค๋ฅผ ๋จ๊ธฐ๋, ํ๋ฒ์ฉ ๋ณด์๋ฉด ์ข์ ๊ฒ ๊ฐ์ต๋๋ค ๐
from wiki.ros.org : Topics vs Services vs Actionlib...
rosservice ์ปค๋งจ๋
์ด๋ฒ์๋ ๋ฑ์ฅ์ํจ r2d2 ๋ก๋ด์ ์ญ์ ์์ผ๋ณด๋๋ก ํ๊ฒ ์ต๋๋ค.
$ rosservice call /gazebo/delete_model "model_name: 'r2d2'"
rostopic์ฒ๋ผ rosservice ๋ํ ์ฌ๋ฌ ์ต์
์ ๊ฐ์ง service์ฉ ์ปค๋งจ๋์
๋๋ค.
rosservice call์ ํตํด ์ฐ๋ฆฌ๋ ๋ฐฉ๊ธ /gazebo/delete_model
๋ผ๋ service์๊ฒ r2d2๋ฅผ ์์ ๋ฌ๋ผ๊ณ ์์ฒญํ ๊ฒ์
๋๋ค.
์ด์ topic ์๊ฐ์์๋ ๋ณด์๋ฏ์ด, ๊ฐ๋จํ ROS ํต์ ๋ฉ์ปค๋์ฆ์ ํฐ๋ฏธ๋์์๋ ํธ์ถ์ํฌ ์ ์์ต๋๋ค. ์ง๊ธ๊ณผ ๊ฐ์ ๊ฒฝ์ฐ, ๋จ์ํ model delete๋ฅผ call ํ ๊ฒ์ด๋ฏ๋ก ์ด๋ ๊ฒ ํฐ๋ฏธ๋ ๋จ์์ ํด๊ฒฐํ ๊ฒ์ด์ฃ .
service client ์ฝ๋ ๋ถ์
gazebo service server์ call์ ํด์ r2d2๋ฅผ ๋ฑ์ฅ์์ผฐ๋ ์ฝ๋๋ฅผ ๋ถ์ํด ๋ณผ๊น์? ?
#! /usr/bin/env python
import math
import rospy
import rospkg
from geometry_msgs.msg import Pose
from gazebo_msgs.srv import SpawnModel
rospy.init_node("gazebo_spawn_model")
# model_name
model_name = 'r2d2'
# model_xml
rospack = rospkg.RosPack()
model_path = rospack.get_path('service_tutorial')+'/models/'
with open (model_path + model_name + '.urdf', 'r') as xml_file:
model_xml = xml_file.read().replace('\n', '')
# robot_namespace
robot_namespace = ''
# initial_pose
initial_pose = Pose()
initial_pose.position.x = -2
initial_pose.position.y = 1
initial_pose.position.z = 1
# z rotation -pi/2 to Quaternion
initial_pose.orientation.z = -0.707
initial_pose.orientation.w = 0.707
# reference_frame
reference_frame = 'world'
# 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)
- ์ด๋ค ํ์์ ์๊ตฌํ๊ธธ๋ ์ด๋ ๊ฒ ๋ณต์กํ ๊น์? ์ฌ์ค์ srv๋ฅผ ์ฑ์ฐ๋ ๋ถ๋ถ์ด ๋ค์์ด๋ฉฐ ์์ topic์์๋ msg ํ์์ ์ฌ์ฉํ๋ ๊ฒ์ฒ๋ผ, ์๋น์ค๋ srv๋ฅผ ์ฌ์ฉํฉ๋๋ค.
import math
import rospy
import rospkg
from geometry_msgs.msg import Pose
from gazebo_msgs.srv import SpawnModel
- ๋ชจ๋ธ์ spawn์ํค๊ธฐ ์ํด์ ์ด๋ค service์๊ฒ ์์ฒญํ ์ง, ์ด๋ฐ ์ ๋ณด๋ gazebo ๋ฌธ์์์ ํ์ธํ ์ ์์ต๋๋ค. โ
gazebo/spawn_urdf_model
$ rosservice info gazebo/spawn_urdf_model
$ rossrv show gazebo_msgs/SpawnModel
string model_name
string model_xml
string robot_namespace
geometry_msgs/Pose initial_pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
string reference_frame
---
bool success
string status_message
topic ์๊ฐ์์ message type์ ์กฐ์ฌํ๋ ๊ณผ์ ๊ณผ ์ ์ฌํ์ฃ ? service์์ ์ค๊ฐ๋ ๋ฐ์ดํฐ
ํ์์ ์กฐํํ๊ธฐ ์ํด์๋ rossrv show
๋ฅผ ์ฌ์ฉํฉ๋๋ค!!
- rossrv show vs rossrv info
- ์ค๊ฐ์ - - - ์ด๋ฐ ๋ถ๋ถ์ด ๋ณด์ ๋๋ค. ์ด๊ฒ์ request์ response๋ฅผ ๊ตฌ๋ถํด์ฃผ๋ ์นธ๋ง์ด๋ผ๊ณ ์๊ฐํ์๋ฉด ๋ฉ๋๋ค.
uint32 time_duration
float64 angular_vel
---
bool success
uint32, float64, bool, string๊ณผ ๊ฐ์ด ํ๋ก๊ทธ๋๋ฐ ์ธ์ด์์ ์ด๋ฏธ ์ต์ํ ์๋ฃํ์ ์ธ ์๋ ์๊ณ , ROS์์ ์ ๊ณตํ๋ geometry_msgs, sensor_msgs์ ๊ฐ์ ์๋ฃํ๋ ์ฌ์ฉ์ด ๊ฐ๋ฅํฉ๋๋ค.
๋๋ถ์ด, ์ด๋ค์ ์กฐํฉํด์ ๋๋ง์ srv ํ์ ๋ ๋ง๋ค ์ ์์ง์. ํ ๊ฐ์์์๋ ์์ธํ๊ฒ ๋ค๋ฃจ์ง ์๊ฒ ์ง๋ง, ์ถ๊ฐ ๋ ํผ๋ฐ์ค๋ฅผ ๊ณต์ ํ๋ ํ๋ฒ์ฉ ๋ฐ๋ผํด ๋ณด์ ๋ ์ข์ ๊ฒ ๊ฐ์ต๋๋ค!!
๋ค์ ์์ค์ฝ๋๋ก ๋์์์, ๋ค์ ๋ณต์กํด ๋ณด์์ง๋ง gazebo์์ ์๊ตฌํ๋ ์ ๋ณด๋ค์ด ๋ค์ํด์ ๋ง์ถฐ์ฃผ๊ธฐ ์ํ ๋ฐ๋จ๊ณ๊ฐ ๋ง์ ๋ฟ ํต์ฌ์ ๋ค์ ๋์ค์ ๋๋ค.
- ์ด๊ฒ์ด ํต์ฌ!!
# 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)
- rospy.ServiceProxy('<service-server-name>', service-type)
rospy.ServiceProxy()๋ฅผ ํตํด service client ๊ฐ์ฒด๋ฅผ ๋ง๋ค๊ณ , ์์ ์ดํด๋ณธ๋๋ก ํ์ํ ๋ฐ์ดํฐ๋ค์ ์ทจํฉํ ๋ค, requestํ๋ ๋ถ๋ถ์ ๋๋ค!!
requestํ ๋ค์๋ ๋ญ๊ฐ ์ค์ฃ ? โ response๊ฐ ์ต๋๋ค.
result์ ๋ฐ๋ก response๊ฐ ๋ด๊ฒจ ์์ผ๋ฉฐ, ๊ทธ๋์ ์ด๋ฅผ ์ถ๋ ฅํ์ ์, ๋ค์๊ณผ ๊ฐ์ ๋ผ์ธ์ด ๋ฑ์ฅํ๋ ๊ฒ์ ๋๋ค.
success: True
status_message: "SpawnModel: Successfully spawned entity"
์ง๊ธ๊น์ง service client์ ๋ํด ์ค๋ช
ํ๋๋ฐ์,
gazebo/spawn_urdf_model
๋ผ๋ service server๋ ์ธ์ ์คํ๋ ๊ฒ์ผ๊น์??
- gazebo_world.launch
<!-- Launch Gazebo World -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="use_sim_time" value="true"/>
<arg name="debug" value="false"/>
<arg name="gui" value="true" />
<arg name="world_name" value="$(arg world_file)"/>
</include>
gazebo_ros/empty_world.launch๋ฅผ ์คํ์ํค๋ฉด, gazebo์ ๊ด๋ จ๋ ์๋ง์ service๋ค์ด ํจ๊ป ์คํ๋ฉ๋๋ค. ์ด ์ค์ gazebo_msgs/SpawnModel๋ ์๋ ๊ฒ์ด์ฃ .
$ rosservice list | grep gazebo
๋ชป๋ณด๋ ์๋ก์ด ํค์๋๊ฐ ๋ฑ์ฅํ์ต๋๋ค. | grep gazebo
?? ์ด๊ฑด ์ด๋ค
๋ป์ผ๊น์??
grep ํค์๋๋ฅผ ์ฌ์ฉํ๋ฉด, ์์ ํค์๋์ ๊ฒฐ๊ณผ์ ์ถ๊ฐ์ ์ผ๋ก ๊ฒ์์กฐ๊ฑด์ ๋ถ์ผ ์ ์์ต๋๋ค.
ํ์ผ์ด๋, ํด๋๋ฅผ ์ฐพ๊ณ ์ถ์ ๊ฒฝ์ฐ๋ ์ฌ์ฉ ๊ฐ๋ฅํ๋ฉฐ, ์ด๋ด ๋๋ find ์ปค๋งจ๋๋ฅผ ์ฌ์ฉํ๊ธฐ๋ ํฉ๋๋ค.
$ ls -al | grep <something>
$ find -name <file-or-directory-name>
์ด๋ฒ ์๊ฐ์๋ ๋ง์ ๊ฒ๋ค์ ๋ฐฐ์ ์ต๋๋ค. ๋ค์ ์๊ฐ์๋ service server์ ๋ํ ๋ด์ฉ๋ถํฐ ์์ํ๋๋ก ํ๊ฒ ์ต๋๋ค! ๐๐๐