サービス通信の例(仮)

serviceを用いたnode間通信の例を挙げます。

task_manager.pyで起動したノードから、 serviceを用いてmove_robot.pyで起動したノードの機能を使用する例です。

pythonファイルの用意

navigation_tutorial/scripts/に以下の2つのファイルを作成してください (コピペで構いません)。

task_manager.py

#!/usr/bin/env python3
import rospy
from navigation_tutorial.srv import MoveTrigger, MoveTriggerRequest

class TaskManager:
    def __init__(self):
        rospy.init_node('task_manager')

        # service client
        self.move_robot = rospy.ServiceProxy('/move_robot', MoveTrigger)

        self.move_robot.wait_for_service()
        rospy.loginfo("Service /move_robot is ready!")

    def move(self, straight, turn):
        command = MoveTriggerRequest()
        command.straight = straight
        command.turn = turn
        rospy.loginfo(f"request: straight={straight}, turn={turn}")
        response = self.move_robot(command) # service call
        return response.success

    def main(self):
        """
        タスクの流れを手続的に記述する
        """
        result = self.move(1.0, 0.0)
        if result:
          rospy.loginfo("move success!")
        else:
          rospy.loginfo("move failed!")
        
        result = self.move(0.0, 90.0)
        if result:
          rospy.loginfo("turn success!")
        else:
          rospy.loginfo("turn failed!")

        rospy.loginfo("task completed!")

if __name__ == '__main__':
    task_manager = TaskManager()
    task_manager.main()

move_robot.py

#!/usr/bin/env python3
import numpy as np

import rospy
import tf
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from navigation_tutorial.srv import MoveTrigger, MoveTriggerResponse

class RobotController:
    def __init__(self):
        rospy.init_node('robot_controller', anonymous=True)
        
        # Publisher
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        # Subscriber
        odom_sub = rospy.Subscriber('/odom', Odometry, self.callback_odom)

        # Service Server
        rospy.Service('/move_robot', MoveTrigger, self.callback_move_robot)

        self.x = None
        self.y = None
        self.yaw = None
        while self.x is None:
            rospy.sleep(0.1)

    def callback_move_robot(self, req):
        res = MoveTriggerResponse()
        try:
            self.go_straight(req.straight)
            if req.turn >= 0:
                self.turn_left(req.turn)
            else:
                self.turn_right(-req.turn)
            res.success = True
            return res
        except rospy.ROSInterruptException:
            res.success = False
            return res

    def callback_odom(self, data):
        self.x = data.pose.pose.position.x
        self.y = data.pose.pose.position.y
        self.yaw = self.get_yaw_from_quaternion(data.pose.pose.orientation)

    def go_straight(self, dis, velocity=0.3):
        vel = Twist()
        x0 = self.x
        y0 = self.y
        while(np.sqrt((self.x-x0)**2+(self.y-y0)**2)<dis):
            vel.linear.x = velocity
            vel.angular.z = 0.0
            self.cmd_vel_pub.publish(vel)
            rospy.sleep(0.1)
        self.stop()

    def turn_right(self, yaw, yawrate=-0.5):
        vel = Twist()
        yaw0 = self.yaw
        while(abs(self.yaw-yaw0)<np.deg2rad(yaw)):
            vel.linear.x = 0.0
            vel.angular.z = yawrate
            self.cmd_vel_pub.publish(vel)
            rospy.sleep(0.1)
        self.stop()

    def turn_left(self, yaw, yawrate=0.5):
        vel = Twist()
        yaw0 = self.yaw
        while(abs(self.yaw-yaw0)<np.deg2rad(yaw)):
            vel.linear.x = 0.0
            vel.angular.z = yawrate
            self.cmd_vel_pub.publish(vel)
            rospy.sleep(0.1)
        self.stop()

    def stop(self):
        vel = Twist()
        vel.linear.x = 0.0
        vel.angular.z = 0.0
        self.cmd_vel_pub.publish(vel)

    def get_yaw_from_quaternion(self, quaternion):
        e = tf.transformations.euler_from_quaternion(
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w))
        return e[2]

if __name__=='__main__':
    simple_controller = SimpleController()
    rospy.spin()

move_robot.pyはsimple_control2.pyを少し修正した程度です。 simple_control2.pyについては、chapter3で詳しく解説しています。

srvファイルの用意

また、このページを参考にして、 navigation_tutorialパッケージ内に navigation_tutorilal/srv/MoveTrigger.srvファイルを作成してください。

とくに、navigation_tutorial/CMakeLists.txtを編集する箇所を抜かさないように気をつけてください （対応する説明）。

MoveTrigger.srv

float32 straight
float32 turn
---
bool success

実行方法

ターミナルで以下のコマンドを実行した後、

$ rosrun navigation_tutorial move_robot.py

別のターミナルで

$ rosrun navigation_tutorial task_manager.py

を実行すると、ロボットが直進して左に回転します。

補足

ファイルがうまく実行できない場合、ファイルに実行権限があたえられていない可能性があります。 このページを参考にして解決してみてください。

それでも解決しない場合はslackで気軽に質問してください。