Make possible to draw UE4 debug shapes from Python API

Docs/python_api.md

@@ -17,6 +17,7 @@
 
 - `id`
 - `map_name`
+- `debug`
 - `get_blueprint_library()`
 - `get_spectator()`
 - `get_weather()`
@@ -27,6 +28,14 @@
 - `wait_for_tick(seconds=1.0)`
 - `on_tick(callback)`
 
+## `carla.DebugHelper`
+
+- `draw_point(location, size=0.1, color={Red}, life_time=-1.0, persistent_lines=True)`
+- `draw_line(begin, end, thickness=0.1, color={Red}, life_time=-1.0, persistent_lines=True)`
+- `draw_arrow(begin, end, thickness=0.1, arrow_size=0.1, color={Red}, life_time=-1.0, persistent_lines=True)`
+- `draw_box(box, rotation, thickness=0.1, color={Red}, life_time=-1.0, persistent_lines=True)`
+- `draw_string(location, text, draw_shadow=False, color={Red}, life_time=-1.0, persistent_lines=True)`
+
 ## `carla.BlueprintLibrary`
 
 - `find(id)`

LibCarla/source/carla/client/DebugHelper.cpp

@@ -0,0 +1,85 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#include "carla/client/DebugHelper.h"
+
+#include "carla/client/detail/Simulator.h"
+#include "carla/rpc/DebugShape.h"
+
+namespace carla {
+namespace client {
+
+  using Shape = rpc::DebugShape;
+
+  template <typename T>
+  static void DrawShape(
+      detail::EpisodeProxy &episode,
+      const T &primitive,
+      rpc::Color color,
+      float life_time,
+      bool persistent_lines) {
+    const Shape shape{primitive, color, life_time, persistent_lines};
+    episode.Lock()->DrawDebugShape(shape);
+  }
+
+  void DebugHelper::DrawPoint(
+      const geom::Location &location,
+      float size,
+      sensor::data::Color color,
+      float life_time,
+      bool persistent_lines) {
+    Shape::Point point{location, size};
+    DrawShape(_episode, point, color, life_time, persistent_lines);
+  }
+
+  void DebugHelper::DrawLine(
+      const geom::Location &begin,
+      const geom::Location &end,
+      float thickness,
+      sensor::data::Color color,
+      float life_time,
+      bool persistent_lines) {
+    Shape::Line line{begin, end, thickness};
+    DrawShape(_episode, line, color, life_time, persistent_lines);
+  }
+
+  void DebugHelper::DrawArrow(
+      const geom::Location &begin,
+      const geom::Location &end,
+      float thickness,
+      float arrow_size,
+      sensor::data::Color color,
+      float life_time,
+      bool persistent_lines) {
+    Shape::Line line{begin, end, thickness};
+    Shape::Arrow arrow{line, arrow_size};
+    DrawShape(_episode, arrow, color, life_time, persistent_lines);
+  }
+
+  void DebugHelper::DrawBox(
+      const geom::BoundingBox &box,
+      const geom::Rotation &rotation,
+      float thickness,
+      sensor::data::Color color,
+      float life_time,
+      bool persistent_lines) {
+    Shape::Box the_box{box, rotation, thickness};
+    DrawShape(_episode, the_box, color, life_time, persistent_lines);
+  }
+
+  void DebugHelper::DrawString(
+      const geom::Location &location,
+      const std::string &text,
+      bool draw_shadow,
+      sensor::data::Color color,
+      float life_time,
+      bool persistent_lines) {
+    Shape::String string{location, text, draw_shadow};
+    DrawShape(_episode, string, color, life_time, persistent_lines);
+  }
+
+} // namespace client
+} // namespace carla

LibCarla/source/carla/client/DebugHelper.h

