def _err_base_mission(self, err):
     self.running_mission = False
     self.current_mission_name = None
     if err.type == defer.CancelledError:
         fprint("Base mission cancelled", msg_color="red", title="BASE MISSION ERROR:")
     else:
         fprint(err, msg_color="red", title="BASE MISSION ERROR:")

    def continuously_align(self):
        fprint("Starting Forest Align", title="DETECT DELIVER", msg_color='green')
        while True:
            shooter_pose = yield self.shooter_pose_sub.get_next_message()
            if self.align_forest_pause:
                yield self.nh.sleep(0.1)
                continue
            shooter_pose = shooter_pose.pose

            cen = np.array([shooter_pose.position.x, shooter_pose.position.y])
            yaw = trns.euler_from_quaternion([shooter_pose.orientation.x,
                                              shooter_pose.orientation.y,
                                              shooter_pose.orientation.z,
                                              shooter_pose.orientation.w])[2]
            q = trns.quaternion_from_euler(0, 0, yaw)
            p = np.append(cen, 0)
            # fprint("Forest Aligning to p=[{}] q=[{}]".format(p, q), title="DETECT DELIVER", msg_color='green')

            # Prepare move to follow shooter
            move = self.move.set_position(p).set_orientation(q).yaw_right(90, 'deg')

            # Adjust move for location of target
            move = move.forward(self.target_offset_meters)

            # Adjust move for location of launcher
            move = move.left(-self.shooter_baselink_tf._p[1]).forward(-self.shooter_baselink_tf._p[0])

            # Move away a fixed distance to make the shot
            move = move.left(self.shoot_distance_meters)

            yield move.go(move_type='bypass')

 def do_mission(self, navigator, planner, module, **kwargs):
     """Perform this mission."""
     yield planner.publish("Starting", self)
     to_run = getattr(module, self.mission_script)
     fprint(self.name, msg_color="green", title="STARTING MISSION")
     res = yield to_run.main(navigator, attempts=self.attempts, **kwargs)
     defer.returnValue(res)

 def shoot_and_align(self):
     move = yield self.align_to_target()
     if move.failure_reason != "":
         fprint("Error Aligning with target = {}. Ending mission :(".format(move.failure_reason), title="DETECT DELIVER", msg_color="red")
         return
     fprint("Aligned successs. Shooting without realignment", title="DETECT DELIVER", msg_color="green")
     yield self.shoot_all_balls()

    def _run_search_pattern(self, loop, **kwargs):
        '''
        Look around using the search pattern.
        If `loop` is true, then keep iterating over the list until timeout is reached or we find it.
        '''
        if self.search_pattern is None:
            return

        def pattern():
            for pose in self.search_pattern:
                fprint("Going to next position.", title="SEARCHER")
                if type(pose) == list or type(pose) == np.ndarray:
                    yield self.nav.move.relative(pose).go(**kwargs)
                else:
                    yield pose.go(**kwargs)

                yield self.nav.nh.sleep(2)
            if not loop:
                fprint("Search Pattern Over", title="SEARCHER")
                self.pattern_done = True

        fprint("Executing search pattern.", title="SEARCHER")

        if loop:
            while True:
                yield util.cancellableInlineCallbacks(pattern)()
        else:
            yield util.cancellableInlineCallbacks(pattern)()

    def count_pipes(self):
        """Count the number of pipes in between the breaks."""
        second_hpipe_found = False
        while not second_hpipe_found:
            try:
                frame = yield self.curr_image
            except util.TimeoutError:
                fprint("Image isn't being received, fail mission", msg_color="red")
                raise Exception("Image isn't being received")

            # get all the pipes in the current frame
            hpipes, vpipes = self._get_all_pipes(frame)
            # Look for NEW pipes
            new_hpipes, new_vpipes = self._update_pipes(
                hpipes, self.old_hpipe_pos), self._update_pipes(vpipes, self.old_vpipe_pos)

            # the second hpipe is found
            if len(new_hpipes) > 0 and self.hpipe_found:
                defer.returnValue(self.count)

            # the first hpipe is found
            elif len(new_hpipes) > 0 and not self.hpipe_found:
                self.hpipe_found = True
                for pipe in vpipes:
                    if pipe[1] > new_hpipes[0][1]:
                        self.count += 1

            # if its above first h, add to count
            elif self.hpipe_found:
                for pipe in new_vpipes:
                    if pipe[1] > new_hpipes[0][1]:
                        self.count += 1

            self.old_hpipe_pos.extend(new_hpipes)
            self.old_vspipe_pos.extend(new_vpipes)

 def _cb_mission(self, result):
     if not self.failed:
         yield self.publish("Ending")
         fprint(str(result) + " TIME: " + str((self.nh.get_time() - self.current_mission.start_time).to_sec()),
                msg_color="green", title="{} MISSION COMPLETE: ".format(self.current_mission.name))
         self.points += self.current_mission.points
         self._mission_complete()

