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.
38 lines
1.1 KiB
Python
38 lines
1.1 KiB
Python
3 years ago
|
#!/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", Neopixels, self.neopixel_ctrl_callback)
|
||
|
|
rate = rospy.spin()
|
||
|
|
|
||
|
|
def neopixel_ctrl_callback(self, data):
|
||
|
|
cmds = ""
|
||
|
|
for i, pixel in data.pixels:
|
||
|
|
cmds += f"{i},{pixel.r},{pixel.g},{pixel.b};"
|
||
|
|
cmds += "\n"
|
||
|
|
rospy.logdebug(f"Sending: {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.")
|