diff --git a/Docs/python_api.md b/Docs/python_api.md index 4cf0c3961..00f8ca9ae 100644 --- a/Docs/python_api.md +++ b/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)` diff --git a/LibCarla/source/carla/client/DebugHelper.cpp b/LibCarla/source/carla/client/DebugHelper.cpp new file mode 100644 index 000000000..53a3868ce --- /dev/null +++ b/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 . + +#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 + 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 diff --git a/LibCarla/source/carla/client/DebugHelper.h b/LibCarla/source/carla/client/DebugHelper.h new file mode 100644 index 000000000..dbd3f0a6c --- /dev/null +++ b/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 . + +#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 diff --git a/LibCarla/source/carla/client/World.h b/LibCarla/source/carla/client/World.h index 574200134..3c49805ca 100644 --- a/LibCarla/source/carla/client/World.h +++ b/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 callback); + DebugHelper MakeDebugHelper() const { + return DebugHelper{_episode}; + } + private: detail::EpisodeProxy _episode; diff --git a/LibCarla/source/carla/client/detail/Client.cpp b/LibCarla/source/carla/client/detail/Client.cpp index f1415d0c1..60b812573 100644 --- a/LibCarla/source/carla/client/detail/Client.cpp +++ b/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 diff --git a/LibCarla/source/carla/client/detail/Client.h b/LibCarla/source/carla/client/detail/Client.h index 4cfeaca02..324561f17 100644 --- a/LibCarla/source/carla/client/detail/Client.h +++ b/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; diff --git a/LibCarla/source/carla/client/detail/Simulator.h b/LibCarla/source/carla/client/detail/Simulator.h index 007207bf8..3590ff15f 100644 --- a/LibCarla/source/carla/client/detail/Simulator.h +++ b/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: diff --git a/LibCarla/source/carla/rpc/Color.h b/LibCarla/source/carla/rpc/Color.h new file mode 100644 index 000000000..cad43505a --- /dev/null +++ b/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 . + +#pragma once + +#include "carla/MsgPack.h" + +#include + +#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 diff --git a/LibCarla/source/carla/rpc/DebugShape.h b/LibCarla/source/carla/rpc/DebugShape.h new file mode 100644 index 000000000..84e1d1ce7 --- /dev/null +++ b/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 . + +#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 + +// #include + +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 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 diff --git a/LibCarla/source/carla/rpc/Variant.h b/LibCarla/source/carla/rpc/Variant.h new file mode 100644 index 000000000..1628cb6bc --- /dev/null +++ b/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 . + +#pragma once + +#include "carla/MsgPack.h" + +#include + +namespace carla { +namespace rpc { + + /// @todo Workaround for packing boost::variant; it uses tuple instead so it's + /// quite inefficient memory-wise. + template + class Variant { + private: + + size_t _index = 0u; + + std::tuple _tuple; + + template + struct IndexFromType; + + template + struct IndexFromType> { + static constexpr size_t value = 0u; + }; + + template + struct IndexFromType> { + static constexpr size_t value = 1u + IndexFromType>::value; + }; + + template + void ApplyVisitorImpl(VisitorT &visitor, std::index_sequence) const { + std::initializer_list ({ (_index == Is ? visitor(std::get(_tuple)), 0 : 0)... }); + } + + public: + + Variant() = default; + + template + Variant(ObjT &&rhs) { + (*this) = std::forward(rhs); + } + + template + Variant &operator=(ObjT &&rhs) { + constexpr auto index = IndexFromType::type, decltype(_tuple)>::value; + _index = index; + std::get(_tuple) = std::forward(rhs); + return *this; + } + + template + void ApplyVisitor(VisitorT visitor) const { + return ApplyVisitorImpl(visitor, std::make_index_sequence()); + } + + MSGPACK_DEFINE_ARRAY(_index, _tuple); + }; + +} // namespace rpc +} // namespace carla diff --git a/LibCarla/source/carla/sensor/data/Color.h b/LibCarla/source/carla/sensor/data/Color.h index bdd0fb8d2..1b9eb591b 100644 --- a/LibCarla/source/carla/sensor/data/Color.h +++ b/LibCarla/source/carla/sensor/data/Color.h @@ -6,6 +6,8 @@ #pragma once +#include "carla/rpc/Color.h" + #include 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; diff --git a/PythonAPI/source/libcarla/World.cpp b/PythonAPI/source/libcarla/World.cpp index 4f428615d..615b17753 100644 --- a/PythonAPI/source/libcarla/World.cpp +++ b/PythonAPI/source/libcarla/World.cpp @@ -84,6 +84,7 @@ void export_world() { class_("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_("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)) + ; } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp index a8293c1f5..86338ce4b 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp +++ b/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 #include #include +#include #include #include #include @@ -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); + }); } // ============================================================================= diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/DebugShapeDrawer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/DebugShapeDrawer.cpp new file mode 100644 index 000000000..7265463f2 --- /dev/null +++ b/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 . + +#include "Carla.h" +#include "Carla/Util/DebugShapeDrawer.h" + +#include "DrawDebugHelpers.h" + +#include +#include +#include +#include + +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); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/DebugShapeDrawer.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/DebugShapeDrawer.h new file mode 100644 index 000000000..d3126e5e1 --- /dev/null +++ b/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 . + +#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; +};