def __init__(self):
     self.actions = Actions()
     self.percepts = Percepts()
     self.tools = Tools()
     self.previous_scan_pose = None
     self.decision_required = True
     EstopGuard.add_callback(self.estop_changed_cb)
     # align camera
     self.actions.look_to(self.look_done_cb)
 def __init__(self):
     self.actions = Actions()
     self.percepts = Percepts()
     self.tools = Tools()
     self.previous_scan_pose = None
     self.decision_required = True
     self.at_approach = False
     self.at_ring = False
     self.loading_cube = False
     self.unloading_cube = False
     self.cube_operation_failure = False
     self.number_approach_failure = False
     self.standstill_failure = False
     self.scan_scheduled = False
     self.camera_aligned = False
     self.cube_status_string = ''
     self.status_string = ''
     EstopGuard.add_callback(self.estop_changed_cb)
class DeliverCubesStrategy(object):
    def __init__(self):
        self.actions = Actions()
        self.percepts = Percepts()
        self.tools = Tools()
        self.previous_scan_pose = None
        self.decision_required = True
        self.at_approach = False
        self.at_ring = False
        self.loading_cube = False
        self.unloading_cube = False
        self.cube_operation_failure = False
        self.number_approach_failure = False
        self.standstill_failure = False
        self.scan_scheduled = False
        self.camera_aligned = False
        self.cube_status_string = ''
        self.status_string = ''
        EstopGuard.add_callback(self.estop_changed_cb)

    def print_state(self):
        rospy.loginfo('***')
        if self.cube_operation_failure:
            self.cube_status_string = 'recovery'
        elif self.percepts.cube_loaded():
            self.cube_status_string = 'cube: {}'.format(
                self.percepts.current_number())
        else:
            self.cube_status_string = 'no cube'
        rospy.loginfo(self.cube_status_string)
        rospy.loginfo(self.status_string)

    def decide(self, event):
        try:
            self.print_state()
            try:
                self.percepts.visualize_worldmodel('{}\n{}'.format(
                    self.cube_status_string, self.status_string))
            except NotEnoughDataException:
                pass
            if not self.decision_required:
                return

            Params().update()
            self.percepts.estimate_center_from_map()

            # cube operation failure recovery
            if self.cube_operation_failure:
                if self.at_ring:
                    self.retreat()
                    return
                else:
                    self.cube_operation_failure = False
                    self.previous_scan_pose = None
                    self.explore()
                    return

            # number operation failure recovery
            if self.number_approach_failure:
                self.number_approach_failure = False
                self.previous_scan_pose = self.tools.get_current_pose()
                self.explore()
                return

            # no move failure recovery
            if self.standstill_failure:
                self.standstill_failure = False
                self.previous_scan_pose = self.tools.get_current_pose()
                self.explore()
                return

            # cube operations
            if self.at_ring:
                if self.loading_cube:
                    self.load_cube()
                    return
                elif self.unloading_cube:
                    self.unload_cube()
                    return
                else:
                    # cube operation complete
                    self.retreat()
                    return

            if not self.camera_aligned:
                self.actions.look_to(self.look_ahead_done_cb)

            self.actions.start_standstill_timer(self.standstill_timeout_cb)
            # drive and camera operations
            if not self.percepts.cube_loaded():
                try:
                    # approach loading station
                    if Params().hack_octa_loading_station:
                        self.approach_average_of_two_loading_station()
                    else:
                        self.approach_loading_station()
                    return
                except NotEnoughDataException as e:
                    # not enough data to find loading station
                    self.explore()
                    return
            else:
                try:
                    # approach number
                    self.approach_number()
                    return
                except NotEnoughDataException as e:
                    # not enough data to find number
                    self.explore()
                    return
        except NotEnoughDataException as e:
            rospy.logwarn('exception {}: {}'.format(type(e), e.message))
            rospy.sleep(1.0)
        except Exception as e:
            rospy.logerr(traceback.format_exc())

    def load_cube(self):
        self.actions.enable_LEDs()
        if not self.percepts.cube_loaded():
            self.actions.start_cube_operation_timer(
                self.cube_operation_timeout_cb)
            self.status_string = 'waiting for cube...'
            rospy.loginfo(self.status_string)
            return  # wait
        number = self.percepts.current_number()
        self.status_string = 'cube received, number {}.'.format(number)
        rospy.loginfo(self.status_string)
        self.loading_cube = False
        self.actions.cancel_cube_timeout()

    def unload_cube(self):
        self.actions.enable_LEDs()
        if self.percepts.cube_loaded():
            self.actions.start_cube_operation_timer(
                self.cube_operation_timeout_cb)
            self.status_string = 'waiting for cube removal...'
            rospy.loginfo(self.status_string)
            return  # wait
        self.status_string = 'cube removed.'
        rospy.loginfo(self.status_string)
        self.unloading_cube = False
        self.actions.cancel_cube_timeout()

    def approach_loading_station(self):
        current = self.tools.get_current_pose()
        loading_station = self.percepts.get_best_loading_station(current)
        if self.tools.is_good_approach_pose(current, loading_station):
            self.status_string = 'approaching loading station...'
            rospy.loginfo(self.status_string)
            stamped = PoseStamped()
            stamped.header = loading_station.header
            stamped.pose = self.tools.project_pose(loading_station.pose.pose)
            stamped.pose.position.z = -0.01
            self.actions.approach(stamped,
                                  self.approach_loading_station_done_cb,
                                  self.approach_timeout_cb)
        else:
            self.status_string = 'moving to loading station...'
            rospy.loginfo(self.status_string)
            approach = self.percepts.sample_approach_pose(
                loading_station.pose.pose, loading_station.header.frame_id)
            self.actions.move_to(approach, self.preapproach_done_cb,
                                 self.move_timeout_cb)
        self.decision_required = False

    def approach_average_of_two_loading_station(self):
        current = self.tools.get_current_pose()
        approach = self.percepts.get_best_average_approach_from_two_line_markers(
            current)
        if self.tools.xy_distance(current, approach) < 0.1:
            self.status_string = 'approaching loading station...'
            rospy.loginfo(self.status_string)
            stamped = PoseStamped(header=approach.header)
            offset = Pose()
            offset.position.x = Params().approach_distance
            offset.orientation.w = 1
            stamped.pose = self.tools.add_poses(approach.pose, offset)
            stamped.pose.position.z = -0.01
            self.actions.approach(stamped,
                                  self.approach_loading_station_done_cb,
                                  self.approach_timeout_cb)
        else:
            self.status_string = 'moving to loading station...'
            rospy.loginfo(self.status_string)
            self.actions.move_to(approach, self.preapproach_done_cb,
                                 self.move_timeout_cb)
        self.decision_required = False

    def approach_number(self):
        number = self.percepts.current_number()
        banner = self.percepts.get_number_banner(number)
        current = self.tools.get_current_pose()
        if banner.info.support < Params().min_trusted_support:
            # go to some good pose
            # look at the number for x seconds
            if self.tools.is_good_verification_pose(current, banner):
                # look at number
                self.status_string = 'looking at {}'.format(number)
                self.actions.look_at(PoseStamped(header=banner.header,
                                                 pose=banner.pose.pose),
                                     done_cb=self.verification_lookat_done_cb)
            else:
                for i in range(20):
                    stamped = self.tools.sample_verification_pose(
                        banner, self.percepts.estimate_center_from_map())
                    sampled_poses = []
                    if self.percepts.check_path(current, stamped):
                        self.status_string = 'moving to look at {}'.format(
                            number)
                        rospy.loginfo(self.status_string)
                        self.actions.move_to(
                            stamped,
                            done_cb=self.move_to_verify_done_cb,
                            timeout_cb=self.move_timeout_cb)
                        self.decision_required = False
                        return
                    sampled_poses.append(stamped)
                self.tools.visualize_poses(sampled_poses,
                                           ns='failed_verification_poses')
                self.status_string = 'failure at {}'.format(number)
                self.number_approach_failure = True

        elif self.tools.is_good_approach_pose(current, banner):
            self.status_string = 'approaching number {}...'.format(number)
            rospy.loginfo(self.status_string)
            stamped = PoseStamped()
            stamped.header = banner.header
            stamped.pose = self.tools.project_pose(banner.pose.pose)
            #stamped.pose = self.tools.set_orientation_from_yaw(banner.pose.pose, self.tools.get_yaw(banner.pose.pose) + math.pi)
            stamped.pose.position.z = 0.01
            self.actions.approach(stamped, self.approach_number_done_cb,
                                  self.approach_timeout_cb)
        else:
            self.status_string = 'moving to number {}...'.format(number)
            rospy.loginfo(self.status_string)
            approach = self.percepts.sample_approach_pose(
                banner.pose.pose, banner.header.frame_id)
            self.actions.move_to(approach, self.preapproach_done_cb,
                                 self.move_timeout_cb)
        self.decision_required = False

    def retreat(self):
        self.status_string = 'retreating...'
        rospy.loginfo(self.status_string)
        self.actions.disable_LEDs()
        self.actions.retreat(self.retreat_done_cb, self.retreat_timeout_cb)
        self.decision_required = False

    def explore(self):
        # scan
        if not self.previous_scan_pose:
            self.previous_scan_pose = self.tools.get_current_pose()
        current_pose = self.tools.get_current_pose()
        if self.tools.xy_distance(
                self.previous_scan_pose,
                current_pose) > Params().min_travel_distance_for_rescan:
            self.status_string = 'sweeping...'
            rospy.loginfo(self.status_string)
            self.actions.camera_sweep(self.sweep_done_cb)
            self.decision_required = False
            return
        self.status_string = 'exploring...'
        rospy.loginfo(self.status_string)
        exploration_pose = self.percepts.sample_scan_pose()
        self.actions.move_to(exploration_pose, self.move_to_scan_done_cb,
                             self.move_to_scan_timeout_cb)
        self.decision_required = False

    def sweep_done_cb(self, status, result):
        self.status_string = 'camera_sweep: done'
        rospy.loginfo(self.status_string)
        self.previous_scan_pose = self.tools.get_current_pose()
        self.camera_aligned = False
        self.decision_required = True

    def look_ahead_done_cb(self, status, result):
        self.status_string = 'camera_look_ahead: done'
        rospy.loginfo(self.status_string)
        self.camera_aligned = True
        self.decision_required = True

    def approach_loading_station_done_cb(self, status, result):
        if status == GoalStatus.SUCCEEDED:
            self.status_string = 'approach_loading_station: done'
            rospy.loginfo(self.status_string)
            self.loading_cube = True
            self.actions.enable_LEDs()
            #self.actions.start_cube_operation_timer(self.cube_operation_timeout_cb)
        self.at_ring = True
        self.decision_required = True

    def approach_number_done_cb(self, status, result):
        if status == GoalStatus.SUCCEEDED:
            self.status_string = 'approach_number: done'
            rospy.loginfo(self.status_string)
            self.unloading_cube = True
            #self.actions.start_cube_operation_timer(self.cube_operation_timeout_cb)
        self.at_ring = True
        self.decision_required = True

    def retreat_done_cb(self, status, result):
        self.status_string = 'retreat: done'
        rospy.loginfo(self.status_string)
        self.at_ring = False
        self.decision_required = True

    def preapproach_done_cb(self, status, result):
        if status == GoalStatus.SUCCEEDED:
            self.status_string = 'pre-approach: done'.format(status, result)
            rospy.loginfo(self.status_string)
        self.decision_required = True

    def move_to_scan_done_cb(self, status, result):
        self.status_string = 'move to scan: done'
        rospy.loginfo(self.status_string)
        self.scan_scheduled = True
        self.decision_required = True

    def move_to_scan_timeout_cb(self, event):
        self.status_string = 'move to scan: timeout'
        rospy.loginfo(self.status_string)
        self.decision_required = True

    def move_to_verify_done_cb(self, status, result):
        self.status_string = 'move to verify: done'
        rospy.loginfo(self.status_string)
        self.decision_required = True

    def move_timeout_cb(self, event):
        self.status_string = 'move to verify: timeout'
        rospy.loginfo(self.status_string)
        self.decision_required = True

    def approach_timeout_cb(self, event):
        self.status_string = 'approach: timeout'
        rospy.loginfo(self.status_string)
        self.at_ring = True
        self.decision_required = True

    def retreat_timeout_cb(self, event):
        self.status_string = 'retreat: timeout'
        rospy.loginfo(self.status_string)
        # TODO: do something here... retreat?
        self.decision_required = True

    def cube_operation_timeout_cb(self, event):
        self.status_string = 'cube_operation: timeout'
        rospy.loginfo(self.status_string)
        self.cube_operation_failure = True
        self.decision_required = True

    def verification_lookat_done_cb(self, status, result):
        self.actions.start_verification_timer(self.verification_timeout_cb)

    def verification_timeout_cb(self, event):
        self.status_string = 'verification: done'
        rospy.loginfo(self.status_string)
        self.decision_required = True

    def standstill_timeout_cb(self, event):
        self.status_string = 'no_move: timeout'
        rospy.loginfo(self.status_string)
        self.standstill_failure = True
        self.decision_required = True

    def estop_changed_cb(self, stop):
        if stop:
            self.status_string = 'estop triggered.'
            rospy.loginfo(self.status_string)
            self.decision_required = False
            rospy.Rate(4).sleep()  # let current decisions finish...
            self.actions.cancel_all_actions()  # ... and then cancel them.
            self.actions.cancel_standstill_timer()
        else:
            self.status_string = 'estop released.'
            rospy.loginfo(self.status_string)
            self.decision_required = True
