| 812 | |
| 813 | |
| 814 | class RadarSensor(object): |
| 815 | def __init__(self, parent_actor): |
| 816 | self.sensor = None |
| 817 | self._parent = parent_actor |
| 818 | self.velocity_range = 7.5 # m/s |
| 819 | world = self._parent.get_world() |
| 820 | self.debug = world.debug |
| 821 | bp = world.get_blueprint_library().find('sensor.other.radar') |
| 822 | bp.set_attribute('horizontal_fov', str(35)) |
| 823 | bp.set_attribute('vertical_fov', str(20)) |
| 824 | self.sensor = world.spawn_actor( |
| 825 | bp, |
| 826 | carla.Transform( |
| 827 | carla.Location(x=2.8, z=1.0), |
| 828 | carla.Rotation(pitch=5)), |
| 829 | attach_to=self._parent) |
| 830 | # We need a weak reference to self to avoid circular reference. |
| 831 | weak_self = weakref.ref(self) |
| 832 | self.sensor.listen( |
| 833 | lambda radar_data: RadarSensor._Radar_callback(weak_self, radar_data)) |
| 834 | |
| 835 | @staticmethod |
| 836 | def _Radar_callback(weak_self, radar_data): |
| 837 | self = weak_self() |
| 838 | if not self: |
| 839 | return |
| 840 | # To get a numpy [[vel, altitude, azimuth, depth],...[,,,]]: |
| 841 | # points = np.frombuffer(radar_data.raw_data, dtype=np.dtype('f4')) |
| 842 | # points = np.reshape(points, (len(radar_data), 4)) |
| 843 | |
| 844 | current_rot = radar_data.transform.rotation |
| 845 | for detect in radar_data: |
| 846 | azi = math.degrees(detect.azimuth) |
| 847 | alt = math.degrees(detect.altitude) |
| 848 | # The 0.25 adjusts a bit the distance so the dots can |
| 849 | # be properly seen |
| 850 | fw_vec = carla.Vector3D(x=detect.depth - 0.25) |
| 851 | carla.Transform( |
| 852 | carla.Location(), |
| 853 | carla.Rotation( |
| 854 | pitch=current_rot.pitch + alt, |
| 855 | yaw=current_rot.yaw + azi, |
| 856 | roll=current_rot.roll)).transform(fw_vec) |
| 857 | |
| 858 | def clamp(min_v, max_v, value): |
| 859 | return max(min_v, min(value, max_v)) |
| 860 | |
| 861 | norm_velocity = detect.velocity / self.velocity_range # range [-1, 1] |
| 862 | r = int(clamp(0.0, 1.0, 1.0 - norm_velocity) * 255.0) |
| 863 | g = int(clamp(0.0, 1.0, 1.0 - abs(norm_velocity)) * 255.0) |
| 864 | b = int(abs(clamp(- 1.0, 0.0, - 1.0 - norm_velocity)) * 255.0) |
| 865 | self.debug.draw_point( |
| 866 | radar_data.transform.location + fw_vec, |
| 867 | size=0.075, |
| 868 | life_time=0.06, |
| 869 | persistent_lines=False, |
| 870 | color=carla.Color(r, g, b)) |
| 871 | |