Merge pull request #118 from iory/improve

Modified ics-manager

rcb4/ics.py

@@ -1,4 +1,5 @@
 import sys
+import threading
 import time
 
 from colorama import Fore
@@ -9,6 +10,36 @@
 import yaml
 
 
+class KeyListener(threading.Thread):
+    def __init__(self):
+        super().__init__()
+        self.key = None
+        self.lock = threading.Lock()
+        self.running = True  # Add a running flag
+
+    def run(self):
+        """Continuously read keys and store the latest key in self.key while running is True."""
+        while self.running:
+            try:
+                key = readchar.readkey()
+            except KeyboardInterrupt:
+                self.stop()
+                break
+            with self.lock:
+                self.key = key
+
+    def get_key(self):
+        """Return the latest key and reset it."""
+        with self.lock:
+            key = self.key
+            self.key = None
+        return key
+
+    def stop(self):
+        """Stop the listener thread."""
+        self.running = False
+
+
 def load_and_process_yaml(yaml_path):
     with open(yaml_path) as f:
         joint_data = yaml.safe_load(f)
@@ -294,10 +325,14 @@ def display_status(self):
             "Free",
         ]
         selectable_options = ["Current Servo ID", "Angle"]
+
+        key_listener = KeyListener()
+        key_listener.daemon = True
+        key_listener.start()
         try:
             use_previous_result = False
             previous_servo_id = None
-            while True:
+            while key_listener.running:
                 if not self.ics or not self.ics.is_open:
                     # Clear the previous output at the start of each loop
                     sys.stdout.write("\033[H\033[J")
@@ -325,43 +360,41 @@ def display_status(self):
                     if ret is False:
                         time.sleep(1.0)
                         continue
-                sys.stdout.write("\033[H\033[J")
-                sys.stdout.flush()
-
                 # Print servo status
                 try:
                     servo_id = self.get_servo_id()
                     if servo_id != previous_servo_id:
                         use_previous_result = False
                     previous_servo_id = servo_id
-                    print("--- Servo Status ---")
                     if use_previous_result is False:
                         _, result = self.read_param()
-                    for i, option in enumerate(options):
-                        if i == self.selected_index:
-                            print(
-                                f">> {option}: {self.get_status(option, result, selected=True)}"
-                            )
-                        else:
-                            print(f"   {option}: {self.get_status(option, result, selected=False)}")
-
-                    print("----------------------\n")
-                    print(
-                        "Use ↑↓ to navigate, ←→ to adjust Current Servo ID or Servo Angles"
-                    )
-                    print(f"Press 'Enter' when Current Servo ID is selected to save the currently highlighted ID in {Fore.GREEN}green{Style.RESET_ALL}.")
-                    print("Press 'z' to reset servo position")
-                    print(
-                        "Press 'r' to toggle rotation mode (enables continuous wheel-like rotation)"
-                    )
-                    print("Press 'f' to set free mode\n")
-                    print("'q' to quit.")
+                        print('======================================')
+                        sys.stdout.write("\033[H\033[J")
+                        sys.stdout.flush()
+                        print("--- Servo Status ---")
+                        for i, option in enumerate(options):
+                            if i == self.selected_index:
+                                print(
+                                    f">> {option}: {self.get_status(option, result, selected=True)}"
+                                )
+                            else:
+                                print(f"   {option}: {self.get_status(option, result, selected=False)}")
+
+                        print("----------------------\n")
+                        print(
+                            "Use ↑↓ to navigate, ←→ to adjust Current Servo ID or Servo Angles"
+                        )
+                        print(f"Press 'Enter' when Current Servo ID is selected to save the currently highlighted ID in {Fore.GREEN}green{Style.RESET_ALL}.")
+                        print("Press 'z' to reset servo position")
+                        print(
+                            "Press 'r' to toggle rotation mode (enables continuous wheel-like rotation)"
+                        )
+                        print("Press 'f' to set free mode\n")
+                        print("'q' to quit.")
 
+                    key = key_listener.get_key()
                     use_previous_result = False
 
-                    # Get key input without Enter
-                    key = readchar.readkey()
-
                     # Perform actions based on key
                     if key == "z":
                         self.reset_servo_position()
@@ -376,12 +409,10 @@ def display_status(self):
                         self.selected_index = (self.selected_index - 1) % len(
                             selectable_options
                         )
-                        use_previous_result = True
                     elif key == readchar.key.DOWN:
                         self.selected_index = (self.selected_index + 1) % len(
                             selectable_options
                         )
-                        use_previous_result = True
                     elif (
                         key == readchar.key.ENTER
                         and selectable_options[self.selected_index] == "Current Servo ID"
@@ -395,7 +426,6 @@ def display_status(self):
                         key == readchar.key.LEFT
                         and selectable_options[self.selected_index] == "Current Servo ID"
                     ):
-                        use_previous_result = True
                         if self.servo_id_index == 0:
                             self.servo_id_index = len(self.servo_candidates) - 1
                         else:
@@ -405,7 +435,6 @@ def display_status(self):
                         key == readchar.key.RIGHT
                         and selectable_options[self.selected_index] == "Current Servo ID"
                     ):
-                        use_previous_result = True
                         if self.servo_id_index == len(self.servo_candidates) - 1:
                             self.servo_id_index = 0
                         else:
@@ -423,6 +452,8 @@ def display_status(self):
                         and selectable_options[self.selected_index] == "Angle"
                     ):
                         self.increase_angle()
+                    else:
+                        use_previous_result = True
                 except Exception as e:
                     print(f'[ERROR] {e}')
                     use_previous_result = False
@@ -436,6 +467,7 @@ def display_status(self):
 
         except KeyboardInterrupt:
             print("\nDisplay stopped by user.")
+        key_listener.stop()
 
     def get_status(self, option, param=None, selected=False):
         """Return the status based on the selected option."""

ros/kxr_controller/node_scripts/rcb4_ros_bridge.py

@@ -400,7 +400,9 @@ def log_error_and_close_interface(function_name):
                 if ret is None:
                     return log_error_and_close_interface("set trim_vector")
         if self.interface.wheel_servo_sorted_ids is None:
-            self.interface.wheel_servo_sorted_ids = wheel_servo_sorted_ids
+            self.interface.wheel_servo_sorted_ids = []
+        self.interface.wheel_servo_sorted_ids = list(
+            set(self.interface.wheel_servo_sorted_ids + wheel_servo_sorted_ids))
 
         # set servo ids to rosparam
         servo_ids = self.get_ids(type="servo")

ros/kxr_controller/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>kxr_controller</name>
-  <version>0.0.4</version>
+  <version>0.0.5</version>
   <description>The kxr_controller package</description>
 
   <maintainer email="[email protected]">Iori Yanokura</maintainer>

ros/kxr_models/package.xml

@@ -1,6 +1,6 @@
 <package format="2">
   <name>kxr_models</name>
-  <version>0.0.4</version>
+  <version>0.0.5</version>
   <description>
     <p>URDF Description package for kxr_models</p>
     <p>This package contains configuration data, 3D models and launch files for kxr robot</p>

ros/kxreus/package.xml

@@ -1,7 +1,7 @@
 <package format="3">
 
   <name>kxreus</name>
-  <version>0.0.4</version>
+  <version>0.0.5</version>
   <description>
     roseus interface for kxr robots.
   </description>

setup.py

@@ -6,7 +6,7 @@
 from setuptools import find_packages
 from setuptools import setup
 
-version = "0.0.4"
+version = "0.0.5"
 
 
 if sys.argv[-1] == "release":