ROSで独自Service用ファイルを作ってService通信を試す

ROSの通信で頭の隅に置いとく知識

・ROSで行う通信（Topic通信とか）はmsgファイルのデータ型で行う。
・Service通信も拡張子がsrvのファイルのデータ型で行う。

備忘録なので独自設定のservice通信を動かすまでの過程を、淡々とまとめるだけの構成。

目次
1. 基礎的なService通信をやってみる
2. 独自Service用ファイルの作成
3. 独自Serviceの実行

1. 基礎的なService通信をやってみる

Service通信は双方向通信のことで、webサービスでよく使うcurlで投げて結果をクライアントから受け取る通信と同じ。

サービスはROSの通信方法の一つで、双方向通信に使います。ある仕事をお願いするクライアントとそれを処理して返すサーバーからなります。今回、作成するプログラムは速度指令値を送るクライアントと、それをロボットへ伝え(リクエスト）、ロボットから現在の速度を取得しクライアントに返す（レスポンス）サーバーです。
引用元：ROS演習４：双方向通信しよう！（サービス） – demura.net

まずは基礎的なService通信をやってみる

Service Serverの作成

---

roscd ros_start
vi scripts/service_server.py

service_server.py

#!/usr/bin/env python3
import rospy 
from std_srvs.srv import Empty
from std_srvs.srv import EmptyResponse

def handle_service(req):
    rospy.loginfo('called !!')
    return EmptyResponse()

def service_server():
    rospy.init_node('service_server')
    s = rospy.Service('call_me', Empty, handle_service)
    print("Ready to serve")
    rospy.spin()

if __name__=='__main__':
    service_server()

Clientの作成

---

vi scripts/service_client.py

service_client.py

#!/usr/bin/env python3
import rospy 
from std_srvs.srv import Empty

def call_service():
    rospy.loginfo('waiting service')
    rospy.wait_for_service('call_me')
    try:
        service = rospy.ServiceProxy('call_me', Empty)
        response = service()
    except rospy.ServiceException as e:
        print('Service call failed: %s' % e)

def service_client():
    rospy.init_node('service_client')
    call_service()
    rospy.spin()

if __name__=='__main__':
    service_client()
    call_service()

Service通信の実行

---

# 1つ目のterminal
roscore 

# 2つ目のterminal
roscd ros_start
rosrun ros_start service_server.py

#### serverが待ちの状態
# 3つ目のterminal
roscd ros_start
rosrun ros_start service_client.py

clientから指令を出して、Service側で受け取った。

2. 独自Service用ファイルの作成

今回はシミュレーション上のロボットを独自の設定のService通信で制御する。

設定内容

・外部プログラムから「速度」「角度」を指定して動かす
・指定範囲内の速度・角度だけ受け取って'success'を返す
・指定範囲外は全て'failed'を返す

入力：速度、角度(float64)
出力：sucessかfailedの文字列(bool)

clientで入力を受け取り、serverで出力を返すように設定したsrvファイルを作ってservice通信を行う。

1. SetVelocity.srvの作成

---

roscd ros_start
mkdir srv
vi srv/SetVelocity.srv

SetVelocity.srv

float64 linear_velocity
float64 angular_velocity
---
bool success

2. package.xmlの書き換え

---

message_generation
message_runtime
をコメントアウトする。

$ roscd ros_start
$ vi package.xml

<build_depend>message_generation</build_depend>
 <!-- Use build_export_depend for packages you need in order to build against this package: -->
 <!--   <build_export_depend>message_generation</build_export_depend> -->
 <!-- Use buildtool_depend for build tool packages: -->
 <!--   <buildtool_depend>catkin</buildtool_depend> -->
 <!-- Use exec_depend for packages you need at runtime: -->
 <exec_depend>message_runtime</exec_depend>

3. CMakeLists.txtの書き換え

---

vi CMakeLists.txt

以下の3箇所を書き換え

CMakeLists.txt

## Generate services in the 'srv' folder
add_service_files(
  FILES
  SetVelocity.srv #書き換え
)

## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs #追加
)

## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
 roscpp
 rospy
 std_msgs
 message_generation #追加
)

# check
rossrv show ros_start/SetVelocity
>>>
### float64 linear_velocity
### float64 angular_velocity
### ---
### bool success

4. プログラムで使うためにcatkin_make

---

cd ~/catkin_ws
catkin_make
>>>
### [100%] Built target ros_start_generate_messages_py
### Scanning dependencies of target ros_start_generate_messages
### [100%] Built target ros_start_generate_messages

# 再起動
sudo reboot

これで独自serviceファイルがプログラムで使えるようになった。

3. 独自Serviceの実行

1. serverスクリプトの作成

---

roscd ros_start/scripts
vi velocity_server.py

velocity_server.py

#!/usr/bin/env python3
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
    print(vel)
    if is_set_success:
        pub.publish(vel)
    return SetVelocityResponse(success=is_set_success)

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

2. clientスクリプトの作成

---

vi velocity_client.py
velocity_client.py

#!/usr/bin/env python3
import rospy
from ros_start.srv import SetVelocity
import sys

if __name__ == '__main__':
    rospy.init_node('velocity_client')
    set_velocity = rospy.ServiceProxy('set_velocity', SetVelocity)
    linear_vel = float(sys.argv[1])
    angular_vel = float(sys.argv[2])
    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))

roscd ros_start
sudo chmod 755 scripts/*

3. service通信を実行

---

# １つ目のteminalで
roscore

# 2つ目のteminalでServer(velocity_server.py)の実行
roscd ros_start
rosrun ros_start velocity_server.py

# 3つ目のteminalでclient(velocity_client.py)の実行

# パラメータ　2.0 3.0で1回目の送信　
rosrun ros_start velocity_client.py 2.0 3.0
### [ERROR] [1641964505.376901]: set [2.000000, 3.000000] failed

指定範囲外なので失敗。failedがserviceから返ってきた

# パラメータ　0.5 0.0で2回目の送信

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

指定範囲内なので成功。=> successがserviceから返ってきた

Serviceの基本と独自Serviceを作って動かすまででした。
これを実際のロボットを遠隔で制御操作するのが目的。