CARLA
WorldObserver.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #include "Carla.h"
10 
18 
19 #include "CoreGlobals.h"
20 
22 #include <carla/rpc/String.h>
26 
27 static auto FWorldObserver_GetActorState(const FCarlaActor &View, const FActorRegistry &Registry)
28 {
29  using AType = FCarlaActor::ActorType;
30 
32 
33  if (AType::Vehicle == View.GetActorType())
34  {
35  auto Vehicle = Cast<ACarlaWheeledVehicle>(View.GetActor());
36  if (Vehicle != nullptr)
37  {
38  state.vehicle_data.control = carla::rpc::VehicleControl{Vehicle->GetVehicleControl()};
39  auto Controller = Cast<AWheeledVehicleAIController>(Vehicle->GetController());
40  if (Controller != nullptr)
41  {
43  state.vehicle_data.traffic_light_state = static_cast<TLS>(Controller->GetTrafficLightState());
44  state.vehicle_data.speed_limit = Controller->GetSpeedLimit();
45  auto TrafficLight = Controller->GetTrafficLight();
46  if (TrafficLight != nullptr)
47  {
48  state.vehicle_data.has_traffic_light = true;
49  auto* TrafficLightView = Registry.FindCarlaActor(TrafficLight);
50  if(TrafficLightView)
51  {
52  state.vehicle_data.traffic_light_id = TrafficLightView->GetActorId();
53  }
54  else
55  {
56  state.vehicle_data.has_traffic_light = false;
57  }
58  }
59  else
60  {
61  state.vehicle_data.has_traffic_light = false;
62  }
63  }
64  // Get the failure state by checking the rollover one as it is the only one currently implemented.
65  // This will have to be expanded once more states are added
66  state.vehicle_data.failure_state = Vehicle->GetFailureState();
67  }
68  }
69 
70  else if (AType::Walker == View.GetActorType())
71  {
72  auto Walker = Cast<APawn>(View.GetActor());
73  auto Controller = Walker != nullptr ? Cast<AWalkerController>(Walker->GetController()) : nullptr;
74  if (Controller != nullptr)
75  {
76  state.walker_control = carla::rpc::WalkerControl{Controller->GetWalkerControl()};
77  }
78  }
79  else if (AType::TrafficLight == View.GetActorType())
80  {
81  auto TrafficLight = Cast<ATrafficLightBase>(View.GetActor());
82  if (TrafficLight != nullptr)
83  {
84  auto* TrafficLightComponent =
85  TrafficLight->GetTrafficLightComponent();
86 
88 
89  if(TrafficLightComponent == nullptr)
90  {
91  // Old way: traffic lights are actors
92  state.traffic_light_data.sign_id[0] = '\0';
93  state.traffic_light_data.state = static_cast<TLS>(TrafficLight->GetTrafficLightState());
94  state.traffic_light_data.green_time = TrafficLight->GetGreenTime();
95  state.traffic_light_data.yellow_time = TrafficLight->GetYellowTime();
96  state.traffic_light_data.red_time = TrafficLight->GetRedTime();
97  state.traffic_light_data.elapsed_time = TrafficLight->GetElapsedTime();
98  state.traffic_light_data.time_is_frozen = TrafficLight->GetTimeIsFrozen();
99  state.traffic_light_data.pole_index = TrafficLight->GetPoleIndex();
100  }
101  else
102  {
103  const UTrafficLightController* Controller = TrafficLightComponent->GetController();
104  const ATrafficLightGroup* Group = TrafficLightComponent->GetGroup();
105 
106  if (!Controller)
107  {
108  UE_LOG(LogCarla, Error, TEXT("TrafficLightComponent doesn't have any Controller assigned"));
109  }
110  else if (!Group)
111  {
112  UE_LOG(LogCarla, Error, TEXT("TrafficLightComponent doesn't have any Group assigned"));
113  }
114  else
115  {
116  const FString fstring_sign_id = TrafficLightComponent->GetSignId();
117  const std::string sign_id = carla::rpc::FromFString(fstring_sign_id);
118  constexpr size_t max_size = sizeof(state.traffic_light_data.sign_id);
119  size_t sign_id_length = sign_id.length();
120  if(max_size < sign_id_length)
121  {
122  UE_LOG(LogCarla, Warning, TEXT("The max size of a signal id is 32. %s (%d)"), *fstring_sign_id, sign_id.length());
123  sign_id_length = max_size;
124  }
125  std::memset(state.traffic_light_data.sign_id, '\0', max_size);
126  std::memcpy(state.traffic_light_data.sign_id, sign_id.c_str(), sign_id_length);
127  state.traffic_light_data.state = static_cast<TLS>(TrafficLightComponent->GetLightState());
128  state.traffic_light_data.green_time = Controller->GetGreenTime();
129  state.traffic_light_data.yellow_time = Controller->GetYellowTime();
130  state.traffic_light_data.red_time = Controller->GetRedTime();
131  state.traffic_light_data.elapsed_time = Controller->GetElapsedTime();
132  state.traffic_light_data.time_is_frozen = Group->IsFrozen();
133  state.traffic_light_data.pole_index = TrafficLight->GetPoleIndex();
134  }
135  }
136  }
137  }
138  else if (AType::TrafficSign == View.GetActorType())
139  {
140  auto TrafficSign = Cast<ATrafficSignBase>(View.GetActor());
141  if (TrafficSign != nullptr)
142  {
143  USignComponent* TrafficSignComponent =
144  Cast<USignComponent>(TrafficSign->FindComponentByClass<USignComponent>());
145 
146  if(TrafficSignComponent)
147  {
148  const FString fstring_sign_id = TrafficSignComponent->GetSignId();
149  const std::string sign_id = carla::rpc::FromFString(fstring_sign_id);
150  constexpr size_t max_size = sizeof(state.traffic_sign_data.sign_id);
151  size_t sign_id_length = sign_id.length();
152  if(max_size < sign_id_length)
153  {
154  UE_LOG(LogCarla, Warning, TEXT("The max size of a signal id is 32. %s (%d)"), *fstring_sign_id, sign_id.length());
155  sign_id_length = max_size;
156  }
157  std::memset(state.traffic_light_data.sign_id, '\0', max_size);
158  std::memcpy(state.traffic_sign_data.sign_id, sign_id.c_str(), sign_id_length);
159  }
160  }
161  }
162  return state;
163 }
164 
165 static auto FWorldObserver_GetDormantActorState(const FCarlaActor &View, const FActorRegistry &Registry)
166 {
167  using AType = FCarlaActor::ActorType;
168 
170 
171  if (AType::Vehicle == View.GetActorType())
172  {
173  const FVehicleData* ActorData = View.GetActorData<FVehicleData>();
174  state.vehicle_data.control = carla::rpc::VehicleControl{ActorData->Control};
176  state.vehicle_data.traffic_light_state = TLS::Green;
177  state.vehicle_data.speed_limit = ActorData->SpeedLimit;
178  state.vehicle_data.has_traffic_light = false;
179  }
180  else if (AType::Walker == View.GetActorType())
181  {
182  const FWalkerData* ActorData = View.GetActorData<FWalkerData>();
183  // auto Walker = Cast<APawn>(View.GetActor());
184  // auto Controller = Walker != nullptr ? Cast<AWalkerController>(Walker->GetController()) : nullptr;
185  // if (Controller != nullptr)
186  // {
187  // state.walker_control = carla::rpc::WalkerControl{Controller->GetWalkerControl()};
188  // }
189  state.walker_control = ActorData->WalkerControl;
190  }
191  else if (AType::TrafficLight == View.GetActorType())
192  {
193  const FTrafficLightData* ActorData = View.GetActorData<FTrafficLightData>();
194  const UTrafficLightController* Controller = ActorData->Controller;
195  if(Controller)
196  {
198  const ATrafficLightGroup* Group = Controller->GetGroup();
199  if(!Group)
200  {
201  UE_LOG(LogCarla, Error, TEXT("TrafficLight doesn't have any Group assigned"));
202  }
203  else
204  {
205  const FString fstring_sign_id = ActorData->SignId;
206  const std::string sign_id = carla::rpc::FromFString(fstring_sign_id);
207  constexpr size_t max_size = sizeof(state.traffic_light_data.sign_id);
208  size_t sign_id_length = sign_id.length();
209  if(max_size < sign_id_length)
210  {
211  UE_LOG(LogCarla, Warning, TEXT("The max size of a signal id is 32. %s (%d)"), *fstring_sign_id, sign_id.length());
212  sign_id_length = max_size;
213  }
214  std::memset(state.traffic_light_data.sign_id, '\0', max_size);
215  std::memcpy(state.traffic_light_data.sign_id, sign_id.c_str(), sign_id_length);
216  state.traffic_light_data.state = static_cast<TLS>(Controller->GetCurrentState().State);
217  state.traffic_light_data.green_time = Controller->GetGreenTime();
218  state.traffic_light_data.yellow_time = Controller->GetYellowTime();
219  state.traffic_light_data.red_time = Controller->GetRedTime();
220  state.traffic_light_data.elapsed_time = Controller->GetElapsedTime();
221  state.traffic_light_data.time_is_frozen = Group->IsFrozen();
222  state.traffic_light_data.pole_index = ActorData->PoleIndex;
223  }
224  }
225  }
226  else if (AType::TrafficSign == View.GetActorType())
227  {
228  const FTrafficSignData* ActorData = View.GetActorData<FTrafficSignData>();
229  const FString fstring_sign_id = ActorData->SignId;
230  const std::string sign_id = carla::rpc::FromFString(fstring_sign_id);
231  constexpr size_t max_size = sizeof(state.traffic_sign_data.sign_id);
232  size_t sign_id_length = sign_id.length();
233  if(max_size < sign_id_length)
234  {
235  UE_LOG(LogCarla, Warning, TEXT("The max size of a signal id is 32. %s (%d)"), *fstring_sign_id, sign_id.length());
236  sign_id_length = max_size;
237  }
238  std::memset(state.traffic_light_data.sign_id, '\0', max_size);
239  std::memcpy(state.traffic_sign_data.sign_id, sign_id.c_str(), sign_id_length);
240  }
241  return state;
242 }
243 
245 {
246  const auto RootComponent = Cast<UPrimitiveComponent>(Actor.GetRootComponent());
247  const FVector AngularVelocity =
248  RootComponent != nullptr ?
249  RootComponent->GetPhysicsAngularVelocityInDegrees() :
250  FVector{0.0f, 0.0f, 0.0f};
251  return {AngularVelocity.X, AngularVelocity.Y, AngularVelocity.Z};
252 }
253 
255  const FCarlaActor &View,
256  const FVector &Velocity,
257  const float DeltaSeconds)
258 {
259  FVector &PreviousVelocity = View.GetActorInfo()->Velocity;
260  const FVector Acceleration = (Velocity - PreviousVelocity) / DeltaSeconds;
261  PreviousVelocity = Velocity;
262  return {Acceleration.X, Acceleration.Y, Acceleration.Z};
263 }
264 
266  carla::Buffer &&buffer,
267  const UCarlaEpisode &Episode,
268  float DeltaSeconds,
269  bool MapChange,
270  bool PendingLightUpdates)
271 {
272  TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
275  using ActorDynamicState = carla::sensor::data::ActorDynamicState;
276 
277 
278  const FActorRegistry &Registry = Episode.GetActorRegistry();
279 
280  auto total_size = sizeof(Serializer::Header) + sizeof(ActorDynamicState) * Registry.Num();
281  auto current_size = 0;
282  // Set up buffer for writing.
283  buffer.reset(total_size);
284  auto write_data = [&current_size, &buffer](const auto &data)
285  {
286  auto begin = buffer.begin() + current_size;
287  std::memcpy(begin, &data, sizeof(data));
288  current_size += sizeof(data);
289  };
290 
291  constexpr float TO_METERS = 1e-2;
292 
293  // Write header.
294  Serializer::Header header;
295  header.episode_id = Episode.GetId();
296  header.platform_timestamp = FPlatformTime::Seconds();
297  header.delta_seconds = DeltaSeconds;
298  FIntVector MapOrigin = Episode.GetCurrentMapOrigin();
299  FIntVector MapOriginInMeters = MapOrigin / 100;
300  header.map_origin = carla::geom::Vector3DInt{ MapOriginInMeters.X, MapOriginInMeters.Y, MapOriginInMeters.Z };
301 
302  uint8_t simulation_state = (SimulationState::MapChange * MapChange);
303  simulation_state |= (SimulationState::PendingLightUpdate * PendingLightUpdates);
304 
305  header.simulation_state = static_cast<SimulationState>(simulation_state);
306 
307  write_data(header);
308 
309  // Write every actor.
310  for (auto& It : Registry)
311  {
312  const FCarlaActor* View = It.Value.Get();
313  const FActorInfo* ActorInfo = View->GetActorInfo();
314 
315  FTransform ActorTransform;
316  FVector Velocity(0.0f);
317  carla::geom::Vector3D AngularVelocity(0.0f, 0.0f, 0.0f);
318  carla::geom::Vector3D Acceleration(0.0f, 0.0f, 0.0f);
320 
321 
322  check(View);
323 
324  if(View->IsDormant())
325  {
326  const FActorData* ActorData = View->GetActorData();
327  Velocity = TO_METERS * ActorData->Velocity;
328  AngularVelocity = carla::geom::Vector3D
329  {ActorData->AngularVelocity.X,
330  ActorData->AngularVelocity.Y,
331  ActorData->AngularVelocity.Z};
332  Acceleration = FWorldObserver_GetAcceleration(*View, Velocity, DeltaSeconds);
333  State = FWorldObserver_GetDormantActorState(*View, Registry);
334  }
335  else
336  {
337  Velocity = TO_METERS * View->GetActor()->GetVelocity();
338  AngularVelocity = FWorldObserver_GetAngularVelocity(*View->GetActor());
339  Acceleration = FWorldObserver_GetAcceleration(*View, Velocity, DeltaSeconds);
340  State = FWorldObserver_GetActorState(*View, Registry);
341  }
342  ActorTransform = View->GetActorGlobalTransform();
343 
344  ActorDynamicState info = {
345  View->GetActorId(),
346  View->GetActorState(),
347  carla::geom::Transform(ActorTransform),
348  carla::geom::Vector3D(Velocity.X, Velocity.Y, Velocity.Z),
349  AngularVelocity,
350  Acceleration,
351  State,
352  };
353  write_data(info);
354  }
355 
356  // Shrink buffer
357  buffer.resize(current_size);
358 
359  check(buffer.size() == current_size);
360 
361  return std::move(buffer);
362 }
363 
365  const UCarlaEpisode &Episode,
366  float DeltaSecond,
367  bool MapChange,
368  bool PendingLightUpdates)
369 {
370  TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
371  auto AsyncStream = Stream.MakeAsyncDataStream(*this, Episode.GetElapsedGameTime());
372 
374  AsyncStream.PopBufferFromPool(),
375  Episode,
376  DeltaSecond,
377  MapChange,
378  PendingLightUpdates);
379 
380  AsyncStream.Send(*this, std::move(buffer));
381 }
Dynamic state of an actor at a certain frame.
FVector Velocity
Definition: ActorInfo.h:35
A registry of all the Carla actors.
Definition: ActorRegistry.h:20
carla::rpc::ActorState GetActorState() const
Definition: CarlaActor.h:105
FDataMultiStream Stream
Definition: WorldObserver.h:49
void BroadcastTick(const UCarlaEpisode &Episode, float DeltaSeconds, bool MapChange, bool PendingLightUpdate)
Send a message to every connected client with the info about the given Episode.
AActor * GetActor()
Definition: CarlaActor.h:90
Serializes the current state of the whole episode.
carla::rpc::WalkerControl WalkerControl
Definition: ActorData.h:83
double GetElapsedGameTime() const
Game seconds since the start of this episode.
Definition: CarlaEpisode.h:97
Class which implements the state changing of traffic lights.
static auto FWorldObserver_GetDormantActorState(const FCarlaActor &View, const FActorRegistry &Registry)
auto MakeAsyncDataStream(const SensorT &Sensor, double Timestamp)
Create a FAsyncDataStream object.
Definition: DataStream.h:40
static carla::geom::Vector3D FWorldObserver_GetAcceleration(const FCarlaActor &View, const FVector &Velocity, const float DeltaSeconds)
FVector Velocity
Definition: ActorData.h:37
FVehicleControl Control
Definition: ActorData.h:60
static carla::Buffer FWorldObserver_Serialize(carla::Buffer &&buffer, const UCarlaEpisode &Episode, float DeltaSeconds, bool MapChange, bool PendingLightUpdates)
float SpeedLimit
Definition: ActorData.h:70
int32 Num() const
Definition: ActorRegistry.h:54
geom::Vector3D Vector3D
Definition: rpc/Vector3D.h:14
const FActorRegistry & GetActorRegistry() const
Definition: CarlaEpisode.h:136
FActorData * GetActorData()
Definition: CarlaActor.h:156
carla::SharedPtr< cc::Actor > Actor
A simulation episode.
Definition: CarlaEpisode.h:36
auto GetId() const
Return the unique id of this episode.
Definition: CarlaEpisode.h:84
FVector AngularVelocity
Definition: ActorData.h:39
const FActorInfo * GetActorInfo() const
Definition: CarlaActor.h:100
FIntVector GetCurrentMapOrigin() const
Definition: CarlaEpisode.h:295
A piece of raw data.
Definition: carla/Buffer.h:41
A view over an actor and its properties.
Definition: ActorInfo.h:22
FTransform GetActorGlobalTransform() const
Definition: CarlaActor.cpp:198
bool IsDormant() const
Definition: CarlaActor.h:70
static carla::geom::Vector3D FWorldObserver_GetAngularVelocity(const AActor &Actor)
IdType GetActorId() const
Definition: CarlaActor.h:80
carla::rpc::TrafficLightState TLS
FCarlaActor * FindCarlaActor(IdType Id)
Definition: ActorRegistry.h:69
FString SignId
Definition: ActorData.h:95
Maps a controller from OpenDrive.
UTrafficLightController * Controller
Definition: ActorData.h:112
static auto FWorldObserver_GetActorState(const FCarlaActor &View, const FActorRegistry &Registry)
geom::Transform Transform
Definition: rpc/Transform.h:16
ActorType GetActorType() const
Definition: CarlaActor.h:85
A view over an actor and its properties.
Definition: CarlaActor.h:23