CARLA
CarlaRecorderQuery.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 "CarlaRecorderHelpers.h"
8 
9 #include <ctime>
10 #include <sstream>
11 #include <string>
12 
17 
19 {
20  if (File.eof())
21  {
22  return false;
23  }
24 
25  ReadValue<char>(File, Header.Id);
26  ReadValue<uint32_t>(File, Header.Size);
27 
28  return true;
29 }
30 
32 {
33  File.seekg(Header.Size, std::ios::cur);
34 }
35 
36 inline bool CarlaRecorderQuery::CheckFileInfo(std::stringstream &Info)
37 {
38  // read Info
39  RecInfo.Read(File);
40 
41  // check magic string
42  if (RecInfo.Magic != "CARLA_RECORDER")
43  {
44  Info << "File is not a CARLA recorder" << std::endl;
45  return false;
46  }
47 
48  // show general Info
49  Info << "Version: " << RecInfo.Version << std::endl;
50  Info << "Map: " << TCHAR_TO_UTF8(*RecInfo.Mapfile) << std::endl;
51  tm *TimeInfo = localtime(&RecInfo.Date);
52  char DateStr[100];
53  strftime(DateStr, sizeof(DateStr), "%x %X", TimeInfo);
54  Info << "Date: " << DateStr << std::endl << std::endl;
55 
56  return true;
57 }
58 
59 std::string CarlaRecorderQuery::QueryInfo(std::string Filename, bool bShowAll)
60 {
61  std::stringstream Info;
62 
63  // get the final path + filename
64  std::string Filename2 = GetRecorderFilename(Filename);
65 
66  // try to open
67  File.open(Filename2, std::ios::binary);
68  if (!File.is_open())
69  {
70  Info << "File " << Filename2 << " not found on server\n";
71  return Info.str();
72  }
73 
74  uint16_t i, Total;
75  bool bFramePrinted = false;
76 
77  // lambda for repeating task
78  auto PrintFrame = [this](std::stringstream &Info)
79  {
80  Info << "Frame " << Frame.Id << " at " << Frame.Elapsed << " seconds\n";
81  };
82 
83  if (!CheckFileInfo(Info))
84  return Info.str();
85 
86  // parse only frames
87  while (File)
88  {
89 
90  // get header
91  if (!ReadHeader())
92  {
93  break;
94  }
95 
96  // check for a frame packet
97  switch (Header.Id)
98  {
99  // frame
100  case static_cast<char>(CarlaRecorderPacketId::FrameStart):
101  Frame.Read(File);
102  if (bShowAll)
103  {
104  PrintFrame(Info);
105  bFramePrinted = true;
106  }
107  else
108  bFramePrinted = false;
109  break;
110 
111  // events add
112  case static_cast<char>(CarlaRecorderPacketId::EventAdd):
113  ReadValue<uint16_t>(File, Total);
114  if (Total > 0 && !bFramePrinted)
115  {
116  PrintFrame(Info);
117  bFramePrinted = true;
118  }
119  for (i = 0; i < Total; ++i)
120  {
121  // add
122  EventAdd.Read(File);
123  Info << " Create " << EventAdd.DatabaseId << ": " << TCHAR_TO_UTF8(*EventAdd.Description.Id) <<
124  " (" <<
125  static_cast<int>(EventAdd.Type) << ") at (" << EventAdd.Location.X << ", " <<
126  EventAdd.Location.Y << ", " << EventAdd.Location.Z << ")" << std::endl;
127  for (auto &Att : EventAdd.Description.Attributes)
128  {
129  Info << " " << TCHAR_TO_UTF8(*Att.Id) << " = " << TCHAR_TO_UTF8(*Att.Value) << std::endl;
130  }
131  }
132  break;
133 
134  // events del
135  case static_cast<char>(CarlaRecorderPacketId::EventDel):
136  ReadValue<uint16_t>(File, Total);
137  if (Total > 0 && !bFramePrinted)
138  {
139  PrintFrame(Info);
140  bFramePrinted = true;
141  }
142  for (i = 0; i < Total; ++i)
143  {
144  EventDel.Read(File);
145  Info << " Destroy " << EventDel.DatabaseId << "\n";
146  }
147  break;
148 
149  // events parenting
150  case static_cast<char>(CarlaRecorderPacketId::EventParent):
151  ReadValue<uint16_t>(File, Total);
152  if (Total > 0 && !bFramePrinted)
153  {
154  PrintFrame(Info);
155  bFramePrinted = true;
156  }
157  for (i = 0; i < Total; ++i)
158  {
160  Info << " Parenting " << EventParent.DatabaseId << " with " << EventParent.DatabaseIdParent <<
161  " (parent)\n";
162  }
163  break;
164 
165  // collisions
166  case static_cast<char>(CarlaRecorderPacketId::Collision):
167  ReadValue<uint16_t>(File, Total);
168  if (Total > 0 && !bFramePrinted)
169  {
170  PrintFrame(Info);
171  bFramePrinted = true;
172  }
173  for (i = 0; i < Total; ++i)
174  {
176  Info << " Collision id " << Collision.Id << " between " << Collision.DatabaseId1;
178  Info << " (hero) ";
179  Info << " with " << Collision.DatabaseId2;
181  Info << " (hero) ";
182  Info << std::endl;
183  }
184  break;
185 
186  // positions
187  case static_cast<char>(CarlaRecorderPacketId::Position):
188  if (bShowAll)
189  {
190  ReadValue<uint16_t>(File, Total);
191  if (Total > 0 && !bFramePrinted)
192  {
193  PrintFrame(Info);
194  bFramePrinted = true;
195  }
196  Info << " Positions: " << Total << std::endl;
197  for (i = 0; i < Total; ++i)
198  {
199  Position.Read(File);
200  Info << " Id: " << Position.DatabaseId << " Location: (" << Position.Location.X << ", " << Position.Location.Y << ", " << Position.Location.Z << ") Rotation (" << Position.Rotation.X << ", " << Position.Rotation.Y << ", " << Position.Rotation.Z << ")" << std::endl;
201  }
202  }
203  else
204  SkipPacket();
205  break;
206 
207  // traffic light
208  case static_cast<char>(CarlaRecorderPacketId::State):
209  if (bShowAll)
210  {
211  ReadValue<uint16_t>(File, Total);
212  if (Total > 0 && !bFramePrinted)
213  {
214  PrintFrame(Info);
215  bFramePrinted = true;
216  }
217  Info << " State traffic lights: " << Total << std::endl;
218  for (i = 0; i < Total; ++i)
219  {
220  StateTraffic.Read(File);
221  Info << " Id: " << StateTraffic.DatabaseId << " state: " << static_cast<char>(0x30 + StateTraffic.State) << " frozen: " <<
222  StateTraffic.IsFrozen << " elapsedTime: " << StateTraffic.ElapsedTime << std::endl;
223  }
224  }
225  else
226  SkipPacket();
227  break;
228 
229  // vehicle animations
230  case static_cast<char>(CarlaRecorderPacketId::AnimVehicle):
231  if (bShowAll)
232  {
233  ReadValue<uint16_t>(File, Total);
234  if (Total > 0 && !bFramePrinted)
235  {
236  PrintFrame(Info);
237  bFramePrinted = true;
238  }
239  Info << " Vehicle animations: " << Total << std::endl;
240  for (i = 0; i < Total; ++i)
241  {
242  Vehicle.Read(File);
243  Info << " Id: " << Vehicle.DatabaseId << " Steering: " << Vehicle.Steering << " Throttle: " << Vehicle.Throttle << " Brake " << Vehicle.Brake << " Handbrake: " << Vehicle.bHandbrake << " Gear: " << Vehicle.Gear << std::endl;
244  }
245  }
246  else
247  SkipPacket();
248  break;
249 
250  // walker animations
251  case static_cast<char>(CarlaRecorderPacketId::AnimWalker):
252  if (bShowAll)
253  {
254  ReadValue<uint16_t>(File, Total);
255  if (Total > 0 && !bFramePrinted)
256  {
257  PrintFrame(Info);
258  bFramePrinted = true;
259  }
260  Info << " Walker animations: " << Total << std::endl;
261  for (i = 0; i < Total; ++i)
262  {
263  Walker.Read(File);
264  Info << " Id: " << Walker.DatabaseId << " speed: " << Walker.Speed << std::endl;
265  }
266  }
267  else
268  SkipPacket();
269  break;
270 
271  // vehicle light animations
272  case static_cast<char>(CarlaRecorderPacketId::VehicleLight):
273  if (bShowAll)
274  {
275  ReadValue<uint16_t>(File, Total);
276  if (Total > 0 && !bFramePrinted)
277  {
278  PrintFrame(Info);
279  bFramePrinted = true;
280  }
281  Info << " Vehicle light animations: " << Total << std::endl;
282  for (i = 0; i < Total; ++i)
283  {
284  LightVehicle.Read(File);
285 
287  FVehicleLightState State(LightState);
288  std::string enabled_lights_list;
289  if (State.Position)
290  enabled_lights_list += "Position ";
291  if (State.LowBeam)
292  enabled_lights_list += "LowBeam ";
293  if (State.HighBeam)
294  enabled_lights_list += "HighBeam ";
295  if (State.Brake)
296  enabled_lights_list += "Brake ";
297  if (State.RightBlinker)
298  enabled_lights_list += "RightBlinker ";
299  if (State.LeftBlinker)
300  enabled_lights_list += "LeftBlinker ";
301  if (State.Reverse)
302  enabled_lights_list += "Reverse ";
303  if (State.Interior)
304  enabled_lights_list += "Interior ";
305  if (State.Fog)
306  enabled_lights_list += "Fog ";
307  if (State.Special1)
308  enabled_lights_list += "Special1 ";
309  if (State.Special2)
310  enabled_lights_list += "Special2 ";
311 
312  if (enabled_lights_list.size())
313  {
314  Info << " Id: " << LightVehicle.DatabaseId << " " <<
315  enabled_lights_list.substr(0, enabled_lights_list.size() - 1) << std::endl;
316  }
317  else
318  {
319  Info << " Id: " << LightVehicle.DatabaseId << " None" << std::endl;
320  }
321  }
322  }
323  else
324  SkipPacket();
325  break;
326 
327  // scene light animations
328  case static_cast<char>(CarlaRecorderPacketId::SceneLight):
329  if (bShowAll)
330  {
331  ReadValue<uint16_t>(File, Total);
332  if (Total > 0 && !bFramePrinted)
333  {
334  PrintFrame(Info);
335  bFramePrinted = true;
336  }
337  Info << " Scene light changes: " << Total << std::endl;
338  for (i = 0; i < Total; ++i)
339  {
340  LightScene.Read(File);
341  Info << " Id: " << LightScene.LightId << " enabled: " << (LightScene.bOn ? "True" : "False")
342  << " intensity: " << LightScene.Intensity
343  << " RGB_color: (" << LightScene.Color.R << ", " << LightScene.Color.G << ", " << LightScene.Color.B << ")"
344  << std::endl;
345  }
346  }
347  else
348  SkipPacket();
349  break;
350 
351  // dynamic actor kinematics
352  case static_cast<char>(CarlaRecorderPacketId::Kinematics):
353  if (bShowAll)
354  {
355  ReadValue<uint16_t>(File, Total);
356  if (Total > 0 && !bFramePrinted)
357  {
358  PrintFrame(Info);
359  bFramePrinted = true;
360  }
361  Info << " Dynamic actors: " << Total << std::endl;
362  for (i = 0; i < Total; ++i)
363  {
364  Kinematics.Read(File);
365  Info << " Id: " << Kinematics.DatabaseId << " linear_velocity: ("
366  << Kinematics.LinearVelocity.X << ", " << Kinematics.LinearVelocity.Y << ", " << Kinematics.LinearVelocity.Z << ")"
367  << " angular_velocity: ("
368  << Kinematics.AngularVelocity.X << ", " << Kinematics.AngularVelocity.Y << ", " << Kinematics.AngularVelocity.Z << ")"
369  << std::endl;
370  }
371  }
372  else
373  SkipPacket();
374  break;
375 
376  // actors bounding boxes
377  case static_cast<char>(CarlaRecorderPacketId::BoundingBox):
378  if (bShowAll)
379  {
380  ReadValue<uint16_t>(File, Total);
381  if (Total > 0 && !bFramePrinted)
382  {
383  PrintFrame(Info);
384  bFramePrinted = true;
385  }
386  Info << " Actor bounding boxes: " << Total << std::endl;
387  for (i = 0; i < Total; ++i)
388  {
389  ActorBoundingBox.Read(File);
390  Info << " Id: " << ActorBoundingBox.DatabaseId << " origin: ("
391  << ActorBoundingBox.BoundingBox.Origin.X << ", "
392  << ActorBoundingBox.BoundingBox.Origin.Y << ", "
394  << " extension: ("
398  << std::endl;
399  }
400  }
401  else
402  SkipPacket();
403  break;
404 
405  // actors trigger volumes
406  case static_cast<char>(CarlaRecorderPacketId::TriggerVolume):
407  if (bShowAll)
408  {
409  ReadValue<uint16_t>(File, Total);
410  if (Total > 0 && !bFramePrinted)
411  {
412  PrintFrame(Info);
413  bFramePrinted = true;
414  }
415  Info << " Actor trigger volumes: " << Total << std::endl;
416  for (i = 0; i < Total; ++i)
417  {
418  ActorBoundingBox.Read(File);
419  Info << " Id: " << ActorBoundingBox.DatabaseId << " origin: ("
420  << ActorBoundingBox.BoundingBox.Origin.X << ", "
421  << ActorBoundingBox.BoundingBox.Origin.Y << ", "
423  << " extension: ("
427  << std::endl;
428  }
429  }
430  else
431  SkipPacket();
432  break;
433 
434  // Platform time
435  case static_cast<char>(CarlaRecorderPacketId::PlatformTime):
436  if (bShowAll)
437  {
438  if (!bFramePrinted)
439  {
440  PrintFrame(Info);
441  bFramePrinted = true;
442  }
443 
445  Info << " Current platform time: " << PlatformTime.Time << std::endl;
446  }
447  else
448  SkipPacket();
449  break;
450 
451  case static_cast<char>(CarlaRecorderPacketId::PhysicsControl):
452  if (bShowAll)
453  {
454  ReadValue<uint16_t>(File, Total);
455  if (Total > 0 && !bFramePrinted)
456  {
457  PrintFrame(Info);
458  bFramePrinted = true;
459  }
460 
461  Info << " Physics Control events: " << Total << std::endl;
462  for (i = 0; i < Total; ++i)
463  {
464  PhysicsControl.Read(File);
466  Info << " Id: " << PhysicsControl.DatabaseId << std::endl
467  << " max_rpm = " << Control.max_rpm << std::endl
468  << " MOI = " << Control.moi << std::endl
469  << " damping_rate_full_throttle = " << Control.damping_rate_full_throttle << std::endl
470  << " damping_rate_zero_throttle_clutch_engaged = " << Control.damping_rate_zero_throttle_clutch_engaged << std::endl
471  << " damping_rate_zero_throttle_clutch_disengaged = " << Control.damping_rate_zero_throttle_clutch_disengaged << std::endl
472  << " use_gear_auto_box = " << (Control.use_gear_autobox ? "true" : "false") << std::endl
473  << " gear_switch_time = " << Control.gear_switch_time << std::endl
474  << " clutch_strength = " << Control.clutch_strength << std::endl
475  << " final_ratio = " << Control.final_ratio << std::endl
476  << " mass = " << Control.mass << std::endl
477  << " drag_coefficient = " << Control.drag_coefficient << std::endl
478  << " center_of_mass = " << "(" << Control.center_of_mass.x << ", " << Control.center_of_mass.y << ", " << Control.center_of_mass.z << ")" << std::endl;
479  Info << " torque_curve =";
480  for (auto& vec : Control.torque_curve)
481  {
482  Info << " (" << vec.x << ", " << vec.y << ")";
483  }
484  Info << std::endl;
485  Info << " steering_curve =";
486  for (auto& vec : Control.steering_curve)
487  {
488  Info << " (" << vec.x << ", " << vec.y << ")";
489  }
490  Info << std::endl;
491  Info << " forward_gears:" << std::endl;
492  uint32_t count = 0;
493  for (auto& Gear : Control.forward_gears)
494  {
495  Info << " gear " << count << ": ratio " << Gear.ratio
496  << " down_ratio " << Gear.down_ratio
497  << " up_ratio " << Gear.up_ratio << std::endl;
498  ++count;
499  }
500  Info << " wheels:" << std::endl;
501  count = 0;
502  for (auto& Wheel : Control.wheels)
503  {
504  Info << " wheel " << count << ": tire_friction " << Wheel.tire_friction
505  << " damping_rate " << Wheel.damping_rate
506  << " max_steer_angle " << Wheel.max_steer_angle
507  << " radius " << Wheel.radius
508  << " max_brake_torque " << Wheel.max_brake_torque
509  << " max_handbrake_torque " << Wheel.max_handbrake_torque
510  << " position " << "(" << Wheel.position.x << ", " << Wheel.position.y << ", " << Wheel.position.z << ")"
511  << std::endl;
512  ++count;
513  }
514  }
515  }
516  else
517  SkipPacket();
518  break;
519 
520  case static_cast<char>(CarlaRecorderPacketId::TrafficLightTime):
521  if (bShowAll)
522  {
523  ReadValue<uint16_t>(File, Total);
524  if (Total > 0 && !bFramePrinted)
525  {
526  PrintFrame(Info);
527  bFramePrinted = true;
528  }
529 
530  Info << " Traffic Light time events: " << Total << std::endl;
531  for (i = 0; i < Total; ++i)
532  {
533  TrafficLightTime.Read(File);
534  Info << " Id: " << TrafficLightTime.DatabaseId
535  << " green_time: " << TrafficLightTime.GreenTime
536  << " yellow_time: " << TrafficLightTime.YellowTime
537  << " red_time: " << TrafficLightTime.RedTime
538  << std::endl;
539  }
540  }
541  else
542  SkipPacket();
543  break;
544 
545  // frame end
546  case static_cast<char>(CarlaRecorderPacketId::FrameEnd):
547  // do nothing, it is empty
548  break;
549 
550  default:
551  SkipPacket();
552  break;
553  }
554  }
555 
556  Info << "\nFrames: " << Frame.Id << "\n";
557  Info << "Duration: " << Frame.Elapsed << " seconds\n";
558 
559  File.close();
560 
561  return Info.str();
562 }
563 
564 std::string CarlaRecorderQuery::QueryCollisions(std::string Filename, char Category1, char Category2)
565 {
566  std::stringstream Info;
567 
568  // get the final path + filename
569  std::string Filename2 = GetRecorderFilename(Filename);
570 
571  // try to open
572  File.open(Filename2, std::ios::binary);
573  if (!File.is_open())
574  {
575  Info << "File " << Filename2 << " not found on server\n";
576  return Info.str();
577  }
578 
579  if (!CheckFileInfo(Info))
580  return Info.str();
581 
582  // other, vehicle, walkers, trafficLight, hero, any
583  char Categories[] = { 'o', 'v', 'w', 't', 'h', 'a' };
584  uint16_t i, Total;
585  struct ReplayerActorInfo
586  {
587  uint8_t Type;
588  FString Id;
589  };
590  std::unordered_map<uint32_t, ReplayerActorInfo> Actors;
591  struct PairHash
592  {
593  std::size_t operator()(const std::pair<uint32_t, uint32_t>& P) const
594  {
595  std::size_t hash = P.first;
596  hash <<= 32;
597  hash += P.second;
598  return hash;
599  }
600  };
601  std::unordered_set<std::pair<uint32_t, uint32_t>, PairHash > oldCollisions, newCollisions;
602 
603  // header
604  Info << std::setw(8) << "Time";
605  Info << " " << std::setw(6) << "Types";
606  Info << " " << std::setw(6) << std::right << "Id";
607  Info << " " << std::setw(35) << std::left << "Actor 1";
608  Info << " " << std::setw(6) << std::right << "Id";
609  Info << " " << std::setw(35) << std::left << "Actor 2";
610  Info << std::endl;
611 
612  // parse only frames
613  while (File)
614  {
615 
616  // get header
617  if (!ReadHeader())
618  {
619  break;
620  }
621 
622  // check for a frame packet
623  switch (Header.Id)
624  {
625  // frame
626  case static_cast<char>(CarlaRecorderPacketId::FrameStart):
627  Frame.Read(File);
628  // exchange sets of collisions (to know when a collision is new or continue from previous frame)
629  oldCollisions = std::move(newCollisions);
630  newCollisions.clear();
631  break;
632 
633  // events add
634  case static_cast<char>(CarlaRecorderPacketId::EventAdd):
635  ReadValue<uint16_t>(File, Total);
636  for (i = 0; i < Total; ++i)
637  {
638  // add
639  EventAdd.Read(File);
640  Actors[EventAdd.DatabaseId] = ReplayerActorInfo { EventAdd.Type, EventAdd.Description.Id };
641  }
642  break;
643 
644  // events del
645  case static_cast<char>(CarlaRecorderPacketId::EventDel):
646  ReadValue<uint16_t>(File, Total);
647  for (i = 0; i < Total; ++i)
648  {
649  EventDel.Read(File);
650  Actors.erase(EventAdd.DatabaseId);
651  }
652  break;
653 
654  // events parenting
655  case static_cast<char>(CarlaRecorderPacketId::EventParent):
656  SkipPacket();
657  break;
658 
659  // collisions
660  case static_cast<char>(CarlaRecorderPacketId::Collision):
661  ReadValue<uint16_t>(File, Total);
662  for (i = 0; i < Total; ++i)
663  {
665 
666  int Valid = 0;
667 
668  // get categories for both actors
669  uint8_t Type1, Type2;
670  if (Collision.DatabaseId1 != uint32_t(-1))
671  Type1 = Categories[Actors[Collision.DatabaseId1].Type];
672  else
673  Type1 = 'o'; // other non-actor object
674 
675  if (Collision.DatabaseId2 != uint32_t(-1))
676  Type2 = Categories[Actors[Collision.DatabaseId2].Type];
677  else
678  Type2 = 'o'; // other non-actor object
679 
680  // filter actor 1
681  if (Category1 == 'a')
682  ++Valid;
683  else if (Category1 == Type1)
684  ++Valid;
685  else if (Category1 == 'h' && Collision.IsActor1Hero)
686  ++Valid;
687 
688  // filter actor 2
689  if (Category2 == 'a')
690  ++Valid;
691  else if (Category2 == Type2)
692  ++Valid;
693  else if (Category2 == 'h' && Collision.IsActor2Hero)
694  ++Valid;
695 
696  // only show if both actors has passed the filter
697  if (Valid == 2)
698  {
699  // check if we need to show as a starting collision or it is a continuation one
700  auto collisionPair = std::make_pair(Collision.DatabaseId1, Collision.DatabaseId2);
701  if (oldCollisions.count(collisionPair) == 0)
702  {
703  Info << std::setw(8) << std::setprecision(0) << std::right << std::fixed << Frame.Elapsed;
704  Info << " " << " " << Type1 << " " << Type2 << " ";
705  Info << " " << std::setw(6) << std::right << Collision.DatabaseId1;
706  Info << " " << std::setw(35) << std::left << TCHAR_TO_UTF8(*Actors[Collision.DatabaseId1].Id);
707  Info << " " << std::setw(6) << std::right << Collision.DatabaseId2;
708  Info << " " << std::setw(35) << std::left << TCHAR_TO_UTF8(*Actors[Collision.DatabaseId2].Id);
709  Info << std::endl;
710  }
711  // save current collision
712  newCollisions.insert(collisionPair);
713  }
714  }
715  break;
716 
717  case static_cast<char>(CarlaRecorderPacketId::Position):
718  SkipPacket();
719  break;
720 
721  case static_cast<char>(CarlaRecorderPacketId::State):
722  SkipPacket();
723  break;
724 
725  // frame end
726  case static_cast<char>(CarlaRecorderPacketId::FrameEnd):
727  // do nothing, it is empty
728  break;
729 
730  default:
731  SkipPacket();
732  break;
733  }
734  }
735 
736  Info << "\nFrames: " << Frame.Id << "\n";
737  Info << "Duration: " << Frame.Elapsed << " seconds\n";
738 
739  File.close();
740 
741  return Info.str();
742 }
743 
744 std::string CarlaRecorderQuery::QueryBlocked(std::string Filename, double MinTime, double MinDistance)
745 {
746  std::stringstream Info;
747 
748  // get the final path + filename
749  std::string Filename2 = GetRecorderFilename(Filename);
750 
751  // try to open
752  File.open(Filename2, std::ios::binary);
753  if (!File.is_open())
754  {
755  Info << "File " << Filename2 << " not found on server\n";
756  return Info.str();
757  }
758 
759  if (!CheckFileInfo(Info))
760  return Info.str();
761 
762  // other, vehicle, walkers, trafficLight, hero, any
763  uint16_t i, Total;
764  struct ReplayerActorInfo
765  {
766  uint8_t Type;
767  FString Id;
768  FVector LastPosition;
769  double Time;
770  double Duration;
771  };
772  std::unordered_map<uint32_t, ReplayerActorInfo> Actors;
773  // to be able to sort the results by the duration of each actor (decreasing order)
774  std::multimap<double, std::string, std::greater<double>> Results;
775 
776  // header
777  Info << std::setw(8) << "Time";
778  Info << " " << std::setw(6) << "Id";
779  Info << " " << std::setw(35) << std::left << "Actor";
780  Info << " " << std::setw(10) << std::right << "Duration";
781  Info << std::endl;
782 
783  // parse only frames
784  while (File)
785  {
786 
787  // get header
788  if (!ReadHeader())
789  {
790  break;
791  }
792 
793  // check for a frame packet
794  switch (Header.Id)
795  {
796  // frame
797  case static_cast<char>(CarlaRecorderPacketId::FrameStart):
798  Frame.Read(File);
799  break;
800 
801  // events add
802  case static_cast<char>(CarlaRecorderPacketId::EventAdd):
803  ReadValue<uint16_t>(File, Total);
804  for (i = 0; i < Total; ++i)
805  {
806  // add
807  EventAdd.Read(File);
808  Actors[EventAdd.DatabaseId] = ReplayerActorInfo { EventAdd.Type, EventAdd.Description.Id, FVector(0, 0, 0), 0.0, 0.0 };
809  }
810  break;
811 
812  // events del
813  case static_cast<char>(CarlaRecorderPacketId::EventDel):
814  ReadValue<uint16_t>(File, Total);
815  for (i = 0; i < Total; ++i)
816  {
817  EventDel.Read(File);
818  Actors.erase(EventAdd.DatabaseId);
819  }
820  break;
821 
822  // events parenting
823  case static_cast<char>(CarlaRecorderPacketId::EventParent):
824  SkipPacket();
825  break;
826 
827  // collisions
828  case static_cast<char>(CarlaRecorderPacketId::Collision):
829  SkipPacket();
830  break;
831 
832  // positions
833  case static_cast<char>(CarlaRecorderPacketId::Position):
834  // read all positions
835  ReadValue<uint16_t>(File, Total);
836  for (i=0; i<Total; ++i)
837  {
838  Position.Read(File);
839  // check if actor moved less than a distance
840  if (FVector::Distance(Actors[Position.DatabaseId].LastPosition, Position.Location) < MinDistance)
841  {
842  // actor stopped
843  if (Actors[Position.DatabaseId].Duration == 0)
844  Actors[Position.DatabaseId].Time = Frame.Elapsed;
845  Actors[Position.DatabaseId].Duration += Frame.DurationThis;
846  }
847  else
848  {
849  // check to show info
850  if (Actors[Position.DatabaseId].Duration >= MinTime)
851  {
852  std::stringstream Result;
853  Result << std::setw(8) << std::setprecision(0) << std::fixed << Actors[Position.DatabaseId].Time;
854  Result << " " << std::setw(6) << Position.DatabaseId;
855  Result << " " << std::setw(35) << std::left << TCHAR_TO_UTF8(*Actors[Position.DatabaseId].Id);
856  Result << " " << std::setw(10) << std::setprecision(0) << std::fixed << std::right << Actors[Position.DatabaseId].Duration;
857  Result << std::endl;
858  Results.insert(std::make_pair(Actors[Position.DatabaseId].Duration, Result.str()));
859  }
860  // actor moving
861  Actors[Position.DatabaseId].Duration = 0;
862  Actors[Position.DatabaseId].LastPosition = Position.Location;
863  }
864  }
865  break;
866 
867  // traffic light
868  case static_cast<char>(CarlaRecorderPacketId::State):
869  SkipPacket();
870  break;
871 
872  // frame end
873  case static_cast<char>(CarlaRecorderPacketId::FrameEnd):
874  // do nothing, it is empty
875  break;
876 
877  default:
878  SkipPacket();
879  break;
880  }
881  }
882 
883  // show actors stopped that were not moving again
884  for (auto &Actor : Actors)
885  {
886  // check to show info
887  if (Actor.second.Duration >= MinTime)
888  {
889  std::stringstream Result;
890  Result << std::setw(8) << std::setprecision(0) << std::fixed << Actor.second.Time;
891  Result << " " << std::setw(6) << Actor.first;
892  Result << " " << std::setw(35) << std::left << TCHAR_TO_UTF8(*Actor.second.Id);
893  Result << " " << std::setw(10) << std::setprecision(0) << std::fixed << std::right << Actor.second.Duration;
894  Result << std::endl;
895  Results.insert(std::make_pair(Actor.second.Duration, Result.str()));
896  }
897  }
898 
899  // show the result
900  for (auto &Result : Results)
901  {
902  Info << Result.second;
903  }
904 
905  Info << "\nFrames: " << Frame.Id << "\n";
906  Info << "Duration: " << Frame.Elapsed << " seconds\n";
907 
908  File.close();
909 
910  return Info.str();
911 }
CarlaRecorderPosition Position
void Read(std::ifstream &InFile)
CarlaRecorderStateTrafficLight StateTraffic
CarlaRecorderBoundingBox BoundingBox
void Read(std::ifstream &InFile)
void Read(std::ifstream &InFile)
void Read(std::ifstream &InFile)
bool CheckFileInfo(std::stringstream &Info)
void Read(std::ifstream &InFile)
CarlaRecorderCollision Collision
CarlaRecorderActorBoundingBox ActorBoundingBox
void Read(std::ifstream &InFile)
std::string QueryBlocked(std::string Filename, double MinTime=30, double MinDistance=10)
void Read(std::ifstream &File)
void Read(std::ifstream &InFile)
CarlaRecorderEventDel EventDel
void Read(std::ifstream &InFile)
std::string QueryCollisions(std::string Filename, char Category1='a', char Category2='a')
carla::SharedPtr< cc::Actor > Actor
std::string QueryInfo(std::string Filename, bool bShowAll=false)
std::vector< CarlaRecorderActorAttribute > Attributes
void Read(std::ifstream &InFile)
CarlaRecorderEventAdd EventAdd
CarlaRecorderKinematics Kinematics
CarlaRecorderLightVehicle LightVehicle
CarlaRecorderPhysicsControl PhysicsControl
CarlaRecorderInfo RecInfo
CarlaRecorderPlatformTime PlatformTime
CarlaRecorderLightScene LightScene
CarlaRecorderFrame Frame
CarlaRecorderEventParent EventParent
void Read(std::ifstream &InFile)
std::string GetRecorderFilename(std::string Filename)
CarlaRecorderAnimVehicle Vehicle
CarlaRecorderAnimWalker Walker
CarlaRecorderTrafficLightTime TrafficLightTime
Defines the physical appearance of a vehicle whitch is obtained by the sensors.
void Read(std::ifstream &InFile)
void Read(std::ifstream &InFile)
void Read(std::ifstream &InFile)
FVehiclePhysicsControl VehiclePhysicsControl
void Read(std::ifstream &InFile)
CarlaRecorderActorDescription Description