From dee3098424be144a7f0f571b91764da6625fa119 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Thu, 8 Feb 2024 16:19:06 +0900
Subject: [PATCH] Add rcb4 mini board test

---
 rcb4/rcb4interface.py                         | 25 ++---
 ros/kxr_controller/CMakeLists.txt             |  2 +
 .../launch/kxr_controller.launch              |  3 +
 .../node_scripts/rcb4_ros_bridge.py           |  9 +-
 ros/kxr_controller/test/kxr_controller.test   | 55 +++++++----
 .../test/test_kxr_controller.py               |  1 -
 tests/rcb4_tests/test_armh7interface.py       |  7 +-
 tests/rcb4_tests/test_rcb4interface.py        | 92 +++++++++++++++++++
 8 files changed, 163 insertions(+), 31 deletions(-)
 create mode 100644 tests/rcb4_tests/test_rcb4interface.py

diff --git a/rcb4/rcb4interface.py b/rcb4/rcb4interface.py
index 21cd7e0b..3bbe6793 100644
--- a/rcb4/rcb4interface.py
+++ b/rcb4/rcb4interface.py
@@ -102,7 +102,6 @@ class RCB4Interface(object):
     # The maximum total byte count of motion data
     MotionDataCount = 2048 * 120
 
-
     def __init__(self):
         self.lock = Lock()
         self.serial = None
@@ -283,7 +282,7 @@ def move_com_to_ram_command(dest_addr, dest_data):
         txbuf.append(CommandTypes.Move.value)
         txbuf.append(SubMoveCmd.ComToRam.value)
         txbuf.append(dest_addr & 0xff)
-        txbuf.append((dest_addr >> 8 ) & 0xff)
+        txbuf.append((dest_addr >> 8) & 0xff)
         txbuf.append(0x00)
         if len(dest_data) == 1:
             txbuf.append(dest_data[0])
@@ -630,9 +629,10 @@ def get_config(self):
     def call_command(self, rom_addr):
         """Creates a command to call a specified ROM address.
 
-        This method generates a command array to be sent to a device, instructing it
-        to perform a jump to the specified ROM address. This function is static and
-        can be accessed externally without an instance of the class.
+        This method generates a command array to be sent to a device,
+        instructing it to perform a jump to the specified ROM address.
+        This function is static and can be accessed externally without
+        an instance of the class.
 
         Parameters
         ----------
@@ -645,13 +645,14 @@ def call_command(self, rom_addr):
             return_data_size : int
                 The size of the data expected to be returned by the command.
             txbuf : list of int
-                The list representing the entire command to be sent, including the
-                command to call the ROM address, the address itself, and the checksum.
+                The list representing the entire command to be sent,
+                including the command to call the ROM address, the address
+                itself, and the checksum.
 
         Notes
         -----
-        The command created by this function does not check conditions before calling
-        the specified address.
+        The command created by this function does not check conditions before
+        calling the specified address.
 
         Warnings
         --------
@@ -796,14 +797,16 @@ def resume_motion(self):
         Notes
         -----
         Modifies the config data to resume the motion.
-        Direct manipulation of internal config data (`self._config_data`) is involved.
+        Direct manipulation of internal config data
+        (`self._config_data`) is involved.
         """
         self._config_data |= ConfigData.EnableRunEeprom.value
         self._config_data &= ~ConfigData.EnableServoResponse.value
         self._config_data |= ConfigData.EnableReferenceTable.value
         self._config_data |= ConfigData.EnableSio.value
         buf = [self._config_data & 0xff, (self._config_data >> 8) & 0xff]
