def continuously_align(self):
      fprint("Starting Forest Align", title="DETECT DELIVER",  msg_color='green')
      try:
        while True:
            shooter_pose = yield self.shooter_pose_sub.get_next_message()
            if self.align_forest_pause:
                yield self.navigator.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.navigator.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')
      except Exception:
        traceback.print_exc()
        raise

 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 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 _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 _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 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 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 setup_mission(navigator):
    color = "ANY"
    shape = "ANY"
    #color = yield navigator.mission_params["scan_the_code_color3"].get()
    fprint("Setting search shape={} color={}".format(shape, color), title="DETECT DELIVER",  msg_color='green')
    yield navigator.mission_params["detect_deliver_shape"].set(shape)
    yield navigator.mission_params["detect_deliver_color"].set(color)

 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 shoot_all_balls(self):
     for i in range(self.NUM_BALLS):
         goal = yield self.shooterLoad.send_goal(ShooterDoAction())
         fprint("Loading Shooter {}".format(i), title="DETECT DELIVER",  msg_color='green')
         res = yield goal.get_result()
         yield self.navigator.nh.sleep(2)
         goal = yield self.shooterFire.send_goal(ShooterDoAction())
         fprint("Firing Shooter {}".format(i), title="DETECT DELIVER",  msg_color='green')
         res = yield goal.get_result()

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

 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 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 search_samples(self):
     """Move to each observation point and listen to the pinger while sitting still"""
     yield self.get_observation_poses()
     for i,p in enumerate(self.observation_points):
         yield self.stop_listen()
         yield self.navigator.move.set_position(p).look_at(self.look_at_points[i]).go()
         yield self.start_listen()
         fprint("PINGER: Listening To Pinger at point {}".format(p), msg_color='green')
         yield self.navigator.nh.sleep(self.LISTEN_TIME)
     yield self.stop_listen()

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