def execute(self, userdata):
rospy.logdebug('CamToDropzone: Executing state CamToDropzone')
listener = TransformListener()
if utils.manipulation is None:
utils.manipulation = Manipulation()
utils.manipulation.set_turbo_mode()
utils.manipulation.set_movement_time(9)
# TODO: Exception schmeissen wenn abort_after vergangen ist
abort_after = 15
then = int(time.time())
now = int(time.time())
while not listener.frameExists("drop_point"):
if not (now - then < abort_after):
rospy.logdebug('drop_point frame not found!')
return 'fail'
rospy.loginfo("wait for drop_point frame")
rospy.sleep(2.)
now = int(time.time())
then = int(time.time())
now = int(time.time())
while not listener.frameExists("mdl_middle"):
if not (now - then < abort_after):
rospy.logdebug('CamToDropzone: mdl_middle frame not found!')
return 'fail'
rospy.sleep(2.)
now = int(time.time())
if len(userdata.scan_conveyor_pose) == 0:
rospy.logdebug('CamToDropzone: No scanPose, calculate now...')
p = utils.manipulation.scan_conveyor_pose()
userdata.scan_conveyor_pose.append(p)
else:
rospy.logdebug('CamToDropzone: scanPose found!')
p = userdata.scan_conveyor_pose[0]
rospy.logdebug('CamToDropzone: Move arm to scan_conveyor_pose')
for i in range(0, 3):
if utils.manipulation.move_to(p):
return 'scanPoseReached'
else:
if i == 2:
rospy.logdebug('CamToDropzone: Cant move arm to scan_conveyor_pose')
return 'fail'
def execute(self, userdata):
rospy.logdebug('PlaceTask6: Executing state PlaceTask6')
listener = TransformListener()
if utils.manipulation is None:
utils.manipulation = Manipulation()
# TODO: Exception schmeissen wenn abort_after vergangen ist
abort_after = 15
then = int(time.time())
now = int(time.time())
while not listener.frameExists("target_zone"):
if not now - then < abort_after:
rospy.logdebug("PlaceTask6: target_zone frame not found")
return 'fail'
rospy.loginfo("wait for target_zone frame")
rospy.sleep(2.)
now = int(time.time())
self.__place_pose = self.get_place_point(userdata)
rospy.logdebug('PlaceTask6: Move Arm to Place Pose')
rospy.logdebug(self.__place_pose)
for i in range(0, 7):
if utils.manipulation.move_to(self.__place_pose):
rospy.logdebug('PlaceTask6: OpenGripper')
utils.manipulation.open_gripper()
return 'success'
else:
rospy.logdebug('PlaceTask6: Calculate new place position...')
if i <= 2:
self.__place_pose = self.get_place_point(userdata)
if i > 2:
self.__place_pose = self.get_place_point(userdata, 3)
if i == 6:
rospy.logdebug('PlaceTask6: Cant find place position!')
utils.manipulation.open_gripper()
return 'fail'
def execute(self, userdata):
rospy.logdebug('PlaceTask6: Executing state PlaceTask6')
listener = TransformListener()
if utils.manipulation is None:
utils.manipulation = Manipulation()
# TODO: Exception schmeissen wenn abort_after vergangen ist
abort_after = 15
then = int(time.time())
now = int(time.time())
while not listener.frameExists("target_zone"):
if not now - then < abort_after:
rospy.logdebug("PlaceTask6: target_zone frame not found")
return 'fail'
rospy.loginfo("wait for target_zone frame")
rospy.sleep(2.)
now = int(time.time())
self.__place_pose = self.get_place_point(userdata)
rospy.logdebug('PlaceTask6: Move Arm to Place Pose')
rospy.logdebug(self.__place_pose)
for i in range(0, 7):
if utils.manipulation.move_to(self.__place_pose):
rospy.logdebug('PlaceTask6: OpenGripper')
utils.manipulation.open_gripper()
return 'success'
else:
rospy.logdebug('PlaceTask6: Calculate new place position...')
if i <= 2:
self.__place_pose = self.get_place_point(userdata)
if i > 2:
self.__place_pose = self.get_place_point(userdata, 3)
if i == 6:
rospy.logdebug('PlaceTask6: Cant find place position!')
utils.manipulation.open_gripper()
return 'fail'
