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 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
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