-
10 week HM (1) service programmingROS2 Study 2021. 11. 9. 11:46728x90반응형
Message, Service interface programming
- Test the created msg interface
[pub_node]
1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677import rclpy# Node class import. In Node class, there are create_publisher(), create_timer(), get_logger() and so on.from rclpy.node import Nodefrom rclpy.node import QoSProfile# msg type importfrom std_msgs.msg import String# Add custum msg interfacefrom my_interface_example.msg import Num# Create class and inherit Node classclass HelloworldPublihser(Node):# Initializer(Constructor)def __init__(self):# calls the Node class's constructor and make node namesuper().__init__('helloworld_publisher')qos_profile = QoSProfile(depth=10)# make publisher# self.helloworld_publisher = self.create_publisher(String, 'helloworld', qos_profile)self.helloworld_publisher = self.create_publisher(Num, 'num_topic', qos_profile)# call callback func. every 1 secself.timer = self.create_timer(1, self.publish_helloworld_msg)self.count = 0# Define callback func.def publish_helloworld_msg(self):# call String class and asign to msg variable# msg = String()msg = Num()# Define msg data using data property which is from String class# msg.data = 'Helloworld: {0}'.format(self.count)msg.num = self.count# Actually publish : push on Topicself.helloworld_publisher.publish(msg)# Print on terminal# self.get_logger().info('Published message: {0}'.format(msg.data))self.get_logger().info('Published message: {0}'.format(msg.num))self.count += 1def main(args=None):# rclpy initializerclpy.init(args=args)# node : instance of HelloworldPublihser classnode = HelloworldPublihser()# play the node. class. and its mean playing callback func.rclpy.spin(node)# close the nodenode.destroy_node()rclpy.shutdown()'''try:rclpy.spin(node)except KeyboardInterrupt:node.get_logger().info('Keyboard Interupt (SIGINT)')finally:node.destroy_node()rclpy.shutdown()'''if __name__ == '__main__':main()cs [sub_node]
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960import rclpy# import Node classfrom rclpy.node import Nodefrom rclpy.node import QoSProfile# import String msg tyme class# from std_msgs.msg import Stringfrom my_interface_example.msg import Num# Create HelloworldSubscriber Class, and inherit Node classclass HelloworldSubscriber(Node):def __init__(self):# Initialize by constructor and give subscribe node namesuper().__init__('Helloworld_subscriber')qos_profile = QoSProfile(depth=10)# Define subscribe nodeself.Helloworld_subscriber = self.create_subscription(Num,'num_topic',self.subscribe_topic_message,qos_profile)# Define callback func.# msg is defined in helloworld_publisher.pydef subscribe_topic_message(self, msg):# print on terminalself.get_logger().info('Received message: {0}'.format(msg.num))def main(args=None):# Initailize rclpyrclpy.init(args=args)# Instance of HelloworldSubscriber classnode = HelloworldSubscriber()# play the node(HelloworldSubscriber class), its mean playing callbackfunc.rclpy.spin(node)# close the nodenode.destroy_node()rclpy.shutdown()'''try:rclpy.spin(node)except KeyboardInterrupt:node.get_logger().info('Keyboard Interrupt (SIGINT)')finally:node.destroy_node()rclpy.shutdown()'''if __name__ == '__main__':main()cs [revise package.xml]
[ros run]
- Test the created srv interface
[server_node]
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748import rclpy# import Node classfrom rclpy.node import Node# import AddTwoInts srv type class# from example_interfaces.srv import AddTwoIntsfrom my_interface_example.srv import AddThreeInts# Define class and inherit Node class to use Node class's func.sclass SumServiceAsync(Node):# Initializer(Constructor)def __init__(self):# calls the Node class's constructor and make node namesuper().__init__('sum_service')# Define srv.self.srv = self.create_service(AddThreeInts, 'add_three_ints', self.add_three_ints_callback)# Define callback func.def add_three_ints_callback(self, request, response):# Define variableresponse.sum = request.a + request.b + request.cself.get_logger().info('Incoming request\na: %d b: %d c: %d' % (request.a, request.b, request.c))return responsedef main(args=None):rclpy.init(args=args)sum_service = SumServiceAsync()try:rclpy.spin(sum_service)except KeyboardInterrupt:sum_service.get_logger().info('This node is finished by keboard interrupt')finally:sum_service.destroy_node()rclpy.shutdown()if __name__ == '__main__':main()cs [client_node]
12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970import sysimport rclpy# import Node classfrom rclpy.node import Node# import srv type class# from example_interfaces.srv import AddTwoIntsfrom my_interface_example.srv import AddThreeInts# Define SumClientAsync classclass SumClientAsync(Node):def __init__(self):super().__init__('sum_client_async')# Define clientself.cli = self.create_client(AddThreeInts, 'add_three_ints')# waiting for service getting readywhile not self.cli.wait_for_service(timeout_sec=1.0):self.get_logger().info('service not available, waiting again...')# req variable declare : Request func. of AddTwoInts classself.req = AddThreeInts.Request()def send_request(self):# get command line input arg for the requestself.req.a = int(sys.argv[1])self.req.b = int(sys.argv[2])self.req.c = int(sys.argv[3])# make a srv request and asynchronously get the resultself.future = self.cli.call_async(self.req)def main(args=None):rclpy.init(args=args)# make instance of classsum_client = SumClientAsync()sum_client.send_request()while rclpy.ok():# excute the instancerclpy.spin_once(sum_client)# if srv request doneif sum_client.future.done():try:# get result and assign to variableresponse = sum_client.future.result()except Exception as e:sum_client.get_logger().info('Service call failed %r' %(e,))else:sum_client.get_logger().info('Result of add_three_ints: for %d + %d + %d = %d' % (sum_client.req.a, sum_client.req.b, sum_client.req.c, response.sum))breaksum_client.destroy_node()rclpy.shutdown()if __name__ == '__main__':main()cs [revise package.xml]
[ros run]
728x90반응형'ROS2 Study' 카테고리의 다른 글
10 week HW (3) action programming : action server node (0) 2021.11.09 10 week HM (2) : create action interface (0) 2021.11.09 10 week HM (1) service programming (0) 2021.11.09 8week ROS homework : Simple node code explain (0) 2021.10.25 ROS homework week7 (0) 2021.10.18 6 week HW Parameter control using turtlesim node (0) 2021.10.12