diff --git a/.gitignore b/.gitignore index 8efdd286c1e..b1cb5f56ede 100644 --- a/.gitignore +++ b/.gitignore @@ -26,4 +26,5 @@ Util/Build __pycache__ _benchmarks_results _images* +_out core diff --git a/CARLA.sublime-project b/CARLA.sublime-project index c119c3665c7..89d65fea87e 100644 --- a/CARLA.sublime-project +++ b/CARLA.sublime-project @@ -76,7 +76,7 @@ { "name": "CARLA - make CarlaUE4", "working_dir": "${project_path}/Unreal/CarlaUE4", - "file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$", + "file_regex": "Unreal\\/CarlaUE4\\/([^:]*):([0-9]+):?([0-9]+)?:? (.*)$", "syntax": "Packages/Makefile/Make Output.sublime-syntax", "linux": { @@ -86,7 +86,7 @@ { "name": "CARLA - make CarlaUE4 ARGS=-clean", "working_dir": "${project_path}/Unreal/CarlaUE4", - "file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$", + "file_regex": "Unreal\\/CarlaUE4\\/([^:]*):([0-9]+):?([0-9]+)?:? (.*)$", "syntax": "Packages/Makefile/Make Output.sublime-syntax", "linux": { @@ -96,7 +96,7 @@ { "name": "CARLA - make CarlaUE4Editor", "working_dir": "${project_path}/Unreal/CarlaUE4", - "file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$", + "file_regex": "Unreal\\/CarlaUE4\\/([^:]*):([0-9]+):?([0-9]+)?:? (.*)$", "syntax": "Packages/Makefile/Make Output.sublime-syntax", "linux": { @@ -106,7 +106,7 @@ { "name": "CARLA - make CarlaUE4Editor ARGS=-clean", "working_dir": "${project_path}/Unreal/CarlaUE4", - "file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$", + "file_regex": "Unreal\\/CarlaUE4\\/([^:]*):([0-9]+):?([0-9]+)?:? (.*)$", "syntax": "Packages/Makefile/Make Output.sublime-syntax", "linux": { diff --git a/Docs/Example.CarlaSettings.ini b/Docs/Example.CarlaSettings.ini index a9e904672df..8e08b0cfd5c 100644 --- a/Docs/Example.CarlaSettings.ini +++ b/Docs/Example.CarlaSettings.ini @@ -1,16 +1,20 @@ ; Example of settings file for CARLA. ; -; Use it with `./CarlaUE4.sh -carla-settings=Path/To/This/File`. +; This file can be loaded with the Python client to be sent to the server. It +; defines the parameters to be used when requesting a new episode. +; +; Note that server specific variables are loaded in server only. Use it with +; `./CarlaUE4.sh -carla-settings=Path/To/This/File`. [CARLA/Server] ; If set to false, a mock controller will be used instead of waiting for a real -; client to connect. +; client to connect. (Server only) UseNetworking=true ; Ports to use for the server-client communication. This can be overridden by ; the command-line switch `-world-port=N`, write and read ports will be set to -; N+1 and N+2 respectively. +; N+1 and N+2 respectively. (Server only) WorldPort=2000 -; Time-out in milliseconds for the networking operations. +; Time-out in milliseconds for the networking operations. (Server only) ServerTimeOut=10000 ; In synchronous mode, CARLA waits every frame until the control from the client ; is received. @@ -36,16 +40,26 @@ WeatherId=1 SeedVehicles=123456789 SeedPedestrians=123456789 -[CARLA/SceneCapture] -; Names of the cameras to be attached to the player, comma-separated, each of -; them should be defined in its own subsection. E.g., Uncomment next line to add -; a camera called MyCamera to the vehicle +[CARLA/Sensor] +; Names of the sensors to be attached to the player, comma-separated, each of +; them should be defined in its own subsection. + +; Uncomment next line to add a camera called MyCamera to the vehicle +; Sensors=MyCamera + +; or uncomment next line to add a camera and a LiDAR +; Sensors=MyCamera,MyLidar -; Cameras=MyCamera +; or uncomment next line to add a regular camera and a depth camera +; Sensors=MyCamera,MyCamera/Depth ; Now, every camera we added needs to be defined it in its own subsection. -[CARLA/SceneCapture/MyCamera] -; Post-processing effect to be applied. Valid values: +[CARLA/Sensor/MyCamera] +; Type of the sensor. The available types are: +; * CAMERA A scene capture camera. +; * LIDAR_RAY_TRACE A LiDAR implementation based on ray-tracing. +SensorType=CAMERA +; Post-processing effect to be applied to this camera. Valid values: ; * None No effects applied. ; * SceneFinal Post-processing present at scene (bloom, fog, etc). ; * Depth Depth map ground-truth only. @@ -55,42 +69,39 @@ PostProcessing=SceneFinal ImageSizeX=800 ImageSizeY=600 ; Camera (horizontal) field of view in degrees. -CameraFOV=90 +FOV=90 ; Position of the camera relative to the car in centimeters. -CameraPositionX=15 -CameraPositionY=0 -CameraPositionZ=123 +PositionX=15 +PositionY=0 +PositionZ=123 ; Rotation of the camera relative to the car in degrees. -CameraRotationPitch=8 -CameraRotationRoll=0 -CameraRotationYaw=0 +RotationPitch=8 +RotationRoll=0 +RotationYaw=0 -; Stereo setup example: -; -; [CARLA/SceneCapture] -; Cameras=CameraStereoLeft/RGB,CameraStereoLeft/Depth,CameraStereoRight/RGB,CameraStereoRight/Depth -; ImageSizeX=720 -; ImageSizeY=512 -; CameraFOV=90 -; [CARLA/SceneCapture/CameraStereoLeft] -; CameraPositionX=170 -; CameraPositionY=-30 -; CameraPositionZ=150 -; CameraRotationPitch=0 -; CameraRotationRoll=0 -; CameraRotationYaw=0 -; [CARLA/SceneCapture/CameraStereoLeft/RGB] -; PostProcessing=SceneFinal -; [CARLA/SceneCapture/CameraStereoLeft/Depth] -; PostProcessing=Depth -; [CARLA/SceneCapture/CameraStereoRight] -; CameraPositionX=170 -; CameraPositionY=30 -; CameraPositionZ=150 -; CameraRotationPitch=0 -; CameraRotationRoll=0 -; CameraRotationYaw=0 -; [CARLA/SceneCapture/CameraStereoRight/RGB] -; PostProcessing=SceneFinal -; [CARLA/SceneCapture/CameraStereoRight/Depth] -; PostProcessing=Depth +[CARLA/Sensor/MyCamera/Depth] +; The sensor can be defined in a subsection of MyCamera so it inherits the +; values in MyCamera. This adds a camera similar to MyCamera but generating +; depth map images instead. +PostProcessing=Depth + +[CARLA/Sensor/MyLidar] +SensorType=LIDAR_RAY_TRACE +; Number of lasers. +Channels=32 +; Measure distance in centimeters. +Range=5000 +; Points generated by all lasers per second. +PointsPerSecond=100000 +; Lidar rotation frequency. +RotationFrequency=10 +; Upper and lower laser angles, positive values means above horizontal line. +UpperFOVLimit=10 +LowerFOVLimit=-30 +; Position and rotation relative to the vehicle. +PositionX=0 +PositionY=0 +PositionZ=140 +RotationPitch=0 +RotationYaw=0 +RotationRoll=0 diff --git a/Docs/carla_server.md b/Docs/carla_server.md index 104b2234396..ebbd38c8f53 100644 --- a/Docs/carla_server.md +++ b/Docs/carla_server.md @@ -48,10 +48,11 @@ Server only writes, first measurements message then the bulk of raw images. Every image is an array of uint32's - [width, height, type, color[0], color[1],...] + [width, height, type, FOV, color[0], color[1],...] -where each color is an [FColor][fcolorlink] (BGRA) as stored in Unreal Engine, -and the possible types of images are +where FOV is the horizontal field of view of the camera as float, each color is +an [FColor][fcolorlink] (BGRA) as stored in Unreal Engine, and the possible +types of images are type = 0 None (RGB without any post-processing) type = 1 SceneFinal (RGB with post-processing present at the scene) diff --git a/PythonClient/.pylintrc b/PythonClient/.pylintrc index dd0ef806f46..a8085ad7b56 100644 --- a/PythonClient/.pylintrc +++ b/PythonClient/.pylintrc @@ -1,4 +1,4 @@ [TYPECHECK] ignore=carla_server_pb2.py ignored-modules=ConfigParser,numpy,numpy.random,pygame,shutil -ignored-classes=_socketobject,EpisodeReady +ignored-classes=_socketobject,EpisodeReady,SceneDescription,Sensor diff --git a/PythonClient/carla/benchmarks/corl_2017.py b/PythonClient/carla/benchmarks/corl_2017.py index 55a12b46c91..5989ee1d2bb 100644 --- a/PythonClient/carla/benchmarks/corl_2017.py +++ b/PythonClient/carla/benchmarks/corl_2017.py @@ -138,7 +138,7 @@ def _build_experiments(self): # This single RGB camera is used on every experiment camera = Camera('CameraRGB') - camera.set(CameraFOV=100) + camera.set(FOV=100) camera.set_image_size(800, 600) diff --git a/PythonClient/carla/carla_server_pb2.py b/PythonClient/carla/carla_server_pb2.py index 258015ba128..d6f42678b6b 100644 --- a/PythonClient/carla/carla_server_pb2.py +++ b/PythonClient/carla/carla_server_pb2.py @@ -19,11 +19,37 @@ name='carla_server.proto', package='carla_server', syntax='proto3', - serialized_pb=_b('\n\x12\x63\x61rla_server.proto\x12\x0c\x63\x61rla_server\"+\n\x08Vector3D\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\t\n\x01z\x18\x03 \x01(\x02\"6\n\nRotation3D\x12\r\n\x05pitch\x18\x01 \x01(\x02\x12\x0b\n\x03yaw\x18\x02 \x01(\x02\x12\x0c\n\x04roll\x18\x03 \x01(\x02\"\x92\x01\n\tTransform\x12(\n\x08location\x18\x01 \x01(\x0b\x32\x16.carla_server.Vector3D\x12/\n\x0borientation\x18\x02 \x01(\x0b\x32\x16.carla_server.Vector3DB\x02\x18\x01\x12*\n\x08rotation\x18\x03 \x01(\x0b\x32\x18.carla_server.Rotation3D\"x\n\x07Vehicle\x12*\n\ttransform\x18\x01 \x01(\x0b\x32\x17.carla_server.Transform\x12*\n\nbox_extent\x18\x02 \x01(\x0b\x32\x16.carla_server.Vector3D\x12\x15\n\rforward_speed\x18\x03 \x01(\x02\"{\n\nPedestrian\x12*\n\ttransform\x18\x01 \x01(\x0b\x32\x17.carla_server.Transform\x12*\n\nbox_extent\x18\x02 \x01(\x0b\x32\x16.carla_server.Vector3D\x12\x15\n\rforward_speed\x18\x03 \x01(\x02\"\x94\x01\n\x0cTrafficLight\x12*\n\ttransform\x18\x01 \x01(\x0b\x32\x17.carla_server.Transform\x12/\n\x05state\x18\x02 \x01(\x0e\x32 .carla_server.TrafficLight.State\"\'\n\x05State\x12\t\n\x05GREEN\x10\x00\x12\n\n\x06YELLOW\x10\x01\x12\x07\n\x03RED\x10\x02\"Q\n\x0eSpeedLimitSign\x12*\n\ttransform\x18\x01 \x01(\x0b\x32\x17.carla_server.Transform\x12\x13\n\x0bspeed_limit\x18\x02 \x01(\x02\"\xe5\x01\n\x05\x41gent\x12\n\n\x02id\x18\x01 \x01(\x07\x12(\n\x07vehicle\x18\x02 \x01(\x0b\x32\x15.carla_server.VehicleH\x00\x12.\n\npedestrian\x18\x03 \x01(\x0b\x32\x18.carla_server.PedestrianH\x00\x12\x33\n\rtraffic_light\x18\x04 \x01(\x0b\x32\x1a.carla_server.TrafficLightH\x00\x12\x38\n\x10speed_limit_sign\x18\x05 \x01(\x0b\x32\x1c.carla_server.SpeedLimitSignH\x00\x42\x07\n\x05\x61gent\"%\n\x11RequestNewEpisode\x12\x10\n\x08ini_file\x18\x01 \x01(\t\"G\n\x10SceneDescription\x12\x33\n\x12player_start_spots\x18\x01 \x03(\x0b\x32\x17.carla_server.Transform\"/\n\x0c\x45pisodeStart\x12\x1f\n\x17player_start_spot_index\x18\x01 \x01(\r\"\x1d\n\x0c\x45pisodeReady\x12\r\n\x05ready\x18\x01 \x01(\x08\"^\n\x07\x43ontrol\x12\r\n\x05steer\x18\x01 \x01(\x02\x12\x10\n\x08throttle\x18\x02 \x01(\x02\x12\r\n\x05\x62rake\x18\x03 \x01(\x02\x12\x12\n\nhand_brake\x18\x04 \x01(\x08\x12\x0f\n\x07reverse\x18\x05 \x01(\x08\"\x8a\x04\n\x0cMeasurements\x12\x1a\n\x12platform_timestamp\x18\x01 \x01(\r\x12\x16\n\x0egame_timestamp\x18\x02 \x01(\r\x12J\n\x13player_measurements\x18\x03 \x01(\x0b\x32-.carla_server.Measurements.PlayerMeasurements\x12.\n\x11non_player_agents\x18\x04 \x03(\x0b\x32\x13.carla_server.Agent\x1a\xc9\x02\n\x12PlayerMeasurements\x12*\n\ttransform\x18\x01 \x01(\x0b\x32\x17.carla_server.Transform\x12,\n\x0c\x61\x63\x63\x65leration\x18\x03 \x01(\x0b\x32\x16.carla_server.Vector3D\x12\x15\n\rforward_speed\x18\x04 \x01(\x02\x12\x1a\n\x12\x63ollision_vehicles\x18\x05 \x01(\x02\x12\x1d\n\x15\x63ollision_pedestrians\x18\x06 \x01(\x02\x12\x17\n\x0f\x63ollision_other\x18\x07 \x01(\x02\x12\x1e\n\x16intersection_otherlane\x18\x08 \x01(\x02\x12\x1c\n\x14intersection_offroad\x18\t \x01(\x02\x12\x30\n\x11\x61utopilot_control\x18\n \x01(\x0b\x32\x15.carla_server.ControlB\x03\xf8\x01\x01\x62\x06proto3') + serialized_pb=_b('\n\x12\x63\x61rla_server.proto\x12\x0c\x63\x61rla_server\"+\n\x08Vector3D\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\t\n\x01z\x18\x03 \x01(\x02\"6\n\nRotation3D\x12\r\n\x05pitch\x18\x01 \x01(\x02\x12\x0b\n\x03yaw\x18\x02 \x01(\x02\x12\x0c\n\x04roll\x18\x03 \x01(\x02\"\x92\x01\n\tTransform\x12(\n\x08location\x18\x01 \x01(\x0b\x32\x16.carla_server.Vector3D\x12/\n\x0borientation\x18\x02 \x01(\x0b\x32\x16.carla_server.Vector3DB\x02\x18\x01\x12*\n\x08rotation\x18\x03 \x01(\x0b\x32\x18.carla_server.Rotation3D\"\x81\x01\n\x06Sensor\x12\n\n\x02id\x18\x01 \x01(\x07\x12\'\n\x04type\x18\x02 \x01(\x0e\x32\x19.carla_server.Sensor.Type\x12\x0c\n\x04name\x18\x03 \x01(\t\"4\n\x04Type\x12\x0b\n\x07UNKNOWN\x10\x00\x12\n\n\x06\x43\x41MERA\x10\x01\x12\x13\n\x0fLIDAR_RAY_TRACE\x10\x02\"x\n\x07Vehicle\x12*\n\ttransform\x18\x01 \x01(\x0b\x32\x17.carla_server.Transform\x12*\n\nbox_extent\x18\x02 \x01(\x0b\x32\x16.carla_server.Vector3D\x12\x15\n\rforward_speed\x18\x03 \x01(\x02\"{\n\nPedestrian\x12*\n\ttransform\x18\x01 \x01(\x0b\x32\x17.carla_server.Transform\x12*\n\nbox_extent\x18\x02 \x01(\x0b\x32\x16.carla_server.Vector3D\x12\x15\n\rforward_speed\x18\x03 \x01(\x02\"\x94\x01\n\x0cTrafficLight\x12*\n\ttransform\x18\x01 \x01(\x0b\x32\x17.carla_server.Transform\x12/\n\x05state\x18\x02 \x01(\x0e\x32 .carla_server.TrafficLight.State\"\'\n\x05State\x12\t\n\x05GREEN\x10\x00\x12\n\n\x06YELLOW\x10\x01\x12\x07\n\x03RED\x10\x02\"Q\n\x0eSpeedLimitSign\x12*\n\ttransform\x18\x01 \x01(\x0b\x32\x17.carla_server.Transform\x12\x13\n\x0bspeed_limit\x18\x02 \x01(\x02\"\xe5\x01\n\x05\x41gent\x12\n\n\x02id\x18\x01 \x01(\x07\x12(\n\x07vehicle\x18\x02 \x01(\x0b\x32\x15.carla_server.VehicleH\x00\x12.\n\npedestrian\x18\x03 \x01(\x0b\x32\x18.carla_server.PedestrianH\x00\x12\x33\n\rtraffic_light\x18\x04 \x01(\x0b\x32\x1a.carla_server.TrafficLightH\x00\x12\x38\n\x10speed_limit_sign\x18\x05 \x01(\x0b\x32\x1c.carla_server.SpeedLimitSignH\x00\x42\x07\n\x05\x61gent\"%\n\x11RequestNewEpisode\x12\x10\n\x08ini_file\x18\x01 \x01(\t\"n\n\x10SceneDescription\x12\x33\n\x12player_start_spots\x18\x01 \x03(\x0b\x32\x17.carla_server.Transform\x12%\n\x07sensors\x18\x02 \x03(\x0b\x32\x14.carla_server.Sensor\"/\n\x0c\x45pisodeStart\x12\x1f\n\x17player_start_spot_index\x18\x01 \x01(\r\"\x1d\n\x0c\x45pisodeReady\x12\r\n\x05ready\x18\x01 \x01(\x08\"^\n\x07\x43ontrol\x12\r\n\x05steer\x18\x01 \x01(\x02\x12\x10\n\x08throttle\x18\x02 \x01(\x02\x12\r\n\x05\x62rake\x18\x03 \x01(\x02\x12\x12\n\nhand_brake\x18\x04 \x01(\x08\x12\x0f\n\x07reverse\x18\x05 \x01(\x08\"\x8a\x04\n\x0cMeasurements\x12\x1a\n\x12platform_timestamp\x18\x01 \x01(\r\x12\x16\n\x0egame_timestamp\x18\x02 \x01(\r\x12J\n\x13player_measurements\x18\x03 \x01(\x0b\x32-.carla_server.Measurements.PlayerMeasurements\x12.\n\x11non_player_agents\x18\x04 \x03(\x0b\x32\x13.carla_server.Agent\x1a\xc9\x02\n\x12PlayerMeasurements\x12*\n\ttransform\x18\x01 \x01(\x0b\x32\x17.carla_server.Transform\x12,\n\x0c\x61\x63\x63\x65leration\x18\x03 \x01(\x0b\x32\x16.carla_server.Vector3D\x12\x15\n\rforward_speed\x18\x04 \x01(\x02\x12\x1a\n\x12\x63ollision_vehicles\x18\x05 \x01(\x02\x12\x1d\n\x15\x63ollision_pedestrians\x18\x06 \x01(\x02\x12\x17\n\x0f\x63ollision_other\x18\x07 \x01(\x02\x12\x1e\n\x16intersection_otherlane\x18\x08 \x01(\x02\x12\x1c\n\x14intersection_offroad\x18\t \x01(\x02\x12\x30\n\x11\x61utopilot_control\x18\n \x01(\x0b\x32\x15.carla_server.ControlB\x03\xf8\x01\x01\x62\x06proto3') ) +_SENSOR_TYPE = _descriptor.EnumDescriptor( + name='Type', + full_name='carla_server.Sensor.Type', + filename=None, + file=DESCRIPTOR, + values=[ + _descriptor.EnumValueDescriptor( + name='UNKNOWN', index=0, number=0, + options=None, + type=None), + _descriptor.EnumValueDescriptor( + name='CAMERA', index=1, number=1, + options=None, + type=None), + _descriptor.EnumValueDescriptor( + name='LIDAR_RAY_TRACE', index=2, number=2, + options=None, + type=None), + ], + containing_type=None, + options=None, + serialized_start=364, + serialized_end=416, +) +_sym_db.RegisterEnumDescriptor(_SENSOR_TYPE) + _TRAFFICLIGHT_STATE = _descriptor.EnumDescriptor( name='State', full_name='carla_server.TrafficLight.State', @@ -45,8 +71,8 @@ ], containing_type=None, options=None, - serialized_start=643, - serialized_end=682, + serialized_start=775, + serialized_end=814, ) _sym_db.RegisterEnumDescriptor(_TRAFFICLIGHT_STATE) @@ -186,6 +212,52 @@ ) +_SENSOR = _descriptor.Descriptor( + name='Sensor', + full_name='carla_server.Sensor', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='id', full_name='carla_server.Sensor.id', index=0, + number=1, type=7, cpp_type=3, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='type', full_name='carla_server.Sensor.type', index=1, + number=2, type=14, cpp_type=8, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='name', full_name='carla_server.Sensor.name', index=2, + number=3, type=9, cpp_type=9, label=1, + has_default_value=False, default_value=_b("").decode('utf-8'), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + _SENSOR_TYPE, + ], + options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=287, + serialized_end=416, +) + + _VEHICLE = _descriptor.Descriptor( name='Vehicle', full_name='carla_server.Vehicle', @@ -226,8 +298,8 @@ extension_ranges=[], oneofs=[ ], - serialized_start=286, - serialized_end=406, + serialized_start=418, + serialized_end=538, ) @@ -271,8 +343,8 @@ extension_ranges=[], oneofs=[ ], - serialized_start=408, - serialized_end=531, + serialized_start=540, + serialized_end=663, ) @@ -310,8 +382,8 @@ extension_ranges=[], oneofs=[ ], - serialized_start=534, - serialized_end=682, + serialized_start=666, + serialized_end=814, ) @@ -348,8 +420,8 @@ extension_ranges=[], oneofs=[ ], - serialized_start=684, - serialized_end=765, + serialized_start=816, + serialized_end=897, ) @@ -410,8 +482,8 @@ name='agent', full_name='carla_server.Agent.agent', index=0, containing_type=None, fields=[]), ], - serialized_start=768, - serialized_end=997, + serialized_start=900, + serialized_end=1129, ) @@ -441,8 +513,8 @@ extension_ranges=[], oneofs=[ ], - serialized_start=999, - serialized_end=1036, + serialized_start=1131, + serialized_end=1168, ) @@ -460,6 +532,13 @@ message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, options=None), + _descriptor.FieldDescriptor( + name='sensors', full_name='carla_server.SceneDescription.sensors', index=1, + number=2, type=11, cpp_type=10, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), ], extensions=[ ], @@ -472,8 +551,8 @@ extension_ranges=[], oneofs=[ ], - serialized_start=1038, - serialized_end=1109, + serialized_start=1170, + serialized_end=1280, ) @@ -503,8 +582,8 @@ extension_ranges=[], oneofs=[ ], - serialized_start=1111, - serialized_end=1158, + serialized_start=1282, + serialized_end=1329, ) @@ -534,8 +613,8 @@ extension_ranges=[], oneofs=[ ], - serialized_start=1160, - serialized_end=1189, + serialized_start=1331, + serialized_end=1360, ) @@ -593,8 +672,8 @@ extension_ranges=[], oneofs=[ ], - serialized_start=1191, - serialized_end=1285, + serialized_start=1362, + serialized_end=1456, ) @@ -680,8 +759,8 @@ extension_ranges=[], oneofs=[ ], - serialized_start=1481, - serialized_end=1810, + serialized_start=1652, + serialized_end=1981, ) _MEASUREMENTS = _descriptor.Descriptor( @@ -731,13 +810,15 @@ extension_ranges=[], oneofs=[ ], - serialized_start=1288, - serialized_end=1810, + serialized_start=1459, + serialized_end=1981, ) _TRANSFORM.fields_by_name['location'].message_type = _VECTOR3D _TRANSFORM.fields_by_name['orientation'].message_type = _VECTOR3D _TRANSFORM.fields_by_name['rotation'].message_type = _ROTATION3D +_SENSOR.fields_by_name['type'].enum_type = _SENSOR_TYPE +_SENSOR_TYPE.containing_type = _SENSOR _VEHICLE.fields_by_name['transform'].message_type = _TRANSFORM _VEHICLE.fields_by_name['box_extent'].message_type = _VECTOR3D _PEDESTRIAN.fields_by_name['transform'].message_type = _TRANSFORM @@ -763,6 +844,7 @@ _AGENT.fields_by_name['speed_limit_sign']) _AGENT.fields_by_name['speed_limit_sign'].containing_oneof = _AGENT.oneofs_by_name['agent'] _SCENEDESCRIPTION.fields_by_name['player_start_spots'].message_type = _TRANSFORM +_SCENEDESCRIPTION.fields_by_name['sensors'].message_type = _SENSOR _MEASUREMENTS_PLAYERMEASUREMENTS.fields_by_name['transform'].message_type = _TRANSFORM _MEASUREMENTS_PLAYERMEASUREMENTS.fields_by_name['acceleration'].message_type = _VECTOR3D _MEASUREMENTS_PLAYERMEASUREMENTS.fields_by_name['autopilot_control'].message_type = _CONTROL @@ -772,6 +854,7 @@ DESCRIPTOR.message_types_by_name['Vector3D'] = _VECTOR3D DESCRIPTOR.message_types_by_name['Rotation3D'] = _ROTATION3D DESCRIPTOR.message_types_by_name['Transform'] = _TRANSFORM +DESCRIPTOR.message_types_by_name['Sensor'] = _SENSOR DESCRIPTOR.message_types_by_name['Vehicle'] = _VEHICLE DESCRIPTOR.message_types_by_name['Pedestrian'] = _PEDESTRIAN DESCRIPTOR.message_types_by_name['TrafficLight'] = _TRAFFICLIGHT @@ -806,6 +889,13 @@ )) _sym_db.RegisterMessage(Transform) +Sensor = _reflection.GeneratedProtocolMessageType('Sensor', (_message.Message,), dict( + DESCRIPTOR = _SENSOR, + __module__ = 'carla_server_pb2' + # @@protoc_insertion_point(class_scope:carla_server.Sensor) + )) +_sym_db.RegisterMessage(Sensor) + Vehicle = _reflection.GeneratedProtocolMessageType('Vehicle', (_message.Message,), dict( DESCRIPTOR = _VEHICLE, __module__ = 'carla_server_pb2' diff --git a/PythonClient/carla/client.py b/PythonClient/carla/client.py index 77780812bd1..57ed04542b8 100644 --- a/PythonClient/carla/client.py +++ b/PythonClient/carla/client.py @@ -11,7 +11,6 @@ from contextlib import contextmanager from . import sensor -from . import settings from . import tcp from . import util @@ -20,6 +19,11 @@ except ImportError: raise RuntimeError('cannot import "carla_server_pb2.py", run the protobuf compiler to generate this file') +try: + import numpy +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed.') + VehicleControl = carla_protocol.Control @@ -40,7 +44,7 @@ def __init__(self, host, world_port, timeout=15): self._control_client = tcp.TCPClient(host, world_port + 2, timeout) self._current_settings = None self._is_episode_requested = False - self._sensor_names = [] + self._sensors = {} def connect(self, connection_attempts=10): """ @@ -69,7 +73,6 @@ def load_settings(self, carla_settings): self._current_settings = carla_settings return self._request_new_episode(carla_settings) - def start_episode(self, player_start_index): """ Start the new episode at the player start given by the @@ -120,8 +123,7 @@ def read_data(self): pb_message = carla_protocol.Measurements() pb_message.ParseFromString(data) # Read sensor data. - raw_sensor_data = self._stream_client.read() - return pb_message, self._parse_raw_sensor_data(raw_sensor_data) + return pb_message, dict(x for x in self._read_sensor_data()) def send_control(self, *args, **kwargs): """ @@ -159,30 +161,66 @@ def _request_new_episode(self, carla_settings): raise RuntimeError('failed to read data from server') pb_message = carla_protocol.SceneDescription() pb_message.ParseFromString(data) - self._sensor_names = settings.get_sensor_names(carla_settings) + self._sensors = dict((sensor.id, sensor) \ + for sensor in _make_sensor_parsers(pb_message.sensors)) self._is_episode_requested = True return pb_message - def _parse_raw_sensor_data(self, raw_data): - """Return a dict of {'sensor_name': sensor_data, ...}.""" - return dict((name, data) for name, data in zip( - self._sensor_names, - self._iterate_sensor_data(raw_data))) - - @staticmethod - def _iterate_sensor_data(raw_data): - # At this point the only sensors available are images, the raw_data - # consists of images only. - image_types = ['None', 'SceneFinal', 'Depth', 'SemanticSegmentation'] - gettype = lambda id: image_types[id] if len(image_types) > id else 'Unknown' - getval = lambda index: struct.unpack(' id else 'Unknown' + getint = lambda data, index: struct.unpack(' max_depth) + normalized_depth = numpy.delete(normalized_depth, max_depth_indexes) + u_coord = numpy.delete(u_coord, max_depth_indexes) + v_coord = numpy.delete(v_coord, max_depth_indexes) + if color is not None: + color = numpy.delete(color, max_depth_indexes, axis=0) + + # pd2 = [u,v,1] + p2d = numpy.array([u_coord, v_coord, numpy.ones_like(u_coord)]) + + # P = [X,Y,Z] + p3d = numpy.dot(numpy.linalg.inv(k), p2d) + p3d *= normalized_depth * far + + # Formating the output to: + # [[X1,Y1,Z1,R1,G1,B1],[X2,Y2,Z2,R2,G2,B2], ... [Xn,Yn,Zn,Rn,Gn,Bn]] + if color is not None: + # numpy.concatenate((numpy.transpose(p3d), color), axis=1) + return sensor.PointCloud(numpy.transpose(p3d), color_array=color) + # [[X1,Y1,Z1],[X2,Y2,Z2], ... [Xn,Yn,Zn]] + return sensor.PointCloud(numpy.transpose(p3d)) diff --git a/PythonClient/carla/sensor.py b/PythonClient/carla/sensor.py index 9710d95f4fc..b1600b413db 100644 --- a/PythonClient/carla/sensor.py +++ b/PythonClient/carla/sensor.py @@ -9,9 +9,30 @@ import os +from collections import namedtuple + +try: + import numpy +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed.') + +from .transform import Transform, Translation, Rotation, Scale + +# ============================================================================== +# -- Helpers ------------------------------------------------------------- +# ============================================================================== + + +Color = namedtuple('Color', 'r g b') +Color.__new__.__defaults__ = (0, 0, 0) + + +Point = namedtuple('Point', 'x y z color') +Point.__new__.__defaults__ = (0.0, 0.0, 0.0, None) + # ============================================================================== -# -- Sensor -------------------------------------------------------------------- +# -- Sensor -------------------------------------------------------------- # ============================================================================== @@ -19,7 +40,51 @@ class Sensor(object): """ Base class for sensor descriptions. Used to add sensors to CarlaSettings. """ - pass + + def __init__(self, name, sensor_type): + self.SensorName = name + self.SensorType = sensor_type + self.PositionX = 140 + self.PositionY = 0 + self.PositionZ = 140 + self.RotationPitch = 0 + self.RotationRoll = 0 + self.RotationYaw = 0 + + def set(self, **kwargs): + for key, value in kwargs.items(): + if not hasattr(self, key): + raise ValueError('sensor.Sensor: no key named %r' % key) + setattr(self, key, value) + + def set_position(self, x, y, z): + self.PositionX = x + self.PositionY = y + self.PositionZ = z + + def set_rotation(self, pitch, yaw, roll): + self.RotationPitch = pitch + self.RotationYaw = yaw + self.RotationRoll = roll + + def get_transform(self): + ''' + Returns the camera to [whatever the camera is attached to] + transformation. + ''' + return Transform( + Translation(self.PositionX, self.PositionY, self.PositionZ), + Rotation(self.RotationPitch, self.RotationYaw, self.RotationRoll)) + + def get_unreal_transform(self): + ''' + Returns the camera to [whatever the camera is attached to] + transformation with the Unreal necessary corrections applied. + + @todo Do we need to expose this? + ''' + to_unreal_transform = Transform(Rotation(roll=-90, yaw=90), Scale(x=-1)) + return self.get_transform() * to_unreal_transform class Camera(Sensor): @@ -29,42 +94,39 @@ class Camera(Sensor): """ def __init__(self, name, **kwargs): - self.CameraName = name + super(Camera, self).__init__(name, sensor_type="CAMERA") self.PostProcessing = 'SceneFinal' self.ImageSizeX = 800 self.ImageSizeY = 600 - self.CameraFOV = 90 - self.CameraPositionX = 140 - self.CameraPositionY = 0 - self.CameraPositionZ = 140 - self.CameraRotationPitch = 0 - self.CameraRotationRoll = 0 - self.CameraRotationYaw = 0 + self.FOV = 90 self.set(**kwargs) - def set(self, **kwargs): - for key, value in kwargs.items(): - if not hasattr(self, key): - raise ValueError('CarlaSettings.Camera: no key named %r' % key) - setattr(self, key, value) - def set_image_size(self, pixels_x, pixels_y): + '''Sets the image size in pixels''' self.ImageSizeX = pixels_x self.ImageSizeY = pixels_y - def set_position(self, x, y, z): - self.CameraPositionX = x - self.CameraPositionY = y - self.CameraPositionZ = z - def set_rotation(self, pitch, roll, yaw): - self.CameraRotationPitch = pitch - self.CameraRotationRoll = roll - self.CameraRotationYaw = yaw +class Lidar(Sensor): + """ + Lidar description. This class can be added to a CarlaSettings object to add + a Lidar to the player vehicle. + """ + + def __init__(self, name, **kwargs): + super(Lidar, self).__init__(name, sensor_type="LIDAR_RAY_TRACE") + self.Channels = 32 + self.Range = 5000 + self.PointsPerSecond = 100000 + self.RotationFrequency = 10 + self.UpperFovLimit = 10 + self.LowerFovLimit = -30 + self.ShowDebugPoints = False + self.set(**kwargs) # ============================================================================== -# -- SensorData ---------------------------------------------------------------- +# -- SensorData ---------------------------------------------------------- # ============================================================================== @@ -76,11 +138,12 @@ class SensorData(object): class Image(SensorData): """Data generated by a Camera.""" - def __init__(self, width, height, image_type, raw_data): + def __init__(self, width, height, image_type, fov, raw_data): assert len(raw_data) == 4 * width * height self.width = width self.height = height self.type = image_type + self.fov = fov self.raw_data = raw_data self._converted_data = None @@ -106,17 +169,154 @@ def save_to_disk(self, filename): try: from PIL import Image as PImage except ImportError: - raise RuntimeError('cannot import PIL, make sure pillow package is installed') + raise RuntimeError( + 'cannot import PIL, make sure pillow package is installed') image = PImage.frombytes( mode='RGBA', size=(self.width, self.height), data=self.raw_data, decoder_name='raw') - b, g, r, _ = image.split() - image = PImage.merge("RGB", (r, g, b)) + color = image.split() + image = PImage.merge("RGB", color[2::-1]) folder = os.path.dirname(filename) if not os.path.isdir(folder): os.makedirs(folder) image.save(filename) + + +class PointCloud(SensorData): + """A list of points.""" + + def __init__(self, array, color_array=None): + self._array = array + self._color_array = color_array + self._has_colors = color_array is not None + + @property + def array(self): + """The numpy array holding the point-cloud. + + 3D points format for n elements: + [ [X0,Y0,Z0], + ..., + [Xn,Yn,Zn] ] + """ + return self._array + + @property + def color_array(self): + """The numpy array holding the colors corresponding to each point. + It is None if there are no colors. + + Colors format for n elements: + [ [R0,G0,B0], + ..., + [Rn,Gn,Bn] ] + """ + return self._color_array + + def has_colors(self): + """Return whether the points have color.""" + return self._has_colors + + def apply_transform(self, transformation): + """Modify the PointCloud instance transforming its points""" + self._array = transformation.transform_points(self._array) + + def save_to_disk(self, filename): + """Save this point-cloud to disk as PLY format.""" + + def construct_ply_header(): + """Generates a PLY header given a total number of 3D points and + coloring property if specified + """ + points = len(self) # Total point number + header = ['ply', + 'format ascii 1.0', + 'element vertex {}', + 'property float32 x', + 'property float32 y', + 'property float32 z', + 'property uchar diffuse_red', + 'property uchar diffuse_green', + 'property uchar diffuse_blue', + 'end_header'] + if not self._has_colors: + return '\n'.join(header[0:6] + [header[-1]]).format(points) + return '\n'.join(header).format(points) + + if not self._has_colors: + ply = '\n'.join(['{:.2f} {:.2f} {:.2f}'.format( + *p) for p in self._array.tolist()]) + else: + points_3d = numpy.concatenate( + (self._array, self._color_array), axis=1) + ply = '\n'.join(['{:.2f} {:.2f} {:.2f} {:.0f} {:.0f} {:.0f}' + .format(*p) for p in points_3d.tolist()]) + + # Create folder to save if does not exist. + folder = os.path.dirname(filename) + if not os.path.isdir(folder): + os.makedirs(folder) + + # Open the file and save with the specific PLY format. + with open(filename, 'w+') as ply_file: + ply_file.write('\n'.join([construct_ply_header(), ply])) + + def __len__(self): + return len(self.array) + + def __getitem__(self, key): + color = None if self._color_array is None else Color( + *self._color_array[key]) + return Point(*self._array[key], color=color) + + def __iter__(self): + class PointIterator(object): + """Iterator class for PointCloud""" + + def __init__(self, point_cloud): + self.point_cloud = point_cloud + self.index = -1 + + def __next__(self): + self.index += 1 + if self.index >= len(self.point_cloud): + raise StopIteration + return self.point_cloud[self.index] + + def next(self): + return self.__next__() + + return PointIterator(self) + + def __str__(self): + return str(self.array) + + +class LidarMeasurement(SensorData): + """Data generated by a Lidar.""" + + def __init__(self, horizontal_angle, channels, points_count_by_channel, point_cloud): + assert numpy.sum(points_count_by_channel) == len(point_cloud.array) + self.horizontal_angle = horizontal_angle + self.channels_count = channels + self.points_count_by_channel = points_count_by_channel + self.point_cloud = point_cloud + + @property + def data(self): + """The numpy array holding the point-cloud. + + 3D points format for n elements: + [ [X0,Y0,Z0], + ..., + [Xn,Yn,Zn] ] + """ + return self.point_cloud.array + + def save_to_disk(self, filename): + """Save point-cloud to disk as PLY format.""" + self.point_cloud.save_to_disk(filename) diff --git a/PythonClient/carla/settings.py b/PythonClient/carla/settings.py index bea02433348..146da89471d 100644 --- a/PythonClient/carla/settings.py +++ b/PythonClient/carla/settings.py @@ -45,7 +45,7 @@ def __init__(self, **kwargs): self.SeedPedestrians = None self.randomize_weather() self.set(**kwargs) - self._cameras = [] + self._sensors = [] def set(self, **kwargs): for key, value in kwargs.items(): @@ -67,18 +67,20 @@ def randomize_weather(self): def add_sensor(self, sensor): """Add a sensor to the player vehicle (see sensor.py).""" - if isinstance(sensor, carla_sensor.Camera): - self._cameras.append(sensor) - else: + if not isinstance(sensor, carla_sensor.Sensor): raise ValueError('Sensor not supported') + self._sensors.append(sensor) def __str__(self): """Converts this object to an INI formatted string.""" ini = ConfigParser() - ini.optionxform=str + ini.optionxform = str S_SERVER = 'CARLA/Server' S_LEVEL = 'CARLA/LevelSettings' - S_CAPTURE = 'CARLA/SceneCapture' + S_SENSOR = 'CARLA/Sensor' + + def get_attribs(obj): + return [a for a in dir(obj) if not a.startswith('_') and not callable(getattr(obj, a))] def add_section(section, obj, keys): for key in keys: @@ -97,21 +99,12 @@ def add_section(section, obj, keys): 'SeedVehicles', 'SeedPedestrians']) - ini.add_section(S_CAPTURE) - ini.set(S_CAPTURE, 'Cameras', ','.join(c.CameraName for c in self._cameras)) - - for camera in self._cameras: - add_section(S_CAPTURE + '/' + camera.CameraName, camera, [ - 'PostProcessing', - 'ImageSizeX', - 'ImageSizeY', - 'CameraFOV', - 'CameraPositionX', - 'CameraPositionY', - 'CameraPositionZ', - 'CameraRotationPitch', - 'CameraRotationRoll', - 'CameraRotationYaw']) + ini.add_section(S_SENSOR) + ini.set(S_SENSOR, 'Sensors', ','.join(s.SensorName for s in self._sensors)) + + for sensor_def in self._sensors: + section = S_SENSOR + '/' + sensor_def.SensorName + add_section(section, sensor_def, get_attribs(sensor_def)) if sys.version_info >= (3, 0): text = io.StringIO() @@ -120,28 +113,3 @@ def add_section(section, obj, keys): ini.write(text) return text.getvalue().replace(' = ', '=') - - -def get_sensor_names(settings): - """ - Return a list with the names of the sensors defined in the settings object. - The settings object can be a CarlaSettings or an INI formatted string. - """ - if isinstance(settings, CarlaSettings): - # pylint: disable=protected-access - return [camera.CameraName for camera in settings._cameras] - ini = ConfigParser() - if sys.version_info >= (3, 2): - ini.read_string(settings) - elif sys.version_info >= (3, 0): - ini.readfp(io.StringIO(settings)) # pylint: disable=deprecated-method - else: - ini.readfp(io.BytesIO(settings)) # pylint: disable=deprecated-method - - section_name = 'CARLA/SceneCapture' - option_name = 'Cameras' - - if ini.has_section(section_name) and ini.has_option(section_name, option_name): - cameras = ini.get(section_name, option_name) - return cameras.split(',') - return [] diff --git a/PythonClient/carla/transform.py b/PythonClient/carla/transform.py new file mode 100644 index 00000000000..d17597e798c --- /dev/null +++ b/PythonClient/carla/transform.py @@ -0,0 +1,137 @@ +# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB), and the INTEL Visual Computing Lab. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +import math + +from collections import namedtuple + +try: + import numpy +except ImportError: + raise RuntimeError( + 'cannot import numpy, make sure numpy package is installed.') + +try: + from . import carla_server_pb2 as carla_protocol +except ImportError: + raise RuntimeError('cannot import "carla_server_pb2.py", run ' + 'the protobuf compiler to generate this file') + + +Translation = namedtuple('Translation', 'x y z') +Translation.__new__.__defaults__ = (0.0, 0.0, 0.0) + +Rotation = namedtuple('Rotation', 'pitch yaw roll') +Rotation.__new__.__defaults__ = (0.0, 0.0, 0.0) + +Scale = namedtuple('Scale', 'x y z') +Scale.__new__.__defaults__ = (1.0, 1.0, 1.0) + + +class Transform(object): + """A 3D transformation. + + The transformation is applied in the order: scale, rotation, translation. + """ + + def __init__(self, *args, **kwargs): + if 'matrix' in kwargs: + self.matrix = kwargs['matrix'] + return + if isinstance(args[0], carla_protocol.Transform): + args = [ + Translation( + args[0].location.x, + args[0].location.y, + args[0].location.z), + Rotation( + args[0].rotation.pitch, + args[0].rotation.yaw, + args[0].rotation.roll) + ] + self.matrix = numpy.matrix(numpy.identity(4)) + self.set(*args, **kwargs) + + def set(self, *args): + """Builds the transform matrix given a Translate, Rotation + and Scale. + """ + translation = Translation() + rotation = Rotation() + scale = Scale() + + if len(args) > 3: + raise ValueError("'Transform' accepts 3 values as maximum.") + + def get_single_obj_type(obj_type): + """Returns the unique object contained in the + arguments lists that is instance of 'obj_type'. + """ + obj = [x for x in args if isinstance(x, obj_type)] + if len(obj) > 1: + raise ValueError("Transform only accepts one instances of " + + str(obj_type) + " as a parameter") + elif not obj: + # Create an instance of the type that is 'obj_type' + return obj_type() + return obj[0] + + translation = get_single_obj_type(Translation) + rotation = get_single_obj_type(Rotation) + scale = get_single_obj_type(Scale) + + for param in args: + if not isinstance(param, Translation) and \ + not isinstance(param, Rotation) and \ + not isinstance(param, Scale): + raise TypeError( + "'" + str(type(param)) + "' type not match with \ + 'Translation', 'Rotation' or 'Scale'") + + # Transformation matrix + cy = math.cos(numpy.radians(rotation.yaw)) + sy = math.sin(numpy.radians(rotation.yaw)) + cr = math.cos(numpy.radians(rotation.roll)) + sr = math.sin(numpy.radians(rotation.roll)) + cp = math.cos(numpy.radians(rotation.pitch)) + sp = math.sin(numpy.radians(rotation.pitch)) + self.matrix[0, 3] = translation.x + self.matrix[1, 3] = translation.y + self.matrix[2, 3] = translation.z + self.matrix[0, 0] = scale.x * (cp * cy) + self.matrix[0, 1] = scale.y * (cy * sp * sr - sy * cr) + self.matrix[0, 2] = -scale.z * (cy * sp * cr + sy * sr) + self.matrix[1, 0] = scale.x * (sy * cp) + self.matrix[1, 1] = scale.y * (sy * sp * sr + cy * cr) + self.matrix[1, 2] = scale.z * (cy * sr - sy * sp * cr) + self.matrix[2, 0] = scale.x * (sp) + self.matrix[2, 1] = -scale.y * (cp * sr) + self.matrix[2, 2] = scale.z * (cp * cr) + + def inverse(self): + """Return the inverse transform.""" + return Transform(matrix=numpy.linalg.inv(self.matrix)) + + def transform_points(self, points): + """ + Given a 4x4 transformation matrix, transform an array of 3D points. + Expected point foramt: [[X0,Y0,Z0],..[Xn,Yn,Zn]] + """ + # Needed foramt: [[X0,..Xn],[Z0,..Zn],[Z0,..Zn]]. So let's transpose + # the point matrix. + points = points.transpose() + # Add 0s row: [[X0..,Xn],[Y0..,Yn],[Z0..,Zn],[0,..0]] + points = numpy.append(points, numpy.ones((1, points.shape[1])), axis=0) + # Point transformation + points = self.matrix * points + # Return all but last row + return points[0:3].transpose() + + def __mul__(self, other): + return Transform(matrix=numpy.dot(self.matrix, other.matrix)) + + def __str__(self): + return str(self.matrix) diff --git a/PythonClient/carla/util.py b/PythonClient/carla/util.py index 517576344ec..09844a60c8c 100644 --- a/PythonClient/carla/util.py +++ b/PythonClient/carla/util.py @@ -31,8 +31,11 @@ def __init__(self): def stop(self): self.end = datetime.datetime.now() + def seconds(self): + return (self.end - self.start).total_seconds() + def milliseconds(self): - return 1000.0 * (self.end - self.start).total_seconds() + return 1000.0 * self.seconds() def to_hex_str(header): diff --git a/PythonClient/client_example.py b/PythonClient/client_example.py index 325e7949d43..fc22420e488 100755 --- a/PythonClient/client_example.py +++ b/PythonClient/client_example.py @@ -16,13 +16,13 @@ import time from carla.client import make_carla_client -from carla.sensor import Camera +from carla.sensor import Camera, Lidar, LidarMeasurement from carla.settings import CarlaSettings from carla.tcp import TCPConnectionError from carla.util import print_over_same_line -def run_carla_client(host, port, autopilot_on, save_images_to_disk, image_filename_format, settings_filepath): +def run_carla_client(args): # Here we will run 3 episodes with 300 frames each. number_of_episodes = 3 frames_per_episode = 300 @@ -32,13 +32,13 @@ def run_carla_client(host, port, autopilot_on, save_images_to_disk, image_filena # context manager, it creates a CARLA client object and starts the # connection. It will throw an exception if something goes wrong. The # context manager makes sure the connection is always cleaned up on exit. - with make_carla_client(host, port) as client: + with make_carla_client(args.host, args.port) as client: print('CarlaClient connected') for episode in range(0, number_of_episodes): # Start a new episode. - if settings_filepath is None: + if args.settings_filepath is None: # Create a CarlaSettings object. This object is a wrapper around # the CarlaSettings.ini file. Here we set the configuration we @@ -70,10 +70,23 @@ def run_carla_client(host, port, autopilot_on, save_images_to_disk, image_filena camera1.set_position(30, 0, 130) settings.add_sensor(camera1) + if args.lidar: + lidar = Lidar('Lidar32') + lidar.set_position(0, 0, 250) + lidar.set_rotation(0, 0, 0) + lidar.set( + Channels=32, + Range=5000, + PointsPerSecond=100000, + RotationFrequency=10, + UpperFovLimit=10, + LowerFovLimit=-30) + settings.add_sensor(lidar) + else: # Alternatively, we can load these settings from a file. - with open(settings_filepath, 'r') as fp: + with open(args.settings_filepath, 'r') as fp: settings = fp.read() # Now we load these settings into the server. The server replies @@ -102,9 +115,13 @@ def run_carla_client(host, port, autopilot_on, save_images_to_disk, image_filena print_measurements(measurements) # Save the images to disk if requested. - if save_images_to_disk: - for name, image in sensor_data.items(): - image.save_to_disk(image_filename_format.format(episode, name, frame)) + if args.save_images_to_disk: + for name, measurement in sensor_data.items(): + if isinstance(measurement, LidarMeasurement): + filename = args.lidar_filename_format + else: + filename = args.image_filename_format + measurement.save_to_disk(filename.format(episode, name, frame)) # We can access the encoded data of a given image as numpy # array using its "data" property. For instance, to get the @@ -118,7 +135,7 @@ def run_carla_client(host, port, autopilot_on, save_images_to_disk, image_filena # If we are in synchronous mode the server will pause the # simulation until we send this control. - if not autopilot_on: + if not args.autopilot: client.send_control( steer=random.uniform(-1.0, 1.0), @@ -183,13 +200,19 @@ def main(): '-a', '--autopilot', action='store_true', help='enable autopilot') + argparser.add_argument( + '-l', '--lidar', + action='store_true', + help='enable Lidar') argparser.add_argument( '-i', '--images-to-disk', action='store_true', - help='save images to disk') + dest='save_images_to_disk', + help='save images (and Lidar data if active) to disk') argparser.add_argument( '-c', '--carla-settings', metavar='PATH', + dest='settings_filepath', default=None, help='Path to a "CarlaSettings.ini" file') @@ -200,16 +223,13 @@ def main(): logging.info('listening to server %s:%s', args.host, args.port) + args.image_filename_format = '_out/episode_{:0>4d}/{:s}/{:0>6d}.png' + args.lidar_filename_format = '_out/episode_{:0>4d}/{:s}/{:0>6d}.ply' + while True: try: - run_carla_client( - host=args.host, - port=args.port, - autopilot_on=args.autopilot, - save_images_to_disk=args.images_to_disk, - image_filename_format='_images/episode_{:0>3d}/{:s}/image_{:0>5d}.png', - settings_filepath=args.carla_settings) + run_carla_client(args) print('Done.') return diff --git a/PythonClient/manual_control.py b/PythonClient/manual_control.py index 029a011a4a0..0eabdcc1957 100755 --- a/PythonClient/manual_control.py +++ b/PythonClient/manual_control.py @@ -6,7 +6,7 @@ # This work is licensed under the terms of the MIT license. # For a copy, see . -# Keyboard controlling for carla. Please refer to client_example for a simpler +# Keyboard controlling for CARLA. Please refer to client_example.py for a simpler # and more documented example. """ @@ -68,7 +68,7 @@ MINI_WINDOW_HEIGHT = 180 -def make_carla_settings(): +def make_carla_settings(enable_lidar): """Make a CarlaSettings object with the settings we need.""" settings = CarlaSettings() settings.set( @@ -93,6 +93,18 @@ def make_carla_settings(): camera2.set_position(200, 0, 140) camera2.set_rotation(0.0, 0.0, 0.0) settings.add_sensor(camera2) + if enable_lidar: + lidar = sensor.Lidar('Lidar32') + lidar.set_position(0, 0, 250) + lidar.set_rotation(0, 0, 0) + lidar.set( + Channels=32, + Range=5000, + PointsPerSecond=100000, + RotationFrequency=10, + UpperFovLimit=10, + LowerFovLimit=-30) + settings.add_sensor(lidar) return settings @@ -117,13 +129,15 @@ def elapsed_seconds_since_lap(self): class CarlaGame(object): - def __init__(self, carla_client, city_name=None): + def __init__(self, carla_client, enable_lidar=False, city_name=None): self.client = carla_client self._timer = None self._display = None self._main_image = None self._mini_view_image1 = None self._mini_view_image2 = None + self._enable_lidar = enable_lidar + self._lidar_measurement = None self._map_view = None self._is_on_reverse = False self._city_name = city_name @@ -161,7 +175,7 @@ def _initialize_game(self): self._on_new_episode() def _on_new_episode(self): - scene = self.client.load_settings(make_carla_settings()) + scene = self.client.load_settings(make_carla_settings(self._enable_lidar)) number_of_player_starts = len(scene.player_start_spots) player_start = np.random.randint(number_of_player_starts) print('Starting new episode...') @@ -177,6 +191,8 @@ def _on_loop(self): self._main_image = sensor_data['CameraRGB'] self._mini_view_image1 = sensor_data['CameraDepth'] self._mini_view_image2 = sensor_data['CameraSemSeg'] + if self._enable_lidar: + self._lidar_measurement = sensor_data['Lidar32'] # Print measurements every second. if self._timer.elapsed_seconds_since_lap() > 1.0: @@ -207,9 +223,9 @@ def _on_loop(self): # Set the player position if self._city_name is not None: self._position = self._map.convert_to_pixel([ - measurements.player_measurements.transform.location.x, - measurements.player_measurements.transform.location.y, - measurements.player_measurements.transform.location.z]) + measurements.player_measurements.transform.location.x, + measurements.player_measurements.transform.location.y, + measurements.player_measurements.transform.location.z]) self._agent_positions = measurements.non_player_agents if control is None: @@ -246,7 +262,8 @@ def _print_player_measurements_map( map_position, lane_orientation): message = 'Step {step} ({fps:.1f} FPS): ' - message += 'Map Position ({map_x:.1f},{map_y:.1f}) Lane Orientation ({ori_x:.1f},{ori_y:.1f}) ' + message += 'Map Position ({map_x:.1f},{map_y:.1f}) ' + message += 'Lane Orientation ({ori_x:.1f},{ori_y:.1f}) ' message += '{speed:.2f} km/h, ' message += '{other_lane:.0f}% other lane, {offroad:.0f}% off-road' message = message.format( @@ -295,17 +312,33 @@ def _on_render(self): self._display.blit( surface, (2 * gap_x + MINI_WINDOW_WIDTH, mini_image_y)) + if self._lidar_measurement is not None: + lidar_data = np.array(self._lidar_measurement.data[:, :2]) + lidar_data /= 50.0 + lidar_data += 100.0 + lidar_data = np.fabs(lidar_data) + lidar_data = lidar_data.astype(np.int32) + lidar_data = np.reshape(lidar_data, (-1, 2)) + #draw lidar + lidar_img_size = (200, 200, 3) + lidar_img = np.zeros(lidar_img_size) + lidar_img[tuple(lidar_data.T)] = (255, 255, 255) + surface = pygame.surfarray.make_surface(lidar_img) + self._display.blit(surface, (10, 10)) + if self._map_view is not None: array = self._map_view array = array[:, :, :3] - new_window_width =(float(WINDOW_HEIGHT)/float(self._map_shape[0]))*float(self._map_shape[1]) + new_window_width = \ + (float(WINDOW_HEIGHT) / float(self._map_shape[0])) * \ + float(self._map_shape[1]) surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) w_pos = int(self._position[0]*(float(WINDOW_HEIGHT)/float(self._map_shape[0]))) h_pos = int(self._position[1] *(new_window_width/float(self._map_shape[1]))) - pygame.draw.circle(surface, [255, 0, 0, 255], (w_pos,h_pos), 6, 0) + pygame.draw.circle(surface, [255, 0, 0, 255], (w_pos, h_pos), 6, 0) for agent in self._agent_positions: if agent.HasField('vehicle'): agent_position = self._map.convert_to_pixel([ @@ -316,7 +349,7 @@ def _on_render(self): w_pos = int(agent_position[0]*(float(WINDOW_HEIGHT)/float(self._map_shape[0]))) h_pos = int(agent_position[1] *(new_window_width/float(self._map_shape[1]))) - pygame.draw.circle(surface, [255, 0, 255, 255], (w_pos ,h_pos), 4, 0) + pygame.draw.circle(surface, [255, 0, 255, 255], (w_pos, h_pos), 4, 0) self._display.blit(surface, (WINDOW_WIDTH, 0)) @@ -342,11 +375,16 @@ def main(): default=2000, type=int, help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-l', '--lidar', + action='store_true', + help='enable Lidar') argparser.add_argument( '-m', '--map-name', metavar='M', default=None, - help='plot the map of the current city (needs to match active map in server, options: Town01 or Town02)') + help='plot the map of the current city (needs to match active map in ' + 'server, options: Town01 or Town02)') args = argparser.parse_args() log_level = logging.DEBUG if args.debug else logging.INFO @@ -360,7 +398,7 @@ def main(): try: with make_carla_client(args.host, args.port) as client: - game = CarlaGame(client, args.map_name) + game = CarlaGame(client, args.lidar, args.map_name) game.execute() break diff --git a/PythonClient/point_cloud_example.py b/PythonClient/point_cloud_example.py new file mode 100755 index 00000000000..50c53867c15 --- /dev/null +++ b/PythonClient/point_cloud_example.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB), and the INTEL Visual Computing Lab. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +"""Basic CARLA client to generate point cloud in PLY format that you + can visualize with MeshLab (meshlab.net) for instance. Please + refer to client_example.py for a simpler and more documented example.""" + +from __future__ import print_function + +import argparse +import logging +import os +import random +import time + +from carla.client import make_carla_client +from carla.sensor import Camera +from carla.settings import CarlaSettings +from carla.tcp import TCPConnectionError +from carla.util import print_over_same_line, StopWatch +from carla.image_converter import depth_to_local_point_cloud, to_rgb_array +from carla.transform import Transform + + +def run_carla_client(host, port, far): + # Here we will run a single episode with 300 frames. + number_of_frames = 3000 + frame_step = 100 # Save one image every 100 frames + output_folder = '_out' + image_size = [800, 600] + camera_local_pos = [30, 0, 130] # [X, Y, Z] + camera_local_rotation = [0, 0, 0] # [pitch(Y), yaw(Z), roll(X)] + fov = 70 + + # Connect with the server + with make_carla_client(host, port) as client: + print('CarlaClient connected') + + # Here we load the settings. + settings = CarlaSettings() + settings.set( + SynchronousMode=True, + SendNonPlayerAgentsInfo=False, + NumberOfVehicles=20, + NumberOfPedestrians=40, + WeatherId=random.choice([1, 3, 7, 8, 14])) + settings.randomize_seeds() + + camera1 = Camera('CameraDepth', PostProcessing='Depth', FOV=fov) + camera1.set_image_size(*image_size) + camera1.set_position(*camera_local_pos) + camera1.set_rotation(*camera_local_rotation) + settings.add_sensor(camera1) + + camera2 = Camera('CameraRGB', PostProcessing='SceneFinal', FOV=fov) + camera2.set_image_size(*image_size) + camera2.set_position(*camera_local_pos) + camera2.set_rotation(*camera_local_rotation) + settings.add_sensor(camera2) + + client.load_settings(settings) + + # Start at location index id '0' + client.start_episode(0) + + # Compute the camera transform matrix + camera_to_car_transform = camera2.get_unreal_transform() + + # Iterate every frame in the episode except for the first one. + for frame in range(1, number_of_frames): + # Read the data produced by the server this frame. + measurements, sensor_data = client.read_data() + + # Save one image every 'frame_step' frames + if not frame % frame_step: + # Start transformations time mesure. + timer = StopWatch() + + # RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]] + image_RGB = to_rgb_array(sensor_data['CameraRGB']) + + # 2d to (camera) local 3d + # We use the image_RGB to colorize each 3D point, this is optional. + # "max_depth" is used to keep only the points that are near to the + # camera, meaning 1.0 the farest points (sky) + point_cloud = depth_to_local_point_cloud( + sensor_data['CameraDepth'], + image_RGB, + max_depth=far + ) + + # (Camera) local 3d to world 3d. + # Get the transform from the player protobuf transformation. + world_transform = Transform( + measurements.player_measurements.transform + ) + + # Compute the final transformation matrix. + car_to_world_transform = world_transform * camera_to_car_transform + + # Car to World transformation given the 3D points and the + # transformation matrix. + point_cloud.apply_transform(car_to_world_transform) + + # End transformations time mesure. + timer.stop() + + # Save PLY to disk + # This generates the PLY string with the 3D points and the RGB colors + # for each row of the file. + point_cloud.save_to_disk(os.path.join( + output_folder, '{:0>5}.ply'.format(frame)) + ) + + print_message(timer.milliseconds(), len(point_cloud), frame) + + client.send_control( + measurements.player_measurements.autopilot_control + ) + + +def print_message(elapsed_time, point_n, frame): + message = ' '.join([ + 'Transformations took {:>3.0f} ms.', + 'Saved {:>6} points to "{:0>5}.ply".' + ]).format(elapsed_time, point_n, frame) + print_over_same_line(message) + + +def check_far(value): + fvalue = float(value) + if fvalue < 0.0 or fvalue > 1.0: + raise argparse.ArgumentTypeError( + "{} must be a float between 0.0 and 1.0") + return fvalue + + +def main(): + argparser = argparse.ArgumentParser(description=__doc__) + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='localhost', + help='IP of the host server (default: localhost)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-f', '--far', + default=0.2, + type=check_far, + help='The maximum save distance of camera-point ' + '[0.0 (near), 1.0 (far)] (default: 0.2)') + + args = argparser.parse_args() + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + while True: + try: + run_carla_client(host=args.host, port=args.port, far=args.far) + print('\nDone!') + return + + except TCPConnectionError as error: + logging.error(error) + time.sleep(1) + + +if __name__ == '__main__': + + try: + main() + except KeyboardInterrupt: + print('\nClient stoped by user.') diff --git a/PythonClient/test/unit_tests/Basic.py b/PythonClient/test/unit_tests/Basic.py index 9197c124197..ab1d1630e3a 100644 --- a/PythonClient/test/unit_tests/Basic.py +++ b/PythonClient/test/unit_tests/Basic.py @@ -13,6 +13,7 @@ from carla.client import CarlaClient from carla.sensor import Camera, Image +from carla.sensor import Lidar, LidarMeasurement from carla.settings import CarlaSettings from carla.util import make_connection @@ -45,8 +46,8 @@ def run_carla_client(self, carla_settings, number_of_episodes, number_of_frames, number_of_agents = len(measurements.non_player_agents) logging.debug('received data of %d agents', number_of_agents) logging.debug('received %d images', len(images)) - if len(images) != len(carla_settings._cameras): - raise RuntimeError('received %d images, expected %d' % (len(images), len(carla_settings._cameras))) + if len(sensor_data) != len(carla_settings._sensors): + raise RuntimeError('received %d, expected %d' % (len(sensor_data), len(carla_settings._sensors))) logging.debug('sending control...') control = measurements.player_measurements.autopilot_control if not use_autopilot_control: @@ -80,7 +81,7 @@ def run(self): settings = CarlaSettings() settings.add_sensor(Camera('DefaultCamera')) camera2 = Camera('Camera2') - camera2.set(PostProcessing='Depth', CameraFOV=120) + camera2.set(PostProcessing='Depth', FOV=120) camera2.set_image_size(1924, 1028) settings.add_sensor(camera2) self.run_carla_client(settings, 3, 100) @@ -110,3 +111,10 @@ def run(self): settings = CarlaSettings() settings.add_sensor(Camera('DefaultCamera')) self.run_carla_client(settings, 1, 2000, use_autopilot_control=True) + + +class LidarTest(_BasicTestBase): + def run(self): + settings = CarlaSettings() + settings.add_sensor(Lidar('DefaultLidar')) + self.run_carla_client(settings, 3, 100) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/TrafficSignBase.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/TrafficSignBase.cpp deleted file mode 100644 index 59e954be413..00000000000 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/TrafficSignBase.cpp +++ /dev/null @@ -1,22 +0,0 @@ -// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#include "Carla.h" -#include "TrafficSignBase.h" - -#include "Game/CarlaGameState.h" - -ATrafficSignBase::ATrafficSignBase() : Super() {} - -void ATrafficSignBase::BeginPlay() -{ - auto *GameState = GetWorld()->GetGameState(); - if (GameState != nullptr) { - GameState->RegisterTrafficSign(this); - } else { - UE_LOG(LogCarla, Error, TEXT("Missing CARLA game state!")); - } -} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/AgentComponent.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/AgentComponent.cpp new file mode 100644 index 00000000000..3cef8518e19 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/AgentComponent.cpp @@ -0,0 +1,38 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Carla.h" +#include "AgentComponent.h" + +#include "Game/CarlaGameModeBase.h" +#include "Game/DataRouter.h" + +static FDataRouter &GetDataRouter(UWorld *World) +{ + check(World != nullptr); + auto *GameMode = Cast(World->GetAuthGameMode()); + check(GameMode != nullptr); + return GameMode->GetDataRouter(); +} + +void UAgentComponent::AcceptVisitor(IAgentComponentVisitor &Visitor) const +{ + unimplemented(); +} + +void UAgentComponent::BeginPlay() +{ + Super::BeginPlay(); + GetDataRouter(GetWorld()).RegisterAgent(this); +} + +void UAgentComponent::EndPlay(const EEndPlayReason::Type EndPlayReason) +{ + GetDataRouter(GetWorld()).DeregisterAgent(this); + Super::EndPlay(EndPlayReason); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/AgentComponent.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/AgentComponent.h new file mode 100644 index 00000000000..a272af4d3f0 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/AgentComponent.h @@ -0,0 +1,37 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Agent/AgentComponentVisitor.h" + +#include "Components/SceneComponent.h" +#include "Templates/SharedPointer.h" + +#include "AgentComponent.generated.h" + +/// Actors with an UAgentComponent are registered as agents in the scene and +/// their status is sent to the client each frame (if requested by the client). +UCLASS(Abstract) +class CARLA_API UAgentComponent : public USceneComponent +{ + GENERATED_BODY() + +public: + + uint32 GetId() const + { + return GetTypeHash(this); + } + + virtual void AcceptVisitor(IAgentComponentVisitor &Visitor) const; + +protected: + + virtual void BeginPlay() override; + + virtual void EndPlay(EEndPlayReason::Type EndPlayReason) override; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/AgentComponentVisitor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/AgentComponentVisitor.h new file mode 100644 index 00000000000..9229e717010 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/AgentComponentVisitor.h @@ -0,0 +1,23 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +class UAgentComponent; +class UTrafficSignAgentComponent; +class UVehicleAgentComponent; +class UWalkerAgentComponent; + +class IAgentComponentVisitor +{ +public: + + virtual void Visit(const UTrafficSignAgentComponent &) = 0; + + virtual void Visit(const UVehicleAgentComponent &) = 0; + + virtual void Visit(const UWalkerAgentComponent &) = 0; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/TrafficSignAgentComponent.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/TrafficSignAgentComponent.cpp new file mode 100644 index 00000000000..e3aaa63a546 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/TrafficSignAgentComponent.cpp @@ -0,0 +1,23 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Carla.h" +#include "TrafficSignAgentComponent.h" + +#include "Traffic/TrafficSignBase.h" + +UTrafficSignAgentComponent::UTrafficSignAgentComponent(const FObjectInitializer &ObjectInitializer) + : Super(ObjectInitializer) {} + +void UTrafficSignAgentComponent::BeginPlay() +{ + TrafficSign = Cast(GetOwner()); + checkf(TrafficSign != nullptr, TEXT("UTrafficSignAgentComponent can only be attached to ATrafficSignBase")); + + Super::BeginPlay(); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/TrafficSignAgentComponent.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/TrafficSignAgentComponent.h new file mode 100644 index 00000000000..752ebee6f39 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/TrafficSignAgentComponent.h @@ -0,0 +1,43 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Agent/AgentComponent.h" + +#include "TrafficSignAgentComponent.generated.h" + +class ATrafficSignBase; + +UCLASS() +class CARLA_API UTrafficSignAgentComponent : public UAgentComponent +{ + GENERATED_BODY() + +public: + + UTrafficSignAgentComponent(const FObjectInitializer &ObjectInitializer); + + const ATrafficSignBase &GetTrafficSign() const + { + check(TrafficSign != nullptr); + return *TrafficSign; + } + +protected: + + virtual void BeginPlay() override; + + virtual void AcceptVisitor(IAgentComponentVisitor &Visitor) const final + { + Visitor.Visit(*this); + } + +private: + + UPROPERTY() + ATrafficSignBase *TrafficSign = nullptr; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/VehicleAgentComponent.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/VehicleAgentComponent.cpp new file mode 100644 index 00000000000..be045161c1e --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/VehicleAgentComponent.cpp @@ -0,0 +1,23 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Carla.h" +#include "VehicleAgentComponent.h" + +#include "Vehicle/CarlaWheeledVehicle.h" + +UVehicleAgentComponent::UVehicleAgentComponent(const FObjectInitializer &ObjectInitializer) + : Super(ObjectInitializer) {} + +void UVehicleAgentComponent::BeginPlay() +{ + WheeledVehicle = Cast(GetOwner()); + checkf(WheeledVehicle != nullptr, TEXT("UVehicleAgentComponent can only be attached to ACarlaWheeledVehicle")); + + Super::BeginPlay(); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/VehicleAgentComponent.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/VehicleAgentComponent.h new file mode 100644 index 00000000000..b15a878b13f --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/VehicleAgentComponent.h @@ -0,0 +1,42 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Agent/AgentComponent.h" + +#include "VehicleAgentComponent.generated.h" + +class ACarlaWheeledVehicle; + +UCLASS() +class CARLA_API UVehicleAgentComponent : public UAgentComponent +{ + GENERATED_BODY() + +public: + + UVehicleAgentComponent(const FObjectInitializer &ObjectInitializer); + + ACarlaWheeledVehicle &GetVehicle() const + { + check(WheeledVehicle != nullptr); + return *WheeledVehicle; + } + +protected: + + virtual void BeginPlay() override; + + virtual void AcceptVisitor(IAgentComponentVisitor &Visitor) const final + { + Visitor.Visit(*this); + } + +private: + + ACarlaWheeledVehicle *WheeledVehicle = nullptr; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/WalkerAgentComponent.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/WalkerAgentComponent.cpp new file mode 100644 index 00000000000..5a918eb296c --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/WalkerAgentComponent.cpp @@ -0,0 +1,29 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Carla.h" +#include "WalkerAgentComponent.h" + +#include "GameFramework/Character.h" + +UWalkerAgentComponent::UWalkerAgentComponent(const FObjectInitializer &ObjectInitializer) + : Super(ObjectInitializer) {} + +float UWalkerAgentComponent::GetForwardSpeed() const +{ + /// @todo Is it necessary to compute this speed every tick? + return FVector::DotProduct(Walker->GetVelocity(), Walker->GetActorRotation().Vector()) * 0.036f; +} + +void UWalkerAgentComponent::BeginPlay() +{ + Walker = Cast(GetOwner()); + checkf(Walker != nullptr, TEXT("UWalkerAgentComponent can only be attached to ACharacter")); + + Super::BeginPlay(); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/WalkerAgentComponent.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/WalkerAgentComponent.h new file mode 100644 index 00000000000..ab822803c0c --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/WalkerAgentComponent.h @@ -0,0 +1,48 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Agent/AgentComponent.h" + +#include "WalkerAgentComponent.generated.h" + +class ACharacter; + +/// This component can be added to any ACharacter to be added as agent. +/// See UAgentComponent. +UCLASS() +class CARLA_API UWalkerAgentComponent : public UAgentComponent +{ + GENERATED_BODY() + +public: + + UWalkerAgentComponent(const FObjectInitializer &ObjectInitializer); + + /// Return forward speed in km/h. + float GetForwardSpeed() const; + + FVector GetBoundingBoxExtent() const + { + /// @todo Perhaps the box it is not the same for every walker... + return {45.0f, 35.0f, 100.0f}; + } + +protected: + + virtual void BeginPlay() override; + + virtual void AcceptVisitor(IAgentComponentVisitor &Visitor) const final + { + Visitor.Visit(*this); + } + +private: + + UPROPERTY() + ACharacter *Walker = nullptr; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.h index 5e9097bbfec..e684e6faaf0 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.h @@ -14,7 +14,7 @@ DECLARE_LOG_CATEGORY_EXTERN(LogCarlaServer, Log, All); // Options to compile with extra debug log. #if WITH_EDITOR // #define CARLA_AI_VEHICLES_EXTRA_LOG -// #define CARLA_AI_WALKERS_EXTRA_LOG +#define CARLA_AI_WALKERS_EXTRA_LOG // #define CARLA_ROAD_GENERATOR_EXTRA_LOG // #define CARLA_SERVER_EXTRA_LOG // #define CARLA_TAGGER_EXTRA_LOG diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CityMapGenerator.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CityMapGenerator.cpp index 6b9748036f7..d4cb13f85c1 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CityMapGenerator.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CityMapGenerator.cpp @@ -9,7 +9,7 @@ #include "MapGen/GraphGenerator.h" #include "MapGen/RoadMap.h" -#include "Tagger.h" +#include "Game/Tagger.h" #include "Components/InstancedStaticMeshComponent.h" #include "Engine/World.h" diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CityMapGenerator.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CityMapGenerator.h index 814d50d21a1..0d0a6e86a9c 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CityMapGenerator.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CityMapGenerator.h @@ -7,8 +7,10 @@ #pragma once #include "MapGen/CityMapMeshHolder.h" + #include "MapGen/DoublyConnectedEdgeList.h" #include "MapGen/GraphParser.h" + #include "CityMapGenerator.generated.h" class URoadMap; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/DynamicWeather.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/DynamicWeather.cpp index b5c93b435ab..15c0348d6ff 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/DynamicWeather.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/DynamicWeather.cpp @@ -46,7 +46,7 @@ void ADynamicWeather::LoadWeatherDescriptionsFromFile( FString DefaultFilePath; if (GetWeatherIniFilePath(GetIniFileName(), DefaultFilePath)) { UE_LOG(LogCarla, Log, TEXT("Loading weather description from %s"), *DefaultFilePath); - IniFile ConfigFile(DefaultFilePath); + FIniFile ConfigFile(DefaultFilePath); { // Override map specific presets. FString MapOverridesFilePath; @@ -190,7 +190,7 @@ bool ADynamicWeather::LoadFromConfigFile() { FString FilePath; if (GetWeatherIniFilePath(FileName, FilePath) && CheckWeatherValidity(Weather)) { - IniFile ConfigFile(FilePath); + FIniFile ConfigFile(FilePath); if (!ConfigFile.HasSection(Weather.Name)) { UE_LOG(LogCarla, Error, TEXT("Weather \"%s\" is not present in config file"), *Weather.Name); return false; @@ -206,7 +206,7 @@ bool ADynamicWeather::SaveToConfigFile() const { FString FilePath; if (GetWeatherIniFilePath(FileName, FilePath) && CheckWeatherValidity(Weather)) { - IniFile ConfigFile(FilePath); + FIniFile ConfigFile(FilePath); Weather.WriteToConfigFile(ConfigFile); return ConfigFile.Write(FilePath); } else { diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/DynamicWeather.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/DynamicWeather.h index 83608c76d69..c28cf13c718 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/DynamicWeather.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/DynamicWeather.h @@ -7,7 +7,9 @@ #pragma once #include "GameFramework/Actor.h" + #include "Settings/WeatherDescription.h" + #include "DynamicWeather.generated.h" class UArrowComponent; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CapturedImage.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CapturedImage.h deleted file mode 100644 index 8ecc4b033a6..00000000000 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CapturedImage.h +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#pragma once - -#include "Settings/PostProcessEffect.h" -#include "CapturedImage.generated.h" - -/// Bitmap and meta info of a scene capture. -/// -/// The bitmap may be empty if the capture failed. -USTRUCT() -struct FCapturedImage -{ - GENERATED_USTRUCT_BODY() - - UPROPERTY(VisibleAnywhere) - uint32 SizeX = 0u; - - UPROPERTY(VisibleAnywhere) - uint32 SizeY = 0u; - - UPROPERTY(VisibleAnywhere) - EPostProcessEffect PostProcessEffect = EPostProcessEffect::INVALID; - - UPROPERTY(VisibleAnywhere) - TArray BitMap; -}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameController.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameController.h deleted file mode 100644 index e54849f608c..00000000000 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameController.h +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#pragma once - -#include "CarlaGameControllerBase.h" - -class ACarlaGameState; -class ACarlaVehicleController; -class CarlaServer; - -/// Implements remote control of game and player. -class CARLA_API CarlaGameController : public CarlaGameControllerBase -{ -public: - - explicit CarlaGameController(); - - ~CarlaGameController(); - - virtual void Initialize(UCarlaSettings &CarlaSettings) override; - - virtual APlayerStart *ChoosePlayerStart(const TArray &AvailableStartSpots) override; - - virtual void RegisterPlayer(AController &NewPlayer) override; - - virtual void BeginPlay() override; - - virtual void Tick(float DeltaSeconds) override; - -private: - - void RestartLevel(); - - TUniquePtr Server; - - ACarlaVehicleController *Player = nullptr; - - const ACarlaGameState *GameState = nullptr; - - UCarlaSettings *CarlaSettings = nullptr; -}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameControllerBase.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameControllerBase.h index cb895ca6f87..50d4c9c2b80 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameControllerBase.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameControllerBase.h @@ -6,18 +6,21 @@ #pragma once -#include "Array.h" +#include "Containers/Array.h" class AController; class APlayerStart; +class FDataRouter; class UCarlaSettings; /// Base class for a CARLA game controller. -class CARLA_API CarlaGameControllerBase +class ICarlaGameControllerBase { public: - virtual ~CarlaGameControllerBase() {} + ICarlaGameControllerBase(FDataRouter &DataRouter) : DataRouter(DataRouter) {} + + virtual ~ICarlaGameControllerBase() {} virtual void Initialize(UCarlaSettings &CarlaSettings) = 0; @@ -28,4 +31,8 @@ class CARLA_API CarlaGameControllerBase virtual void BeginPlay() = 0; virtual void Tick(float DeltaSeconds) = 0; + +protected: + + FDataRouter &DataRouter; }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameInstance.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameInstance.cpp index 55ae20cafa7..467c11e84a6 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameInstance.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameInstance.cpp @@ -7,8 +7,8 @@ #include "Carla.h" #include "CarlaGameInstance.h" -#include "CarlaGameController.h" -#include "MockGameController.h" +#include "Game/MockGameController.h" +#include "Server/ServerGameController.h" #include "Settings/CarlaSettings.h" UCarlaGameInstance::UCarlaGameInstance() { @@ -25,9 +25,9 @@ void UCarlaGameInstance::InitializeGameControllerIfNotPresent( { if (GameController == nullptr) { if (CarlaSettings->bUseNetworking) { - GameController = MakeUnique(); + GameController = MakeUnique(DataRouter); } else { - GameController = MakeUnique(MockControllerSettings); + GameController = MakeUnique(DataRouter, MockControllerSettings); UE_LOG(LogCarla, Log, TEXT("Using mock CARLA controller")); } } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameInstance.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameInstance.h index 896f5af7f3d..e138c395db5 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameInstance.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameInstance.h @@ -7,7 +7,10 @@ #pragma once #include "Engine/GameInstance.h" -#include "CarlaGameControllerBase.h" + +#include "Game/CarlaGameControllerBase.h" +#include "Game/DataRouter.h" + #include "CarlaGameInstance.generated.h" class UCarlaSettings; @@ -29,7 +32,7 @@ class CARLA_API UCarlaGameInstance : public UGameInstance void InitializeGameControllerIfNotPresent( const FMockGameControllerSettings &MockControllerSettings); - CarlaGameControllerBase &GetGameController() + ICarlaGameControllerBase &GetGameController() { check(GameController != nullptr); return *GameController; @@ -54,10 +57,17 @@ class CARLA_API UCarlaGameInstance : public UGameInstance return CarlaSettings; } + FDataRouter &GetDataRouter() + { + return DataRouter; + } + private: UPROPERTY(Category = "CARLA Settings", EditAnywhere) UCarlaSettings *CarlaSettings; - TUniquePtr GameController; + FDataRouter DataRouter; + + TUniquePtr GameController; }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp index 7afbb80bea5..5efd1be6393 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp @@ -7,21 +7,23 @@ #include "Carla.h" #include "CarlaGameModeBase.h" +#include "Game/CarlaGameInstance.h" +#include "Game/CarlaHUD.h" +#include "Game/CarlaPlayerState.h" +#include "Game/Tagger.h" +#include "Game/TaggerDelegate.h" +#include "Sensor/Sensor.h" +#include "Sensor/SensorFactory.h" +#include "Settings/CarlaSettings.h" +#include "Util/RandomEngine.h" +#include "Vehicle/CarlaVehicleController.h" + #include "ConstructorHelpers.h" #include "Engine/PlayerStartPIE.h" #include "EngineUtils.h" #include "GameFramework/PlayerStart.h" #include "SceneViewport.h" -#include "CarlaGameInstance.h" -#include "CarlaGameState.h" -#include "CarlaHUD.h" -#include "CarlaPlayerState.h" -#include "CarlaVehicleController.h" -#include "Settings/CarlaSettings.h" -#include "Tagger.h" -#include "TaggerDelegate.h" - ACarlaGameModeBase::ACarlaGameModeBase(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer), GameController(nullptr), @@ -32,7 +34,6 @@ ACarlaGameModeBase::ACarlaGameModeBase(const FObjectInitializer& ObjectInitializ bAllowTickBeforeBeginPlay = false; PlayerControllerClass = ACarlaVehicleController::StaticClass(); - GameStateClass = ACarlaGameState::StaticClass(); PlayerStateClass = ACarlaPlayerState::StaticClass(); HUDClass = ACarlaHUD::StaticClass(); @@ -129,17 +130,6 @@ void ACarlaGameModeBase::BeginPlay() { Super::BeginPlay(); - auto CarlaGameState = Cast(GameState); - checkf( - CarlaGameState != nullptr, - TEXT("GameState is not a ACarlaGameState, did you forget to set it in the project settings?")); - if (WalkerSpawner != nullptr) { - CarlaGameState->WalkerSpawner = WalkerSpawner; - } - if (VehicleSpawner != nullptr) { - CarlaGameState->VehicleSpawner = VehicleSpawner; - } - const auto &CarlaSettings = GameInstance->GetCarlaSettings(); // Setup semantic segmentation if necessary. @@ -204,17 +194,19 @@ void ACarlaGameModeBase::RegisterPlayer(AController &NewPlayer) { check(GameController != nullptr); AddTickPrerequisiteActor(&NewPlayer); - GameController->RegisterPlayer(NewPlayer); PlayerController = Cast(&NewPlayer); - AttachCaptureCamerasToPlayer(); + if (PlayerController != nullptr) { + GetDataRouter().RegisterPlayer(*PlayerController); + GameController->RegisterPlayer(*PlayerController); + AttachSensorsToPlayer(); + } else { + UE_LOG(LogCarla, Error, TEXT("ACarlaGameModeBase: Player is not a ACarlaVehicleController")); + } } -void ACarlaGameModeBase::AttachCaptureCamerasToPlayer() +void ACarlaGameModeBase::AttachSensorsToPlayer() { - if (PlayerController == nullptr) { - UE_LOG(LogCarla, Warning, TEXT("Trying to add capture cameras but player is not a ACarlaVehicleController")); - return; - } + check(PlayerController != nullptr); const auto &Settings = GameInstance->GetCarlaSettings(); const auto *Weather = Settings.GetActiveWeatherDescription(); const FCameraPostProcessParameters *OverridePostProcessParameters = nullptr; @@ -222,8 +214,11 @@ void ACarlaGameModeBase::AttachCaptureCamerasToPlayer() OverridePostProcessParameters = &Weather->CameraPostProcessParameters; } - for (const auto &Item : Settings.CameraDescriptions) { - PlayerController->AddSceneCaptureCamera(Item.Value, OverridePostProcessParameters); + for (const auto &Item : Settings.SensorDescriptions) { + auto *Sensor = FSensorFactory::Make(*Item.Value, *GetWorld()); + check(Sensor != nullptr); + Sensor->AttachToActor(PlayerController->GetPawn()); + GetDataRouter().RegisterSensor(*Sensor); } } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.h index 9dc4247a1dc..b5c5f7c1fdd 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.h @@ -7,21 +7,22 @@ #pragma once #include "GameFramework/GameModeBase.h" -#include "AI/VehicleSpawnerBase.h" -#include "AI/WalkerSpawnerBase.h" -#include "CarlaGameControllerBase.h" + #include "DynamicWeather.h" -#include "MockGameControllerSettings.h" +#include "Game/CarlaGameControllerBase.h" +#include "Game/CarlaGameInstance.h" +#include "Game/MockGameControllerSettings.h" +#include "Vehicle/VehicleSpawnerBase.h" +#include "Walker/WalkerSpawnerBase.h" + #include "CarlaGameModeBase.generated.h" class ACarlaVehicleController; class APlayerStart; class ASceneCaptureCamera; +class UCarlaGameInstance; class UTaggerDelegate; -/** - * - */ UCLASS(HideCategories=(ActorTick)) class CARLA_API ACarlaGameModeBase : public AGameModeBase { @@ -39,6 +40,12 @@ class CARLA_API ACarlaGameModeBase : public AGameModeBase virtual void Tick(float DeltaSeconds) override; + FDataRouter &GetDataRouter() + { + check(GameInstance != nullptr); + return GameInstance->GetDataRouter(); + } + protected: /** Used only when networking is disabled. */ @@ -61,7 +68,7 @@ class CARLA_API ACarlaGameModeBase : public AGameModeBase void RegisterPlayer(AController &NewPlayer); - void AttachCaptureCamerasToPlayer(); + void AttachSensorsToPlayer(); void TagActorsForSemanticSegmentation(); @@ -73,7 +80,7 @@ class CARLA_API ACarlaGameModeBase : public AGameModeBase AController *Player, TArray &UnOccupiedStartPoints); - CarlaGameControllerBase *GameController; + ICarlaGameControllerBase *GameController; UPROPERTY() UCarlaGameInstance *GameInstance; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameState.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameState.h deleted file mode 100644 index b4c008be082..00000000000 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameState.h +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#pragma once - -#include "GameFramework/GameStateBase.h" -#include "AI/TrafficSignBase.h" -#include "AI/VehicleSpawnerBase.h" -#include "AI/WalkerSpawnerBase.h" -#include "CarlaGameState.generated.h" - -UCLASS() -class CARLA_API ACarlaGameState : public AGameStateBase -{ - GENERATED_BODY() - -public: - - const AVehicleSpawnerBase *GetVehicleSpawner() const - { - return VehicleSpawner; - } - - const AWalkerSpawnerBase *GetWalkerSpawner() const - { - return WalkerSpawner; - } - - const TArray &GetTrafficSigns() const - { - return TrafficSigns; - } - - void RegisterTrafficSign(ATrafficSignBase *TrafficSign) - { - TrafficSigns.Add(TrafficSign); - } - -private: - - friend class ACarlaGameModeBase; - - UPROPERTY() - AVehicleSpawnerBase *VehicleSpawner = nullptr; - - UPROPERTY() - AWalkerSpawnerBase *WalkerSpawner = nullptr; - - UPROPERTY() - TArray TrafficSigns; -}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaHUD.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaHUD.cpp index 8b2753db452..b2484286d5d 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaHUD.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaHUD.cpp @@ -7,7 +7,8 @@ #include "Carla.h" #include "CarlaHUD.h" -#include "CarlaVehicleController.h" +#include "Vehicle/CarlaVehicleController.h" + #include "CommandLine.h" #include "ConstructorHelpers.h" #include "Engine/Canvas.h" diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaHUD.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaHUD.h index 761babefb5e..3b5c932e5fb 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaHUD.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaHUD.h @@ -12,7 +12,7 @@ class FTexture; UCLASS() -class ACarlaHUD : public AHUD +class CARLA_API ACarlaHUD : public AHUD { GENERATED_BODY() diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaPlayerState.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaPlayerState.cpp index e72561aca9d..03eb98d08db 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaPlayerState.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaPlayerState.cpp @@ -15,7 +15,6 @@ void ACarlaPlayerState::Reset() CollisionIntensityCars = 0.0f; CollisionIntensityPedestrians = 0.0f; CollisionIntensityOther = 0.0f; - Images.Empty(); } void ACarlaPlayerState::CopyProperties(APlayerState *PlayerState) @@ -44,7 +43,6 @@ void ACarlaPlayerState::CopyProperties(APlayerState *PlayerState) CollisionIntensityOther = Other->CollisionIntensityOther; OtherLaneIntersectionFactor = Other->OtherLaneIntersectionFactor; OffRoadIntersectionFactor = Other->OffRoadIntersectionFactor; - Images = Other->Images; UE_LOG(LogCarla, Log, TEXT("Copied properties of ACarlaPlayerState")); } } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaPlayerState.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaPlayerState.h index fddbf0325ed..20cc2925084 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaPlayerState.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaPlayerState.h @@ -7,8 +7,9 @@ #pragma once #include "GameFramework/PlayerState.h" -#include "AI/TrafficLightState.h" -#include "CapturedImage.h" + +#include "Traffic/TrafficLightState.h" + #include "CarlaPlayerState.generated.h" /// Current state of the player, updated every frame by ACarlaVehicleController. @@ -183,29 +184,6 @@ class CARLA_API ACarlaPlayerState : public APlayerState return OffRoadIntersectionFactor; } - /// @} - // =========================================================================== - /// @name Images - // =========================================================================== - /// @{ - - UFUNCTION(BlueprintCallable) - int32 GetNumberOfImages() const - { - return Images.Num(); - } - - UFUNCTION(BlueprintCallable) - bool HasImages() const - { - return GetNumberOfImages() > 0; - } - - const TArray &GetImages() const - { - return Images; - } - /// @} // =========================================================================== // -- Modifiers -------------------------------------------------------------- @@ -283,7 +261,4 @@ class CARLA_API ACarlaPlayerState : public APlayerState UPROPERTY(VisibleAnywhere) float OffRoadIntersectionFactor = 0.0f; - - UPROPERTY(VisibleAnywhere) - TArray Images; }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaServer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaServer.cpp deleted file mode 100644 index a82cdf4eaca..00000000000 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaServer.cpp +++ /dev/null @@ -1,350 +0,0 @@ -// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#include "Carla.h" -#include "CarlaServer.h" - -#include "GameFramework/PlayerStart.h" - -#include "CarlaPlayerState.h" -#include "CarlaVehicleController.h" -#include "CarlaWheeledVehicle.h" -#include "SceneCaptureCamera.h" -#include "Settings/CarlaSettings.h" - -#include - -// ============================================================================= -// -- Static local methods ----------------------------------------------------- -// ============================================================================= - -static CarlaServer::ErrorCode ParseErrorCode(const uint32 ErrorCode) -{ - if (ErrorCode == CARLA_SERVER_SUCCESS) { - return CarlaServer::Success; - } else if (ErrorCode == CARLA_SERVER_TRY_AGAIN) { - return CarlaServer::TryAgain; - } else { - return CarlaServer::Error; - } -} - -static int32 GetTimeOut(uint32 TimeOut, const bool bBlocking) -{ - return (bBlocking ? TimeOut : 0u); -} - -// ============================================================================= -// -- Set functions ------------------------------------------------------------ -// ============================================================================= - -static inline void Set(bool &lhs, bool rhs) -{ - lhs = rhs; -} - -static inline void Set(float &lhs, float rhs) -{ - lhs = rhs; -} - -static inline void Set(carla_vector3d &lhs, const FVector &rhs) -{ - lhs = {rhs.X, rhs.Y, rhs.Z}; -} - -static inline void Set(carla_rotation3d &lhs, const FRotator &rhs) -{ - lhs.pitch = rhs.Pitch; - lhs.roll = rhs.Roll; - lhs.yaw = rhs.Yaw; -} - -static inline void Set(carla_transform &lhs, const FTransform &rhs) -{ - Set(lhs.location, rhs.GetLocation()); - Set(lhs.orientation, rhs.GetRotation().GetForwardVector()); - Set(lhs.rotation, rhs.Rotator()); -} - -static void Set(carla_image &cImage, const FCapturedImage &uImage) -{ - if (uImage.BitMap.Num() > 0) { - cImage.width = uImage.SizeX; - cImage.height = uImage.SizeY; - cImage.type = PostProcessEffect::ToUInt(uImage.PostProcessEffect); - cImage.data = &uImage.BitMap.GetData()->DWColor(); - -#ifdef CARLA_SERVER_EXTRA_LOG - { - const auto Size = uImage.BitMap.Num(); - UE_LOG(LogCarlaServer, Log, TEXT("Sending image %dx%d (%d) type %d"), cImage.width, cImage.height, Size, cImage.type); - } - } else { - UE_LOG(LogCarlaServer, Warning, TEXT("Sending empty image")); -#endif // CARLA_SERVER_EXTRA_LOG - } -} - -static void SetBoxSpeedAndType(carla_agent &values, const ACharacter *Walker) -{ - values.type = CARLA_SERVER_AGENT_PEDESTRIAN; - values.forward_speed = FVector::DotProduct(Walker->GetVelocity(), Walker->GetActorRotation().Vector()) * 0.036f; - /// @todo Perhaps the box it is not the same for every walker... - values.box_extent = {45.0f, 35.0f, 100.0f}; -} - -static void SetBoxSpeedAndType(carla_agent &values, const ACarlaWheeledVehicle *Vehicle) -{ - values.type = CARLA_SERVER_AGENT_VEHICLE; - values.forward_speed = Vehicle->GetVehicleForwardSpeed(); - Set(values.box_extent, Vehicle->GetVehicleBoundsExtent()); -} - -static void SetBoxSpeedAndType(carla_agent &values, const ATrafficSignBase *TrafficSign) -{ - switch (TrafficSign->GetTrafficSignState()) { - case ETrafficSignState::TrafficLightRed: - values.type = CARLA_SERVER_AGENT_TRAFFICLIGHT_RED; - break; - case ETrafficSignState::TrafficLightYellow: - values.type = CARLA_SERVER_AGENT_TRAFFICLIGHT_YELLOW; - break; - case ETrafficSignState::TrafficLightGreen: - values.type = CARLA_SERVER_AGENT_TRAFFICLIGHT_GREEN; - break; - case ETrafficSignState::SpeedLimit_30: - values.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; - values.forward_speed = 30.0f; - break; - case ETrafficSignState::SpeedLimit_40: - values.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; - values.forward_speed = 40.0f; - break; - case ETrafficSignState::SpeedLimit_50: - values.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; - values.forward_speed = 50.0f; - break; - case ETrafficSignState::SpeedLimit_60: - values.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; - values.forward_speed = 60.0f; - break; - case ETrafficSignState::SpeedLimit_90: - values.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; - values.forward_speed = 90.0f; - break; - case ETrafficSignState::SpeedLimit_100: - values.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; - values.forward_speed = 100.0f; - break; - case ETrafficSignState::SpeedLimit_120: - values.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; - values.forward_speed = 120.0f; - break; - case ETrafficSignState::SpeedLimit_130: - values.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; - values.forward_speed = 130.0f; - break; - default: - UE_LOG(LogCarla, Error, TEXT("Unknown traffic sign!")); - } -} - -// ============================================================================= -// -- CarlaServer -------------------------------------------------------------- -// ============================================================================= - -CarlaServer::CarlaServer(const uint32 InWorldPort, const uint32 InTimeOut) : - WorldPort(InWorldPort), - TimeOut(InTimeOut), - Server(carla_make_server()) { - check(Server != nullptr); -} - -CarlaServer::~CarlaServer() -{ -#ifdef CARLA_SERVER_EXTRA_LOG - UE_LOG(LogCarlaServer, Warning, TEXT("Destroying CarlaServer")); -#endif // CARLA_SERVER_EXTRA_LOG - carla_free_server(Server); -} - -CarlaServer::ErrorCode CarlaServer::Connect() -{ - UE_LOG(LogCarlaServer, Log, TEXT("Waiting for the client to connect...")); - return ParseErrorCode(carla_server_connect(Server, WorldPort, TimeOut)); -} - -CarlaServer::ErrorCode CarlaServer::ReadNewEpisode(UCarlaSettings &Settings, const bool bBlocking) -{ - carla_request_new_episode values; - auto ec = ParseErrorCode(carla_read_request_new_episode(Server, values, GetTimeOut(TimeOut, bBlocking))); - if (Success == ec) { - auto IniFile = FString(values.ini_file_length, ANSI_TO_TCHAR(values.ini_file)); - UE_LOG(LogCarlaServer, Log, TEXT("Received new episode")); -#ifdef CARLA_SERVER_EXTRA_LOG - UE_LOG(LogCarlaServer, Log, TEXT("Received CarlaSettings.ini:\n%s"), *IniFile); -#endif // CARLA_SERVER_EXTRA_LOG - Settings.LoadSettingsFromString(IniFile); - } - return ec; -} - -CarlaServer::ErrorCode CarlaServer::SendSceneDescription( - const TArray &AvailableStartSpots, - const bool bBlocking) -{ - const int32 NumberOfStartSpots = AvailableStartSpots.Num(); - auto StartSpots = MakeUnique(NumberOfStartSpots); - - for (auto i = 0; i < NumberOfStartSpots; ++i) { - Set(StartSpots[i], AvailableStartSpots[i]->GetActorTransform()); - } - - UE_LOG(LogCarlaServer, Log, TEXT("Sending %d available start positions"), NumberOfStartSpots); - carla_scene_description scene; - scene.player_start_spots = StartSpots.Get(); - scene.number_of_player_start_spots = NumberOfStartSpots; - - return ParseErrorCode(carla_write_scene_description(Server, scene, GetTimeOut(TimeOut, bBlocking))); -} - -CarlaServer::ErrorCode CarlaServer::ReadEpisodeStart(uint32 &StartPositionIndex, const bool bBlocking) -{ - carla_episode_start values; - auto ec = ParseErrorCode(carla_read_episode_start(Server, values, GetTimeOut(TimeOut, bBlocking))); - if (Success == ec) { - StartPositionIndex = values.player_start_spot_index; - UE_LOG(LogCarlaServer, Log, TEXT("Episode start received: { StartIndex = %d }"), StartPositionIndex); - } - return ec; -} - -CarlaServer::ErrorCode CarlaServer::SendEpisodeReady(const bool bBlocking) -{ - UE_LOG(LogCarlaServer, Log, TEXT("Ready to play, notifying client")); - const carla_episode_ready values = {true}; - return ParseErrorCode(carla_write_episode_ready(Server, values, GetTimeOut(TimeOut, bBlocking))); -} - -CarlaServer::ErrorCode CarlaServer::ReadControl(ACarlaVehicleController &Player, const bool bBlocking) -{ - carla_control values; - auto ec = ParseErrorCode(carla_read_control(Server, values, GetTimeOut(TimeOut, bBlocking))); - if (Success == ec) { - check(Player.IsPossessingAVehicle()); - auto Vehicle = Player.GetPossessedVehicle(); - Vehicle->SetSteeringInput(values.steer); - Vehicle->SetThrottleInput(values.throttle); - Vehicle->SetBrakeInput(values.brake); - Vehicle->SetHandbrakeInput(values.hand_brake); - Vehicle->SetReverse(values.reverse); -#ifdef CARLA_SERVER_EXTRA_LOG - UE_LOG( - LogCarlaServer, - Log, - TEXT("Read control (%s): { Steer = %f, Throttle = %f, Brake = %f, Handbrake = %s, Reverse = %s }"), - (bBlocking ? TEXT("Sync") : TEXT("Async")), - values.steer, - values.throttle, - values.brake, - (values.hand_brake ? TEXT("True") : TEXT("False")), - (values.reverse ? TEXT("True") : TEXT("False"))); -#endif // CARLA_SERVER_EXTRA_LOG - } else if ((!bBlocking) && (TryAgain == ec)) { - UE_LOG(LogCarlaServer, Warning, TEXT("No control received from the client this frame!")); - } - return ec; -} - -template -static void AddAgents(TArray &Agents, const TArray &Actors) -{ - for (auto &&Actor : Actors) { - if (Actor != nullptr) { - Agents.Emplace(); - auto &values = Agents.Last(); - values.id = GetTypeHash(Actor); - Set(values.transform, Actor->GetActorTransform()); - SetBoxSpeedAndType(values, Actor); - } - } -} - -static void GetAgentInfo( - const ACarlaGameState &GameState, - TArray &Agents) -{ - const auto *WalkerSpawner = GameState.GetWalkerSpawner(); - const auto *VehicleSpawner = GameState.GetVehicleSpawner(); - const auto &TrafficSigns = GameState.GetTrafficSigns(); - - auto NumberOfAgents = TrafficSigns.Num(); - if (WalkerSpawner != nullptr) { - NumberOfAgents += WalkerSpawner->GetCurrentNumberOfWalkers(); - } - if (VehicleSpawner != nullptr) { - NumberOfAgents += VehicleSpawner->GetNumberOfSpawnedVehicles(); - } - Agents.Reserve(NumberOfAgents); - - AddAgents(Agents, TrafficSigns); - if (WalkerSpawner != nullptr) { - AddAgents(Agents, WalkerSpawner->GetWalkersWhiteList()); - AddAgents(Agents, WalkerSpawner->GetWalkersBlackList()); - } - if (VehicleSpawner != nullptr) { - AddAgents(Agents, VehicleSpawner->GetVehicles()); - } -} - -CarlaServer::ErrorCode CarlaServer::SendMeasurements( - const ACarlaGameState &GameState, - const ACarlaPlayerState &PlayerState, - const bool bSendNonPlayerAgentsInfo) -{ - // Measurements. - carla_measurements values; - values.platform_timestamp = PlayerState.GetPlatformTimeStamp(); - values.game_timestamp = PlayerState.GetGameTimeStamp(); - auto &player = values.player_measurements; - Set(player.transform, PlayerState.GetTransform()); - Set(player.acceleration, PlayerState.GetAcceleration()); - Set(player.forward_speed, PlayerState.GetForwardSpeed()); - Set(player.collision_vehicles, PlayerState.GetCollisionIntensityCars()); - Set(player.collision_pedestrians, PlayerState.GetCollisionIntensityPedestrians()); - Set(player.collision_other, PlayerState.GetCollisionIntensityOther()); - Set(player.intersection_otherlane, PlayerState.GetOtherLaneIntersectionFactor()); - Set(player.intersection_offroad, PlayerState.GetOffRoadIntersectionFactor()); - Set(player.autopilot_control.steer, PlayerState.GetSteer()); - Set(player.autopilot_control.throttle, PlayerState.GetThrottle()); - Set(player.autopilot_control.brake, PlayerState.GetBrake()); - Set(player.autopilot_control.hand_brake, PlayerState.GetHandBrake()); - Set(player.autopilot_control.reverse, PlayerState.GetCurrentGear() < 0); - - TArray Agents; - if (bSendNonPlayerAgentsInfo) { - GetAgentInfo(GameState, Agents); - } - values.non_player_agents = (Agents.Num() > 0 ? Agents.GetData() : nullptr); - values.number_of_non_player_agents = Agents.Num(); - -#ifdef CARLA_SERVER_EXTRA_LOG - UE_LOG(LogCarlaServer, Log, TEXT("Sending data of %d agents"), values.number_of_non_player_agents); -#endif // CARLA_SERVER_EXTRA_LOG - - // Images. - const auto NumberOfImages = PlayerState.GetNumberOfImages(); - TUniquePtr images; - if (NumberOfImages > 0) { - images = MakeUnique(NumberOfImages); - for (auto i = 0; i < NumberOfImages; ++i) { - Set(images[i], PlayerState.GetImages()[i]); - } - } - - return ParseErrorCode(carla_write_measurements(Server, values, images.Get(), NumberOfImages)); -} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/DataRouter.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/DataRouter.cpp new file mode 100644 index 00000000000..4724b340952 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/DataRouter.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "Carla.h" +#include "DataRouter.h" + +#include "Sensor/Sensor.h" + +void FDataRouter::RegisterPlayer(ACarlaVehicleController &InPlayer) +{ + if (Player == nullptr) { + Player = &InPlayer; + } else { + UE_LOG( + LogCarla, + Error, + TEXT("FDataRouter: Trying to register a second player but only one is supported")); + } +} + +void FDataRouter::RegisterSensor(ASensor &InSensor) +{ + if (SensorDataSink.IsValid()) { + InSensor.SetSensorDataSink(SensorDataSink); + } else { + UE_LOG( + LogCarla, + Error, + TEXT("FDataRouter: Trying to register a sensor but I don't have a SensorDataSink")); + } +} + +void FDataRouter::RestartLevel() +{ + if (Player != nullptr) { + Player->RestartLevel(); + Player = nullptr; + } else { + UE_LOG( + LogCarla, + Error, + TEXT("FDataRouter: Trying to restart level but I don't have any player registered")); + } +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/DataRouter.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/DataRouter.h new file mode 100644 index 00000000000..0491e9ca49e --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/DataRouter.h @@ -0,0 +1,73 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Util/NonCopyable.h" + +#include "Sensor/SensorDataSink.h" + +#include "Vehicle/CarlaVehicleController.h" +#include "Vehicle/CarlaWheeledVehicle.h" + +class ACarlaPlayerState; +class ASensor; +class UAgentComponent; +struct FVehicleControl; + +class FDataRouter : private NonCopyable +{ +public: + + void SetSensorDataSink(TSharedPtr InSensorDataSink) + { + check(!SensorDataSink.IsValid()); + SensorDataSink = InSensorDataSink; + } + + void RegisterPlayer(ACarlaVehicleController &InPlayer); + + void RegisterSensor(ASensor &InSensor); + + void RegisterAgent(const UAgentComponent *Agent) + { + check(Agent != nullptr); + Agents.Emplace(Agent); + } + + void DeregisterAgent(const UAgentComponent *Agent) + { + check(Agent != nullptr); + Agents.RemoveSwap(Agent); + } + + const ACarlaPlayerState &GetPlayerState() const + { + check(Player != nullptr); + return Player->GetPlayerState(); + } + + const TArray &GetAgents() const + { + return Agents; + } + + void ApplyVehicleControl(const FVehicleControl &VehicleControl) + { + check((Player != nullptr) && (Player->IsPossessingAVehicle())); + Player->GetPossessedVehicle()->ApplyVehicleControl(VehicleControl); + } + + void RestartLevel(); + +private: + + TArray Agents; + + ACarlaVehicleController *Player = nullptr; + + TSharedPtr SensorDataSink = nullptr; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/MockGameController.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/MockGameController.cpp index c5735275860..9d2a14f692a 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/MockGameController.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/MockGameController.cpp @@ -7,8 +7,22 @@ #include "Carla.h" #include "MockGameController.h" -MockGameController::MockGameController(const FMockGameControllerSettings &InSettings) : - Settings(InSettings) {} +#include "Game/DataRouter.h" +#include "Sensor/SensorDataSink.h" + +class FMockSensorDataSink : public ISensorDataSink { +public: + + virtual void Write(const FSensorDataView &) final {} +}; + +MockGameController::MockGameController( + FDataRouter &InDataRouter, + const FMockGameControllerSettings &InSettings) + : ICarlaGameControllerBase(InDataRouter), + Settings(InSettings) { + DataRouter.SetSensorDataSink(MakeShared()); +} void MockGameController::Initialize(UCarlaSettings &CarlaSettings) { diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/MockGameController.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/MockGameController.h index ff97121414d..ce4060ec27d 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/MockGameController.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/MockGameController.h @@ -6,25 +6,26 @@ #pragma once -#include "CarlaGameControllerBase.h" -#include "MockGameControllerSettings.h" +#include "Game/CarlaGameControllerBase.h" + +#include "Game/MockGameControllerSettings.h" /// Mocks the CARLA game controller class for testing purposes. -class CARLA_API MockGameController : public CarlaGameControllerBase +class MockGameController : public ICarlaGameControllerBase { public: - explicit MockGameController(const FMockGameControllerSettings &Settings); + explicit MockGameController(FDataRouter &DataRouter, const FMockGameControllerSettings &Settings); - virtual void Initialize(UCarlaSettings &CarlaSettings) override; + virtual void Initialize(UCarlaSettings &CarlaSettings) final; - virtual APlayerStart *ChoosePlayerStart(const TArray &AvailableStartSpots) override; + virtual APlayerStart *ChoosePlayerStart(const TArray &AvailableStartSpots) final; - virtual void RegisterPlayer(AController &NewPlayer) override; + virtual void RegisterPlayer(AController &NewPlayer) final; - virtual void BeginPlay() override; + virtual void BeginPlay() final; - virtual void Tick(float DeltaSeconds) override; + virtual void Tick(float DeltaSeconds) final; private: diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/MockGameControllerSettings.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/MockGameControllerSettings.h index a32033f3995..fceeb074bac 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/MockGameControllerSettings.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/MockGameControllerSettings.h @@ -9,7 +9,7 @@ #include "MockGameControllerSettings.generated.h" USTRUCT(BlueprintType) -struct FMockGameControllerSettings +struct CARLA_API FMockGameControllerSettings { GENERATED_USTRUCT_BODY() diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Tagger.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp similarity index 99% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Tagger.cpp rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp index 9cf36179b0f..f2b1eef91fb 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Tagger.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp @@ -6,11 +6,12 @@ #include "Carla.h" #include "Tagger.h" -#include "EngineUtils.h" -#include "Engine/StaticMesh.h" -#include "Engine/SkeletalMesh.h" -#include "Components/StaticMeshComponent.h" + #include "Components/SkeletalMeshComponent.h" +#include "Components/StaticMeshComponent.h" +#include "Engine/SkeletalMesh.h" +#include "Engine/StaticMesh.h" +#include "EngineUtils.h" #include "PhysicsEngine/PhysicsAsset.h" #ifdef CARLA_TAGGER_EXTRA_LOG diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Tagger.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.h similarity index 100% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Tagger.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.h diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/TaggerDelegate.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/TaggerDelegate.cpp index 97c0c44afaf..bfbc906f34b 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/TaggerDelegate.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/TaggerDelegate.cpp @@ -7,8 +7,9 @@ #include "Carla.h" #include "TaggerDelegate.h" +#include "Game/Tagger.h" + #include "Engine/World.h" -#include "Tagger.h" UTaggerDelegate::UTaggerDelegate() : ActorSpawnedDelegate(FOnActorSpawned::FDelegate::CreateUObject(this, &UTaggerDelegate::OnActorSpawned)) {} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/CityMapMeshTag.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/CityMapMeshTag.h index 32354d2d7ec..d0d6ea937b4 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/CityMapMeshTag.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/CityMapMeshTag.h @@ -73,7 +73,7 @@ enum class ECityMapMeshTag : uint8 }; /// Helper class for working with ECityMapMeshTag. -class CARLA_API CityMapMeshTag +class CityMapMeshTag { public: diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/GraphGenerator.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/GraphGenerator.h index b36c32dacc9..755cee26759 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/GraphGenerator.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/GraphGenerator.h @@ -11,7 +11,7 @@ namespace MapGen { /// Random DoublyConnectedEdgeList generator. - class CARLA_API GraphGenerator : private NonCopyable + class GraphGenerator : private NonCopyable { public: diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/GraphParser.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/GraphParser.h index e60405f0ed8..2f2bf5ee168 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/GraphParser.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/GraphParser.h @@ -16,7 +16,7 @@ namespace MapGen { class DoublyConnectedEdgeList; - class CARLA_API GraphParser : private NonCopyable + class GraphParser : private NonCopyable { public: diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/Position.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/Position.h index d7e04244360..b72321e9a4a 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/Position.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/Position.h @@ -11,7 +11,7 @@ namespace MapGen { template - class CARLA_API Position { + class Position { public: using number_type = T; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/RoadMap.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/RoadMap.h index ceb9974b78c..a543db91938 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/RoadMap.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/RoadMap.h @@ -12,7 +12,7 @@ /// Road map intersection result. See URoadMap. USTRUCT(BlueprintType) -struct FRoadMapIntersectionResult +struct CARLA_API FRoadMapIntersectionResult { GENERATED_BODY() diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/RoadSegmentDescription.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/RoadSegmentDescription.h index de8afa1a208..009fd91e506 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/RoadSegmentDescription.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/RoadSegmentDescription.h @@ -13,7 +13,7 @@ namespace MapGen { - class CARLA_API RoadSegmentDescription : private NonCopyable + class RoadSegmentDescription : private NonCopyable { public: diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/RoadSegment.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/RoadSegment.h index 53a6e044aed..c47e9fe35c3 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/RoadSegment.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/RoadSegment.h @@ -19,7 +19,7 @@ enum class ELaneMarkingType : uint8 /// Description of a road segment piece. USTRUCT(BlueprintType) -struct FRoadSegmentPiece +struct CARLA_API FRoadSegmentPiece { GENERATED_BODY() diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.cpp new file mode 100644 index 00000000000..0cf7633ad86 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.cpp @@ -0,0 +1,144 @@ +// Fill out your copyright notice in the Description page of Project Settings. + +#include "Carla.h" +#include "Lidar.h" + +#include "DrawDebugHelpers.h" +#include "Engine/CollisionProfile.h" +#include "Runtime/Engine/Classes/Kismet/KismetMathLibrary.h" +#include "StaticMeshResources.h" + +ALidar::ALidar(const FObjectInitializer& ObjectInitializer) + : Super(ObjectInitializer) +{ + PrimaryActorTick.bCanEverTick = true; + + auto MeshComp = CreateDefaultSubobject(TEXT("CamMesh0")); + MeshComp->SetCollisionProfileName(UCollisionProfile::NoCollision_ProfileName); + MeshComp->bHiddenInGame = true; + MeshComp->CastShadow = false; + MeshComp->PostPhysicsComponentTick.bCanEverTick = false; + RootComponent = MeshComp; +} + +void ALidar::Set(const ULidarDescription &LidarDescription) +{ + Super::Set(LidarDescription); + Description = &LidarDescription; + LidarMeasurement = FLidarMeasurement(GetId(), Description->Channels); + CreateLasers(); +} + +void ALidar::CreateLasers() +{ + check(Description != nullptr); + const auto NumberOfLasers = Description->Channels; + check(NumberOfLasers > 0u); + const float DeltaAngle = + (Description->UpperFovLimit - Description->LowerFovLimit) / + static_cast(NumberOfLasers - 1); + LaserAngles.Empty(NumberOfLasers); + for(auto i = 0u; i < NumberOfLasers; ++i) + { + const float VerticalAngle = + Description->UpperFovLimit - static_cast(i) * DeltaAngle; + LaserAngles.Emplace(VerticalAngle); + } +} + +void ALidar::Tick(const float DeltaTime) +{ + Super::Tick(DeltaTime); + + ReadPoints(DeltaTime); + WriteSensorData(LidarMeasurement.GetView()); +} + +void ALidar::ReadPoints(const float DeltaTime) +{ + const uint32 ChannelCount = Description->Channels; + const uint32 PointsToScanWithOneLaser = + FMath::RoundHalfFromZero( + Description->PointsPerSecond * DeltaTime / float(ChannelCount)); + check(PointsToScanWithOneLaser > 0); + check(ChannelCount == LaserAngles.Num()); + check(Description != nullptr); + + const float CurrentHorizontalAngle = LidarMeasurement.GetHorizontalAngle(); + const float AngleDistanceOfTick = Description->RotationFrequency * 360.0f * DeltaTime; + const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser; + + LidarMeasurement.Reset(ChannelCount * PointsToScanWithOneLaser); + + for (auto Channel = 0u; Channel < ChannelCount; ++Channel) + { + for (auto i = 0u; i < PointsToScanWithOneLaser; ++i) + { + FVector Point; + const float Angle = CurrentHorizontalAngle + AngleDistanceOfLaserMeasure * i; + if (ShootLaser(Channel, Angle, Point)) + { + LidarMeasurement.WritePoint(Channel, Point); + } + } + } + + const float HorizontalAngle = std::fmod(CurrentHorizontalAngle + AngleDistanceOfTick, 360.0f); + LidarMeasurement.SetHorizontalAngle(HorizontalAngle); +} + +bool ALidar::ShootLaser(const uint32 Channel, const float HorizontalAngle, FVector &XYZ) const +{ + const float VerticalAngle = LaserAngles[Channel]; + + FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this); + TraceParams.bTraceComplex = true; + TraceParams.bReturnPhysicalMaterial = false; + + FHitResult HitInfo(ForceInit); + + FVector LidarBodyLoc = GetActorLocation(); + FRotator LidarBodyRot = GetActorRotation(); + FRotator LaserRot (VerticalAngle, HorizontalAngle, 0); // float InPitch, float InYaw, float InRoll + FRotator ResultRot = UKismetMathLibrary::ComposeRotators( + LaserRot, + LidarBodyRot + ); + const auto Range = Description->Range; + FVector EndTrace = Range * UKismetMathLibrary::GetForwardVector(ResultRot) + LidarBodyLoc; + + GetWorld()->LineTraceSingleByChannel( + HitInfo, + LidarBodyLoc, + EndTrace, + ECC_MAX, + TraceParams, + FCollisionResponseParams::DefaultResponseParam + ); + + if (HitInfo.bBlockingHit) + { + if (Description->ShowDebugPoints) + { + DrawDebugPoint( + GetWorld(), + HitInfo.ImpactPoint, + 10, //size + FColor(255,0,255), + false, //persistent (never goes away) + 0.1 //point leaves a trail on moving object + ); + } + + XYZ = LidarBodyLoc - HitInfo.ImpactPoint; + XYZ = UKismetMathLibrary::RotateAngleAxis( + XYZ, + - LidarBodyRot.Yaw + 90, + FVector(0, 0, 1) + ); + + return true; + } else { + return false; + } +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.h new file mode 100644 index 00000000000..01519456cef --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.h @@ -0,0 +1,45 @@ +// Fill out your copyright notice in the Description page of Project Settings. + +#pragma once + +#include "Sensor/Sensor.h" + +#include "Sensor/LidarMeasurement.h" +#include "Settings/LidarDescription.h" + +#include "Lidar.generated.h" + +/// A ray-trace based Lidar sensor. +UCLASS() +class CARLA_API ALidar : public ASensor +{ + GENERATED_BODY() + +public: + + ALidar(const FObjectInitializer &ObjectInitializer); + + void Set(const ULidarDescription &LidarDescription); + +protected: + + virtual void Tick(float DeltaTime) override; + +private: + + /// Creates a Laser for each channel. + void CreateLasers(); + + /// Updates LidarMeasurement with the points read in DeltaTime. + void ReadPoints(float DeltaTime); + + /// Shoot a laser ray-trace, return whether the laser hit something. + bool ShootLaser(uint32 Channel, float HorizontalAngle, FVector &Point) const; + + UPROPERTY(Category = "LiDAR", VisibleAnywhere) + const ULidarDescription *Description; + + TArray LaserAngles; + + FLidarMeasurement LidarMeasurement; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarMeasurement.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarMeasurement.h new file mode 100644 index 00000000000..d1299f77fc7 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarMeasurement.h @@ -0,0 +1,95 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Sensor/SensorDataView.h" + +#include "Containers/Array.h" + +/// Stores the data generated by ALidar. To be used by ALidar solely. +/// +/// The header consists of an array of uint32's in the following layout +/// +/// { +/// Horizontal angle (float), +/// Channel count, +/// Point count of channel 0, +/// ... +/// Point count of channel n, +/// } +/// +/// The points are stored in an array of floats +/// +/// { +/// X0, Y0, Z0, +/// ... +/// Xn, Yn, Zn, +/// } +/// +class FLidarMeasurement { + static_assert(sizeof(float) == sizeof(uint32), "Invalid float size"); +public: + + explicit FLidarMeasurement(uint32 SensorId = 0u, uint32 ChannelCount = 0u) + : SensorId(SensorId) + { + Header.AddDefaulted(2u + ChannelCount); + Header[1] = ChannelCount; + } + + FLidarMeasurement &operator=(FLidarMeasurement &&Other) + { + SensorId = Other.SensorId; + Header = std::move(Other.Header); + Points = std::move(Other.Points); + Other.SensorId = 0u; + return *this; + } + + float GetHorizontalAngle() const + { + return reinterpret_cast(Header[0]); + } + + void SetHorizontalAngle(float HorizontalAngle) + { + Header[0] = reinterpret_cast(HorizontalAngle); + } + + uint32 GetChannelCount() const + { + return Header[1]; + } + + void Reset(uint32 TotalPointCount) + { + std::memset(Header.GetData() + 2u, 0, sizeof(uint32) * GetChannelCount()); + Points.Reset(3u * TotalPointCount); + } + + void WritePoint(uint32 Channel, const FVector &Point) + { + check(Header[1] > Channel); + Header[2u + Channel] += 1u; + Points.Emplace(Point.X); + Points.Emplace(Point.Y); + Points.Emplace(Point.Z); + } + + FSensorDataView GetView() const + { + return FSensorDataView(SensorId, Header, Points); + } + +private: + + uint32 SensorId; + + TArray Header; + + TArray Points; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/ReadOnlyBufferView.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/ReadOnlyBufferView.h new file mode 100644 index 00000000000..37a640b0e57 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/ReadOnlyBufferView.h @@ -0,0 +1,45 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Containers/Array.h" + +/// A view over a read-only buffer. Does not own the data. +class FReadOnlyBufferView { +public: + + FReadOnlyBufferView() = default; + + explicit FReadOnlyBufferView(const void *InData, uint32 InSize) + : Data(InData), + Size(InSize) {} + + template + FReadOnlyBufferView(const TArray &Array) + : FReadOnlyBufferView(Array.GetData(), sizeof(T) * Array.Num()) {} + + bool HasData() const + { + return (Data != nullptr) && (Size > 0u); + } + + const void *GetData() const + { + return Data; + } + + uint32 GetSize() const + { + return Size; + } + +private: + + const void *Data = nullptr; + + uint32 Size = 0u; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/SceneCaptureCamera.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureCamera.cpp similarity index 83% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/SceneCaptureCamera.cpp rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureCamera.cpp index 439f54fb069..690434d6111 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/SceneCaptureCamera.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureCamera.cpp @@ -7,6 +7,8 @@ #include "Carla.h" #include "SceneCaptureCamera.h" +#include "Sensor/SensorDataView.h" + #include "Components/DrawFrustumComponent.h" #include "Components/SceneCaptureComponent2D.h" #include "Components/StaticMeshComponent.h" @@ -38,7 +40,7 @@ ASceneCaptureCamera::ASceneCaptureCamera(const FObjectInitializer& ObjectInitial SizeY(512u), PostProcessEffect(EPostProcessEffect::SceneFinal) { - PrimaryActorTick.bCanEverTick = true; /// @todo Does it need to tick? + PrimaryActorTick.bCanEverTick = true; PrimaryActorTick.TickGroup = TG_PrePhysics; MeshComp = CreateDefaultSubobject(TEXT("CamMesh0")); @@ -137,6 +139,25 @@ void ASceneCaptureCamera::BeginPlay() Super::BeginPlay(); } +void ASceneCaptureCamera::Tick(const float DeltaSeconds) +{ + Super::Tick(DeltaSeconds); + auto fn = [=](){WritePixels(DeltaSeconds);}; + ENQUEUE_UNIQUE_RENDER_COMMAND_ONEPARAMETER( + FWritePixels, + decltype(fn),write_function,fn, + { + write_function(); + }); + +} + +float ASceneCaptureCamera::GetFOVAngle() const +{ + check(CaptureComponent2D != nullptr); + return CaptureComponent2D->FOVAngle; +} + void ASceneCaptureCamera::SetImageSize(uint32 otherSizeX, uint32 otherSizeY) { SizeX = otherSizeX; @@ -167,29 +188,27 @@ void ASceneCaptureCamera::SetTargetGamma(const float TargetGamma) CaptureRenderTarget->TargetGamma = TargetGamma; } -void ASceneCaptureCamera::Set(const FCameraDescription &CameraDescription) +void ASceneCaptureCamera::Set(const UCameraDescription &CameraDescription) { + /// @todo What to do with OverridePostProcessParameters? + // if (CameraDescription.OverridePostProcessParameters != nullptr) { + // auto Override = *CameraDescription.OverridePostProcessParameters; + // auto &PostProcessSettings = CaptureComponent2D->PostProcessSettings; + // PostProcessSettings.bOverride_AutoExposureMethod = true; + // PostProcessSettings.AutoExposureMethod = Override.AutoExposureMethod; + // PostProcessSettings.bOverride_AutoExposureMinBrightness = true; + // PostProcessSettings.AutoExposureMinBrightness = Override.AutoExposureMinBrightness; + // PostProcessSettings.bOverride_AutoExposureMaxBrightness = true; + // PostProcessSettings.AutoExposureMaxBrightness = Override.AutoExposureMaxBrightness; + // PostProcessSettings.bOverride_AutoExposureBias = true; + // PostProcessSettings.AutoExposureBias = Override.AutoExposureBias; + // } + Super::Set(CameraDescription); SetImageSize(CameraDescription.ImageSizeX, CameraDescription.ImageSizeY); SetPostProcessEffect(CameraDescription.PostProcessEffect); SetFOVAngle(CameraDescription.FOVAngle); } -void ASceneCaptureCamera::Set( - const FCameraDescription &CameraDescription, - const FCameraPostProcessParameters &OverridePostProcessParameters) -{ - auto &PostProcessSettings = CaptureComponent2D->PostProcessSettings; - PostProcessSettings.bOverride_AutoExposureMethod = true; - PostProcessSettings.AutoExposureMethod = OverridePostProcessParameters.AutoExposureMethod; - PostProcessSettings.bOverride_AutoExposureMinBrightness = true; - PostProcessSettings.AutoExposureMinBrightness = OverridePostProcessParameters.AutoExposureMinBrightness; - PostProcessSettings.bOverride_AutoExposureMaxBrightness = true; - PostProcessSettings.AutoExposureMaxBrightness = OverridePostProcessParameters.AutoExposureMaxBrightness; - PostProcessSettings.bOverride_AutoExposureBias = true; - PostProcessSettings.AutoExposureBias = OverridePostProcessParameters.AutoExposureBias; - Set(CameraDescription); -} - bool ASceneCaptureCamera::ReadPixels(TArray &BitMap) const { if(!CaptureRenderTarget) @@ -197,7 +216,7 @@ bool ASceneCaptureCamera::ReadPixels(TArray &BitMap) const UE_LOG(LogCarla, Error, TEXT("SceneCaptureCamera: Missing render target")); return false; } - FTextureRenderTargetResource* RTResource = CaptureRenderTarget->GetRenderTargetResource(); // CaptureRenderTarget->GameThread_GetRenderTargetResource(); + FTextureRenderTargetResource* RTResource = CaptureRenderTarget->GameThread_GetRenderTargetResource(); if (RTResource == nullptr) { UE_LOG(LogCarla, Error, TEXT("SceneCaptureCamera: Missing render target")); return false; @@ -207,7 +226,7 @@ bool ASceneCaptureCamera::ReadPixels(TArray &BitMap) const return RTResource->ReadPixels(BitMap, ReadPixelFlags); } -void ASceneCaptureCamera::WritePixels(TArray& RawData) +void ASceneCaptureCamera::WritePixels(float DeltaTime) const { check(IsInRenderingThread()); if(!CaptureRenderTarget) @@ -216,22 +235,39 @@ void ASceneCaptureCamera::WritePixels(TArray& RawData) return ; } FRHITexture2D *texture = CaptureRenderTarget->GetRenderTargetResource()->GetRenderTargetTexture(); - - uint32 width = texture->GetSizeX(); + if(!texture) + { + UE_LOG(LogCarla, Error, TEXT("SceneCaptureCamera: Missing render texture")); + return ; + } + const uint32 width = texture->GetSizeX(); uint32 height = texture->GetSizeY(); uint32 stride; uint8 *src = reinterpret_cast(RHILockTexture2D(texture, 0, RLM_ReadOnly, stride, false)); + struct { + uint32 Width; + uint32 Height; + uint32 Type; + float FOV; + } ImageHeader = { + SizeX, + SizeY, + PostProcessEffect::ToUInt(PostProcessEffect), + CaptureComponent2D->FOVAngle + }; + FSensorDataView DataView( + GetId(), + FReadOnlyBufferView{reinterpret_cast(&ImageHeader), sizeof(ImageHeader)}, + FReadOnlyBufferView{src,stride*height} + ); + + WriteSensorData(DataView); + + /* + * example of buffer manual copy const uint32 bufferSize = stride * height; - //Alternative Method 0: deeplearning - /*void *buffer = FMemory::Malloc(bufferSize, 4); - if(!buffer) - { - UE_LOG(LogCarla, Display, TEXT("Error allocating memory")); - RHIUnlockTexture2D(texture, 0, false); - return ; - } - //FMemory::BigBlockMemcpy(buffer, src, bufferSize);*/ RawData.Empty(width*height); + FMemory::BigBlockMemcpy(captureBuffer->getBuffer(), src, captureBuffer->getBufferSize()); for(unsigned y = 0; y < height; ++y) { for(unsigned x = 0; x < width; ++x) @@ -240,38 +276,10 @@ void ASceneCaptureCamera::WritePixels(TArray& RawData) RawData.Add(FColor(src[2],src[1],src[0],src[3])); src += 4; } - } + }*/ RHIUnlockTexture2D(texture, 0, false); - //Alternative Method 1 - /*FColor* RHIData = reinterpret_cast(RHILockTexture2D(TextureRHI, 0, RLM_ReadOnly, stride, false, false)); - if(!RHIData) { - UE_LOG(LogCarla,Display,TEXT("ASceneCaptureCamera::WritePixels : RHIData is null")); - return; - } - RawData.Append(RHIData,Width*Height);*/ - //Alternative Method 2 - /*uint8* OriginBuffer = reinterpret_cast(RHILockTexture2D(TextureRHI, 0, RLM_ReadOnly, stride, false,false)); - if(OriginBuffer) - { - for (int32 y = 0; y < Height; y++) - { - uint8* OriginPtr = &OriginBuffer[(Height - 1 - y) * stride]; - for (int32 x = 0; x < Width; x++) - { - RawData[(Height - 1 - y) * Width] = FColor(*OriginPtr++,*OriginPtr++,*OriginPtr++,*OriginPtr++); - } - } - } - */ - //Alternative Method 3 - /* - FRHITexture2D* Texture2D = TextureRHI->GetTexture2D(); - uint8* TextureBuffer = (uint8*)RHILockTexture2D(Texture2D, 0, RLM_ReadOnly, stride, false, false); - FMemory::Memcpy( RawData.GetData(), TextureBuffer, stride + (Width*Height*4) ); - */ - //End of Alternative methods lock - //RHIUnlockTexture2D(TextureRHI, 0, false, false); + } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/SceneCaptureCamera.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureCamera.h similarity index 87% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/SceneCaptureCamera.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureCamera.h index d88dca7659b..3fb87e633d3 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/SceneCaptureCamera.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureCamera.h @@ -6,9 +6,12 @@ #pragma once -#include "GameFramework/Actor.h" -#include "StaticMeshResources.h" +#include "Sensor/Sensor.h" + #include "Settings/CameraDescription.h" + +#include "StaticMeshResources.h" + #include "SceneCaptureCamera.generated.h" class UDrawFrustumComponent; @@ -19,7 +22,7 @@ class UTextureRenderTarget2D; /// Own SceneCapture, re-implementing some of the methods since ASceneCapture /// cannot be subclassed. UCLASS(hidecategories=(Collision, Attachment, Actor)) -class CARLA_API ASceneCaptureCamera : public AActor +class CARLA_API ASceneCaptureCamera : public ASensor { GENERATED_BODY() @@ -35,6 +38,8 @@ class CARLA_API ASceneCaptureCamera : public AActor virtual void BeginPlay() override; + virtual void Tick(float DeltaSeconds) override; + uint32 GetImageSizeX() const { return SizeX; @@ -50,6 +55,8 @@ class CARLA_API ASceneCaptureCamera : public AActor return PostProcessEffect; } + float GetFOVAngle() const; + void SetImageSize(uint32 SizeX, uint32 SizeY); void SetPostProcessEffect(EPostProcessEffect PostProcessEffect); @@ -58,16 +65,14 @@ class CARLA_API ASceneCaptureCamera : public AActor void SetTargetGamma(float TargetGamma); - void Set(const FCameraDescription &CameraDescription); - - void Set( - const FCameraDescription &CameraDescription, - const FCameraPostProcessParameters &OverridePostProcessParameters); + void Set(const UCameraDescription &CameraDescription); bool ReadPixels(TArray &BitMap) const; - void WritePixels(TArray& RawData); + private: + ///Read the camera buffer and write it to a color array + void WritePixels(float DeltaTime) const; /// Used to synchronize the DrawFrustumComponent with the /// SceneCaptureComponent2D settings. diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/SceneCaptureToDiskCamera.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureToDiskCamera.cpp similarity index 100% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/SceneCaptureToDiskCamera.cpp rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureToDiskCamera.cpp diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/SceneCaptureToDiskCamera.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureToDiskCamera.h similarity index 96% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/SceneCaptureToDiskCamera.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureToDiskCamera.h index 38698478b27..6dc8329e510 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/SceneCaptureToDiskCamera.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureToDiskCamera.h @@ -6,7 +6,8 @@ #pragma once -#include "SceneCaptureCamera.h" +#include "Sensor/SceneCaptureCamera.h" + #include "SceneCaptureToDiskCamera.generated.h" UCLASS(Blueprintable,BlueprintType) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Sensor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Sensor.cpp new file mode 100644 index 00000000000..9ff56e0ba10 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Sensor.cpp @@ -0,0 +1,20 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "Carla.h" +#include "Sensor.h" + +ASensor::ASensor(const FObjectInitializer& ObjectInitializer) + : Super(ObjectInitializer), + Id(0u) {} + +void ASensor::AttachToActor(AActor *Actor) +{ + check(Actor != nullptr); + Super::AttachToActor(Actor, FAttachmentTransformRules::KeepRelativeTransform); + SetOwner(Actor); + Actor->AddTickPrerequisiteActor(this); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Sensor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Sensor.h new file mode 100644 index 00000000000..4dd664f39ad --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Sensor.h @@ -0,0 +1,60 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "GameFramework/Actor.h" + +#include "Sensor/SensorDataSink.h" +#include "Settings/SensorDescription.h" + +#include "Sensor.generated.h" + +/// Base class for sensors. +UCLASS(Abstract, hidecategories=(Collision, Attachment, Actor)) +class CARLA_API ASensor : public AActor +{ + GENERATED_BODY() + +public: + + ASensor(const FObjectInitializer& ObjectInitializer); + + uint32 GetId() const + { + return Id; + } + + void AttachToActor(AActor *Actor); + + void SetSensorDataSink(TSharedPtr InSensorDataSink) + { + SensorDataSink = InSensorDataSink; + } + +protected: + + void Set(const USensorDescription &SensorDescription) + { + Id = SensorDescription.GetId(); + } + + void WriteSensorData(const FSensorDataView &SensorData) const + { + if (SensorDataSink.IsValid()) { + SensorDataSink->Write(SensorData); + } else { + UE_LOG(LogCarla, Warning, TEXT("Sensor %d has no data sink."), Id); + } + } + +private: + + UPROPERTY(VisibleAnywhere) + uint32 Id; + + TSharedPtr SensorDataSink = nullptr; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameState.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SensorDataSink.h similarity index 53% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameState.cpp rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SensorDataSink.h index 3ce608ee10c..f9a242f88ad 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameState.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SensorDataSink.h @@ -4,9 +4,15 @@ // This work is licensed under the terms of the MIT license. // For a copy, see . -#include "Carla.h" -#include "CarlaGameState.h" +#pragma once +class FSensorDataView; +/// Interface for sensor data sinks. +class ISensorDataSink { +public: + virtual ~ISensorDataSink() {} + virtual void Write(const FSensorDataView &SensorData) = 0; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SensorDataView.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SensorDataView.h new file mode 100644 index 00000000000..db2006ae557 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SensorDataView.h @@ -0,0 +1,45 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Sensor/ReadOnlyBufferView.h" + +/// A view over a sensor's output data. Does not own the data. +class FSensorDataView { +public: + + FSensorDataView( + uint32 InSensorId, + FReadOnlyBufferView InHeader, + FReadOnlyBufferView InData) + : SensorId(InSensorId), + Header(InHeader), + Data(InData) {} + + uint32 GetSensorId() const + { + return SensorId; + } + + FReadOnlyBufferView GetHeader() const + { + return Header; + } + + FReadOnlyBufferView GetData() const + { + return Data; + } + +private: + + uint32 SensorId; + + FReadOnlyBufferView Header; + + FReadOnlyBufferView Data; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SensorFactory.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SensorFactory.cpp new file mode 100644 index 00000000000..3fe042a7762 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SensorFactory.cpp @@ -0,0 +1,60 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Carla.h" +#include "SensorFactory.h" + +#include "Sensor/Lidar.h" +#include "Sensor/SceneCaptureCamera.h" +#include "Settings/CameraDescription.h" +#include "Settings/LidarDescription.h" + +template +static T *SpawnSensor(const D &Description, UWorld &World) +{ + FActorSpawnParameters Params; + Params.Name = FName(*Description.Name); + return World.SpawnActor(Description.Position, Description.Rotation, Params); +} + +ASensor *FSensorFactory::Make( + const USensorDescription &Description, + UWorld &World) +{ + FSensorFactory Visitor(World); + Description.AcceptVisitor(Visitor); + check(Visitor.Sensor != nullptr); + return Visitor.Sensor; +} + +FSensorFactory::FSensorFactory(UWorld &World) : World(World) {} + +void FSensorFactory::Visit(const UCameraDescription &Description) +{ + auto Camera = SpawnSensor(Description, World); + Camera->Set(Description); + UE_LOG( + LogCarla, + Log, + TEXT("Created Capture Camera %d with postprocess \"%s\""), + Camera->GetId(), + *PostProcessEffect::ToString(Camera->GetPostProcessEffect())); + Sensor = Camera; +} + +void FSensorFactory::Visit(const ULidarDescription &Description) +{ + auto Lidar = SpawnSensor(Description, World); + Lidar->Set(Description); + UE_LOG( + LogCarla, + Log, + TEXT("Created LiDAR %d"), + Lidar->GetId()); + Sensor = Lidar; +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SensorFactory.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SensorFactory.h new file mode 100644 index 00000000000..11b334cbed6 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SensorFactory.h @@ -0,0 +1,33 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Settings/SensorDescriptionVisitor.h" + +class ASensor; +class UWorld; + +class FSensorFactory : private ISensorDescriptionVisitor +{ +public: + + static ASensor *Make( + const USensorDescription &Description, + UWorld &World); + +private: + + FSensorFactory(UWorld &World); + + virtual void Visit(const UCameraDescription &) final; + + virtual void Visit(const ULidarDescription &) final; + + UWorld &World; + + ASensor *Sensor = nullptr; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaEncoder.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaEncoder.cpp new file mode 100644 index 00000000000..408aad9f85e --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaEncoder.cpp @@ -0,0 +1,213 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "Carla.h" +#include "CarlaEncoder.h" + +#include "Agent/TrafficSignAgentComponent.h" +#include "Agent/VehicleAgentComponent.h" +#include "Agent/WalkerAgentComponent.h" +#include "Game/CarlaPlayerState.h" +#include "Settings/SensorDescription.h" + +#include "GameFramework/PlayerStart.h" + +#include + +// ============================================================================= +// -- Static local methods ----------------------------------------------------- +// ============================================================================= + +static auto MakeCharBuffer(const FString &String) +{ + const char *Ptr = TCHAR_TO_UTF8(*String); + auto Buffer = MakeUnique(std::strlen(Ptr) + 1u); // + null terminator. + #if _WIN32 + strcpy_s(Buffer.Get(),String.Len()+1, Ptr); + #else + std::strcpy(Buffer.Get(), Ptr); + #endif + return TUniquePtr(Buffer.Release()); +} + +static void Encode(const FVector &Vector, carla_vector3d &Data) +{ + Data = {Vector.X, Vector.Y, Vector.Z}; +} + +static void Encode(const FRotator &Rotator, carla_rotation3d &Data) +{ + Data.pitch = Rotator.Pitch; + Data.roll = Rotator.Roll; + Data.yaw = Rotator.Yaw; +} + +static void Encode(const FTransform &Transform, carla_transform &Data) +{ + Encode(Transform.GetLocation(), Data.location); + Encode(Transform.GetRotation().GetForwardVector(), Data.orientation); + Encode(Transform.Rotator(), Data.rotation); +} + +static TUniquePtr Encode( + const USensorDescription &SensorDescription, + carla_sensor_definition &Data) +{ + Data.id = SensorDescription.GetId(); + Data.type = [](const FString &Type) { +#define CARLA_CHECK_TYPE(Str) if (Type == TEXT(#Str)) return CARLA_SERVER_ ## Str; + CARLA_CHECK_TYPE(CAMERA) + CARLA_CHECK_TYPE(LIDAR_RAY_TRACE) + else return CARLA_SERVER_SENSOR_UNKNOWN; +#undef CARLA_CHECK_TYPE + }(SensorDescription.Type); + auto Memory = MakeCharBuffer(SensorDescription.Name); + Data.name = Memory.Get(); + return Memory; +} + +// ============================================================================= +// -- FCarlaEncoder static methods --------------------------------------------- +// ============================================================================= + +void FCarlaEncoder::Encode( + const TArray &AvailableStartSpots, + TArray &Data) +{ + const int32 NumberOfStartSpots = AvailableStartSpots.Num(); + Data.AddUninitialized(NumberOfStartSpots); + for (auto i = 0; i < NumberOfStartSpots; ++i) { + check(AvailableStartSpots[i] != nullptr); + ::Encode(AvailableStartSpots[i]->GetActorTransform(), Data[i]); + } +} + +void FCarlaEncoder::Encode( + const TArray &SensorDescriptions, + TArray &Data, + TArray> &SensorNames) +{ + const int32 NumberOfSensors = SensorDescriptions.Num(); + Data.AddUninitialized(NumberOfSensors); + SensorNames.Reserve(NumberOfSensors); + for (auto i = 0; i < NumberOfSensors; ++i) { + check(SensorDescriptions[i] != nullptr); + SensorNames.Emplace(::Encode(*SensorDescriptions[i], Data[i])); + } +} + +void FCarlaEncoder::Encode( + const ACarlaPlayerState &PlayerState, + carla_measurements &Data) +{ + Data.platform_timestamp = PlayerState.GetPlatformTimeStamp(); + Data.game_timestamp = PlayerState.GetGameTimeStamp(); + auto &Player = Data.player_measurements; + ::Encode(PlayerState.GetTransform(), Player.transform); + ::Encode(PlayerState.GetAcceleration(), Player.acceleration); + Player.forward_speed = PlayerState.GetForwardSpeed(); + Player.collision_vehicles = PlayerState.GetCollisionIntensityCars(); + Player.collision_pedestrians = PlayerState.GetCollisionIntensityPedestrians(); + Player.collision_other = PlayerState.GetCollisionIntensityOther(); + Player.intersection_otherlane = PlayerState.GetOtherLaneIntersectionFactor(); + Player.intersection_offroad = PlayerState.GetOffRoadIntersectionFactor(); + Player.autopilot_control.steer = PlayerState.GetSteer(); + Player.autopilot_control.throttle = PlayerState.GetThrottle(); + Player.autopilot_control.brake = PlayerState.GetBrake(); + Player.autopilot_control.hand_brake = PlayerState.GetHandBrake(); + Player.autopilot_control.reverse = PlayerState.GetCurrentGear() < 0; +} + +void FCarlaEncoder::Encode( + const TArray &Agents, + TArray &Data) +{ + const int32 NumberOfAgents = Agents.Num(); + Data.AddUninitialized(NumberOfAgents); + for (auto i = 0; i < NumberOfAgents; ++i) { + check(Agents[i] != nullptr); + Encode(*Agents[i], Data[i]); + } +} + +void FCarlaEncoder::Encode(const UAgentComponent &AgentComponent, carla_agent &AgentData) +{ + AgentData.id = AgentComponent.GetId(); + ::Encode(AgentComponent.GetComponentTransform(), AgentData.transform); + FCarlaEncoder Encoder(AgentData); + AgentComponent.AcceptVisitor(Encoder); +} + +// ============================================================================= +// -- FCarlaEncoder ------------------------------------------------------------ +// ============================================================================= + +FCarlaEncoder::FCarlaEncoder(carla_agent &InData) : Data(InData) {} + +void FCarlaEncoder::Visit(const UTrafficSignAgentComponent &Agent) +{ + auto &TrafficSign = Agent.GetTrafficSign(); + switch (TrafficSign.GetTrafficSignState()) { + case ETrafficSignState::TrafficLightRed: + Data.type = CARLA_SERVER_AGENT_TRAFFICLIGHT_RED; + break; + case ETrafficSignState::TrafficLightYellow: + Data.type = CARLA_SERVER_AGENT_TRAFFICLIGHT_YELLOW; + break; + case ETrafficSignState::TrafficLightGreen: + Data.type = CARLA_SERVER_AGENT_TRAFFICLIGHT_GREEN; + break; + case ETrafficSignState::SpeedLimit_30: + Data.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; + Data.forward_speed = 30.0f; + break; + case ETrafficSignState::SpeedLimit_40: + Data.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; + Data.forward_speed = 40.0f; + break; + case ETrafficSignState::SpeedLimit_50: + Data.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; + Data.forward_speed = 50.0f; + break; + case ETrafficSignState::SpeedLimit_60: + Data.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; + Data.forward_speed = 60.0f; + break; + case ETrafficSignState::SpeedLimit_90: + Data.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; + Data.forward_speed = 90.0f; + break; + case ETrafficSignState::SpeedLimit_100: + Data.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; + Data.forward_speed = 100.0f; + break; + case ETrafficSignState::SpeedLimit_120: + Data.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; + Data.forward_speed = 120.0f; + break; + case ETrafficSignState::SpeedLimit_130: + Data.type = CARLA_SERVER_AGENT_SPEEDLIMITSIGN; + Data.forward_speed = 130.0f; + break; + default: + UE_LOG(LogCarla, Error, TEXT("Unknown traffic sign!")); + } +} + +void FCarlaEncoder::Visit(const UVehicleAgentComponent &Agent) +{ + auto &Vehicle = Agent.GetVehicle(); + Data.type = CARLA_SERVER_AGENT_VEHICLE; + Data.forward_speed = Vehicle.GetVehicleForwardSpeed(); + ::Encode(Vehicle.GetVehicleBoundsExtent(), Data.box_extent); +} + +void FCarlaEncoder::Visit(const UWalkerAgentComponent &Agent) +{ + Data.type = CARLA_SERVER_AGENT_PEDESTRIAN; + Data.forward_speed = Agent.GetForwardSpeed(); + ::Encode(Agent.GetBoundingBoxExtent(), Data.box_extent); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaEncoder.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaEncoder.h new file mode 100644 index 00000000000..4c32bfaa6c3 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaEncoder.h @@ -0,0 +1,74 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Agent/AgentComponentVisitor.h" + +#include "Sensor/SensorDataView.h" +#include "Vehicle/VehicleControl.h" + +#include + +/// Encodes Unreal classes to CarlaServer API. To be used by FCarlaServer only. +class FCarlaEncoder : private IAgentComponentVisitor +{ +public: + + static void Encode( + const TArray &AvailableStartSpots, + TArray &Data); + + static void Encode( + const TArray &SensorDescriptions, + TArray &Data, + TArray> &SensorNamesMemory); + + static void Encode( + const ACarlaPlayerState &PlayerState, + carla_measurements &Data); + + static void Encode( + const TArray &Agents, + TArray &Data); + + static void Encode(const FSensorDataView &SensorData, carla_sensor_data &Data) + { + Data.id = SensorData.GetSensorId(); + Data.header = SensorData.GetHeader().GetData(); + Data.header_size = SensorData.GetHeader().GetSize(); + Data.data = SensorData.GetData().GetData(); + Data.data_size = SensorData.GetData().GetSize(); + } + + static void Decode(const carla_request_new_episode &Data, FString &IniFile) + { + IniFile = FString(Data.ini_file_length, ANSI_TO_TCHAR(Data.ini_file)); + } + + static void Decode(const carla_control &Data, FVehicleControl &Control) + { + Control.Steer = Data.steer; + Control.Throttle = Data.throttle; + Control.Brake = Data.brake; + Control.bHandBrake = Data.hand_brake; + Control.bReverse = Data.reverse; + } + +private: + + static void Encode(const UAgentComponent &AgentComponent, carla_agent &Data); + + FCarlaEncoder(carla_agent &Data); + + virtual void Visit(const UTrafficSignAgentComponent &) override; + + virtual void Visit(const UVehicleAgentComponent &) override; + + virtual void Visit(const UWalkerAgentComponent &) override; + + carla_agent &Data; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp new file mode 100644 index 00000000000..ee4f41de5e1 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp @@ -0,0 +1,165 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "Carla.h" +#include "CarlaServer.h" + +#include "Server/CarlaEncoder.h" + +#include + +// ============================================================================= +// -- Static local methods ----------------------------------------------------- +// ============================================================================= + +static FCarlaServer::ErrorCode ParseErrorCode(const uint32 ErrorCode) +{ + if (ErrorCode == CARLA_SERVER_SUCCESS) { + return FCarlaServer::Success; + } else if (ErrorCode == CARLA_SERVER_TRY_AGAIN) { + return FCarlaServer::TryAgain; + } else { + return FCarlaServer::Error; + } +} + +static int32 GetTimeOut(uint32 TimeOut, const bool bBlocking) +{ + return (bBlocking ? TimeOut : 0u); +} + +// ============================================================================= +// -- CarlaServer -------------------------------------------------------------- +// ============================================================================= + +FCarlaServer::FCarlaServer(const uint32 InWorldPort, const uint32 InTimeOut) : + WorldPort(InWorldPort), + TimeOut(InTimeOut), + Server(carla_make_server()) { + check(Server != nullptr); +} + +FCarlaServer::~FCarlaServer() +{ +#ifdef CARLA_SERVER_EXTRA_LOG + UE_LOG(LogCarlaServer, Warning, TEXT("Destroying CarlaServer")); +#endif // CARLA_SERVER_EXTRA_LOG + carla_free_server(Server); +} + +FCarlaServer::ErrorCode FCarlaServer::Connect() +{ + UE_LOG(LogCarlaServer, Log, TEXT("Waiting for the client to connect...")); + return ParseErrorCode(carla_server_connect(Server, WorldPort, TimeOut)); +} + +FCarlaServer::ErrorCode FCarlaServer::ReadNewEpisode(FString &IniFile, const bool bBlocking) +{ + carla_request_new_episode values; + auto ec = ParseErrorCode(carla_read_request_new_episode(Server, values, GetTimeOut(TimeOut, bBlocking))); + if (Success == ec) { + FCarlaEncoder::Decode(values, IniFile); + UE_LOG(LogCarlaServer, Log, TEXT("Received new episode")); +#ifdef CARLA_SERVER_EXTRA_LOG + UE_LOG(LogCarlaServer, Log, TEXT("Received CarlaSettings.ini:\n%s"), *IniFile); +#endif // CARLA_SERVER_EXTRA_LOG + } + return ec; +} + +FCarlaServer::ErrorCode FCarlaServer::SendSceneDescription( + const TArray &AvailableStartSpots, + const TArray &SensorDescriptions, + const bool bBlocking) +{ + carla_scene_description scene; + // Encode start spots. + TArray Transforms; + FCarlaEncoder::Encode(AvailableStartSpots, Transforms); + scene.player_start_spots = (Transforms.Num() > 0 ? Transforms.GetData() : nullptr);; + scene.number_of_player_start_spots = Transforms.Num(); + // Encode sensors. + TArray> SensorNames; // This holds the memory while we send it. + TArray Sensors; + FCarlaEncoder::Encode(SensorDescriptions, Sensors, SensorNames); + scene.sensors = (Sensors.Num() > 0 ? Sensors.GetData() : nullptr);; + scene.number_of_sensors = Sensors.Num(); + // Send scene description. + UE_LOG(LogCarlaServer, Log, TEXT("Sending %d available start positions"), scene.number_of_player_start_spots); + UE_LOG(LogCarlaServer, Log, TEXT("Sending %d sensor descriptions"), scene.number_of_sensors); + return ParseErrorCode(carla_write_scene_description(Server, scene, GetTimeOut(TimeOut, bBlocking))); +} + +FCarlaServer::ErrorCode FCarlaServer::ReadEpisodeStart(uint32 &StartPositionIndex, const bool bBlocking) +{ + carla_episode_start values; + auto ec = ParseErrorCode(carla_read_episode_start(Server, values, GetTimeOut(TimeOut, bBlocking))); + if (Success == ec) { + StartPositionIndex = values.player_start_spot_index; + UE_LOG(LogCarlaServer, Log, TEXT("Episode start received: { StartIndex = %d }"), StartPositionIndex); + } + return ec; +} + +FCarlaServer::ErrorCode FCarlaServer::SendEpisodeReady(const bool bBlocking) +{ + UE_LOG(LogCarlaServer, Log, TEXT("Ready to play, notifying client")); + const carla_episode_ready values = {true}; + return ParseErrorCode(carla_write_episode_ready(Server, values, GetTimeOut(TimeOut, bBlocking))); +} + +FCarlaServer::ErrorCode FCarlaServer::ReadControl(FVehicleControl &Control, const bool bBlocking) +{ + carla_control values; + auto ec = ParseErrorCode(carla_read_control(Server, values, GetTimeOut(TimeOut, bBlocking))); + if (Success == ec) { +#ifdef CARLA_SERVER_EXTRA_LOG + UE_LOG( + LogCarlaServer, + Log, + TEXT("Read control (%s): { Steer = %f, Throttle = %f, Brake = %f, Handbrake = %s, Reverse = %s }"), + (bBlocking ? TEXT("Sync") : TEXT("Async")), + values.steer, + values.throttle, + values.brake, + (values.hand_brake ? TEXT("True") : TEXT("False")), + (values.reverse ? TEXT("True") : TEXT("False"))); +#endif // CARLA_SERVER_EXTRA_LOG + FCarlaEncoder::Decode(values, Control); + } else if ((!bBlocking) && (TryAgain == ec)) { + UE_LOG(LogCarlaServer, Warning, TEXT("No control received from the client this frame!")); + } + return ec; +} + +FCarlaServer::ErrorCode FCarlaServer::SendSensorData(const FSensorDataView &Data) +{ + carla_sensor_data values; + FCarlaEncoder::Encode(Data, values); + return ParseErrorCode(carla_write_sensor_data(Server, values)); +} + +FCarlaServer::ErrorCode FCarlaServer::SendMeasurements( + const ACarlaPlayerState &PlayerState, + const TArray &Agents, + const bool bSendNonPlayerAgentsInfo) +{ + // Encode measurements. + carla_measurements values; + FCarlaEncoder::Encode(PlayerState, values); + // Encode agents. + TArray AgentsData; + if (bSendNonPlayerAgentsInfo) { + FCarlaEncoder::Encode(Agents, AgentsData); + } + values.non_player_agents = (AgentsData.Num() > 0 ? AgentsData.GetData() : nullptr);; + values.number_of_non_player_agents = AgentsData.Num(); + // Send measurements. +#ifdef CARLA_SERVER_EXTRA_LOG + UE_LOG(LogCarlaServer, Log, TEXT("Sending data of %d agents"), values.number_of_non_player_agents); +#endif // CARLA_SERVER_EXTRA_LOG + return ParseErrorCode(carla_write_measurements(Server, values)); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaServer.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.h similarity index 62% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaServer.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.h index f4ee095e29c..09e1e311998 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaServer.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.h @@ -6,13 +6,17 @@ #pragma once -class ACarlaGameState; -class ACarlaVehicleController; +#include "Containers/Array.h" + +class ACarlaPlayerState; class APlayerStart; -class UCarlaSettings; +class FSensorDataView; +class FString; +class USensorDescription; +struct FVehicleControl; /// Wrapper around carla_server API. -class CARLA_API CarlaServer +class FCarlaServer { public: @@ -22,29 +26,32 @@ class CARLA_API CarlaServer Error }; - explicit CarlaServer(uint32 WorldPort, uint32 TimeOutInMilliseconds); + explicit FCarlaServer(uint32 WorldPort, uint32 TimeOutInMilliseconds); - ~CarlaServer(); + ~FCarlaServer(); /// Connect with the client, block until the client connects or the time-out /// is met. ErrorCode Connect(); - ErrorCode ReadNewEpisode(UCarlaSettings &Settings, bool bBlocking); + ErrorCode ReadNewEpisode(FString &IniFile, bool bBlocking); ErrorCode SendSceneDescription( const TArray &AvailableStartSpots, + const TArray &SensorDescriptions, bool bBlocking); ErrorCode ReadEpisodeStart(uint32 &StartPositionIndex, bool bBlocking); ErrorCode SendEpisodeReady(bool bBlocking); - ErrorCode ReadControl(ACarlaVehicleController &Player, bool bBlocking); + ErrorCode ReadControl(FVehicleControl &Control, bool bBlocking); + + ErrorCode SendSensorData(const FSensorDataView &Data); ErrorCode SendMeasurements( - const ACarlaGameState &GameState, const ACarlaPlayerState &PlayerState, + const TArray &Agents, bool bSendNonPlayerAgentsInfo); private: diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameController.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/ServerGameController.cpp similarity index 52% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameController.cpp rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/ServerGameController.cpp index 78a07f603fb..90bbfceb4a7 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameController.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/ServerGameController.cpp @@ -5,48 +5,55 @@ // For a copy, see . #include "Carla.h" -#include "CarlaGameController.h" - -#include "CarlaVehicleController.h" +#include "ServerGameController.h" +#include "Game/DataRouter.h" +#include "Server/CarlaServer.h" +#include "Server/ServerSensorDataSink.h" #include "Settings/CarlaSettings.h" -#include "CarlaServer.h" -#include "RenderingThread.h" - -using Errc = CarlaServer::ErrorCode; +using Errc = FCarlaServer::ErrorCode; static constexpr bool BLOCKING = true; static constexpr bool NON_BLOCKING = false; -CarlaGameController::CarlaGameController() : - Server(nullptr), - Player(nullptr) {} +FServerGameController::FServerGameController(FDataRouter &InDataRouter) + : ICarlaGameControllerBase(InDataRouter), + DataSink(MakeShared()), + Server(nullptr) { + DataRouter.SetSensorDataSink(DataSink); +} -CarlaGameController::~CarlaGameController() {} +FServerGameController::~FServerGameController() {} -void CarlaGameController::Initialize(UCarlaSettings &InCarlaSettings) +void FServerGameController::Initialize(UCarlaSettings &InCarlaSettings) { CarlaSettings = &InCarlaSettings; // Initialize server if missing. - if (Server == nullptr) { - Server = MakeUnique(CarlaSettings->WorldPort, CarlaSettings->ServerTimeOut); - if ((Errc::Success != Server->Connect()) || - (Errc::Success != Server->ReadNewEpisode(*CarlaSettings, BLOCKING))) { + if (!Server.IsValid()) { + Server = MakeShared(CarlaSettings->WorldPort, CarlaSettings->ServerTimeOut); + DataSink->SetServer(Server); + FString IniFile; + if ((Errc::Success == Server->Connect()) && + (Errc::Success == Server->ReadNewEpisode(IniFile, BLOCKING))) { + CarlaSettings->LoadSettingsFromString(IniFile); + } else { UE_LOG(LogCarlaServer, Warning, TEXT("Failed to initialize, server needs restart")); Server = nullptr; } } } -APlayerStart *CarlaGameController::ChoosePlayerStart( +APlayerStart *FServerGameController::ChoosePlayerStart( const TArray &AvailableStartSpots) { check(AvailableStartSpots.Num() > 0); // Send scene description. - if (Server != nullptr) { - if (Errc::Success != Server->SendSceneDescription(AvailableStartSpots, BLOCKING)) { + if (Server.IsValid()) { + TArray Sensors; + CarlaSettings->SensorDescriptions.GenerateValueArray(Sensors); + if (Errc::Success != Server->SendSceneDescription(AvailableStartSpots, Sensors, BLOCKING)) { UE_LOG(LogCarlaServer, Warning, TEXT("Failed to send scene description, server needs restart")); Server = nullptr; } @@ -54,7 +61,7 @@ APlayerStart *CarlaGameController::ChoosePlayerStart( // Read episode start. uint32 StartIndex = 0u; // default. - if (Server != nullptr) { + if (Server.IsValid()) { if (Errc::Success != Server->ReadEpisodeStart(StartIndex, BLOCKING)) { UE_LOG(LogCarlaServer, Warning, TEXT("Failed to read episode start, server needs restart")); Server = nullptr; @@ -69,18 +76,14 @@ APlayerStart *CarlaGameController::ChoosePlayerStart( return AvailableStartSpots[StartIndex]; } -void CarlaGameController::RegisterPlayer(AController &NewPlayer) +void FServerGameController::RegisterPlayer(AController &NewPlayer) { - Player = Cast(&NewPlayer); - check(Player != nullptr); + } -void CarlaGameController::BeginPlay() +void FServerGameController::BeginPlay() { - check(Player != nullptr); - GameState = Cast(Player->GetWorld()->GetGameState()); - check(GameState != nullptr); - if (Server != nullptr) { + if (Server.IsValid()) { if (Errc::Success != Server->SendEpisodeReady(BLOCKING)) { UE_LOG(LogCarlaServer, Warning, TEXT("Failed to read episode start, server needs restart")); Server = nullptr; @@ -88,12 +91,11 @@ void CarlaGameController::BeginPlay() } } -void CarlaGameController::Tick(float DeltaSeconds) +void FServerGameController::Tick(float DeltaSeconds) { - check(Player != nullptr); check(CarlaSettings != nullptr); - if (Server == nullptr) { + if (!Server.IsValid()) { UE_LOG(LogCarlaServer, Warning, TEXT("Client disconnected, server needs restart")); RestartLevel(); return; @@ -101,9 +103,11 @@ void CarlaGameController::Tick(float DeltaSeconds) // Check if the client requested a new episode. { - auto ec = Server->ReadNewEpisode(*CarlaSettings, NON_BLOCKING); + FString IniFile; + auto ec = Server->ReadNewEpisode(IniFile, NON_BLOCKING); switch (ec) { case Errc::Success: + CarlaSettings->LoadSettingsFromString(IniFile); RestartLevel(); return; case Errc::Error: @@ -116,39 +120,30 @@ void CarlaGameController::Tick(float DeltaSeconds) // Send measurements. { - check(GameState != nullptr); - ENQUEUE_UNIQUE_RENDER_COMMAND_FOURPARAMETER( /** @TODO: move to the screencapturecamera */ - FSendMeasurements, - CarlaServer*,Server,Server.Get(), /** @TODO: move unique pointer lambda reference */ - const ACarlaGameState*, GameState, GameState, - const ACarlaPlayerState&, PlayerState,Player->GetPlayerState(), - //,ParamType4,ParamName4,ParamValue4,Code - bool,bSendNonPlayerAgentsInfo,CarlaSettings->bSendNonPlayerAgentsInfo, - { - if (Errc::Error == Server->SendMeasurements( - *GameState, - PlayerState, - bSendNonPlayerAgentsInfo)) - { - Server = nullptr; - } - }); - + if (Errc::Error == Server->SendMeasurements( + DataRouter.GetPlayerState(), + DataRouter.GetAgents(), + CarlaSettings->bSendNonPlayerAgentsInfo)) { + Server = nullptr; + return; + } } // Read control, block if the settings say so. - if(Server!=nullptr) { const bool bShouldBlock = CarlaSettings->bSynchronousMode; - if (Errc::Error == Server->ReadControl(*Player, bShouldBlock)) { + FVehicleControl Control; + if (Errc::Error == Server->ReadControl(Control, bShouldBlock)) { Server = nullptr; return; + } else { + DataRouter.ApplyVehicleControl(Control); } } } -void CarlaGameController::RestartLevel() +void FServerGameController::RestartLevel() { UE_LOG(LogCarlaServer, Log, TEXT("Restarting the level...")); - Player->RestartLevel(); + DataRouter.RestartLevel(); } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/ServerGameController.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/ServerGameController.h new file mode 100644 index 00000000000..6409837c676 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/ServerGameController.h @@ -0,0 +1,42 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Game/CarlaGameControllerBase.h" + +class FCarlaServer; +class FServerSensorDataSink; + +/// Implements remote control of game and player. +class FServerGameController : public ICarlaGameControllerBase +{ +public: + + FServerGameController(FDataRouter &DataRouter); + + ~FServerGameController(); + + virtual void Initialize(UCarlaSettings &CarlaSettings) final; + + virtual APlayerStart *ChoosePlayerStart(const TArray &AvailableStartSpots) final; + + virtual void RegisterPlayer(AController &NewPlayer) final; + + virtual void BeginPlay() final; + + virtual void Tick(float DeltaSeconds) final; + +private: + + void RestartLevel(); + + const TSharedPtr DataSink; + + TSharedPtr Server; + + UCarlaSettings *CarlaSettings = nullptr; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/ServerSensorDataSink.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/ServerSensorDataSink.h new file mode 100644 index 00000000000..ba817ff3eb5 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/ServerSensorDataSink.h @@ -0,0 +1,31 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Sensor/SensorDataSink.h" + +/// Sends the sensor data to the client. +class FServerSensorDataSink : public ISensorDataSink { +public: + + void SetServer(TSharedPtr InServer) + { + Server = InServer; + } + + virtual void Write(const FSensorDataView &SensorData) final + { + auto Ptr = Server.Pin(); + if (Ptr.IsValid()) { + Ptr->SendSensorData(SensorData); + } + } + +private: + + TWeakPtr Server; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CameraDescription.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CameraDescription.cpp new file mode 100644 index 00000000000..6a26b68b874 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CameraDescription.cpp @@ -0,0 +1,36 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "Carla.h" +#include "CameraDescription.h" + +#include "Util/IniFile.h" + +void UCameraDescription::Load(const FIniFile &Config, const FString &Section) +{ + Super::Load(Config, Section); + FString PostProcessing = TEXT("SceneFinal"); + Config.GetString(*Section, TEXT("PostProcessing"), PostProcessing); + PostProcessEffect = PostProcessEffect::FromString(PostProcessing); + Config.GetInt(*Section, TEXT("ImageSizeX"), ImageSizeX); + Config.GetInt(*Section, TEXT("ImageSizeY"), ImageSizeY); + Config.GetFloat(*Section, TEXT("FOV"), FOVAngle); +} + +void UCameraDescription::Validate() +{ + FMath::Clamp(FOVAngle, 0.001f, 360.0f); + ImageSizeX = (ImageSizeX == 0u ? 720u : ImageSizeX); + ImageSizeY = (ImageSizeY == 0u ? 512u : ImageSizeY); +} + +void UCameraDescription::Log() const +{ + Super::Log(); + UE_LOG(LogCarla, Log, TEXT("Image Size = %dx%d"), ImageSizeX, ImageSizeY); + UE_LOG(LogCarla, Log, TEXT("Post-Processing = %s"), *PostProcessEffect::ToString(PostProcessEffect)); + UE_LOG(LogCarla, Log, TEXT("FOV = %f"), FOVAngle); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CameraDescription.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CameraDescription.h index d10650fc8aa..54f823fe28d 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CameraDescription.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CameraDescription.h @@ -6,13 +6,34 @@ #pragma once -#include "PostProcessEffect.h" +#include "Settings/SensorDescription.h" + +#include "Settings/PostProcessEffect.h" + #include "CameraDescription.generated.h" -USTRUCT() -struct FCameraDescription +UCLASS() +class CARLA_API UCameraDescription : public USensorDescription { - GENERATED_USTRUCT_BODY() + GENERATED_BODY() + +public: + + virtual void AcceptVisitor(ISensorDescriptionVisitor &Visitor) const final + { + Visitor.Visit(*this); + } + + virtual void Load(const FIniFile &Config, const FString &Section) final; + + virtual void Validate() final; + + virtual bool RequiresSemanticSegmentation() const final + { + return PostProcessEffect == EPostProcessEffect::SemanticSegmentation; + } + + virtual void Log() const final; /** X size in pixels of the captured image. */ UPROPERTY(Category = "Camera Description", EditDefaultsOnly, meta=(ClampMin = "1")) @@ -22,14 +43,6 @@ struct FCameraDescription UPROPERTY(Category = "Camera Description", EditDefaultsOnly, meta=(ClampMin = "1")) uint32 ImageSizeY = 512u; - /** Position relative to the player. */ - UPROPERTY(Category = "Camera Description", EditDefaultsOnly) - FVector Position = {170.0f, 0.0f, 150.0f}; - - /** Rotation relative to the player. */ - UPROPERTY(Category = "Camera Description", EditDefaultsOnly) - FRotator Rotation = {0.0f, 0.0f, 0.0f}; - /** Post-process effect to be applied to the captured image. */ UPROPERTY(Category = "Camera Description", EditDefaultsOnly) EPostProcessEffect PostProcessEffect = EPostProcessEffect::SceneFinal; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CameraPostProcessParameters.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CameraPostProcessParameters.h index 07ebded37cc..9d686dad1e3 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CameraPostProcessParameters.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CameraPostProcessParameters.h @@ -7,6 +7,7 @@ #pragma once #include "Engine/Scene.h" + #include "CameraPostProcessParameters.generated.h" /** @@ -14,7 +15,7 @@ * the weather and lighting conditions. */ USTRUCT(BlueprintType) -struct FCameraPostProcessParameters +struct CARLA_API FCameraPostProcessParameters { GENERATED_USTRUCT_BODY() diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettings.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettings.cpp index cd312caff7c..d89203235e9 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettings.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettings.cpp @@ -5,85 +5,87 @@ // For a copy, see . #include "Carla.h" - -#include "CommandLine.h" -#include "UnrealMathUtility.h" +#include "CarlaSettings.h" #include "DynamicWeather.h" -#include "Settings/CarlaSettings.h" +#include "Settings/CameraDescription.h" +#include "Settings/LidarDescription.h" #include "Util/IniFile.h" +#include "CommandLine.h" +#include "UnrealMathUtility.h" + // INI file sections. #define S_CARLA_SERVER TEXT("CARLA/Server") #define S_CARLA_LEVELSETTINGS TEXT("CARLA/LevelSettings") -#define S_CARLA_SCENECAPTURE TEXT("CARLA/SceneCapture") +#define S_CARLA_SENSOR TEXT("CARLA/Sensor") // ============================================================================= -// -- MyIniFile ---------------------------------------------------------------- +// -- Static methods ----------------------------------------------------------- // ============================================================================= -class MyIniFile : public IniFile { -public: - - MyIniFile() = default; - - explicit MyIniFile(const FString &FileName) : IniFile(FileName) {} - - void GetPostProcessEffect(const TCHAR* Section, const TCHAR* Key, EPostProcessEffect &Target) const - { - FString ValueString; - if (GetFConfigFile().GetString(Section, Key, ValueString)) { - if (ValueString == "None") { - Target = EPostProcessEffect::None; - } else if (ValueString == "SceneFinal") { - Target = EPostProcessEffect::SceneFinal; - } else if (ValueString == "Depth") { - Target = EPostProcessEffect::Depth; - } else if (ValueString == "SemanticSegmentation") { - Target = EPostProcessEffect::SemanticSegmentation; - } else { - UE_LOG(LogCarla, Error, TEXT("Invalid post-processing \"%s\" in INI file"), *ValueString); - Target = EPostProcessEffect::INVALID; - } - } +template +static void ForEachSectionInName(const FString &SensorName, T &&Callback) +{ + TArray SubSections; + SensorName.ParseIntoArray(SubSections, TEXT("/"), true); + check(SubSections.Num() > 0); + FString Section = S_CARLA_SENSOR; + Callback(Section); + for (FString &SubSection : SubSections) { + Section += TEXT("/"); + Section += SubSection; + Callback(Section); } -}; +} -// ============================================================================= -// -- Static methods ----------------------------------------------------------- -// ============================================================================= +static FString GetSensorType( + const FIniFile &ConfigFile, + const FString &SensorName) +{ + FString SensorType; + ForEachSectionInName(SensorName, [&](const auto &Section){ + ConfigFile.GetString(*Section, TEXT("SensorType"), SensorType); + }); + return SensorType; +} -static void GetCameraDescription( - const MyIniFile &ConfigFile, - const TCHAR* Section, - FCameraDescription &Camera) +static void LoadSensorFromConfig( + const FIniFile &ConfigFile, + USensorDescription &Sensor) { - ConfigFile.GetInt(Section, TEXT("ImageSizeX"), Camera.ImageSizeX); - ConfigFile.GetInt(Section, TEXT("ImageSizeY"), Camera.ImageSizeY); - ConfigFile.GetInt(Section, TEXT("CameraFOV"), Camera.FOVAngle); - ConfigFile.GetInt(Section, TEXT("CameraPositionX"), Camera.Position.X); - ConfigFile.GetInt(Section, TEXT("CameraPositionY"), Camera.Position.Y); - ConfigFile.GetInt(Section, TEXT("CameraPositionZ"), Camera.Position.Z); - ConfigFile.GetInt(Section, TEXT("CameraRotationPitch"), Camera.Rotation.Pitch); - ConfigFile.GetInt(Section, TEXT("CameraRotationRoll"), Camera.Rotation.Roll); - ConfigFile.GetInt(Section, TEXT("CameraRotationYaw"), Camera.Rotation.Yaw); - ConfigFile.GetPostProcessEffect(Section, TEXT("PostProcessing"), Camera.PostProcessEffect); + ForEachSectionInName(Sensor.Name, [&](const auto &Section){ + Sensor.Load(ConfigFile, Section); + }); } -static void ValidateCameraDescription(FCameraDescription &Camera) +template +static T *MakeSensor(UObject *Parent, const FString &Name, const FString &Type) { - FMath::Clamp(Camera.FOVAngle, 0.001f, 360.0f); - Camera.ImageSizeX = (Camera.ImageSizeX == 0u ? 720u : Camera.ImageSizeX); - Camera.ImageSizeY = (Camera.ImageSizeY == 0u ? 512u : Camera.ImageSizeY); + auto *Sensor = NewObject(Parent); + Sensor->Name = Name; + Sensor->Type = Type; + return Sensor; } -static bool RequestedSemanticSegmentation(const FCameraDescription &Camera) +static USensorDescription *MakeSensor( + const FIniFile &ConfigFile, + UObject *Parent, + const FString &SensorName) { - return (Camera.PostProcessEffect == EPostProcessEffect::SemanticSegmentation); + const auto SensorType = GetSensorType(ConfigFile, SensorName); + if (SensorType == TEXT("CAMERA")) { + return MakeSensor(Parent, SensorName, SensorType); + } else if (SensorType == TEXT("LIDAR_RAY_TRACE")) { + return MakeSensor(Parent, SensorName, SensorType); + } else { + UE_LOG(LogCarla, Error, TEXT("Invalid sensor type '%s'"), *SensorType); + return nullptr; + } } static void LoadSettingsFromConfig( - const MyIniFile &ConfigFile, + const FIniFile &ConfigFile, UCarlaSettings &Settings, const bool bLoadCarlaServerSection) { @@ -102,27 +104,19 @@ static void LoadSettingsFromConfig( ConfigFile.GetInt(S_CARLA_LEVELSETTINGS, TEXT("WeatherId"), Settings.WeatherId); ConfigFile.GetInt(S_CARLA_LEVELSETTINGS, TEXT("SeedVehicles"), Settings.SeedVehicles); ConfigFile.GetInt(S_CARLA_LEVELSETTINGS, TEXT("SeedPedestrians"), Settings.SeedPedestrians); - // SceneCapture. - FString Cameras; - ConfigFile.GetString(S_CARLA_SCENECAPTURE, TEXT("Cameras"), Cameras); - TArray CameraNames; - Cameras.ParseIntoArray(CameraNames, TEXT(","), true); - for (FString &Name : CameraNames) { - FCameraDescription &Camera = Settings.CameraDescriptions.FindOrAdd(Name); - GetCameraDescription(ConfigFile, S_CARLA_SCENECAPTURE, Camera); - - TArray SubSections; - Name.ParseIntoArray(SubSections, TEXT("/"), true); - check(SubSections.Num() > 0); - FString Section = S_CARLA_SCENECAPTURE; - for (FString &SubSection : SubSections) { - Section += TEXT("/"); - Section += SubSection; - GetCameraDescription(ConfigFile, *Section, Camera); + // Sensors. + FString Sensors; + ConfigFile.GetString(S_CARLA_SENSOR, TEXT("Sensors"), Sensors); + TArray SensorNames; + Sensors.ParseIntoArray(SensorNames, TEXT(","), true); + for (const FString &Name : SensorNames) { + auto *Sensor = MakeSensor(ConfigFile, &Settings, Name); + if (Sensor != nullptr) { + LoadSensorFromConfig(ConfigFile, *Sensor); + Sensor->Validate(); + Settings.bSemanticSegmentationEnabled |= Sensor->RequiresSemanticSegmentation(); + Settings.SensorDescriptions.Add(Name, Sensor); } - - ValidateCameraDescription(Camera); - Settings.bSemanticSegmentationEnabled |= RequestedSemanticSegmentation(Camera); } } @@ -173,8 +167,8 @@ void UCarlaSettings::LoadSettings() void UCarlaSettings::LoadSettingsFromString(const FString &INIFileContents) { UE_LOG(LogCarla, Log, TEXT("Loading CARLA settings from string")); - ResetCameraDescriptions(); - MyIniFile ConfigFile; + ResetSensorDescriptions(); + FIniFile ConfigFile; ConfigFile.ProcessInputFileContents(INIFileContents); constexpr bool bLoadCarlaServerSection = false; LoadSettingsFromConfig(ConfigFile, *this, bLoadCarlaServerSection); @@ -218,23 +212,19 @@ void UCarlaSettings::LogSettings() const for (auto i = 0; i < WeatherDescriptions.Num(); ++i) { UE_LOG(LogCarla, Log, TEXT(" * %d - %s"), i, *WeatherDescriptions[i].Name); } - UE_LOG(LogCarla, Log, TEXT("[%s]"), S_CARLA_SCENECAPTURE); - UE_LOG(LogCarla, Log, TEXT("Added %d cameras."), CameraDescriptions.Num()); + UE_LOG(LogCarla, Log, TEXT("[%s]"), S_CARLA_SENSOR); + UE_LOG(LogCarla, Log, TEXT("Added %d sensors."), SensorDescriptions.Num()); UE_LOG(LogCarla, Log, TEXT("Semantic Segmentation = %s"), EnabledDisabled(bSemanticSegmentationEnabled)); - for (auto &Item : CameraDescriptions) { - UE_LOG(LogCarla, Log, TEXT("[%s/%s]"), S_CARLA_SCENECAPTURE, *Item.Key); - UE_LOG(LogCarla, Log, TEXT("Image Size = %dx%d"), Item.Value.ImageSizeX, Item.Value.ImageSizeY); - UE_LOG(LogCarla, Log, TEXT("FOV = %f"), Item.Value.FOVAngle); - UE_LOG(LogCarla, Log, TEXT("Camera Position = (%s)"), *Item.Value.Position.ToString()); - UE_LOG(LogCarla, Log, TEXT("Camera Rotation = (%s)"), *Item.Value.Rotation.ToString()); - UE_LOG(LogCarla, Log, TEXT("Post-Processing = %s"), *PostProcessEffect::ToString(Item.Value.PostProcessEffect)); + for (auto &&Sensor : SensorDescriptions) { + check(Sensor.Value != nullptr); + Sensor.Value->Log(); } UE_LOG(LogCarla, Log, TEXT("================================================================================")); } #undef S_CARLA_SERVER #undef S_CARLA_LEVELSETTINGS -#undef S_CARLA_SCENECAPTURE +#undef S_CARLA_SENSOR void UCarlaSettings::GetActiveWeatherDescription( bool &bWeatherWasChanged, @@ -256,9 +246,9 @@ const FWeatherDescription &UCarlaSettings::GetWeatherDescriptionByIndex(int32 In return WeatherDescriptions[Index]; } -void UCarlaSettings::ResetCameraDescriptions() +void UCarlaSettings::ResetSensorDescriptions() { - CameraDescriptions.Empty(); + SensorDescriptions.Empty(); bSemanticSegmentationEnabled = false; } @@ -266,8 +256,8 @@ void UCarlaSettings::LoadSettingsFromFile(const FString &FilePath, const bool bL { if (FPaths::FileExists(FilePath)) { UE_LOG(LogCarla, Log, TEXT("Loading CARLA settings from \"%s\""), *FilePath); - ResetCameraDescriptions(); - const MyIniFile ConfigFile(FilePath); + ResetSensorDescriptions(); + const FIniFile ConfigFile(FilePath); constexpr bool bLoadCarlaServerSection = true; LoadSettingsFromConfig(ConfigFile, *this, bLoadCarlaServerSection); CurrentFileName = FilePath; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettings.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettings.h index d024870c3f7..c65a839e725 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettings.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettings.h @@ -6,12 +6,14 @@ #pragma once -#include "CameraDescription.h" #include "WeatherDescription.h" #include "UObject/NoExportTypes.h" + #include "CarlaSettings.generated.h" +class USensorDescription; + /// Global settings for CARLA. UCLASS() class CARLA_API UCarlaSettings : public UObject @@ -56,7 +58,7 @@ class CARLA_API UCarlaSettings : public UObject void LoadSettingsFromFile(const FString &FilePath, bool bLogOnFailure); - void ResetCameraDescriptions(); + void ResetSensorDescriptions(); /** File name of the settings file used to load this settings. Empty if none used. */ UPROPERTY(Category = "CARLA Settings|Debug", VisibleAnywhere) @@ -127,20 +129,20 @@ class CARLA_API UCarlaSettings : public UObject /// @} // =========================================================================== - /// @name Scene Capture + /// @name Sensors // =========================================================================== /// @{ public: /** Descriptions of the cameras to be attached to the player. */ - UPROPERTY(Category = "Scene Capture", VisibleAnywhere) - TMap CameraDescriptions; + UPROPERTY(Category = "Sensors", VisibleAnywhere) + TMap SensorDescriptions; /** Whether semantic segmentation should be activated. The mechanisms for * semantic segmentation impose some performance penalties even if it is not * used, we only enable it if necessary. */ - UPROPERTY(Category = "Scene Capture", VisibleAnywhere) + UPROPERTY(Category = "Sensors", VisibleAnywhere) bool bSemanticSegmentationEnabled = false; /// @} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/LidarDescription.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/LidarDescription.cpp new file mode 100644 index 00000000000..dd469ab74ff --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/LidarDescription.cpp @@ -0,0 +1,39 @@ +// Fill out your copyright notice in the Description page of Project Settings. + +#include "Carla.h" +#include "LidarDescription.h" + +#include "Util/IniFile.h" + +void ULidarDescription::Load(const FIniFile &Config, const FString &Section) +{ + Super::Load(Config, Section); + Config.GetInt(*Section, TEXT("Channels"), Channels); + Config.GetFloat(*Section, TEXT("Range"), Range); + Config.GetInt(*Section, TEXT("PointsPerSecond"), PointsPerSecond); + Config.GetFloat(*Section, TEXT("RotationFrequency"), RotationFrequency); + Config.GetFloat(*Section, TEXT("UpperFovLimit"), UpperFovLimit); + Config.GetFloat(*Section, TEXT("LowerFovLimit"), LowerFovLimit); + Config.GetBool(*Section, TEXT("ShowDebugPoints"), ShowDebugPoints); +} + +void ULidarDescription::Validate() +{ + Channels = (Channels == 0u ? 32u : Channels); + FMath::Clamp(Range, 0.10f, 50000.0f); + PointsPerSecond = (PointsPerSecond == 0u ? 56000u : PointsPerSecond); + FMath::Clamp(RotationFrequency, 0.001f, 50000.0f); + FMath::Clamp(UpperFovLimit, -89.9f, 90.0f); + FMath::Clamp(LowerFovLimit, -90.0f, UpperFovLimit); +} + +void ULidarDescription::Log() const +{ + Super::Log(); + UE_LOG(LogCarla, Log, TEXT("Channels = %d"), Channels); + UE_LOG(LogCarla, Log, TEXT("Range = %f"), Range); + UE_LOG(LogCarla, Log, TEXT("PointsPerSecond = %d"), PointsPerSecond); + UE_LOG(LogCarla, Log, TEXT("RotationFrequency = %f"), RotationFrequency); + UE_LOG(LogCarla, Log, TEXT("UpperFovLimit = %f"), UpperFovLimit); + UE_LOG(LogCarla, Log, TEXT("LowerFovLimit = %f"), LowerFovLimit); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/LidarDescription.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/LidarDescription.h new file mode 100644 index 00000000000..fd84511c97c --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/LidarDescription.h @@ -0,0 +1,60 @@ +// Fill out your copyright notice in the Description page of Project Settings. + +#pragma once + +#include "Settings/SensorDescription.h" + +#include "LidarDescription.generated.h" + +UCLASS() +class CARLA_API ULidarDescription : public USensorDescription +{ + GENERATED_BODY() + +public: + + virtual void AcceptVisitor(ISensorDescriptionVisitor &Visitor) const final + { + Visitor.Visit(*this); + } + + virtual void Load(const FIniFile &Config, const FString &Section) final; + + virtual void Validate() final; + + virtual void Log() const final; + + /** Number of lasers. */ + UPROPERTY(EditDefaultsOnly, Category = "Lidar Description") + uint32 Channels = 32u; + + /** Measure distance. */ + UPROPERTY(EditDefaultsOnly, Category = "Lidar Description") + float Range = 5000.0f; + + /** Points generated by all lasers per second. */ + UPROPERTY(EditDefaultsOnly, Category = "Lidar Description") + uint32 PointsPerSecond = 56000u; + + /** Lidar rotation frequency. */ + UPROPERTY(EditDefaultsOnly, Category = "Lidar Description") + float RotationFrequency = 10.0f; + + /** + * Upper laser angle, counts from horizontal, positive values means above + * horizontal line. + */ + UPROPERTY(EditDefaultsOnly, Category = "Lidar Description") + float UpperFovLimit = 10.0f; + + /** + * Lower laser angle, counts from horizontal, negative values means under + * horizontal line. + */ + UPROPERTY(EditDefaultsOnly, Category = "Lidar Description") + float LowerFovLimit = -30.0f; + + /** Wether to show debug points of laser hits in simulator. */ + UPROPERTY(EditDefaultsOnly, Category = "Lidar Description") + bool ShowDebugPoints = false; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/PostProcessEffect.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/PostProcessEffect.cpp index 660bfac1242..12c5c229723 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/PostProcessEffect.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/PostProcessEffect.cpp @@ -6,6 +6,7 @@ #include "Carla.h" #include "PostProcessEffect.h" + #include "Package.h" FString PostProcessEffect::ToString(EPostProcessEffect PostProcessEffect) @@ -15,3 +16,19 @@ FString PostProcessEffect::ToString(EPostProcessEffect PostProcessEffect) return FString("Invalid"); return ptr->GetNameStringByIndex(static_cast(PostProcessEffect)); } + +EPostProcessEffect PostProcessEffect::FromString(const FString &String) +{ + if (String == "None") { + return EPostProcessEffect::None; + } else if (String == "SceneFinal") { + return EPostProcessEffect::SceneFinal; + } else if (String == "Depth") { + return EPostProcessEffect::Depth; + } else if (String == "SemanticSegmentation") { + return EPostProcessEffect::SemanticSegmentation; + } else { + UE_LOG(LogCarla, Error, TEXT("Invalid post-processing effect \"%s\""), *String); + return EPostProcessEffect::INVALID; + } +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/PostProcessEffect.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/PostProcessEffect.h index 3a60054854b..c217fefb08a 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/PostProcessEffect.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/PostProcessEffect.h @@ -28,6 +28,8 @@ class CARLA_API PostProcessEffect { static FString ToString(EPostProcessEffect PostProcessEffect); + static EPostProcessEffect FromString(const FString &String); + static constexpr uint_type ToUInt(EPostProcessEffect PostProcessEffect) { return static_cast(PostProcessEffect); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/SensorDescription.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/SensorDescription.cpp new file mode 100644 index 00000000000..446eca375e3 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/SensorDescription.cpp @@ -0,0 +1,34 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "Carla.h" +#include "SensorDescription.h" + +#include "Util/IniFile.h" + +void USensorDescription::AcceptVisitor(ISensorDescriptionVisitor &Visitor) const +{ + unimplemented(); +} + +void USensorDescription::Load(const FIniFile &Config, const FString &Section) +{ + Config.GetFloat(*Section, TEXT("PositionX"), Position.X); + Config.GetFloat(*Section, TEXT("PositionY"), Position.Y); + Config.GetFloat(*Section, TEXT("PositionZ"), Position.Z); + Config.GetFloat(*Section, TEXT("RotationPitch"), Rotation.Pitch); + Config.GetFloat(*Section, TEXT("RotationYaw"), Rotation.Yaw); + Config.GetFloat(*Section, TEXT("RotationRoll"), Rotation.Roll); +} + +void USensorDescription::Log() const +{ + UE_LOG(LogCarla, Log, TEXT("[%s/%s]"), TEXT("CARLA/Sensor"), *Name); + UE_LOG(LogCarla, Log, TEXT("Id = %d"), GetId()); + UE_LOG(LogCarla, Log, TEXT("Type = %s"), *Type); + UE_LOG(LogCarla, Log, TEXT("Position = (%s)"), *Position.ToString()); + UE_LOG(LogCarla, Log, TEXT("Rotation = (%s)"), *Rotation.ToString()); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/SensorDescription.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/SensorDescription.h new file mode 100644 index 00000000000..2fb33c09202 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/SensorDescription.h @@ -0,0 +1,57 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Settings/SensorDescriptionVisitor.h" + +#include "SensorDescription.generated.h" + +class FIniFile; + +UCLASS(Abstract) +class CARLA_API USensorDescription : public UObject +{ + GENERATED_BODY() + +public: + + uint32 GetId() const + { + return GetTypeHash(this); + } + + virtual void AcceptVisitor(ISensorDescriptionVisitor &Visitor) const; + + virtual void Load(const FIniFile &Config, const FString &Section); + + virtual void Validate() {} + + virtual bool RequiresSemanticSegmentation() const + { + return false; + } + + virtual void Log() const; + +public: + + /** Display name of the sensor. */ + UPROPERTY(Category = "Sensor Description", EditDefaultsOnly) + FString Name; + + /** Sensor type. */ + UPROPERTY(Category = "Sensor Description", EditDefaultsOnly) + FString Type; + + /** Position relative to the player. */ + UPROPERTY(Category = "Sensor Description", EditDefaultsOnly) + FVector Position = {170.0f, 0.0f, 150.0f}; + + /** Rotation relative to the player. */ + UPROPERTY(Category = "Sensor Description", EditDefaultsOnly) + FRotator Rotation = {0.0f, 0.0f, 0.0f}; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/SensorDescriptionVisitor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/SensorDescriptionVisitor.h new file mode 100644 index 00000000000..7ca79718492 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/SensorDescriptionVisitor.h @@ -0,0 +1,19 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +class UCameraDescription; +class ULidarDescription; + +class ISensorDescriptionVisitor +{ +public: + + virtual void Visit(const UCameraDescription &) = 0; + + virtual void Visit(const ULidarDescription &) = 0; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/WeatherDescription.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/WeatherDescription.cpp index 383c1edf7ee..5b94e4b67e5 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/WeatherDescription.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/WeatherDescription.cpp @@ -18,7 +18,7 @@ static FString PrecipitationTypeToString(EPrecipitationType PrecipitationType) } static void LoadPrecipitationType( - const IniFile &ConfigFile, + const FIniFile &ConfigFile, const TCHAR* Section, const TCHAR* Key, EPrecipitationType &Target) @@ -44,7 +44,7 @@ static FString AutoExposureMethodToString(EAutoExposureMethod AutoExposureMethod } static void LoadAutoExposureMethod( - const IniFile &ConfigFile, + const FIniFile &ConfigFile, const TCHAR* Section, const TCHAR* Key, EAutoExposureMethod &Target) @@ -59,7 +59,7 @@ static void LoadAutoExposureMethod( } } -void FWeatherDescription::ReadFromConfigFile(const IniFile &ConfigFile, const FString &Section) +void FWeatherDescription::ReadFromConfigFile(const FIniFile &ConfigFile, const FString &Section) { Name = Section; #define CARLA_LOAD_FROM_INI(Type, Key) ConfigFile.Get ## Type(*Section, TEXT(#Key), Key); @@ -103,7 +103,7 @@ void FWeatherDescription::ReadFromConfigFile(const IniFile &ConfigFile, const FS #undef CARLA_LOAD_FROM_INI } -void FWeatherDescription::WriteToConfigFile(IniFile &ConfigFile) const +void FWeatherDescription::WriteToConfigFile(FIniFile &ConfigFile) const { const FString &Section = Name; ConfigFile.AddSectionIfMissing(Section); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/WeatherDescription.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/WeatherDescription.h index 67f5f074e52..c85686b6ce9 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/WeatherDescription.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/WeatherDescription.h @@ -6,10 +6,11 @@ #pragma once -#include "CameraPostProcessParameters.h" +#include "Settings/CameraPostProcessParameters.h" + #include "WeatherDescription.generated.h" -class IniFile; +class FIniFile; UENUM(BlueprintType) enum class EPrecipitationType : uint8 @@ -18,13 +19,13 @@ enum class EPrecipitationType : uint8 }; USTRUCT(BlueprintType) -struct FWeatherDescription +struct CARLA_API FWeatherDescription { GENERATED_USTRUCT_BODY() - void ReadFromConfigFile(const IniFile &ConfigFile, const FString &Section); + void ReadFromConfigFile(const FIniFile &ConfigFile, const FString &Section); - void WriteToConfigFile(IniFile &ConfigFile) const; + void WriteToConfigFile(FIniFile &ConfigFile) const; // =========================================================================== /// @name Weather diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/RoutePlanner.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.cpp similarity index 97% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/RoutePlanner.cpp rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.cpp index 906cfe69881..f2b29b9af0e 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/RoutePlanner.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.cpp @@ -7,9 +7,9 @@ #include "Carla.h" #include "RoutePlanner.h" -#include "CarlaWheeledVehicle.h" #include "Util/RandomEngine.h" -#include "WheeledVehicleAIController.h" +#include "Vehicle/CarlaWheeledVehicle.h" +#include "Vehicle/WheeledVehicleAIController.h" #include "Engine/CollisionProfile.h" diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/RoutePlanner.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.h similarity index 99% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/RoutePlanner.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.h index 633887e5447..133068ab89d 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/RoutePlanner.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.h @@ -7,8 +7,10 @@ #pragma once #include "GameFramework/Actor.h" + #include "Components/BoxComponent.h" #include "Components/SplineComponent.h" + #include "RoutePlanner.generated.h" /// Assign a random route to every ACarlaWheeledVehicle entering the trigger diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/TrafficLightBase.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.cpp similarity index 93% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/TrafficLightBase.cpp rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.cpp index 9a15ad2a5cc..557828ebd34 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/TrafficLightBase.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.cpp @@ -7,8 +7,8 @@ #include "Carla.h" #include "TrafficLightBase.h" -#include "AI/WheeledVehicleAIController.h" -#include "CarlaWheeledVehicle.h" +#include "Vehicle/CarlaWheeledVehicle.h" +#include "Vehicle/WheeledVehicleAIController.h" // ============================================================================= // -- Static local methods ----------------------------------------------------- @@ -35,7 +35,8 @@ static ETrafficSignState ToTrafficSignState(ETrafficLightState State) { // -- ATrafficLightBase -------------------------------------------------------- // ============================================================================= -ATrafficLightBase::ATrafficLightBase() : Super() +ATrafficLightBase::ATrafficLightBase(const FObjectInitializer &ObjectInitializer) + : Super(ObjectInitializer) { PrimaryActorTick.bCanEverTick = false; } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/TrafficLightBase.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.h similarity index 91% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/TrafficLightBase.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.h index b48e6c994ac..762da424b6f 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/TrafficLightBase.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.h @@ -6,8 +6,10 @@ #pragma once -#include "TrafficSignBase.h" -#include "TrafficLightState.h" +#include "Traffic/TrafficSignBase.h" + +#include "Traffic/TrafficLightState.h" + #include "TrafficLightBase.generated.h" class ACarlaWheeledVehicle; @@ -20,7 +22,7 @@ class CARLA_API ATrafficLightBase : public ATrafficSignBase { public: - ATrafficLightBase(); + ATrafficLightBase(const FObjectInitializer &ObjectInitializer); protected: diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/TrafficLightState.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightState.h similarity index 100% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/TrafficLightState.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightState.h diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficSignBase.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficSignBase.cpp new file mode 100644 index 00000000000..3320febbe74 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficSignBase.cpp @@ -0,0 +1,19 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "Carla.h" +#include "TrafficSignBase.h" + +ATrafficSignBase::ATrafficSignBase(const FObjectInitializer &ObjectInitializer) + : Super(ObjectInitializer) { + RootComponent = + ObjectInitializer.CreateDefaultSubobject(this, TEXT("SceneRootComponent")); + RootComponent->SetMobility(EComponentMobility::Static); + + TrafficSignAgentComponent = + CreateDefaultSubobject(TEXT("TrafficSignAgentComponent")); + TrafficSignAgentComponent->SetupAttachment(RootComponent); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/TrafficSignBase.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficSignBase.h similarity index 90% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/TrafficSignBase.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficSignBase.h index 8832dc7edca..2e4fcdb8d6e 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/TrafficSignBase.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficSignBase.h @@ -32,9 +32,7 @@ class CARLA_API ATrafficSignBase : public AActor { public: - ATrafficSignBase(); - - virtual void BeginPlay() override; + ATrafficSignBase(const FObjectInitializer &ObjectInitializer); UFUNCTION(BlueprintCallable) ETrafficSignState GetTrafficSignState() const @@ -52,4 +50,7 @@ class CARLA_API ATrafficSignBase : public AActor { UPROPERTY(Category = "Traffic Sign", EditAnywhere) ETrafficSignState TrafficSignState = ETrafficSignState::UNKNOWN; + + UPROPERTY(Category = "Traffic Sign", VisibleAnywhere) + UTrafficSignAgentComponent *TrafficSignAgentComponent; }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ActorWithRandomEngine.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ActorWithRandomEngine.h index 58437e410b8..a2e88cd0589 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ActorWithRandomEngine.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ActorWithRandomEngine.h @@ -7,6 +7,7 @@ #pragma once #include "GameFramework/Actor.h" + #include "ActorWithRandomEngine.generated.h" class URandomEngine; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/IniFile.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/IniFile.h index 9e6e7276f78..201416fba08 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/IniFile.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/IniFile.h @@ -12,7 +12,7 @@ /// Wrapper around Unreal's INI file. In get functions, @a Target value is only /// set if it was present in the INI file, otherwise it keeps its value. -class CARLA_API IniFile : private NonCopyable +class CARLA_API FIniFile : private NonCopyable { private: @@ -23,7 +23,7 @@ class CARLA_API IniFile : private NonCopyable (source <= std::numeric_limits::max())) { target = static_cast(source); } else { - UE_LOG(LogCarla, Error, TEXT("IniFile: Type cast failed")); + UE_LOG(LogCarla, Error, TEXT("FIniFile: Type cast failed")); } } @@ -34,9 +34,9 @@ class CARLA_API IniFile : private NonCopyable // =========================================================================== /// @{ - IniFile() = default; + FIniFile() = default; - explicit IniFile(const FString &FileName) + explicit FIniFile(const FString &FileName) { ConfigFile.Read(FileName); } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ListView.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ListView.h index cd6ddf7e35b..94d56cc4908 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ListView.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ListView.h @@ -7,7 +7,7 @@ #pragma once template -class CARLA_API ListView +class ListView { public: diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/NonCopyable.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/NonCopyable.h index 4641a5ef912..750c299c849 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/NonCopyable.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/NonCopyable.h @@ -1,6 +1,6 @@ #pragma once -class CARLA_API NonCopyable { +class NonCopyable { public: NonCopyable() = default; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/RandomEngine.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/RandomEngine.h index e3539ca69ed..c4beac27702 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/RandomEngine.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/RandomEngine.h @@ -11,7 +11,7 @@ #include "RandomEngine.generated.h" UCLASS(Blueprintable,BlueprintType) -class URandomEngine : public UObject +class CARLA_API URandomEngine : public UObject { GENERATED_BODY() diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaVehicleController.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaVehicleController.cpp similarity index 65% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaVehicleController.cpp rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaVehicleController.cpp index 469eb9333f9..a5d41e16e41 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaVehicleController.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaVehicleController.cpp @@ -7,18 +7,14 @@ #include "Carla.h" #include "CarlaVehicleController.h" -#include "SceneCaptureCamera.h" +#include "Sensor/Lidar.h" +#include "Sensor/SceneCaptureCamera.h" #include "Components/BoxComponent.h" #include "EngineUtils.h" #include "GameFramework/Pawn.h" #include "WheeledVehicle.h" #include "WheeledVehicleMovementComponent.h" -#include "CarlaPlayerState.h" -#include "CarlaWheeledVehicle.h" -#include "CarlaHUD.h" -#include "MapGen/RoadMap.h" -#include "Settings/CameraPostProcessParameters.h" // ============================================================================= // -- Constructor and destructor ----------------------------------------------- @@ -60,27 +56,6 @@ void ACarlaVehicleController::Possess(APawn *aPawn) // -- AActor ------------------------------------------------------------------- // ============================================================================= -void ACarlaVehicleController::BeginPlay() -{ - Super::BeginPlay(); - - if (CarlaPlayerState != nullptr) { - CarlaPlayerState->Images.Empty(); - const auto NumberOfCameras = SceneCaptureCameras.Num(); - if (NumberOfCameras > 0) { - CarlaPlayerState->Images.AddDefaulted(NumberOfCameras); - for (auto i = 0; i < NumberOfCameras; ++i) { - auto *Camera = SceneCaptureCameras[i]; - check(Camera != nullptr); - auto &Image = CarlaPlayerState->Images[i]; - Image.SizeX = Camera->GetImageSizeX(); - Image.SizeY = Camera->GetImageSizeY(); - Image.PostProcessEffect = Camera->GetPostProcessEffect(); - } - } - } -} - void ACarlaVehicleController::Tick(float DeltaTime) { Super::Tick(DeltaTime); @@ -102,48 +77,7 @@ void ACarlaVehicleController::Tick(float DeltaTime) CarlaPlayerState->SpeedLimit = GetSpeedLimit(); CarlaPlayerState->TrafficLightState = GetTrafficLightState(); IntersectPlayerWithRoadMap(); - const auto NumberOfCameras = SceneCaptureCameras.Num(); - check(NumberOfCameras == CarlaPlayerState->Images.Num()); - - for (auto i = 0; i < NumberOfCameras; ++i) - { - FCapturedImage *Image = &CarlaPlayerState->Images[i]; - ASceneCaptureCamera *Camera = SceneCaptureCameras[i]; - ENQUEUE_UNIQUE_RENDER_COMMAND_TWOPARAMETER( - FReadFromTexture, - FCapturedImage*, Image, Image, - ASceneCaptureCamera *, Camera, Camera, - { - Camera->WritePixels(Image->BitMap); - }); - } - } -} - -// ============================================================================= -// -- Scene capture ------------------------------------------------------------ -// ============================================================================= - -void ACarlaVehicleController::AddSceneCaptureCamera( - const FCameraDescription &Description, - const FCameraPostProcessParameters *OverridePostProcessParameters) -{ - auto Camera = GetWorld()->SpawnActor(Description.Position, Description.Rotation); - if (OverridePostProcessParameters != nullptr) { - Camera->Set(Description, *OverridePostProcessParameters); - } else { - Camera->Set(Description); } - Camera->AttachToActor(GetPawn(), FAttachmentTransformRules::KeepRelativeTransform); - Camera->SetOwner(GetPawn()); - AddTickPrerequisiteActor(Camera); - SceneCaptureCameras.Add(Camera); - UE_LOG( - LogCarla, - Log, - TEXT("Created capture camera %d with postprocess \"%s\""), - SceneCaptureCameras.Num() - 1, - *PostProcessEffect::ToString(Camera->GetPostProcessEffect())); } // ============================================================================= diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaVehicleController.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaVehicleController.h similarity index 85% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaVehicleController.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaVehicleController.h index 8fd69b7dc4b..f785e6ed023 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaVehicleController.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaVehicleController.h @@ -6,14 +6,18 @@ #pragma once -#include "WheeledVehicleController.h" +#include "Vehicle/WheeledVehicleController.h" + #include "CarlaVehicleController.generated.h" struct FCameraPostProcessParameters; class ACarlaHUD; class ACarlaPlayerState; +class ALidar; class ASceneCaptureCamera; -struct FCameraDescription; +class UCameraDescription; +class ULidarDescription; +struct FCameraPostProcessParameters; /// The CARLA player controller. UCLASS() @@ -47,8 +51,6 @@ class CARLA_API ACarlaVehicleController : public AWheeledVehicleController /// @{ public: - virtual void BeginPlay() override; - virtual void Tick(float DeltaTime) override; /// @} @@ -75,17 +77,6 @@ class CARLA_API ACarlaVehicleController : public AWheeledVehicleController return *CarlaPlayerState; } - /// @} - // =========================================================================== - /// @name Scene Capture - // =========================================================================== - /// @{ -public: - - void AddSceneCaptureCamera( - const FCameraDescription &CameraDescription, - const FCameraPostProcessParameters *OverridePostProcessParameters); - /// @} // =========================================================================== /// @name Events @@ -115,9 +106,6 @@ class CARLA_API ACarlaVehicleController : public AWheeledVehicleController // =========================================================================== private: - UPROPERTY() - TArray SceneCaptureCameras; - // Cast for quick access to the custom player state. UPROPERTY() ACarlaPlayerState *CarlaPlayerState; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CarlaWheeledVehicle.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp similarity index 85% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CarlaWheeledVehicle.cpp rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp index c74aa981cc0..5b194f693e3 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CarlaWheeledVehicle.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp @@ -7,6 +7,9 @@ #include "Carla.h" #include "CarlaWheeledVehicle.h" +#include "Agent/VehicleAgentComponent.h" +#include "Vehicle/VehicleControl.h" + #include "Components/BoxComponent.h" #include "Engine/CollisionProfile.h" @@ -22,6 +25,9 @@ ACarlaWheeledVehicle::ACarlaWheeledVehicle(const FObjectInitializer& ObjectIniti VehicleBounds->SetHiddenInGame(true); VehicleBounds->SetCollisionProfileName(UCollisionProfile::NoCollision_ProfileName); + VehicleAgentComponent = CreateDefaultSubobject(TEXT("VehicleAgentComponent")); + VehicleAgentComponent->SetupAttachment(RootComponent); + GetVehicleMovementComponent()->bReverseAsBrake = false; } @@ -64,6 +70,15 @@ float ACarlaWheeledVehicle::GetMaximumSteerAngle() const // -- Set functions ------------------------------------------------------------ // ============================================================================= +void ACarlaWheeledVehicle::ApplyVehicleControl(const FVehicleControl &VehicleControl) +{ + SetThrottleInput(VehicleControl.Throttle); + SetSteeringInput(VehicleControl.Steer); + SetBrakeInput(VehicleControl.Brake); + SetHandbrakeInput(VehicleControl.bHandBrake); + SetReverse(VehicleControl.bReverse); +} + void ACarlaWheeledVehicle::SetThrottleInput(const float Value) { GetVehicleMovementComponent()->SetThrottleInput(Value); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CarlaWheeledVehicle.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h similarity index 88% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CarlaWheeledVehicle.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h index b2cdd2d365f..09eb3e10ee9 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CarlaWheeledVehicle.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h @@ -6,12 +6,17 @@ #pragma once -#include "AI/CarlaWheeledVehicleState.h" -#include "CoreMinimal.h" #include "WheeledVehicle.h" + +#include "Vehicle/CarlaWheeledVehicleState.h" + +#include "CoreMinimal.h" + #include "CarlaWheeledVehicle.generated.h" class UBoxComponent; +class UVehicleAgentComponent; +struct FVehicleControl; /// Base class for CARLA wheeled vehicles. UCLASS() @@ -63,6 +68,9 @@ class CARLA_API ACarlaWheeledVehicle : public AWheeledVehicle /// @{ public: + UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable) + void ApplyVehicleControl(const FVehicleControl &VehicleControl); + UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable) void SetThrottleInput(float Value); @@ -107,9 +115,12 @@ class CARLA_API ACarlaWheeledVehicle : public AWheeledVehicle UPROPERTY(Category = "AI Controller", VisibleAnywhere) ECarlaWheeledVehicleState State = ECarlaWheeledVehicleState::UNKNOWN; - UPROPERTY(EditAnywhere) + UPROPERTY(Category = "CARLA Wheeled Vehicle", EditAnywhere) UBoxComponent *VehicleBounds; + UPROPERTY(Category = "CARLA Wheeled Vehicle", VisibleAnywhere) + UVehicleAgentComponent *VehicleAgentComponent; + UPROPERTY() bool bIsInReverse = false; }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/CarlaWheeledVehicleState.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicleState.h similarity index 100% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/CarlaWheeledVehicleState.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicleState.h diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/VehicleControl.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/VehicleControl.h new file mode 100644 index 00000000000..b59d3a32f7d --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/VehicleControl.h @@ -0,0 +1,30 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "VehicleControl.generated.h" + +USTRUCT(BlueprintType) +struct CARLA_API FVehicleControl +{ + GENERATED_BODY() + + UPROPERTY(Category = "Vehicle Control", EditAnywhere) + float Throttle = 0.0f; + + UPROPERTY(Category = "Vehicle Control", EditAnywhere) + float Steer = 0.0f; + + UPROPERTY(Category = "Vehicle Control", EditAnywhere) + float Brake = 0.0f; + + UPROPERTY(Category = "Vehicle Control", EditAnywhere) + bool bHandBrake = false; + + UPROPERTY(Category = "Vehicle Control", EditAnywhere) + bool bReverse = false; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/VehicleSpawnerBase.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/VehicleSpawnerBase.cpp similarity index 97% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/VehicleSpawnerBase.cpp rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/VehicleSpawnerBase.cpp index f7140189334..839bf83982c 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/VehicleSpawnerBase.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/VehicleSpawnerBase.cpp @@ -7,9 +7,9 @@ #include "Carla.h" #include "VehicleSpawnerBase.h" -#include "AI/WheeledVehicleAIController.h" -#include "CarlaWheeledVehicle.h" #include "Util/RandomEngine.h" +#include "Vehicle/CarlaWheeledVehicle.h" +#include "Vehicle/WheeledVehicleAIController.h" #include "Engine/PlayerStartPIE.h" #include "EngineUtils.h" diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/VehicleSpawnerBase.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/VehicleSpawnerBase.h similarity index 99% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/VehicleSpawnerBase.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/VehicleSpawnerBase.h index b3fc2a623af..c079c53a609 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/VehicleSpawnerBase.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/VehicleSpawnerBase.h @@ -7,6 +7,7 @@ #pragma once #include "Util/ActorWithRandomEngine.h" + #include "VehicleSpawnerBase.generated.h" class ACarlaWheeledVehicle; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WheeledVehicleAIController.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp similarity index 98% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WheeledVehicleAIController.cpp rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp index b2b8879417d..752c1f9cb4d 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WheeledVehicleAIController.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp @@ -7,13 +7,13 @@ #include "Carla.h" #include "WheeledVehicleAIController.h" +#include "MapGen/RoadMap.h" +#include "Vehicle/CarlaWheeledVehicle.h" + #include "EngineUtils.h" #include "GameFramework/Pawn.h" #include "WheeledVehicleMovementComponent.h" -#include "CarlaWheeledVehicle.h" -#include "MapGen/RoadMap.h" - // ============================================================================= // -- Static local methods ----------------------------------------------------- // ============================================================================= @@ -107,9 +107,7 @@ void AWheeledVehicleAIController::Tick(const float DeltaTime) TickAutopilotController(); if (bAutopilotEnabled) { - Vehicle->SetThrottleInput(AutopilotControl.Throttle); - Vehicle->SetSteeringInput(AutopilotControl.Steer); - Vehicle->SetBrakeInput(AutopilotControl.Brake); + Vehicle->ApplyVehicleControl(AutopilotControl); } } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WheeledVehicleAIController.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.h similarity index 95% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WheeledVehicleAIController.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.h index fa623f08e3f..2e7f36bf99e 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WheeledVehicleAIController.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.h @@ -9,7 +9,10 @@ #include #include "GameFramework/PlayerController.h" -#include "TrafficLightState.h" + +#include "Traffic/TrafficLightState.h" +#include "Vehicle/VehicleControl.h" + #include "WheeledVehicleAIController.generated.h" class ACarlaWheeledVehicle; @@ -182,14 +185,7 @@ class CARLA_API AWheeledVehicleAIController : public APlayerController /// @{ protected: - struct FAutopilotControl { - float Throttle = 0.0f; - float Steer = 0.0f; - float Brake = 0.0f; - bool bHandBrake = false; - }; - - const FAutopilotControl &GetAutopilotControl() const + const FVehicleControl &GetAutopilotControl() const { return AutopilotControl; } @@ -237,7 +233,7 @@ class CARLA_API AWheeledVehicleAIController : public APlayerController UPROPERTY(VisibleAnywhere) float MaximumSteerAngle = -1.0f; - FAutopilotControl AutopilotControl; + FVehicleControl AutopilotControl; std::queue TargetLocations; }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/WheeledVehicleController.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleController.cpp similarity index 100% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/WheeledVehicleController.cpp rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleController.cpp diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/WheeledVehicleController.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleController.h similarity index 98% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/WheeledVehicleController.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleController.h index 690566a994d..04e261f370e 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/WheeledVehicleController.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleController.h @@ -6,8 +6,10 @@ #pragma once +#include "Vehicle/WheeledVehicleAIController.h" + #include "CoreMinimal.h" -#include "AI/WheeledVehicleAIController.h" + #include "WheeledVehicleController.generated.h" class UCameraComponent; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WalkerAIController.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerAIController.cpp similarity index 89% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WalkerAIController.cpp rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerAIController.cpp index da0d98ff9c7..488b6bf9030 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WalkerAIController.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerAIController.cpp @@ -23,11 +23,14 @@ # define EXTRA_LOG_ONLY(predicate) #endif // CARLA_AI_WALKERS_EXTRA_LOG -static constexpr float UPDATE_TIME_IN_SECONDS = 10.0f; +static constexpr float UPDATE_TIME_IN_SECONDS = 0.6f; static constexpr float PREVISION_TIME_IN_SECONDS = 5.0f; static constexpr float WALKER_SIGHT_RADIUS = 500.0f; -static constexpr float WALKER_PERIPHERAL_VISION_ANGLE_IN_DEGREES = 90.0f; -static constexpr float VEHICLE_SAFETY_RADIUS = 400.0f; +static constexpr float WALKER_SPEED_DAMPING = 4.0f; +static constexpr float WALKER_PERIPHERAL_VISION_ANGLE_IN_DEGREES = 155.0f; +static constexpr float WALKER_MAX_TIME_PAUSED = 10.0f; +static constexpr float VEHICLE_SAFETY_RADIUS = 600.0f; + // ============================================================================= // -- PawnPath ----------------------------------------------------------------- @@ -70,7 +73,7 @@ class PawnPath { explicit PawnPath(const APawn &Walker) : Start(GetLocation(Walker)), - End(GetLocation(Walker) + GetForwardVector(Walker) * WALKER_SIGHT_RADIUS) {} + End(GetLocation(Walker) + GetForwardVector(Walker) * WALKER_SPEED_DAMPING * WALKER_SIGHT_RADIUS) {} explicit PawnPath(const AWheeledVehicle &Vehicle) : PawnPath(GetLocation(Vehicle), GetForwardVector(Vehicle), GetForwardSpeed(Vehicle)) {} @@ -145,7 +148,6 @@ AWalkerAIController::AWalkerAIController(const FObjectInitializer& ObjectInitial auto Perception = CreateDefaultSubobject(TEXT("AIPerception Component")); check(Perception != nullptr); - SetPerceptionComponent(*Perception); SightConfiguration = CreateDefaultSubobject(TEXT("SightConfiguration")); SightConfiguration->SightRadius = WALKER_SIGHT_RADIUS; @@ -154,10 +156,12 @@ AWalkerAIController::AWalkerAIController(const FObjectInitializer& ObjectInitial SightConfiguration->DetectionByAffiliation.bDetectEnemies = true; SightConfiguration->DetectionByAffiliation.bDetectNeutrals = true; SightConfiguration->DetectionByAffiliation.bDetectFriendlies = true; - + Perception->ConfigureSense(*SightConfiguration); Perception->SetDominantSense(SightConfiguration->GetSenseImplementation()); Perception->OnPerceptionUpdated.AddDynamic(this, &AWalkerAIController::SenseActors); + SetPerceptionComponent(*Perception); + TimeInState=0.0f; } @@ -171,17 +175,20 @@ void AWalkerAIController::Possess(APawn *aPawn) void AWalkerAIController::Tick(float DeltaSeconds) { Super::Tick(DeltaSeconds); - + TimeInState+=DeltaSeconds; if (Status != EWalkerStatus::RunOver) { - switch (GetMoveStatus()) { + switch (GetMoveStatus()) + { case EPathFollowingStatus::Idle: case EPathFollowingStatus::Waiting: LOG_AI_WALKER(Warning, "is stuck!"); - Status = EWalkerStatus::Stuck; + ChangeStatus(EWalkerStatus::Stuck); break; case EPathFollowingStatus::Paused: - LOG_AI_WALKER(Log, "is paused"); - TryResumeMovement(); + if(TimeInState>WALKER_MAX_TIME_PAUSED){ + LOG_AI_WALKER(Log, "is paused, trying resume movement"); + TryResumeMovement(); + } break; }; } @@ -195,9 +202,12 @@ FPathFollowingRequestResult AWalkerAIController::MoveTo( UE_LOG(LogCarla, Log, TEXT("Walker %s requested move from (%s) to (%s)"), *GetPawn()->GetName(), *GetPawn()->GetActorLocation().ToString(), - *MoveRequest.GetGoalLocation().ToString()); + *MoveRequest.GetGoalLocation().ToString()) + +; #endif // CARLA_AI_WALKERS_EXTRA_LOG - Status = EWalkerStatus::Moving; + + ChangeStatus(EWalkerStatus::Moving); return Super::MoveTo(MoveRequest, OutPath); } @@ -209,9 +219,10 @@ void AWalkerAIController::OnMoveCompleted( #ifdef CARLA_AI_WALKERS_EXTRA_LOG UE_LOG(LogCarla, Log, TEXT("Walker %s completed move at (%s)"), *GetPawn()->GetName(), - *GetPawn()->GetActorLocation().ToString()); + *GetPawn()->GetActorLocation().ToString()) +; #endif // CARLA_AI_WALKERS_EXTRA_LOG - Status = EWalkerStatus::MoveCompleted; + ChangeStatus(EWalkerStatus::MoveCompleted); } void AWalkerAIController::SenseActors(const TArray Actors) @@ -220,7 +231,7 @@ void AWalkerAIController::SenseActors(const TArray Actors) if ((Status == EWalkerStatus::Moving) && (aPawn != nullptr) && IntersectsWithVehicle(*aPawn, Actors)) { - TryPauseMovement(); + TryPauseMovement(); } } @@ -230,6 +241,14 @@ void AWalkerAIController::TrySetMovement(bool paused) else TryResumeMovement(); } +void AWalkerAIController::ChangeStatus(EWalkerStatus status) +{ + if(status==Status) return; + //switch (status) { } + TimeInState = 0.0f; + Status = status; +} + void AWalkerAIController::TryResumeMovement() { if (Status != EWalkerStatus::Moving) { @@ -241,7 +260,7 @@ void AWalkerAIController::TryResumeMovement() LOG_AI_WALKER(Error, "is unable to resume movement"); } else { LOG_AI_WALKER(Log, "resuming movement"); - Status = EWalkerStatus::Moving; + ChangeStatus(EWalkerStatus::Moving); } } } @@ -258,7 +277,7 @@ void AWalkerAIController::TryPauseMovement(const bool bItWasRunOver) LOG_AI_WALKER(Error, "is unable to pause movement"); } else { LOG_AI_WALKER(Log, "paused"); - Status = (bItWasRunOver ? EWalkerStatus::RunOver : EWalkerStatus::Paused); + ChangeStatus(bItWasRunOver ? EWalkerStatus::RunOver : EWalkerStatus::Paused); } } } @@ -274,7 +293,7 @@ void AWalkerAIController::OnPawnTookDamage( LOG_AI_WALKER(Warning, "has been run over"); constexpr bool bItWasRunOver = true; TryPauseMovement(bItWasRunOver); - Status = EWalkerStatus::RunOver; + ChangeStatus(EWalkerStatus::RunOver); } #undef EXTRA_LOG_ONLY diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WalkerAIController.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerAIController.h similarity index 96% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WalkerAIController.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerAIController.h index 9c05486b27b..cc218d1f779 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WalkerAIController.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerAIController.h @@ -35,6 +35,7 @@ class CARLA_API AWalkerAIController : public AAIController virtual void Tick(float DeltaSeconds) override; + virtual FPathFollowingRequestResult MoveTo( const FAIMoveRequest& MoveRequest, FNavPathSharedPtr* OutPath = nullptr) override; @@ -54,7 +55,7 @@ class CARLA_API AWalkerAIController : public AAIController void TrySetMovement(bool paused); private: - + void ChangeStatus(EWalkerStatus status); void TryResumeMovement(); void TryPauseMovement(bool bItWasRunOver = false); @@ -68,4 +69,5 @@ class CARLA_API AWalkerAIController : public AAIController UPROPERTY(VisibleAnywhere) EWalkerStatus Status = EWalkerStatus::Unknown; + float TimeInState=0.0f; }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WalkerSpawnPoint.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerSpawnPoint.h similarity index 100% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WalkerSpawnPoint.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerSpawnPoint.h diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WalkerSpawnerBase.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerSpawnerBase.cpp similarity index 98% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WalkerSpawnerBase.cpp rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerSpawnerBase.cpp index 409a033a031..58531505211 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WalkerSpawnerBase.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerSpawnerBase.cpp @@ -7,14 +7,14 @@ #include "Carla.h" #include "WalkerSpawnerBase.h" +#include "Util/RandomEngine.h" +#include "Walker/WalkerAIController.h" +#include "Walker/WalkerSpawnPoint.h" + #include "Components/BoxComponent.h" #include "EngineUtils.h" #include "GameFramework/Character.h" -#include "Util/RandomEngine.h" -#include "WalkerAIController.h" -#include "WalkerSpawnPoint.h" - // ============================================================================= // -- Static local methods ----------------------------------------------------- // ============================================================================= diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WalkerSpawnerBase.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerSpawnerBase.h similarity index 99% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WalkerSpawnerBase.h rename to Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerSpawnerBase.h index 0adb22b7733..ba17fae529c 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WalkerSpawnerBase.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerSpawnerBase.h @@ -7,6 +7,7 @@ #pragma once #include "Util/ActorWithRandomEngine.h" + #include "WalkerSpawnerBase.generated.h" class AWalkerSpawnPoint; diff --git a/Util/CarlaServer/include/carla/carla_server.h b/Util/CarlaServer/include/carla/carla_server.h index 4ffb02f3121..fb46df9649a 100644 --- a/Util/CarlaServer/include/carla/carla_server.h +++ b/Util/CarlaServer/include/carla/carla_server.h @@ -34,19 +34,16 @@ extern "C" { float roll; }; - struct carla_image { - uint32_t width; - uint32_t height; - uint32_t type; - const uint32_t *data; - }; - struct carla_transform { struct carla_vector3d location; struct carla_vector3d orientation; struct carla_rotation3d rotation; }; + /* ======================================================================== */ + /* -- agents -------------------------------------------------------------- */ + /* ======================================================================== */ + #define CARLA_SERVER_AGENT_UNKNOWN 0u #define CARLA_SERVER_AGENT_VEHICLE 10u #define CARLA_SERVER_AGENT_PEDESTRIAN 20u @@ -70,6 +67,31 @@ extern "C" { float forward_speed; }; + /* ======================================================================== */ + /* -- sensors ------------------------------------------------------------- */ + /* ======================================================================== */ + +#define CARLA_SERVER_SENSOR_UNKNOWN 0u +#define CARLA_SERVER_CAMERA 101u +#define CARLA_SERVER_LIDAR_RAY_TRACE 102u + + struct carla_sensor_definition { + /** Id of the sensor. */ + uint32_t id; + /** Type of the sensor (one of the above defines). */ + uint32_t type; + /** Display name of the sensor. */ + const char *name; + }; + + struct carla_sensor_data { + uint32_t id; + const void *header; + uint32_t header_size; + const void *data; + uint32_t data_size; + }; + /* ======================================================================== */ /* -- carla_request_new_episode ------------------------------------------- */ /* ======================================================================== */ @@ -94,6 +116,9 @@ extern "C" { /** Collection of the initial player start locations. */ const struct carla_transform *player_start_spots; uint32_t number_of_player_start_spots; + /** Definitions of the sensors present in the scene. */ + const struct carla_sensor_definition *sensors; + uint32_t number_of_sensors; }; /* ======================================================================== */ @@ -254,15 +279,21 @@ extern "C" { carla_control &values, uint32_t timeout_milliseconds); + /** Return values: + * CARLA_SERVER_SUCCESS Value was posted for sending. + * CARLA_SERVER_OPERATION_ABORTED Agent server is missing. + */ + CARLA_SERVER_API int32_t carla_write_sensor_data( + CarlaServerPtr self, + const carla_sensor_data &data); + /** Return values: * CARLA_SERVER_SUCCESS Value was posted for sending. * CARLA_SERVER_OPERATION_ABORTED Agent server is missing. */ CARLA_SERVER_API int32_t carla_write_measurements( CarlaServerPtr self, - const carla_measurements &values, - const struct carla_image *images, - uint32_t number_of_images); + const carla_measurements &values); #ifdef __cplusplus } diff --git a/Util/CarlaServer/source/carla/server/AgentServer.cpp b/Util/CarlaServer/source/carla/server/AgentServer.cpp index 6eac5bd9433..458f6ccb390 100644 --- a/Util/CarlaServer/source/carla/server/AgentServer.cpp +++ b/Util/CarlaServer/source/carla/server/AgentServer.cpp @@ -13,9 +13,11 @@ namespace server { CarlaEncoder &encoder, const uint32_t out_port, const uint32_t in_port, + const SensorDataInbox::Sensors &sensors, const time_duration timeout) : _out(encoder), _in(encoder), + _sensor_inbox(sensors), _measurements(timeout), _control(timeout) { _out.Connect(out_port, timeout); @@ -24,5 +26,32 @@ namespace server { _in.Execute(_control); } + error_code AgentServer::WriteSensorData(const carla_sensor_data &data) { + _sensor_inbox.Write(data); + return errc::success(); + } + + error_code AgentServer::WriteMeasurements(const carla_measurements &measurements) { + error_code ec; + if (!_control.TryGetResult(ec)) { /// @todo Why _control here?? is it a bug? + auto writer = _measurements.buffer()->MakeWriter(); + writer->Write(measurements, _sensor_inbox); + ec = errc::success(); + } + return ec; + }; + + error_code AgentServer::ReadControl(carla_control &control, timeout_t timeout) { + error_code ec = errc::try_again(); + if (!_control.TryGetResult(ec)) { + auto reader = _control.buffer()->TryMakeReader(timeout); + if (reader != nullptr) { + control = *reader; + ec = errc::success(); + } + } + return ec; + } + } // namespace server } // namespace carla diff --git a/Util/CarlaServer/source/carla/server/AgentServer.h b/Util/CarlaServer/source/carla/server/AgentServer.h index 37494f387a3..ecaad7b9111 100644 --- a/Util/CarlaServer/source/carla/server/AgentServer.h +++ b/Util/CarlaServer/source/carla/server/AgentServer.h @@ -9,6 +9,7 @@ #include "carla/NonCopyable.h" #include "carla/server/AsyncServer.h" #include "carla/server/EncoderServer.h" +#include "carla/server/SensorDataInbox.h" #include "carla/server/TCPServer.h" namespace carla { @@ -23,31 +24,14 @@ namespace server { CarlaEncoder &encoder, uint32_t out_port, uint32_t in_port, + const SensorDataInbox::Sensors &sensors, time_duration timeout); - error_code WriteMeasurements( - const carla_measurements &measurements, - const_array_view images) { - error_code ec; - if (!_control.TryGetResult(ec)) { - auto writer = _measurements.buffer()->MakeWriter(); - writer->Write(measurements, images); - ec = errc::success(); - } - return ec; - }; - - error_code ReadControl(carla_control &control, timeout_t timeout) { - error_code ec = errc::try_again(); - if (!_control.TryGetResult(ec)) { - auto reader = _control.buffer()->TryMakeReader(timeout); - if (reader != nullptr) { - control = *reader; - ec = errc::success(); - } - } - return ec; - } + error_code WriteSensorData(const carla_sensor_data &data); + + error_code WriteMeasurements(const carla_measurements &measurements); + + error_code ReadControl(carla_control &control, timeout_t timeout); private: @@ -55,6 +39,8 @@ namespace server { AsyncServer> _in; + SensorDataInbox _sensor_inbox; + StreamWriteTask _measurements; StreamReadTask _control; diff --git a/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp b/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp index 39cb2e654fa..7abc669ad16 100644 --- a/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp +++ b/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp @@ -11,6 +11,8 @@ #include "carla/ArrayView.h" #include "carla/Debug.h" #include "carla/Logging.h" +#include "carla/server/CarlaSceneDescription.h" +#include "carla/server/RequestNewEpisode.h" #include "carla/server/carla_server.pb.h" @@ -23,6 +25,10 @@ namespace server { return array_view::make_const(values.player_start_spots, values.number_of_player_start_spots); } + static auto sensors(const carla_scene_description &values) { + return array_view::make_const(values.sensors, values.number_of_sensors); + } + static auto agents(const carla_measurements &values) { return array_view::make_const(values.non_player_agents, values.number_of_non_player_agents); } @@ -48,6 +54,19 @@ namespace server { Set(lhs->mutable_rotation(), rhs.rotation); } + static void Set(cs::Sensor *lhs, const carla_sensor_definition &rhs) { + DEBUG_ASSERT(lhs != nullptr); + lhs->set_id(rhs.id); + lhs->set_name(std::string(rhs.name)); + lhs->set_type([&](){ + switch (rhs.type) { + case CARLA_SERVER_CAMERA: return cs::Sensor::CAMERA; + case CARLA_SERVER_LIDAR_RAY_TRACE: return cs::Sensor::LIDAR_RAY_TRACE; + default: return cs::Sensor::UNKNOWN; + } + }()); + } + static void Set(cs::Control *lhs, const carla_control &rhs) { DEBUG_ASSERT(lhs != nullptr); lhs->set_steer(rhs.steer); @@ -99,6 +118,8 @@ namespace server { return SetTrafficLight(lhs->mutable_traffic_light(), rhs, cs::TrafficLight::YELLOW); case CARLA_SERVER_AGENT_TRAFFICLIGHT_RED: return SetTrafficLight(lhs->mutable_traffic_light(), rhs, cs::TrafficLight::RED); + default: + log_error("invalid agent type"); } } @@ -108,9 +129,16 @@ namespace server { for (auto &spot : start_spots(values)) { Set(message->add_player_start_spots(), spot); } + for (auto &sensor : sensors(values)) { + Set(message->add_sensors(), sensor); + } return Protobuf::Encode(*message); } + std::string CarlaEncoder::Encode(const CarlaSceneDescription &values) { + return values.pop_scene(); + } + std::string CarlaEncoder::Encode(const carla_episode_ready &values) { auto *message = _protobuf.CreateMessage(); DEBUG_ASSERT(message != nullptr); diff --git a/Util/CarlaServer/source/carla/server/CarlaEncoder.h b/Util/CarlaServer/source/carla/server/CarlaEncoder.h index 91732671c4f..78eb48743d8 100644 --- a/Util/CarlaServer/source/carla/server/CarlaEncoder.h +++ b/Util/CarlaServer/source/carla/server/CarlaEncoder.h @@ -8,11 +8,13 @@ #include "carla/server/CarlaServerAPI.h" #include "carla/server/Protobuf.h" -#include "carla/server/RequestNewEpisode.h" namespace carla { namespace server { + class CarlaSceneDescription; + class RequestNewEpisode; + /// Converts the data between the C interface types and the Protobuf message /// that is going to be sent and received through the socket. class CarlaEncoder { @@ -40,6 +42,8 @@ namespace server { std::string Encode(const carla_scene_description &values); + std::string Encode(const CarlaSceneDescription &values); + std::string Encode(const carla_episode_ready &values); std::string Encode(const carla_measurements &values); diff --git a/Util/CarlaServer/source/carla/server/CarlaSceneDescription.h b/Util/CarlaServer/source/carla/server/CarlaSceneDescription.h new file mode 100644 index 00000000000..37c8fa2ccde --- /dev/null +++ b/Util/CarlaServer/source/carla/server/CarlaSceneDescription.h @@ -0,0 +1,48 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "carla/NonCopyable.h" +#include "carla/server/CarlaServerAPI.h" + +#include + +namespace carla { +namespace server { + + /// This is the only message that for convenience it is encoded in the main + /// thread. + /// + /// Since the messages are only sent once, it is safe to invalidate the + /// encoded string on the first read. + class CarlaSceneDescription : private NonCopyable { + public: + + CarlaSceneDescription() = default; + + CarlaSceneDescription(std::string &&encoded_scene) + : _encoded_scene(std::move(encoded_scene)) {} + + CarlaSceneDescription(CarlaSceneDescription &&rhs) + : _encoded_scene(std::move(rhs._encoded_scene)) {} + + CarlaSceneDescription &operator=(CarlaSceneDescription &&rhs) { + _encoded_scene = std::move(rhs._encoded_scene); + return *this; + } + + std::string pop_scene() const { + return std::move(_encoded_scene); + } + + private: + + mutable std::string _encoded_scene; + }; + +} // namespace server +} // namespace carla diff --git a/Util/CarlaServer/source/carla/server/CarlaServer.cpp b/Util/CarlaServer/source/carla/server/CarlaServer.cpp index 7add00d0567..d7670f369e0 100644 --- a/Util/CarlaServer/source/carla/server/CarlaServer.cpp +++ b/Util/CarlaServer/source/carla/server/CarlaServer.cpp @@ -116,19 +116,28 @@ int32_t carla_read_control( return agent->ReadControl(values, timeout_t::milliseconds(timeout)).value(); } +int32_t carla_write_sensor_data( + CarlaServerPtr self, + const carla_sensor_data &sensor_data) { + CARLA_PROFILE_SCOPE(C_API, WriteSensorData); + auto agent = Cast(self)->GetAgentServer(); + if (agent == nullptr) { + log_debug("trying to write sensor data but agent server is missing"); + return CARLA_SERVER_OPERATION_ABORTED; + } else { + return agent->WriteSensorData(sensor_data).value(); + } +} + int32_t carla_write_measurements( - CarlaServerPtr self, - const carla_measurements &values, - const struct carla_image *images, - const uint32_t number_of_images) { + CarlaServerPtr self, + const carla_measurements &measurements) { CARLA_PROFILE_SCOPE(C_API, WriteMeasurements); auto agent = Cast(self)->GetAgentServer(); if (agent == nullptr) { log_debug("trying to write measurements but agent server is missing"); return CARLA_SERVER_OPERATION_ABORTED; } else { - return agent->WriteMeasurements( - values, - carla::const_array_view(images, number_of_images)).value(); + return agent->WriteMeasurements(measurements).value(); } } diff --git a/Util/CarlaServer/source/carla/server/DoubleBuffer.h b/Util/CarlaServer/source/carla/server/DoubleBuffer.h index a9172e823b9..7f7f49709ba 100644 --- a/Util/CarlaServer/source/carla/server/DoubleBuffer.h +++ b/Util/CarlaServer/source/carla/server/DoubleBuffer.h @@ -92,6 +92,10 @@ namespace detail { return std::unique_ptr(pointer, deleter); } + auto TryMakeReader() { + return TryMakeReader(timeout_t::milliseconds(0u)); + } + /// Returns an unique_ptr to the buffer to be written. The given buffer /// will be locked for writing until the unique_ptr is destroyed. /// diff --git a/Util/CarlaServer/source/carla/server/EncoderServer.h b/Util/CarlaServer/source/carla/server/EncoderServer.h index 70067d7ff2e..f79d9e82d66 100644 --- a/Util/CarlaServer/source/carla/server/EncoderServer.h +++ b/Util/CarlaServer/source/carla/server/EncoderServer.h @@ -6,10 +6,11 @@ #pragma once -#include "carla/NonCopyable.h" #include "carla/Logging.h" +#include "carla/NonCopyable.h" #include "carla/server/CarlaEncoder.h" #include "carla/server/MeasurementsMessage.h" +#include "carla/server/SensorDataInbox.h" #include "carla/server/ServerTraits.h" namespace carla { @@ -58,19 +59,34 @@ namespace server { return _server.Write(boost::asio::buffer(string), timeout); } - /// @warning This operation consists of two Writes, the timeout applies to - /// each individual Write. Effectively, it may wait twice the timeout. + /// @warning This operation consists of several Writes, the timeout applies + /// to each individual Write. Effectively, it may wait the timeout for each + /// sensor. error_code Write(const MeasurementsMessage &values, time_duration timeout) { const auto string = _encoder.Encode(values.measurements()); auto ec = _server.Write(boost::asio::buffer(string), timeout); if (!ec) { - ec = _server.Write(values.images(), timeout); + ec = Write(values.sensor_inbox(), timeout); } return ec; } private: + error_code Write(SensorDataInbox &inbox, time_duration timeout) { + error_code ec; + for (auto &sensor_buffer : inbox) { + auto reader = sensor_buffer.TryMakeReader(); + if (reader != nullptr) { + auto ec = _server.Write(reader->buffer(), timeout); + if (ec) + return ec; + } + } + const uint32_t end_message = 0u; + return _server.Write(boost::asio::buffer(&end_message, sizeof(end_message)), timeout);; + } + error_code ReadString(std::string &string, time_duration timeout) { // Get the message's size. uint32_t message_size; diff --git a/Util/CarlaServer/source/carla/server/Future.h b/Util/CarlaServer/source/carla/server/Future.h index 61c51acd099..fea5205c409 100644 --- a/Util/CarlaServer/source/carla/server/Future.h +++ b/Util/CarlaServer/source/carla/server/Future.h @@ -36,7 +36,7 @@ namespace future { template static inline bool wait_and_get(std::future &future, T &result, timeout_t timeout) { if (is_ready(future, timeout)) { - result = future.get(); + result = std::move(future.get()); return true; } else { return false; diff --git a/Util/CarlaServer/source/carla/server/ImagesMessage.cpp b/Util/CarlaServer/source/carla/server/ImagesMessage.cpp deleted file mode 100644 index 95499b032a6..00000000000 --- a/Util/CarlaServer/source/carla/server/ImagesMessage.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#include "carla/server/ImagesMessage.h" - -#include - -#include "carla/Debug.h" -#include "carla/Logging.h" - -namespace carla { -namespace server { - - static size_t GetSizeOfBuffer(const_array_view images) { - size_t total = 0u; - for (const auto &image : images) { - total += 3u; // width, height, type. - total += image.width * image.height; - } - return total * sizeof(uint32_t); - } - - static size_t WriteSizeToBuffer(unsigned char *buffer, uint32_t size) { - std::memcpy(buffer, &size, sizeof(uint32_t)); - return sizeof(uint32_t); - } - - static size_t WriteImageToBuffer(unsigned char *buffer, const carla_image &image) { - const auto size = sizeof(uint32_t) * image.width * image.height; - DEBUG_ASSERT(image.data != nullptr); - std::memcpy(buffer, image.data, size); - return size; - } - - void ImagesMessage::Write(const_array_view images) { - const size_t buffer_size = GetSizeOfBuffer(images); - Reset(sizeof(uint32_t) + buffer_size); // header + buffer. - - auto begin = _buffer.get(); - begin += WriteSizeToBuffer(begin, buffer_size); - for (const auto &image : images) { - begin += WriteSizeToBuffer(begin, image.width); - begin += WriteSizeToBuffer(begin, image.height); - begin += WriteSizeToBuffer(begin, image.type); - begin += WriteImageToBuffer(begin, image); - } - DEBUG_ASSERT(std::distance(_buffer.get(), begin) == _size); - } - - void ImagesMessage::Reset(const uint32_t count) { - if (_capacity < count) { - log_info("allocating image buffer of", count, "bytes"); - _buffer = std::make_unique(count); - _capacity = count; - } - _size = count; - } - -} // namespace server -} // namespace carla diff --git a/Util/CarlaServer/source/carla/server/ImagesMessage.h b/Util/CarlaServer/source/carla/server/ImagesMessage.h deleted file mode 100644 index 58a6dfb2925..00000000000 --- a/Util/CarlaServer/source/carla/server/ImagesMessage.h +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#pragma once - -#include -#include - -#include "carla/ArrayView.h" -#include "carla/NonCopyable.h" -#include "carla/server/CarlaServerAPI.h" -#include "carla/server/ServerTraits.h" - -namespace carla { -namespace server { - - /// Encodes the given images as binary array to be sent to the client. - /// - /// The message consists of an array of uint32's in the following layout - /// - /// { - /// total size, - /// width, height, type, color[0], color[1],..., <- first image - /// width, height, type, color[0], color[1],..., <- second image - /// ... - /// } - /// - class ImagesMessage : private NonCopyable { - public: - - /// Allocates a new buffer if the capacity is not enough to hold the images, - /// but it does not allocate a smaller one if the capacity is greater than - /// the size of the images. - /// - /// @note The expected usage of this class is to mantain a constant size - /// buffer of images, so memory allocation occurs only once. - void Write(const_array_view images); - - const_buffer buffer() const { - return boost::asio::buffer(_buffer.get(), _size); - } - - private: - - void Reset(uint32_t count); - - std::unique_ptr _buffer = nullptr; - - uint32_t _size = 0u; - - uint32_t _capacity = 0u; - }; - -} // namespace server -} // namespace carla diff --git a/Util/CarlaServer/source/carla/server/MeasurementsMessage.h b/Util/CarlaServer/source/carla/server/MeasurementsMessage.h index bcffc829b06..fae6a7fc45d 100644 --- a/Util/CarlaServer/source/carla/server/MeasurementsMessage.h +++ b/Util/CarlaServer/source/carla/server/MeasurementsMessage.h @@ -6,37 +6,40 @@ #pragma once +#include "carla/Debug.h" #include "carla/NonCopyable.h" #include "carla/server/CarlaMeasurements.h" #include "carla/server/CarlaServerAPI.h" -#include "carla/server/ImagesMessage.h" namespace carla { namespace server { + class SensorDataInbox; + class MeasurementsMessage : private NonCopyable { public: void Write( const carla_measurements &measurements, - const_array_view images) { + SensorDataInbox &sensor_inbox) { _measurements.Write(measurements); - _images.Write(images); + _sensor_inbox = &sensor_inbox; } const carla_measurements &measurements() const { return _measurements.measurements(); } - const_buffer images() const { - return _images.buffer(); + SensorDataInbox &sensor_inbox() const { + DEBUG_ASSERT(_sensor_inbox != nullptr); + return *_sensor_inbox; } private: CarlaMeasurements _measurements; - ImagesMessage _images; + SensorDataInbox *_sensor_inbox = nullptr; }; } // namespace server diff --git a/Util/CarlaServer/source/carla/server/RequestNewEpisode.h b/Util/CarlaServer/source/carla/server/RequestNewEpisode.h index 0e615ffbd83..2d463e2a5bf 100644 --- a/Util/CarlaServer/source/carla/server/RequestNewEpisode.h +++ b/Util/CarlaServer/source/carla/server/RequestNewEpisode.h @@ -17,7 +17,8 @@ namespace server { /// ownership of data throught the C interface we need to hold it internally. /// The data is hold in memory until the next call to /// carla_read_request_new_episode(). - struct RequestNewEpisode { + class RequestNewEpisode { + public: carla_request_new_episode values; std::unique_ptr data; }; diff --git a/Util/CarlaServer/source/carla/server/SensorDataInbox.h b/Util/CarlaServer/source/carla/server/SensorDataInbox.h new file mode 100644 index 00000000000..67cd6c12ac2 --- /dev/null +++ b/Util/CarlaServer/source/carla/server/SensorDataInbox.h @@ -0,0 +1,103 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "carla/Debug.h" +#include "carla/NonCopyable.h" +#include "carla/server/DoubleBuffer.h" +#include "carla/server/SensorDataMessage.h" + +#include +#include +#include + +struct carla_sensor_data; +struct carla_sensor_definition; + +namespace carla { +namespace server { + +namespace detail { + + template + class value_iterator { + public: + + value_iterator(IT original_iterator) : _it(original_iterator) {} + + value_iterator &operator++() { + ++_it; + return *this; + } + + auto &operator*() { + return _it->second; + } + + bool operator!=(value_iterator rhs) { + return _it != rhs._it; + } + + private: + + IT _it; + }; + +} // detail + + /// Stores the data received from the sensors (asynchronously) to be sent next + /// on next tick. + /// + /// Each sensor has a double-buffer for one producer and one consumer per + /// sensor. Several threads can simultaneously write as long as they write to + /// different buffers, i.e. each sensor can have its own producer and consumer + /// threads. + class SensorDataInbox : private NonCopyable { + + using DataBuffer = DoubleBuffer; + + using Map = std::unordered_map; + + public: + + using Sensors = std::vector; + + using buffer_iterator = detail::value_iterator; + + explicit SensorDataInbox(const Sensors &sensors) { + // We need to initialize the map before hand so it remains constant and + // doesn't need a lock. + for (auto &sensor : sensors) + _buffers[sensor.id]; + } + + void Write(const carla_sensor_data &data) { + auto writer = _buffers.at(data.id).MakeWriter(); + writer->Write(data); + } + + /// Tries to acquire a reader on the buffer of the given sensor. See + /// DoubleBuffer. + auto TryMakeReader(uint32_t sensor_id) { + return _buffers.at(sensor_id).TryMakeReader(); + } + + buffer_iterator begin() { + return _buffers.begin(); + } + + buffer_iterator end() { + return _buffers.end(); + } + + private: + + Map _buffers; + }; + +} // namespace server +} // namespace carla diff --git a/Util/CarlaServer/source/carla/server/SensorDataMessage.cpp b/Util/CarlaServer/source/carla/server/SensorDataMessage.cpp new file mode 100644 index 00000000000..7cafa557903 --- /dev/null +++ b/Util/CarlaServer/source/carla/server/SensorDataMessage.cpp @@ -0,0 +1,48 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "carla/server/SensorDataMessage.h" + +#include "carla/Logging.h" +#include "carla/server/CarlaServerAPI.h" + +namespace carla { +namespace server { + + void SensorDataMessage::Write(const carla_sensor_data &data) { + // The buffer contains id + data-header + data. + const uint32_t buffer_size = + sizeof(uint32_t) + + data.header_size + + data.data_size; + // The message is prepended by the size of the buffer. + Reset(sizeof(uint32_t) + buffer_size); + + auto begin = _buffer.get(); + + std::memcpy(begin, &buffer_size, sizeof(uint32_t)); + begin += sizeof(uint32_t); + + std::memcpy(begin, &data.id, sizeof(uint32_t)); + begin += sizeof(uint32_t); + + std::memcpy(begin, data.header, data.header_size); + begin += data.header_size; + + std::memcpy(begin, data.data, data.data_size); + } + + void SensorDataMessage::Reset(uint32_t count) { + if (_capacity < count) { + log_debug("allocating sensor buffer of", count, "bytes"); + _buffer = std::make_unique(count); + _capacity = count; + } + _size = count; + } + +} // namespace server +} // namespace carla diff --git a/Util/CarlaServer/source/carla/server/SensorDataMessage.h b/Util/CarlaServer/source/carla/server/SensorDataMessage.h new file mode 100644 index 00000000000..f29527d0b48 --- /dev/null +++ b/Util/CarlaServer/source/carla/server/SensorDataMessage.h @@ -0,0 +1,41 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "carla/NonCopyable.h" +#include "carla/server/ServerTraits.h" + +#include + +struct carla_sensor_data; +struct carla_sensor_definition; + +namespace carla { +namespace server { + + class SensorDataMessage : private NonCopyable { + public: + + void Write(const carla_sensor_data &data); + + const_buffer buffer() const { + return boost::asio::buffer(_buffer.get(), _size); + } + + private: + + void Reset(uint32_t count); + + std::unique_ptr _buffer = nullptr; + + uint32_t _size = 0u; + + uint32_t _capacity = 0u; + }; + +} // namespace server +} // namespace carla diff --git a/Util/CarlaServer/source/carla/server/WorldServer.cpp b/Util/CarlaServer/source/carla/server/WorldServer.cpp index 55377af82ee..8fb8b6bdb58 100644 --- a/Util/CarlaServer/source/carla/server/WorldServer.cpp +++ b/Util/CarlaServer/source/carla/server/WorldServer.cpp @@ -39,10 +39,10 @@ namespace server { return reading.error_code; } - template - static std::future Write(WriteTask &task, const T &value) { + template ::type> + static std::future Write(WriteTask &task, T &&value) { DEBUG_ASSERT(task.valid()); - task.set_message(value); + task.set_message(std::forward(value)); return task.ReleaseResult(); } @@ -91,7 +91,13 @@ namespace server { std::future WorldServer::Write( const carla_scene_description &scene_description) { - return carla::server::Write(_protocol.scene_description, scene_description); + /// @todo Here sensor names are not copied and will be invalidated. + decltype(_sensor_definitions) defs( + scene_description.sensors, + scene_description.sensors + scene_description.number_of_sensors); + _sensor_definitions = std::move(defs); + CarlaSceneDescription scene(_encoder.Encode(scene_description)); + return carla::server::Write(_protocol.scene_description, std::move(scene)); } error_code WorldServer::TryRead( @@ -106,11 +112,17 @@ namespace server { } void WorldServer::StartAgentServer() { - _agent_server = std::make_unique(_encoder, _port + 1u, _port + 2u, _timeout); + _agent_server = std::make_unique( + _encoder, + _port + 1u, + _port + 2u, + _sensor_definitions, + _timeout); } void WorldServer::KillAgentServer() { _agent_server = nullptr; + _sensor_definitions.clear(); } void WorldServer::ResetProtocol() { diff --git a/Util/CarlaServer/source/carla/server/WorldServer.h b/Util/CarlaServer/source/carla/server/WorldServer.h index 87cfeb1456a..bd9fc24b060 100644 --- a/Util/CarlaServer/source/carla/server/WorldServer.h +++ b/Util/CarlaServer/source/carla/server/WorldServer.h @@ -9,7 +9,9 @@ #include "carla/NonCopyable.h" #include "carla/server/AsyncServer.h" #include "carla/server/CarlaEncoder.h" +#include "carla/server/CarlaSceneDescription.h" #include "carla/server/EncoderServer.h" +#include "carla/server/RequestNewEpisode.h" #include "carla/server/TCPServer.h" namespace carla { @@ -58,7 +60,7 @@ namespace server { explicit Protocol(time_duration timeout); ReadTask request_new_episode; - WriteTask scene_description; + WriteTask scene_description; ReadTask episode_start; WriteTask episode_ready; }; @@ -75,6 +77,8 @@ namespace server { AsyncServer> _world_server; + std::vector _sensor_definitions; + std::unique_ptr _agent_server; RequestNewEpisode _new_episode_data; diff --git a/Util/CarlaServer/source/test/Sensor.cpp b/Util/CarlaServer/source/test/Sensor.cpp new file mode 100644 index 00000000000..ef121338b23 --- /dev/null +++ b/Util/CarlaServer/source/test/Sensor.cpp @@ -0,0 +1,49 @@ +#include "Sensor.h" + +#include + +#include + +namespace test { + + static uint32_t ID_COUNT = 0u; + + Sensor::Sensor() : Sensor(++ID_COUNT) {} + + Sensor::Sensor(const uint32_t id) + : _name(std::string("Sensor") + std::to_string(id)), + _definition({id, 0u, _name.c_str()}), + _data({id, nullptr, 0u, nullptr, 0u}) {} + + carla_sensor_data Sensor::MakeRandomData() { + std::lock_guard lock(_mutex); + std::random_device device; + std::default_random_engine rng(device()); + std::uniform_int_distribution dist(1, 10000); + + _data.header_size = dist(rng); + _data.data_size = dist(rng); + + _header = std::make_unique(_data.header_size); + _buffer = std::make_unique(_data.data_size); + + _data.header = _header.get(); + _data.data = _buffer.get(); + + return _data; + } + + void Sensor::CheckData(boost::asio::const_buffer buffer) const { + std::lock_guard lock(_mutex); + const auto size = boost::asio::buffer_size(buffer); + const auto begin = boost::asio::buffer_cast(buffer); + const auto header_begin = begin + 2u * sizeof(uint32_t); + const auto data_begin = header_begin + _data.header_size; + + const auto expected_size = 2u * sizeof(uint32_t) + _data.header_size + _data.data_size; + ASSERT_EQ(size, expected_size); + ASSERT_EQ(0, std::memcmp(header_begin, _header.get(), _data.header_size)); + ASSERT_EQ(0, std::memcmp(data_begin, _buffer.get(), _data.data_size)); + } + +} // test diff --git a/Util/CarlaServer/source/test/Sensor.h b/Util/CarlaServer/source/test/Sensor.h new file mode 100644 index 00000000000..745cc158764 --- /dev/null +++ b/Util/CarlaServer/source/test/Sensor.h @@ -0,0 +1,50 @@ +#pragma once + +#include + +#include "carla/server/ServerTraits.h" + +#include + +#include +#include +#include + +namespace test { + + /// A class for testing usage of sensor data. + class Sensor { + public: + + Sensor(); + + uint32_t id() const { + return _definition.id; + } + + const carla_sensor_definition &definition() const { + return _definition; + } + + carla_sensor_data MakeRandomData(); + + void CheckData(boost::asio::const_buffer buffer) const; + + private: + + Sensor(uint32_t id); + + mutable std::mutex _mutex; + + const std::string _name; + + const carla_sensor_definition _definition; + + std::unique_ptr _header; + + std::unique_ptr _buffer; + + carla_sensor_data _data; + }; + +} // test diff --git a/Util/CarlaServer/source/test/Test_CarlaServer.cpp b/Util/CarlaServer/source/test/Test_CarlaServer.cpp index ffb37f0935c..9d8b0bb7a72 100644 --- a/Util/CarlaServer/source/test/Test_CarlaServer.cpp +++ b/Util/CarlaServer/source/test/Test_CarlaServer.cpp @@ -1,6 +1,5 @@ #include #include -#include #include @@ -9,6 +8,8 @@ #include "carla/Logging.h" #include "carla/server/ServerTraits.h" +#include "Sensor.h" + using namespace carla::server; using namespace boost::posix_time; @@ -49,12 +50,13 @@ TEST(CarlaServerAPI, SimBlocking) { CarlaServerPtr CarlaServer = CarlaServerGuard.get(); ASSERT_TRUE(CarlaServer != nullptr); - constexpr uint32_t ImageSizeX = 300u; - constexpr uint32_t ImageSizeY = 200u; - const uint32_t image0[ImageSizeX*ImageSizeY] = {0u}; - const carla_image images[] = { - {ImageSizeX, ImageSizeY, 1u, image0} - }; + std::array sensors; + + carla_sensor_definition sensor_definitions[sensors.size()]; + + for (auto i = 0u; i < sensors.size(); ++i) { + sensor_definitions[i] = sensors[i].definition(); + } const carla_transform start_locations[] = { {carla_vector3d{0.0f, 0.0f, 0.0f}, carla_vector3d{0.0f, 0.0f, 0.0f}, carla_rotation3d{0.0f, 0.0f, 0.0f}}, @@ -83,7 +85,9 @@ TEST(CarlaServerAPI, SimBlocking) { test_log("sending scene description..."); const carla_scene_description values{ start_locations, - SIZE_OF_ARRAY(start_locations)}; + SIZE_OF_ARRAY(start_locations), + sensor_definitions, + SIZE_OF_ARRAY(sensor_definitions)}; ASSERT_EQ(S, carla_write_scene_description(CarlaServer, values, TIMEOUT)); } { @@ -100,13 +104,15 @@ TEST(CarlaServerAPI, SimBlocking) { std::array agents_data; std::atomic_bool done{false}; - auto agent_thread_result = std::async(std::launch::async, [&](){ + + // Simulate game thread. + auto game_thread_result = std::async(std::launch::async, [&](){ while (!done) { { carla_measurements measurements; measurements.non_player_agents = agents_data.data(); measurements.number_of_non_player_agents = agents_data.size(); - auto ec = carla_write_measurements(CarlaServer, measurements, images, SIZE_OF_ARRAY(images)); + auto ec = carla_write_measurements(CarlaServer, measurements); if (ec != S) break; } @@ -121,6 +127,16 @@ TEST(CarlaServerAPI, SimBlocking) { } }); + // Simulate render thread. + auto render_thread_result = std::async(std::launch::async, [&](){ + while (!done) { + for (auto &sensor : sensors) { + carla_write_sensor_data(CarlaServer, sensor.MakeRandomData()); + std::this_thread::sleep_for(std::chrono::microseconds(150)); + } + }; + }); + for (;;) { carla_request_new_episode new_episode; auto ec = carla_read_request_new_episode(CarlaServer, new_episode, 0); @@ -134,7 +150,8 @@ TEST(CarlaServerAPI, SimBlocking) { } test_log("waiting for async's future"); - agent_thread_result.get(); + game_thread_result.get(); + render_thread_result.get(); } test_log("###### End Test ######"); } diff --git a/Util/CarlaServer/source/test/Test_SensorDataInbox.cpp b/Util/CarlaServer/source/test/Test_SensorDataInbox.cpp new file mode 100644 index 00000000000..dfdbc1ce964 --- /dev/null +++ b/Util/CarlaServer/source/test/Test_SensorDataInbox.cpp @@ -0,0 +1,90 @@ +#include + +#include + +#include + +#include "carla/server/SensorDataInbox.h" + +#include "Sensor.h" + +#include +#include +#include + +TEST(SensorDataInbox, SyncSingleSensor) { + using namespace carla::server; + test::Sensor sensor0; + SensorDataInbox::Sensors defs; + defs.push_back(sensor0.definition()); + SensorDataInbox inbox(defs); + for (auto j = 0u; j < 1000u; ++j) { + auto data = sensor0.MakeRandomData(); + inbox.Write(data); + auto buffer = (*inbox.begin()).TryMakeReader(); + ASSERT_TRUE(buffer != nullptr); + sensor0.CheckData(buffer->buffer()); + } +} + +TEST(SensorDataInbox, SyncMultipleSensors) { + using namespace carla::server; + std::array sensors; + SensorDataInbox::Sensors defs; + defs.reserve(sensors.size()); + std::for_each(sensors.begin(), sensors.end(), [&](auto &s){ + defs.push_back(s.definition()); + }); + SensorDataInbox inbox(defs); + for (auto j = 0u; j < 1000u; ++j) { + for (auto &sensor : sensors) { + inbox.Write(sensor.MakeRandomData()); + auto buffer = inbox.TryMakeReader(sensor.id()); + ASSERT_TRUE(buffer != nullptr); + sensor.CheckData(buffer->buffer()); + } + } +} + +TEST(SensorDataInbox, Async) { + using namespace carla::server; + std::array sensors; + SensorDataInbox::Sensors defs; + defs.reserve(sensors.size()); + std::for_each(sensors.begin(), sensors.end(), [&](auto &s){ + defs.push_back(s.definition()); + }); + SensorDataInbox inbox(defs); + + constexpr auto numberOfWrites = 200u; + + std::atomic_bool done{false}; + + auto result_writer = std::async(std::launch::async, [&](){ + for (size_t i = 0u; i < numberOfWrites; ++i) { + for (auto &sensor : sensors) { + inbox.Write(sensor.MakeRandomData()); + } + std::this_thread::sleep_for(std::chrono::milliseconds(10u)); + }; + std::this_thread::sleep_for(std::chrono::milliseconds(200u)); + done = true; + }); + + auto result_reader = std::async(std::launch::async, [&](){ + auto readings = 0u; + while (!done && (readings < numberOfWrites * sensors.size())) { + for (auto &sensor : sensors) { + auto buffer = inbox.TryMakeReader(sensor.id()); + if (buffer != nullptr) { + sensor.CheckData(buffer->buffer()); + ++readings; + } + } + } + ASSERT_EQ(numberOfWrites * sensors.size(), readings); + }); + + result_reader.get(); + result_writer.get(); +} diff --git a/Util/CarlaServer/source/test/Test_TCPServer.cpp b/Util/CarlaServer/source/test/Test_TCPServer.cpp index ef33951c754..26dd5a39563 100644 --- a/Util/CarlaServer/source/test/Test_TCPServer.cpp +++ b/Util/CarlaServer/source/test/Test_TCPServer.cpp @@ -1,5 +1,3 @@ -#include - #include #include diff --git a/Util/Proto/carla_server.proto b/Util/Proto/carla_server.proto index 1ce3a8296cc..348c165b604 100644 --- a/Util/Proto/carla_server.proto +++ b/Util/Proto/carla_server.proto @@ -32,6 +32,26 @@ message Transform { Rotation3D rotation = 3; } +// ============================================================================= +// -- Sensors ------------------------------------------------------------------ +// ============================================================================= + +message Sensor { + enum Type { + UNKNOWN = 0; + CAMERA = 1; + LIDAR_RAY_TRACE = 2; + } + + fixed32 id = 1; + Type type = 2; + string name = 3; +} + +// ============================================================================= +// -- Agents ------------------------------------------------------------------- +// ============================================================================= + message Vehicle { Transform transform = 1; Vector3D box_extent = 2; @@ -79,6 +99,7 @@ message RequestNewEpisode { message SceneDescription { repeated Transform player_start_spots = 1; + repeated Sensor sensors = 2; } message EpisodeStart {