Feat/add in experiment

src/external_signage/src/external_signage/external_signage_core.py

@@ -2,9 +2,14 @@
 import datetime
 import time
 import serial
+import json
+import os
+import rclpy
 from std_srvs.srv import SetBool
+from std_msgs.msg import Bool
 from ament_index_python.packages import get_package_share_directory
 import external_signage.packet_tools as packet_tools
+import uuid
 
 
 @dataclass
@@ -59,9 +64,13 @@ def __init__(self, bus, parser, protocol, node_logger):
         self._logger = node_logger
         self._delay_time = 0.02
 
+    def randomname(self):
+        u = uuid.uuid4()
+        return u.bytes
+
     def _send_heartbeat(self, data, ACK_QueryACK):
         timestamp = datetime.datetime.now()
-        name_time_packet = packet_tools.gen_name_time_packet(data.linename, timestamp, False)
+        name_time_packet = packet_tools.gen_name_time_packet(self.randomname(), timestamp, False)
         self._bus.write(name_time_packet)
         packet_tools.dump_packet(name_time_packet, None, self._protocol.SEND_COLOR)
         time.sleep(self._delay_time)
@@ -95,7 +104,7 @@ class ExternalSignage:
     def __init__(self, node):
         self.node = node
         self.protocol = Protocol()
-        package_path = get_package_share_directory("signage") + "/resource/td5_file/"
+        package_path = get_package_share_directory("external_signage") + "/resource/td5_file/"
         try:
             self.bus = serial.Serial(
                 "/dev/ttyUSB0",
@@ -106,23 +115,57 @@ def __init__(self, node):
             )
             self.parser = packet_tools.Parser(self.bus)
             self._external_signage_available = True
-        except:
+        except Exception as e:
+            self.node.get_logger().error(str(e))
             self._external_signage_available = False
 
         self.displays = {
             "front": self._load_display_data(self.protocol.front, package_path),
             "back": self._load_display_data(self.protocol.back, package_path),
             "side": self._load_display_data(self.protocol.side, package_path),
         }
+
+        api_qos = rclpy.qos.QoSProfile(
+            history=rclpy.qos.QoSHistoryPolicy.KEEP_LAST,
+            depth=10,
+            reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE,
+            durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL,
+        )
+
         node.create_service(SetBool, "/signage/trigger_external", self.trigger_external_signage)
+        node.create_service(SetBool, "/signage/mode_change", self.experiment_set)
+        self.mode_status_pub_ = node.create_publisher(Bool, "/signage/mode_status", api_qos)
+
+        self._settings_file = "/home/" + os.environ.get("USER") + "/settings.json"
+        if os.path.exists(self._settings_file):
+            with open(self._settings_file, "r") as f:
+                self._settings = json.load(f)
+        else:
+            self._settings = {"in_experiment": False}
+            with open(self._settings_file, "w") as f:
+                json.dump(self._settings, f, indent=4)
+
+        if self._settings.get("in_experiment", False):
+            self.pub_mode_status(True)
+            self.display_signage("experiment")
+        else:
+            self.pub_mode_status(False)
+
+    def pub_mode_status(self, status):
+        msg = Bool()
+        msg.data = status
 
     def _load_display_data(self, display, package_path):
-        auto_path = package_path + f"/automatic_{display.width}x{display.height}.td5"
-        null_path = package_path + f"/null_{display.width}x{display.height}.td5"
+        auto_path = package_path + f"automatic_{display.width}x{display.height}.td5"
+        experiment_path = package_path + f"experiment_{display.width}x{display.height}.td5"
+        null_path = package_path + f"null_{display.width}x{display.height}.td5"
         return {
             "auto": packet_tools.TD5Data(
                 auto_path, display.address1, display.address2, display.height, display.width
             ),
+            "experiment": packet_tools.TD5Data(
+                experiment_path, display.address1, display.address2, display.height, display.width
+            ),
             "null": packet_tools.TD5Data(
                 null_path, display.address1, display.address2, display.height, display.width
             ),
@@ -137,14 +180,41 @@ def send_data(self, display_key, data_key):
         sender.send(data, ack_query_ack, ack_data_chunk)
 
     def trigger_external_signage(self, request, response):
-        if not self._external_signage_available:
+        if self._settings["in_experiment"]:
             return response
 
-        if request.data:
-            for display_key in self.displays:
-                self.send_data(display_key, "auto")
-                time.sleep(1)
-        else:
-            for display_key in self.displays:
-                self.send_data(display_key, "null")
+        try:
+            if request.data:
+                self.display_signage("auto")
+            else:
+                self.display_signage("null")
+            response.success = True
+        except Exception as e:
+            self.node.get_logger().error(str(e))
+        return response
+
+    def experiment_set(self, request, response):
+        try:
+            if request.data:
+                self.pub_mode_status(True)
+                self._settings["in_experiment"] = True
+                self.display_signage("experiment")
+            else:
+                self.pub_mode_status(False)
+                self._settings["in_experiment"] = False
+                self.display_signage("null")
+
+            with open(self._settings_file, "w") as f:
+                json.dump(self._settings, f, indent=4)
+            response.success = True
+        except Exception as e:
+            self.node.get_logger().error(str(e))
         return response
+
+    def display_signage(self, display_file):
+        if not self._external_signage_available:
+            return
+
+        for display_key in self.displays:
+            self.send_data(display_key, display_file)
+            time.sleep(1)