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  // visual time for FX
294  case static_cast<char>(CarlaRecorderPacketId::VisualTime):
296  break;
297 
298  // events add
299  case static_cast<char>(CarlaRecorderPacketId::EventAdd):
301  break;
302 
303  // events del
304  case static_cast<char>(CarlaRecorderPacketId::EventDel):
306  break;
307 
308  // events parent
309  case static_cast<char>(CarlaRecorderPacketId::EventParent):
311  break;
312 
313  // collisions
314  case static_cast<char>(CarlaRecorderPacketId::Collision):
315  SkipPacket();
316  break;
317 
318  // positions
319  case static_cast<char>(CarlaRecorderPacketId::Position):
320  if (bFrameFound)
321  ProcessPositions(IsFirstTime);
322  else
323  SkipPacket();
324  break;
325 
326  // states
327  case static_cast<char>(CarlaRecorderPacketId::State):
328  if (bFrameFound)
329  ProcessStates();
330  else
331  SkipPacket();
332  break;
333 
334  // vehicle animation
335  case static_cast<char>(CarlaRecorderPacketId::AnimVehicle):
336  if (bFrameFound)
338  else
339  SkipPacket();
340  break;
341 
342  // walker animation
343  case static_cast<char>(CarlaRecorderPacketId::AnimWalker):
344  if (bFrameFound)
346  else
347  SkipPacket();
348  break;
349 
350  // vehicle light animation
351  case static_cast<char>(CarlaRecorderPacketId::VehicleLight):
352  if (bFrameFound)
354  else
355  SkipPacket();
356  break;
357 
358  // scene lights animation
359  case static_cast<char>(CarlaRecorderPacketId::SceneLight):
360  if (bFrameFound)
362  else
363  SkipPacket();
364  break;
365 
366  // walker bones
367  case static_cast<char>(CarlaRecorderPacketId::WalkerBones):
368  if (bFrameFound)
370  else
371  SkipPacket();
372  break;
373 
374  // frame end
375  case static_cast<char>(CarlaRecorderPacketId::FrameEnd):
376  if (bFrameFound)
377  bExitLoop = true;
378  break;
379 
380  // unknown packet, just skip
381  default:
382  // skip packet
383  SkipPacket();
384  break;
385 
386  }
387  }
388 
389  // update all positions
390  if (Enabled && bFrameFound)
391  {
392  UpdatePositions(Per, Time);
393  }
394 
395  // save current time
396  CurrentTime = NewTime;
397 
398  // stop replay?
399  if (CurrentTime >= TimeToStop)
400  {
401  // keep actors in scene and let them continue with autopilot
402  Stop(true);
403  }
404 }
405 
407 {
409  VisualTime.Read(File);
410 
411  // set the visual time
412  Episode->SetVisualGameTime(VisualTime.Time);
413 }
414 
416 {
417  uint16_t i, Total;
419 
420  // process creation events
421  ReadValue<uint16_t>(File, Total);
422  for (i = 0; i < Total; ++i)
423  {
424  EventAdd.Read(File);
425 
426  // auto Result = CallbackEventAdd(
427  auto Result = Helper.ProcessReplayerEventAdd(
428  EventAdd.Location,
429  EventAdd.Rotation,
430  EventAdd.Description,
431  EventAdd.DatabaseId,
432  IgnoreHero,
434 
435  switch (Result.first)
436  {
437  // actor not created
438  case 0:
439  UE_LOG(LogCarla, Log, TEXT("actor could not be created"));
440  break;
441 
442  // actor created but with different id
443  case 1:
444  // mapping id (recorded Id is a new Id in replayer)
445  MappedId[EventAdd.DatabaseId] = Result.second;
446  break;
447 
448  // actor reused from existing
449  case 2:
450  // mapping id (say desired Id is mapped to what)
451  MappedId[EventAdd.DatabaseId] = Result.second;
452  break;
453  }
454 
455  // check to mark if actor is a hero vehicle or not
456  if (Result.first > 0)
457  {
458  // init
459  IsHeroMap[Result.second] = false;
460  for (const auto &Item : EventAdd.Description.Attributes)
461  {
462  if (Item.Id == "role_name" && Item.Value == "hero")
463  {
464  // mark as hero
465  IsHeroMap[Result.second] = true;
466  break;
467  }
468  }
469  }
470  }
471 }
472 
474 {
475  uint16_t i, Total;
477 
478  // process destroy events
479  ReadValue<uint16_t>(File, Total);
480  for (i = 0; i < Total; ++i)
481  {
482  EventDel.Read(File);
484  MappedId.erase(EventDel.DatabaseId);
485  }
486 }
487 
489 {
490  uint16_t i, Total;
492  std::stringstream Info;
493 
494  // process parenting events
495  ReadValue<uint16_t>(File, Total);
496  for (i = 0; i < Total; ++i)
497  {
498  EventParent.Read(File);
500  }
501 }
502 
504 {
505  uint16_t i, Total;
506  CarlaRecorderStateTrafficLight StateTrafficLight;
507  std::stringstream Info;
508 
509  // read Total traffic light states
510  ReadValue<uint16_t>(File, Total);
511  for (i = 0; i < Total; ++i)
512  {
513  StateTrafficLight.Read(File);
514 
515  StateTrafficLight.DatabaseId = MappedId[StateTrafficLight.DatabaseId];
516  if (!Helper.ProcessReplayerStateTrafficLight(StateTrafficLight))
517  {
518  UE_LOG(LogCarla,
519  Log,
520  TEXT("callback state traffic light %d called but didn't work"),
521  StateTrafficLight.DatabaseId);
522  }
523  }
524 }
525 
527 {
528  uint16_t i, Total;
530  std::stringstream Info;
531 
532  // read Total Vehicles
533  ReadValue<uint16_t>(File, Total);
534  for (i = 0; i < Total; ++i)
535  {
536  Vehicle.Read(File);
537  Vehicle.DatabaseId = MappedId[Vehicle.DatabaseId];
538  // check if ignore this actor
539  if (!(IgnoreHero && IsHeroMap[Vehicle.DatabaseId]))
540  {
542  }
543  }
544 }
545 
547 {
548  uint16_t i, Total;
550  std::stringstream Info;
551 
552  // read Total walkers
553  ReadValue<uint16_t>(File, Total);
554  for (i = 0; i < Total; ++i)
555  {
556  Walker.Read(File);
557  Walker.DatabaseId = MappedId[Walker.DatabaseId];
558  // check if ignore this actor
559  if (!(IgnoreHero && IsHeroMap[Walker.DatabaseId]))
560  {
562  }
563  }
564 }
565 
567 {
568  uint16_t Total;
569  CarlaRecorderLightVehicle LightVehicle;
570 
571  // read Total walkers
572  ReadValue<uint16_t>(File, Total);
573  for (uint16_t i = 0; i < Total; ++i)
574  {
575  LightVehicle.Read(File);
576  LightVehicle.DatabaseId = MappedId[LightVehicle.DatabaseId];
577  // check if ignore this actor
578  if (!(IgnoreHero && IsHeroMap[LightVehicle.DatabaseId]))
579  {
580  Helper.ProcessReplayerLightVehicle(LightVehicle);
581  }
582  }
583 }
584 
586 {
587  uint16_t Total;
588  CarlaRecorderLightScene LightScene;
589 
590  // read Total light events
591  ReadValue<uint16_t>(File, Total);
592  for (uint16_t i = 0; i < Total; ++i)
593  {
594  LightScene.Read(File);
595  Helper.ProcessReplayerLightScene(LightScene);
596  }
597 }
598 
599 void CarlaReplayer::ProcessPositions(bool IsFirstTime)
600 {
601  uint16_t i, Total;
602 
603  // save current as previous
604  PrevPos = std::move(CurrPos);
605 
606  // read all positions
607  ReadValue<uint16_t>(File, Total);
608  CurrPos.clear();
609  CurrPos.reserve(Total);
610  for (i = 0; i < Total; ++i)
611  {
613  Pos.Read(File);
614  // assign mapped Id
615  auto NewId = MappedId.find(Pos.DatabaseId);
616  if (NewId != MappedId.end())
617  {
618  Pos.DatabaseId = NewId->second;
619  }
620  else
621  UE_LOG(LogCarla, Log, TEXT("Actor not found when trying to move from replayer (id. %d)"), Pos.DatabaseId);
622  CurrPos.push_back(std::move(Pos));
623  }
624 
625  // check to copy positions the first time
626  if (IsFirstTime)
627  {
628  PrevPos.clear();
629  }
630 }
631 
633 {
634  uint16_t i, Total;
636  std::stringstream Info;
637 
638  // read Total walkers
639  ReadValue<uint16_t>(File, Total);
640  for (i = 0; i < Total; ++i)
641  {
642  Walker.Read(File);
643  Walker.DatabaseId = MappedId[Walker.DatabaseId];
644  // check if ignore this actor
645  if (!(IgnoreHero && IsHeroMap[Walker.DatabaseId]))
646  {
648  }
649  }
650 }
651 
652 void CarlaReplayer::UpdatePositions(double Per, double DeltaTime)
653 {
654  unsigned int i;
655  uint32_t NewFollowId = 0;
656  std::unordered_map<int, int> TempMap;
657 
658  // map the id of all previous positions to its index
659  for (i = 0; i < PrevPos.size(); ++i)
660  {
661  TempMap[PrevPos[i].DatabaseId] = i;
662  }
663 
664  // get the Id of the actor to follow
665  if (FollowId != 0)
666  {
667  auto NewId = MappedId.find(FollowId);
668  if (NewId != MappedId.end())
669  {
670  NewFollowId = NewId->second;
671  }
672  }
673 
674  // go through each actor and update
675  for (auto &Pos : CurrPos)
676  {
677  // check if ignore this actor
678  if (!(IgnoreHero && IsHeroMap[Pos.DatabaseId]))
679  {
680  // check if exist a previous position
681  auto Result = TempMap.find(Pos.DatabaseId);
682  if (Result != TempMap.end())
683  {
684  // check if time factor is high
685  if (TimeFactor >= 2.0)
686  // assign first position
687  InterpolatePosition(PrevPos[Result->second], Pos, 0.0, DeltaTime);
688  else
689  // interpolate
690  InterpolatePosition(PrevPos[Result->second], Pos, Per, DeltaTime);
691  }
692  else
693  {
694  // assign last position (we don't have previous one)
695  InterpolatePosition(Pos, Pos, 0.0, DeltaTime);
696  }
697  }
698 
699  // move the camera to follow this actor if required
700  if (NewFollowId != 0)
701  {
702  if (NewFollowId == Pos.DatabaseId)
703  Helper.SetCameraPosition(NewFollowId, FVector(-1000, 0, 500), FQuat::MakeFromEuler({0, -25, 0}));
704  }
705  }
706 }
707 
708 // interpolate a position (transform, velocity...)
710  const CarlaRecorderPosition &Pos1,
711  const CarlaRecorderPosition &Pos2,
712  double Per,
713  double DeltaTime)
714 {
715  // call the callback
716  Helper.ProcessReplayerPosition(Pos1, Pos2, Per, DeltaTime);
717 }
718 
719 // tick for the replayer
720 void CarlaReplayer::Tick(float Delta)
721 {
722  TRACE_CPUPROFILER_EVENT_SCOPE(CarlaReplayer::Tick);
723  // check if there are events to process
724  if (Enabled)
725  {
726  ProcessToTime(Delta * TimeFactor, false);
727  }
728 }
void CheckPlayAfterMapLoaded(void)
void ProcessLightScene(void)
double GetTotalTime(void)
void ProcessWalkerBones(void)
void ProcessLightVehicle(void)
void ProcessReplayerLightVehicle(CarlaRecorderLightVehicle LightVehicle)
void ProcessReplayerAnimVehicle(CarlaRecorderAnimVehicle Vehicle)
void ProcessStates(void)
void Read(std::istream &InFile)
void ProcessAnimWalker(void)
CarlaReplayerHelper Helper
void Read(std::istream &InFile)
bool ProcessReplayerStateTrafficLight(CarlaRecorderStateTrafficLight State)
void Read(std::istream &InFile)
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
Definition: CarlaReplayer.h:99
const FString & GetMapName() const
Return the name of the map loaded in this episode.
Definition: CarlaEpisode.h:92
void Read(std::ifstream &InFile)
std::vector< CarlaRecorderPosition > CurrPos
void ProcessReplayerLightScene(CarlaRecorderLightScene LightScene)
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.
std::pair< int, uint32_t > ProcessReplayerEventAdd(FVector Location, FVector Rotation, CarlaRecorderActorDescription Description, uint32_t DesiredId, bool bIgnoreHero, bool ReplaySensors)
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:109
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)