diff --git a/content/course/appendix/service_example.md b/content/course/appendix/service_example.md new file mode 100644 index 0000000..89bf4a6 --- /dev/null +++ b/content/course/appendix/service_example.md @@ -0,0 +1,198 @@ +--- +title: サービス通信の例(仮) +date: '2023-08-27' +type: book +weight: 2 +--- +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)