Refactor code structure for improved readability and maintainability

unity/XR_RM_PICO_UDP_Sender/Assets/Editor/XrRmUdpSenderProjectSetup.cs

@@ -80,7 +80,7 @@ public static class XrRmUdpSenderProjectSetup
         string targetHost = ResolveTargetHost();
         component.host = targetHost;
         component.port = 15000;
-        component.sendHz = 60.0f;
+        component.sendHz = 90.0f;
         component.convertUnityToProjectCoordinates = true;
         component.sendOnStart = true;
 

unity/XR_RM_PICO_UDP_Sender/Assets/Scenes/Main.unity

@@ -168,9 +168,9 @@ MonoBehaviour:
   m_Script: {fileID: 11500000, guid: e4c7a11d85854eddb40a6a614bfb129c, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
-  host: 192.168.9.99
+  host: 192.168.9.92
   port: 15000
-  sendHz: 60
+  sendHz: 90
   convertUnityToProjectCoordinates: 1
   sendOnStart: 1
 --- !u!114 &100003

unity/XR_RM_PICO_UDP_Sender/Assets/Scripts/PicoControllerUdpSender.cs

@@ -12,13 +12,14 @@ public class PicoControllerUdpSender : MonoBehaviour
     private const string PortPrefsKey = "xr_rm_udp_sender.port";
     private const string SendHzPrefsKey = "xr_rm_udp_sender.send_hz";
     private const string ConvertPrefsKey = "xr_rm_udp_sender.convert_coordinates";
+    private const int MaxPacketsPerFrame = 4;
 
     [Header("ROS UDP Target")]
     public string host = "192.168.9.99";
     public int port = 15000;
 
     [Header("Send")]
-    public float sendHz = 60.0f;
+    public float sendHz = 90.0f;
     public bool convertUnityToProjectCoordinates = true;
     public bool sendOnStart = true;
 
@@ -106,17 +107,24 @@ public class PicoControllerUdpSender : MonoBehaviour
             return;
         }
 
-        if (Time.unscaledTime < nextSendTime)
-        {
-            return;
-        }
-
-        nextSendTime = Time.unscaledTime + 1.0f / Mathf.Max(sendHz, 1.0f);
-        packet.t = Time.realtimeSinceStartupAsDouble;
-
         FillController(XRNode.LeftHand, packet.controllers.left);
         FillController(XRNode.RightHand, packet.controllers.right);
+
+        float now = Time.unscaledTime;
+        float interval = 1.0f / Mathf.Max(sendHz, 1.0f);
+        if (nextSendTime <= 0.0f || now - nextSendTime > interval * MaxPacketsPerFrame)
+        {
+            nextSendTime = now;
+        }
+
+        int sentThisFrame = 0;
+        while (now >= nextSendTime && sentThisFrame < MaxPacketsPerFrame)
+        {
+            packet.t = Time.realtimeSinceStartupAsDouble;
             SendPacket();
+            nextSendTime += interval;
+            sentThisFrame++;
+        }
     }
 
     public bool ApplyConfig(string newHost, int newPort, float newSendHz, bool newConvertUnityToProjectCoordinates, bool save)

unity/XR_RM_PICO_UDP_Sender/Assets/Scripts/PicoUdpConfigPanel.cs

@@ -3,6 +3,11 @@ using System.Globalization;
 using UnityEngine;
 using UnityEngine.UI;
 using UnityEngine.XR;
+#if PICO_OPENXR_SDK
+using Unity.XR.OpenXR.Features.PICOSupport;
+#else
+using Unity.XR.PXR;
+#endif
 
 public class PicoUdpConfigPanel : MonoBehaviour
 {
@@ -90,6 +95,29 @@ public class PicoUdpConfigPanel : MonoBehaviour
         QualitySettings.vSyncCount = 0;
         Screen.sleepTimeout = SleepTimeout.NeverSleep;
         XRSettings.eyeTextureResolutionScale = 1.0f;
+        RequestPicoDisplayRefreshRate();
+    }
+
+    private void RequestPicoDisplayRefreshRate()
+    {
+#if UNITY_ANDROID && !UNITY_EDITOR
+        try
+        {
+#if PICO_OPENXR_SDK
+            bool applied = DisplayRefreshRateFeature.SetDisplayRefreshRate((float)targetFrameRate);
+            if (!applied)
+            {
+                Debug.LogWarning($"XR-RM failed to request PICO display refresh rate: {targetFrameRate} Hz");
+            }
+#else
+            PXR_System.SetSystemDisplayFrequency((float)targetFrameRate);
+#endif
+        }
+        catch (Exception exception)
+        {
+            Debug.LogWarning($"XR-RM failed to request PICO display refresh rate: {exception.Message}");
+        }
+#endif
     }
 
     private void EnsurePanel()

