LED Control on RPi4B using ROS2 Service

my_client.py : 

# !/usr/bin/env/ python3 from urllib import response import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile from my_interfaces.srv import SetLed class ClientLed(Node):     def __init__(self):         super().__init__('client_node_led')                 self.client_led = self.create_client(SetLed, 'service_led')         while not self.client_led.wait_for_service(timeout_sec=0.1):             self.get_logger().info('The service is not available, Waiting again...')         self.request =SetLed.Request()                   def send_request(self):                 self.request.data = 3.0         self.request.message = "led_on"         self.calls = self.client_led.call_async(self.request)                  def main(args=None):     rclpy.init(args=args)     client_service_led = ClientLed()     client_service_led.send_request()          while rclpy.ok():         rclpy.spin_once(client_service_led)         if client_service_led.calls.done():             try:                 response = client_service_led.calls.result()             except Exception as e:                 client_service_led.get_logger().info(                     'Service call failed %r' % (e,)                 )             break     client_service_led.destroy_node()     rclpy.shutdown() if __name__ == '__main__':     main()

 

my_service.py :

# !/usr/bin/env/ python3 import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile from my_interfaces.srv import SetLed import RPi.GPIO as GPIO import time # setup for RPi4B LED26 = 26 GPIO.setwarnings(False) GPIO.setmode(GPIO.BCM) GPIO.setup(LED26, GPIO.OUT) class ServiceLed(Node):     def __init__(self):         super().__init__('service_node_led')                     self.service_led = self.create_service(             SetLed, 'service_led', self.service_led_callback             )     def service_led_callback(self, request, response):         self.request_message = request.message         self.request_data = request.data                  if self.request_message == 'led_on':                         GPIO.output(LED26, GPIO.HIGH)             time.sleep(1.0)             GPIO.output(LED26, GPIO.LOW)             time.sleep(self.request_data)             response.success = True             clock_time_sub = self.get_clock().now().to_msg().sec             self.get_logger().info(f'{response.success} is responsed at {clock_time_sub}')                             return response              def main(args=None):     rclpy.init(args=args)     service_service_led = ServiceLed()     try:         rclpy.spin(service_service_led)     except KeyboardInterrupt:         service_service_led.get_logger().info('keyboard Interrupt')     finally:         service_service_led.destroy_node()         rclpy.shutdown()         GPIO.cleanup() if __name__ == '__main__':     main()

my_interfaces/srv/SetLed.srv :

# Request float32 data string message --- # Response bool success

setup.py : 

import os from glob import glob from setuptools import setup package_name = 'my_pkg' setup(     name=package_name,     version='0.0.0',     packages=[package_name],     data_files=[         ('share/ament_index/resource_index/packages',             ['resource/' + package_name]),         ('share/' + package_name, ['package.xml']),         (os.path.join('share', package_name), glob('launch/*.launch.py')),     ],     install_requires=['setuptools'],     zip_safe=True,     maintainer='ubuntu',     maintainer_email='ubuntu@todo.todo',     description='TODO: Package description',     license='TODO: License declaration',     tests_require=['pytest'],     entry_points={         'console_scripts': [             'led_pub_node = my_pkg.my_pub:main',             'led_sub_node = my_pkg.my_sub:main',             'client_node_led = my_pkg.my_client:main',             'service_node_led = my_pkg.my_service:main',         ],     }, )

package.xml :

<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3">   <name>my_pkg</name>   <version>0.0.0</version>   <description>TODO: Package description</description>   <maintainer email="ubuntu@todo.todo">ubuntu</maintainer>   <license>TODO: License declaration</license>   <exec_depend>ros2launch</exec_depend>   <exec_depend>my_interfaces</exec_depend>   <test_depend>ament_copyright</test_depend>   <test_depend>ament_flake8</test_depend>   <test_depend>ament_pep257</test_depend>   <test_depend>python3-pytest</test_depend>   <export>     <build_type>ament_python</build_type>   </export> </package>

colcon build --packages-select my_pkg

ros2 run my_pkg client_node_led

ros2 run my_pkg service_node_led

 

[Reference]

[1] https://docs.ros.org/en/foxy/Tutorials/Writing-A-Simple-Py-Service-And-Client.html