led_dev add thread

led_dev/led_dev/led_dev_node.py

@@ -3,8 +3,9 @@ from rclpy.node import Node
 from std_msgs.msg import String
 import serial
 import time
+import threading
 class TM512AC_RS485_Driver:
-    def __init__(self, port='/dev/ttyUSB0', baudrate=250000, num_ic=5):
+    def __init__(self, port, baudrate=250000, num_ic=5):
         self.ser = serial.Serial(
             port=port,
             baudrate=baudrate,
@@ -41,18 +42,30 @@ class LedCommand(Node):
         super().__init__('led_dev_node')
         self.create_subscription(String,'/led_cmd',self.led_cmd_cb,10)
         self.driver = TM512AC_RS485_Driver(port='/dev/ttyUSB0', num_ic=5)
-        self.get_logger().info("led_dev_node start....")
+        self.cur_color='green'
+        self.cur_val=5
+        self.get_logger().info("led_dev_node will start....")
+        self.th1=threading.Thread(target=self.com_task)
+        self.th1.start()
+        
+    def com_task(self):
+        idx=0
+        while True:
+            self.led_blink(self.cur_color,self.cur_val)
+    def wait_task(self):
+        self.th1.join()
     def all_color(self,rgb_data,xnum):
-        num_ic=5
-        rgb = [0, 0, 0] * num_ic
+        num_ic=4
+        rgb = [0, 0, 0,0] * num_ic
         start_id=0
         end_id=256
         if xnum<0:
             start_id=256
             end_id=0
+        val=0
         for i in range(start_id,end_id,xnum):
-            offset=i*5
-            val=i#val+offset
+            #offset=i*5
+            val=i##offset
             if val > 255:
                 val=255
             if rgb_data=='red':
@@ -69,9 +82,9 @@ class LedCommand(Node):
                 rgb = [val, 0, val,0] * num_ic
             elif rgb_data=='white':#紫色
                 rgb = [val, val, val,0] * num_ic
-            #print(rgb)
+            print(rgb)
             self.driver.send_frame(rgb)
-            time.sleep(0.1)
+            time.sleep(0.02)
     def led_blink(self,color,val):
         self.all_color(color, -val)
         self.all_color(color, val)
@@ -79,10 +92,18 @@ class LedCommand(Node):
         xcmd=msg.data
         print('led cmd:',xcmd)
         cmd,val=xcmd.split(',')
-        self.led_blink(cmd,int(val))
-if __name__=='__main__':
+        self.cur_color=cmd
+        self.cur_val=int(val)
+        ###self.led_blink(cmd,int(val))
+def main():
     rclpy.init()
     node=LedCommand()
     rclpy.spin(node)
+    node.wait_task()
     node.destroy_node()
     rclpy.shutdown()
+
+if __name__=='__main__':
+    main()
+
+

led_dev/setup.py

@@ -20,6 +20,7 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+            'led_dev_node=led_dev.led_dev_node:main',
         ],
     },
 )