unity/XR_RM_PICO_UDP_Sender/Assets/XR/Settings/PXR_Settings.asset

@@ -13,7 +13,7 @@ MonoBehaviour:
   m_Name: PXR_Settings
   m_EditorClassIdentifier: 
   stereoRenderingModeAndroid: 0
-  systemDisplayFrequency: 0
+  systemDisplayFrequency: 2
   optimizeBufferDiscards: 1
   enableAppSpaceWarp: 0
   systemSplashScreen: {fileID: 0}

xr_rm_bringup/tools/launcher_ui.py

@@ -56,6 +56,21 @@ CMD_VEL_MONITORS = [
     ("Right Target Vel", "/xr_rm/right_rm75/cmd_vel"),
 ]
 
+CONTROLLER_POSITION_MONITOR_TITLE = "XR-RM Controller Position Monitor"
+CONTROLLER_POSITION_MONITOR_ACTION = "__xr_rm_controller_position_monitor__"
+CONTROLLER_HZ_MONITOR_TITLE = "XR-RM Controller Hz Monitor"
+CONTROLLER_HZ_MONITOR_ACTION = "__xr_rm_controller_hz_monitor__"
+
+CONTROLLER_POSITION_MONITORS = [
+    ("Left Controller Position", "ros2 topic echo /xr/left_controller --field pose.position"),
+    ("Right Controller Position", "ros2 topic echo /xr/right_controller --field pose.position"),
+]
+
+CONTROLLER_HZ_MONITORS = [
+    ("Left Controller Hz", "ros2 topic hz /xr/left_controller"),
+    ("Right Controller Hz", "ros2 topic hz /xr/right_controller"),
+]
+
 ROS_GRAPH_MONITORS = [
     ("ROS Topic List", "ros2 topic list"),
     ("ROS Node List", "ros2 node list"),
@@ -171,21 +186,42 @@ def _cmd_vel_monitor_item() -> tuple[str, str]:
 
 
 def _is_topic_monitor_action(action: str) -> bool:
-    return action in (TOPIC_MONITOR_ACTION, CMD_VEL_MONITOR_ACTION)
+    return action in (
+        TOPIC_MONITOR_ACTION,
+        CMD_VEL_MONITOR_ACTION,
+        CONTROLLER_POSITION_MONITOR_ACTION,
+        CONTROLLER_HZ_MONITOR_ACTION,
+    )
 
 
 def _topic_monitor_spec(action: str) -> tuple[str, list[tuple[str, str]], str, str, str]:
+    if action == CONTROLLER_POSITION_MONITOR_ACTION:
+        return (
+            CONTROLLER_POSITION_MONITOR_TITLE,
+            CONTROLLER_POSITION_MONITORS,
+            "xr_rm_controller_position_monitor",
+            "xr_rm_controller_position_monitor_",
+            "controller position topic",
+        )
+    if action == CONTROLLER_HZ_MONITOR_ACTION:
+        return (
+            CONTROLLER_HZ_MONITOR_TITLE,
+            CONTROLLER_HZ_MONITORS,
+            "xr_rm_controller_hz_monitor",
+            "xr_rm_controller_hz_monitor_",
+            "controller hz topic",
+        )
     if action == CMD_VEL_MONITOR_ACTION:
         return (
             CMD_VEL_MONITOR_TITLE,
-            CMD_VEL_MONITORS,
+            [(title, f"ros2 topic echo {topic}") for title, topic in CMD_VEL_MONITORS],
             "xr_rm_cmd_vel_monitor",
             "xr_rm_cmd_vel_monitor_",
             "target velocity topic",
         )
     return (
         TOPIC_MONITOR_TITLE,
-        TOPIC_MONITORS,
+        [(title, f"ros2 topic echo {topic}") for title, topic in TOPIC_MONITORS],
         "xr_rm_topic_monitor",
         "xr_rm_topic_monitor_",
         "controller topic",
@@ -227,6 +263,14 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
             ("One-Click Left Mock Demo", _one_click_mock("left", "left")),
             ("One-Click Right Mock Demo", _one_click_mock("right", "right")),
             ("One-Click Dual Mock Demo", _one_click_mock("both", "both")),
+            (
+                "Open Controller Position Monitor",
+                CONTROLLER_POSITION_MONITOR_ACTION,
+            ),
+            (
+                "Open Controller Hz Monitor",
+                CONTROLLER_HZ_MONITOR_ACTION,
+            ),
         ]
         one_click = None
     elif mode == "Left Arm":
@@ -420,8 +464,8 @@ class LauncherApp:
         text = selected[1] if selected else ""
         if _is_topic_monitor_action(text):
             _window_title, monitors, _layout_name, _config_prefix, _label = _topic_monitor_spec(text)
-            topics = ", ".join(topic for _title, topic in monitors)
+            commands = "\n".join(command for _title, command in monitors)
-            text = f"Open one Terminator window split into topic panes:\n{topics}"
+            text = f"Open one Terminator window split into left/right panes:\n{commands}"
         elif text == ROS_GRAPH_MONITOR_ACTION:
             text = "Open one Terminator window split into 2 horizontal panes:\nros2 topic list | ros2 node list"
         self.preview.configure(state=tk.NORMAL)
@@ -622,7 +666,7 @@ class LauncherApp:
         except OSError:
             return Path(terminal).name
 
-    def topic_pane_command(self, title: str, topic: str, window_title: str = TOPIC_MONITOR_TITLE) -> str:
+    def topic_pane_command(self, title: str, command: str, window_title: str = TOPIC_MONITOR_TITLE) -> str:
         pane_title = f"{window_title} - {title}"
         lines = _source_lines(self.workspace_root)
         lines.extend([
@@ -630,11 +674,11 @@ class LauncherApp:
             f"printf '\\033]0;%s\\007' {_quote(pane_title)}",
             "clear",
             f"echo {_quote('=== ' + title + ' ===')}",
-            f"echo {_quote('ros2 topic echo ' + topic)}",
+            f"echo {_quote(command)}",
             "echo",
-            f"ros2 topic echo {topic}",
+            command,
             "echo",
-            "echo 'Topic echo stopped. This pane is left open for inspection.'",
+            "echo 'Monitor stopped. This pane is left open for inspection.'",
             "exec bash",
         ])
         return "\n".join(lines)
@@ -661,7 +705,7 @@ class LauncherApp:
     def write_topic_monitor_script(
         self,
         title: str,
-        topic: str,
+        command: str,
         window_title: str = TOPIC_MONITOR_TITLE,
         config_prefix: str = "xr_rm_topic_monitor_",
     ) -> Path:
@@ -673,7 +717,7 @@ class LauncherApp:
             encoding="utf-8",
         )
         with script_file:
-            script_file.write(self.topic_pane_command(title, topic, window_title))
+            script_file.write(self.topic_pane_command(title, command, window_title))
             script_file.write("\n")
         os.chmod(script_file.name, 0o755)
         return Path(script_file.name)
@@ -993,8 +1037,8 @@ class LauncherApp:
         window_title, monitors, layout_name, config_prefix, _label = _topic_monitor_spec(action)
         width, height, left, top = target_rect or self.topic_monitor_rect()
         left_script, right_script = [
-            self.write_topic_monitor_script(title, topic, window_title, config_prefix)
+            self.write_topic_monitor_script(title, command, window_title, config_prefix)
-            for title, topic in monitors
+            for title, command in monitors
         ]
         config_text = "\n".join([
             "[global_config]",
@@ -1189,6 +1233,8 @@ class LauncherApp:
             TERMINAL_TITLE_PREFIX,
             TOPIC_MONITOR_TITLE,
             CMD_VEL_MONITOR_TITLE,
+            CONTROLLER_POSITION_MONITOR_TITLE,
+            CONTROLLER_HZ_MONITOR_TITLE,
             ROS_GRAPH_MONITOR_TITLE,
         )
         closed = 0
@@ -1247,12 +1293,18 @@ class LauncherApp:
             ROS_GRAPH_MONITOR_TITLE,
             "xr_rm_topic_monitor_",
             "xr_rm_cmd_vel_monitor_",
+            "xr_rm_controller_position_monitor_",
+            "xr_rm_controller_hz_monitor_",
             "xr_rm_ros_graph_monitor_",
             "udp_controller_receiver",
             "sample_udp_sender",
             "single_arm_velocity_teleop",
+            "ros2 topic echo /xr/left_controller --field pose.position",
+            "ros2 topic echo /xr/right_controller --field pose.position",
             "ros2 topic echo /xr/left_controller",
             "ros2 topic echo /xr/right_controller",
+            "ros2 topic hz /xr/left_controller",
+            "ros2 topic hz /xr/right_controller",
             "ros2 topic echo /xr_rm/left_rm75/cmd_vel",
             "ros2 topic echo /xr_rm/right_rm75/cmd_vel",
             "ros2 topic list",