def test_lookup_asc_edge(self):
self.assertAlmostEqual(interpolate_lookup_table(1000, TABLE_ASC),
0.05,
places=3)
self.assertAlmostEqual(interpolate_lookup_table(30, TABLE_ASC),
0.37,
places=3)
def test_lookup_asc_extrapolation(self):
self.assertAlmostEqual(interpolate_lookup_table(2000, TABLE_ASC),
0,
places=3)
self.assertAlmostEqual(interpolate_lookup_table(0, TABLE_ASC),
0.475,
places=3)
def test_lookup_asc_interpolation(self):
self.assertAlmostEqual(interpolate_lookup_table(900, TABLE_ASC),
0.117,
places=3)
self.assertAlmostEqual(interpolate_lookup_table(31, TABLE_ASC),
0.3665,
places=3)
def __publish_laserscan_data(self):
stamp = Time(seconds=self.robot.getTime()).to_msg()
lookup_table_infrared = self.distance_sensors[
'front infrared sensor'].getLookupTable()
lookup_table_ultrasonic = self.distance_sensors[
'front ultrasonic sensor'].getLookupTable()
msg = LaserScan()
msg.header.frame_id = 'laser_scanner'
msg.header.stamp = stamp
msg.angle_min = -135 * pi / 180
msg.angle_max = 180 * pi / 180
msg.angle_increment = 45 * pi / 180
msg.range_min = min(lookup_table_infrared[0],
lookup_table_infrared[-3]) + 0.01
msg.range_max = max(lookup_table_ultrasonic[0],
lookup_table_ultrasonic[-3]) - 0.1
for name, angle in DISTANCE_SENSOR_ANGLE.items():
distance = interpolate_lookup_table(
self.distance_sensors[name].getValue(), lookup_table_infrared)
if distance > max(lookup_table_infrared[0],
lookup_table_infrared[-3]) - 0.03:
ultrasonic_sensor = self.__get_ultrasonic_at_angle(angle)
if ultrasonic_sensor:
distance = interpolate_lookup_table(
ultrasonic_sensor.getValue(), lookup_table_ultrasonic)
else:
distance = 0
msg.ranges.append(distance)
self.laser_publisher.publish(msg)
def __publish_laserscan_data(self):
stamp = Time(seconds=self.robot.getTime()).to_msg()
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_lookup_table(
self.distance_sensors[key].getValue(),
self.distance_sensors[key].getLookupTable())
# Publish range: ToF
if self.tof_sensor:
dist_tof = interpolate_lookup_table(
self.tof_sensor.getValue(), self.tof_sensor.getLookupTable())
# 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
laser_dists = [
OUT_OF_RANGE if dist > INFRARED_MAX_RANGE else dist
for dist in dists
]
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 + TOF_MAX_RANGE
msg.ranges = [
laser_dists[3] + SENSOR_DIST_FROM_CENTER, # -150
OUT_OF_RANGE, # -135
OUT_OF_RANGE, # -120
OUT_OF_RANGE, # -105
laser_dists[2] + SENSOR_DIST_FROM_CENTER, # -90
OUT_OF_RANGE, # -75
OUT_OF_RANGE, # -60
laser_dists[1] + SENSOR_DIST_FROM_CENTER, # -45
OUT_OF_RANGE, # -30
laser_dists[0] + SENSOR_DIST_FROM_CENTER, # -15
dist_tof + SENSOR_DIST_FROM_CENTER, # 0
laser_dists[7] + SENSOR_DIST_FROM_CENTER, # 15
OUT_OF_RANGE, # 30
laser_dists[6] + SENSOR_DIST_FROM_CENTER, # 45
OUT_OF_RANGE, # 60
OUT_OF_RANGE, # 75
laser_dists[5] + SENSOR_DIST_FROM_CENTER, # 90
OUT_OF_RANGE, # 105
OUT_OF_RANGE, # 120
OUT_OF_RANGE, # 135
laser_dists[4] + SENSOR_DIST_FROM_CENTER, # 150
]
self.laser_publisher.publish(msg)
def step(self):
stamp = super().step()
if not stamp:
return
# Publish light sensor data
if self._publisher.get_subscription_count() > 0 or self._always_publish:
self._wb_device.enable(self._timestep)
msg = Illuminance()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.illuminance = interpolate_lookup_table(self._wb_device.getValue(
), self._wb_device.getLookupTable()) * IRRADIANCE_TO_ILLUMINANCE
msg.variance = self.__get_variance(self._wb_device.getValue())
self._publisher.publish(msg)
else:
self._wb_device.disable()
def step(self):
stamp = super().step()
if not stamp:
return
if self._publisher.get_subscription_count(
) > 0 or self._always_publish:
self.__enable_imu()
msg = Imu()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
if self.__accelerometer:
raw_data = self.__accelerometer.getValues()
msg.linear_acceleration.x = interpolate_lookup_table(
raw_data[0], self.__accelerometer.getLookupTable())
msg.linear_acceleration.y = interpolate_lookup_table(
raw_data[1], self.__accelerometer.getLookupTable())
msg.linear_acceleration.z = interpolate_lookup_table(
raw_data[2], self.__accelerometer.getLookupTable())
if self.__gyro:
raw_data = self.__gyro.getValues()
msg.angular_velocity.x = interpolate_lookup_table(
raw_data[0], self.__gyro.getLookupTable())
msg.angular_velocity.y = interpolate_lookup_table(
raw_data[1], self.__gyro.getLookupTable())
msg.angular_velocity.z = interpolate_lookup_table(
raw_data[2], self.__gyro.getLookupTable())
if self.__inertial_unit:
raw_data = self.__inertial_unit.getValues()
msg.orientation.x = interpolate_lookup_table(
raw_data[0], self.__inertial_unit.getLookupTable())
msg.orientation.y = interpolate_lookup_table(
raw_data[1], self.__inertial_unit.getLookupTable())
msg.orientation.z = interpolate_lookup_table(
raw_data[2], self.__inertial_unit.getLookupTable())
self._publisher.publish(msg)
else:
self.__disable_imu()
def __get_variance(self, raw_value):
table = self._wb_device.getLookupTable()
# Find relative standard deviation in lookup table
relative_std = None
for i in range(0, len(table) - 3, 3):
if table[i+1] < raw_value < table[i+3+1] or table[i+1] > raw_value > table[i+3+1]:
relative_std = table[i+2]
break
if relative_std is None:
if raw_value < table[1]:
relative_std = table[2]
else:
relative_std = table[-1]
# Calculate variance from the relative standard deviation
std = interpolate_lookup_table(raw_value, self._wb_device.getLookupTable()) * IRRADIANCE_TO_ILLUMINANCE * relative_std
return std**2
def step(self):
stamp = super().step()
if not stamp:
return
# Publish distance sensor data
if self._publisher.get_subscription_count(
) > 0 or self._always_publish:
self._wb_device.enable(self._timestep)
msg = Range()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.field_of_view = self._wb_device.getAperture()
msg.min_range = self._min_range
msg.max_range = self._max_range
msg.range = interpolate_lookup_table(
self._wb_device.getValue(), self._wb_device.getLookupTable())
msg.radiation_type = Range.INFRARED
self._publisher.publish(msg)
else:
self._wb_device.disable()
