CARLA
DebugShapeDrawer.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"
9 #include "Carla/Game/CarlaHUD.h"
12 
13 #include "DrawDebugHelpers.h"
14 #include "Components/LineBatchComponent.h"
15 
17 #include <carla/rpc/DebugShape.h>
18 #include <carla/rpc/String.h>
20 
21 
22 
24 {
26 
27  FShapeVisitor(UWorld &InWorld, FColor InColor, float InLifeTime, bool bInPersistentLines)
28  : World(&InWorld),
29  Color(InColor.ReinterpretAsLinear() * BrightMultiplier),
30  LifeTime(InLifeTime),
31  bPersistentLines(bInPersistentLines)
32  {
33  World->PersistentLineBatcher->SetCollisionEnabled(ECollisionEnabled::NoCollision);
34  }
35 
36  void operator()(const Shape::Point &Point) const
37  {
38  FVector Location = FVector(Point.location);
40  if (LargeMap)
41  {
42  Location = LargeMap->GlobalToLocalLocation(Location);
43  }
44  World->PersistentLineBatcher->DrawPoint(
45  Location,
46  Color,
47  1e2f * Point.size,
49  LifeTime);
50  }
51 
52  void operator()(const Shape::Line &Line) const
53  {
54  FVector Begin = FVector(Line.begin);
55  FVector End = FVector(Line.end);
57  if (LargeMap)
58  {
59  Begin = LargeMap->GlobalToLocalLocation(Begin);
60  End = LargeMap->GlobalToLocalLocation(End);
61  }
62  World->PersistentLineBatcher->DrawLine(
63  Begin,
64  End,
65  Color,
67  1e2f * Line.thickness,
68  LifeTime);
69  }
70 
71  void operator()(const Shape::Arrow &Arrow) const
72  {
73  FVector Begin = FVector(Arrow.line.begin);
74  FVector End = FVector(Arrow.line.end);
76  if (LargeMap)
77  {
78  Begin = LargeMap->GlobalToLocalLocation(Begin);
79  End = LargeMap->GlobalToLocalLocation(End);
80  }
81  const auto Diff = End - Begin;
82  const FRotator LookAt = FRotationMatrix::MakeFromX(Diff).Rotator();
83  const FTransform Transform = {LookAt, Begin};
84 
85  // Everything in centimeters
86  const auto Dist = Diff.Size();
87  const auto ArrowSize = 1e2f * Arrow.arrow_size;
88  const auto ArrowTipDist = Dist - ArrowSize;
89  const auto Thickness = 1e2f * Arrow.line.thickness;
90 
91  World->PersistentLineBatcher->DrawLines(TArray<FBatchedLine>({
92  FBatchedLine(
93  Begin,
94  End,
95  Color,
96  LifeTime,
97  Thickness,
99  FBatchedLine(
100  Transform.TransformPosition(FVector(ArrowTipDist, +ArrowSize, +ArrowSize)),
101  End,
102  Color,
103  LifeTime,
104  Thickness,
105  DepthPriority),
106  FBatchedLine(
107  Transform.TransformPosition(FVector(ArrowTipDist, +ArrowSize, -ArrowSize)),
108  End,
109  Color,
110  LifeTime,
111  Thickness,
112  DepthPriority),
113  FBatchedLine(
114  Transform.TransformPosition(FVector(ArrowTipDist, -ArrowSize, +ArrowSize)),
115  End,
116  Color,
117  LifeTime,
118  Thickness,
119  DepthPriority),
120  FBatchedLine(
121  Transform.TransformPosition(FVector(ArrowTipDist, -ArrowSize, -ArrowSize)),
122  End,
123  Color,
124  LifeTime,
125  Thickness,
126  DepthPriority)}));
127  }
128 
129  void operator()(const Shape::Box &Box) const
130  {
131  const FVector Extent = 1e2f * FVector{Box.box.extent.x, Box.box.extent.y, Box.box.extent.z};
132  FTransform Transform = {FRotator(Box.rotation), Box.box.location};
133  const auto Thickness = 1e2f * Box.thickness;
134 
136  if (LargeMap)
137  {
138  Transform = LargeMap->GlobalToLocalTransform(Transform);
139  }
140 
141  FVector B[2], P, Q;
142  B[0] = -Extent;
143  B[1] = Extent;
144 
145  for(int32 i = 0; i < 2; ++i)
146  {
147  for(int32 j = 0; j < 2; ++j)
148  {
149  P.X=B[i].X;
150  Q.X=B[i].X;
151  P.Y=B[j].Y;
152  Q.Y=B[j].Y;
153  P.Z=B[0].Z;
154  Q.Z=B[1].Z;
155  World->PersistentLineBatcher->DrawLine(
156  Transform.TransformPosition(P),
157  Transform.TransformPosition(Q),
158  Color,
160  Thickness,
161  LifeTime);
162 
163  P.Y=B[i].Y;
164  Q.Y=B[i].Y;
165  P.Z=B[j].Z;
166  Q.Z=B[j].Z;
167  P.X=B[0].X;
168  Q.X=B[1].X;
169  World->PersistentLineBatcher->DrawLine(
170  Transform.TransformPosition(P),
171  Transform.TransformPosition(Q),
172  Color,
174  Thickness,
175  LifeTime);
176 
177  P.Z=B[i].Z;
178  Q.Z=B[i].Z;
179  P.X=B[j].X;
180  Q.X=B[j].X;
181  P.Y=B[0].Y;
182  Q.Y=B[1].Y;
183  World->PersistentLineBatcher->DrawLine(
184  Transform.TransformPosition(P),
185  Transform.TransformPosition(Q),
186  Color,
188  Thickness,
189  LifeTime);
190  }
191  }
192  }
193 
194  void operator()(const Shape::String &Str) const
195  {
196  auto PlayerController = UGameplayStatics::GetPlayerController(World, 0);
197  if (PlayerController == nullptr)
198  {
199  UE_LOG(LogCarla, Error, TEXT("Can't find player controller!"));
200  return;
201  }
202  FVector Location = FVector(Str.location);
204  if (LargeMap)
205  {
206  Location = LargeMap->GlobalToLocalLocation(Location);
207  }
208  ACarlaHUD *Hud = Cast<ACarlaHUD>(PlayerController->GetHUD());
209  Hud->AddHUDString(carla::rpc::ToFString(Str.text), Location, Color.Quantize(), LifeTime);
210  }
211 
212 private:
213 
214  UWorld *World;
215 
216  FLinearColor Color;
217 
218  float LifeTime;
219 
221 
222  uint8 DepthPriority = SDPG_World;
223 
224  // Debug lines are way more dark in the package, that's why this
225  // multiplier is needed.
226 #if UE_BUILD_SHIPPING
227  static constexpr double BrightMultiplier = 1000.0;
228 #else
229  // @TODO: Use UKismetSystemLibrary::IsStandalone to support colors
230  // in Editor's standalone mode.
231  static constexpr double BrightMultiplier = 1.0;
232 #endif
233 
234 };
235 
237 {
238  auto Visitor = FShapeVisitor(World, Shape.color, Shape.life_time, Shape.persistent_lines);
239  boost::variant2::visit(Visitor, Shape.primitive);
240 }
static constexpr double BrightMultiplier
void operator()(const Shape::Line &Line) const
void AddHUDString(const FString Str, const FVector Location, const FColor Color, double LifeTime)
Definition: CarlaHUD.cpp:48
void operator()(const Shape::Arrow &Arrow) const
geom::BoundingBox box
Definition: DebugShape.h:52
FShapeVisitor(UWorld &InWorld, FColor InColor, float InLifeTime, bool bInPersistentLines)
carla::rpc::DebugShape Shape
void operator()(const Shape::Box &Box) const
Location location
Center of the BoundingBox in local space.
FVector GlobalToLocalLocation(const FVector &InLocation) const
Vector3D extent
Half the size of the BoundingBox in local space.
void operator()(const Shape::Point &Point) const
bg::model::box< Point3D > Box
Definition: InMemoryMap.h:45
geom::Location Location
Definition: rpc/Location.h:14
void Draw(const carla::rpc::DebugShape &Shape)
void operator()(const Shape::String &Str) const
FLinearColor Color
FTransform GlobalToLocalTransform(const FTransform &InTransform) const
static ALargeMapManager * GetLargeMapManager(const UObject *WorldContextObject)
Definition: CarlaStatics.h:100
boost::variant2::variant< Point, Line, Arrow, Box, String > primitive
Definition: DebugShape.h:65
geom::Rotation rotation
Definition: DebugShape.h:53
Class to draw on HUD.
Definition: CarlaHUD.h:39
geom::Transform Transform
Definition: rpc/Transform.h:16