class HandeyeCalibrationCommander:
def __init__(self):
self.client = HandeyeClient()
def _take_menu(self):
print('Press SPACE to take a sample or ENTER to continue\n')
i = getchar()
if i == ' ':
self.client.take_sample()
def _display_sample_list(self, sample_list):
for i in range(len(sample_list.hand_world_samples.transforms)):
print('{}) \n hand->world {} \n camera->marker {}\n'.format(
i, sample_list.hand_world_samples.transforms[i],
sample_list.camera_marker_samples.transforms[i]))
def _edit_menu(self):
while len(self.client.get_sample_list().hand_world_samples.transforms
) > 0:
prompt_str = 'Press a number and ENTER to delete the respective sample, or ENTER to continue:\n'
self._display_sample_list(self.client.get_sample_list())
sample_to_delete = raw_input(prompt_str)
if sample_to_delete.isdigit():
self.client.remove_sample(int(sample_to_delete))
else:
break
def _save_menu(self):
print('Press c to compute the calibration or ENTER to continue\n')
i = getchar()
if i == 'c':
cal = self.client.compute_calibration()
print(cal)
print(
'Press s to save the calibration to parameters and namespace, q to quit or ENTER to continue\n'
)
i = getchar()
if i == 's':
self.client.save()
elif i == 'q':
quit()
def _interactive_menu(self):
self._take_menu()
self._edit_menu()
self._save_menu()
def spin_interactive(self):
self._edit_menu(
) # the sample list might not be empty when we start the commander
while not rospy.is_shutdown():
self._interactive_menu()
class Calibration(object):
"""Calibration"""
node_name = "CALIBRATION"
def __init__(self, node_name, playback=False):
# TODO: whe shouldn't have to run this node in a seperate calibration namespace, it would make things much better
self.robot_ns = 'px150'
self.node_name = node_name
self.playback = playback
self.command = None
if self.playback:
rospy.loginfo(
"[{0}] Calibration launched in playback mode!".format(
self.node_name))
rospy.sleep(5)
rospy.logdebug("[CALIBRATE] initializing hand eye client")
self.client = HandeyeClient()
self.experiment_info = ExperimentInfo(self.node_name,
namespace=self.robot_ns,
id="initial_calibration")
# Listen
rospy.logdebug("[CALIBRATE] initializing tf2_ros buffer")
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
self.calibration_frame = rospy.get_param('/' + self.robot_ns + '/' +
'robot_base_frame')
self.planning_frame = rospy.get_param('/' + self.robot_ns + '/' +
'planning_frame')
self.pose_array = None
self.pub_e_out = rospy.Publisher("~e_out",
FlexGraspErrorCodes,
queue_size=10,
latch=True)
move_robot_topic = '/' + self.robot_ns + '/' + 'move_robot'
robot_pose_topic = '/' + self.robot_ns + '/' + 'robot_pose'
pose_array_topic = '/' + self.robot_ns + '/' + 'pose_array'
self.move_robot_communication = Communication(move_robot_topic,
timeout=15,
node_name=self.node_name)
self.robot_pose_publisher = rospy.Publisher(robot_pose_topic,
PoseStamped,
queue_size=10,
latch=False)
self.pose_array_publisher = rospy.Publisher(pose_array_topic,
PoseArray,
queue_size=5,
latch=True)
self.output_logger = DataLogger(
self.node_name, {"calibration": "calibration_transform"},
{"calibration": TransformStamped},
bag_name=self.node_name)
# Subscribe
rospy.Subscriber("~e_in", String, self.e_in_cb)
# Subscribe to aruco tracker for it to publish the tf
rospy.Subscriber('/' + self.robot_ns + '/' + 'aruco_tracker/pose',
PoseStamped, self.aruco_tracker_cb)
def e_in_cb(self, msg):
if self.command is None:
self.command = msg.data
rospy.logdebug("[{0}] Received event message: {1}".format(
self.node_name, self.command))
# reset outputting message
msg = FlexGraspErrorCodes()
msg.val = FlexGraspErrorCodes.NONE
self.pub_e_out.publish(msg)
def aruco_tracker_cb(self, msg):
pass
def init_poses_1(self):
r_amplitude = 0.00
z_amplitude = 0.00
r_min = 0.24
z_min = 0.28 # 0.05
pos_intervals = 1
if pos_intervals == 1:
r_vec = [r_min + r_amplitude
] # np.linspace(x_min, x_min + 2*x_amplitude, 2) #
z_vec = [z_min + z_amplitude]
else:
r_vec = np.linspace(r_min, r_min + 2 * r_amplitude, pos_intervals)
z_vec = np.linspace(z_min, z_min + 2 * z_amplitude, pos_intervals)
ai_amplitude = np.deg2rad(38.0)
aj_amplitude = np.deg2rad(38.0)
ak_amplitude = np.deg2rad(15.0)
rot_intervals = 2
ai_vec = np.linspace(-ai_amplitude, ai_amplitude, rot_intervals)
aj_vec = np.linspace(-aj_amplitude, aj_amplitude, rot_intervals)
ak_vec = [-ak_amplitude, ak_amplitude]
return self.generate_poses(r_vec, z_vec, ai_vec, aj_vec, ak_vec)
def init_poses_2(self):
surface_height = 0.019
height_finger = 0.040 # [m]
finger_link2ee_link = 0.023 # [m]
grasp_height = height_finger + finger_link2ee_link - surface_height
sag_angle = np.deg2rad(6.0) # [deg]
r_amplitude = 0.08
z_amplitude = 0.00
r_min = 0.10
z_min = grasp_height # 0.05
pos_intervals = 3
if pos_intervals == 1:
r_vec = [r_min + r_amplitude
] # np.linspace(x_min, x_min + 2*x_amplitude, 2) #
z_vec = [z_min + z_amplitude]
else:
r_vec = np.linspace(r_min, r_min + 2 * r_amplitude, pos_intervals)
z_vec = np.linspace(z_min, z_min + 2 * z_amplitude,
pos_intervals) + np.tan(sag_angle) * r_vec
ak_amplitude = np.deg2rad(15.0)
rot_intervals = 2
ai_vec = [np.deg2rad(0)]
aj_vec = [np.deg2rad(90)]
ak_vec = [-ak_amplitude, ak_amplitude]
return self.generate_poses_2(r_vec, z_vec, ai_vec, aj_vec, ak_vec)
def generate_poses(self, r_vec, z_vec, ai_vec, aj_vec, ak_vec):
pose_array = PoseArray()
pose_array.header.frame_id = self.calibration_frame
pose_array.header.stamp = rospy.Time.now()
poses = []
for ak in ak_vec:
for r in r_vec:
for z in z_vec:
for aj in aj_vec:
for ai in ai_vec:
pose = Pose()
x = r * np.cos(ak)
y = r * np.sin(ak)
pose.position = list_to_position([x, y, z])
pose.orientation = list_to_orientation(
[ai, aj, ak])
poses.append(pose)
pose_array.poses = poses
self.pose_array_publisher.publish(pose_array)
self.pose_array = pose_array
return FlexGraspErrorCodes.SUCCESS
def generate_poses_2(self, r_vec, z_vec, ai_vec, aj_vec, ak_vec):
pose_array = PoseArray()
pose_array.header.frame_id = self.calibration_frame
pose_array.header.stamp = rospy.Time.now()
poses = []
for ak in ak_vec:
for r, z in zip(r_vec, z_vec):
for aj in aj_vec:
for ai in ai_vec:
pose = Pose()
x = r * np.cos(ak)
y = r * np.sin(ak)
pose.position = list_to_position([x, y, z])
pose.orientation = list_to_orientation([ai, aj, ak])
poses.append(pose)
pose_array.poses = poses
self.pose_array_publisher.publish(pose_array)
self.pose_array = pose_array
return FlexGraspErrorCodes.SUCCESS
def calibrate(self, track_marker=True):
sample_list = self.client.get_sample_list().camera_marker_samples
n_samples = len(sample_list)
if n_samples > 0:
rospy.loginfo(
"[{0}] Found {1} old calibration samples, deleting them before recalibrating!"
.format(self.node_name, n_samples))
for i in reversed(range(n_samples)):
rospy.loginfo("[{0}] Deleting sample {1}".format(
self.node_name, i))
self.client.remove_sample(i)
sample_list = self.client.get_sample_list().camera_marker_samples
n_samples = len(sample_list)
if n_samples > 0:
rospy.logwarn(
"[{0}] Failed to remove all old samples, cannot calibrate".
format(self.node_name))
print sample_list
return FlexGraspErrorCodes.FAILURE
else:
rospy.loginfo("[{0}] All old samples have been removed".format(
self.node_name))
if self.playback:
rospy.loginfo(
"[{0}] Playback is active: publishing messages from bag!".
format(self.node_name))
messages = self.output_logger.load_messages_from_bag(
self.experiment_info.path, self.experiment_info.id)
if messages is not None:
self.broadcast(messages['calibration'])
return FlexGraspErrorCodes.SUCCESS
else:
return FlexGraspErrorCodes.FAILURE
if self.pose_array is None:
rospy.logwarn("[CALIBRATE] pose_array is still empty")
return FlexGraspErrorCodes.REQUIRED_DATA_MISSING
try:
trans = self.tfBuffer.lookup_transform(
self.planning_frame, self.pose_array.header.frame_id,
rospy.Time(0))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logwarn("[CALIBRATE] failed to get transform from %s to %s",
self.pose_array.header.frame_id, self.planning_frame)
return FlexGraspErrorCodes.TRANSFORM_POSE_FAILED
result = self.move_robot_communication.wait_for_result("reset")
for pose in self.pose_array.poses:
if rospy.is_shutdown():
return FlexGraspErrorCodes.SHUTDOWN
pose_stamped = PoseStamped()
pose_stamped.header = self.pose_array.header
pose_stamped.pose = pose
# transform to planning frame
pose_trans = tf2_geometry_msgs.do_transform_pose(
pose_stamped, trans)
self.robot_pose_publisher.publish(pose_trans)
result = self.move_robot_communication.wait_for_result(
"move_manipulator") # timout = 5?
if result == FlexGraspErrorCodes.SUCCESS:
if track_marker:
# camera delay + wait a small amount of time for vibrations to stop
rospy.sleep(1.5)
try:
self.client.take_sample()
except:
rospy.logwarn(
"[CALIBRATE] Failed to take sample, marker might not be visible."
)
elif result == FlexGraspErrorCodes.DYNAMIXEL_ERROR:
rospy.logwarn(
"[{0}] Dynamixel error triggered, returning error".format(
self.node_name))
return result
elif result == FlexGraspErrorCodes.DYNAMIXEL_SEVERE_ERROR:
rospy.logwarn(
"[{0}] Dynamixel error triggered, returning error".format(
self.node_name))
return result
# reset
result = self.move_robot_communication.wait_for_result("home")
# compute calibration transform
if not track_marker:
return FlexGraspErrorCodes.SUCCESS
else:
calibration_transform = self.client.compute_calibration()
if calibration_transform.valid:
rospy.loginfo("[CALIBRATE] Found valid transfrom")
self.broadcast(calibration_transform.calibration.transform)
self.client.save()
return FlexGraspErrorCodes.SUCCESS
else:
rospy.logwarn("[CALIBRATE] Computed calibration is invalid")
return FlexGraspErrorCodes.FAILURE
def broadcast(self, transform):
rospy.loginfo("[{0}] Broadcasting result".format(self.node_name))
broadcaster = tf2_ros.StaticTransformBroadcaster()
broadcaster.sendTransform(transform)
if not self.playback:
self.output_logger.write_messages_to_bag(
{"calibration": transform}, self.experiment_info.path,
self.experiment_info.id)
def take_action(self):
msg = FlexGraspErrorCodes()
result = None
if self.command == 'e_init':
result = FlexGraspErrorCodes.SUCCESS
elif self.command == 'calibrate':
result = self.init_poses_1()
if result == FlexGraspErrorCodes.SUCCESS:
result = self.calibrate()
elif self.command == 'calibrate_height':
result = self.init_poses_2()
if result == FlexGraspErrorCodes.SUCCESS:
result = self.calibrate(track_marker=False)
elif self.command is not None:
rospy.logwarn(
"[CALIBRATE] Can not take an action: received unknown command %s!",
self.command)
result = FlexGraspErrorCodes.UNKNOWN_COMMAND
# publish success
if result is not None:
msg.val = result
flex_grasp_error_log(result, self.node_name)
self.pub_e_out.publish(msg)
self.command = None