CARLA
CarlaReplayer.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 "CarlaReplayer.h"
8 #include "CarlaRecorder.h"
10 
11 #include <ctime>
12 #include <sstream>
13 
14 // structure to save replaying info when need to load a new map (static member by now)
15 CarlaReplayer::PlayAfterLoadMap CarlaReplayer::Autoplay { false, "", "", 0.0, 0.0, 0, 1.0, false };
16 
17 void CarlaReplayer::Stop(bool bKeepActors)
18 {
19  if (Enabled)
20  {
21  Enabled = false;
22 
23  // destroy actors if event was recorded?
24  if (!bKeepActors)
25  {
26  ProcessToTime(TotalTime, false);
27  }
28 
29  // callback
31  }
32 
33  File.close();
34 }
35 
37 {
38  if (File.eof())
39  {
40  return false;
41  }
42 
43  ReadValue<char>(File, Header.Id);
44  ReadValue<uint32_t>(File, Header.Size);
45 
46  return true;
47 }
48 
50 {
51  File.seekg(Header.Size, std::ios::cur);
52 }
53 
55 {
56  CurrentTime = 0.0f;
57  TotalTime = 0.0f;
58  TimeToStop = 0.0f;
59 
60  File.clear();
61  File.seekg(0, std::ios::beg);
62 
63  // mark as header as invalid to force reload a new one next time
64  Frame.Elapsed = -1.0f;
65  Frame.DurationThis = 0.0f;
66 
67  MappedId.clear();
68  IsHeroMap.clear();
69 
70  // read geneal Info
71  RecInfo.Read(File);
72 }
73 
74 // read last frame in File and return the Total time recorded
76 {
77  std::streampos Current = File.tellg();
78 
79  // parse only frames
80  while (File)
81  {
82  // get header
83  if (!ReadHeader())
84  {
85  break;
86  }
87 
88  // check for a frame packet
89  switch (Header.Id)
90  {
91  case static_cast<char>(CarlaRecorderPacketId::FrameStart):
92  Frame.Read(File);
93  break;
94  default:
95  SkipPacket();
96  break;
97  }
98  }
99 
100  File.clear();
101  File.seekg(Current, std::ios::beg);
102  return Frame.Elapsed;
103 }
104 
105 std::string CarlaReplayer::ReplayFile(std::string Filename, double TimeStart, double Duration,
106  uint32_t ThisFollowId, bool ReplaySensors)
107 {
108  std::stringstream Info;
109  std::string s;
110 
111  // check to stop if we are replaying another
112  if (Enabled)
113  {
114  Stop();
115  }
116 
117  // get the final path + filename
118  std::string Filename2 = GetRecorderFilename(Filename);
119 
120  Info << "Replaying File: " << Filename2 << std::endl;
121 
122  // try to open
123  File.open(Filename2, std::ios::binary);
124  if (!File.is_open())
125  {
126  Info << "File " << Filename2 << " not found on server\n";
127  Stop();
128  return Info.str();
129  }
130 
131  // from start
132  Rewind();
133 
134  // check to load map if different
135  if (Episode->GetMapName() != RecInfo.Mapfile)
136  {
138  {
139  Info << "Could not load mapfile " << TCHAR_TO_UTF8(*RecInfo.Mapfile) << std::endl;
140  Stop();
141  return Info.str();
142  }
143  Info << "Loading map " << TCHAR_TO_UTF8(*RecInfo.Mapfile) << std::endl;
144  Info << "Replayer will start after map is loaded..." << std::endl;
145 
146  // prepare autoplay after map is loaded
147  Autoplay.Enabled = true;
148  Autoplay.Filename = Filename2;
150  Autoplay.TimeStart = TimeStart;
151  Autoplay.Duration = Duration;
152  Autoplay.FollowId = ThisFollowId;
154  Autoplay.ReplaySensors = ReplaySensors;
155  }
156 
157  // get Total time of recorder
159  Info << "Total time recorded: " << TotalTime << std::endl;
160 
161  // set time to start replayer
162  if (TimeStart < 0.0f)
163  {
164  TimeStart = TotalTime + TimeStart;
165  if (TimeStart < 0.0f)
166  TimeStart = 0.0f;
167  }
168 
169  // set time to stop replayer
170  if (Duration > 0.0f)
171  TimeToStop = TimeStart + Duration;
172  else
174 
175  Info << "Replaying from " << TimeStart << " s - " << TimeToStop << " s (" << TotalTime << " s) at " <<
176  std::setprecision(1) << std::fixed << TimeFactor << "x" << std::endl;
177 
178  // set the follow Id
179  FollowId = ThisFollowId;
180 
181  bReplaySensors = ReplaySensors;
182  // if we don't need to load a new map, then start
183  if (!Autoplay.Enabled)
184  {
186  // process all events until the time
187  ProcessToTime(TimeStart, true);
188  // mark as enabled
189  Enabled = true;
190  }
191 
192  return Info.str();
193 }
194 
196 {
197 
198  // check if the autoplay is enabled (means waiting until map is loaded)
199  if (!Autoplay.Enabled)
200  return;
201 
202  // disable
203  Autoplay.Enabled = false;
204 
205  // check to stop if we are replaying another
206  if (Enabled)
207  {
208  Stop();
209  }
210 
211  // try to open
212  File.open(Autoplay.Filename, std::ios::binary);
213  if (!File.is_open())
214  {
215  return;
216  }
217 
218  // from start
219  Rewind();
220 
221  // get Total time of recorder
223 
224  // set time to start replayer
225  double TimeStart = Autoplay.TimeStart;
226  if (TimeStart < 0.0f)
227  {
228  TimeStart = TotalTime + Autoplay.TimeStart;
229  if (TimeStart < 0.0f)
230  TimeStart = 0.0f;
231  }
232 
233  // set time to stop replayer
234  if (Autoplay.Duration > 0.0f)
235  TimeToStop = TimeStart + Autoplay.Duration;
236  else
238 
239  // set the follow Id
241 
243 
244  // apply time factor
246 
248 
249  // process all events until the time
250  ProcessToTime(TimeStart, true);
251 
252  // mark as enabled
253  Enabled = true;
254 }
255 
256 void CarlaReplayer::ProcessToTime(double Time, bool IsFirstTime)
257 {
258  double Per = 0.0f;
259  double NewTime = CurrentTime + Time;
260  bool bFrameFound = false;
261  bool bExitAtNextFrame = false;
262  bool bExitLoop = false;
263 
264  // check if we are in the right frame
265  if (NewTime >= Frame.Elapsed && NewTime < Frame.Elapsed + Frame.DurationThis)
266  {
267  Per = (NewTime - Frame.Elapsed) / Frame.DurationThis;
268  bFrameFound = true;
269  bExitLoop = true;
270  }
271 
272  // process all frames until time we want or end
273  while (!File.eof() && !bExitLoop)
274  {
275  // get header
276  ReadHeader();
277 
278  // check for a frame packet
279  switch (Header.Id)
280  {
281  // frame
282  case static_cast<char>(CarlaRecorderPacketId::FrameStart):
283  // only read if we are not in the right frame
284  Frame.Read(File);
285  // check if target time is in this frame
286  if (NewTime < Frame.Elapsed + Frame.DurationThis)
287  {
288  Per = (NewTime - Frame.Elapsed) / Frame.DurationThis;
289  bFrameFound = true;
290  }
291  break;
292 
293  // events add
294  case static_cast<char>(CarlaRecorderPacketId::EventAdd):
296  break;
297 
298  // events del
299  case static_cast<char>(CarlaRecorderPacketId::EventDel):
301  break;
302 
303  // events parent
304  case static_cast<char>(CarlaRecorderPacketId::EventParent):
306  break;
307 
308  // collisions
309  case static_cast<char>(CarlaRecorderPacketId::Collision):
310  SkipPacket();
311  break;
312 
313  // positions
314  case static_cast<char>(CarlaRecorderPacketId::Position):
315  if (bFrameFound)
316  ProcessPositions(IsFirstTime);
317  else
318  SkipPacket();
319  break;
320 
321  // states
322  case static_cast<char>(CarlaRecorderPacketId::State):
323  if (bFrameFound)
324  ProcessStates();
325  else
326  SkipPacket();
327  break;
328 
329  // vehicle animation
330  case static_cast<char>(CarlaRecorderPacketId::AnimVehicle):
331  if (bFrameFound)
333  else
334  SkipPacket();
335  break;
336 
337  // walker animation
338  case static_cast<char>(CarlaRecorderPacketId::AnimWalker):
339  if (bFrameFound)
341  else
342  SkipPacket();
343  break;
344 
345  // vehicle light animation
346  case static_cast<char>(CarlaRecorderPacketId::VehicleLight):
347  if (bFrameFound)
349  else
350  SkipPacket();
351  break;
352 
353  // scene lights animation
354  case static_cast<char>(CarlaRecorderPacketId::SceneLight):
355  if (bFrameFound)
357  else
358  SkipPacket();
359  break;
360 
361  // frame end
362  case static_cast<char>(CarlaRecorderPacketId::FrameEnd):
363  if (bFrameFound)
364  bExitLoop = true;
365  break;
366 
367  // unknown packet, just skip
368  default:
369  // skip packet
370  SkipPacket();
371  break;
372 
373  }
374  }
375 
376  // update all positions
377  if (Enabled && bFrameFound)
378  {
379  UpdatePositions(Per, Time);
380  }
381 
382  // save current time
383  CurrentTime = NewTime;
384 
385  // stop replay?
386  if (CurrentTime >= TimeToStop)
387  {
388  // keep actors in scene and let them continue with autopilot
389  Stop(true);
390  }
391 }
392 
394 {
395  uint16_t i, Total;
397 
398  // process creation events
399  ReadValue<uint16_t>(File, Total);
400  for (i = 0; i < Total; ++i)
401  {
402  EventAdd.Read(File);
403 
404  // auto Result = CallbackEventAdd(
405  auto Result = Helper.ProcessReplayerEventAdd(
406  EventAdd.Location,
407  EventAdd.Rotation,
408  EventAdd.Description,
409  EventAdd.DatabaseId,
410  IgnoreHero,
412 
413  switch (Result.first)
414  {
415  // actor not created
416  case 0:
417  UE_LOG(LogCarla, Log, TEXT("actor could not be created"));
418  break;
419 
420  // actor created but with different id
421  case 1:
422  // mapping id (recorded Id is a new Id in replayer)
423  MappedId[EventAdd.DatabaseId] = Result.second;
424  break;
425 
426  // actor reused from existing
427  case 2:
428  // mapping id (say desired Id is mapped to what)
429  MappedId[EventAdd.DatabaseId] = Result.second;
430  break;
431  }
432 
433  // check to mark if actor is a hero vehicle or not
434  if (Result.first > 0)
435  {
436  // init
437  IsHeroMap[Result.second] = false;
438  for (const auto &Item : EventAdd.Description.Attributes)
439  {
440  if (Item.Id == "role_name" && Item.Value == "hero")
441  {
442  // mark as hero
443  IsHeroMap[Result.second] = true;
444  break;
445  }
446  }
447  }
448  }
449 }
450 
452 {
453  uint16_t i, Total;
455 
456  // process destroy events
457  ReadValue<uint16_t>(File, Total);
458  for (i = 0; i < Total; ++i)
459  {
460  EventDel.Read(File);
462  MappedId.erase(EventDel.DatabaseId);
463  }
464 }
465 
467 {
468  uint16_t i, Total;
470  std::stringstream Info;
471 
472  // process parenting events
473  ReadValue<uint16_t>(File, Total);
474  for (i = 0; i < Total; ++i)
475  {
476  EventParent.Read(File);
478  }
479 }
480 
482 {
483  uint16_t i, Total;
484  CarlaRecorderStateTrafficLight StateTrafficLight;
485  std::stringstream Info;
486 
487  // read Total traffic light states
488  ReadValue<uint16_t>(File, Total);
489  for (i = 0; i < Total; ++i)
490  {
491  StateTrafficLight.Read(File);
492 
493  StateTrafficLight.DatabaseId = MappedId[StateTrafficLight.DatabaseId];
494  if (!Helper.ProcessReplayerStateTrafficLight(StateTrafficLight))
495  {
496  UE_LOG(LogCarla,
497  Log,
498  TEXT("callback state traffic light %d called but didn't work"),
499  StateTrafficLight.DatabaseId);
500  }
501  }
502 }
503 
505 {
506  uint16_t i, Total;
508  std::stringstream Info;
509 
510  // read Total Vehicles
511  ReadValue<uint16_t>(File, Total);
512  for (i = 0; i < Total; ++i)
513  {
514  Vehicle.Read(File);
515  Vehicle.DatabaseId = MappedId[Vehicle.DatabaseId];
516  // check if ignore this actor
517  if (!(IgnoreHero && IsHeroMap[Vehicle.DatabaseId]))
518  {
520  }
521  }
522 }
523 
525 {
526  uint16_t i, Total;
528  std::stringstream Info;
529 
530  // read Total walkers
531  ReadValue<uint16_t>(File, Total);
532  for (i = 0; i < Total; ++i)
533  {
534  Walker.Read(File);
535  Walker.DatabaseId = MappedId[Walker.DatabaseId];
536  // check if ignore this actor
537  if (!(IgnoreHero && IsHeroMap[Walker.DatabaseId]))
538  {
540  }
541  }
542 }
543 
545 {
546  uint16_t Total;
547  CarlaRecorderLightVehicle LightVehicle;
548 
549  // read Total walkers
550  ReadValue<uint16_t>(File, Total);
551  for (uint16_t i = 0; i < Total; ++i)
552  {
553  LightVehicle.Read(File);
554  LightVehicle.DatabaseId = MappedId[LightVehicle.DatabaseId];
555  // check if ignore this actor
556  if (!(IgnoreHero && IsHeroMap[LightVehicle.DatabaseId]))
557  {
558  Helper.ProcessReplayerLightVehicle(LightVehicle);
559  }
560  }
561 }
562 
564 {
565  uint16_t Total;
566  CarlaRecorderLightScene LightScene;
567 
568  // read Total light events
569  ReadValue<uint16_t>(File, Total);
570  for (uint16_t i = 0; i < Total; ++i)
571  {
572  LightScene.Read(File);
573  Helper.ProcessReplayerLightScene(LightScene);
574  }
575 }
576 
577 void CarlaReplayer::ProcessPositions(bool IsFirstTime)
578 {
579  uint16_t i, Total;
580 
581  // save current as previous
582  PrevPos = std::move(CurrPos);
583 
584  // read all positions
585  ReadValue<uint16_t>(File, Total);
586  CurrPos.clear();
587  CurrPos.reserve(Total);
588  for (i = 0; i < Total; ++i)
589  {
591  Pos.Read(File);
592  // assign mapped Id
593  auto NewId = MappedId.find(Pos.DatabaseId);
594  if (NewId != MappedId.end())
595  {
596  Pos.DatabaseId = NewId->second;
597  }
598  else
599  UE_LOG(LogCarla, Log, TEXT("Actor not found when trying to move from replayer (id. %d)"), Pos.DatabaseId);
600  CurrPos.push_back(std::move(Pos));
601  }
602 
603  // check to copy positions the first time
604  if (IsFirstTime)
605  {
606  PrevPos.clear();
607  }
608 }
609 
610 void CarlaReplayer::UpdatePositions(double Per, double DeltaTime)
611 {
612  unsigned int i;
613  uint32_t NewFollowId = 0;
614  std::unordered_map<int, int> TempMap;
615 
616  // map the id of all previous positions to its index
617  for (i = 0; i < PrevPos.size(); ++i)
618  {
619  TempMap[PrevPos[i].DatabaseId] = i;
620  }
621 
622  // get the Id of the actor to follow
623  if (FollowId != 0)
624  {
625  auto NewId = MappedId.find(FollowId);
626  if (NewId != MappedId.end())
627  {
628  NewFollowId = NewId->second;
629  }
630  }
631 
632  // go through each actor and update
633  for (auto &Pos : CurrPos)
634  {
635  // check if ignore this actor
636  if (!(IgnoreHero && IsHeroMap[Pos.DatabaseId]))
637  {
638  // check if exist a previous position
639  auto Result = TempMap.find(Pos.DatabaseId);
640  if (Result != TempMap.end())
641  {
642  // check if time factor is high
643  if (TimeFactor >= 2.0)
644  // assign first position
645  InterpolatePosition(PrevPos[Result->second], Pos, 0.0, DeltaTime);
646  else
647  // interpolate
648  InterpolatePosition(PrevPos[Result->second], Pos, Per, DeltaTime);
649  }
650  else
651  {
652  // assign last position (we don't have previous one)
653  InterpolatePosition(Pos, Pos, 0.0, DeltaTime);
654  }
655  }
656 
657  // move the camera to follow this actor if required
658  if (NewFollowId != 0)
659  {
660  if (NewFollowId == Pos.DatabaseId)
661  Helper.SetCameraPosition(NewFollowId, FVector(-1000, 0, 500), FQuat::MakeFromEuler({0, -25, 0}));
662  }
663  }
664 }
665 
666 // interpolate a position (transform, velocity...)
668  const CarlaRecorderPosition &Pos1,
669  const CarlaRecorderPosition &Pos2,
670  double Per,
671  double DeltaTime)
672 {
673  // call the callback
674  Helper.ProcessReplayerPosition(Pos1, Pos2, Per, DeltaTime);
675 }
676 
677 // tick for the replayer
678 void CarlaReplayer::Tick(float Delta)
679 {
680  TRACE_CPUPROFILER_EVENT_SCOPE(CarlaReplayer::Tick);
681  // check if there are events to process
682  if (Enabled)
683  {
684  ProcessToTime(Delta * TimeFactor, false);
685  }
686 }
void CheckPlayAfterMapLoaded(void)
void ProcessLightScene(void)
double GetTotalTime(void)
void ProcessLightVehicle(void)
void ProcessReplayerLightVehicle(CarlaRecorderLightVehicle LightVehicle)
void ProcessReplayerAnimVehicle(CarlaRecorderAnimVehicle Vehicle)
void ProcessStates(void)
void Read(std::ifstream &InFile)
void Read(std::ifstream &InFile)
void Read(std::ifstream &InFile)
void ProcessAnimWalker(void)
CarlaReplayerHelper Helper
bool ProcessReplayerStateTrafficLight(CarlaRecorderStateTrafficLight State)
void Read(std::ifstream &InFile)
std::ifstream File
void InterpolatePosition(const CarlaRecorderPosition &Start, const CarlaRecorderPosition &End, double Per, double DeltaTime)
void Read(std::ifstream &InFile)
void ProcessAnimVehicle(void)
void ProcessPositions(bool IsFirstTime=false)
void Rewind(void)
void ProcessEventsAdd(void)
void ProcessToTime(double Time, bool IsFirstTime=false)
void Read(std::ifstream &File)
std::unordered_map< uint32_t, bool > IsHeroMap
std::unordered_map< uint32_t, uint32_t > MappedId
CarlaRecorderFrame Frame
UCarlaEpisode * Episode
Definition: CarlaReplayer.h:99
const FString & GetMapName() const
Return the name of the map loaded in this episode.
Definition: CarlaEpisode.h:89
void Read(std::ifstream &InFile)
std::vector< CarlaRecorderPosition > CurrPos
void ProcessReplayerLightScene(CarlaRecorderLightScene LightScene)
bool ProcessReplayerFinish(bool bApplyAutopilot, bool bIgnoreHero, std::unordered_map< uint32_t, bool > &IsHero)
bool LoadNewEpisode(const FString &MapString, bool ResetSettings=true)
Load a new map and start a new episode.
std::pair< int, uint32_t > ProcessReplayerEventAdd(FVector Location, FVector Rotation, CarlaRecorderActorDescription Description, uint32_t DesiredId, bool bIgnoreHero, bool ReplaySensors)
std::vector< CarlaRecorderActorAttribute > Attributes
CarlaRecorderInfo RecInfo
bool ProcessReplayerPosition(CarlaRecorderPosition Pos1, CarlaRecorderPosition Pos2, double Per, double DeltaTime)
double CurrentTime
uint32_t FollowId
bool ProcessReplayerEventParent(uint32_t ChildId, uint32_t ParentId)
std::string ReplayFile(std::string Filename, double TimeStart=0.0f, double Duration=0.0f, uint32_t FollowId=0, bool ReplaySensors=false)
void UpdatePositions(double Per, double DeltaTime)
std::vector< CarlaRecorderPosition > PrevPos
void Tick(float Time)
std::string GetRecorderFilename(std::string Filename)
void Stop(bool KeepActors=false)
bool ProcessReplayerEventDel(uint32_t DatabaseId)
static PlayAfterLoadMap Autoplay
Definition: CarlaReplayer.h:50
void ProcessEventsDel(void)
void ProcessEventsParent(void)
void Read(std::ifstream &InFile)
void Read(std::ifstream &InFile)
bool SetCameraPosition(uint32_t Id, FVector Offset, FQuat Rotation)
void Read(std::ifstream &InFile)
void Read(std::ifstream &InFile)
CarlaRecorderActorDescription Description
void ProcessReplayerAnimWalker(CarlaRecorderAnimWalker Walker)