@@ -0,0 +1,72 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/client/detail/EpisodeProxy.h"
+#include "carla/geom/BoundingBox.h"
+#include "carla/geom/Location.h"
+#include "carla/geom/Rotation.h"
+#include "carla/sensor/data/Color.h"
+
+namespace carla {
+namespace client {
+
+  class DebugHelper {
+  public:
+
+    using Color = sensor::data::Color;
+
+    explicit DebugHelper(detail::EpisodeProxy episode)
+      : _episode(std::move(episode)) {}
+
+    void DrawPoint(
+        const geom::Location &location,
+        float size = 0.1f,
+        Color color = Color{255u, 0u, 0u},
+        float life_time = -1.0f,
+        bool persistent_lines = true);
+
+    void DrawLine(
+        const geom::Location &begin,
+        const geom::Location &end,
+        float thickness = 0.1f,
+        Color color = Color{255u, 0u, 0u},
+        float life_time = -1.0f,
+        bool persistent_lines = true);
+
+    void DrawArrow(
+        const geom::Location &begin,
+        const geom::Location &end,
+        float thickness = 0.1f,
+        float arrow_size = 0.1f,
+        Color color = Color{255u, 0u, 0u},
+        float life_time = -1.0f,
+        bool persistent_lines = true);
+
+    void DrawBox(
+        const geom::BoundingBox &box,
+        const geom::Rotation &rotation,
+        float thickness = 0.1f,
+        Color color = Color{255u, 0u, 0u},
+        float life_time = -1.0f,
+        bool persistent_lines = true);
+
+    void DrawString(
+        const geom::Location &location,
+        const std::string &text,
+        bool draw_shadow = false,
+        Color color = Color{255u, 0u, 0u},
+        float life_time = -1.0f,
+        bool persistent_lines = true);
+
+  private:
+
+    detail::EpisodeProxy _episode;
+  };
+
+} // namespace client
+} // namespace carla

LibCarla/source/carla/client/World.h

@@ -8,6 +8,7 @@
 
 #include "carla/Memory.h"
 #include "carla/Time.h"
+#include "carla/client/DebugHelper.h"
 #include "carla/client/Timestamp.h"
 #include "carla/client/detail/EpisodeProxy.h"
 #include "carla/geom/Transform.h"
@@ -79,6 +80,10 @@ namespace client {
     /// Register a @a callback to be called every time a world tick is received.
     void OnTick(std::function<void(Timestamp)> callback);
 
+    DebugHelper MakeDebugHelper() const {
+      return DebugHelper{_episode};
+    }
+
   private:
 
     detail::EpisodeProxy _episode;

LibCarla/source/carla/client/detail/Client.cpp

@@ -9,6 +9,7 @@
 #include "carla/Version.h"
 #include "carla/rpc/ActorDescription.h"
 #include "carla/rpc/Client.h"
+#include "carla/rpc/DebugShape.h"
 #include "carla/rpc/VehicleControl.h"
 #include "carla/streaming/Client.h"
 
@@ -148,6 +149,10 @@ namespace detail {
     _pimpl->streaming_client.UnSubscribe(token);
   }
 
+  void Client::DrawDebugShape(const rpc::DebugShape &shape) {
+    _pimpl->AsyncCall("draw_debug_shape", shape);
+  }
+
 } // namespace detail
 } // namespace client
 } // namespace carla

LibCarla/source/carla/client/detail/Client.h

