change host name

demos/SimulationFramework/pick_and_place.py

@@ -3,7 +3,7 @@
 from alr_sim.sims.SimFactory import SimRepository
 from alr_sim.sims.universal_sim.PrimitiveObjects import Box
 from simpub.sim.sf_publisher import SFPublisher
-from simpub.xr_device.xr_device import XRDevice
+from simpub.xr_device import XRDevice
 
 if __name__ == "__main__":
 
@@ -72,7 +72,6 @@
         scene, host, no_tracked_objects=["table_plane", "table0"]
     )
     device = XRDevice()
-    # device = MetaQuest3("ALRMetaQuest3")
 
     duration = 2
 

demos/SimulationFramework/pick_and_place_with_meta_quest.py

@@ -7,7 +7,7 @@
 from alr_sim.sims.mj_beta import MjRobot
 
 from simpub.sim.sf_publisher import SFPublisher
-from simpub.xr_device.meta_quest3 import MetaQuest3
+from simpub.xr_device import MetaQuest3
 
 
 class MetaQuest3Controller(CartPosQuatImpedenceController):

simpub/core/__init__.py

@@ -0,0 +1,5 @@
+from .simpub_server import init_net_manager
+from .simpub_server import SimPublisher
+from .simpub_server import MsgServer
+from .log import logger
+

simpub/core/net_manager.py

@@ -168,7 +168,7 @@ def __init__(
         host_name: str = "SimPub"
     ) -> None:
         NetManager.manager = self
-        self._initialized = True
+        self._initialized = False
         self.zmq_context = zmq.asyncio.Context()
         # subscriber
         self.sub_socket_dict: Dict[IPAddress, zmq.Socket] = {}
@@ -206,9 +206,22 @@ def start_server_thread(self) -> None:
         while self.loop is None:
             time.sleep(0.01)
 
+    def start(self):
+        self._initialized = True
+
     def start_event_loop(self):
         self.loop = asyncio.new_event_loop()
         asyncio.set_event_loop(self.loop)
+        # wait for the start signal
+        while not self._initialized:
+            time.sleep(0.01)
+        # default service for client registration
+        self.register_service = Service(
+            "Register", self.register_client_callback, str
+        )
+        self.server_timestamp_service = Service(
+            "GetServerTimestamp", self.get_server_timestamp_callback, str
+        )
         # default task for client registration
         self.submit_task(self.broadcast_loop)
         self.submit_task(self.service_loop)
@@ -228,13 +241,6 @@ def join(self):
     async def service_loop(self):
         # try:
         logger.info("The service is running...")
-        # default service for client registration
-        self.register_service = Service(
-            "Register", self.register_client_callback, str
-        )
-        self.server_timestamp_service = Service(
-            "GetServerTimestamp", self.get_server_timestamp_callback, str
-        )
         while self.running:
             message = await self.service_socket.recv_string()
             if ":" not in message:
@@ -268,6 +274,7 @@ async def broadcast_loop(self):
         broadcast_bin = ip_bin | ~netmask_bin & 0xFFFFFFFF
         broadcast_ip = socket.inet_ntoa(struct.pack('!I', broadcast_bin))
         while self.running:
