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'