Streaming Position command to receive JointState type message

stretch_core/stretch_core/stretch_driver.py

@@ -29,18 +29,16 @@
 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
 from .stretch_diagnostics import StretchDiagnostics
 
 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

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()