+            local_info = self.local_info  # update local info
             msg = f"SimPub:{_id}:{json.dumps(local_info)}"
             _socket.sendto(
                 msg.encode(), (broadcast_ip, ServerPort.DISCOVERY.value)
@@ -303,7 +310,7 @@ def shutdown(self):
         logger.info("Server has been shut down")
 
 
-def init_net_manager(host: str):
+def init_net_manager(host: str) -> NetManager:
     if NetManager.manager is not None:
         return NetManager.manager
     return NetManager(host)

simpub/core/simpub_server.py

@@ -13,13 +13,21 @@ class ServerBase(abc.ABC):
     def __init__(self, host: str = "127.0.0.1"):
         self.host: str = host
         self.net_manager = init_net_manager(host)
+        self.initialize()
+        self.net_manager.start()
 
     def join(self):
         self.net_manager.join()
 
+    @abc.abstractmethod
+    def initialize(self):
+        raise NotImplementedError
+
 
 class MsgServer(ServerBase):
-    pass
+
+    def initialize(self):
+        pass
 
 
 class SimPublisher(ServerBase):
@@ -41,6 +49,8 @@ def __init__(
         else:
             self.no_tracked_objects = no_tracked_objects
         super().__init__(host)
+
+    def initialize(self):
         self.scene_update_streamer = Streamer("SceneUpdate", self.get_update)
         self.scene_service = Service("Scene", self._on_scene_request, str)
         self.asset_service = Service("Asset", self._on_asset_request, bytes)

simpub/xr_device/__init__.py

@@ -0,0 +1,2 @@
+from .xr_device import XRDevice
+from .meta_quest3 import MetaQuest3

simpub/xr_device/meta_quest3.py

@@ -41,26 +41,82 @@ def __init__(
         )
         self.start_vib_pub = Publisher(f"{device_name}/StartVibration")
         self.stop_vib_pub = Publisher(f"{device_name}/StopVibration")
-        self.button_trigger_event: Dict[str, List[Callable]] = {
+        self.button_press_event: Dict[str, List[Callable]] = {
             "A": [],
             "B": [],
             "X": [],
             "Y": [],
         }
-        self.on_vibration = {"left": False, "right": False,}
+        self.left_trigger_press_event: Dict[str, List[Callable]] = {
+            "hand": [],
+            "trigger": [],
+        }
+        self.left_trigger_release_event: Dict[str, List[Callable]] = {
+            "hand": [],
+            "trigger": [],
+        }
+        self.right_trigger_press_event: Dict[str, List[Callable]] = {
+            "hand": [],
+            "trigger": [],
+        }
+        self.right_trigger_release_event: Dict[str, List[Callable]] = {
+            "hand": [],
+            "trigger": [],
+        }
+        self.on_vibration = {"left": False, "right": False}
 
     def update(self, data: str):
         self.last_input_data = self.input_data
         self.input_data = json.loads(data)
         if self.last_input_data is None:
             return
-        for button, callbacks in self.button_trigger_event.items():
+        for button, callbacks in self.button_press_event.items():
             if self.input_data[button] and not self.last_input_data[button]:
                 [callback() for callback in callbacks]
+        left_hand = self.input_data["left"]
+        last_left_hand = self.last_input_data["left"]
+        for trigger, callbacks in self.left_trigger_press_event.items():
+            if left_hand[trigger] and not last_left_hand["left"][trigger]:
+                [callback() for callback in callbacks]
+        for trigger, callbacks in self.left_trigger_release_event.items():
+            if not left_hand[trigger] and last_left_hand["left"][trigger]:
+                [callback() for callback in callbacks]
+        right_hand = self.input_data["right"]
+        last_right_hand = self.last_input_data["right"]
+        for trigger, callbacks in self.right_trigger_press_event.items():
+            if right_hand[trigger] and not last_right_hand["right"][trigger]:
+                [callback() for callback in callbacks]
+        for trigger, callbacks in self.right_trigger_release_event.items():
+            if not right_hand[trigger] and last_right_hand["right"][trigger]:
+                [callback() for callback in callbacks]
 
-    def register_button_trigger_event(self, button: str, callback: Callable):
+    def register_button_press_event(self, button: str, callback: Callable):
         # button should be one of A, B, X, Y
-        self.button_trigger_event[button].append(callback)
+        self.button_press_event[button].append(callback)
+
+    def register_trigger_press_event(
+        self, trigger: str, hand: str, callback: Callable
+    ):
+        # hand should be one of left or right
+        # trigger should be one of hand or trigger
+        if hand == "left":
+            self.left_trigger_press_event[trigger].append(callback)
+        elif hand == "right":
+            self.right_trigger_press_event[trigger].append(callback)
+        else:
+            raise ValueError("Invalid hand")
+
+    def register_trigger_release_event(
+        self, trigger: str, hand: str, callback: Callable
+    ):
+        # hand should be one of
+        # left_hand, left_trigger, right_hand, right_trigger
+        if hand == "left":
+            self.left_trigger_release_event[trigger].append(callback)
+        elif hand == "right":
+            self.right_trigger_release_event[trigger].append(callback)
+        else:
+            raise ValueError("Invalid hand")
 
     def get_input_data(self) -> MetaQuest3InputData:
         return self.input_data

simpub/xr_device/xr_device.py

@@ -18,7 +18,7 @@ class XRDevice:
 
     def __init__(
         self,
-        device_name: str = "UnityEditor",
+        device_name: str = "UnityClient",
     ) -> None:
         self.connected = False
         self.manager: NetManager = NetManager.manager
@@ -91,3 +91,13 @@ def print_log(self, log: str):
 
     def get_input_data(self) -> InputData:
         pass
+
+    def change_host_name(self, name: str):
+        if self.connected:
+            self.request("ChangeHostName", name)
+            self.manager.clients_info[name] = self.manager.clients_info.pop(
+                self.device
+            )
+            self.manager.clients_info[name]["name"] = name
+            self.client_info = self.manager.clients_info[name]
+            self.device = name

tools/change_name.py

@@ -0,0 +1,20 @@
+from simpub.core.simpub_server import init_net_manager
+from simpub.xr_device.xr_device import XRDevice
+from simpub.core.log import logger
+
+import time
+import argparse
+
+if __name__ == "__main__":
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--old", type=str, default="UnityClient")
+    parser.add_argument("--new", type=str, default="UnityClient")
+    args = parser.parse_args()
+
+    net_manager = init_net_manager("127.0.0.1")
+    xr_device = XRDevice(args.old)
+    while not xr_device.connected:
+        time.sleep(0.01)
+    result = xr_device.request("ChangeHostName", args.new)
+    logger.info(result)