diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py index 2189a75e..39bb34fa 100644 --- a/rcb4/armh7interface.py +++ b/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(' 0: - return servo_on_indices + return self.servo_sorted_ids[servo_on_indices] return [] diff --git a/ros/kxr_controller/launch/kxr_controller.launch b/ros/kxr_controller/launch/kxr_controller.launch index 83101f66..01f9ae7b 100644 --- a/ros/kxr_controller/launch/kxr_controller.launch +++ b/ros/kxr_controller/launch/kxr_controller.launch @@ -20,15 +20,6 @@ - - - - - @@ -45,14 +36,6 @@ - - - - diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py index 275f31df..ae48d5bd 100644 --- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py +++ b/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,7 +333,7 @@ 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) @@ -302,6 +341,9 @@ def command_joint_state_callback(self, msg): 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() diff --git a/ros/kxr_controller/src/kxr_robot_hardware.cpp b/ros/kxr_controller/src/kxr_robot_hardware.cpp index 7449a6c8..49a9f09e 100644 --- a/ros/kxr_controller/src/kxr_robot_hardware.cpp +++ b/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); diff --git a/tests/rcb4_tests/test_armh7interface.py b/tests/rcb4_tests/test_armh7interface.py index 61404113..7bfafb0b 100644 --- a/tests/rcb4_tests/test_armh7interface.py +++ b/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()