diff --git a/stretch_core/stretch_core/stretch_driver.py b/stretch_core/stretch_core/stretch_driver.py index d7daebff..078b710a 100755 --- a/stretch_core/stretch_core/stretch_driver.py +++ b/stretch_core/stretch_core/stretch_driver.py @@ -29,10 +29,8 @@ from nav_msgs.msg import Odometry from rcl_interfaces.msg import ParameterDescriptor, ParameterType, SetParametersResult from sensor_msgs.msg import BatteryState, JointState, Imu, MagneticField, Joy -from std_msgs.msg import Bool, String, Float64MultiArray - +from std_msgs.msg import Bool, String from hello_helpers.gripper_conversion import GripperConversion -from hello_helpers.joint_qpos_conversion import get_Idx, UnsupportedToolError from hello_helpers.hello_misc import LoopTimer from hello_helpers.gamepad_conversion import unpack_joy_to_gamepad_state, unpack_gamepad_state_to_joy, get_default_joy_msg from .joint_trajectory_server import JointTrajectoryAction @@ -40,7 +38,7 @@ GRIPPER_DEBUG = False BACKLASH_DEBUG = False -STREAMING_POSITION_DEBUG = False +STREAMING_POSITION_DEBUG = True class StretchDriver(Node): @@ -113,7 +111,7 @@ def set_mobile_base_velocity_callback(self, twist): self.last_twist_time = self.get_clock().now() self.robot_mode_rwlock.release_read() - def set_robot_streaming_position_callback(self, msg): + def set_robot_streaming_position_callback(self, msg: JointState): self.robot_mode_rwlock.acquire_read() if not self.streaming_position_activated: self.get_logger().error('Streaming position is not activated.' @@ -132,40 +130,44 @@ def set_robot_streaming_position_callback(self, msg): if (self.get_clock().now().nanoseconds * 1e-9) - self.streaming_controller_lt.last_update_time > 5.0: print('Reset Streaming position looptimer after 5s no message received.') self.streaming_controller_lt.reset() - qpos = msg.data - self.move_to_position(qpos) + self.move_to_position(msg) self.robot_mode_rwlock.release_read() if STREAMING_POSITION_DEBUG: self.streaming_controller_lt.update() - def move_to_position(self, qpos): - try: - try: - Idx = get_Idx(self.robot.params['tool']) - except UnsupportedToolError: - self.get_logger().error('Unsupported tool for streaming position control.') - if len(qpos) != Idx.num_joints: - self.get_logger().error('Received qpos does not match the number of joints in the robot') - return - self.robot.arm.move_to(qpos[Idx.ARM]) - self.robot.lift.move_to(qpos[Idx.LIFT]) - self.robot.end_of_arm.move_to('wrist_yaw', qpos[Idx.WRIST_YAW]) - if 'wrist_pitch' in self.robot.end_of_arm.joints: - self.robot.end_of_arm.move_to('wrist_pitch', qpos[Idx.WRIST_PITCH]) - if 'wrist_roll' in self.robot.end_of_arm.joints: - self.robot.end_of_arm.move_to('wrist_roll', qpos[Idx.WRIST_ROLL]) - self.robot.head.move_to('head_pan', qpos[Idx.HEAD_PAN]) - self.robot.head.move_to('head_tilt', qpos[Idx.HEAD_TILT]) - if abs(qpos[Idx.BASE_TRANSLATE]) > 0.0 and abs(qpos[Idx.BASE_ROTATE]) > 0.0 and self.robot_mode != 'position': + def move_to_position(self, msg: JointState): + try: + if 'joint_base_translate' in msg.name and 'joint_base_rotate' in msg.name: self.get_logger().error('Cannot move base in both translation and rotation at the same time in position mode') - elif abs(qpos[Idx.BASE_TRANSLATE]) > 0.0 and self.robot_mode == 'position': - self.robot.base.translate_by(qpos[Idx.BASE_TRANSLATE]) - elif abs(qpos[Idx.BASE_ROTATE]) > 0.0 and self.robot_mode == 'position': - self.robot.base.rotate_by(qpos[Idx.BASE_ROTATE]) - if 'stretch_gripper' in self.robot.end_of_arm.joints: - pos = self.gripper_conversion.finger_to_robotis(qpos[Idx.GRIPPER]) - self.robot.end_of_arm.move_to('stretch_gripper', pos) - self.get_logger().info(f"Moved to position qpos: {qpos}") + return + + pos_length_match = len(msg.name) == len(msg.position) + if not pos_length_match: + self.get_logger().error('Streaming Position Command: Length of joint names and position list does not match.') + return + + vel_present = len(msg.velocity) > 0 + if vel_present and len(msg.name) != len(msg.velocity): + self.get_logger().error('Streaming Position Command: Length of joint names and velocity list does not match.') + return + + joint_move_map = { + 'joint_lift': lambda pos, vel: self.robot.lift.move_to(pos, vel), + 'joint_arm': lambda pos, vel: self.robot.arm.move_to(pos, vel), + 'joint_wrist_yaw': lambda pos, vel: self.robot.end_of_arm.move_to('wrist_yaw', pos, vel), + 'joint_wrist_pitch': lambda pos, vel: self.robot.end_of_arm.move_to('wrist_pitch', pos, vel) if 'wrist_pitch' in self.robot.end_of_arm.joints else None, + 'joint_wrist_roll': lambda pos, vel: self.robot.end_of_arm.move_to('wrist_roll', pos, vel) if 'wrist_roll' in self.robot.end_of_arm.joints else None, + 'joint_head_pan': lambda pos, vel: self.robot.head.move_to('head_pan', pos, vel), + 'joint_head_tilt': lambda pos, vel: self.robot.head.move_to('head_tilt', pos, vel), + 'joint_base_translate': lambda pos, vel: self.robot.base.translate_by(pos, vel), + 'joint_base_rotate': lambda pos, vel: self.robot.base.rotate_by(pos, vel), + 'joint_gripper': lambda pos, vel: self.robot.end_of_arm.move_to('stretch_gripper', self.gripper_conversion.finger_to_robotis(pos), vel) if 'stretch_gripper' in self.robot.end_of_arm.joints else None, + } + + for joint, pos, i in zip(msg.name, msg.position, range(len(msg.name))): + v = msg.velocity[i] if vel_present else None + if joint in joint_move_map: + joint_move_map[joint](pos, v) except Exception as e: self.get_logger().error('Failed to move to position: {0}'.format(e)) @@ -989,7 +991,7 @@ def ros_setup(self): self.create_subscription(Joy, "gamepad_joy", self.set_gamepad_motion_callback, 1, callback_group=self.group) - self.create_subscription(Float64MultiArray, "joint_pose_cmd", self.set_robot_streaming_position_callback, 1, callback_group=self.group) + self.create_subscription(JointState, "joint_pose_cmd", self.set_robot_streaming_position_callback, 1, callback_group=self.group) self.declare_parameter('rate', 30.0) self.joint_state_rate = self.get_parameter('rate').value diff --git a/stretch_core/test/test_streaming_joint_state_position_commands.py b/stretch_core/test/test_streaming_joint_state_position_commands.py new file mode 100644 index 00000000..f5a928f4 --- /dev/null +++ b/stretch_core/test/test_streaming_joint_state_position_commands.py @@ -0,0 +1,183 @@ +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64MultiArray +from hello_helpers.hello_misc import LoopTimer +from sensor_msgs.msg import JointState +from std_srvs.srv import Trigger +from hello_helpers.gripper_conversion import GripperConversion +from rclpy.callback_groups import ReentrantCallbackGroup +import numpy as np +import time + + + +class JointPosePublisher(Node): + def __init__(self): + rclpy.init() + super().__init__('float_array_publisher') + self.reentrant_cb = ReentrantCallbackGroup() + self.publisher_ = self.create_publisher(JointState, 'joint_pose_cmd', 10, callback_group=self.reentrant_cb) + # subscribe to joint states + self.joint_state = JointState() + self.joint_states_subscriber = self.create_subscription(JointState, + '/stretch/joint_states', + self.joint_states_callback, 10,callback_group=self.reentrant_cb) + self.switch_to_position_mode_service = self.create_client(Trigger, '/switch_to_position_mode',callback_group=self.reentrant_cb) + + self.switch_to_navigation_mode_service = self.create_client(Trigger, '/switch_to_navigation_mode',callback_group=self.reentrant_cb) + + self.activate_streaming_position_service = self.create_client(Trigger, '/activate_streaming_position', callback_group=self.reentrant_cb) + self.deactivate_streaming_position_service = self.create_client(Trigger, '/deactivate_streaming_position', callback_group=self.reentrant_cb) + + while not self.switch_to_position_mode_service.wait_for_service(timeout_sec=2.0): + self.get_logger().info("Waiting on '/switch_to_position_mode' service...") + self.gripper_conversion = GripperConversion() + self.lt = LoopTimer(print_debug=True) + + def joint_states_callback(self, msg): + self.joint_state = msg + + def switch_to_position_mode(self): + trigger_request = Trigger.Request() + trigger_result = self.switch_to_position_mode_service.call_async(trigger_request) + return + + def activate_streaming_position(self): + trigger_request = Trigger.Request() + trigger_result = self.activate_streaming_position_service.call_async(trigger_request) + time.sleep(1) + + def deactivate_streaming_position(self): + trigger_request = Trigger.Request() + trigger_result = self.deactivate_streaming_position_service.call_async(trigger_request) + return + + + def switch_to_navigation_mode(self): + trigger_request = Trigger.Request() + trigger_result = self.switch_to_navigation_mode_service.call_async(trigger_request) + return + + def parse_joint_state(self, joint_state_msg): + joint_status = {} + for name, position in zip(joint_state_msg.name, joint_state_msg.position): + joint_status[name] = position + return joint_status + + def publish_joint_pose(self, joint_pose): + self.publisher_.publish(joint_pose) + self.get_logger().info('Publishing: "%s"' % joint_pose) + +if __name__ == '__main__': + joint_pose_publisher = JointPosePublisher() + rclpy.spin_once(joint_pose_publisher) + + # joint_pose_publisher.switch_to_navigation_mode() + joint_pose_publisher.activate_streaming_position() + + home_pose = { + 'joint_lift': 0.6, + 'joint_arm': 0.0, + 'joint_wrist_yaw': 0.0, + 'joint_wrist_pitch': 0.0, + 'joint_wrist_roll': 0.0, + 'joint_head_pan': 0.0, + 'joint_head_tilt': 0.0, + # 'joint_base_translate', + # 'joint_base_rotate', + 'joint_gripper': 0 + } + goal = JointState() + for name, pos in home_pose.items(): + goal.name.append(name) + goal.position.append(pos) + joint_pose_publisher.publish_joint_pose(goal) + time.sleep(3) + + start = time.time() + target_rate = 30 + + i = 0 + joint_pose_publisher.lt.reset() + while i<100: + start = time.time() + i = 1 + i + rclpy.spin_once(joint_pose_publisher) + j_state = joint_pose_publisher.joint_state + goal = JointState() + for name, pos in zip(j_state.name, j_state.position): + if name == 'joint_lift': + goal.name.append('joint_lift') + goal.position.append(pos + 0.05) + elif name == "wrist_extension": + goal.name.append('joint_arm') + goal.position.append(pos + 0.05) + elif name == 'joint_wrist_pitch': + goal.name.append('joint_wrist_pitch') + goal.position.append(pos + 0.1) + elif name == 'joint_wrist_roll': + goal.name.append('joint_wrist_roll') + goal.position.append(pos + 0.1) + elif name == 'joint_wrist_yaw': + goal.name.append('joint_wrist_yaw') + goal.position.append(pos + 0.1) + elif name == 'joint_head_pan': + goal.name.append('joint_head_pan') + goal.position.append(pos + 0.1) + elif name == 'joint_head_tilt': + goal.name.append('joint_head_tilt') + goal.position.append(pos + 0.1) + elif name == 'joint_gripper_finger_left': + goal.name.append('joint_gripper') + goal.position.append(pos + 0.1) + joint_pose_publisher.publish_joint_pose(goal) + joint_pose_publisher.lt.update() + elp = time.time()-start + if elp<1/target_rate: + time.sleep((1/target_rate)-elp) + + + i = 0 + joint_pose_publisher.lt.reset() + while i<100: + start = time.time() + i = 1 + i + rclpy.spin_once(joint_pose_publisher) + j_state = joint_pose_publisher.joint_state + print(j_state) + goal = JointState() + for name, pos in zip(j_state.name, j_state.position): + if name == 'joint_lift': + goal.name.append('joint_lift') + goal.position.append(pos - 0.05) + elif name == "wrist_extension": + goal.name.append('joint_arm') + goal.position.append(pos - 0.05) + elif name == 'joint_wrist_pitch': + goal.name.append('joint_wrist_pitch') + goal.position.append(pos - 0.1) + elif name == 'joint_wrist_roll': + goal.name.append('joint_wrist_roll') + goal.position.append(pos - 0.1) + elif name == 'joint_wrist_yaw': + goal.name.append('joint_wrist_yaw') + goal.position.append(pos - 0.1) + elif name == 'joint_head_pan': + goal.name.append('joint_head_pan') + goal.position.append(pos - 0.1) + elif name == 'joint_head_tilt': + goal.name.append('joint_head_tilt') + goal.position.append(pos - 0.1) + elif name == 'joint_gripper_finger_left': + goal.name.append('joint_gripper') + goal.position.append(pos - 0.1) + joint_pose_publisher.publish_joint_pose(goal) + joint_pose_publisher.lt.update() + elp = time.time()-start + if elp<1/target_rate: + time.sleep((1/target_rate)-elp) + + + + joint_pose_publisher.destroy_node() + rclpy.shutdown() \ No newline at end of file