Merge pull request #18 from iory/refactor

Refactor

rcb4/armh7interface.py

@@ -153,9 +153,14 @@ def open(self, port='/dev/ttyUSB1', baudrate=1000000, timeout=0.01):
         except serial.SerialException as e:
             print(f"Error opening serial port: {e}")
             raise serial.SerialException(e)
+        ack = self.check_ack()
+        if ack is not True:
+            return False
         self.check_firmware_version()
         self.copy_worm_params_from_flash()
-        return self.check_ack()
+        self.search_worm_ids()
+        self.search_servo_ids()
+        return True
 
     def auto_open(self):
         system_info = platform.uname()
@@ -420,14 +425,22 @@ def angle_vector(self):
     def search_servo_ids(self):
         if self.servo_sorted_ids is not None:
             return self.servo_sorted_ids
-        indices = []
+        servo_indices = []
+        wheel_indices = []
         for idx in range(rcb4_dof):
             servo = self.memory_cstruct(ServoStruct, idx)
             if servo.flag > 0:
-                indices.append(idx)
-        indices = np.array(indices)
-        self.servo_sorted_ids = indices
-        return indices
+                servo_indices.append(idx)
+                if idx not in self._servo_id_to_worm_id:
+                    # wheel
+                    if servo.rotation > 0:
+                        wheel_indices.append(idx)
+                    if servo.feedback > 0:
+                        self.set_cstruct_slot(ServoStruct, idx, 'feedback', 0)
+        servo_indices = np.array(servo_indices)
+        self.wheel_servo_sorted_ids = sorted(wheel_indices)
+        self.servo_sorted_ids = servo_indices
+        return servo_indices
 
     def hold(self, servo_ids=None):
         if servo_ids is None:
@@ -528,27 +541,23 @@ def set_cstruct_slot(self, cls, idx, slot_name, v):
         bytes[0:len(byte_data)] = byte_data
         return self.memory_write(addr, slot_size, bytes)
 
-    def search_wheel_sids(self):
-        indices = []
-        for idx in self.servo_sorted_ids:
-            servo = self.memory_cstruct(ServoStruct, idx)
-            if servo.rotation > 0:
-                indices.append(idx)
-            if servo.feedback > 0:
-                self.set_cstruct_slot(ServoStruct, idx, 'feedback', 0)
-        self.wheel_servo_sorted_ids = sorted(indices)
-        return indices
-
     def search_worm_ids(self):
         if self.worm_sorted_ids is not None:
             return self.worm_sorted_ids
         indices = []
+        self._servo_id_to_worm_id = {}
+        self._worm_id_to_servo_id = {}
         for idx in range(max_sensor_num):
             worm = self.memory_cstruct(WormmoduleStruct, idx)
             if worm.module_type == 1:
                 servo = self.memory_cstruct(ServoStruct, worm.servo_id)
                 if servo.rotation == 1:
                     indices.append(idx)
+                    if servo.feedback > 0:
+                        self.set_cstruct_slot(
+                            ServoStruct, worm.servo_id, 'feedback', 0)
+                self._servo_id_to_worm_id[worm.servo_id] = idx
+                self._worm_id_to_servo_id[idx] = worm.servo_id
         self.worm_sorted_ids = indices
         return indices
 
@@ -641,8 +650,11 @@ def read_worm_calib_data(self, worm_idx):
         return self.memory_cstruct(WormmoduleStruct, worm_idx)
 
     def copy_worm_params_from_flash(self):
-        for i in self.search_worm_ids():
+        for i in range(max_sensor_num):
             self.dataflash_to_dataram(WormmoduleStruct, i)
+        self.write_cstruct_slot_v(
+            WormmoduleStruct, 'move_state',
+            np.zeros(max_sensor_num))
 
     def buzzer(self):
         return self.cfunc_call('buzzer_init_sound', [])
@@ -731,9 +743,15 @@ def write_cstruct_slot_v(self, cls, slot_name, vec):
                     v = vec
                 if not isinstance(v, (int, float)):
                     v = v[0] if len(v) > 1 else v
-                if typ in ('uint8', 'uint16', 'uint32'):
-                    v = round(v)
-                    struct.pack_into('I', byte_list, 10 + i * esize, v)
+                if typ == 'uint8':
+                    v = int(round(v))
+                    struct.pack_into('<B', byte_list, 10 + i * esize, v)
+                elif typ == 'uint16':
+                    v = int(round(v))
+                    struct.pack_into('<H', byte_list, 10 + i * esize, v)
+                elif typ == 'uint32':
+                    v = int(round(v))
+                    struct.pack_into('<I', byte_list, 10 + i * esize, v)
                 elif typ in ('float', 'double'):
                     struct.pack_into('<f', byte_list, 10 + i * esize, v)
                 else:
@@ -768,26 +786,10 @@ def all_jointbase_sensors(self):
 
     @property
     def servo_id_to_worm_id(self):
-        if self._servo_id_to_worm_id is not None:
-            return self._servo_id_to_worm_id
-        self._servo_id_to_worm_id = {}
-        self._worm_id_to_servo_id = {}
-        for idx in self.search_worm_ids():
-            worm = self.read_worm_calib_data(idx)
-            self._servo_id_to_worm_id[worm.servo_id] = idx
-            self._worm_id_to_servo_id[idx] = worm.servo_id
         return self._servo_id_to_worm_id
 
     @property
     def worm_id_to_servo_id(self):
-        if self._worm_id_to_servo_id is not None:
-            return self._worm_id_to_servo_id
-        self._servo_id_to_worm_id = {}
-        self._worm_id_to_servo_id = {}
-        for idx in self.search_worm_ids():
-            worm = self.read_worm_calib_data(idx)
-            self._servo_id_to_worm_id[worm.servo_id] = idx
-            self._worm_id_to_servo_id[idx] = worm.servo_id
         return self._worm_id_to_servo_id
 
     @property
@@ -832,7 +834,7 @@ def servo_states(self):
         servo_on_indices = np.where(
             self.reference_angle_vector() != 32768)[0]
         if len(servo_on_indices) > 0:
-            return servo_on_indices
+            return self.servo_sorted_ids[servo_on_indices]
         return []
 
 

ros/kxr_controller/launch/kxr_controller.launch

@@ -20,15 +20,6 @@
       </rosparam>
     </node>
 
-    <node name="kxr_controller"
-          pkg="kxr_controller"
-          type="kxr_controller"
-          clear_params="true"
-          respawn="true" output="screen">
-      <rosparam>
-      </rosparam>
-    </node>
-
   </group>
 
   <group unless="$(eval len(arg('namespace')) > 0)">
@@ -45,14 +36,6 @@
       </rosparam>
     </node>
 
-    <node name="kxr_controller"
-          pkg="kxr_controller"
-          type="kxr_controller"
-          clear_params="true"
-          respawn="true" output="screen">
-      <rosparam>
-      </rosparam>
-    </node>
   </group>
 
 </launch>

ros/kxr_controller/node_scripts/rcb4_ros_bridge.py

@@ -5,7 +5,6 @@
 import shlex
 import subprocess
 import sys
-from threading import Lock
 
 import actionlib
 from kxr_controller.msg import ServoOnOffAction
@@ -62,6 +61,20 @@ def run_robot_state_publisher(namespace=None):
     return process
 
 
+def run_kxr_controller(namespace=None):
+    command = f'/opt/ros/{os.environ["ROS_DISTRO"]}/bin/rosrun'
+    command += " kxr_controller kxr_controller"
+    command += ' __name=:kxr_controller'
+    command = shlex.split(command)
+    process = subprocess.Popen(command)
+    return process
+
+
+def set_initial_position(positions, namespace=None):
+    rospy.set_param(namespace + '/initial_position',
+                    positions)
+
+
 def set_fullbody_controller(joint_names):
     controller_yaml_dict = {
         'type': 'position_controllers/JointTrajectoryController',
@@ -86,7 +99,6 @@ def set_robot_description(urdf_path,
 class RCB4ROSBridge(object):
 
     def __init__(self):
-        self.lock = Lock()
         r = RobotModel()
         urdf_path = rospy.get_param('~urdf_path')
         with open(urdf_path) as f:
@@ -120,11 +132,8 @@ def __init__(self):
         if ret is not True:
             rospy.logerr('Could not open port!')
             sys.exit(1)
-        arm.search_servo_ids()
-        arm.search_worm_ids()
-        arm.search_wheel_sids()
         arm.all_jointbase_sensors()
-        arm.send_stretch(40)
+        # arm.send_stretch(40)
         self.id_to_index = self.arm.servo_id_to_index()
         self._prev_velocity_command = None
 
@@ -158,6 +167,8 @@ def __init__(self):
         self.joint_servo_on = {jn: False for jn in self.joint_names}
         servo_on_states = arm.servo_states()
         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
@@ -166,6 +177,22 @@ def __init__(self):
         print(self.joint_servo_on)
         print(self.id_to_index)
 
+        initial_positions = {}
+        init_av = arm.angle_vector()
+        arm.servo_id_to_index()
+        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 arm.wheel_servo_sorted_ids:
+                continue
+            initial_positions[jn] = float(
+                np.deg2rad(init_av[self.id_to_index[servo_id]]))
+        rospy.loginfo('run kxr_controller')
+        set_initial_position(initial_positions, namespace=clean_namespace)
+        self.proc_kxr_controller = run_kxr_controller(
+            namespace=clean_namespace)
+
         self.worm_servo_ids = [
             arm.memory_cstruct(WormmoduleStruct, idx).servo_id
             for idx in arm.worm_sorted_ids]
@@ -204,6 +231,15 @@ def __init__(self):
                 sensor_msgs.msg.Imu,
                 queue_size=1)
 
+    def __del__(self):
+        self.proc_controller_spawner.kill()
+        self.proc_robot_state_publisher.kill()
+        self.proc_kxr_controller.kill()
+
+    def unsubscribe(self):
+        self.command_joint_state_sub.unregister()
+        self.velocity_command_joint_state_sub.unregister()
+
     def velocity_command_joint_state_callback(self, msg):
         servo_ids = []
         av_length = len(self.arm.servo_sorted_ids)
@@ -246,8 +282,11 @@ def velocity_command_joint_state_callback(self, msg):
                 self._prev_velocity_command, svs):
             return
         self._prev_velocity_command = svs
