Python text_effects.fprint函数代码示例

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