def __init__(self, armName, imageDetector):
"""
armName must be from ConstantsClass.ArmName
imageDetector is instance of ImageDetectionClass
- pass in so two MasterClasses (i.e. two arms) can share imageDetector
"""
self.armName = armName
if (armName == ConstantsClass.ArmName.Left):
self.gripperName = ConstantsClass.GripperName.Left
self.toolframe = ConstantsClass.ToolFrame.Left
self.calibrateGripperState = ImageDetectionClass.State.CalibrateLeft
else:
self.gripperName = ConstantsClass.GripperName.Right
self.toolframe = ConstantsClass.ToolFrame.Right
self.calibrateGripperState = ImageDetectionClass.State.CalibrateRight
self.listener = tf.TransformListener()
# initialize the three main control mechanisms
# image detection, gripper control, and arm control
self.imageDetector = imageDetector
self.imageDetector.setState(self.calibrateGripperState)
rospy.loginfo('image detector')
self.commandGripper = CommandGripperClass(self.gripperName)
rospy.loginfo('gripper')
self.armControl = ArmControlClass(self.armName)
rospy.loginfo('arm control')
class MasterClass():
"""
Contains the master pipeline for Pr2Debridement in the run method
The general pipeline is as follows:
- identify the receptacle, object, and gripper
- open the gripper
- move to a point near the object
- servo to the object
- close the gripper
- move up with the object
- move to the receptacle
- open the gripper
... repeat
If any of the steps fails, the loop goes back to the beginning
Each pipeline staged is logged using rospy.loginfo
"""
def __init__(self, armName, imageDetector):
"""
armName must be from ConstantsClass.ArmName
imageDetector is instance of ImageDetectionClass
- pass in so two MasterClasses (i.e. two arms) can share imageDetector
"""
self.armName = armName
if (armName == ConstantsClass.ArmName.Left):
self.gripperName = ConstantsClass.GripperName.Left
self.toolframe = ConstantsClass.ToolFrame.Left
self.calibrateGripperState = ImageDetectionClass.State.CalibrateLeft
else:
self.gripperName = ConstantsClass.GripperName.Right
self.toolframe = ConstantsClass.ToolFrame.Right
self.calibrateGripperState = ImageDetectionClass.State.CalibrateRight
self.listener = tf.TransformListener()
# initialize the three main control mechanisms
# image detection, gripper control, and arm control
self.imageDetector = imageDetector
self.imageDetector.setState(self.calibrateGripperState)
rospy.loginfo('image detector')
self.commandGripper = CommandGripperClass(self.gripperName)
rospy.loginfo('gripper')
self.armControl = ArmControlClass(self.armName)
rospy.loginfo('arm control')
def run(self):
"""
Loops through the pipeline
"""
while not rospy.is_shutdown():
# can change rate
rospy.sleep(.5)
# delay between parts of the pipeline
delay = .5
# timeout class with 15 second timeout, can change
timeout = TimeoutClass(15)
# bounds, can change for each particular
# pipeline section. keep loose for testing
# translation bound in meters
transBound = .1
# rotation bound in radians
rotBound = float("inf")
rospy.loginfo('Searching for the receptacle')
if not self.imageDetector.hasFoundReceptacle():
continue
rospy.loginfo('Searching for object point')
# find object point and pose
objectPose = self.imageDetector.getObjectPose()
if objectPose == None:
continue
objectPoint = poseStampedToPointStamped(objectPose)
rospy.loginfo('Searching for ' + self.gripperName)
# find gripper point
if self.imageDetector.hasFoundGripper(self.gripperName):
gripperPose = self.imageDetector.getGripperPose(self.gripperName)
gripperPoint = poseStampedToPointStamped(gripperPose)
else:
continue
rospy.loginfo('Opening the gripper')
# open gripper
if not self.commandGripper.openGripper():
continue
rospy.loginfo('Moving close to the object point')
nearObjectPose = PoseStamped(objectPose.header, objectPose.pose)
nearObjectPose.pose.position.y += .1 # 10 cm to left
nearObjectPose.pose.position.z += .1 # and a little above, for safety
self.armControl.goToArmPose(nearObjectPose, True, ConstantsClass.Request.goNear)
success = True
timeout.start()
while not withinBounds(gripperPose, nearObjectPose, transBound, rotBound, self.listener):
gripperPose = self.imageDetector.getGripperPose(self.gripperName)
if timeout.hasTimedOut():
success = False
break
rospy.sleep(.5)
if not success:
continue
raw_input()
rospy.sleep(delay)
rospy.loginfo('Visual servoing to the object point')
# visual servo to get to the object point
transBound = .03
rotBound = float("inf")
success = True
while not withinBounds(gripperPose, objectPose, transBound, rotBound, self.listener):
timeout.start()
gripperPose = self.imageDetector.getGripperPose(self.gripperName)
objectPose = self.imageDetector.getObjectPose()
if not objectPose:
success = False
break
# for servoing to work, make "goal" point be to the right more
# might not work, just trying this
objectPose.pose.position.y -= .03
rospy.loginfo('starting to servo')
# servo to pose by one step, wait for user to press enter
self.armControl.servoToPose(gripperPose, objectPose)
raw_input()
rospy.loginfo('finished servoing')
"""
# originally tried servoing by computing "differences" in poses
# didn't work.
reportedGripperPose = self.commandGripper.gripperPose()
if reportedGripperPose == None:
continue
gripperPoseDifference = subPoses(reportedGripperPose, gripperPose)
if gripperPoseDifference == None:
continue
desiredObjectPose = addPoses(objectPose, gripperPoseDifference)
if desiredObjectPose == None:
continue
self.armControl.goToArmPose(desiredObjectPose, False)
"""
if timeout.hasTimedOut():
success = False
# commented out the break
# while stepping through the servoing
#break
rospy.sleep(.1)
if not success:
continue
rospy.sleep(delay)
rospy.loginfo('Closing the gripper')
# close gripper (consider not all the way)
if not self.commandGripper.setGripper(.5):
continue
rospy.sleep(delay)
rospy.loginfo('Moving vertical with the object')
# move vertical with the object
transBound = .05
rotBound = .1
vertObjectPose = PoseStamped(objectPose.header, objectPose.pose)
vertObjectPose.pose.position.z += .1
# currently have set to no planning, can change
self.armControl.goToArmPose(vertObjectPose, False)
rospy.sleep(delay)
rospy.loginfo('Moving to the receptacle')
transBound = .07
rotBound = float("inf")
# move to receptacle
receptaclePose = self.imageDetector.getReceptaclePose()
self.armControl.goToArmPose(receptaclePose, True, ConstantsClass.Request.goReceptacle)
success = True
timeout.start()
while not withinBounds(gripperPose, receptaclePose, transBound, rotBound, self.listener):
gripperPose = self.imageDetector.getGripperPose(self.gripperName)
if timeout.hasTimedOut():
success = False
break
rospy.sleep(.5)
if not success:
continue
rospy.loginfo('Opening the gripper to drop in the receptacle')
# open gripper to drop off
if not self.commandGripper.openGripper():
continue
