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