From 10e1ce62d213478b50255cb73b8a480671d5c1bc Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 03:01:47 +0900
Subject: [PATCH 01/18] Delete whitespace

---
 ros/kxr_controller/node_scripts/rcb4_ros_bridge.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index 03395ed6..cb8a0d6c 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -399,7 +399,7 @@ def get_ids(self, type='servo', retry_count=-1):
         ids = []
         while retry_count == -1 or attempts < retry_count:
             try:
-                if type == 'servo' :
+                if type == 'servo':
                     ids = self.interface.search_servo_ids()
                 elif type == 'air_board':
                     ids = self.interface.search_air_board_ids()

From 54d2b547c2cb5547bc0d095200b25a25c97d4690 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 03:02:24 +0900
Subject: [PATCH 02/18] Add setup_interface function

---
 .../node_scripts/rcb4_ros_bridge.py           | 37 +++++++++----------
 1 file changed, 18 insertions(+), 19 deletions(-)

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index cb8a0d6c..43e1096e 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -184,25 +184,8 @@ def __init__(self):
             queue_size=1,
         )
 
-        self.interface = None
-        while not rospy.is_shutdown():
-            try:
-                if rospy.get_param("~device", None):
-                    self.interface = ARMH7Interface.from_port(
-                        rospy.get_param("~device", None)
-                    )
-                elif rospy.get_param("~use_rcb4"):
-                    self.interface = RCB4Interface()
-                    self.interface.auto_open()
-                else:
-                    self.interface = ARMH7Interface.from_port()
-                break
-            except serial.SerialException as e:
-                rospy.logerr(f"Waiting for the port to become available. {e}")
-            rospy.sleep(1.0)
-        if self.interface is None:
-            rospy.logerr("Could not open port!")
-            sys.exit(1)
+        self.use_rcb4 = rospy.get_param("~use_rcb4", False)
+        self.interface = self.setup_interface()
         self._prev_velocity_command = None
 
         self.srv = Server(Config, self.config_callback)
@@ -377,6 +360,22 @@ def __init__(self):
                 clean_namespace + "/battery_voltage", std_msgs.msg.Float32, queue_size=1
             )
 
+    def setup_interface(self):
+        while not rospy.is_shutdown():
+            try:
+                if rospy.get_param("~device", None):
+                    return ARMH7Interface.from_port(rospy.get_param("~device"))
+                if self.use_rcb4:
+                    interface = RCB4Interface()
+                    interface.auto_open()
+                    return interface
+                return ARMH7Interface.from_port()
+            except serial.SerialException as e:
+                rospy.logerr(f"Waiting for the port to become available: {e}")
+                rospy.sleep(1.0)
+        rospy.logerr("Could not open port!")
+        sys.exit(1)
+
     def __del__(self):
         if self.proc_controller_spawner:
             self.proc_controller_spawner.kill()

From 572b1cef3e1210bdbeea1329a2e21255969c92e6 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 03:03:03 +0900
Subject: [PATCH 03/18] Refactor delete RuntimeError exception and call
 unsubscribe in __del__

---
 ros/kxr_controller/node_scripts/rcb4_ros_bridge.py | 11 +----------
 1 file changed, 1 insertion(+), 10 deletions(-)

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index 43e1096e..bca838c6 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -377,6 +377,7 @@ def setup_interface(self):
         sys.exit(1)
 
     def __del__(self):
+        self.unsubscribe()
         if self.proc_controller_spawner:
             self.proc_controller_spawner.kill()
         if self.proc_robot_state_publisher:
@@ -551,9 +552,6 @@ def servo_on_off_callback(self, goal):
                 servo_vector.append(32768)
         try:
             self.interface.servo_angle_vector(servo_ids, servo_vector, velocity=1)
-        except RuntimeError as e:
-            self.unsubscribe()
-            rospy.signal_shutdown(f"Disconnected {e}.")
         except serial.serialutil.SerialException as e:
             rospy.logerr(f"[servo_on_off] {e!s}")
         return self.servo_on_off_server.set_succeeded(ServoOnOffResult())
@@ -575,9 +573,6 @@ def adjust_angle_vector_callback(self, goal):
             if adjust is True:
                 self.cancel_motion_pub.publish(GoalID())
                 rospy.logwarn("Stop motion by sending follow joint trajectory cancel.")
-        except RuntimeError as e:
-            self.unsubscribe()
-            rospy.signal_shutdown(f"Disconnected {e}.")
         except serial.serialutil.SerialException as e:
             rospy.logerr(f"[adjust_angle_vector] {e!s}")
         return self.adjust_angle_vector_server.set_succeeded(AdjustAngleVectorResult())
@@ -853,10 +848,6 @@ def run(self):
         )
 
         while not rospy.is_shutdown():
-            if self.interface.is_opened() is False:
-                self.unsubscribe()
-                rospy.signal_shutdown("Disconnected.")
-                break
             self.publish_joint_states()
             self.publish_servo_on_off()
 

From f6e800542ecc281dcffc3aaa1ca1de5afca64e0e Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 14:21:22 +0900
Subject: [PATCH 04/18] Retry when failed

---
 .../node_scripts/rcb4_ros_bridge.py           | 123 +++++++++++++++---
 1 file changed, 102 insertions(+), 21 deletions(-)

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index bca838c6..e19bc2a1 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -225,20 +225,7 @@ def __init__(self):
 
         rospy.loginfo("run kxr_controller")
         self.proc_kxr_controller = run_kxr_controller(namespace=clean_namespace)
-
-        self.command_joint_state_sub = rospy.Subscriber(
-            clean_namespace + "/command_joint_state",
-            JointState,
-            queue_size=1,
-            callback=self.command_joint_state_callback,
-        )
-
-        self.velocity_command_joint_state_sub = rospy.Subscriber(
-            clean_namespace + "/velocity_command_joint_state",
-            JointState,
-            queue_size=1,
-            callback=self.velocity_command_joint_state_callback,
-        )
+        self.subscribe()
 
         self.servo_on_off_server = actionlib.SimpleActionServer(
             clean_namespace + "/fullbody_controller/servo_on_off_real_interface",
@@ -361,18 +348,24 @@ def __init__(self):
             )
 
     def setup_interface(self):
+        rospy.loginfo('Try to connect servo control boards.')
         while not rospy.is_shutdown():
+            rospy.loginfo("Waiting for the port to become available")
             try:
                 if rospy.get_param("~device", None):
                     return ARMH7Interface.from_port(rospy.get_param("~device"))
                 if self.use_rcb4:
                     interface = RCB4Interface()
-                    interface.auto_open()
+                    ret = interface.auto_open()
+                    if ret is True:
+                        return interface
+                interface = ARMH7Interface()
+                ret = interface.auto_open()
+                if ret is True:
                     return interface
-                return ARMH7Interface.from_port()
             except serial.SerialException as e:
                 rospy.logerr(f"Waiting for the port to become available: {e}")
-                rospy.sleep(1.0)
+            rospy.sleep(1.0)
         rospy.logerr("Could not open port!")
         sys.exit(1)
 
@@ -385,6 +378,20 @@ def __del__(self):
         if self.proc_kxr_controller:
             self.proc_kxr_controller.kill()
 
+    def subscribe(self):
+        self.command_joint_state_sub = rospy.Subscriber(
+            self.clean_namespace + "/command_joint_state",
+            JointState,
+            queue_size=1,
+            callback=self.command_joint_state_callback,
+        )
+        self.velocity_command_joint_state_sub = rospy.Subscriber(
+            self.clean_namespace + "/velocity_command_joint_state",
+            JointState,
+            queue_size=1,
+            callback=self.velocity_command_joint_state_callback,
+        )
+
     def unsubscribe(self):
         self.command_joint_state_sub.unregister()
         self.velocity_command_joint_state_sub.unregister()
@@ -513,6 +520,8 @@ def _msg_to_angle_vector_and_servo_ids(self, msg, velocity_control=False):
         return angle_vector[valid_indices], servo_ids[valid_indices]
 
     def velocity_command_joint_state_callback(self, msg):
+        if not self.interface.is_opened():
+            return
         av, servo_ids = self._msg_to_angle_vector_and_servo_ids(
             msg, velocity_control=True
         )
@@ -529,6 +538,8 @@ def velocity_command_joint_state_callback(self, msg):
             rospy.logerr(f"[velocity_command_joint] {e!s}")
 
     def command_joint_state_callback(self, msg):
+        if not self.interface.is_opened():
+            return
         av, servo_ids = self._msg_to_angle_vector_and_servo_ids(
             msg, velocity_control=False
         )
@@ -540,6 +551,8 @@ def command_joint_state_callback(self, msg):
             rospy.logerr(f"[command_joint] {e!s}")
 
     def servo_on_off_callback(self, goal):
+        if not self.interface.is_opened():
+            return
         servo_vector = []
         servo_ids = []
         for joint_name, servo_on in zip(goal.joint_names, goal.servo_on_states):
@@ -557,6 +570,8 @@ def servo_on_off_callback(self, goal):
         return self.servo_on_off_server.set_succeeded(ServoOnOffResult())
 
     def adjust_angle_vector_callback(self, goal):
+        if not self.interface.is_opened():
+            return
         servo_ids = []
         error_thresholds = []
         for joint_name, error_threshold in zip(goal.joint_names, goal.error_threshold):
@@ -578,6 +593,8 @@ def adjust_angle_vector_callback(self, goal):
         return self.adjust_angle_vector_server.set_succeeded(AdjustAngleVectorResult())
 
     def publish_stretch(self):
+        if not self.interface.is_opened():
+            return
         # Get current stretch of all servo motors and publish them
         joint_names = []
         servo_ids = []
@@ -595,6 +612,8 @@ def publish_stretch(self):
         self.stretch_publisher.publish(stretch_msg)
 
     def stretch_callback(self, goal):
+        if not self.interface.is_opened():
+            return
         if len(goal.joint_names) == 0:
             goal.joint_names = self.joint_names
         joint_names = []
@@ -616,6 +635,8 @@ def stretch_callback(self, goal):
         return self.stretch_server.set_succeeded(StretchResult())
 
     def publish_pressure(self):
+        if not self.interface.is_opened():
+            return
         for idx in self.air_board_ids:
             try:
                 key = f"{idx}"
