cccccccc
/
stretch_ros
mirror of https://github.com/hello-robot/stretch_ros.git
1
0
Fork 0
Code Issues 0 Projects 0 Releases 0 Wiki Activity
Browse Source

Fixed legacy yaml load

pull/1/head
hello-binit 4 years ago
parent
8a082638ef
commit
2d5883f755
1 changed files with 39 additions and 40 deletions
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +39
    -40
      stretch_core/nodes/stretch_driver

+ 39
- 40
stretch_core/nodes/stretch_driver View File

@ -1250,7 +1250,6 @@ class StretchBodyNode:
rospy.loginfo("{0} started".format(self.node_name)) rospy.loginfo("{0} started".format(self.node_name))
filename = rospy.get_param('~controller_calibration_file')
mode = rospy.get_param('~mode', "position") mode = rospy.get_param('~mode', "position")
rospy.loginfo('mode = ' + str(mode)) rospy.loginfo('mode = ' + str(mode))
@ -1261,46 +1260,46 @@ class StretchBodyNode:
large_ang = 45.0 * np.pi/180.0 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)) 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.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.angular_velocity_radps = 0.0 # rad/s ROS SI standard for cmd_vel (REP 103)

Write Preview
Loading…
Cancel
Save