コード例 #1
0
    def distance_callback(self, stamp):
        distance_from_center = 0.035

        for key in self.sensors:
            msg = Range()
            msg.field_of_view = self.sensors[key].getAperture()
            msg.min_range = intensity_to_distance(
                self.sensors[key].getMaxValue() - 8.2) + distance_from_center
            msg.max_range = intensity_to_distance(
                self.sensors[key].getMinValue() + 3.3) + distance_from_center
            msg.range = intensity_to_distance(self.sensors[key].getValue())
            msg.radiation_type = Range.INFRARED
            self.sensor_publishers[key].publish(msg)

        # Max range of ToF sensor is 2m so we put it as maximum laser range.
        # Therefore, for all invalid ranges we put 0 so it get deleted by rviz

        msg = LaserScan()
        msg.header.frame_id = 'laser_scanner'
        msg.header.stamp = stamp
        msg.angle_min = 0.0
        msg.angle_max = 2 * pi
        msg.angle_increment = 15 * pi / 180.0
        msg.scan_time = self.timestep.value / 1000
        msg.range_min = intensity_to_distance(
            self.sensors['ps0'].getMaxValue() - 20) + distance_from_center
        msg.range_max = 1.0 + distance_from_center
        msg.ranges = [
            self.sensors['tof'].getValue(),  # 0
            intensity_to_distance(self.sensors['ps7'].getValue()),  # 15
            0.0,  # 30
            intensity_to_distance(self.sensors['ps6'].getValue()),  # 45
            0.0,  # 60
            0.0,  # 75
            intensity_to_distance(self.sensors['ps5'].getValue()),  # 90
            0.0,  # 105
            0.0,  # 120
            0.0,  # 135
            intensity_to_distance(self.sensors['ps4'].getValue()),  # 150
            0.0,  # 165
            0.0,  # 180
            0.0,  # 195
            intensity_to_distance(self.sensors['ps3'].getValue()),  # 210
            0.0,  # 225
            0.0,  # 240
            0.0,  # 255
            intensity_to_distance(self.sensors['ps2'].getValue()),  # 270
            0.0,  # 285
            0.0,  # 300
            intensity_to_distance(self.sensors['ps1'].getValue()),  # 315
            0.0,  # 330
            intensity_to_distance(self.sensors['ps0'].getValue()),  # 345
            self.sensors['tof'].getValue(),  # 0
        ]
        for i in range(len(msg.ranges)):
            if msg.ranges[i] != 0:
                msg.ranges[i] += distance_from_center

        self.laser_publisher.publish(msg)
コード例 #2
0
    def publish_distance_data(self, stamp):
        dists = [OUT_OF_RANGE] * NB_INFRARED_SENSORS
        dist_tof = OUT_OF_RANGE

        # Calculate distances
        for i, key in enumerate(self.distance_sensors):
            dists[i] = interpolate_table(
                self.distance_sensors[key].getValue(), DISTANCE_TABLE)

        # Publish range: Infrared
        for i, key in enumerate(self.distance_sensors):
            msg = Range()
            msg.header.stamp = stamp
            msg.header.frame_id = key
            msg.field_of_view = self.distance_sensors[key].getAperture()
            msg.min_range = INFRARED_MIN_RANGE
            msg.max_range = INFRARED_MAX_RANGE
            msg.range = dists[i]
            msg.radiation_type = Range.INFRARED
            self.distance_sensor_publishers[key].publish(msg)

        # Publish range: ToF
        if self.tof_sensor:
            dist_tof = interpolate_table(self.tof_sensor.getValue(), TOF_TABLE)
            msg = Range()
            msg.header.stamp = stamp
            msg.header.frame_id = 'tof'
            msg.field_of_view = self.tof_sensor.getAperture()
            msg.min_range = TOF_MAX_RANGE
            msg.max_range = TOF_MIN_RANGE
            msg.range = dist_tof
            msg.radiation_type = Range.INFRARED
            self.tof_publisher.publish(msg)

        # Max range of ToF sensor is 2m so we put it as maximum laser range.
        # Therefore, for all invalid ranges we put 0 so it get deleted by rviz
        msg = LaserScan()
        msg.header.frame_id = 'laser_scanner'
        msg.header.stamp = stamp
        msg.angle_min = - 150 * pi / 180
        msg.angle_max = 150 * pi / 180
        msg.angle_increment = 15 * pi / 180
        msg.range_min = SENSOR_DIST_FROM_CENTER + INFRARED_MIN_RANGE
        msg.range_max = SENSOR_DIST_FROM_CENTER + INFRARED_MAX_RANGE
        msg.ranges = [
            dists[3] + SENSOR_DIST_FROM_CENTER,     # -150
            OUT_OF_RANGE,                           # -135
            OUT_OF_RANGE,                           # -120
            OUT_OF_RANGE,                           # -105
            dists[2] + SENSOR_DIST_FROM_CENTER,     # -90
            OUT_OF_RANGE,                           # -75
            OUT_OF_RANGE,                           # -60
            dists[1] + SENSOR_DIST_FROM_CENTER,     # -45
            OUT_OF_RANGE,                           # -30
            dists[0] + SENSOR_DIST_FROM_CENTER,     # -15
            OUT_OF_RANGE,                           # 0
            dists[7] + SENSOR_DIST_FROM_CENTER,     # 15
            OUT_OF_RANGE,                           # 30
            dists[6] + SENSOR_DIST_FROM_CENTER,     # 45
            OUT_OF_RANGE,                           # 60
            OUT_OF_RANGE,                           # 75
            dists[5] + SENSOR_DIST_FROM_CENTER,     # 90
            OUT_OF_RANGE,                           # 105
            OUT_OF_RANGE,                           # 120
            OUT_OF_RANGE,                           # 135
            dists[4] + SENSOR_DIST_FROM_CENTER,     # 150
        ]
        self.laser_publisher.publish(msg)