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  if (File.is_open())
34  File.close();
35 }
36 
38 {
39  if (File.eof())
40  {
41  return false;
42  }
43 
44  ReadValue<char>(File, Header.Id);
45  ReadValue<uint32_t>(File, Header.Size);
46 
47  return true;
48 }
49 
51 {
52  File.seekg(Header.Size, std::ios::cur);
53 }
54 
56 {
57  CurrentTime = 0.0f;
58  TotalTime = 0.0f;
59  TimeToStop = 0.0f;
60 
61  File.clear();
62  File.seekg(0, std::ios::beg);
63 
64  // mark as header as invalid to force reload a new one next time
65  Frame.Elapsed = -1.0f;
66  Frame.DurationThis = 0.0f;
67 
68  MappedId.clear();
69  IsHeroMap.clear();
70 
71  // read geneal Info
72  RecInfo.Read(File);
73 }
74 
75 // read last frame in File and return the Total time recorded
77 {
78  std::streampos Current = File.tellg();
79 
80  // parse only frames
81  while (File)
82  {
83  // get header
84  if (!ReadHeader())
85  {
86  break;
87  }
88 
89  // check for a frame packet
90  switch (Header.Id)
91  {
92  case static_cast<char>(CarlaRecorderPacketId::FrameStart):
93  Frame.Read(File);
94  break;
95  default:
96  SkipPacket();
97  break;
98  }
99  }
100 
101  File.clear();
102  File.seekg(Current, std::ios::beg);
103  return Frame.Elapsed;
104 }
105 
106 std::string CarlaReplayer::ReplayFile(std::string Filename, double TimeStart, double Duration,
107  uint32_t ThisFollowId, bool ReplaySensors)
108 {
109  std::stringstream Info;
110  std::string s;
111 
112  // check to stop if we are replaying another
113  if (Enabled)
114  {
115  Stop();
116  }
117 
118  // get the final path + filename
119  std::string Filename2 = GetRecorderFilename(Filename);
120 
121  Info << "Replaying File: " << Filename2 << std::endl;
122 
123  // try to open
124  File.open(Filename2, std::ios::binary);
125  if (!File.is_open())
126  {
127  Info << "File " << Filename2 << " not found on server\n";
128  Stop();
129  return Info.str();
130  }
131 
132  // from start
133  Rewind();
134 
135  // check to load map if different
136  if (Episode->GetMapName() != RecInfo.Mapfile)
137  {
139  {
140  Info << "Could not load mapfile " << TCHAR_TO_UTF8(*RecInfo.Mapfile) << std::endl;
141  Stop();
142  return Info.str();
143  }
144  Info << "Loading map " << TCHAR_TO_UTF8(*RecInfo.Mapfile) << std::endl;
145  Info << "Replayer will start after map is loaded..." << std::endl;
146 
147  // prepare autoplay after map is loaded
148  Autoplay.Enabled = true;
149  Autoplay.Filename = Filename2;
151  Autoplay.TimeStart = TimeStart;
152  Autoplay.Duration = Duration;
153  Autoplay.FollowId = ThisFollowId;
155  Autoplay.ReplaySensors = ReplaySensors;
156  }
157 
158  // get Total time of recorder
160  Info << "Total time recorded: " << TotalTime << std::endl;
161 
162  // set time to start replayer
163  if (TimeStart < 0.0f)
164  {
165  TimeStart = TotalTime + TimeStart;
166  if (TimeStart < 0.0f)
167  TimeStart = 0.0f;
168  }
169 
170  // set time to stop replayer
171  if (Duration > 0.0f)
172  TimeToStop = TimeStart + Duration;
173  else
175 
176  Info << "Replaying from " << TimeStart << " s - " << TimeToStop << " s (" << TotalTime << " s) at " <<
177  std::setprecision(1) << std::fixed << TimeFactor << "x" << std::endl;
178 
179  if (IgnoreHero)
180  Info << "Ignoring Hero vehicle" << std::endl;
181 
182  if (IgnoreSpectator)
183  Info << "Ignoring Spectator camera" << std::endl;
184 
185  // set the follow Id
186  FollowId = ThisFollowId;
187 
188  bReplaySensors = ReplaySensors;
189  // if we don't need to load a new map, then start
190  if (!Autoplay.Enabled)
191  {
193  // process all events until the time
194  ProcessToTime(TimeStart, true);
195  // mark as enabled
196  Enabled = true;
197  }
198 
199  return Info.str();
200 }
201 
203 {
204 
205  // check if the autoplay is enabled (means waiting until map is loaded)
206  if (!Autoplay.Enabled)
207  return;
208 
209  // disable
210  Autoplay.Enabled = false;
211 
212  // check to stop if we are replaying another
213  if (Enabled)
214  {
215  Stop();
216  }
217 
218  // try to open
219  File.open(Autoplay.Filename, std::ios::binary);
220  if (!File.is_open())
221  {
222  return;
223  }
224 
225  // from start
226  Rewind();
227 
228  // get Total time of recorder
230 
231  // set time to start replayer
232  double TimeStart = Autoplay.TimeStart;
233  if (TimeStart < 0.0f)
234  {
235  TimeStart = TotalTime + Autoplay.TimeStart;
236  if (TimeStart < 0.0f)
237  TimeStart = 0.0f;
238  }
239 
240  // set time to stop replayer
241  if (Autoplay.Duration > 0.0f)
242  TimeToStop = TimeStart + Autoplay.Duration;
243  else
245 
246  // set the follow Id
248 
250 
251  // apply time factor
253 
255 
256  // process all events until the time
257  ProcessToTime(TimeStart, true);
258 
259  // mark as enabled
260  Enabled = true;
261 }
262 
263 void CarlaReplayer::ProcessToTime(double Time, bool IsFirstTime)
264 {
265  double Per = 0.0f;
266  double NewTime = CurrentTime + Time;
267  bool bFrameFound = false;
268  bool bExitAtNextFrame = false;
269  bool bExitLoop = false;
270 
271  // check if we are in the right frame
272  if (NewTime >= Frame.Elapsed && NewTime < Frame.Elapsed + Frame.DurationThis)
273  {
274  Per = (NewTime - Frame.Elapsed) / Frame.DurationThis;
275  bFrameFound = true;
276  bExitLoop = true;
277  }
278 
279  // process all frames until time we want or end
280  while (!File.eof() && !bExitLoop)
281  {
282  // get header
283  ReadHeader();
284 
285  // check for a frame packet
286  switch (Header.Id)
287  {
288  // frame
289  case static_cast<char>(CarlaRecorderPacketId::FrameStart):
290  // only read if we are not in the right frame
291  Frame.Read(File);
292  // check if target time is in this frame
293  if (NewTime < Frame.Elapsed + Frame.DurationThis)
294  {
295  Per = (NewTime - Frame.Elapsed) / Frame.DurationThis;
296  bFrameFound = true;
297  }
298  break;
299 
300  // visual time for FX
301  case static_cast<char>(CarlaRecorderPacketId::VisualTime):
303  break;
304 
305  // events add
306  case static_cast<char>(CarlaRecorderPacketId::EventAdd):
308  break;
309 
310  // events del
311  case static_cast<char>(CarlaRecorderPacketId::EventDel):
313  break;
314 
315  // events parent
316  case static_cast<char>(CarlaRecorderPacketId::EventParent):
318  break;
319 
320  // collisions
321  case static_cast<char>(CarlaRecorderPacketId::Collision):
322  SkipPacket();
323  break;
324 
325  // positions
326  case static_cast<char>(CarlaRecorderPacketId::Position):
327  if (bFrameFound)
328  ProcessPositions(IsFirstTime);
329  else
330  SkipPacket();
331  break;
332 
333  // states
334  case static_cast<char>(CarlaRecorderPacketId::State):
335  if (bFrameFound)
336  ProcessStates();
337  else
338  SkipPacket();
339  break;
340 
341  // vehicle animation
342  case static_cast<char>(CarlaRecorderPacketId::AnimVehicle):
343  if (bFrameFound)
345  else
346  SkipPacket();
347  break;
348 
349  // vehicle wheels animation
350  case static_cast<char>(CarlaRecorderPacketId::AnimVehicleWheels):
351  if (bFrameFound)
353  else
354  SkipPacket();
355  break;
356 
357  // walker animation
358  case static_cast<char>(CarlaRecorderPacketId::AnimWalker):
359  if (bFrameFound)
361  else
362  SkipPacket();
363  break;
364 
365  // biker animation
366  case static_cast<char>(CarlaRecorderPacketId::AnimBiker):
367  if (bFrameFound)
369  else
370  SkipPacket();
371  break;
372 
373  // vehicle light animation
374  case static_cast<char>(CarlaRecorderPacketId::VehicleLight):
375  if (bFrameFound)
377  else
378  SkipPacket();
379  break;
380 
381  // scene lights animation
382  case static_cast<char>(CarlaRecorderPacketId::SceneLight):
383  if (bFrameFound)
385  else
386  SkipPacket();
387  break;
388 
389  // walker bones
390  case static_cast<char>(CarlaRecorderPacketId::WalkerBones):
391  if (bFrameFound)
393  else
394  SkipPacket();
395  break;
396 
397  // frame end
398  case static_cast<char>(CarlaRecorderPacketId::FrameEnd):
399  if (bFrameFound)
400  bExitLoop = true;
401  break;
402 
403  // unknown packet, just skip
404  default:
405  // skip packet
406  SkipPacket();
407  break;
408 
409  }
410  }
411 
412  // update all positions
413  if (Enabled && bFrameFound)
414  {
415  UpdatePositions(Per, Time);
416  }
417 
418  // save current time
419  CurrentTime = NewTime;
420 
421  // stop replay?
422  if (CurrentTime >= TimeToStop)
423  {
424  // keep actors in scene and let them continue with autopilot
425  Stop(true);
426  }
427 }
428 
430 {
432  VisualTime.Read(File);
433 
434  // set the visual time
435  Episode->SetVisualGameTime(VisualTime.Time);
436 }
437 
439 {
440  uint16_t i, Total;
442 
443  // process creation events
444  ReadValue<uint16_t>(File, Total);
445  for (i = 0; i < Total; ++i)
446  {
447  EventAdd.Read(File);
448 
449  // auto Result = CallbackEventAdd(
450  auto Result = Helper.ProcessReplayerEventAdd(
451  EventAdd.Location,
452  EventAdd.Rotation,
453  EventAdd.Description,
454  EventAdd.DatabaseId,
455  IgnoreHero,
458 
459  switch (Result.first)
460  {
461  // actor not created
462  case 0:
463  UE_LOG(LogCarla, Log, TEXT("actor could not be created"));
464  break;
465 
466  // actor created but with different id
467  case 1:
468  // mapping id (recorded Id is a new Id in replayer)
469  MappedId[EventAdd.DatabaseId] = Result.second;
470  break;
471 
472  // actor reused from existing
473  case 2:
474  // mapping id (say desired Id is mapped to what)
475  MappedId[EventAdd.DatabaseId] = Result.second;
476  break;
477 
478  // actor ignored (either Hero or Spectator)
479  case 3:
480  UE_LOG(LogCarla, Log, TEXT("ignoring actor from replayer (Hero or Spectator)"));
481  break;
482 
483  }
484 
485  // check to mark if actor is a hero vehicle or not
486  if (Result.first > 0 && Result.first < 3)
487  {
488  // init
489  IsHeroMap[Result.second] = false;
490  for (const auto &Item : EventAdd.Description.Attributes)
491  {
492  if (Item.Id == "role_name" && Item.Value == "hero")
493  {
494  // mark as hero
495  IsHeroMap[Result.second] = true;
496  break;
497  }
498  }
499  }
500  }
501 }
502 
504 {
505  uint16_t i, Total;
507 
508  // process destroy events
509  ReadValue<uint16_t>(File, Total);
510  for (i = 0; i < Total; ++i)
511  {
512  EventDel.Read(File);
514  MappedId.erase(EventDel.DatabaseId);
515  }
516 }
517 
519 {
520  uint16_t i, Total;
522  std::stringstream Info;
523 
524  // process parenting events
525  ReadValue<uint16_t>(File, Total);
526  for (i = 0; i < Total; ++i)
527  {
528  EventParent.Read(File);
530  }
531 }
532 
534 {
535  uint16_t i, Total;
536  CarlaRecorderStateTrafficLight StateTrafficLight;
537  std::stringstream Info;
538 
539  // read Total traffic light states
540  ReadValue<uint16_t>(File, Total);
541  for (i = 0; i < Total; ++i)
542  {
543  StateTrafficLight.Read(File);
544 
545  StateTrafficLight.DatabaseId = MappedId[StateTrafficLight.DatabaseId];
546  if (!Helper.ProcessReplayerStateTrafficLight(StateTrafficLight))
547  {
548  UE_LOG(LogCarla,
549  Log,
550  TEXT("callback state traffic light %d called but didn't work"),
551  StateTrafficLight.DatabaseId);
552  }
553  }
554 }
555 
557 {
558  uint16_t i, Total;
560  std::stringstream Info;
561 
562  // read Total Vehicles
563  ReadValue<uint16_t>(File, Total);
564  for (i = 0; i < Total; ++i)
565  {
566  Vehicle.Read(File);
567  Vehicle.DatabaseId = MappedId[Vehicle.DatabaseId];
568  // check if ignore this actor
569  if (!(IgnoreHero && IsHeroMap[Vehicle.DatabaseId]))
570  {
572  }
573  }
574 }
575 
577 {
578  uint16_t i, Total;
579 
580  // read Total Vehicles
581  ReadValue<uint16_t>(File, Total);
582  for (i = 0; i < Total; ++i)
583  {
585  Vehicle.Read(File);
586  Vehicle.DatabaseId = MappedId[Vehicle.DatabaseId];
587  // check if ignore this actor
588  if (!(IgnoreHero && IsHeroMap[Vehicle.DatabaseId]))
589  {
591  }
592  }
593 }
594 
596 {
597  uint16_t i, Total;
599  std::stringstream Info;
600 
601  // read Total walkers
602  ReadValue<uint16_t>(File, Total);
603  for (i = 0; i < Total; ++i)
604  {
605  Walker.Read(File);
606  Walker.DatabaseId = MappedId[Walker.DatabaseId];
607  // check if ignore this actor
608  if (!(IgnoreHero && IsHeroMap[Walker.DatabaseId]))
609  {
611  }
612  }
613 }
614 
616 {
617  uint16_t i, Total;
619  std::stringstream Info;
620 
621  ReadValue<uint16_t>(File, Total);
622  for (i = 0; i < Total; ++i)
623  {
624  Biker.Read(File);
625  Biker.DatabaseId = MappedId[Biker.DatabaseId];
626  if (!(IgnoreHero && IsHeroMap[Biker.DatabaseId]))
627  {
629  }
630  }
631 }
632 
634 {
635  uint16_t Total;
636  CarlaRecorderLightVehicle LightVehicle;
637 
638  // read Total walkers
639  ReadValue<uint16_t>(File, Total);
640  for (uint16_t i = 0; i < Total; ++i)
641  {
642  LightVehicle.Read(File);
643  LightVehicle.DatabaseId = MappedId[LightVehicle.DatabaseId];
644  // check if ignore this actor
645  if (!(IgnoreHero && IsHeroMap[LightVehicle.DatabaseId]))
646  {
647  Helper.ProcessReplayerLightVehicle(LightVehicle);
648  }
649  }
650 }
651 
653 {
654  uint16_t Total;
655  CarlaRecorderLightScene LightScene;
656 
657  // read Total light events
658  ReadValue<uint16_t>(File, Total);
659  for (uint16_t i = 0; i < Total; ++i)
660  {
661  LightScene.Read(File);
662  Helper.ProcessReplayerLightScene(LightScene);
663  }
664 }
665 
666 void CarlaReplayer::ProcessPositions(bool IsFirstTime)
667 {
668  uint16_t i, Total;
669 
670  // save current as previous
671  PrevPos = std::move(CurrPos);
672 
673  // read all positions
674  ReadValue<uint16_t>(File, Total);
675  CurrPos.clear();
676  CurrPos.reserve(Total);
677  for (i = 0; i < Total; ++i)
678  {
680  Pos.Read(File);
681  // assign mapped Id
682  auto NewId = MappedId.find(Pos.DatabaseId);
683  if (NewId != MappedId.end())
684  {
685  Pos.DatabaseId = NewId->second;
686  }
687  else
688  UE_LOG(LogCarla, Log, TEXT("Actor not found when trying to move from replayer (id. %d)"), Pos.DatabaseId);
689  CurrPos.push_back(std::move(Pos));
690  }
691 
692  // check to copy positions the first time
693  if (IsFirstTime)
694  {
695  PrevPos.clear();
696  }
697 }
698 
700 {
701  uint16_t i, Total;
703  std::stringstream Info;
704 
705  // read Total walkers
706  ReadValue<uint16_t>(File, Total);
707  for (i = 0; i < Total; ++i)
708  {
709  Walker.Read(File);
710  Walker.DatabaseId = MappedId[Walker.DatabaseId];
711  // check if ignore this actor
712  if (!(IgnoreHero && IsHeroMap[Walker.DatabaseId]))
713  {
715  }
716  }
717 }
718 
719 void CarlaReplayer::UpdatePositions(double Per, double DeltaTime)
720 {
721  unsigned int i;
722  uint32_t NewFollowId = 0;
723  std::unordered_map<int, int> TempMap;
724 
725  // map the id of all previous positions to its index
726  for (i = 0; i < PrevPos.size(); ++i)
727  {
728  TempMap[PrevPos[i].DatabaseId] = i;
729  }
730 
731  // get the Id of the actor to follow
732  if (FollowId != 0)
733  {
734  auto NewId = MappedId.find(FollowId);
735  if (NewId != MappedId.end())
736  {
737  NewFollowId = NewId->second;
738  }
739  }
740 
741  // go through each actor and update
742  for (auto &Pos : CurrPos)
743  {
744  // check if ignore this actor (hero) or the spectator (id == 1)
745  if (!(IgnoreHero && IsHeroMap[Pos.DatabaseId]) &&
746  !(IgnoreSpectator && Pos.DatabaseId == 1))
747  {
748  // check if exist a previous position
749  auto Result = TempMap.find(Pos.DatabaseId);
750  if (Result != TempMap.end())
751  {
752  // check if time factor is high
753  if (TimeFactor >= 2.0)
754  // assign first position
755  InterpolatePosition(PrevPos[Result->second], Pos, 0.0, DeltaTime);
756  else
757  // interpolate
758  InterpolatePosition(PrevPos[Result->second], Pos, Per, DeltaTime);
759  }
760  else
761  {
762  // assign last position (we don't have previous one)
763  InterpolatePosition(Pos, Pos, 0.0, DeltaTime);
764  }
765  }
766 
767  // move the camera to follow this actor if required
768  if (NewFollowId != 0)
769  {
770  if (NewFollowId == Pos.DatabaseId)
771  Helper.SetCameraPosition(NewFollowId, FVector(-1000, 0, 500), FQuat::MakeFromEuler({0, -25, 0}));
772  }
773  }
774 }
775 
776 // interpolate a position (transform, velocity...)
778  const CarlaRecorderPosition &Pos1,
779  const CarlaRecorderPosition &Pos2,
780  double Per,
781  double DeltaTime)
782 {
783  // call the callback
784  Helper.ProcessReplayerPosition(Pos1, Pos2, Per, DeltaTime);
785 }
786 
787 // tick for the replayer
788 void CarlaReplayer::Tick(float Delta)
789 {
790  TRACE_CPUPROFILER_EVENT_SCOPE(CarlaReplayer::Tick);
791  // check if there are events to process
792  if (Enabled)
793  {
794  ProcessToTime(Delta * TimeFactor, false);
795  }
796 }
void CheckPlayAfterMapLoaded(void)
void ProcessLightScene(void)
void ProcessReplayerAnimBiker(CarlaRecorderAnimBiker Biker)
double GetTotalTime(void)
void ProcessWalkerBones(void)
void ProcessLightVehicle(void)
void ProcessReplayerLightVehicle(CarlaRecorderLightVehicle LightVehicle)
void ProcessReplayerAnimVehicle(CarlaRecorderAnimVehicle Vehicle)
void ProcessStates(void)
void ProcessAnimVehicleWheels(void)
void Read(std::istream &InFile)
void ProcessAnimWalker(void)
CarlaReplayerHelper Helper
void Read(std::istream &InFile)
bool ProcessReplayerStateTrafficLight(CarlaRecorderStateTrafficLight State)
void ProcessAnimBiker(void)
void ProcessReplayerAnimVehicleWheels(CarlaRecorderAnimWheels Vehicle)
void Read(std::istream &InFile)
std::pair< int, uint32_t > ProcessReplayerEventAdd(FVector Location, FVector Rotation, CarlaRecorderActorDescription Description, uint32_t DesiredId, bool bIgnoreHero, bool bIgnoreSpectator, bool ReplaySensors)
std::ifstream File
void ProcessReplayerWalkerBones(const CarlaRecorderWalkerBones &Walker)
void InterpolatePosition(const CarlaRecorderPosition &Start, const CarlaRecorderPosition &End, double Per, double DeltaTime)
void ProcessAnimVehicle(void)
void ProcessPositions(bool IsFirstTime=false)
void Rewind(void)
void ProcessEventsAdd(void)
void Read(std::istream &InFile)
void ProcessToTime(double Time, bool IsFirstTime=false)
std::unordered_map< uint32_t, bool > IsHeroMap
std::unordered_map< uint32_t, uint32_t > MappedId
CarlaRecorderFrame Frame
UCarlaEpisode * Episode
const FString & GetMapName() const
Return the name of the map loaded in this episode.
Definition: CarlaEpisode.h:93
void Read(std::ifstream &InFile)
std::vector< CarlaRecorderPosition > CurrPos
void ProcessReplayerLightScene(CarlaRecorderLightScene LightScene)
void Read(std::istream &InFile)
void Read(std::istream &InFile)
void Read(std::istream &InFile)
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.
void Read(std::istream &File)
std::vector< CarlaRecorderActorAttribute > Attributes
CarlaRecorderInfo RecInfo
bool ProcessReplayerPosition(CarlaRecorderPosition Pos1, CarlaRecorderPosition Pos2, double Per, double DeltaTime)
void ProcessVisualTime(void)
double CurrentTime
uint32_t FollowId
bool ProcessReplayerEventParent(uint32_t ChildId, uint32_t ParentId)
void Read(std::istream &InFile)
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)
void Read(std::ifstream &InFile)
std::string GetRecorderFilename(std::string Filename)
void Stop(bool KeepActors=false)
bool ProcessReplayerEventDel(uint32_t DatabaseId)
static PlayAfterLoadMap Autoplay
Definition: CarlaReplayer.h:50
void SetVisualGameTime(double Time)
Definition: CarlaEpisode.h:110
void Read(std::istream &InFile)
void ProcessEventsDel(void)
void ProcessEventsParent(void)
bool SetCameraPosition(uint32_t Id, FVector Offset, FQuat Rotation)
void Read(std::istream &InFile)
CarlaRecorderActorDescription Description
void Read(std::istream &InFile)
void ProcessReplayerAnimWalker(CarlaRecorderAnimWalker Walker)