From 60411c661991824d1c96ad88aea4a05523f903b6 Mon Sep 17 00:00:00 2001 From: hello-binit <64861565+hello-binit@users.noreply.github.com> Date: Thu, 11 Jun 2020 04:02:12 -0400 Subject: [PATCH] Cleaned up whitespace --- stretch_core/nodes/keyboard_teleop | 28 +++--- stretch_core/nodes/stretch_driver | 143 ++++++++++++++--------------- 2 files changed, 81 insertions(+), 90 deletions(-) diff --git a/stretch_core/nodes/keyboard_teleop b/stretch_core/nodes/keyboard_teleop index 767025a..06de74f 100755 --- a/stretch_core/nodes/keyboard_teleop +++ b/stretch_core/nodes/keyboard_teleop @@ -20,7 +20,7 @@ class GetKeyboardCommands: self.clean_surface_on = clean_surface_on self.grasp_object_on = grasp_object_on self.deliver_object_on = deliver_object_on - + self.step_size = 'medium' # self.persistent_command_count = 0 # self.prev_persistent_c = None @@ -81,10 +81,9 @@ class GetKeyboardCommands: print(' ') print('-------------------------------------------') - def get_command(self, node): command = None - + c = kb.getch() #rospy.loginfo('c =', c) if ((c == '+') or (c == '=')) and self.mapping_on: @@ -164,7 +163,7 @@ class GetKeyboardCommands: command = {'joint': 'rotate_mobile_base', 'inc': -self.get_deltas()['rad']} elif self.mode == 'navigation': rospy.loginfo('ERROR: Navigation mode is not currently supported.') - + if c == 'w' or c == 'W': command = {'joint': 'wrist_extension', 'delta': self.get_deltas()['translate']} if c == 'x' or c == 'X': @@ -201,7 +200,7 @@ class GetKeyboardCommands: rospy.signal_shutdown('Received quit character (q), so exiting') return command - + class KeyboardTeleopNode(hm.HelloNode): @@ -216,7 +215,7 @@ class KeyboardTeleopNode(hm.HelloNode): self.clean_surface_on = clean_surface_on self.grasp_object_on = grasp_object_on self.deliver_object_on = deliver_object_on - + def joint_states_callback(self, joint_state): self.joint_state = joint_state @@ -242,7 +241,6 @@ class KeyboardTeleopNode(hm.HelloNode): self.trajectory_client.send_goal(self.trajectory_goal) rospy.loginfo('Done sending pose.') - def main(self): hm.HelloNode.main(self, 'keyboard_teleop', 'keyboard_teleop', wait_for_first_pointcloud=False) @@ -252,7 +250,7 @@ class KeyboardTeleopNode(hm.HelloNode): rospy.wait_for_service('/funmap/trigger_head_scan') rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_head_scan.') self.trigger_head_scan_service = rospy.ServiceProxy('/funmap/trigger_head_scan', Trigger) - + rospy.wait_for_service('/funmap/trigger_drive_to_scan') rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_drive_to_scan.') self.trigger_drive_to_scan_service = rospy.ServiceProxy('/funmap/trigger_drive_to_scan', Trigger) @@ -286,7 +284,7 @@ class KeyboardTeleopNode(hm.HelloNode): rospy.wait_for_service('/open_drawer/trigger_open_drawer') rospy.loginfo('Node ' + self.node_name + ' connected to /open_drawer/trigger_open_drawer.') self.trigger_open_drawer_service = rospy.ServiceProxy('/open_drawer/trigger_open_drawer', Trigger) - + if self.clean_surface_on: rospy.wait_for_service('/clean_surface/trigger_clean_surface') rospy.loginfo('Node ' + self.node_name + ' connected to /clean_surface/trigger_clean_surface.') @@ -296,15 +294,14 @@ class KeyboardTeleopNode(hm.HelloNode): rospy.wait_for_service('/grasp_object/trigger_grasp_object') rospy.loginfo('Node ' + self.node_name + ' connected to /grasp_object/trigger_grasp_object.') self.trigger_grasp_object_service = rospy.ServiceProxy('/grasp_object/trigger_grasp_object', Trigger) - + if self.deliver_object_on: rospy.wait_for_service('/deliver_object/trigger_deliver_object') rospy.loginfo('Node ' + self.node_name + ' connected to /deliver_object/trigger_deliver_object.') self.trigger_deliver_object_service = rospy.ServiceProxy('/deliver_object/trigger_deliver_object', Trigger) - rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback) - + rate = rospy.Rate(self.rate) while not rospy.is_shutdown(): @@ -314,7 +311,7 @@ class KeyboardTeleopNode(hm.HelloNode): self.send_command(command) rate.sleep() - + if __name__ == '__main__': try: parser = ap.ArgumentParser(description='Keyboard teleoperation for stretch.') @@ -324,7 +321,7 @@ if __name__ == '__main__': parser.add_argument('--clean_surface_on', action='store_true', help='Enable Clean Surface trigger, which requires connection to the appropriate clean_surface service.') parser.add_argument('--grasp_object_on', action='store_true', help='Enable Grasp Object trigger, which requires connection to the appropriate grasp_object service.') parser.add_argument('--deliver_object_on', action='store_true', help='Enable Deliver Object trigger, which requires connection to the appropriate deliver_object service.') - + args, unknown = parser.parse_known_args() mapping_on = args.mapping_on hello_world_on = args.hello_world_on @@ -332,9 +329,8 @@ if __name__ == '__main__': clean_surface_on = args.clean_surface_on grasp_object_on = args.grasp_object_on deliver_object_on = args.deliver_object_on - + node = KeyboardTeleopNode(mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, deliver_object_on) node.main() except KeyboardInterrupt: rospy.loginfo('interrupt received, so shutting down') - diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 6242b01..5878ef6 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -73,7 +73,7 @@ class GripperConversion: self.open_robotis = 70.0 self.closed_robotis = 0.0 - + self.robotis_to_aperture_slope = ((self.open_aperture_m - self.closed_aperture_m) / (self.open_robotis - self.closed_robotis)) def robotis_to_aperture(self, robotis_in): @@ -85,7 +85,7 @@ class GripperConversion: # linear model robotis_out = ((aperture_m - self.closed_aperture_m) / self.robotis_to_aperture_slope) + self.closed_robotis return robotis_out - + def aperture_to_finger_rad(self, aperture_m): # arc length / radius = ang_rad finger_rad = (aperture_m/2.0)/self.finger_length_m @@ -94,12 +94,12 @@ class GripperConversion: def finger_rad_to_aperture(self, finger_rad): aperture_m = 2.0 * (finger_rad * self.finger_length_m) return aperture_m - + def finger_to_robotis(self, finger_ang_rad): aperture_m = self.finger_rad_to_aperture(finger_ang_rad) robotis_out = self.aperture_to_robotis(aperture_m) return robotis_out - + def status_to_all(self, gripper_status): aperture_m = self.robotis_to_aperture(gripper_status['pos_pct']) finger_rad = self.aperture_to_finger_rad(aperture_m) @@ -107,13 +107,13 @@ class GripperConversion: finger_vel = (self.robotis_to_aperture_slope * gripper_status['vel'])/2.0 return aperture_m, finger_rad, finger_effort, finger_vel - + class SimpleCommandGroup: def __init__(self, joint_name, acceptable_joint_error=0.015, clip_ros_tolerance=0.001): self.clip_ros_tolerance = clip_ros_tolerance self.acceptable_joint_error = acceptable_joint_error self.joint_name = joint_name - + def update(self, joint_names, invalid_joints_error_func, robot_mode): self.use_joint = False if self.joint_name in joint_names: @@ -150,19 +150,20 @@ class SimpleCommandGroup: def update_execution(self, robot_status, backlash_state): # This method needs to be implemented. It also needs to set self.joint_error. return None - + def goal_reached(self): if self.joint_goal: return (abs(self.joint_error) < self.acceptable_joint_error) else: return True + class HeadPanCommandGroup(SimpleCommandGroup): def __init__(self, head_pan_calibrated_offset, head_pan_calibrated_looked_left_offset): SimpleCommandGroup.__init__(self, 'joint_head_pan', acceptable_joint_error=0.15, clip_ros_tolerance=0.001) self.head_pan_calibrated_offset = head_pan_calibrated_offset self.head_pan_calibrated_looked_left_offset = head_pan_calibrated_looked_left_offset - + def update_execution(self, robot_status, backlash_state): if self.joint_goal: if backlash_state['head_pan_looked_left']: @@ -211,14 +212,13 @@ class WristYawCommandGroup(SimpleCommandGroup): return None - class GripperCommandGroup: def __init__(self, acceptable_joint_error=0.015, clip_ros_tolerance=0.001): self.clip_ros_tolerance = clip_ros_tolerance self.acceptable_joint_error = acceptable_joint_error self.gripper_joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture'] self.gripper_conversion = GripperConversion() - + def update(self, joint_names, invalid_joints_error_func, robot_mode): self.use_gripper_joint = False self.gripper_joints_to_command_names = [j for j in self.gripper_joint_names if j in joint_names] @@ -236,7 +236,7 @@ class GripperCommandGroup: self.gripper_joint_command_name = self.gripper_joints_to_command_names[0] self.gripper_joint_command_index = self.gripper_joints_to_command_indices[0] self.use_gripper_joint = True - + return True def get_num_valid_commands(self): @@ -268,7 +268,7 @@ class GripperCommandGroup: invalid_goal_error_func(error_string) return False return True - + def ros_to_mechaduino(self, joint_ros): return joint_ros @@ -277,14 +277,14 @@ class GripperCommandGroup: def update_execution(self, robot_status, backlash_state): pass - + def goal_reached(self): # TODO: check the gripper state if self.use_gripper_joint: return True else: return True - + class TelescopingCommandGroup: def __init__(self, wrist_extension_calibrated_retracted_offset): @@ -355,13 +355,13 @@ class TelescopingCommandGroup: return self.extension_error_m else: return None - + def goal_reached(self): if self.extension_goal: return (abs(self.extension_error_m) < self.acceptable_joint_error_m) else: return True - + class LiftCommandGroup: def __init__(self, max_arm_height): @@ -410,13 +410,13 @@ class LiftCommandGroup: return self.lift_error_m else: return None - + def goal_reached(self): if self.lift_goal: return (abs(self.lift_error_m) < self.acceptable_joint_error_m) else: return True - + class MobileBaseCommandGroup: def __init__(self): @@ -433,7 +433,7 @@ class MobileBaseCommandGroup: self.use_mobile_base_inc_trans = ('translate_mobile_base' in joint_names) self.use_mobile_base_inc = (self.use_mobile_base_inc_rot or self.use_mobile_base_inc_trans) self.use_mobile_base_virtual_joint = ('joint_mobile_base_translation' in joint_names) - + if self.use_mobile_base_inc and self.use_mobile_base_virtual_joint: # Commands that attempt to control the mobile base's # virtual joint together with mobile base incremental @@ -446,7 +446,7 @@ class MobileBaseCommandGroup: error_string = 'received a command for the mobile base virtual joint (joint_mobile_base_translation) and mobile base incremental motions ({0}). These are mutually exclusive options. The joint names in the received command = {1}'.format(command_string, joint_names) invalid_joints_error_func(error_string) return False - + if self.use_mobile_base_virtual_joint: # Check if a mobile base command was received. if robot_mode != 'manipulation': @@ -470,7 +470,7 @@ class MobileBaseCommandGroup: self.translate_mobile_base_index = joint_names.index('translate_mobile_base') return True - + def get_num_valid_commands(self): number_of_valid_joints = 0 if self.use_mobile_base_virtual_joint: @@ -485,7 +485,7 @@ class MobileBaseCommandGroup: self.rotate_mobile_base_goal = False self.translate_mobile_base_goal = False self.virtual_joint_mobile_base_goal = False - + if self.use_mobile_base_virtual_joint: self.goal_mobile_base_virtual_joint_ros = point.positions[self.virtual_joint_mobile_base_index] self.goal_mobile_base_virtual_joint_mecha = self.ros_to_mechaduino(self.goal_mobile_base_virtual_joint_ros, manipulation_origin) @@ -505,7 +505,7 @@ class MobileBaseCommandGroup: return False return True - + def ros_to_mechaduino(self, ros_ros, manipulation_origin): bounds = self.mobile_base_virtual_joint_ros_range ros_pos = bound_ros_command(bounds, ros_ros, self.clip_ros_tolerance) @@ -551,7 +551,7 @@ class MobileBaseCommandGroup: b = robot_status['base'] speed = np.sqrt(np.square(b['x_vel']) + np.square(b['y_vel'])) self.mobile_base_goal_reached = self.mobile_base_goal_reached and (speed < min_m_per_s) - + elif self.rotate_mobile_base_goal: incremental_rad = hm.angle_diff_rad(current_mobile_base_rotation_mecha, self.initial_mobile_base_rotation_mecha) self.mobile_base_error_rad = hm.angle_diff_rad(self.goal_rotate_mobile_base_mecha, incremental_rad) @@ -565,7 +565,7 @@ class MobileBaseCommandGroup: min_rad_per_s = min_deg_per_s * (np.pi/180.0) self.mobile_base_goal_reached = self.mobile_base_goal_reached and (abs(robot_status['base']['theta_vel']) < min_rad_per_s) return self.mobile_base_error_m, self.mobile_base_error_rad - + def goal_reached(self): return self.mobile_base_goal_reached @@ -595,9 +595,9 @@ class StretchBodyNode: self.head_tilt_calibrated_looking_up_offset_rad = 0.0 self.wrist_extension_calibrated_retracted_offset_m = 0.0 self.head_tilt_backlash_transition_angle_rad = -0.4 - + self.gripper_conversion = GripperConversion() - + self.mobile_base_manipulation_origin = None #{'x':None, 'y':None, 'theta':None} self.use_lift = True @@ -605,10 +605,10 @@ class StretchBodyNode: # Clip the command, instead of invalidating it, if it is close # enough to the valid ranges. self.trajectory_debug = True - + self.robot_stop_lock = threading.Lock() self.stop_the_robot = False - + self.robot_mode_lock = threading.Lock() with self.robot_mode_lock: self.robot_mode = None @@ -616,7 +616,6 @@ class StretchBodyNode: self.mode_change_polling_rate_hz = 4.0 - def trajectory_action_server_callback(self, goal): with self.robot_stop_lock: @@ -632,7 +631,7 @@ class StretchBodyNode: # command was sent before the stop_the_robot service # trigger. self.stop_the_robot = False - + with self.robot_mode_lock: self.robot_mode_read_only += 1 @@ -662,7 +661,7 @@ class StretchBodyNode: self.trajectory_action_server.set_aborted(result) with self.robot_mode_lock: self.robot_mode_read_only -= 1 - + # For now, ignore goal time and configuration tolerances. joint_names = goal.trajectory.joint_names if self.trajectory_debug: @@ -683,7 +682,7 @@ class StretchBodyNode: return number_of_valid_joints = sum([c.get_num_valid_commands() for c in command_groups]) - + if number_of_valid_joints <= 0: # Abort if no valid joints were received. error_string = 'received a command without any valid joint names. Received joint names = ' + str(joint_names) @@ -702,14 +701,14 @@ class StretchBodyNode: if self.trajectory_debug: rospy.loginfo('position # {0} = {1}'.format(point_number, point.positions)) - + valid_goals = [c.set_goal(point, invalid_goal_error, self.mobile_base_manipulation_origin) for c in command_groups] if not all(valid_goals): # At least one of the goals violated the requirements # of a command group. Any violations should have been # reported as errors by the command groups. return - + # Attempt to reach the goal. update_rate = rospy.Rate(15.0) @@ -717,7 +716,7 @@ class StretchBodyNode: incremental_commands_executed = False goal_start_time = rospy.Time.now() - + while True: # Get copy of the current robot status (uses lock held by the robot). robot_status = self.robot.get_status() @@ -735,7 +734,7 @@ class StretchBodyNode: self.head_tilt_cg.update_execution(robot_status, self.backlash_state) self.wrist_yaw_cg.update_execution(robot_status, self.backlash_state) self.gripper_cg.update_execution(robot_status, self.backlash_state) - + # Check if a premption request has been received. with self.robot_stop_lock: if self.stop_the_robot or self.trajectory_action_server.is_preempt_requested(): @@ -777,14 +776,14 @@ class StretchBodyNode: self.backlash_state['head_pan_looked_left'] = True else: self.backlash_state['head_pan_looked_left'] = False - + if self.head_tilt_cg.joint_goal: self.robot.head.move_by('head_tilt', self.head_tilt_cg.joint_error) if self.head_tilt_cg.joint_target > (self.head_tilt_backlash_transition_angle_rad + self.head_tilt_calibrated_offset_rad): self.backlash_state['head_tilt_looking_up'] = True else: self.backlash_state['head_tilt_looking_up'] = False - + if self.wrist_yaw_cg.joint_goal: self.robot.end_of_arm.move_to('wrist_yaw', self.wrist_yaw_cg.joint_target) @@ -808,7 +807,7 @@ class StretchBodyNode: error_string = 'time to execute the current goal point = {0} exceeded the default_goal_timeout = {1}'.format(point, self.default_goal_timeout_s) goal_tolerance_violated(error_string) return - + update_rate.sleep() # Currently not providing feedback. @@ -824,9 +823,8 @@ class StretchBodyNode: self.robot_mode_read_only -= 1 return - ###### MOBILE BASE VELOCITY METHODS ####### - + def set_mobile_base_velocity_callback(self, twist): # check on thread safety for this callback function if self.robot_mode != 'navigation': @@ -836,7 +834,7 @@ class StretchBodyNode: self.linear_velocity_mps = twist.linear.x self.angular_velocity_radps = twist.angular.z self.last_twist_time = rospy.get_time() - + def command_mobile_base_velocity_and_publish_state(self): with self.robot_mode_lock: self.robot_mode_read_only += 1 @@ -867,9 +865,9 @@ class StretchBodyNode: #robot_time = robot_status['timestamp_pc'] #rospy.loginfo('robot_time =', robot_time) #current_time = rospy.Time.from_sec(robot_time) - + current_time = rospy.Time.now() - + # obtain odometry # assign relevant base status to variables base_status = robot_status['base'] @@ -920,7 +918,7 @@ class StretchBodyNode: wrist_rad = wrist_status['pos'] wrist_vel = wrist_status['vel'] wrist_effort = wrist_status['effort'] - + # assign relevant gripper status to variables gripper_status = robot_status['end_of_arm']['stretch_gripper'] if GRIPPER_DEBUG: @@ -1047,7 +1045,7 @@ class StretchBodyNode: positions.append(gripper_finger_rad) velocities.append(gripper_finger_vel) efforts.append(gripper_finger_effort) - + # set virtual joint for mobile base translation joint_state.name.append('joint_mobile_base_translation') if self.robot_mode == 'manipulation': @@ -1090,7 +1088,7 @@ class StretchBodyNode: i.linear_acceleration.y = ay i.linear_acceleration.z = az self.imu_mobile_base_pub.publish(i) - + m = MagneticField() m.header.stamp = current_time m.header.frame_id = 'imu_mobile_base' @@ -1109,14 +1107,13 @@ class StretchBodyNode: i.linear_acceleration.z = az self.imu_wrist_pub.publish(i) ################################################## - + with self.robot_mode_lock: self.robot_mode_read_only -= 1 return - ######## CHANGE MODES ######### - + def change_mode(self, new_mode, code_to_run): polling_rate = rospy.Rate(self.mode_change_polling_rate_hz) changed = False @@ -1131,7 +1128,7 @@ class StretchBodyNode: polling_rate.sleep() # TODO : add a freewheel mode or something comparable for the mobile base? - + def turn_on_navigation_mode(self): # Navigation mode enables mobile base velocity control via # cmd_vel, and disables position-based control of the mobile @@ -1140,7 +1137,7 @@ class StretchBodyNode: self.linear_velocity_mps = 0.0 self.angular_velocity_radps = 0.0 self.change_mode('navigation', code_to_run) - + def turn_on_manipulation_mode(self): # Manipulation mode enables mobile base translation using # position control via joint_mobile_base_translation, and @@ -1173,7 +1170,7 @@ class StretchBodyNode: def code_to_run(): self.robot.base.enable_pos_incr_mode() self.change_mode('position', code_to_run) - + def calibrate(self): def code_to_run(): if self.use_lift: @@ -1181,14 +1178,12 @@ class StretchBodyNode: self.robot.arm.home() self.change_mode('calibration', code_to_run) - ############################### - ######## SERVICE CALLBACKS ####### def stop_the_robot_callback(self, request): with self.robot_stop_lock: self.stop_the_robot = True - + self.robot.base.translate_by(0.0) self.robot.base.rotate_by(0.0) self.robot.arm.move_by(0.0) @@ -1200,13 +1195,13 @@ class StretchBodyNode: self.robot.end_of_arm.move_by('wrist_yaw', 0.0) self.robot.end_of_arm.move_by('stretch_gripper', 0.0) self.robot.push_command() - + rospy.loginfo('Received stop_the_robot service call, so commanded all actuators to stop.') return TriggerResponse( success=True, message='Stopped the robot.' ) - + def navigation_mode_service_callback(self, request): self.turn_on_navigation_mode() return TriggerResponse( @@ -1227,10 +1222,11 @@ class StretchBodyNode: success=True, message='Now in position mode.' ) - ############################### - + + ########### MAIN ############ + def main(self): - + rospy.init_node('stretch_driver') self.node_name = rospy.get_name() @@ -1245,9 +1241,9 @@ class StretchBodyNode: rospy.loginfo('broadcast_odom_tf = ' + str(self.broadcast_odom_tf)) if self.broadcast_odom_tf: self.tf_broadcaster = tf2_ros.TransformBroadcaster() - + large_ang = 45.0 * np.pi/180.0 - + filename = rospy.get_param('~controller_calibration_file') rospy.loginfo('Loading controller calibration parameters for the head from YAML file named {0}'.format(filename)) with open(filename, 'r') as fid: @@ -1288,12 +1284,12 @@ class StretchBodyNode: if (abs(m) > 0.05): rospy.loginfo('WARNING: self.wrist_extension_calibrated_retracted_offset_m HAS AN UNUSUALLY LARGE MAGNITUDE') rospy.loginfo('self.wrist_extension_calibrated_retracted_offset_m in meters = {0}'.format(self.wrist_extension_calibrated_retracted_offset_m)) - + self.linear_velocity_mps = 0.0 # m/s ROS SI standard for cmd_vel (REP 103) self.angular_velocity_radps = 0.0 # rad/s ROS SI standard for cmd_vel (REP 103) - + self.max_arm_height = 1.1 - + self.telescoping_cg = TelescopingCommandGroup(self.wrist_extension_calibrated_retracted_offset_m) if self.use_lift: self.lift_cg = LiftCommandGroup(self.max_arm_height) @@ -1304,14 +1300,13 @@ class StretchBodyNode: self.head_tilt_calibrated_looking_up_offset_rad) self.wrist_yaw_cg = WristYawCommandGroup() self.gripper_cg = GripperCommandGroup() - + self.odom_pub = rospy.Publisher('odom', Odometry, queue_size=1) - + self.imu_mobile_base_pub = rospy.Publisher('imu_mobile_base', Imu, queue_size=1) self.magnetometer_mobile_base_pub = rospy.Publisher('magnetometer_mobile_base', MagneticField, queue_size=1) self.imu_wrist_pub = rospy.Publisher('imu_wrist', Imu, queue_size=1) - - + rospy.Subscriber("cmd_vel", Twist, self.set_mobile_base_velocity_callback) # ~ symbol gets parameter from private namespace @@ -1330,12 +1325,12 @@ class StretchBodyNode: self.robot = rb.Robot() self.robot.startup() - + # TODO: check with the actuators to see if calibration is required #self.calibrate() - + self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1) - + command_base_velocity_and_publish_joint_state_rate = rospy.Rate(self.joint_state_rate) self.last_twist_time = rospy.get_time()