-        return self.write_move_com_to_ram_command(RamAddr.ConfigRamAddress.value, buf)
+        return self.write_move_com_to_ram_command(
+            RamAddr.ConfigRamAddress.value, buf)
 
     def play_motion(self, motion_num):
         """Plays the specified motion.
diff --git a/ros/kxr_controller/CMakeLists.txt b/ros/kxr_controller/CMakeLists.txt
index 778f6ef6..eb981805 100644
--- a/ros/kxr_controller/CMakeLists.txt
+++ b/ros/kxr_controller/CMakeLists.txt
@@ -114,4 +114,6 @@ if(CATKIN_ENABLE_TESTING)
   endif()
   add_rostest(test/kxr_controller.test ARGS namespace:="")
   add_rostest(test/kxr_controller.test ARGS namespace:="robot_a")
+  add_rostest(test/kxr_controller.test ARGS namespace:="" use_rcb4:=true)
+  add_rostest(test/kxr_controller.test ARGS namespace:="robot_a" use_rcb4:=true)
 endif()
diff --git a/ros/kxr_controller/launch/kxr_controller.launch b/ros/kxr_controller/launch/kxr_controller.launch
index 0aa8d751..4ad02281 100644
--- a/ros/kxr_controller/launch/kxr_controller.launch
+++ b/ros/kxr_controller/launch/kxr_controller.launch
@@ -6,6 +6,7 @@
   <arg name="publish_imu" default="true" />
   <arg name="imu_frame_id" default="/bodyset94472077639384" />
   <arg name="control_loop_rate" default="20" />
+  <arg name="use_rcb4" default="false" doc="Flag to use RCB4 mini board"/>
 
   <group if="$(eval len(arg('namespace')) > 0)" ns="$(arg namespace)" >
     <param name="control_loop_rate" value="$(arg control_loop_rate)" />
@@ -19,6 +20,7 @@
         servo_config_path: $(arg servo_config_path)
         publish_imu: $(arg publish_imu)
         imu_frame_id: $(arg namespace)/$(arg imu_frame_id)
+        use_rcb4: $(arg use_rcb4)
       </rosparam>
     </node>
 
@@ -36,6 +38,7 @@
         servo_config_path: $(arg servo_config_path)
         publish_imu: $(arg publish_imu)
         imu_frame_id: $(arg imu_frame_id)
+        use_rcb4: $(arg use_rcb4)
       </rosparam>
     </node>
 
diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index d831d8f9..08225ba1 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -18,6 +18,7 @@
 import yaml
 
 from rcb4.armh7interface import ARMH7Interface
+from rcb4.rcb4interface import RCB4Interface
 
 
 np.set_printoptions(precision=0, suppress=True)
@@ -129,7 +130,11 @@ def __init__(self):
             JointState,
             queue_size=1)
 
-        self.interface = ARMH7Interface.from_port()
+        if rospy.get_param('~use_rcb4'):
+            self.interface = RCB4Interface()
+            self.interface.auto_open()
+        else:
+            self.interface = ARMH7Interface.from_port()
         if self.interface is None:
             rospy.logerr('Could not open port!')
             sys.exit(1)
@@ -185,6 +190,8 @@ def __init__(self):
             clean_namespace)
 
         self.publish_imu = rospy.get_param('~publish_imu', True)
+        if self.interface.__class__.__name__ == 'RCB4Interface':
+            self.publish_imu = False
         if self.publish_imu:
             self.imu_frame_id = rospy.get_param(
                 '~imu_frame_id', clean_namespace + '/' + r.root_link.name)
diff --git a/ros/kxr_controller/test/kxr_controller.test b/ros/kxr_controller/test/kxr_controller.test
index 29209365..220c5711 100644
--- a/ros/kxr_controller/test/kxr_controller.test
+++ b/ros/kxr_controller/test/kxr_controller.test
@@ -1,11 +1,13 @@
 <launch>
 
   <arg name="namespace" default="" />
+  <arg name="use_rcb4" default="false" doc="Flag to use RCB4 mini board"/>
 
   <include file="$(find kxr_controller)/launch/kxr_controller.launch">
     <arg name="urdf_path" value="$(find kxr_models)/urdf/kxr_test_head.urdf" />
     <arg name="servo_config_path" value="$(find kxr_models)/config/kxr_test_head_servo_config.yaml" />
     <arg name="namespace" value="$(arg namespace)" />
+    <arg name="use_rcb4" value="$(arg use_rcb4)" />
   </include>
 
   <group if="$(eval len(arg('namespace')) > 0)" ns="$(arg namespace)" >
@@ -13,23 +15,34 @@
           name="test_ros_topics"
           pkg="jsk_tools" type="test_topic_published.py"
           retry="1" >
-      <rosparam>
-        topic_0: current_joint_states
-        timeout_0: 30
-        topic_1: joint_states
-        timeout_1: 30
-        topic_2: kxr_fullbody_controller/follow_joint_trajectory/status
-        timeout_2: 30
-        topic_3: kxr_fullbody_controller/servo_on_off_real_interface/status
-        timeout_3: 30
-        topic_4: kxr_fullbody_controller/servo_on_off/status
-        timeout_4: 30
-        topic_5: imu
-        timeout_5: 30
-      </rosparam>
+        <rosparam>
+          topic_0: current_joint_states
+          timeout_0: 30
+          topic_1: joint_states
+          timeout_1: 30
+          topic_2: kxr_fullbody_controller/follow_joint_trajectory/status
+          timeout_2: 30
+          topic_3: kxr_fullbody_controller/servo_on_off_real_interface/status
+          timeout_3: 30
+          topic_4: kxr_fullbody_controller/servo_on_off/status
+          timeout_4: 30
+        </rosparam>
     </test>
+
+    <group if="$(arg use_rcb4)" >
+      <test test-name="test_ros_topics_for_armh7"
+            name="test_ros_topics_for_armh7"
+            pkg="jsk_tools" type="test_topic_published.py"
+            retry="1" >
+        <rosparam>
+          topic_0: imu
+          timeout_0: 30
+        </rosparam>
+      </test>
+    </group>
   </group>
 
+
   <group unless="$(eval len(arg('namespace')) > 0)">
     <test test-name="test_ros_topics"
           name="test_ros_topics"
@@ -46,10 +59,20 @@
         timeout_3: 30
         topic_4: kxr_fullbody_controller/servo_on_off/status
         timeout_4: 30
-        topic_5: imu
-        timeout_5: 30
       </rosparam>
     </test>
+
+    <group unless="$(arg use_rcb4)" >
+      <test test-name="test_ros_topics_for_armh7"
+            name="test_ros_topics_for_armh7"
+            pkg="jsk_tools" type="test_topic_published.py"
+            retry="1" >
+        <rosparam>
+          topic_0: imu
+          timeout_0: 30
+        </rosparam>
+      </test>
+    </group>
   </group>
 
   <test test-name="test_kxr_controller"
diff --git a/ros/kxr_controller/test/test_kxr_controller.py b/ros/kxr_controller/test/test_kxr_controller.py
index b940fd64..e02a0771 100644
--- a/ros/kxr_controller/test/test_kxr_controller.py
+++ b/ros/kxr_controller/test/test_kxr_controller.py
@@ -6,7 +6,6 @@
 
 from kxr_controller.kxr_interface import KXRROSRobotInterface
 import numpy as np
-from numpy import testing
 import rospy
 from skrobot.model import RobotModel
 
diff --git a/tests/rcb4_tests/test_armh7interface.py b/tests/rcb4_tests/test_armh7interface.py
index b2107edf..aa8f17c3 100644
--- a/tests/rcb4_tests/test_armh7interface.py
+++ b/tests/rcb4_tests/test_armh7interface.py
@@ -7,14 +7,17 @@
 from rcb4.armh7interface import ARMH7Interface
 
 
-class TestRobotModel(unittest.TestCase):
+class TestARMH7Interface(unittest.TestCase):
 
     interface = None
 
     @classmethod
     def setUpClass(cls):
         cls.interface = ARMH7Interface()
-        cls.interface.auto_open()
+        if cls.interface.auto_open() is not True:
+            raise unittest.SkipTest(
+                "auto_open failed, skipping all tests in {}."
+                .format(cls.__class__.__name__))
 
     def test_reference_angle_vector(self):
         self.interface.reference_angle_vector([])
diff --git a/tests/rcb4_tests/test_rcb4interface.py b/tests/rcb4_tests/test_rcb4interface.py
new file mode 100644
index 00000000..da111aec
--- /dev/null
+++ b/tests/rcb4_tests/test_rcb4interface.py
@@ -0,0 +1,92 @@
+import time
+import unittest
+
+import numpy as np
+from numpy import testing
+
+from rcb4.rcb4interface import RCB4Interface
+
+
+class TestRCB4Interface(unittest.TestCase):
+
+    interface = None
+
+    @classmethod
+    def setUpClass(cls):
+        cls.interface = RCB4Interface()
+        if cls.interface.auto_open() is not True:
+            raise unittest.SkipTest(
+                "auto_open failed, skipping all tests in {}."
+                .format(cls.__class__.__name__))
+
+    def test_reference_angle_vector(self):
+        self.interface.reference_angle_vector([])
+
+    def test_sequentialized_servo_ids(self):
+        self.interface.sequentialized_servo_ids([])
+
+        testing.assert_array_equal(
+            self.interface.sequentialized_servo_ids([32, 34]),
+            [0, 1])
+        testing.assert_array_equal(
+            self.interface.sequentialized_servo_ids([34, 32]),
+            [1, 0])
+
+    def test_angle_vector_to_servo_angle_vector(self):
+        sv = self.interface.angle_vector_to_servo_angle_vector(
+            [0, 0])
+        testing.assert_array_almost_equal(
+            sv, [7500, 7500])
+
+        sv = self.interface.angle_vector_to_servo_angle_vector(
+            [30, 60], [32, 34])
+        testing.assert_array_almost_equal(
+            sv, [8400, 9300])
+
+        sv = self.interface.angle_vector_to_servo_angle_vector(
+            [30, 60], [34, 32])
+        testing.assert_array_almost_equal(
+            sv, [8400, 9300])
+
+    def test_angle_vector(self):
+        self.interface.hold()
+        self.interface.neutral()
+        time.sleep(4.0)
+        reference = [60, 30]
+        self.interface.angle_vector(
+            reference,
+            velocity=127)
+        time.sleep(4.0)
+        testing.assert_array_almost_equal(
+            self.interface.angle_vector(), reference,
+            decimal=0)
+        self.interface.neutral()
+        time.sleep(4.0)
+        self.interface.free()
+
+        self.interface.angle_vector([], [])
+
+    def test_servo_angle_vector(self):
+        self.interface.hold()
+        self.interface.neutral()
+        reference = [8000, ]
+        target_servo_ids = [32, ]
+
+        self.interface.servo_angle_vector(
+            target_servo_ids,
+            reference,
+            velocity=1)
+        time.sleep(1.0)
+        testing.assert_array_almost_equal(
+            self.interface.reference_angle_vector(target_servo_ids),
+            reference)
+        time.sleep(4.0)
+        if np.any(np.abs(self.interface.angle_vector(
+                servo_ids=target_servo_ids) - 16) > 2.0):
+            raise AssertionError(
+                "Servo angles are beyond the acceptable range.")
+
+        time.sleep(1.0)
+        self.interface.neutral()
+        time.sleep(4.0)
+        self.interface.free()