diff --git a/scripts/NeopixelDriver.py b/scripts/NeopixelDriver.py index 7a77ad7..e94e95d 100644 --- a/scripts/NeopixelDriver.py +++ b/scripts/NeopixelDriver.py @@ -18,15 +18,16 @@ class NeopixelDriverNode: def listen(self): rospy.init_node("light_ctrl", anonymous=True) - rospy.Subscriber("/neopixel", Neopixels, self.neopixel_ctrl_callback) + rospy.Subscriber("/neopixel", self._msg, self.neopixel_ctrl_callback) rate = rospy.spin() def neopixel_ctrl_callback(self, data): + rospy.loginfo(f"Recieved: {data}") cmds = "" for i, pixel in data.pixels: cmds += f"{i},{pixel.r},{pixel.g},{pixel.b};" cmds += "\n" - rospy.logdebug(f"Sending: {cmds}") + rospy.loginfo(f"Sending to ESP32: {cmds}") self.serial.write(cmds.encode("ascii")) diff --git a/scripts/light_command_node.py b/scripts/light_command_node.py new file mode 100644 index 0000000..65b7881 --- /dev/null +++ b/scripts/light_command_node.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 +import random +import rospy +from simple_example.msg import Neopixel, Neopixels + +COLORS = { + "red": Neopixel(r=255, b=0, g=0, w=0), + "blue": Neopixel(r=125, b=204, g=223, w=0), + "yellow": Neopixel(r=120, b=153, g=23, w=0), + "purple": Neopixel(r=255, b=0, g=153, w=0), +} + +last_color = Neopixel() + +msg = """ +Starting Light Control. + +This example is intended to work with a esp32 connected +with via usb uart. It is listening for stdin lines of +this format: + + ",,,;\n" +------------------------------------------------------------ +""" + + +def broadcaster(): + + pub = rospy.Publisher("/neopixel", Neopixels, queue_size=1) + rospy.init_node("light_control", anonymous=True, log_level=rospy.INFO) + + print(msg) + + while not rospy.is_shutdown(): + neo_pixel = pick_color() + pub.publish(Neopixel(pixels=[neo_pixel])) + rospy.sleep(2) + + +def pick_color(): + """Picks a random color from the color map""" + global last_color + + color = random.choice(list(COLORS.keys())) + while last_color == color: + color = random.choice(list(COLORS.keys())) + rospy.loginfo(f"Selected: {color}") + return COLORS[color] + + +if __name__ == "__main__": + try: + broadcaster() + except rospy.ROSInterruptException: + pass