def pick_Towel(self):
self.calibrate_obj_det_pub.publish(True)
while self.calibrated == False:
pass
self.calibrate_obj_det_pub.publish(False)
print("Finger Sensors calibrated")
if self.listen.frameExists("/root") and self.listen.frameExists(
"/Towel_position"):
print("we have the spoon frame")
self.listen.waitForTransform('/root', '/Towel_position',
rospy.Time(), rospy.Duration(100.0))
t = self.listen.getLatestCommonTime("/root", "/Towel_position")
translation, quaternion = self.listen.lookupTransform(
"/root", "/Towel_position", t)
translation = list(translation)
quaternion = list(quaternion)
pose_value = translation + quaternion
# print (quaternion)
orientation_XYZ = pose_action_client.Quaternion2EulerXYZ(
quaternion)
# self.j.gripper.open()
#second arg=0 (absolute movement), arg = '-r' (relative movement)
self.cmmnd_CartesianPosition(pose_value, 0)
def goto_straw(self):
self.calibrate_obj_det_pub.publish(True)
while self.calibrated == False:
pass
self.calibrate_obj_det_pub.publish(False)
print("Finger Sensors calibrated")
if self.listen.frameExists("/root") and self.listen.frameExists("/straw_position"):
print ("we have the straw frame")
self.listen.waitForTransform('/root','/straw_position',rospy.Time(),rospy.Duration(100.0))
t = self.listen.getLatestCommonTime("/root", "/straw_position")
translation, quaternion = self.listen.lookupTransform("/root", "/straw_position", t)
translation = list(translation)
quaternion = list(quaternion)
pose_value = translation + quaternion
orientation_XYZ = pose_action_client.Quaternion2EulerXYZ(quaternion)
self.cmmnd_CartesianPosition(pose_value, 0)
else:
print ("We DONT have the frame")
def cmmnd_CartesianPosition(self, pose_value, relative):
pose_action_client.getcurrentCartesianCommand('j2n6a300_')
pose_mq, pose_mdeg, pose_mrad = pose_action_client.unitParser('mq', pose_value, relative)
poses = [float(n) for n in pose_mq]
orientation_XYZ = pose_action_client.Quaternion2EulerXYZ(poses[3:])
try:
poses = [float(n) for n in pose_mq]
result = pose_action_client.cartesian_pose_client(poses[:3], poses[3:])
except rospy.ROSInterruptException:
print ("program interrupted before completion")
def goto_syringe(self):
print ("we are in fuctn")
if self.listen.frameExists("/root") and self.listen.frameExists("/syringe_position"):
print ("we have the syringe_position frame")
self.listen.waitForTransform('/root','/syringe_position',rospy.Time(),rospy.Duration(100.0))
t = self.listen.getLatestCommonTime("/root", "/syringe_position")
translation, quaternion = self.listen.lookupTransform("/root", "/syringe_position", t)
translation = list(translation)
quaternion = list(quaternion)
pose_value = translation + quaternion
# print (quaternion)
orientation_XYZ = pose_action_client.Quaternion2EulerXYZ(quaternion)
# self.j.gripper.open()
#second arg=0 (absolute movement), arg = '-r' (relative movement)
self.cmmnd_CartesianPosition(pose_value, 0)
