wall_follow.py 文件源码

def lidarCB(self, msg):
        if not self.received_data:
            rospy.loginfo("success! first message received")

            # populate cached constants
            if self.direction == RIGHT:
                center_angle = -math.pi / 2
                self.direction_muliplier = -1
            else:
                center_angle = math.pi / 2
                self.direction_muliplier = 1

            self.min_angle = center_angle - FAN_ANGLE
            self.max_angle = center_angle + FAN_ANGLE
            self.laser_angles = (np.arange(len(msg.ranges)) * msg.angle_increment) + msg.angle_min

        self.data = msg.ranges
        values = np.array(msg.ranges)

        # remove out of range values
        ranges = values[(values > msg.range_min) & (values < msg.range_max)]
        angles = self.laser_angles[(values > msg.range_min) & (values < msg.range_max)]

        # apply median filter to clean outliers
        filtered_ranges = signal.medfilt(ranges, MEDIAN_FILTER_SIZE)

        # apply a window function to isolate values to the side of the car
        window = (angles > self.min_angle) & (angles < self.max_angle)
        filtered_ranges = filtered_ranges[window]
        filtered_angles = angles[window]

        # convert from polar to euclidean coordinate space
        self.ys = filtered_ranges * np.cos(filtered_angles)
        self.xs = filtered_ranges * np.sin(filtered_angles)

        self.fit_line()
        self.compute_pd_control()

        # filter lidar data to clean it up and remove outlisers
        self.received_data = True

        if PUBLISH_LINE:
            self.publish_line()

        if SHOW_VIS:
            # cache data for development visualization
            self.filtered_ranges = filtered_ranges
            self.filtered_angles = filtered_angles