wall_follow.py 文件源码

def __init__(self, direction):
        if direction not in [RIGHT, LEFT]:
            rospy.loginfo("incorect %s wall selected.  choose left or right")
            rospy.signal_shutdown()
        self.direction = direction

        if SHOW_VIS:
            self.viz = DynamicPlot()
            self.viz.initialize()

        self.pub = rospy.Publisher("/vesc/high_level/ackermann_cmd_mux/input/nav_0",\
                AckermannDriveStamped, queue_size =1 )
        self.sub = rospy.Subscriber("/scan", LaserScan, self.lidarCB, queue_size=1)

        if PUBLISH_LINE:
            self.line_pub = rospy.Publisher("/viz/line_fit", PolygonStamped, queue_size =1 )

        # computed control instructions
        self.control = None
        self.steering_hist = CircularArray(HISTORY_SIZE)

        # containers for laser scanner related data
        self.data = None
        self.xs = None
        self.ys = None
        self.m = 0
        self.c = 0

        # flag to indicate the first laser scan has been received
        self.received_data = False

        # cached constants
        self.min_angle = None
        self.max_angle = None
        self.direction_muliplier = 0
        self.laser_angles = None

        self.drive_thread = Thread(target=self.apply_control)
        self.drive_thread.start()

        if SHOW_VIS:
            while not rospy.is_shutdown():
                self.viz_loop()
                rospy.sleep(0.1)