@@ -696,6 +717,8 @@ def release_vacuum(self, idx):
 
         After 1s, all valves are closed and pump is stopped.
         """
+        if not self.interface.is_opened():
+            return
         try:
             self.interface.stop_pump()
             self.interface.open_work_valve(idx)
@@ -708,6 +731,8 @@ def release_vacuum(self, idx):
 
     def start_vacuum(self, idx):
         """Vacuum air in work"""
+        if not self.interface.is_opened():
+            return
         try:
             self.interface.start_pump()
             self.interface.open_work_valve(idx)
@@ -717,6 +742,8 @@ def start_vacuum(self, idx):
 
     def stop_vacuum(self, idx):
         """Seal air in work"""
+        if not self.interface.is_opened():
+            return
         try:
             self.interface.close_work_valve(idx)
             self.interface.close_air_connect_valve()
@@ -726,6 +753,8 @@ def stop_vacuum(self, idx):
             rospy.logerr(f"[stop_vacuum] {e!s}")
 
     def pressure_control_callback(self, goal):
+        if not self.interface.is_opened():
+            return
         if self.pressure_control_thread is not None:
             # Finish existing thread
             self.pressure_control_running = False
@@ -752,6 +781,8 @@ def pressure_control_callback(self, goal):
         return self.pressure_control_server.set_succeeded(PressureControlResult())
 
     def publish_imu_message(self):
+        if not self.interface.is_opened():
+            return
         msg = sensor_msgs.msg.Imu()
         msg.header.frame_id = self.imu_frame_id
         msg.header.stamp = rospy.Time.now()
@@ -772,6 +803,8 @@ def publish_imu_message(self):
         self.imu_publisher.publish(msg)
 
     def publish_sensor_values(self):
+        if not self.interface.is_opened():
+            return
         stamp = rospy.Time.now()
         msg = geometry_msgs.msg.WrenchStamped()
         msg.header.stamp = stamp
@@ -814,13 +847,16 @@ def publish_joint_states(self):
             torque_vector = self.interface.servo_error()
         except IndexError as e:
             rospy.logerr(f"[publish_joint_states] {e!s}")
-            return
+            return False
         except ValueError as e:
             rospy.logerr(f"[publish_joint_states] {e!s}")
-            return
+            return False
         except serial.serialutil.SerialException as e:
             rospy.logerr(f"[publish_joint_states] {e!s}")
-            return
+            return False
+        except OSError as e:
+            rospy.logerr(f"[publish_joint_states] {e!s}")
+            return False
         msg = JointState()
         msg.header.stamp = rospy.Time.now()
         for name in self.joint_names:
@@ -833,8 +869,11 @@ def publish_joint_states(self):
                 msg.effort.append(torque_vector[idx])
                 msg.name.append(name)
         self.current_joint_states_pub.publish(msg)
+        return True
 
     def publish_servo_on_off(self):
+        if not self.interface.is_opened():
+            return
         self.check_servo_states()
         servo_on_off_msg = ServoOnOff()
         servo_on_off_msg.joint_names = list(self.joint_servo_on.keys())
@@ -842,13 +881,55 @@ def publish_servo_on_off(self):
             self.joint_servo_on.values())
         self.servo_on_off_pub.publish(servo_on_off_msg)
 
+    def reinitialize_interface(self):
+        self.unsubscribe()
+        self.interface.close()
+        self.interface = self.setup_interface()
+        self.subscribe()
+
+    def check_success_rate(self):
+        # Calculate success rate
+        if self.publish_joint_states_attempts > 0:
+            success_rate = (
+                self.publish_joint_states_successes / self.publish_joint_states_attempts
+            )
+            rospy.loginfo(f"Current communication success rate: {success_rate:.2%}")
+
+            # Check if the success rate is below the threshold
+            if success_rate < self.success_rate_threshold:
+                rospy.logwarn(
+                    f"communication success rate ({success_rate:.2%}) below threshold; "
+                    + "reinitializing interface."
+                )
+                self.reinitialize_interface()
+
+        # Reset counters and timer
+        self.publish_joint_states_successes = 0
+        self.publish_joint_states_attempts = 0
+        self.last_check_time = rospy.Time.now()
+
     def run(self):
         rate = rospy.Rate(
             rospy.get_param(self.clean_namespace + "/control_loop_rate", 20)
         )
 
+        self.publish_joint_states_attempts = 0
+        self.publish_joint_states_successes = 0
+        self.last_check_time = rospy.Time.now()
+        check_interval = rospy.get_param('~check_interval', 3)
+        self.success_rate_threshold = 0.8  # Minimum success rate required
+
         while not rospy.is_shutdown():
-            self.publish_joint_states()
+            success = self.publish_joint_states()
+            self.publish_joint_states_attempts += 1
+            if success:
+                self.publish_joint_states_successes += 1
+
+            # Check success rate periodically
+            current_time = rospy.Time.now()
+            if (current_time - self.last_check_time).to_sec() >= check_interval:
+                self.check_success_rate()
+
             self.publish_servo_on_off()
 
             if self.publish_imu and self.imu_publisher.get_num_connections():

From 4a9450077c678b63c4352f00e297b06d9ff35d09 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 14:29:24 +0900
Subject: [PATCH 05/18] Clean namespace to base_namespace

---
 .../node_scripts/rcb4_ros_bridge.py           | 71 ++++++++++---------
 1 file changed, 39 insertions(+), 32 deletions(-)

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index e19bc2a1..3203a128 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -161,23 +161,23 @@ def __init__(self):
 
         full_namespace = rospy.get_namespace()
         last_slash_pos = full_namespace.rfind("/")
-        clean_namespace = full_namespace[:last_slash_pos] if last_slash_pos != 0 else ""
-        self.clean_namespace = clean_namespace
+        base_namespace = full_namespace[:last_slash_pos] if last_slash_pos != 0 else ""
+        self.base_namespace = base_namespace
 
         set_robot_description(
-            urdf_path, param_name=clean_namespace + "/robot_description"
+            urdf_path, param_name=base_namespace + "/robot_description"
         )
         if tmp_urdf:
             os.remove(urdf_path)
 
         set_joint_state_controler()
         self.current_joint_states_pub = rospy.Publisher(
-            clean_namespace + "/current_joint_states", JointState, queue_size=1
+            base_namespace + "/current_joint_states", JointState, queue_size=1
         )
         # Publish servo state like joint_trajectory_controller
         # https://wiki.ros.org/joint_trajectory_controller#Published_Topics
         self.servo_on_off_pub = rospy.Publisher(
-            clean_namespace
+            base_namespace
             + "/fullbody_controller/servo_on_off_real_interface"
             + "/state",
             ServoOnOff,
@@ -192,7 +192,7 @@ def __init__(self):
 
         # set servo ids to rosparam
         rospy.set_param(
-            clean_namespace + "/servo_ids", self.get_ids(type='servo')
+            base_namespace + "/servo_ids", self.get_ids(type='servo')
         )
 
         wheel_servo_sorted_ids = []
@@ -219,16 +219,16 @@ def __init__(self):
         if self.interface.wheel_servo_sorted_ids is None:
             self.interface.wheel_servo_sorted_ids = wheel_servo_sorted_ids
 
-        self.set_fullbody_controller(clean_namespace)
-        self.set_initial_positions(clean_namespace)
+        self.set_fullbody_controller(base_namespace)
+        self.set_initial_positions(base_namespace)
         self.check_servo_states(retry_count=-1)
 
         rospy.loginfo("run kxr_controller")
-        self.proc_kxr_controller = run_kxr_controller(namespace=clean_namespace)
+        self.proc_kxr_controller = run_kxr_controller(namespace=base_namespace)
         self.subscribe()
 
         self.servo_on_off_server = actionlib.SimpleActionServer(
-            clean_namespace + "/fullbody_controller/servo_on_off_real_interface",
+            base_namespace + "/fullbody_controller/servo_on_off_real_interface",
             ServoOnOffAction,
             execute_cb=self.servo_on_off_callback,
             auto_start=False,
@@ -238,7 +238,7 @@ def __init__(self):
         self.servo_on_off_server.start()
 
         self.adjust_angle_vector_server = actionlib.SimpleActionServer(
-            clean_namespace + "/fullbody_controller/adjust_angle_vector_interface",
+            base_namespace + "/fullbody_controller/adjust_angle_vector_interface",
             AdjustAngleVectorAction,
             execute_cb=self.adjust_angle_vector_callback,
             auto_start=False,
@@ -247,7 +247,7 @@ def __init__(self):
         rospy.sleep(0.1)
         self.adjust_angle_vector_server.start()
         self.cancel_motion_pub = rospy.Publisher(
-            clean_namespace
+            base_namespace
             + "/fullbody_controller/follow_joint_trajectory"
             + "/cancel",
             GoalID,
@@ -258,7 +258,7 @@ def __init__(self):
         if not rospy.get_param("~use_rcb4"):
             # Stretch
             self.stretch_server = actionlib.SimpleActionServer(
-                clean_namespace + "/fullbody_controller/stretch_interface",
+                base_namespace + "/fullbody_controller/stretch_interface",
                 StretchAction,
                 execute_cb=self.stretch_callback,
                 auto_start=False,
@@ -268,7 +268,7 @@ def __init__(self):
             rospy.sleep(0.1)
             self.stretch_server.start()
             self.stretch_publisher = rospy.Publisher(
-                clean_namespace + "/fullbody_controller/stretch",
+                base_namespace + "/fullbody_controller/stretch",
                 Stretch,
                 queue_size=1,
                 latch=True,
@@ -282,7 +282,7 @@ def __init__(self):
             if self.control_pressure is True:
                 self.pressure_control_thread = None
                 self.pressure_control_server = actionlib.SimpleActionServer(
-                    clean_namespace + "/fullbody_controller/pressure_control_interface",
+                    base_namespace + "/fullbody_controller/pressure_control_interface",
                     PressureControlAction,
                     execute_cb=self.pressure_control_callback,
                     auto_start=False,
@@ -294,7 +294,7 @@ def __init__(self):
                 # Publish state topic like joint_trajectory_controller
                 # https://wiki.ros.org/joint_trajectory_controller#Published_Topics
                 self.pressure_control_pub = rospy.Publisher(
-                    clean_namespace
+                    base_namespace
                     + "/fullbody_controller/pressure_control_interface"
                     + "/state",
                     PressureControl,
@@ -302,7 +302,7 @@ def __init__(self):
                 )
                 self.air_board_ids = self.get_ids(type='air_board')
                 rospy.set_param(
-                    clean_namespace + "/air_board_ids",
+                    base_namespace + "/air_board_ids",
                     self.air_board_ids
                 )
                 self.pressure_control_state = {}
@@ -314,7 +314,7 @@ def __init__(self):
                 self._pressure_publisher_dict = {}
                 self._avg_pressure_publisher_dict = {}
                 # Record 1 seconds pressure data.
-                hz = rospy.get_param(self.clean_namespace + "/control_loop_rate", 20)
+                hz = rospy.get_param(self.base_namespace + "/control_loop_rate", 20)
                 self.recent_pressures = deque([], maxlen=1 * int(hz))
 
         self.proc_controller_spawner = subprocess.Popen(
@@ -325,7 +325,7 @@ def __init__(self):
             ]
             + ["joint_state_controller", "fullbody_controller"]
         )
-        self.proc_robot_state_publisher = run_robot_state_publisher(clean_namespace)
+        self.proc_robot_state_publisher = run_robot_state_publisher(base_namespace)
 
         self.publish_imu = rospy.get_param("~publish_imu", True)
         self.publish_sensor = rospy.get_param("~publish_sensor", False)
@@ -335,16 +335,16 @@ def __init__(self):
             self.publish_sensor = False
         if self.publish_imu:
             self.imu_frame_id = rospy.get_param(
-                "~imu_frame_id", clean_namespace + "/" + r.root_link.name
+                "~imu_frame_id", base_namespace + "/" + r.root_link.name
             )
             self.imu_publisher = rospy.Publisher(
-                clean_namespace + "/imu", sensor_msgs.msg.Imu, queue_size=1
+                base_namespace + "/imu", sensor_msgs.msg.Imu, queue_size=1
             )
         if self.publish_sensor:
             self._sensor_publisher_dict = {}
         if self.publish_battery_voltage:
             self.battery_voltage_publisher = rospy.Publisher(
-                clean_namespace + "/battery_voltage", std_msgs.msg.Float32, queue_size=1
+                base_namespace + "/battery_voltage", std_msgs.msg.Float32, queue_size=1
             )
 
     def setup_interface(self):
@@ -369,6 +369,14 @@ def setup_interface(self):
         rospy.logerr("Could not open port!")
         sys.exit(1)
 
+    def get_base_namespace(self):
+        """Return the clean namespace for the node.
+
+        """
+        full_namespace = rospy.get_namespace()
+        last_slash_pos = full_namespace.rfind("/")
+        return full_namespace[:last_slash_pos] if last_slash_pos != 0 else ""
+
     def __del__(self):
         self.unsubscribe()
         if self.proc_controller_spawner:
@@ -380,13 +388,13 @@ def __del__(self):
 
     def subscribe(self):
         self.command_joint_state_sub = rospy.Subscriber(
-            self.clean_namespace + "/command_joint_state",
+            self.base_namespace + "/command_joint_state",
             JointState,
             queue_size=1,
             callback=self.command_joint_state_callback,
         )
         self.velocity_command_joint_state_sub = rospy.Subscriber(
-            self.clean_namespace + "/velocity_command_joint_state",
+            self.base_namespace + "/velocity_command_joint_state",
             JointState,
             queue_size=1,
             callback=self.velocity_command_joint_state_callback,
@@ -455,7 +463,7 @@ def check_servo_states(self, retry_count=1):
             else:
                 self.joint_servo_on[jn] = False
 
-    def set_fullbody_controller(self, clean_namespace):
+    def set_fullbody_controller(self, base_namespace):
         self.fullbody_jointnames = []
         for jn in self.joint_names:
             if jn not in self.joint_name_to_id:
@@ -466,7 +474,7 @@ def set_fullbody_controller(self, clean_namespace):
             self.fullbody_jointnames.append(jn)
         set_fullbody_controller(self.fullbody_jointnames)
 
-    def set_initial_positions(self, clean_namespace):
+    def set_initial_positions(self, base_namespace):
         initial_positions = {}
         while True:
             try:
@@ -490,7 +498,7 @@ def set_initial_positions(self, clean_namespace):
             if idx is None:
                 continue
             initial_positions[jn] = float(np.deg2rad(init_av[idx]))
-        set_initial_position(initial_positions, namespace=clean_namespace)
+        set_initial_position(initial_positions, namespace=base_namespace)
 
     def _msg_to_angle_vector_and_servo_ids(self, msg, velocity_control=False):
         used_servo_id = {}
@@ -642,12 +650,12 @@ def publish_pressure(self):
                 key = f"{idx}"
                 if key not in self._pressure_publisher_dict:
                     self._pressure_publisher_dict[key] = rospy.Publisher(
-                        self.clean_namespace + "/fullbody_controller/pressure/" + key,
+                        self.base_namespace + "/fullbody_controller/pressure/" + key,
                         std_msgs.msg.Float32,
                         queue_size=1,
                     )
                     self._avg_pressure_publisher_dict[key] = rospy.Publisher(
-                        self.clean_namespace
+                        self.base_namespace
                         + "/fullbody_controller/average_pressure/"
                         + key,
                         std_msgs.msg.Float32,
@@ -823,7 +831,7 @@ def publish_sensor_values(self):
                         msg.wrench.force.x = sensor.adc[i]
                     if key not in self._sensor_publisher_dict:
                         self._sensor_publisher_dict[key] = rospy.Publisher(
-                            self.clean_namespace + f"/kjs/{sensor.id}/{typ}/{i}",
+                            self.base_namespace + f"/kjs/{sensor.id}/{typ}/{i}",
                             geometry_msgs.msg.WrenchStamped,
                             queue_size=1,
                         )
@@ -893,7 +901,6 @@ def check_success_rate(self):
             success_rate = (
                 self.publish_joint_states_successes / self.publish_joint_states_attempts
             )
-            rospy.loginfo(f"Current communication success rate: {success_rate:.2%}")
 
             # Check if the success rate is below the threshold
             if success_rate < self.success_rate_threshold:
@@ -910,7 +917,7 @@ def check_success_rate(self):
 
     def run(self):
         rate = rospy.Rate(
-            rospy.get_param(self.clean_namespace + "/control_loop_rate", 20)
+            rospy.get_param(self.base_namespace + "/control_loop_rate", 20)
         )
 
         self.publish_joint_states_attempts = 0

From 6269a8c91f9230d6c66a93f7332b59b48f037304 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 16:35:19 +0900
Subject: [PATCH 06/18] Filter non-active servo on servo_angle_vector

---
 rcb4/armh7interface.py | 34 +++++++++++++++++++++++++++++++---
 rcb4/rcb4interface.py  | 38 +++++++++++++++++++++++++++++++++++---
 2 files changed, 66 insertions(+), 6 deletions(-)

diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py
index 9e04eb8e..5529567e 100644
--- a/rcb4/armh7interface.py
+++ b/rcb4/armh7interface.py
@@ -703,6 +703,13 @@ def search_servo_ids(self):
             self._servo_id_to_sequentialized_servo_id[servo_indices] = np.arange(
                 len(servo_indices)
             )
+        servo_on_states = self.servo_states()
+        self.servo_on_states_dict = {}
+        for index in servo_indices:
+            if index in servo_on_states:
+                self.servo_on_states_dict[index] = True
+            else:
+                self.servo_on_states_dict[index] = False
         return servo_indices
 
     def search_air_board_ids(self):
@@ -777,10 +784,31 @@ def servo_angle_vector(self, servo_ids, servo_vector, velocity=127):
         if len(servo_ids) != len(servo_vector):
             raise ValueError("Length of servo_ids and servo_vector must be the same.")
 
+        # Update servo on/off states based on 32767 and 32768 values in servo_vector
+        for servo_id, angle in zip(servo_ids, servo_vector):
+            if angle == 32767:
+                self.servo_on_states_dict[servo_id] = True
+            elif angle == 32768:
+                self.servo_on_states_dict[servo_id] = False
+
+        # Filter servo IDs based on their on state in servo_on_states_dict
+        active_ids = []
+        active_angles = []
+        for servo_id, angle in zip(servo_ids, servo_vector):
+            if self.servo_on_states_dict.get(servo_id, False) or angle in (32767, 32768):
+                # Only include active servos
+                active_ids.append(servo_id)
+                active_angles.append(angle)
+
+        # If no active servos, skip command sending
+        if not active_ids:
+            # print("[servo_angle_vector] No active servos to send commands.")
+            return
+
         # Sort the servo vectors based on servo IDs
-        sorted_indices = np.argsort(servo_ids)
-        sorted_servo_ids = np.array(servo_ids)[sorted_indices]
-        sorted_servo_vector = np.array(servo_vector)[sorted_indices]
+        sorted_indices = np.argsort(active_ids)
+        sorted_servo_ids = np.array(active_ids)[sorted_indices]
+        sorted_servo_vector = np.array(active_angles)[sorted_indices]
 
         # Prepare the command byte list
         if (
diff --git a/rcb4/rcb4interface.py b/rcb4/rcb4interface.py
index bb513fe5..069d7cea 100644
--- a/rcb4/rcb4interface.py
+++ b/rcb4/rcb4interface.py
@@ -413,6 +413,14 @@ def search_servo_ids(self):
             self._servo_id_to_sequentialized_servo_id[servo_indices] = np.arange(
                 len(servo_indices)
             )
+
+        servo_on_states = self.servo_states()
+        self.servo_on_states_dict = {}
+        for index in servo_indices:
+            if index in servo_on_states:
+                self.servo_on_states_dict[index] = True
+            else:
+                self.servo_on_states_dict[index] = False
         return servo_indices
 
     def valid_servo_ids(self, servo_ids):
@@ -473,10 +481,34 @@ def servo_angle_vector(self, servo_ids, servo_vector, velocity=127):
         if len(servo_ids) != len(servo_vector):
             raise ValueError("Length of servo_ids and servo_vector must be the same.")
 
+        # Update servo on/off states based on 32767 and 32768 values in servo_vector
+        for servo_id, angle in zip(servo_ids, servo_vector):
+            if angle == 32767:
+                self.servo_on_states_dict[servo_id] = True
+            elif angle == 32768:
+                self.servo_on_states_dict[servo_id] = False
+
+        # Filter servo IDs based on their on state in servo_on_states_dict
+        active_ids = []
+        active_angles = []
+        for servo_id, angle in zip(servo_ids, servo_vector):
+            if self.servo_on_states_dict.get(servo_id, False) or angle in (
+                32767,
+                32768,
+            ):
+                # Only include active servos
+                active_ids.append(servo_id)
+                active_angles.append(angle)
+
+        # If no active servos, skip command sending
+        if not active_ids:
+            # print("[servo_angle_vector] No active servos to send commands.")
+            return
+
         # Sort the servo vectors based on servo IDs
-        sorted_indices = np.argsort(servo_ids)
-        sorted_servo_ids = np.array(servo_ids)[sorted_indices]
-        sorted_servo_vector = np.array(servo_vector)[sorted_indices]
+        sorted_indices = np.argsort(active_ids)
+        sorted_servo_ids = np.array(active_ids)[sorted_indices]
+        sorted_servo_vector = np.array(active_angles)[sorted_indices]
 
         # Prepare the command byte list
         if (

From c01111a743eda886c3169731cece104511a2f9ba Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 17:12:07 +0900
Subject: [PATCH 07/18] Use serial_call_with_retry

---
 .../node_scripts/rcb4_ros_bridge.py           | 264 +++++++-----------
 .../python/kxr_controller/serial.py           |  41 +++
 2 files changed, 141 insertions(+), 164 deletions(-)
 create mode 100644 ros/kxr_controller/python/kxr_controller/serial.py

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index 3203a128..8e9b9fcf 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -39,6 +39,8 @@
 from rcb4.armh7interface import ARMH7Interface
 from rcb4.rcb4interface import RCB4Interface
 
+from kxr_controller.serial import serial_call_with_retry
+
 np.set_printoptions(precision=0, suppress=True)
 
 
@@ -215,13 +217,14 @@ def __init__(self):
             trim_vector_servo_ids.append(servo_id)
             trim_vector_offset.append(direction * offset)
         if self.interface.__class__.__name__ != "RCB4Interface":
-            self.interface.trim_vector(trim_vector_offset, trim_vector_servo_ids)
+            serial_call_with_retry(self.interface.trim_vector,
+                                   trim_vector_offset, trim_vector_servo_ids,
+                                   max_retries=10)
         if self.interface.wheel_servo_sorted_ids is None:
             self.interface.wheel_servo_sorted_ids = wheel_servo_sorted_ids
 
         self.set_fullbody_controller(base_namespace)
         self.set_initial_positions(base_namespace)
-        self.check_servo_states(retry_count=-1)
 
         rospy.loginfo("run kxr_controller")
         self.proc_kxr_controller = run_kxr_controller(namespace=base_namespace)
@@ -409,59 +412,17 @@ def config_callback(self, config, level):
         self.wheel_frame_count = config.wheel_frame_count
         return config
 
-    def get_ids(self, type='servo', retry_count=-1):
-        attempts = 0
-        ids = []
-        while retry_count == -1 or attempts < retry_count:
-            try:
-                if type == 'servo':
-                    ids = self.interface.search_servo_ids()
-                elif type == 'air_board':
-                    ids = self.interface.search_air_board_ids()
-                ids = ids.tolist()
-                break
-            except serial.serialutil.SerialException:
-                rospy.logerr(f"[get_ids] Failed to retrieve {type} ids.")
-                rospy.sleep(0.1)
-                attempts += 1
-                continue
-            except Exception as e:
-                rospy.logerr(f"[get_ids] Unexpected error: {e}")
-                rospy.sleep(0.1)
-                attempts += 1
-                continue
-        return ids
-
-    def check_servo_states(self, retry_count=1):
-        self.joint_servo_on = {jn: False for jn in self.joint_names}
-        attempts = 0
-        while retry_count == -1 or attempts < retry_count:
-            try:
-                servo_on_states = self.interface.servo_states()
-                break
-            except serial.serialutil.SerialException as e:
-                rospy.logerr(f"[check_servo_states] Failed to retrieve servo states. {e}")
-                rospy.sleep(0.1)
-                attempts += 1
-                continue
-            except Exception as e:
-                rospy.logerr(f"[check_servo_states] Unexpected error: {e}")
-                rospy.sleep(0.1)
-                attempts += 1
-                continue
-        if retry_count != -1 and attempts >= retry_count:
-            rospy.logerr(
-                "[check_servo_states] Failed to retrieve servo states after maximum retries."
-            )
+    def get_ids(self, type='servo', max_retries=10):
+        if type == 'servo':
+            ids = serial_call_with_retry(self.interface.search_servo_ids,
+                                         max_retries=max_retries)
+        elif type == 'air_board':
+            ids = serial_call_with_retry(self.interface.search_air_board_ids,
+                                         max_retries=max_retries)
+        if ids is None:
             return
-        for jn in self.joint_names:
-            if jn not in self.joint_name_to_id:
-                continue
-            idx = self.joint_name_to_id[jn]
-            if idx in servo_on_states:
-                self.joint_servo_on[jn] = True
-            else:
-                self.joint_servo_on[jn] = False
+        ids = ids.tolist()
+        return ids
 
     def set_fullbody_controller(self, base_namespace):
         self.fullbody_jointnames = []
@@ -476,18 +437,8 @@ def set_fullbody_controller(self, base_namespace):
 
     def set_initial_positions(self, base_namespace):
         initial_positions = {}
-        while True:
-            try:
-                init_av = self.interface.angle_vector()
-                break
-            except serial.serialutil.SerialException:
-                rospy.logerr("[set_initial_positions] Failed to retrieve initial_positions.")
-                rospy.sleep(0.1)
-                continue
-            except Exception as e:
-                rospy.logerr(f"[set_initial_positions] Unexpected error: {e}")
-                rospy.sleep(0.1)
-                continue
+        init_av = serial_call_with_retry(self.interface.angle_vector,
+                                         max_retries=float('inf'))
         for jn in self.joint_names:
             if jn not in self.joint_name_to_id:
                 continue
@@ -506,7 +457,8 @@ def _msg_to_angle_vector_and_servo_ids(self, msg, velocity_control=False):
         angle_vector = []
         for name, angle in zip(msg.name, msg.position):
             if name not in self.joint_name_to_id or (
-                name in self.joint_servo_on and not self.joint_servo_on[name]
+                    self.joint_name_to_id[name] in self.interface.servo_on_states_dict
+                    and self.interface.servo_on_states_dict[self.joint_name_to_id[name]]
             ):
                 continue
             idx = self.joint_name_to_id[name]
@@ -539,11 +491,11 @@ def velocity_command_joint_state_callback(self, msg):
             self._prev_velocity_command, av
         ):
             return
-        try:
-            self.interface.angle_vector(av, servo_ids, velocity=self.wheel_frame_count)
-            self._prev_velocity_command = av
-        except serial.serialutil.SerialException as e:
-            rospy.logerr(f"[velocity_command_joint] {e!s}")
+        ret = self.interface.angle_vector(
+            av, servo_ids, velocity=self.wheel_frame_count)
+        if ret is None:
+            return
+        self._prev_velocity_command = av
 
     def command_joint_state_callback(self, msg):
         if not self.interface.is_opened():
@@ -553,10 +505,8 @@ def command_joint_state_callback(self, msg):
         )
         if len(av) == 0:
             return
-        try:
-            self.interface.angle_vector(av, servo_ids, velocity=self.frame_count)
-        except serial.serialutil.SerialException as e:
-            rospy.logerr(f"[command_joint] {e!s}")
+        serial_call_with_retry(self.interface.angle_vector,
+                               av, servo_ids, velocity=self.frame_count)
 
     def servo_on_off_callback(self, goal):
         if not self.interface.is_opened():
@@ -601,6 +551,8 @@ def adjust_angle_vector_callback(self, goal):
         return self.adjust_angle_vector_server.set_succeeded(AdjustAngleVectorResult())
 
     def publish_stretch(self):
+        if self.stretch_publisher.get_num_connections() == 0:
+            return
         if not self.interface.is_opened():
             return
         # Get current stretch of all servo motors and publish them
@@ -611,17 +563,15 @@ def publish_stretch(self):
                 continue
             joint_names.append(joint_name)
             servo_ids.append(self.joint_name_to_id[joint_name])
-        try:
-            stretch = self.interface.read_stretch(servo_ids=servo_ids)
-        except serial.serialutil.SerialException as e:
-            rospy.logerr(f"[read_stretch] {e!s}")
+        stretch = serial_call_with_retry(self.interface.read_stretch)
+        if stretch is None:
             return
         stretch_msg = Stretch(joint_names=joint_names, stretch=stretch)
         self.stretch_publisher.publish(stretch_msg)
 
     def stretch_callback(self, goal):
         if not self.interface.is_opened():
-            return
+            return self.stretch_server.set_aborted(text="Failed to update stretch")
         if len(goal.joint_names) == 0:
             goal.joint_names = self.joint_names
         joint_names = []
@@ -633,11 +583,12 @@ def stretch_callback(self, goal):
             servo_ids.append(self.joint_name_to_id[joint_name])
         # Send new stretch
         stretch = goal.stretch
-        try:
-            self.interface.send_stretch(value=stretch, servo_ids=servo_ids)
-        except serial.serialutil.SerialException as e:
-            rospy.logerr(f"[send_stretch] {e!s}")
-        # Return result
+        success = serial_call_with_retry(self.interface.send_stretch,
+                                         value=stretch, servo_ids=servo_ids)
+        if success is None:
+            return self.stretch_server.set_aborted(text="Failed to update stretch")
+        # need to wait for stretch value update.
+        rospy.sleep(1.0)
         self.publish_stretch()
         rospy.loginfo(f"Update {joint_names} stretch to {stretch}")
         return self.stretch_server.set_succeeded(StretchResult())
@@ -646,34 +597,33 @@ def publish_pressure(self):
         if not self.interface.is_opened():
             return
         for idx in self.air_board_ids:
-            try:
-                key = f"{idx}"
-                if key not in self._pressure_publisher_dict:
-                    self._pressure_publisher_dict[key] = rospy.Publisher(
-                        self.base_namespace + "/fullbody_controller/pressure/" + key,
-                        std_msgs.msg.Float32,
-                        queue_size=1,
-                    )
-                    self._avg_pressure_publisher_dict[key] = rospy.Publisher(
-                        self.base_namespace
-                        + "/fullbody_controller/average_pressure/"
-                        + key,
-                        std_msgs.msg.Float32,
-                        queue_size=1,
-                    )
-                    # Avoid 'rospy.exceptions.ROSException:
-                    # publish() to a closed topic'
-                    rospy.sleep(0.1)
-                pressure = self.read_pressure_sensor(idx)
-                self._pressure_publisher_dict[key].publish(
-                    std_msgs.msg.Float32(data=pressure)
+            key = f"{idx}"
+            if key not in self._pressure_publisher_dict:
+                self._pressure_publisher_dict[key] = rospy.Publisher(
+                    self.base_namespace + "/fullbody_controller/pressure/" + key,
+                    std_msgs.msg.Float32,
+                    queue_size=1,
                 )
-                # Publish average pressure (noise removed pressure)
-                self._avg_pressure_publisher_dict[key].publish(
-                    std_msgs.msg.Float32(data=self.average_pressure)
+                self._avg_pressure_publisher_dict[key] = rospy.Publisher(
+                    self.base_namespace
+                    + "/fullbody_controller/average_pressure/"
+                    + key,
+                    std_msgs.msg.Float32,
+                    queue_size=1,
                 )
-            except serial.serialutil.SerialException as e:
-                rospy.logerr(f"[publish_pressure] {e!s}")
+                # Avoid 'rospy.exceptions.ROSException:
+                # publish() to a closed topic'
+                rospy.sleep(0.1)
+            pressure = serial_call_with_retry(self.read_pressure_sensor, idx)
+            if pressure is None:
+                continue
+            self._pressure_publisher_dict[key].publish(
+                std_msgs.msg.Float32(data=pressure)
+            )
+            # Publish average pressure (noise removed pressure)
+            self._avg_pressure_publisher_dict[key].publish(
+                std_msgs.msg.Float32(data=self.average_pressure)
+            )
 
     def publish_pressure_control(self):
         for idx in list(self.pressure_control_state.keys()):
@@ -705,17 +655,6 @@ def pressure_control_loop(self, idx, start_pressure, stop_pressure, release):
                 vacuum_on = False
             rospy.sleep(0.1)
 
-    def read_pressure_sensor(self, idx, force=False):
-        while True:
-            try:
-                pressure = self.interface.read_pressure_sensor(idx)
-                self.recent_pressures.append(pressure)
-                return pressure
-            except serial.serialutil.SerialException as e:
-                rospy.logerr(f"[read_pressure_sensor] {e!s}")
-                if force is True:
-                    continue
-
     @property
     def average_pressure(self):
         return sum(self.recent_pressures) / len(self.recent_pressures)
@@ -789,16 +728,17 @@ def pressure_control_callback(self, goal):
         return self.pressure_control_server.set_succeeded(PressureControlResult())
 
     def publish_imu_message(self):
+        if self.publish_imu is False or self.imu_publisher.get_num_connections() == 0:
+            return
         if not self.interface.is_opened():
             return
         msg = sensor_msgs.msg.Imu()
         msg.header.frame_id = self.imu_frame_id
         msg.header.stamp = rospy.Time.now()
-        try:
-            q_wxyz, acc, gyro = self.interface.read_imu_data()
-        except serial.serialutil.SerialException as e:
-            rospy.logerr(f"[publish_imu] {e!s}")
+        q_wxyz_acc_gyro = serial_call_with_retry(self.interface.read_imu_data)
+        if q_wxyz_acc_gyro is None:
             return
+        q_wxyz, acc, gyro = q_wxyz_acc_gyro
         (msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z) = (
             q_wxyz
         )
@@ -811,15 +751,15 @@ def publish_imu_message(self):
         self.imu_publisher.publish(msg)
 
     def publish_sensor_values(self):
+        if self.publish_sensor is False:
+            return
         if not self.interface.is_opened():
             return
         stamp = rospy.Time.now()
         msg = geometry_msgs.msg.WrenchStamped()
         msg.header.stamp = stamp
-        try:
-            sensors = self.interface.all_jointbase_sensors()
-        except serial.serialutil.SerialException as e:
-            rospy.logerr(f"[publish_sensor_values] {e!s}")
+        sensors = serial_call_with_retry(self.interface.all_jointbase_sensors)
+        if sensors is None:
             return
         for sensor in sensors:
             for i in range(4):
@@ -842,29 +782,19 @@ def publish_sensor_values(self):
                     self._sensor_publisher_dict[key].publish(msg)
 
     def publish_battery_voltage_value(self):
-        try:
-            volt = self.interface.battery_voltage()
-        except serial.serialutil.SerialException as e:
-            rospy.logerr(f"[publish_battery_voltage] {e!s}")
+        if self.publish_battery_voltage is False or self.battery_voltage_publisher.get_num_connections() == 0:
             return
-        self.battery_voltage_publisher.publish(std_msgs.msg.Float32(data=volt))
+        battery_voltage = serial_call_with_retry(self.interface.battery_voltage)
+        if battery_voltage is None:
+            return
+        self.battery_voltage_publisher.publish(
+            std_msgs.msg.Float32(data=battery_voltage))
 
     def publish_joint_states(self):
-        try:
-            av = self.interface.angle_vector()
-            torque_vector = self.interface.servo_error()
-        except IndexError as e:
-            rospy.logerr(f"[publish_joint_states] {e!s}")
-            return False
-        except ValueError as e:
-            rospy.logerr(f"[publish_joint_states] {e!s}")
-            return False
-        except serial.serialutil.SerialException as e:
-            rospy.logerr(f"[publish_joint_states] {e!s}")
-            return False
-        except OSError as e:
-            rospy.logerr(f"[publish_joint_states] {e!s}")
-            return False
+        av = serial_call_with_retry(self.interface.angle_vector)
+        torque_vector = serial_call_with_retry(self.interface.servo_error)
+        if av is None or torque_vector is None:
+            return
         msg = JointState()
         msg.header.stamp = rospy.Time.now()
         for name in self.joint_names:
@@ -880,20 +810,30 @@ def publish_joint_states(self):
         return True
 
     def publish_servo_on_off(self):
+        if self.servo_on_off_pub.get_num_connections() == 0:
+            return
         if not self.interface.is_opened():
             return
-        self.check_servo_states()
+
         servo_on_off_msg = ServoOnOff()
-        servo_on_off_msg.joint_names = list(self.joint_servo_on.keys())
-        servo_on_off_msg.servo_on_states = list(
-            self.joint_servo_on.values())
+        for jn in self.joint_names:
+            if jn not in self.joint_name_to_id:
+                continue
+            idx = self.joint_name_to_id[jn]
+            if idx not in self.interface.servo_on_states_dict:
+                continue
+            servo_on_off_msg.joint_names.append(jn)
+            servo_state = self.interface.servo_on_states_dict[idx]
+            servo_on_off_msg.servo_on_states.append(servo_state)
         self.servo_on_off_pub.publish(servo_on_off_msg)
 
     def reinitialize_interface(self):
+        rospy.loginfo('Reinitialize interface.')
         self.unsubscribe()
         self.interface.close()
         self.interface = self.setup_interface()
         self.subscribe()
+        rospy.loginfo('Successfully reinitialized interface.')
 
     def check_success_rate(self):
         # Calculate success rate
@@ -923,7 +863,7 @@ def run(self):
         self.publish_joint_states_attempts = 0
         self.publish_joint_states_successes = 0
         self.last_check_time = rospy.Time.now()
-        check_interval = rospy.get_param('~check_interval', 3)
+        check_board_communication_interval = rospy.get_param('~check_board_communication_interval', 2)
         self.success_rate_threshold = 0.8  # Minimum success rate required
 
         while not rospy.is_shutdown():
@@ -934,17 +874,13 @@ def run(self):
 
             # Check success rate periodically
             current_time = rospy.Time.now()
-            if (current_time - self.last_check_time).to_sec() >= check_interval:
+            if (current_time - self.last_check_time).to_sec() >= check_board_communication_interval:
                 self.check_success_rate()
 
             self.publish_servo_on_off()
-
-            if self.publish_imu and self.imu_publisher.get_num_connections():
-                self.publish_imu_message()
-            if self.publish_sensor:
-                self.publish_sensor_values()
-            if self.publish_battery_voltage:
-                self.publish_battery_voltage_value()
+            self.publish_imu_message()
+            self.publish_sensor_values()
+            self.publish_battery_voltage_value()
             if rospy.get_param("~use_rcb4") is False and self.control_pressure:
                 self.publish_pressure()
                 self.publish_pressure_control()
diff --git a/ros/kxr_controller/python/kxr_controller/serial.py b/ros/kxr_controller/python/kxr_controller/serial.py
new file mode 100644
index 00000000..4c6e2a0c
--- /dev/null
+++ b/ros/kxr_controller/python/kxr_controller/serial.py
@@ -0,0 +1,41 @@
+import rospy
+import serial
+
+
+def serial_call_with_retry(func, *args, max_retries=1, retry_interval=0.1, **kwargs):
+    """Wraps a serial communication function with error handling and retry logic.
+
+    Parameters
+    ----------
+    func : callable
+        The serial communication function to call.
+    *args
+        Positional arguments to pass to the function.
+    max_retries : int, optional
+        Maximum number of retry attempts (default is 3).
+    retry_interval : float, optional
+        Time to wait between retries, in seconds (default is 0.1 seconds).
+    **kwargs
+        Keyword arguments to pass to the function.
+
+    Returns
+    -------
+    Any
+        The return value of the function, if successful.
+        Returns None if all attempts fail.
+
+    Raises
+    ------
+    serial.SerialException, OSError
+        Logs the errors and retries if any exception occurs. Logs an error if all retries fail.
+    """
+    attempts = 0
+    while attempts < max_retries:
+        try:
+            return func(*args, **kwargs)
+        except (serial.SerialException, OSError, ValueError, IndexError) as e:
+            rospy.logerr(f"[{func.__name__}] Error: {e}")
+            attempts += 1
+            rospy.sleep(retry_interval)
+    rospy.logerr(f"[{func.__name__}] Failed after {max_retries} attempts.")
+    return None

From 9d92f9da6df71b203fa802db2fd29dd4f6933faa Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 18:47:28 +0900
Subject: [PATCH 08/18] Retry if control boards connection failed

---
 rcb4/armh7interface.py                        |   9 +-
 .../node_scripts/rcb4_ros_bridge.py           | 490 ++++++++++--------
 .../python/kxr_controller/serial.py           |   8 +-
 3 files changed, 299 insertions(+), 208 deletions(-)

diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py
index 5529567e..af901c36 100644
--- a/rcb4/armh7interface.py
+++ b/rcb4/armh7interface.py
@@ -491,7 +491,7 @@ def adjust_angle_vector(self, servo_ids=None, error_threshold=None):
         # Ignore free servo
         servo_ids = servo_ids[self.reference_angle_vector(servo_ids=servo_ids) != 32768]
         if len(servo_ids) == 0:
-            return
+            return False
 
         # Calculate error threshold[deg]
         if error_threshold is None:
@@ -633,7 +633,7 @@ def _set_trim_vector(self, trim_vector=None, servo_ids=None):
             return
         current_trim_vector = self._trim_servo_vector()
         current_trim_vector[np.array(servo_ids)] = trim_vector
-        self.write_cstruct_slot_v(ServoStruct, "trim", current_trim_vector)
+        return self.write_cstruct_slot_v(ServoStruct, "trim", current_trim_vector)
 
     def trim_vector(self, av=None, servo_ids=None):
         if av is not None:
@@ -795,7 +795,10 @@ def servo_angle_vector(self, servo_ids, servo_vector, velocity=127):
         active_ids = []
         active_angles = []
         for servo_id, angle in zip(servo_ids, servo_vector):
-            if self.servo_on_states_dict.get(servo_id, False) or angle in (32767, 32768):
+            if self.servo_on_states_dict.get(servo_id, False) or angle in (
+                32767,
+                32768,
+            ):
                 # Only include active servos
                 active_ids.append(servo_id)
                 active_angles.append(angle)
diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index 8e9b9fcf..55e0215a 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -140,98 +140,118 @@ def set_robot_description(urdf_path, param_name="robot_description"):
 
 class RCB4ROSBridge:
     def __init__(self):
+        # Set up configuration paths and parameters
+        self.setup_paths_and_params()
+
+        # Set up URDF and Robot Model
+        self.robot_model, self.joint_names = self.setup_urdf_and_model()
+
+        while not rospy.is_shutdown():
+            ret = self.setup_interface_and_servo_parameters()
+            if ret is True:
+                break
+
+        # Set up ROS parameters and controllers
+        self.setup_ros_parameters()
+        self.run_ros_robot_controllers()
+
+        self.setup_publishers_and_servers()
+
+        self.subscribe()
+        rospy.loginfo("RCB4 ROS Bridge initialization completed.")
+
+    def setup_paths_and_params(self):
         self.proc_controller_spawner = None
         self.proc_robot_state_publisher = None
         self.proc_kxr_controller = None
-
         servo_config_path = rospy.get_param("~servo_config_path")
-        self.joint_name_to_id, servo_infos = load_yaml(servo_config_path)
-
-        r = RobotModel()
-        urdf_path = rospy.get_param("~urdf_path", None)
-        tmp_urdf = False
-        if urdf_path is None:
-            urdf_path = make_urdf_file(self.joint_name_to_id)
-            tmp_urdf = True
+        self.joint_name_to_id, self.servo_infos = load_yaml(servo_config_path)
+        self.urdf_path = rospy.get_param("~urdf_path", None)
+        self.use_rcb4 = rospy.get_param("~use_rcb4", False)
+        self.control_pressure = rospy.get_param("~control_pressure", False)
+        self.base_namespace = self.get_base_namespace()
+
+    def setup_urdf_and_model(self):
+        robot_model = RobotModel()
+        if self.urdf_path is None:
+            self.urdf_path = make_urdf_file(self.joint_name_to_id)
             rospy.loginfo("Use temporary URDF")
-        with open(urdf_path) as f:
+        with open(self.urdf_path) as f:
             with no_mesh_load_mode():
-                r.load_urdf_file(f)
-
-        joint_list = [j for j in r.joint_list if j.__class__.__name__ != "FixedJoint"]
-        self.joint_names = [j.name for j in joint_list]
-
-        full_namespace = rospy.get_namespace()
-        last_slash_pos = full_namespace.rfind("/")
-        base_namespace = full_namespace[:last_slash_pos] if last_slash_pos != 0 else ""
-        self.base_namespace = base_namespace
-
+                robot_model.load_urdf_file(f)
+        joint_list = [
+            j for j in robot_model.joint_list if j.__class__.__name__ != "FixedJoint"
+        ]
+        joint_names = [j.name for j in joint_list]
         set_robot_description(
-            urdf_path, param_name=base_namespace + "/robot_description"
+            self.urdf_path, param_name=self.base_namespace + "/robot_description"
         )
-        if tmp_urdf:
-            os.remove(urdf_path)
+        if self.urdf_path is None:
+            os.remove(self.urdf_path)
+        return robot_model, joint_names
 
+    def setup_ros_parameters(self):
+        """Configure joint state and full body controllers."""
         set_joint_state_controler()
+        self.set_fullbody_controller()
+
+    def setup_publishers_and_servers(self):
+        """Set up ROS publishers and action servers."""
         self.current_joint_states_pub = rospy.Publisher(
-            base_namespace + "/current_joint_states", JointState, queue_size=1
+            self.base_namespace + "/current_joint_states", JointState, queue_size=1
         )
+
         # Publish servo state like joint_trajectory_controller
         # https://wiki.ros.org/joint_trajectory_controller#Published_Topics
         self.servo_on_off_pub = rospy.Publisher(
-            base_namespace
-            + "/fullbody_controller/servo_on_off_real_interface"
-            + "/state",
+            self.base_namespace
+            + "/fullbody_controller/servo_on_off_real_interface/state",
             ServoOnOff,
             queue_size=1,
         )
 
-        self.use_rcb4 = rospy.get_param("~use_rcb4", False)
-        self.interface = self.setup_interface()
-        self._prev_velocity_command = None
-
-        self.srv = Server(Config, self.config_callback)
+        self.cancel_motion_pub = rospy.Publisher(
+            self.base_namespace
+            + "/fullbody_controller/follow_joint_trajectory"
+            + "/cancel",
+            GoalID,
+            queue_size=1,
+        )
 
-        # set servo ids to rosparam
-        rospy.set_param(
-            base_namespace + "/servo_ids", self.get_ids(type='servo')
+        self.publish_imu = rospy.get_param("~publish_imu", True) and not self.use_rcb4
+        self.publish_sensor = (
+            rospy.get_param("~publish_sensor", False) and not self.use_rcb4
+        )
+        self.publish_battery_voltage = (
+            rospy.get_param("~publish_battery_voltage", True) and not self.use_rcb4
         )
 
-        wheel_servo_sorted_ids = []
-        trim_vector_servo_ids = []
-        trim_vector_offset = []
-        for _, info in servo_infos.items():
-            if isinstance(info, int):
-                continue
-            servo_id = info["id"]
-            direction = info.get("direction", 1)
-            offset = info.get("offset", 0)
-            if "type" in info and info["type"] == "continuous":
-                wheel_servo_sorted_ids.append(servo_id)
-            idx = self.interface.servo_id_to_index(servo_id)
-            if idx is None:
-                continue
-            self.interface._joint_to_actuator_matrix[idx, idx] = (
-                direction * self.interface._joint_to_actuator_matrix[idx, idx]
+        if self.publish_imu:
+            self.imu_frame_id = rospy.get_param(
+                "~imu_frame_id",
+                self.base_namespace + "/" + self.robot_model.root_link.name,
+            )
+            self.imu_publisher = rospy.Publisher(
+                self.base_namespace + "/imu", sensor_msgs.msg.Imu, queue_size=1
+            )
+        if self.publish_sensor:
+            self._sensor_publisher_dict = {}
+        if self.publish_battery_voltage:
+            self.battery_voltage_publisher = rospy.Publisher(
+                self.base_namespace + "/battery_voltage",
+                std_msgs.msg.Float32,
+                queue_size=1,
             )
-            trim_vector_servo_ids.append(servo_id)
-            trim_vector_offset.append(direction * offset)
-        if self.interface.__class__.__name__ != "RCB4Interface":
-            serial_call_with_retry(self.interface.trim_vector,
-                                   trim_vector_offset, trim_vector_servo_ids,
-                                   max_retries=10)
-        if self.interface.wheel_servo_sorted_ids is None:
-            self.interface.wheel_servo_sorted_ids = wheel_servo_sorted_ids
-
-        self.set_fullbody_controller(base_namespace)
-        self.set_initial_positions(base_namespace)
 
-        rospy.loginfo("run kxr_controller")
-        self.proc_kxr_controller = run_kxr_controller(namespace=base_namespace)
-        self.subscribe()
+        # Action servers for servo control
+        self.setup_action_servers()
+        self.srv = Server(Config, self.config_callback)
 
+    def setup_action_servers(self):
+        """Set up action servers for controlling servos and pressure."""
+        # Servo on/off action server
         self.servo_on_off_server = actionlib.SimpleActionServer(
-            base_namespace + "/fullbody_controller/servo_on_off_real_interface",
+            self.base_namespace + "/fullbody_controller/servo_on_off_real_interface",
             ServoOnOffAction,
             execute_cb=self.servo_on_off_callback,
             auto_start=False,
@@ -240,8 +260,9 @@ def __init__(self):
         rospy.sleep(0.1)
         self.servo_on_off_server.start()
 
+        # Adjust angle vector action server
         self.adjust_angle_vector_server = actionlib.SimpleActionServer(
-            base_namespace + "/fullbody_controller/adjust_angle_vector_interface",
+            self.base_namespace + "/fullbody_controller/adjust_angle_vector_interface",
             AdjustAngleVectorAction,
             execute_cb=self.adjust_angle_vector_callback,
             auto_start=False,
@@ -249,77 +270,124 @@ def __init__(self):
         # Avoid 'rospy.exceptions.ROSException: publish() to a closed topic'
         rospy.sleep(0.1)
         self.adjust_angle_vector_server.start()
-        self.cancel_motion_pub = rospy.Publisher(
-            base_namespace
-            + "/fullbody_controller/follow_joint_trajectory"
-            + "/cancel",
-            GoalID,
+
+        # Additional action servers based on configuration
+        if not self.use_rcb4:
+            self.setup_stretch_and_pressure_control_servers()
+
+    def setup_stretch_and_pressure_control_servers(self):
+        """Configure stretch and pressure control action servers if enabled."""
+        self.stretch_server = actionlib.SimpleActionServer(
+            self.base_namespace + "/fullbody_controller/stretch_interface",
+            StretchAction,
+            execute_cb=self.stretch_callback,
+            auto_start=False,
+        )
+        rospy.sleep(0.1)
+        self.stretch_server.start()
+
+        self.stretch_publisher = rospy.Publisher(
+            self.base_namespace + "/fullbody_controller/stretch",
+            Stretch,
             queue_size=1,
+            latch=True,
         )
+        rospy.sleep(0.1)
+        while not rospy.is_shutdown():
+            rospy.loginfo("Try to publish stretch values.")
+            ret = self.publish_stretch()
+            if ret is True:
+                break
+            rospy.sleep(0.1)
 
-        # TODO(someone) support rcb-4 miniboard
-        if not rospy.get_param("~use_rcb4"):
-            # Stretch
-            self.stretch_server = actionlib.SimpleActionServer(
-                base_namespace + "/fullbody_controller/stretch_interface",
-                StretchAction,
-                execute_cb=self.stretch_callback,
+        if self.control_pressure:
+            self.pressure_control_server = actionlib.SimpleActionServer(
+                self.base_namespace + "/fullbody_controller/pressure_control_interface",
+                PressureControlAction,
+                execute_cb=self.pressure_control_callback,
                 auto_start=False,
             )
-            # Avoid 'rospy.exceptions.ROSException:
-            # publish() to a closed topic'
             rospy.sleep(0.1)
-            self.stretch_server.start()
-            self.stretch_publisher = rospy.Publisher(
-                base_namespace + "/fullbody_controller/stretch",
-                Stretch,
+            self.pressure_control_server.start()
+
+            self.pressure_control_pub = rospy.Publisher(
+                self.base_namespace
+                + "/fullbody_controller/pressure_control_interface"
+                + "/state",
+                PressureControl,
                 queue_size=1,
-                latch=True,
             )
-            # Avoid 'rospy.exceptions.ROSException:
-            # publish() to a closed topic'
-            rospy.sleep(0.1)
-            self.publish_stretch()
-            # Pressure control
-            self.control_pressure = rospy.get_param("~control_pressure", False)
-            if self.control_pressure is True:
-                self.pressure_control_thread = None
-                self.pressure_control_server = actionlib.SimpleActionServer(
-                    base_namespace + "/fullbody_controller/pressure_control_interface",
-                    PressureControlAction,
-                    execute_cb=self.pressure_control_callback,
-                    auto_start=False,
-                )
-                # Avoid 'rospy.exceptions.ROSException:
-                # publish() to a closed topic'
-                rospy.sleep(0.1)
-                self.pressure_control_server.start()
-                # Publish state topic like joint_trajectory_controller
-                # https://wiki.ros.org/joint_trajectory_controller#Published_Topics
-                self.pressure_control_pub = rospy.Publisher(
-                    base_namespace
-                    + "/fullbody_controller/pressure_control_interface"
-                    + "/state",
-                    PressureControl,
-                    queue_size=1,
-                )
-                self.air_board_ids = self.get_ids(type='air_board')
-                rospy.set_param(
-                    base_namespace + "/air_board_ids",
-                    self.air_board_ids
+
+            rospy.set_param(self.base_namespace + "/air_board_ids", self.air_board_ids)
+            self.pressure_control_state = {}
+            for idx in self.air_board_ids:
+                self.pressure_control_state[f"{idx}"] = {}
+                self.pressure_control_state[f"{idx}"]["start_pressure"] = 0
+                self.pressure_control_state[f"{idx}"]["stop_pressure"] = 0
+                self.pressure_control_state[f"{idx}"]["release"] = True
+            self._pressure_publisher_dict = {}
+            self._avg_pressure_publisher_dict = {}
+            # Record 1 seconds pressure data.
+            hz = rospy.get_param(self.base_namespace + "/control_loop_rate", 20)
+            self.recent_pressures = deque([], maxlen=1 * int(hz))
+
+    def setup_interface_and_servo_parameters(self):
+        self.interface = self.setup_interface()
+
+        def log_error_and_close_interface(function_name):
+            msg = f"Failed to {function_name}. "
+            msg += "Control board is switch off or cable is disconnected?"
+            rospy.logerr(msg)
+            self.interface.close()
+            return False
+
+        wheel_servo_sorted_ids = []
+        trim_vector_servo_ids = []
+        trim_vector_offset = []
+        for _, info in self.servo_infos.items():
+            if isinstance(info, int):
+                continue
+            servo_id = info["id"]
+            direction = info.get("direction", 1)
+            offset = info.get("offset", 0)
+            if "type" in info and info["type"] == "continuous":
+                wheel_servo_sorted_ids.append(servo_id)
+            idx = self.interface.servo_id_to_index(servo_id)
+            if idx is None:
+                continue
+            self.interface._joint_to_actuator_matrix[idx, idx] = (
+                direction * self.interface._joint_to_actuator_matrix[idx, idx]
+            )
+            trim_vector_servo_ids.append(servo_id)
+            trim_vector_offset.append(direction * offset)
+        if self.interface.__class__.__name__ != "RCB4Interface":
+            if len(trim_vector_offset) > 0:
+                ret = serial_call_with_retry(
+                    self.interface.trim_vector,
+                    trim_vector_offset,
+                    trim_vector_servo_ids,
+                    max_retries=10,
                 )
-                self.pressure_control_state = {}
-                for idx in self.air_board_ids:
-                    self.pressure_control_state[f"{idx}"] = {}
-                    self.pressure_control_state[f"{idx}"]["start_pressure"] = 0
-                    self.pressure_control_state[f"{idx}"]["stop_pressure"] = 0
-                    self.pressure_control_state[f"{idx}"]["release"] = True
-                self._pressure_publisher_dict = {}
-                self._avg_pressure_publisher_dict = {}
-                # Record 1 seconds pressure data.
-                hz = rospy.get_param(self.base_namespace + "/control_loop_rate", 20)
-                self.recent_pressures = deque([], maxlen=1 * int(hz))
+                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
 
+        # set servo ids to rosparam
+        servo_ids = self.get_ids(type="servo")
+        if servo_ids is None:
+            return log_error_and_close_interface("get initial servo ids")
+        rospy.set_param(self.base_namespace + "/servo_ids", servo_ids)
+        ret = self.set_initial_positions()
+        if ret is False:
+            return log_error_and_close_interface("get initial angle vector")
+        if self.control_pressure:
+            self.air_board_ids = self.get_ids(type="air_board")
+            if self.air_board_ids is None:
+                return log_error_and_close_interface("get air board ids")
+        return True
+
+    def run_ros_robot_controllers(self):
         self.proc_controller_spawner = subprocess.Popen(
             [
                 f'/opt/ros/{os.environ["ROS_DISTRO"]}/bin/rosrun',
@@ -328,30 +396,11 @@ def __init__(self):
             ]
             + ["joint_state_controller", "fullbody_controller"]
         )
-        self.proc_robot_state_publisher = run_robot_state_publisher(base_namespace)
-
-        self.publish_imu = rospy.get_param("~publish_imu", True)
-        self.publish_sensor = rospy.get_param("~publish_sensor", False)
-        self.publish_battery_voltage = rospy.get_param("~publish_battery_voltage", True)
-        if self.interface.__class__.__name__ == "RCB4Interface":
-            self.publish_imu = False
-            self.publish_sensor = False
-        if self.publish_imu:
-            self.imu_frame_id = rospy.get_param(
-                "~imu_frame_id", base_namespace + "/" + r.root_link.name
-            )
-            self.imu_publisher = rospy.Publisher(
-                base_namespace + "/imu", sensor_msgs.msg.Imu, queue_size=1
-            )
-        if self.publish_sensor:
-            self._sensor_publisher_dict = {}
-        if self.publish_battery_voltage:
-            self.battery_voltage_publisher = rospy.Publisher(
-                base_namespace + "/battery_voltage", std_msgs.msg.Float32, queue_size=1
-            )
+        self.proc_robot_state_publisher = run_robot_state_publisher(self.base_namespace)
+        self.proc_kxr_controller = run_kxr_controller(namespace=self.base_namespace)
 
     def setup_interface(self):
-        rospy.loginfo('Try to connect servo control boards.')
+        rospy.loginfo("Try to connect servo control boards.")
         while not rospy.is_shutdown():
             rospy.loginfo("Waiting for the port to become available")
             try:
@@ -373,9 +422,7 @@ def setup_interface(self):
         sys.exit(1)
 
     def get_base_namespace(self):
-        """Return the clean namespace for the node.
-
-        """
+        """Return the clean namespace for the node."""
         full_namespace = rospy.get_namespace()
         last_slash_pos = full_namespace.rfind("/")
         return full_namespace[:last_slash_pos] if last_slash_pos != 0 else ""
@@ -412,33 +459,36 @@ def config_callback(self, config, level):
         self.wheel_frame_count = config.wheel_frame_count
         return config
 
-    def get_ids(self, type='servo', max_retries=10):
-        if type == 'servo':
-            ids = serial_call_with_retry(self.interface.search_servo_ids,
-                                         max_retries=max_retries)
-        elif type == 'air_board':
-            ids = serial_call_with_retry(self.interface.search_air_board_ids,
-                                         max_retries=max_retries)
+    def get_ids(self, type="servo", max_retries=10):
+        if type == "servo":
+            ids = serial_call_with_retry(
+                self.interface.search_servo_ids, max_retries=max_retries
+            )
+        elif type == "air_board":
+            ids = serial_call_with_retry(
+                self.interface.search_air_board_ids, max_retries=max_retries
+            )
         if ids is None:
             return
         ids = ids.tolist()
         return ids
 
-    def set_fullbody_controller(self, base_namespace):
-        self.fullbody_jointnames = []
+    def set_fullbody_controller(self):
+        fullbody_jointnames = []
         for jn in self.joint_names:
             if jn not in self.joint_name_to_id:
                 continue
             servo_id = self.joint_name_to_id[jn]
             if servo_id in self.interface.wheel_servo_sorted_ids:
                 continue
-            self.fullbody_jointnames.append(jn)
-        set_fullbody_controller(self.fullbody_jointnames)
+            fullbody_jointnames.append(jn)
+        set_fullbody_controller(fullbody_jointnames)
 
-    def set_initial_positions(self, base_namespace):
+    def set_initial_positions(self):
         initial_positions = {}
-        init_av = serial_call_with_retry(self.interface.angle_vector,
-                                         max_retries=float('inf'))
+        init_av = serial_call_with_retry(self.interface.angle_vector, max_retries=10)
+        if init_av is None:
+            return False
         for jn in self.joint_names:
             if jn not in self.joint_name_to_id:
                 continue
@@ -449,17 +499,21 @@ def set_initial_positions(self, base_namespace):
             if idx is None:
                 continue
             initial_positions[jn] = float(np.deg2rad(init_av[idx]))
-        set_initial_position(initial_positions, namespace=base_namespace)
+        set_initial_position(initial_positions, namespace=self.base_namespace)
+        return True
+
+    def _valid_joint(self, joint_name):
+        return joint_name not in self.joint_name_to_id or (
+            self.joint_name_to_id[joint_name] in self.interface.servo_on_states_dict
+            and self.interface.servo_on_states_dict[self.joint_name_to_id[joint_name]]
+        )
 
     def _msg_to_angle_vector_and_servo_ids(self, msg, velocity_control=False):
         used_servo_id = {}
         servo_ids = []
         angle_vector = []
         for name, angle in zip(msg.name, msg.position):
-            if name not in self.joint_name_to_id or (
-                    self.joint_name_to_id[name] in self.interface.servo_on_states_dict
-                    and self.interface.servo_on_states_dict[self.joint_name_to_id[name]]
-            ):
+            if not self._valid_joint(name):
                 continue
             idx = self.joint_name_to_id[name]
             if velocity_control:
@@ -487,12 +541,15 @@ def velocity_command_joint_state_callback(self, msg):
         )
         if len(av) == 0:
             return
+        if not hasattr(self, "_prev_velocity_command"):
+            self._prev_velocity_command = None
         if self._prev_velocity_command is not None and np.allclose(
             self._prev_velocity_command, av
         ):
             return
         ret = self.interface.angle_vector(
-            av, servo_ids, velocity=self.wheel_frame_count)
+            av, servo_ids, velocity=self.wheel_frame_count
+        )
         if ret is None:
             return
         self._prev_velocity_command = av
@@ -505,12 +562,16 @@ def command_joint_state_callback(self, msg):
         )
         if len(av) == 0:
             return
-        serial_call_with_retry(self.interface.angle_vector,
-                               av, servo_ids, velocity=self.frame_count)
+        serial_call_with_retry(
+            self.interface.angle_vector, av, servo_ids, velocity=self.frame_count
+        )
 
     def servo_on_off_callback(self, goal):
         if not self.interface.is_opened():
-            return
+            return self.servo_on_off_server.set_aborted(
+                text="Failed to call servo on off. "
+                + "Control board is switch off or cable is disconnected?"
+            )
         servo_vector = []
         servo_ids = []
         for joint_name, servo_on in zip(goal.joint_names, goal.servo_on_states):
@@ -521,40 +582,51 @@ def servo_on_off_callback(self, goal):
                 servo_vector.append(32767)
             else:
                 servo_vector.append(32768)
-        try:
-            self.interface.servo_angle_vector(servo_ids, servo_vector, velocity=1)
-        except serial.serialutil.SerialException as e:
-            rospy.logerr(f"[servo_on_off] {e!s}")
+
+        ret = serial_call_with_retry(
+            self.interface.servo_angle_vector,
+            servo_ids,
+            servo_vector,
+            velocity=1,
+            max_retries=10,
+        )
+        if ret is None:
+            return self.servo_on_off_server.set_aborted(
+                text="Failed to call servo on off. "
+                + "Control board is switch off or cable is disconnected?"
+            )
         return self.servo_on_off_server.set_succeeded(ServoOnOffResult())
 
     def adjust_angle_vector_callback(self, goal):
         if not self.interface.is_opened():
-            return
+            return self.adjust_angle_vector_server.set_aborted(
+                text="Failed to adjust angle vector"
+            )
         servo_ids = []
         error_thresholds = []
         for joint_name, error_threshold in zip(goal.joint_names, goal.error_threshold):
-            if joint_name not in self.joint_name_to_id:
+            if not self._valid_joint(joint_name):
                 continue
             servo_ids.append(self.joint_name_to_id[joint_name])
             error_thresholds.append(error_threshold)
-        try:
-            adjust = self.interface.adjust_angle_vector(
-                servo_ids=servo_ids,
-                error_threshold=np.array(error_thresholds, dtype=np.float32),
+        adjust = serial_call_with_retry(
+            self.interface.adjust_angle_vector,
+            servo_ids=servo_ids,
+            error_threshold=np.array(error_thresholds, dtype=np.float32),
+        )
+        if adjust is None:
+            return self.adjust_angle_vector_server.set_aborted(
+                text="Failed to adjust angle vector"
             )
-            # If adjustment occurs, cancel motion via follow joint trajectory
-            if adjust is True:
-                self.cancel_motion_pub.publish(GoalID())
-                rospy.logwarn("Stop motion by sending follow joint trajectory cancel.")
-        except serial.serialutil.SerialException as e:
-            rospy.logerr(f"[adjust_angle_vector] {e!s}")
+        # If adjustment occurs, cancel motion via follow joint trajectory
+        if adjust is True:
+            self.cancel_motion_pub.publish(GoalID())
+            rospy.logwarn("Stop motion by sending follow joint trajectory cancel.")
         return self.adjust_angle_vector_server.set_succeeded(AdjustAngleVectorResult())
 
     def publish_stretch(self):
-        if self.stretch_publisher.get_num_connections() == 0:
-            return
         if not self.interface.is_opened():
-            return
+            return False
         # Get current stretch of all servo motors and publish them
         joint_names = []
         servo_ids = []
@@ -565,9 +637,10 @@ def publish_stretch(self):
             servo_ids.append(self.joint_name_to_id[joint_name])
         stretch = serial_call_with_retry(self.interface.read_stretch)
         if stretch is None:
-            return
+            return False
         stretch_msg = Stretch(joint_names=joint_names, stretch=stretch)
         self.stretch_publisher.publish(stretch_msg)
+        return True
 
     def stretch_callback(self, goal):
         if not self.interface.is_opened():
@@ -583,8 +656,9 @@ def stretch_callback(self, goal):
             servo_ids.append(self.joint_name_to_id[joint_name])
         # Send new stretch
         stretch = goal.stretch
-        success = serial_call_with_retry(self.interface.send_stretch,
-                                         value=stretch, servo_ids=servo_ids)
+        success = serial_call_with_retry(
+            self.interface.send_stretch, value=stretch, servo_ids=servo_ids
+        )
         if success is None:
             return self.stretch_server.set_aborted(text="Failed to update stretch")
         # need to wait for stretch value update.
@@ -782,13 +856,17 @@ def publish_sensor_values(self):
                     self._sensor_publisher_dict[key].publish(msg)
 
     def publish_battery_voltage_value(self):
-        if self.publish_battery_voltage is False or self.battery_voltage_publisher.get_num_connections() == 0:
+        if (
+            self.publish_battery_voltage is False
+            or self.battery_voltage_publisher.get_num_connections() == 0
+        ):
             return
         battery_voltage = serial_call_with_retry(self.interface.battery_voltage)
         if battery_voltage is None:
             return
         self.battery_voltage_publisher.publish(
-            std_msgs.msg.Float32(data=battery_voltage))
+            std_msgs.msg.Float32(data=battery_voltage)
+        )
 
     def publish_joint_states(self):
         av = serial_call_with_retry(self.interface.angle_vector)
@@ -828,12 +906,12 @@ def publish_servo_on_off(self):
         self.servo_on_off_pub.publish(servo_on_off_msg)
 
     def reinitialize_interface(self):
-        rospy.loginfo('Reinitialize interface.')
+        rospy.loginfo("Reinitialize interface.")
         self.unsubscribe()
         self.interface.close()
         self.interface = self.setup_interface()
         self.subscribe()
-        rospy.loginfo('Successfully reinitialized interface.')
+        rospy.loginfo("Successfully reinitialized interface.")
 
     def check_success_rate(self):
         # Calculate success rate
@@ -863,7 +941,9 @@ def run(self):
         self.publish_joint_states_attempts = 0
         self.publish_joint_states_successes = 0
         self.last_check_time = rospy.Time.now()
-        check_board_communication_interval = rospy.get_param('~check_board_communication_interval', 2)
+        check_board_communication_interval = rospy.get_param(
+            "~check_board_communication_interval", 2
+        )
         self.success_rate_threshold = 0.8  # Minimum success rate required
 
         while not rospy.is_shutdown():
@@ -874,7 +954,9 @@ def run(self):
 
             # Check success rate periodically
             current_time = rospy.Time.now()
-            if (current_time - self.last_check_time).to_sec() >= check_board_communication_interval:
+            if (
+                current_time - self.last_check_time
+            ).to_sec() >= check_board_communication_interval:
                 self.check_success_rate()
 
             self.publish_servo_on_off()
diff --git a/ros/kxr_controller/python/kxr_controller/serial.py b/ros/kxr_controller/python/kxr_controller/serial.py
index 4c6e2a0c..9e7e7982 100644
--- a/ros/kxr_controller/python/kxr_controller/serial.py
+++ b/ros/kxr_controller/python/kxr_controller/serial.py
@@ -33,7 +33,13 @@ def serial_call_with_retry(func, *args, max_retries=1, retry_interval=0.1, **kwa
     while attempts < max_retries:
         try:
             return func(*args, **kwargs)
-        except (serial.SerialException, OSError, ValueError, IndexError) as e:
+        except (
+            serial.SerialException,
+            OSError,
+            ValueError,
+            IndexError,
+            RuntimeError,
+        ) as e:
             rospy.logerr(f"[{func.__name__}] Error: {e}")
             attempts += 1
             rospy.sleep(retry_interval)

From f8cfab694c79644360516d7af0c85423ba271a39 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 20:37:22 +0900
Subject: [PATCH 09/18] Overwrite servo command after servo off

---
 .../node_scripts/rcb4_ros_bridge.py           | 59 ++++++++++++++++---
 .../python/kxr_controller/kxr_interface.py    | 10 ----
 .../node_scripts/urdf_model_server.py         |  5 +-
 ros/kxreus/euslisp/kxr-interface.l            |  9 ---
 ros/kxreus/node_scripts/eus_model_server.py   |  4 +-
 5 files changed, 56 insertions(+), 31 deletions(-)

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index 55e0215a..d8718a3f 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -14,6 +14,7 @@
 from actionlib_msgs.msg import GoalID
 from dynamic_reconfigure.server import Server
 import geometry_msgs.msg
+from trajectory_msgs.msg import JointTrajectoryPoint
 from kxr_controller.cfg import KXRParameteresConfig as Config
 from kxr_controller.msg import AdjustAngleVectorAction
 from kxr_controller.msg import AdjustAngleVectorResult
@@ -26,6 +27,8 @@
 from kxr_controller.msg import Stretch
 from kxr_controller.msg import StretchAction
 from kxr_controller.msg import StretchResult
+from control_msgs.msg import FollowJointTrajectoryAction
+from control_msgs.msg import FollowJointTrajectoryGoal
 import numpy as np
 import rospy
 import sensor_msgs.msg
@@ -140,6 +143,7 @@ def set_robot_description(urdf_path, param_name="robot_description"):
 
 class RCB4ROSBridge:
     def __init__(self):
+        self._during_servo_off = False
         # Set up configuration paths and parameters
         self.setup_paths_and_params()
 
@@ -158,6 +162,13 @@ def __init__(self):
         self.setup_publishers_and_servers()
 
         self.subscribe()
+
+        self.traj_action_client = actionlib.SimpleActionClient(
+            self.base_namespace + "/fullbody_controller/follow_joint_trajectory",
+            FollowJointTrajectoryAction,
+        )
+        self.traj_action_client.wait_for_server()
+
         rospy.loginfo("RCB4 ROS Bridge initialization completed.")
 
     def setup_paths_and_params(self):
@@ -244,11 +255,12 @@ def setup_publishers_and_servers(self):
             )
 
         # Action servers for servo control
-        self.setup_action_servers()
+        self.setup_action_servers_and_clients()
         self.srv = Server(Config, self.config_callback)
 
-    def setup_action_servers(self):
+    def setup_action_servers_and_clients(self):
         """Set up action servers for controlling servos and pressure."""
+
         # Servo on/off action server
         self.servo_on_off_server = actionlib.SimpleActionServer(
             self.base_namespace + "/fullbody_controller/servo_on_off_real_interface",
@@ -474,15 +486,15 @@ def get_ids(self, type="servo", max_retries=10):
         return ids
 
     def set_fullbody_controller(self):
-        fullbody_jointnames = []
+        self.fullbody_jointnames = []
         for jn in self.joint_names:
             if jn not in self.joint_name_to_id:
                 continue
             servo_id = self.joint_name_to_id[jn]
             if servo_id in self.interface.wheel_servo_sorted_ids:
                 continue
-            fullbody_jointnames.append(jn)
-        set_fullbody_controller(fullbody_jointnames)
+            self.fullbody_jointnames.append(jn)
+        set_fullbody_controller(self.fullbody_jointnames)
 
     def set_initial_positions(self):
         initial_positions = {}
@@ -534,7 +546,7 @@ def _msg_to_angle_vector_and_servo_ids(self, msg, velocity_control=False):
         return angle_vector[valid_indices], servo_ids[valid_indices]
 
     def velocity_command_joint_state_callback(self, msg):
-        if not self.interface.is_opened():
+        if not self.interface.is_opened() or self._during_servo_off:
             return
         av, servo_ids = self._msg_to_angle_vector_and_servo_ids(
             msg, velocity_control=True
@@ -555,7 +567,7 @@ def velocity_command_joint_state_callback(self, msg):
         self._prev_velocity_command = av
 
     def command_joint_state_callback(self, msg):
-        if not self.interface.is_opened():
+        if not self.interface.is_opened() or self._during_servo_off:
             return
         av, servo_ids = self._msg_to_angle_vector_and_servo_ids(
             msg, velocity_control=False
@@ -572,6 +584,12 @@ def servo_on_off_callback(self, goal):
                 text="Failed to call servo on off. "
                 + "Control board is switch off or cable is disconnected?"
             )
+
+        # Publish current joint position to follow_joint_trajectory
+        # to prevent sudden movements.
+        self.cancel_motion_pub.publish(GoalID())  # Ensure no active motion
+        rospy.sleep(0.1)  # Slight delay for smooth transition
+
         servo_vector = []
         servo_ids = []
         for joint_name, servo_on in zip(goal.joint_names, goal.servo_on_states):
@@ -583,6 +601,7 @@ def servo_on_off_callback(self, goal):
             else:
                 servo_vector.append(32768)
 
+        self._during_servo_off = True
         ret = serial_call_with_retry(
             self.interface.servo_angle_vector,
             servo_ids,
@@ -595,6 +614,32 @@ def servo_on_off_callback(self, goal):
                 text="Failed to call servo on off. "
                 + "Control board is switch off or cable is disconnected?"
             )
+
+        av = self.interface.angle_vector()
+        joint_names = []
+        positions = []
+        for joint_name in self.fullbody_jointnames:
+            angle = 0
+            if joint_name in self.joint_name_to_id:
+                servo_id = self.joint_name_to_id[joint_name]
+                idx = self.interface.servo_id_to_index(servo_id)
+                if idx is not None:
+                    angle = np.deg2rad(av[idx])
+            joint_names.append(joint_name)
+            positions.append(angle)
+        # Create JointTrajectoryGoal to set current position on follow_joint_trajectory
+        trajectory_goal = FollowJointTrajectoryGoal()
+        trajectory_goal.trajectory.joint_names = joint_names
+        point = JointTrajectoryPoint()
+        point.positions = positions
+        point.time_from_start = rospy.Duration(
+            0.01
+        )  # Short duration for immediate application
+        trajectory_goal.trajectory.points = [point]
+        # Initialize action client and wait for server
+        self.traj_action_client.send_goal(trajectory_goal)
+        self.traj_action_client.wait_for_result()  # Wait for trajectory to complete
+        self._during_servo_off = False
         return self.servo_on_off_server.set_succeeded(ServoOnOffResult())
 
     def adjust_angle_vector_callback(self, goal):
diff --git a/ros/kxr_controller/python/kxr_controller/kxr_interface.py b/ros/kxr_controller/python/kxr_controller/kxr_interface.py
index 32b13409..0d788ba7 100644
--- a/ros/kxr_controller/python/kxr_controller/kxr_interface.py
+++ b/ros/kxr_controller/python/kxr_controller/kxr_interface.py
@@ -82,16 +82,6 @@ def servo_on(self, joint_names=None):
         if client.get_state() == actionlib_msgs.msg.GoalStatus.ACTIVE:
             client.cancel_goal()
             client.wait_for_result(timeout=rospy.Duration(10))
-        av = self.angle_vector()
-        while not rospy.is_shutdown():
-            self.angle_vector(av, 1.0)
-            self.wait_interpolation()
-            if (
-                self.controller_table[self.controller_type][0].get_state()
-                == actionlib_msgs.msg.GoalStatus.SUCCEEDED
-            ):
-                break
-            rospy.logwarn("waiting for the angle vector to be sent.")
         goal.joint_names = joint_names
         goal.servo_on_states = [True] * len(joint_names)
         client.send_goal(goal)
diff --git a/ros/kxr_models/node_scripts/urdf_model_server.py b/ros/kxr_models/node_scripts/urdf_model_server.py
index 11114be8..ba320ec8 100755
--- a/ros/kxr_models/node_scripts/urdf_model_server.py
+++ b/ros/kxr_models/node_scripts/urdf_model_server.py
@@ -36,8 +36,9 @@ def run(self):
                 continue
 
             previous_urdf = urdf
-            with tempfile.NamedTemporaryFile("w", suffix='.urdf',
-                                             delete=False) as temp_file:
+            with tempfile.NamedTemporaryFile(
+                "w", suffix=".urdf", delete=False
+            ) as temp_file:
                 temp_file.write(urdf)
                 urdf_path = temp_file.name
 
diff --git a/ros/kxreus/euslisp/kxr-interface.l b/ros/kxreus/euslisp/kxr-interface.l
index b9493efe..a64111b9 100644
--- a/ros/kxreus/euslisp/kxr-interface.l
+++ b/ros/kxreus/euslisp/kxr-interface.l
@@ -109,15 +109,6 @@
    (when (null names)
      (setq names joint-names))
    (let* (goal result)
-     ;; send current angle-vector
-     (while (ros::ok)
-       (send self :angle-vector (send self :state :potentio-vector) 1.0)
-       (send self :wait-interpolation)
-       (setq tmp-result (send-all controller-actions :get-result))
-       (when (every #'identity (send-all controller-actions :get-result))
-         (return))
-       (ros::ros-warn "waiting for the angle vector to be sent."))
-
      (setq goal (instance kxr_controller::ServoOnOffGoal :init))
      (send goal :joint_names names)
      (send goal :servo_on_states (make-array (length names) :initial-element t))
diff --git a/ros/kxreus/node_scripts/eus_model_server.py b/ros/kxreus/node_scripts/eus_model_server.py
index 8078e1f7..10bc11b0 100644
--- a/ros/kxreus/node_scripts/eus_model_server.py
+++ b/ros/kxreus/node_scripts/eus_model_server.py
@@ -94,9 +94,7 @@ def run(self):
                     self.robot_model = self.load_robot_model(urdf_content)
                     self.previous_md5sum = md5sum
 
-                self.save_eus_model(
-                    md5sum, urdf_content, joint_group_description
-                )
+                self.save_eus_model(md5sum, urdf_content, joint_group_description)
                 robot_name = self.robot_model.urdf_robot_model.name
 
                 rospy.set_param(self.namespace + "/eus_robot_name", robot_name)

From 2c2a59614e174d35a2f2f5d43a9d48ed7e1d0e4d Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 20:45:47 +0900
Subject: [PATCH 10/18] Use pyproject.toml for ruff instead of setup.cfg

---
 pyproject.toml                                | 45 +++++++++++++++++++
 rcb4/armh7interface.py                        | 24 +++-------
 rcb4/data/__init__.py                         |  4 +-
 rcb4/rcb4interface.py                         |  8 +---
 rcb4/usb_utils.py                             |  4 +-
 .../node_scripts/rcb4_ros_bridge.py           | 25 ++++-------
 .../python/kxr_controller/kxr_interface.py    |  4 +-
 .../node_scripts/http_server_node.py          |  4 +-
 .../node_scripts/urdf_model_server.py         |  1 +
 ros/kxr_models/scripts/get_urdf_model.py      |  4 +-
 ros/kxreus/node_scripts/eus_model_server.py   |  6 +--
 setup.cfg                                     | 41 -----------------
 setup.py                                      |  6 +--
 13 files changed, 72 insertions(+), 104 deletions(-)
 create mode 100644 pyproject.toml
 delete mode 100644 setup.cfg

diff --git a/pyproject.toml b/pyproject.toml
new file mode 100644
index 00000000..6bcdf674
--- /dev/null
+++ b/pyproject.toml
@@ -0,0 +1,45 @@
+[tool.ruff]
+target-version = "py38"
+line-length = 90
+
+# See https://github.com/charliermarsh/ruff#rules for error code definitions.
+
+[tool.ruff.lint]
+select = [
+    # "ANN", # annotations
+    "B", # bugbear
+    "C", # comprehensions
+    "E", # style errors
+    "F", # flakes
+    "I", # import sorting
+    "RUF", # ruff specific rules
+    "UP", # upgrade
+    "W", # style warnings
+    "YTT", # sys.version
+    "ISC002",
+    "NPY201",
+    "TID251"
+]
+
+ignore = [
+  "C901", # Comprehension is too complex (11 > 10)
+  "N802", # Function name should be lowercase
+  "N806", # Variable in function should be lowercase
+  "E501", # Line too long ({width} > {limit} characters)
+  "B904", # raise ... from err
+  "B905", # zip() without an explicit strict= parameter
+  "RUF005", # recommends non-type-aware expansion
+  "UP008",
+]
+
+# don't allow implicit string concatenation
+flake8-implicit-str-concat = {"allow-multiline" = false}
+
+[tool.ruff.lint.isort]
+force-single-line = true
+force-sort-within-sections = true
+order-by-type = false
+
+[tool.ruff.lint.flake8-tidy-imports.banned-api]
+    "IPython.embed".msg = "you forgot to remove a debug embed ;)"
+    "numpy.empty".msg = "uninitialized arrays are haunted try numpy.zeros"
diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py
index af901c36..c1e95e63 100644
--- a/rcb4/armh7interface.py
+++ b/rcb4/armh7interface.py
@@ -566,9 +566,7 @@ def _send_angle_vector(self, av, servo_ids=None, velocity=127):
                     dtype=np.float32,
                 )
             self._worm_ref_angle[np.array(worm_indices)] = np.array(worm_av)
-            self.write_cstruct_slot_v(
-                WormmoduleStruct, "ref_angle", self._worm_ref_angle
-            )
+            self.write_cstruct_slot_v(WormmoduleStruct, "ref_angle", self._worm_ref_angle)
         svs = self.angle_vector_to_servo_angle_vector(av, servo_ids)
         return self.servo_angle_vector(servo_ids, svs, velocity=velocity)
 
@@ -656,14 +654,10 @@ def clear_trim_vector(self, write_to_flash=True):
             self.write_to_flash()
 
     def ics_start(self):
-        return self.set_cstruct_slot(
-            SystemStruct, 0, "ics_comm_stop", [0, 0, 0, 0, 0, 0]
-        )
+        return self.set_cstruct_slot(SystemStruct, 0, "ics_comm_stop", [0, 0, 0, 0, 0, 0])
 
     def ics_stop(self):
-        return self.set_cstruct_slot(
-            SystemStruct, 0, "ics_comm_stop", [1, 1, 1, 1, 1, 1]
-        )
+        return self.set_cstruct_slot(SystemStruct, 0, "ics_comm_stop", [1, 1, 1, 1, 1, 1])
 
     def idmode_scan(self):
         self.ics_stop()
@@ -723,9 +717,7 @@ def search_air_board_ids(self):
         return np.array(air_board_ids)
 
     def valid_servo_ids(self, servo_ids):
-        return np.isfinite(
-            self._servo_id_to_sequentialized_servo_id[np.array(servo_ids)]
-        )
+        return np.isfinite(self._servo_id_to_sequentialized_servo_id[np.array(servo_ids)])
 
     def hold(self, servo_ids=None):
         if servo_ids is None:
@@ -1076,9 +1068,7 @@ def send_worm_calib_data(
 
         # Write the parameters to the wormmodule vector cstruct
         if module_type is not None:
-            self.write_cls_alist(
-                WormmoduleStruct, worm_idx, "module_type", [module_type]
-            )
+            self.write_cls_alist(WormmoduleStruct, worm_idx, "module_type", [module_type])
         if servo_idx is not None:
             self.write_cls_alist(WormmoduleStruct, worm_idx, "servo_id", [servo_idx])
         if sensor_idx is not None:
@@ -1382,9 +1372,7 @@ def joint_to_actuator_matrix(self):
     @property
     def actuator_to_joint_matrix(self):
         if self._actuator_to_joint_matrix is None:
-            self._actuator_to_joint_matrix = np.linalg.inv(
-                self.joint_to_actuator_matrix
-            )
+            self._actuator_to_joint_matrix = np.linalg.inv(self.joint_to_actuator_matrix)
         return self._actuator_to_joint_matrix
 
     @property
diff --git a/rcb4/data/__init__.py b/rcb4/data/__init__.py
index 4595658e..306ad636 100644
--- a/rcb4/data/__init__.py
+++ b/rcb4/data/__init__.py
@@ -1,13 +1,13 @@
 from collections import namedtuple
+from distutils.version import StrictVersion
 import os
 import os.path as osp
 import subprocess
-from distutils.version import StrictVersion
 
-import pkg_resources
 from colorama import Fore
 from colorama import Style
 import gdown
+import pkg_resources
 
 data_dir = osp.abspath(osp.dirname(__file__))
 _default_cache_dir = osp.expanduser("~/.rcb4")
diff --git a/rcb4/rcb4interface.py b/rcb4/rcb4interface.py
index 069d7cea..50602e5a 100644
--- a/rcb4/rcb4interface.py
+++ b/rcb4/rcb4interface.py
@@ -424,9 +424,7 @@ def search_servo_ids(self):
         return servo_indices
 
     def valid_servo_ids(self, servo_ids):
-        return np.isfinite(
-            self._servo_id_to_sequentialized_servo_id[np.array(servo_ids)]
-        )
+        return np.isfinite(self._servo_id_to_sequentialized_servo_id[np.array(servo_ids)])
 
     def hold(self, servo_ids=None):
         if servo_ids is None:
@@ -641,9 +639,7 @@ def joint_to_actuator_matrix(self):
     @property
     def actuator_to_joint_matrix(self):
         if self._actuator_to_joint_matrix is None:
-            self._actuator_to_joint_matrix = np.linalg.inv(
-                self.joint_to_actuator_matrix
-            )
+            self._actuator_to_joint_matrix = np.linalg.inv(self.joint_to_actuator_matrix)
         return self._actuator_to_joint_matrix
 
     def servo_states(self):
diff --git a/rcb4/usb_utils.py b/rcb4/usb_utils.py
index 1bebd980..04233854 100644
--- a/rcb4/usb_utils.py
+++ b/rcb4/usb_utils.py
@@ -48,9 +48,7 @@ def reset_usb_device(port):
     vendor_id, product_id = get_vendor_id_and_product_id(port)
     # Create a USB context and open the device by vendor ID and product ID
     context = usb1.USBContext()
-    handle = context.openByVendorIDAndProductID(
-        vendor_id, product_id, skip_on_error=True
-    )
+    handle = context.openByVendorIDAndProductID(vendor_id, product_id, skip_on_error=True)
     if handle is None:
         raise ValueError("Device not found")
     # Reset the USB device
diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index d8718a3f..f860de2a 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -12,9 +12,10 @@
 
 import actionlib
 from actionlib_msgs.msg import GoalID
+from control_msgs.msg import FollowJointTrajectoryAction
+from control_msgs.msg import FollowJointTrajectoryGoal
 from dynamic_reconfigure.server import Server
 import geometry_msgs.msg
-from trajectory_msgs.msg import JointTrajectoryPoint
 from kxr_controller.cfg import KXRParameteresConfig as Config
 from kxr_controller.msg import AdjustAngleVectorAction
 from kxr_controller.msg import AdjustAngleVectorResult
@@ -27,8 +28,7 @@
 from kxr_controller.msg import Stretch
 from kxr_controller.msg import StretchAction
 from kxr_controller.msg import StretchResult
-from control_msgs.msg import FollowJointTrajectoryAction
-from control_msgs.msg import FollowJointTrajectoryGoal
+from kxr_controller.serial import serial_call_with_retry
 import numpy as np
 import rospy
 import sensor_msgs.msg
@@ -37,13 +37,12 @@
 from skrobot.model import RobotModel
 from skrobot.utils.urdf import no_mesh_load_mode
 import std_msgs.msg
+from trajectory_msgs.msg import JointTrajectoryPoint
 import yaml
 
 from rcb4.armh7interface import ARMH7Interface
 from rcb4.rcb4interface import RCB4Interface
 
-from kxr_controller.serial import serial_call_with_retry
-
 np.set_printoptions(precision=0, suppress=True)
 
 
@@ -559,9 +558,7 @@ def velocity_command_joint_state_callback(self, msg):
             self._prev_velocity_command, av
         ):
             return
-        ret = self.interface.angle_vector(
-            av, servo_ids, velocity=self.wheel_frame_count
-        )
+        ret = self.interface.angle_vector(av, servo_ids, velocity=self.wheel_frame_count)
         if ret is None:
             return
         self._prev_velocity_command = av
@@ -724,9 +721,7 @@ def publish_pressure(self):
                     queue_size=1,
                 )
                 self._avg_pressure_publisher_dict[key] = rospy.Publisher(
-                    self.base_namespace
-                    + "/fullbody_controller/average_pressure/"
-                    + key,
+                    self.base_namespace + "/fullbody_controller/average_pressure/" + key,
                     std_msgs.msg.Float32,
                     queue_size=1,
                 )
@@ -909,9 +904,7 @@ def publish_battery_voltage_value(self):
         battery_voltage = serial_call_with_retry(self.interface.battery_voltage)
         if battery_voltage is None:
             return
-        self.battery_voltage_publisher.publish(
-            std_msgs.msg.Float32(data=battery_voltage)
-        )
+        self.battery_voltage_publisher.publish(std_msgs.msg.Float32(data=battery_voltage))
 
     def publish_joint_states(self):
         av = serial_call_with_retry(self.interface.angle_vector)
@@ -979,9 +972,7 @@ def check_success_rate(self):
         self.last_check_time = rospy.Time.now()
 
     def run(self):
-        rate = rospy.Rate(
-            rospy.get_param(self.base_namespace + "/control_loop_rate", 20)
-        )
+        rate = rospy.Rate(rospy.get_param(self.base_namespace + "/control_loop_rate", 20))
 
         self.publish_joint_states_attempts = 0
         self.publish_joint_states_successes = 0
diff --git a/ros/kxr_controller/python/kxr_controller/kxr_interface.py b/ros/kxr_controller/python/kxr_controller/kxr_interface.py
index 0d788ba7..287a26d2 100644
--- a/ros/kxr_controller/python/kxr_controller/kxr_interface.py
+++ b/ros/kxr_controller/python/kxr_controller/kxr_interface.py
@@ -103,9 +103,7 @@ def servo_off(self, joint_names=None):
 
     def adjust_angle_vector(self, joint_names=None, error_threshold=None):
         if self.use_sim_time:
-            rospy.logwarn(
-                "In simulation mode, adjust angle vector function is disabled."
-            )
+            rospy.logwarn("In simulation mode, adjust angle vector function is disabled.")
             return
         if error_threshold is None:
             error_threshold = np.deg2rad(5)
diff --git a/ros/kxr_models/node_scripts/http_server_node.py b/ros/kxr_models/node_scripts/http_server_node.py
index 7d8f7d49..e4613351 100755
--- a/ros/kxr_models/node_scripts/http_server_node.py
+++ b/ros/kxr_models/node_scripts/http_server_node.py
@@ -71,9 +71,7 @@ def stop(self):
     namespace = get_namespace()
     rospy.set_param(namespace + "/model_server_ip", get_local_ip())
     port = rospy.get_param(namespace + "/model_server_port", 8123)
-    server = ThreadedHTTPServer(
-        "0.0.0.0", port, CustomHTTPRequestHandler, www_directory
-    )
+    server = ThreadedHTTPServer("0.0.0.0", port, CustomHTTPRequestHandler, www_directory)
     server.start()
 
     rospy.spin()
diff --git a/ros/kxr_models/node_scripts/urdf_model_server.py b/ros/kxr_models/node_scripts/urdf_model_server.py
index ba320ec8..d8c49539 100755
--- a/ros/kxr_models/node_scripts/urdf_model_server.py
+++ b/ros/kxr_models/node_scripts/urdf_model_server.py
@@ -3,6 +3,7 @@
 import os
 import tempfile
 import xml.etree.ElementTree as ET
+
 from filelock import FileLock
 from kxr_models.md5sum_utils import checksum_md5
 from kxr_models.urdf import aggregate_urdf_mesh_files
diff --git a/ros/kxr_models/scripts/get_urdf_model.py b/ros/kxr_models/scripts/get_urdf_model.py
index 86a59713..3d01b76a 100755
--- a/ros/kxr_models/scripts/get_urdf_model.py
+++ b/ros/kxr_models/scripts/get_urdf_model.py
@@ -19,7 +19,5 @@
         rate = rospy.Rate(rospy.get_param("~rate", 20))
         while not rospy.is_shutdown():
             br = tf.TransformBroadcaster()
-            br.sendTransform(
-                (0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), root_link, "map"
-            )
+            br.sendTransform((0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), root_link, "map")
             rate.sleep()
diff --git a/ros/kxreus/node_scripts/eus_model_server.py b/ros/kxreus/node_scripts/eus_model_server.py
index 10bc11b0..524cac56 100644
--- a/ros/kxreus/node_scripts/eus_model_server.py
+++ b/ros/kxreus/node_scripts/eus_model_server.py
@@ -1,8 +1,8 @@
 #!/usr/bin/env python
 
+from contextlib import contextmanager
 import os
 import tempfile
-from contextlib import contextmanager
 
 from filelock import FileLock
 from kxr_models.md5sum_utils import checksum_md5
@@ -62,9 +62,7 @@ def get_md5sum(self, urdf_content, joint_group_description):
         return md5sum
 
     def save_eus_model(self, md5sum, urdf_content, joint_group_description):
-        eus_path = os.path.join(
-            self.kxr_models_path, "models", "euslisp", f"{md5sum}.l"
-        )
+        eus_path = os.path.join(self.kxr_models_path, "models", "euslisp", f"{md5sum}.l")
         if os.path.exists(eus_path):
             return eus_path  # Model already exists
 
diff --git a/setup.cfg b/setup.cfg
deleted file mode 100644
index 1a995ae2..00000000
--- a/setup.cfg
+++ /dev/null
@@ -1,41 +0,0 @@
-[tool:ruff]
-target-version = py38
-line-length = 90
-
-# See https://github.com/charliermarsh/ruff#rules for error code definitions.
-
-select =
-    B   # bugbear
-    C   # comprehensions
-    E   # style errors
-    F   # flakes
-    I   # import sorting
-    RUF # ruff specific rules
-    UP  # upgrade
-    W   # style warnings
-    YTT # sys.version
-    ISC002
-    NPY201
-    TID251
-
-ignore =
-    C901    # Comprehension is too complex (11 > 10)
-    N802    # Function name should be lowercase
-    N806    # Variable in function should be lowercase
-    E501    # Line too long ({width} > {limit} characters)
-    B904    # raise ... from err
-    B905    # zip() without an explicit strict= parameter
-    RUF005  # recommends non-type-aware expansion
-
-# don't allow implicit string concatenation
-flake8-implicit-str-concat = {"allow-multiline" = false}
-
-[tool.ruff.lint.isort]
-force-single-line-imports = true
-force-sort-within-sections = true
-order-by-type = false
-
-[tool.ruff.lint.flake8-tidy-imports.banned-api]
-banned-api =
-    IPython.embed = you forgot to remove a debug embed ;)
-    numpy.empty = uninitialized arrays are haunted try numpy.zeros
diff --git a/setup.py b/setup.py
index 76a25277..bd94cc57 100644
--- a/setup.py
+++ b/setup.py
@@ -1,4 +1,3 @@
-from __future__ import print_function
 
 import shlex
 import subprocess
@@ -7,18 +6,17 @@
 from setuptools import find_packages
 from setuptools import setup
 
-
 version = "0.0.1"
 
 
 if sys.argv[-1] == "release":
     # Release via github-actions.
     commands = [
-        "git tag v{:s}".format(version),
+        f"git tag v{version:s}",
         "git push origin master --tag",
     ]
     for cmd in commands:
-        print("+ {}".format(cmd))
+        print(f"+ {cmd}")
         subprocess.check_call(shlex.split(cmd))
     sys.exit(0)
 

From 4f412172475c5da9c49f6dff11f4ed42f3fa4e27 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 21:05:51 +0900
Subject: [PATCH 11/18] Modified timing for follow joint trajectory action
 client

---
 ros/kxr_controller/node_scripts/rcb4_ros_bridge.py | 13 ++++++-------
 1 file changed, 6 insertions(+), 7 deletions(-)

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index f860de2a..9ac9affc 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -161,13 +161,6 @@ def __init__(self):
         self.setup_publishers_and_servers()
 
         self.subscribe()
-
-        self.traj_action_client = actionlib.SimpleActionClient(
-            self.base_namespace + "/fullbody_controller/follow_joint_trajectory",
-            FollowJointTrajectoryAction,
-        )
-        self.traj_action_client.wait_for_server()
-
         rospy.loginfo("RCB4 ROS Bridge initialization completed.")
 
     def setup_paths_and_params(self):
@@ -271,6 +264,12 @@ def setup_action_servers_and_clients(self):
         rospy.sleep(0.1)
         self.servo_on_off_server.start()
 
+        self.traj_action_client = actionlib.SimpleActionClient(
+            self.base_namespace + "/fullbody_controller/follow_joint_trajectory",
+            FollowJointTrajectoryAction,
+        )
+        self.traj_action_client.wait_for_server()
+
         # Adjust angle vector action server
         self.adjust_angle_vector_server = actionlib.SimpleActionServer(
             self.base_namespace + "/fullbody_controller/adjust_angle_vector_interface",

From 5cb577bbb44ce0dedfabb5ceda255343990861d8 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 21:06:02 +0900
Subject: [PATCH 12/18] Fixed valid joints conditions

---
 ros/kxr_controller/node_scripts/rcb4_ros_bridge.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index 9ac9affc..ebc11bd9 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -513,7 +513,7 @@ def set_initial_positions(self):
         return True
 
     def _valid_joint(self, joint_name):
-        return joint_name not in self.joint_name_to_id or (
+        return joint_name in self.joint_name_to_id and (
             self.joint_name_to_id[joint_name] in self.interface.servo_on_states_dict
             and self.interface.servo_on_states_dict[self.joint_name_to_id[joint_name]]
         )

From 9ee28dc505e9a93f4715c5b32195425e90ff7838 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 21:10:52 +0900
Subject: [PATCH 13/18] Fixed read_pressure_sensor

---
 ros/kxr_controller/node_scripts/rcb4_ros_bridge.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index ebc11bd9..c8e00fc7 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -727,7 +727,7 @@ def publish_pressure(self):
                 # Avoid 'rospy.exceptions.ROSException:
                 # publish() to a closed topic'
                 rospy.sleep(0.1)
-            pressure = serial_call_with_retry(self.read_pressure_sensor, idx)
+            pressure = serial_call_with_retry(self.interface.read_pressure_sensor, idx)
             if pressure is None:
                 continue
             self._pressure_publisher_dict[key].publish(

From abc6167d648b491a650614959953c8d11abf0909 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 21:17:07 +0900
Subject: [PATCH 14/18] Fixed averate pressure calculation and append pressure

---
 ros/kxr_controller/node_scripts/rcb4_ros_bridge.py | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index c8e00fc7..cd20e20f 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -730,6 +730,7 @@ def publish_pressure(self):
             pressure = serial_call_with_retry(self.interface.read_pressure_sensor, idx)
             if pressure is None:
                 continue
+            self.recent_pressures.append(pressure)
             self._pressure_publisher_dict[key].publish(
                 std_msgs.msg.Float32(data=pressure)
             )
@@ -760,17 +761,23 @@ def pressure_control_loop(self, idx, start_pressure, stop_pressure, release):
             return
         vacuum_on = False
         while self.pressure_control_running:
-            if vacuum_on is False and self.average_pressure > start_pressure:
+            pressure = self.average_pressure
+            if pressure is None:
+                rospy.sleep()
+            if vacuum_on is False and pressure > start_pressure:
                 self.start_vacuum(idx)
                 vacuum_on = True
-            if vacuum_on and self.average_pressure <= stop_pressure:
+            if vacuum_on and pressure <= stop_pressure:
                 self.stop_vacuum(idx)
                 vacuum_on = False
             rospy.sleep(0.1)
 
     @property
     def average_pressure(self):
-        return sum(self.recent_pressures) / len(self.recent_pressures)
+        n = len(self.recent_pressures)
+        if n == 0:
+            return None
+        return sum(self.recent_pressures) / n
 
     def release_vacuum(self, idx):
         """Connect work to air.