@@ -25,6 +25,7 @@ namespace carla {
   class Buffer;
 namespace rpc {
   class ActorDescription;
+  class DebugShape;
   class VehicleControl;
 }
 namespace sensor { class SensorData; }
@@ -104,6 +105,8 @@ namespace detail {
 
     void UnSubscribeFromStream(const streaming::Token &token);
 
+    void DrawDebugShape(const rpc::DebugShape &shape);
+
   private:
 
     class Pimpl;

LibCarla/source/carla/client/detail/Simulator.h

@@ -212,6 +212,16 @@ namespace detail {
     void UnSubscribeFromSensor(const Sensor &sensor);
 
     /// @}
+    // =========================================================================
+    /// @name Debug
+    // =========================================================================
+    /// @{
+
+    void DrawDebugShape(const rpc::DebugShape &shape) {
+      _client.DrawDebugShape(shape);
+    }
+
+    /// @}
 
   private:
 

LibCarla/source/carla/rpc/Color.h

@@ -0,0 +1,50 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/MsgPack.h"
+
+#include <cstdint>
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+#  include "Math/Color.h"
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+namespace carla {
+namespace rpc {
+
+  class Color {
+  public:
+
+    uint8_t r = 0u;
+    uint8_t g = 0u;
+    uint8_t b = 0u;
+
+    Color() = default;
+    Color(const Color &) = default;
+
+    Color(uint8_t r, uint8_t g, uint8_t b)
+      : r(r), g(g), b(b) {}
+
+    Color &operator=(const Color &) = default;
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+
+    Color(const FColor &color)
+      : Color(color.R, color.G, color.B) {}
+
+    operator FColor() const {
+      return FColor{r, g, b};
+    }
+
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+    MSGPACK_DEFINE_ARRAY(r, g, b);
+  };
+
+} // namespace rpc
+} // namespace carla

LibCarla/source/carla/rpc/DebugShape.h

@@ -0,0 +1,70 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/geom/BoundingBox.h"
+#include "carla/geom/Location.h"
+#include "carla/geom/Rotation.h"
+#include "carla/rpc/Color.h"
+#include "carla/rpc/Variant.h"
+
+// #include <rpc/msgpack/adaptor/boost/msgpack_variant.hpp>
+
+// #include <boost/variant.hpp>
+
+namespace carla {
+namespace rpc {
+
+  class DebugShape {
+  public:
+
+    struct Point {
+      geom::Location location;
+      float size;
+      MSGPACK_DEFINE_ARRAY(location, size);
+    };
+
+    struct Line {
+      geom::Location begin;
+      geom::Location end;
+      float thickness;
+      MSGPACK_DEFINE_ARRAY(begin, end, thickness);
+    };
+
+    struct Arrow {
+      Line line;
+      float arrow_size;
+      MSGPACK_DEFINE_ARRAY(line, arrow_size);
+    };
+
+    struct Box {
+      geom::BoundingBox box;
+      geom::Rotation rotation;
+      float thickness;
+      MSGPACK_DEFINE_ARRAY(box, rotation, thickness);
+    };
+
+    struct String {
+      geom::Location location;
+      std::string text;
+      bool draw_shadow;
+      MSGPACK_DEFINE_ARRAY(location, text, draw_shadow);
+    };
+
+    Variant<Point, Line, Arrow, Box, String> primitive;
+
+    Color color = {255u, 0u, 0u};
+
+    float life_time = -1.0f;
+
+    bool persistent_lines = true;
+
+    MSGPACK_DEFINE_ARRAY(primitive, color, life_time, persistent_lines);
+  };
+
+} // namespace rpc
+} // namespace carla

LibCarla/source/carla/rpc/Variant.h

@@ -0,0 +1,70 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/MsgPack.h"
+
+#include <tuple>
+
+namespace carla {
+namespace rpc {
+
+  /// @todo Workaround for packing boost::variant; it uses tuple instead so it's
+  /// quite inefficient memory-wise.
+  template <typename... Ts>
+  class Variant {
+  private:
+
+    size_t _index = 0u;
+
+    std::tuple<Ts...> _tuple;
+
+    template <typename T, typename Tuple>
+    struct IndexFromType;
+
+    template <typename T, typename... Types>
+    struct IndexFromType<T, std::tuple<T, Types...>> {
+      static constexpr size_t value = 0u;
+    };
+
+    template <typename T, typename U, typename... Types>
+    struct IndexFromType<T, std::tuple<U, Types...>> {
+      static constexpr size_t value = 1u + IndexFromType<T, std::tuple<Types...>>::value;
+    };
+
+    template <typename VisitorT, size_t... Is>
+    void ApplyVisitorImpl(VisitorT &visitor, std::index_sequence<Is...>) const {
+      std::initializer_list<int> ({ (_index == Is ? visitor(std::get<Is>(_tuple)), 0 : 0)... });
+    }
+
+  public:
+
+    Variant() = default;
+
+    template <typename ObjT>
+    Variant(ObjT &&rhs) {
+      (*this) = std::forward<ObjT>(rhs);
+    }
+
+    template <typename ObjT>
+    Variant &operator=(ObjT &&rhs) {
+      constexpr auto index = IndexFromType<typename std::decay<ObjT>::type, decltype(_tuple)>::value;
+      _index = index;
+      std::get<index>(_tuple) = std::forward<ObjT>(rhs);
+      return *this;
+    }
+
+    template <typename VisitorT>
+    void ApplyVisitor(VisitorT visitor) const {
+      return ApplyVisitorImpl(visitor, std::make_index_sequence<sizeof...(Ts)>());
+    }
+
+    MSGPACK_DEFINE_ARRAY(_index, _tuple);
+  };
+
+} // namespace rpc
+} // namespace carla

LibCarla/source/carla/sensor/data/Color.h

@@ -6,6 +6,8 @@
 
 #pragma once
 
+#include "carla/rpc/Color.h"
+
 #include <cstdint>
 
 namespace carla {
@@ -31,6 +33,10 @@ namespace data {
       return !(*this == rhs);
     }
 
+    operator rpc::Color() const {
+      return {r, g, b};
+    }
+
     uint8_t b = 0u;
     uint8_t g = 0u;
     uint8_t r = 0u;

PythonAPI/source/libcarla/World.cpp

@@ -84,6 +84,7 @@ void export_world() {
   class_<cc::World>("World", no_init)
     .add_property("id", &cc::World::GetId)
     .add_property("map_name", CALL_RETURNING_COPY(cc::World, GetMapName))
+    .add_property("debug", &cc::World::MakeDebugHelper)
     .def("get_blueprint_library", CONST_CALL_WITHOUT_GIL(cc::World, GetBlueprintLibrary))
     .def("get_spectator", CONST_CALL_WITHOUT_GIL(cc::World, GetSpectator))
     .def("get_weather", CONST_CALL_WITHOUT_GIL(cc::World, GetWeather))
@@ -97,4 +98,42 @@ void export_world() {
   ;
 
 #undef SPAWN_ACTOR_WITHOUT_GIL
+
+  class_<cc::DebugHelper>("DebugHelper", no_init)
+    .def("draw_point", &cc::DebugHelper::DrawPoint,
+        (arg("location"),
+         arg("size")=0.1f,
+         arg("color")=cc::DebugHelper::Color(255u, 0u, 0u),
+         arg("life_time")=-1.0f,
+         arg("persistent_lines")=true))
+    .def("draw_line", &cc::DebugHelper::DrawLine,
+        (arg("begin"),
+         arg("end"),
+         arg("thickness")=0.1f,
+         arg("color")=cc::DebugHelper::Color(255u, 0u, 0u),
+         arg("life_time")=-1.0f,
+         arg("persistent_lines")=true))
+    .def("draw_arrow", &cc::DebugHelper::DrawArrow,
+        (arg("begin"),
+         arg("end"),
+         arg("thickness")=0.1f,
+         arg("arrow_size")=0.1f,
+         arg("color")=cc::DebugHelper::Color(255u, 0u, 0u),
+         arg("life_time")=-1.0f,
+         arg("persistent_lines")=true))
+    .def("draw_box", &cc::DebugHelper::DrawBox,
+        (arg("box"),
+         arg("rotation"),
+         arg("thickness")=0.1f,
+         arg("color")=cc::DebugHelper::Color(255u, 0u, 0u),
+         arg("life_time")=-1.0f,
+         arg("persistent_lines")=true))
+    .def("draw_string", &cc::DebugHelper::DrawString,
+        (arg("location"),
+         arg("text"),
+         arg("draw_shadow")=false,
+         arg("color")=cc::DebugHelper::Color(255u, 0u, 0u),
+         arg("life_time")=-1.0f,
+         arg("persistent_lines")=true))
+  ;
 }

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp

@@ -8,6 +8,7 @@
 #include "Carla/Server/TheNewCarlaServer.h"
 
 #include "Carla/Sensor/Sensor.h"
+#include "Carla/Util/DebugShapeDrawer.h"
 #include "Carla/Vehicle/CarlaWheeledVehicle.h"
 
 #include "GameFramework/SpectatorPawn.h"
@@ -17,6 +18,7 @@
 #include <carla/rpc/Actor.h>
 #include <carla/rpc/ActorDefinition.h>
 #include <carla/rpc/ActorDescription.h>
+#include <carla/rpc/DebugShape.h>
 #include <carla/rpc/EpisodeInfo.h>
 #include <carla/rpc/Server.h>
 #include <carla/rpc/Transform.h>
@@ -364,6 +366,14 @@ void FTheNewCarlaServer::FPimpl::BindActions()
     }
     Controller->SetAutopilot(bEnabled);
   });
+
+  Server.BindSync("draw_debug_shape", [this](const cr::DebugShape &shape) {
+    RequireEpisode();
+    auto *World = Episode->GetWorld();
+    check(World != nullptr);
+    FDebugShapeDrawer Drawer(*World);
+    Drawer.Draw(shape);
+  });
 }
 
 // =============================================================================

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/DebugShapeDrawer.cpp

@@ -0,0 +1,110 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#include "Carla.h"
+#include "Carla/Util/DebugShapeDrawer.h"
+
+#include "DrawDebugHelpers.h"
+
+#include <compiler/disable-ue4-macros.h>
+#include <carla/rpc/DebugShape.h>
+#include <carla/rpc/String.h>
+#include <compiler/enable-ue4-macros.h>
+
+struct FShapeVisitor
+{
+  using Shape = carla::rpc::DebugShape;
+
+  FShapeVisitor(UWorld &InWorld, FColor InColor, float InLifeTime, bool bInPersistentLines)
+    : World(&InWorld),
+      Color(InColor),
+      LifeTime(InLifeTime),
+      bPersistentLines(bInPersistentLines) {}
+
+  void operator()(const Shape::Point &Point) const
+  {
+    DrawDebugPoint(
+        World,
+        Point.location,
+        1e2f * Point.size,
+        Color,
+        bPersistentLines,
+        LifeTime,
+        DepthPriority);
+  }
+
+  void operator()(const Shape::Line &Line) const
+  {
+    DrawDebugLine(
+        World,
+        Line.begin,
+        Line.end,
+        Color,
+        bPersistentLines,
+        LifeTime,
+        DepthPriority,
+        1e2f * Line.thickness);
+  }
+
+  void operator()(const Shape::Arrow &Arrow) const
+  {
+    DrawDebugDirectionalArrow(
+        World,
+        Arrow.line.begin,
+        Arrow.line.end,
+        1e2f * Arrow.arrow_size,
+        Color,
+        bPersistentLines,
+        LifeTime,
+        DepthPriority,
+        1e2f * Arrow.line.thickness);
+  }
+
+  void operator()(const Shape::Box &Box) const
+  {
+    FVector Extent = {Box.box.extent.x, Box.box.extent.y, Box.box.extent.z};
+    DrawDebugBox(
+        World,
+        Box.box.location,
+        1e2f * Extent,
+        FQuat(FRotator(Box.rotation)),
+        Color,
+        bPersistentLines,
+        LifeTime,
+        DepthPriority,
+        1e2f * Box.thickness);
+  }
+
+  void operator()(const Shape::String &Str) const
+  {
+    DrawDebugString(
+        World,
+        Str.location,
+        carla::rpc::ToFString(Str.text),
+        nullptr,
+        Color,
+        LifeTime,
+        Str.draw_shadow);
+  }
+
+private:
+
+  UWorld *World;
+
+  FColor Color;
+
+  float LifeTime;
+
+  bool bPersistentLines;
+
+  uint8 DepthPriority = 0;
+};
+
+void FDebugShapeDrawer::Draw(const carla::rpc::DebugShape &Shape)
+{
+  auto Visitor = FShapeVisitor(World, Shape.color, Shape.life_time, Shape.persistent_lines);
+  Shape.primitive.ApplyVisitor(Visitor);
+}

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/DebugShapeDrawer.h

@@ -0,0 +1,24 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+class UWorld;
+
+namespace carla { namespace rpc { class DebugShape; }}
+
+class FDebugShapeDrawer
+{
+public:
+
+  explicit FDebugShapeDrawer(UWorld &InWorld) : World(InWorld) {}
+
+  void Draw(const carla::rpc::DebugShape &Shape);
+
+private:
+
+  UWorld &World;
+};