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 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 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()
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 __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 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_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_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)