10 week HM (1) service programming

Message, Service interface programming

- Test the created msg interface

[pub_node]

import rclpy
 
# Node class import. In Node class, there are create_publisher(), create_timer(), get_logger() and so on.
from rclpy.node import Node
from rclpy.node import QoSProfile
 
# msg type import
from std_msgs.msg import String
 
# Add custum msg interface
from my_interface_example.msg import Num
 
# Create class and inherit Node class
class HelloworldPublihser(Node):
 
    # Initializer(Constructor)
    def __init__(self):
 
        # calls the Node class's constructor and make node name
        super().__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 sec
        self.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 Topic
        self.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 += 1
 
def main(args=None):
 
    # rclpy initialize
    rclpy.init(args=args)
 
    # node : instance of HelloworldPublihser class
    node = HelloworldPublihser()
 
    # play the node. class. and its mean playing callback func.
    rclpy.spin(node)
 
    # close the node
    node.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()

 

[sub_node]

import rclpy
 
# import Node class
from rclpy.node import Node
from rclpy.node import QoSProfile
 
# import String msg tyme class
# from std_msgs.msg import String
from my_interface_example.msg import Num
 
# Create HelloworldSubscriber Class, and inherit Node class
class HelloworldSubscriber(Node):
 
    def __init__(self):
        # Initialize by constructor and give subscribe node name
        super().__init__('Helloworld_subscriber')
        qos_profile = QoSProfile(depth=10)
 
        # Define subscribe node
        self.Helloworld_subscriber = self.create_subscription(
            Num,
            'num_topic',
            self.subscribe_topic_message,
            qos_profile)
 
    # Define callback func.
    # msg is defined in helloworld_publisher.py
    def subscribe_topic_message(self, msg):
        # print on terminal
        self.get_logger().info('Received message: {0}'.format(msg.num))
 
 
def main(args=None):
 
    # Initailize rclpy
    rclpy.init(args=args)
 
    # Instance of HelloworldSubscriber class
    node = HelloworldSubscriber()
 
    # play the node(HelloworldSubscriber class), its mean playing callbackfunc.
    rclpy.spin(node)
 
    # close the node
    node.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()

[revise package.xml]

[ros run]

- Test the created srv interface

[server_node]

import rclpy
 
# import Node class
from rclpy.node import Node
 
# import AddTwoInts srv type class
# from example_interfaces.srv import AddTwoInts
from my_interface_example.srv import AddThreeInts
 
# Define class and inherit Node class to use Node class's func.s
class SumServiceAsync(Node):
 
    # Initializer(Constructor)
    def __init__(self):
 
        # calls the Node class's constructor and make node name
        super().__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 variable
        response.sum = request.a + request.b + request.c
        self.get_logger().info('Incoming request\na: %d b: %d c: %d' % (request.a, request.b, request.c))
 
        return response
 
 
def 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()

 

[client_node]

import sys
 
import rclpy
 
# import Node class
from rclpy.node import Node
 
# import srv type class
# from example_interfaces.srv import AddTwoInts
from my_interface_example.srv import AddThreeInts
 
# Define SumClientAsync class
class SumClientAsync(Node):
 
    def __init__(self):
        super().__init__('sum_client_async')
 
        # Define client
        self.cli = self.create_client(AddThreeInts, 'add_three_ints')
 
        # waiting for service getting ready
        while 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 class
        self.req = AddThreeInts.Request()
 
 
    def send_request(self):
 
        # get command line input arg for the request
        self.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 result
        self.future = self.cli.call_async(self.req)
 
def main(args=None):
    rclpy.init(args=args)
 
    # make instance of class
    sum_client = SumClientAsync()
    sum_client.send_request()
 
    while rclpy.ok():
 
        # excute the instance
        rclpy.spin_once(sum_client)
 
        # if srv request done
        if sum_client.future.done():
 
            try:
                # get result and assign to variable
                response = 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))
            break
 
    sum_client.destroy_node()
    rclpy.shutdown()
 
if __name__ == '__main__':
    main()

[revise package.xml]

[ros run]