【ROS】Topic/Serviceの作成

画像元

独自のService型の製作

• 外部プログラムから速度（linear.xとangular.z）を指定して動かす
• 指定できる速度の値の範囲が決まっており、それ以上の速度は無視する
• 速度がセットできたか、無視されたかわかる

$ roscd ros_start
$ mkdir srv
$ vi srv/SetVelocity.srv

SetVelocity.srvに以下の4行を記入

float64 linear_velocity
float64 angular_velocity
---
bool success

ros_start直下のpackage.xmlの編集

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

上の2つのコメントアウトを解除

CMakeLists.txtの編集

• find_packageにmessage_generationを書き加える
• add_service_filesを以下のように編集

## Generate services in the 'srv' folder
add_service_files(
   FILES
   SetVelocity.srv
)

• generate_messagesのコメントアウトを解除

$ cd ~/catkin_ws/
$ catkin_make

$ rossrv show ros_start/SetVelocity 
float64 linear_velocity
float64 angular_velocity
---
bool success

うまくできていれば、SetVelocity.srvに書いた定義が出力されます。

ServiceServerの作成

$ roscd ros_start/scripts/
$ vi velocity_server.py 

#!/usr/bin/env python

############### srv file adress #################
## ~/catkin_ws/src/ros_start/srv/SetVelocity.srv
#################################################

import rospy
from geometry_msgs.msg import Twist
from ros_start.srv import SetVelocity
from ros_start.srv import SetVelocityResponse

MAX_LINEAR_VELOCITY = 1.0
MIN_LINEAR_VELOCITY = -1.0
MAX_ANGULAR_VELOCITY = 2.0
MIN_ANGULAR_VELOCITY = -2.0

def velocity_handler( req ):
    vel = Twist()
    is_set_success = True
    
    if req.linear_velocity <= MAX_LINEAR_VELOCITY and ( req.linear_velocity >= MIN_LINEAR_VELOCITY):
        vel.linear.x = req.linear_velocity
    else:
        is_set_success = False
    
    if req.angular_velocity <= MAX_ANGULAR_VELOCITY and ( req.angular_velocity >= MIN_ANGULAR_VELOCITY ):
        vel.angular.z = req.angular_velocity
    else:
        is_set_success = False

    if is_set_success:
        pub.publish( vel )
    
    return SetVelocityResponse( success = is_set_success )


if __name__ == '__main__':
    rospy.init_node( 'velocity_server' )
     
    # Publish
    pub = rospy.Publisher( '/mobile_base/commands/velocity', Twist, queue_size = 10 )
    # Service
    service_server = rospy.Service( 'set_velocity', SetVelocity, velocity_handler )
    
    rospy.spin()

• 9、10行目はsrvであるSetVelocityと、その返り値のSetVrlocityResponseをインポート
• 17行目のvelocity_handlerはServiceが呼ばれた時に実行される関数。reqにはSetVelocityRequestの以下のようなインスタンスが入る

linear_velocity: 0.5
angular_velocity: 0.0

• 34行目はis_set_successをセットしたインスタンスを返している。（Serviceではsrv名＋Response型のインスタンスを返り値として返す必要がある）
• 45行目はServiceが呼ばれるのを待機している

ServiceClientの作成

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from ros_start.srv import SetVelocity
import sys

if __name__ == '__main__':
    rospy.init_node( 'velocity_client' )
    
    # ServiceProxy to call Service
    set_velocity = rospy.ServiceProxy( 'set_velocity', SetVelocity )
    
    linear_vel = float( sys.argv[1] )
    angular_vel = float( sys.argv[2] )

    # Discrimination
    response = set_velocity( linear_vel, angular_vel )
    if response.success:
        rospy.loginfo( 'set[%f, %f] success' % ( linear_vel, angular_vel ))
    else:
        rospy.logerr( 'set[%f, %f] failed' % ( linear_vel, angular_vel ))

ServerとClinetを実行

$ roscore

$ chmod 755 velocity_server.py velocity_client.py 
$ roslaunch kobuki_gazebo kobuki_playground.launch

$ rosrun ros_start velocity_client.py 0.5 0.0
[INFO] [1557723633.163754, 0.000000]: set[0.500000, 0.000000] success

rosserviceについて

$ rosservice list 

でrosserviceの一覧を取得できる

$ rosservice call /set_velocity 1.0 0.0
success: True

このように、ServiceClientがなくても、Serviceを直接呼び出すことができる

$ rosservice info /set_velocity 
Node: /velocity_server
URI: rosrpc://localhost:41959
Type: ros_start/SetVelocity
Args: linear_velocity angular_velocity

rosservice infoで方や引数に関する情報を見ることができる
rosserviceをクライアントの代わりに使用しデバッグを行うことができる

しかし、Serviceはrosbagで記録が取れないことや、rvizで可視化ができないため、可能な限りTopicを使用するほうが望ましい