diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index b117cbc..4217b4e 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -1252,7 +1252,6 @@ class StretchBodyNode: rospy.loginfo("{0} started".format(self.node_name)) - filename = rospy.get_param('~controller_calibration_file') mode = rospy.get_param('~mode', "position") rospy.loginfo('mode = ' + str(mode)) @@ -1263,46 +1262,46 @@ class StretchBodyNode: 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)) - fid = open(filename, 'r') - controller_parameters = yaml.load(fid) - fid.close() - - rospy.loginfo('controller parameters loaded = {0}'.format(controller_parameters)) - - deg_per_rad = 180.0/math.pi - self.head_tilt_calibrated_offset_rad = controller_parameters['tilt_angle_offset'] - ang = self.head_tilt_calibrated_offset_rad - if (abs(ang) > large_ang): - rospy.loginfo('WARNING: self.head_tilt_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') - rospy.loginfo('self.head_tilt_calibrated_offset_rad in degrees = {0}'.format(self.head_tilt_calibrated_offset_rad * deg_per_rad)) - - self.head_pan_calibrated_offset_rad = controller_parameters['pan_angle_offset'] - ang = self.head_pan_calibrated_offset_rad - if (abs(ang) > large_ang): - rospy.loginfo('WARNING: self.head_pan_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') - rospy.loginfo('self.head_pan_calibrated_offset_rad in degrees = {0}'.format(self.head_pan_calibrated_offset_rad * deg_per_rad)) - - self.head_pan_calibrated_looked_left_offset_rad = controller_parameters['pan_looked_left_offset'] - ang = self.head_pan_calibrated_looked_left_offset_rad - if (abs(ang) > large_ang): - rospy.loginfo('WARNING: self.head_pan_calibrated_looked_left_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') - rospy.loginfo('self.head_pan_calibrated_looked_left_offset_rad in degrees = {0}'.format(self.head_pan_calibrated_looked_left_offset_rad * deg_per_rad)) - - self.head_tilt_backlash_transition_angle_rad = controller_parameters['tilt_angle_backlash_transition'] - rospy.loginfo('self.head_tilt_backlash_transition_angle_rad in degrees = {0}'.format(self.head_tilt_backlash_transition_angle_rad * deg_per_rad)) - - self.head_tilt_calibrated_looking_up_offset_rad = controller_parameters['tilt_looking_up_offset'] - ang = self.head_tilt_calibrated_looking_up_offset_rad - if (abs(ang) > large_ang): - rospy.loginfo('WARNING: self.head_tilt_calibrated_looking_up_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') - rospy.loginfo('self.head_tilt_calibrated_looking_up_offset_rad in degrees = {0}'.format(self.head_tilt_calibrated_looking_up_offset_rad * deg_per_rad)) - - self.wrist_extension_calibrated_retracted_offset_m = controller_parameters['arm_retracted_offset'] - m = self.wrist_extension_calibrated_retracted_offset_m - 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)) + with open(filename, 'r') as fid: + controller_parameters = yaml.safe_load(fid) + + rospy.loginfo('controller parameters loaded = {0}'.format(controller_parameters)) + + deg_per_rad = 180.0/math.pi + self.head_tilt_calibrated_offset_rad = controller_parameters['tilt_angle_offset'] + ang = self.head_tilt_calibrated_offset_rad + if (abs(ang) > large_ang): + rospy.loginfo('WARNING: self.head_tilt_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') + rospy.loginfo('self.head_tilt_calibrated_offset_rad in degrees = {0}'.format(self.head_tilt_calibrated_offset_rad * deg_per_rad)) + + self.head_pan_calibrated_offset_rad = controller_parameters['pan_angle_offset'] + ang = self.head_pan_calibrated_offset_rad + if (abs(ang) > large_ang): + rospy.loginfo('WARNING: self.head_pan_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') + rospy.loginfo('self.head_pan_calibrated_offset_rad in degrees = {0}'.format(self.head_pan_calibrated_offset_rad * deg_per_rad)) + + self.head_pan_calibrated_looked_left_offset_rad = controller_parameters['pan_looked_left_offset'] + ang = self.head_pan_calibrated_looked_left_offset_rad + if (abs(ang) > large_ang): + rospy.loginfo('WARNING: self.head_pan_calibrated_looked_left_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') + rospy.loginfo('self.head_pan_calibrated_looked_left_offset_rad in degrees = {0}'.format(self.head_pan_calibrated_looked_left_offset_rad * deg_per_rad)) + + self.head_tilt_backlash_transition_angle_rad = controller_parameters['tilt_angle_backlash_transition'] + rospy.loginfo('self.head_tilt_backlash_transition_angle_rad in degrees = {0}'.format(self.head_tilt_backlash_transition_angle_rad * deg_per_rad)) + + self.head_tilt_calibrated_looking_up_offset_rad = controller_parameters['tilt_looking_up_offset'] + ang = self.head_tilt_calibrated_looking_up_offset_rad + if (abs(ang) > large_ang): + rospy.loginfo('WARNING: self.head_tilt_calibrated_looking_up_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') + rospy.loginfo('self.head_tilt_calibrated_looking_up_offset_rad in degrees = {0}'.format(self.head_tilt_calibrated_looking_up_offset_rad * deg_per_rad)) + + self.wrist_extension_calibrated_retracted_offset_m = controller_parameters['arm_retracted_offset'] + m = self.wrist_extension_calibrated_retracted_offset_m + 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)