You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
40 lines
1.2 KiB
Python
40 lines
1.2 KiB
Python
#!/usr/bin/env python3
|
|
import rospy
|
|
|
|
from serial import Serial
|
|
|
|
from simple_example.msg import Neopixel, Neopixels
|
|
from simple_example.srv import FillNeopixels, FillNeopixelsResponse
|
|
|
|
|
|
class NeopixelDriverNode:
|
|
"""ROS Node for controllering NeoPixels."""
|
|
|
|
def __init__(self):
|
|
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):
|
|
rospy.init_node("light_ctrl", anonymous=True)
|
|
rospy.Subscriber("/neopixel", self._msg, self.neopixel_ctrl_callback)
|
|
rate = rospy.spin()
|
|
|
|
def neopixel_ctrl_callback(self, data):
|
|
rospy.loginfo(f"Recieved: {data}")
|
|
#import pdb;pdb.set_trace()
|
|
cmds = ""
|
|
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}")
|
|
self.serial.write(cmds.encode("ascii"))
|
|
|
|
|
|
if __name__ == "__main__":
|
|
rospy.loginfo("Firing up neopixel driver.")
|
|
node = NeopixelDriverNode()
|
|
node.listen()
|
|
rospy.loginfo("Neopixel driver is shutting down.")
|