| 
						
						
							
								
							
						
						
					 | 
				
			
			 | 
			 | 
			
				@ -11,25 +11,21 @@ class NeopixelDriverNode:
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				    """ROS Node for controllering NeoPixels."""
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				    def __init__(self):
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        self._msg = Neopixels()
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        self._msg = Neopixels
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        self.pixel_count = rospy.get_param('/simple_example/pixel_count', 1)
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        self.port = rospy.get_param('/simple_example/port', "/dev/ttyUSB0")
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        self.serial = Serial(self.port, 115200, timeout=1)
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				    def listen(self):
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				<<<<<<< HEAD
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        rospy.init_node("light_ctrl", anonymous=True)
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        rospy.Subscriber("/neopixel", self._msg, self.neopixel_ctrl_callback)
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				=======
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        rospy.init_node("light_ctrl", anonymous=True, log_level=rospy.INFO)
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        rospy.Subscriber("/neopixel", Neopixels, self.neopixel_ctrl_callback)
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				>>>>>>> 37b2adbedcf87e091d5ebd6efde78e86c64171d8
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        rate = rospy.spin()
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				    def neopixel_ctrl_callback(self, data):
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        rospy.loginfo(f"Recieved: {data}")
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        #import pdb;pdb.set_trace()
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        cmds = ""
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        for i, pixel in data.pixels:
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        for i, pixel in enumerate(data.pixels):
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				            cmds += f"{i},{pixel.r},{pixel.g},{pixel.b};"
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        cmds += "\n"
 | 
			
		
		
	
		
			
				 | 
				 | 
			
			 | 
			 | 
			
				        rospy.loginfo(f"Sending to ESP32: {cmds}")
 | 
			
		
		
	
	
		
			
				
					| 
						
							
								
							
						
						
						
					 | 
				
			
			 | 
			 | 
			
				
 
 |