def go_to_objects(navigator, position, objs):
    objects = get_closest_objects(position, objs)
    for o in objects:
        fprint("MOVING TO OBJECT WITH ID {}".format(o.id), msg_color="green")
        yield navigator.nh.sleep(5)
        pos = nt.rosmsg_to_numpy(o.position)
        yield navigator.move.look_at(pos).set_position(pos).backward(7).go()

 def _object_gone_missing(self, missing_objects):
     fprint("This object {} is no longer in the list".format(missing_objects), msg_color="red")
     self.helper.stop_ensuring_object_permanence()
     for o in missing_objects:
         if o in self.found:
             self.helper.remove_found(o)
             self.found.remove(o)
     self.mission_defer.cancel()

 def set_shape_and_color(self):
     target = yield self.navigator.mission_params["detect_deliver_target"].get()
     if target == "BIG":
         self.target_offset_meters = self.SHAPE_CENTER_TO_BIG_TARGET
     elif target == "SMALL":
         self.target_offset_meters = self.SHAPE_CENTER_TO_SMALL_TARGET
     self.Shape = yield self.navigator.mission_params["detect_deliver_shape"].get()
     self.Color = yield self.navigator.mission_params["detect_deliver_color"].get()
     fprint("Color={} Shape={} Target={}".format(self.Color, self.Shape, target), title="DETECT DELIVER",  msg_color='green')

 def align_to_target(self):
     if self.shape_pose == None:
         self.select_backup_shape()
     goal_point, goal_orientation = self.get_aligned_pose(self.shape_pose[0], self.shape_pose[1])
     move = self.navigator.move.set_position(goal_point).set_orientation(goal_orientation).forward(self.target_offset_meters)
     move = move.left(-self.shooter_baselink_tf._p[1]).forward(-self.shooter_baselink_tf._p[0]) #Adjust for location of shooter
     fprint("Aligning to shoot at {}".format(move), title="DETECT DELIVER", msg_color='green')
     move_complete = yield move.go(move_type="skid", blind=True)
     defer.returnValue(move_complete)

 def select_backup_shape(self):
     for (shape, color), point_normal in self.identified_shapes.iteritems():
         self.shape_pose = point_normal
         if self.Shape == shape or self.Color == color:
             fprint("Correct shape not found, resorting to shape={} color={}".format(shape, color), title="DETECT DELIVER",  msg_color='yellow')
             return
     if self.shape_pose == None:
         raise Exception("None seen")
     fprint("Correct shape not found, resorting to random shape", title="DETECT DELIVER",  msg_color='yellow')

 def run(self):
     fprint("PINGER EXIT: Starting", msg_color='green') 
     self.gate_index = yield self.navigator.mission_params["acoustic_pinger_active_index"].get()
     self.gate_index = self.gate_index - 1
     yield self.get_objects()
     yield self.set_side()
     self.get_gate_thru_points()
     yield self.go_thru_gate()
     fprint("PINGER EXIT: Done", msg_color='green') 

 def search_side(self):
     fprint("Searching side", title="DETECT DELIVER", msg_color='green')
     start_time = self.nh.get_time()
     while self.nh.get_time() - start_time < genpy.Duration(self.LOOK_AT_TIME):
         res = yield self.get_any_shape()
         if res is not False:
             defer.returnValue(res)
         yield self.nh.sleep(0.1)
     defer.returnValue(False)

def main(navigator, **kwargs):
    nh = navigator.nh
    attempts = kwargs["attempts"]
    fprint("{} running".format(__name__), msg_color='red')
    if attempts > 1:
        yield nh.sleep(1)
    else:
        yield nh.sleep(2)

    fprint("{} stopped running".format(__name__), msg_color='red')

 def _end_mission(self, result):
     self.pub_msn_info.publish(String("Ending Mission {}".format(self.current_mission_name)))
     fprint(str(result) + " TIME: " + str((self.nh.get_time() - self.current_mission_start_time).to_sec()),
            msg_color="green", title="{} MISSION COMPLETE: ".format(self.current_mission_name))
     self.running_mission = False
     self.current_mission_name = None
     self.helper.stop_ensuring_object_permanence()
     self._mission_complete(self.current_mission)
     self.current_mission = None
     self.current_mission_timeout = None

 def _mission_complete(self, mission):
     self.tree.remove(mission)
     self.current_mission_name = None
     for m in mission.children:
         self.tree.append(m)
         self.refresh()
     self.completed_mission += 1
     if self.completed_mission == self.total_mission_count:
         fprint("ALL MISSIONS COMPLETE", msg_color="green")
         self.keep_running = False

    def refresh(self):
        """
        Called when the state of the DAG needs to be updated due to a mission completing or an object being found.

        CALLED ASYNCHRONOUS
        """
        for mission in self.tree:
            if self.can_complete(mission) and not self._is_in_queue(mission) and mission.name != self.current_mission_name:
                fprint("mission: {}".format(mission.name), msg_color="blue", title="ADDING")
                self.queue.put(mission)

 def setup_mission(self):
     stc_color = yield self.mission_params["scan_the_code_color3"].get(raise_exception=False)
     if stc_color is False:
         color = "ANY"
     else:
         color = stc_color
     # color = "ANY"
     shape = "ANY"
     fprint("Setting search shape={} color={}".format(shape, color), title="DETECT DELIVER", msg_color='green')
     yield self.mission_params["detect_deliver_shape"].set(shape)
     yield self.mission_params["detect_deliver_color"].set(color)

    def new_item(self, obj):
        """
        Callback for a new object being found.

        ASYNCHRONOUS
        """
        fprint("NEW ITEM: {}".format(obj.name), msg_color="blue")
        if self.base_mission is not None and self.current_mission_name is self.base_mission.name:
            self.mission_defer.cancel()
        self.found.add(obj.name)
        self.refresh()