Add contol node for the LEDs

webui/backend/ros_node.py

@@ -4,6 +4,7 @@ import rclpy
 from geometry_msgs.msg import Twist
 from rclpy.node import Node
 from sensor_msgs.msg import JointState, Range
+from std_msgs.msg import ColorRGBA, String
 
 
 class RobotBridgeNode(Node):
@@ -12,6 +13,8 @@ class RobotBridgeNode(Node):
 
         self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
         self._joint_cmd_pub = self.create_publisher(JointState, '/joint_command', 10)
+        self._led_color_pub = self.create_publisher(ColorRGBA, '/led/color', 10)
+        self._led_effect_pub = self.create_publisher(String, '/led/effect', 10)
 
         self.create_subscription(JointState, '/joint_states', self._on_joint_states, 10)
         self.create_subscription(Range, '/ultrasonic/range', self._on_ultrasonic, 10)
@@ -37,6 +40,19 @@ class RobotBridgeNode(Node):
         msg.position = [float(pan), float(tilt)]
         self._joint_cmd_pub.publish(msg)
 
+    def publish_led_color(self, r: float, g: float, b: float, a: float) -> None:
+        msg = ColorRGBA()
+        msg.r = float(r)
+        msg.g = float(g)
+        msg.b = float(b)
+        msg.a = float(a)
+        self._led_color_pub.publish(msg)
+
+    def publish_led_effect(self, effect: str) -> None:
+        msg = String()
+        msg.data = effect
+        self._led_effect_pub.publish(msg)
+
     def _on_joint_states(self, msg: JointState) -> None:
         data = {'type': 'joint_states', 'positions': dict(zip(msg.name, msg.position))}
         for cb in self.on_joint_states_callbacks: