add robot options, update led

rae_sdk/rae_sdk/robot/api/ros/ros_interface.py

@@ -29,6 +29,8 @@
 from rclpy.subscription import Subscription
 from rclpy.publisher import Publisher
 from rclpy.executors import Executor, MultiThreadedExecutor, SingleThreadedExecutor
+from ...robot_options import RobotOptions
+
 
 QOS_PROFILE = 10
 
@@ -80,21 +82,19 @@ class ROSInterface:
 
     """
 
-    def __init__(self, name: str, namespace='', launch_mock=False, start_hardware=True) -> None:
+    def __init__(self, robot_options: RobotOptions=RobotOptions()):
         """
         Initialize the ROS2Manager instance.
 
         Args:
         ----
-            name (str): The name of the ROS2 node.
-            namespace (str): The namespace of the ROS2 node.
-            launch_mock (bool): Whether to launch the mock hardware or not.
+            robot_options (RobotOptions): An object containing the robot's options.
 
         """
-        self._namespace = namespace
-        self._name = name
-        self._launch_mock = launch_mock
-        self._start_hardware = start_hardware
+        self._namespace = robot_options.namespace
+        self._name = robot_options.name
+        self._launch_mock = robot_options.launch_mock
+        self._start_hardware = robot_options.start_hardware
         self._launch_service = None
         self._stop_event = None
         self._process = None
@@ -146,7 +146,7 @@ def start(self) -> None:
             start_hardware (bool): Whether to start the hardware process or not.
 
         """
-        if self._start_hardware:
+        if self._start_hardware or self._launch_mock:
             self.start_hardware_process()
         self._context = rclpy.Context()
         self._context.init()

rae_sdk/rae_sdk/robot/led.py

@@ -1,5 +1,5 @@
 import logging as log
-from rae_msgs.msg import LEDControl
+from rae_msgs.msg import LEDControl, ColorPeriod
 from std_msgs.msg import ColorRGBA
 
 
@@ -20,28 +20,79 @@ class LEDController:
     def __init__(self, ros_interface):
         self._ros_interface = ros_interface
         self._ros_interface.create_publisher("/leds", LEDControl)
+        self._effect_types_map = {
+            "all" : LEDControl.CTRL_TYPE_ALL,
+            "single" : LEDControl.CTRL_TYPE_SINGLE,
+            "spinner" : LEDControl.CTRL_TYPE_SPINNER,
+            "fan" : LEDControl.CTRL_TYPE_FAN,
+            "custom" : LEDControl.CTRL_TYPE_CUSTOM,
+        }
         log.info("LED Controller ready")
 
-    def set_leds(self, payload):
-        def hex_to_rgb(hex):
-            value = hex.lstrip('#')
-            lv = len(value)
-            return tuple(int(value[i:i + lv // 3], 16) for i in range(0, lv, lv // 3))
-
-        def normalize(num):
-            if num == 0:
-                return float(num)
-            else:
-                return float(num)/255.0
+    def hex_to_rgb(self, hex):
+        """Convert a hex color to an RGB tuple."""
+        value = hex.lstrip('#')
+        lv = len(value)
+        return tuple(int(value[i:i + lv // 3], 16) for i in range(0, lv, lv // 3))
+
+    def normalize(self, num):
+        """Normalize a number to a float between 0 and 1."""
+        if num == 0:
+            return float(num)
+        else:
+            return float(num)/255.0
+
+    def set_leds_from_payload(self, payload):
+        """
+        Set the robot's LEDs to a given color.
+        
+        Args:
+        ----
+            payload (dict): A dictionary containing the color to set the LEDs to. 
+            Example payload struct: {'brightness': 50, 'color': '#FFFFFF', 'effect': 'pulse', 'interval': 5}
+            
+        """
         led_msg = LEDControl()
 
-        r, g, b = hex_to_rgb(payload['color'])
+        r, g, b = self.hex_to_rgb(payload['color'])
 
-        color_msg = ColorRGBA()
-        color_msg.a = 1.0
-        color_msg.r = normalize(r)
-        color_msg.g = normalize(g)
-        color_msg.b = normalize(b)
+        color_msg = ColorPeriod()
+        color_msg.period = payload['interval']
+        color_msg.color.a = 1.0
+        color_msg.color.r = self.normalize(r)
+        color_msg.color.g = self.normalize(g)
+        color_msg.color.b = self.normalize(b)
         led_msg.data = [color_msg]
         led_msg.control_type = led_msg.CTRL_TYPE_ALL
         self._ros_interface.publish("/leds", led_msg)
+
+    def set_leds(self, color: str, brightness: int = 100, effect: str = "solid", interval: int = 5):
+        """
+        Set the robot's LEDs to a given color.
+
+        Args:
+        ----
+            color (str): The color to set the LEDs to.
+            brightness (int): The brightness of the LEDs. (Default: 100)
+            effect (str): The effect to apply to the LEDs. (Default: "solid")
+            interval (int): The interval of the effect. (Default: 5)
+            
+        """
+        payload = {
+            'brightness': brightness,
+            'color': color,
+            'effect': effect,
+            'interval': interval
+        }
+        self.set_leds_from_payload(payload)
+
+    def set_leds_from_msg(self, msg: LEDControl):
+        """
+        Set the robot's LEDs to a given color.
+
+        Args:
+        ----
+            msg (LEDControl): The message containing the color to set the LEDs to.
+            
+        """
+        self._ros_interface.publish("/leds", msg)

rae_sdk/rae_sdk/robot/perception/perception_system.py

@@ -322,7 +322,6 @@ def setup_sai_slam(self):
                                  'spectacularAI::ros2::Node', self.opts)
 
     def setup_rtabmap(self):
-        self.set_executor_type("multi_threaded")
 
         self.start_pipeline(rtabmap_pipeline())
 
@@ -334,7 +333,6 @@ def setup_rtabmap(self):
                                 "rae_right_camera_optical_frame", dai.CameraBoardSocket.CAM_C, 640, 400)
 
         self.add_queue("imu", self.publish_ros)
-        self.add_queue("left", self.publish_ros)
         self.add_queue("right", self.publish_ros)
         self.add_queue("stereo", self.publish_ros)
         self.scan_front_opts = dai_ros.ROSNodeOptions("laserscan_kinect_front", self._namespace, self._config_path,

rae_sdk/rae_sdk/robot/robot.py

@@ -5,50 +5,9 @@
 from .led import LEDController
 from .movement import MovementController
 from .audio import AudioController
-from sensor_msgs.msg import BatteryState
 from .perception.perception_system import PerceptionSystem
-
-
-class RobotOptions:
-    """
-    A class for storing the robot's options.
-
-    Attributes
-    ----------
-        start_hardware (bool): Whether to start the robot's hardware.
-        launch_mock (bool): Whether to launch the robot's mock.
-        name (str): The robot's name.
-        namespace (str): The robot's namespace.
-        launch_controllers (bool): Whether to launch the robot's controllers.
-
-        """
-
-    def __init__(self, name='rae_api', namespace='', launch_controllers=True, start_hardware=True, launch_mock=False):
-        self._start_hardware = start_hardware
-        self._launch_mock = launch_mock
-        self._name = name
-        self._namespace = namespace
-        self._launch_controllers = launch_controllers
-
-    @property
-    def start_hardware(self):
-        return self._start_hardware
-
-    @property
-    def launch_mock(self):
-        return self._launch_mock
-
-    @property
-    def name(self):
-        return self._name
-
-    @property
-    def namespace(self):
-        return self._namespace
-
-    @property
-    def launch_controllers(self):
-        return self._launch_controllers
+from .robot_options import RobotOptions
+from sensor_msgs.msg import BatteryState
 
 
 class Robot:
@@ -82,11 +41,8 @@ def __init__(self, robot_options: RobotOptions = RobotOptions()):
             robot_options (RobotOptions): An object containing the robot's options.
 
         """
-        self._name = robot_options.name
-        self._namespace = robot_options.namespace
-        self._launch_mock = robot_options.launch_mock
-        self._ros_interface = ROSInterface(
-            self._name, self._namespace, self._launch_mock, robot_options.launch_controllers)
+        self._robot_options = robot_options
+        self._ros_interface = ROSInterface(robot_options)
         self._ros_interface.start()
         if robot_options.launch_controllers:
             self._led_controller = LEDController(self._ros_interface)
@@ -95,7 +51,7 @@ def __init__(self, robot_options: RobotOptions = RobotOptions()):
             self._audio_controller = AudioController(self._ros_interface)
             self._ros_interface.create_subscriber(
                 "/battery_status", BatteryState, self.battery_state_cb)
-        self._perception_system = PerceptionSystem(self._namespace)
+        self._perception_system = PerceptionSystem(self._robot_options.namespace)
 
         log.info('Robot ready')
 

rae_sdk/rae_sdk/robot/robot_options.py

@@ -0,0 +1,40 @@
+class RobotOptions:
+    """
+    A class for storing the robot's options.
+
+    Attributes
+    ----------
+        start_hardware (bool): Whether to start the robot's hardware.
+        launch_mock (bool): Whether to launch the robot's mock interfaces if start_hardware=True.
+        name (str): The robot's name.
+        namespace (str): The robot's namespace.
+        launch_controllers (bool): Whether to launch the robot's controllers.
+
+    """
+
+    def __init__(self, name='rae_api', namespace='', launch_controllers=True, start_hardware=True, launch_mock=False):
+        self._start_hardware = start_hardware
+        self._launch_mock = launch_mock
+        self._name = name
+        self._namespace = namespace
+        self._launch_controllers = launch_controllers
+
+    @property
+    def start_hardware(self):
+        return self._start_hardware
+
+    @property
+    def launch_mock(self):
+        return self._launch_mock
+
+    @property
+    def name(self):
+        return self._name
+
+    @property
+    def namespace(self):
+        return self._namespace
+
+    @property
+    def launch_controllers(self):
+        return self._launch_controllers