-        with self.lock:
+        try:
             self.arm.servo_angle_vector(servo_ids, svs, velocity=0)
+        except RuntimeError as e:
+            self.unsubscribe()
+            rospy.signal_shutdown('Disconnected {}.'.format(e))
 
     def command_joint_state_callback(self, msg):
         servo_ids = []
@@ -294,14 +333,17 @@ def command_joint_state_callback(self, msg):
         indices = np.argsort(servo_ids)
         svs = svs[indices]
         servo_ids = np.array(servo_ids)[indices]
-        with self.lock:
+        try:
             if len(worm_indices) > 0:
                 worm_av_tmp = np.array(self.arm.read_cstruct_slot_vector(
                     WormmoduleStruct, 'ref_angle'), dtype=np.float32)
                 worm_av_tmp[np.array(worm_indices)] = np.array(worm_av)
                 self.arm.write_cstruct_slot_v(
                     WormmoduleStruct, 'ref_angle', worm_av_tmp)
             self.arm.servo_angle_vector(servo_ids, svs, velocity=0)
+        except RuntimeError as e:
+            self.unsubscribe()
+            rospy.signal_shutdown('Disconnected {}.'.format(e))
 
     def servo_on_off_callback(self, goal):
         servo_vector = []
@@ -321,8 +363,11 @@ def servo_on_off_callback(self, goal):
         indices = np.argsort(servo_ids)
         servo_vector = servo_vector[indices]
         servo_ids = np.array(servo_ids)[indices]
-        with self.lock:
+        try:
             self.arm.servo_angle_vector(servo_ids, servo_vector, velocity=10)
+        except RuntimeError as e:
+            self.unsubscribe()
+            rospy.signal_shutdown('Disconnected {}.'.format(e))
         return self.servo_on_off_server.set_succeeded(ServoOnOffResult())
 
     def create_imu_message(self):
@@ -342,10 +387,15 @@ def run(self):
 
         while not rospy.is_shutdown():
             if self.arm.is_opened() is False:
-                rospy.logwarn('Disconnected.')
+                self.unsubscribe()
+                rospy.signal_shutdown('Disconnected.')
                 break
-            with self.lock:
+            try:
                 av = self.arm.angle_vector()
+            except RuntimeError as e:
+                self.unsubscribe()
+                rospy.signal_shutdown('Disconnected {}.'.format(e))
+                break
             msg = JointState()
             msg.header.stamp = rospy.Time.now()
             for name in self.joint_names:
@@ -358,7 +408,7 @@ def run(self):
                         msg.name.append(name)
             self.current_joint_states_pub.publish(msg)
 
-            if self.publish_imu:
+            if self.publish_imu and self.imu_publisher.get_num_connections():
                 imu_msg = self.create_imu_message()
                 self.imu_publisher.publish(imu_msg)
             rate.sleep()

ros/kxr_controller/src/kxr_robot_hardware.cpp

@@ -56,6 +56,8 @@ namespace kxr_controller {
       }
       const std::string jointname = joint_pair.first;
       jointname_to_id_[jointname] = i;
+      robot_hw_nh.getParam(clean_namespace + "/initial_position/" + jointname, joint_position_command_[i]);
+      robot_hw_nh.getParam(clean_namespace + "/initial_position/" + jointname, joint_state_position_[i]);
 
       hardware_interface::JointStateHandle state_handle(jointname, &joint_state_position_[i], &joint_state_velocity_[i], &joint_state_effort_[i]);
       joint_state_interface.registerHandle(state_handle);

tests/rcb4_tests/test_armh7interface.py

@@ -15,7 +15,6 @@ class TestRobotModel(unittest.TestCase):
     def setUpClass(cls):
         cls.interface = ARMH7Interface()
         cls.interface.auto_open()
-        cls.interface.search_servo_ids()
 
     def test_servo_angle_vector(self):
         self.interface.hold()