class RandomMoveStrategy(object):
    def __init__(self):
        self.actions = Actions()
        self.percepts = Percepts()
        self.tools = Tools()
        self.previous_scan_pose = None
        self.decision_required = True
        EstopGuard.add_callback(self.estop_changed_cb)
        # align camera
        self.actions.look_to(self.look_done_cb)

    def decide(self, event):
        if not self.decision_required:
            return
        try:
            Params.update()
            self.percepts.estimate_center_from_map()
            #self.percepts.estimate_center_from_worldmodel()

            # scan
            if not self.previous_scan_pose:
                rospy.loginfo('initial sweeping...')
                self.actions.camera_sweep(self.sweep_done_cb)
                self.decision_required = False
                return

            current_pose = self.tools.get_current_pose()
            if self.tools.xy_distance(
                    self.previous_scan_pose,
                    current_pose) > Params().min_travel_distance_for_rescan:
                rospy.loginfo('sweeping...')
                self.actions.stop()
                self.actions.camera_sweep(self.sweep_done_cb)
                self.decision_required = False
                return

            # explore
            rospy.loginfo('exploring...')
            exploration_pose = self.percepts.sample_scan_pose()
            self.actions.move_to(exploration_pose, self.move_done_cb,
                                 self.move_timeout_cb)
            self.decision_required = False
        except Exception as e:
            rospy.logwarn('exception {}: {}'.format(type(e), e.message))
            rospy.sleep(1.0)

    def sweep_done_cb(self, status, result):
        rospy.loginfo('camera_sweep: done')
        self.previous_scan_pose = self.tools.get_current_pose()
        self.actions.look_to(self.look_done_cb)
        self.decision_required = True

    def look_done_cb(self, status, result):
        rospy.loginfo('camera_look: done')
        self.decision_required = True

    def move_done_cb(self, status, result):
        rospy.loginfo('random_move: done')
        self.actions.cancel_move_timeout()
        self.decision_required = True

    def move_timeout_cb(self, event):
        rospy.loginfo('random_move: timeout')
        self.decision_required = True

    def estop_changed_cb(self, stop):
        if stop:
            rospy.loginfo('estop triggered.')
            self.decision_required = False
            rospy.Rate(4).sleep()  # let current decisions finish...
            self.actions.cancel_all_actions()  # ... and then cancel them.
        else:
            rospy.loginfo('estop released.')
            self.decision_required = True