swarmctrl.py 文件源码

def __init__(self):
        self._angle = 0;

        self._step = STEP * -1
        self._x = LINE_START_X
        self._y = LINE_START_Y

        self._x = RECT_START_X
        self._y = RECT_START_Y
        self._step_x = STEP
        self._step_y = STEP
        self._q_side = 0

        self._cf = [Swarmfly(sc=self, cfid=0, color=ColorRGBA(1, 0, 1, 1), hoverpoint=Point(SPACE_OFFSET_X+1.2, SPACE_OFFSET_Y+1.2, SPACE_OFFSET_Z+1), takeoffpoint=Point(SPACE_OFFSET_X+1.25, SPACE_OFFSET_Y+0.15, SPACE_OFFSET_Z+0.5), landingpoint=Point(SPACE_OFFSET_X+0.25, SPACE_OFFSET_Y+0.15, SPACE_OFFSET_Z+0.5)),
                    Swarmfly(sc=self, cfid=1, color=ColorRGBA(1, 1, 0, 1), hoverpoint=Point(SPACE_OFFSET_X+0.7, SPACE_OFFSET_Y+0.7, SPACE_OFFSET_Z+1), takeoffpoint=Point(SPACE_OFFSET_X+1.75, SPACE_OFFSET_Y+0.15, SPACE_OFFSET_Z+0.5), landingpoint=Point(SPACE_OFFSET_X+0.75, SPACE_OFFSET_Y+0.15, SPACE_OFFSET_Z+0.5))]

        for cf in self._cf:
            cf.hoverpoint = self._calc_circle_position(cf.id)

        self._rand_p1 = None
        self._rand_p2 = None

        self.timer = rospy.Timer(rospy.Duration(1.0/UPDATE_RATE),
                                 self._run)