From b9ea714cc0cd851077d9f3a4819abe9ac89bed62 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 21:19:16 +0900
Subject: [PATCH 15/18] Set during servo off False in servo_on_off_callback

---
 ros/kxr_controller/node_scripts/rcb4_ros_bridge.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index cd20e20f..9c06fae3 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -606,6 +606,7 @@ def servo_on_off_callback(self, goal):
             max_retries=10,
         )
         if ret is None:
+            self._during_servo_off = False
             return self.servo_on_off_server.set_aborted(
                 text="Failed to call servo on off. "
                 + "Control board is switch off or cable is disconnected?"

From ce5b66a1613ada86ff9bc8e1a00755816c209933 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 21:25:19 +0900
Subject: [PATCH 16/18] Check having pressure_control_thread

---
 ros/kxr_controller/node_scripts/rcb4_ros_bridge.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index 9c06fae3..5a8f9738 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -823,7 +823,7 @@ def stop_vacuum(self, idx):
     def pressure_control_callback(self, goal):
         if not self.interface.is_opened():
             return
-        if self.pressure_control_thread is not None:
+        if hasattr(self, 'pressure_control_thread') and self.pressure_control_thread is not None:
             # Finish existing thread
             self.pressure_control_running = False
             # Wait for the finishing process complete

From a40e2f68a07f3e477f976f365dfa35bfa0dd141f Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 21:36:34 +0900
Subject: [PATCH 17/18] Use 0.5 time

---
 ros/kxr_controller/node_scripts/rcb4_ros_bridge.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index 5a8f9738..f329e6f9 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -630,7 +630,7 @@ def servo_on_off_callback(self, goal):
         point = JointTrajectoryPoint()
         point.positions = positions
         point.time_from_start = rospy.Duration(
-            0.01
+            0.5
         )  # Short duration for immediate application
         trajectory_goal.trajectory.points = [point]
         # Initialize action client and wait for server

From ae9155ddde3e36489858c1fa4ef2cee683ec95b7 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 31 Oct 2024 21:49:49 +0900
Subject: [PATCH 18/18] Use serial_call_with_retry on pump control

---
 rcb4/armh7interface.py                        | 16 ++--
 .../node_scripts/rcb4_ros_bridge.py           | 77 ++++++++++++-------
 2 files changed, 56 insertions(+), 37 deletions(-)

diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py
index c1e95e63..6616693e 100644
--- a/rcb4/armh7interface.py
+++ b/rcb4/armh7interface.py
@@ -1288,19 +1288,19 @@ def read_pressure_sensor(self, board_idx):
 
     def start_pump(self):
         """Drive pump. There is supposed to be one pump for the entire system."""
-        self.cfunc_call("pump_switch", True)
+        return self.cfunc_call("pump_switch", True)
 
     def stop_pump(self):
         """Stop driving pump. There should be one pump for the entire system."""
-        self.cfunc_call("pump_switch", False)
+        return self.cfunc_call("pump_switch", False)
 
     def open_air_connect_valve(self):
         """Open valve to release air to atmosphere"""
-        self.cfunc_call("valve_switch", True)
+        return self.cfunc_call("valve_switch", True)
 
     def close_air_connect_valve(self):
         """Close valve to shut off air from atmosphere"""
-        self.cfunc_call("valve_switch", False)
+        return self.cfunc_call("valve_switch", False)
 
     def gpio_mode(self, board_idx):
         """Return current GPIO mode of air relay board
@@ -1318,7 +1318,7 @@ def open_work_valve(self, board_idx):
         """
         gpio_mode = self.gpio_mode(board_idx)
         command = gpio_mode | 0b00000001
-        self.cfunc_call("gpio_cmd", board_idx, command)
+        return self.cfunc_call("gpio_cmd", board_idx, command)
 
     def close_work_valve(self, board_idx):
         """Close work valve
@@ -1327,7 +1327,7 @@ def close_work_valve(self, board_idx):
         """
         gpio_mode = self.gpio_mode(board_idx)
         command = gpio_mode & 0b11111110
-        self.cfunc_call("gpio_cmd", board_idx, command)
+        return self.cfunc_call("gpio_cmd", board_idx, command)
 
     def open_relay_valve(self, board_idx):
         """Open valve to relay air to next work
@@ -1336,7 +1336,7 @@ def open_relay_valve(self, board_idx):
         """
         gpio_mode = self.gpio_mode(board_idx)
         command = gpio_mode | 0b00000010
-        self.cfunc_call("gpio_cmd", board_idx, command)
+        return self.cfunc_call("gpio_cmd", board_idx, command)
 
     def close_relay_valve(self, board_idx):
         """Close valve to relay air to next work
@@ -1345,7 +1345,7 @@ def close_relay_valve(self, board_idx):
         """
         gpio_mode = self.gpio_mode(board_idx)
         command = gpio_mode & 0b11111101
-        self.cfunc_call("gpio_cmd", board_idx, command)
+        return self.cfunc_call("gpio_cmd", board_idx, command)
 
     @property
     def servo_id_to_worm_id(self):
diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index f329e6f9..7c32fe33 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -766,11 +766,9 @@ def pressure_control_loop(self, idx, start_pressure, stop_pressure, release):
             if pressure is None:
                 rospy.sleep()
             if vacuum_on is False and pressure > start_pressure:
-                self.start_vacuum(idx)
-                vacuum_on = True
+                vacuum_on = self.start_vacuum(idx)
             if vacuum_on and pressure <= stop_pressure:
-                self.stop_vacuum(idx)
-                vacuum_on = False
+                vacuum_on = not self.stop_vacuum(idx)
             rospy.sleep(0.1)
 
     @property
@@ -786,39 +784,60 @@ def release_vacuum(self, idx):
         After 1s, all valves are closed and pump is stopped.
         """
         if not self.interface.is_opened():
-            return
-        try:
-            self.interface.stop_pump()
-            self.interface.open_work_valve(idx)
-            self.interface.open_air_connect_valve()
-            rospy.sleep(1)  # Wait until air is completely released
-            self.interface.close_air_connect_valve()
-            self.interface.close_work_valve(idx)
-        except serial.serialutil.SerialException as e:
-            rospy.logerr(f"[release_vacuum] {e!s}")
+            return False
+        ret = serial_call_with_retry(self.interface.stop_pump, max_retries=3)
+        if ret is None:
+            return False
+        ret = serial_call_with_retry(self.interface.open_work_valve, idx, max_retries=3)
+        if ret is None:
+            return False
+        ret = serial_call_with_retry(self.interface.open_air_connect_valve,
+                                     max_retries=3)
+        if ret is None:
+            return False
+        rospy.sleep(1)  # Wait until air is completely released
+        ret = serial_call_with_retry(self.interface.close_air_connect_valve,
+                                     max_retries=3)
+        if ret is None:
+            return False
+        ret = serial_call_with_retry(self.interface.close_work_valve,
+                                     idx, max_retries=3)
+        if ret is None:
+            return False
+        return True
 
     def start_vacuum(self, idx):
         """Vacuum air in work"""
         if not self.interface.is_opened():
-            return
-        try:
-            self.interface.start_pump()
-            self.interface.open_work_valve(idx)
-            self.interface.close_air_connect_valve()
-        except serial.serialutil.SerialException as e:
-            rospy.logerr(f"[start_vacuum] {e!s}")
+            return False
+
+        ret = serial_call_with_retry(self.interface.start_pump, max_retries=3)
+        if ret is None:
+            return False
+        ret = serial_call_with_retry(self.interface.open_work_valve, idx, max_retries=3)
+        if ret is None:
+            return False
+        ret = serial_call_with_retry(self.interface.close_air_connect_valve, max_retries=3)
+        if ret is None:
+            return False
+        return True
 
     def stop_vacuum(self, idx):
         """Seal air in work"""
         if not self.interface.is_opened():
-            return
-        try:
-            self.interface.close_work_valve(idx)
-            self.interface.close_air_connect_valve()
-            rospy.sleep(0.3)  # Wait for valve to close completely
-            self.interface.stop_pump()
-        except serial.serialutil.SerialException as e:
-            rospy.logerr(f"[stop_vacuum] {e!s}")
+            return False
+
+        ret = serial_call_with_retry(self.interface.close_work_valve, idx, max_retries=3)
+        if ret is None:
+            return False
+        ret = serial_call_with_retry(self.interface.close_air_connect_valve, max_retries=3)
+        if ret is None:
+            return False
+        rospy.sleep(0.3)  # Wait for valve to close completely
+        ret = serial_call_with_retry(self.interface.stop_pump, max_retries=3)
+        if ret is None:
+            return False
+        return True
 
     def pressure_control_callback(self, goal):
         if not self.interface.is_opened():