diff --git a/.travis.yml b/.travis.yml index 43c7f26b4..605906bc4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -13,10 +13,10 @@ matrix: apt: sources: - ubuntu-toolchain-r-test - - llvm-toolchain-trusty-5.0 + - llvm-toolchain-trusty-6.0 packages: - g++-7 # we need this one for the libstdc++. - - clang-5.0 + - clang-6.0 - ninja-build - python - python-pip @@ -38,18 +38,6 @@ matrix: after_failure: - tail --lines=2000 build.log - - env: TEST="Pylint 2" - addons: - apt: - packages: - - python - - python-pip - install: - - pip2 install -q --user -r Deprecated/PythonClient/requirements.txt - - pip2 install -q --user pylint - script: - - pylint --disable=R,C --rcfile=Deprecated/PythonClient/.pylintrc Deprecated/PythonClient/carla Deprecated/PythonClient/*.py - - env: TEST="MkDocs" install: - pip install -q --user mkdocs diff --git a/CHANGELOG.md b/CHANGELOG.md index b5615db8a..7e2469355 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,20 @@ -## Latest Release Version +## Latest changes - * Added point transformation functionality for LibCarla and PythonAPI. + * Upgraded to Unreal Engine 4.21 + * Upgraded Boost to 1.69.0 + * Added point transformation functionality for LibCarla and PythonAPI + * Added "sensor_tick" attribute to sensors (cameras and lidars) to specify the capture rate in seconds + * Added "get_forward_vector()" to rotation and transform, retrieves the unit vector on the rotation's X-axis + * Added support for Deepin in PythonAPI's setup.py + * Added support for spawning and controlling walkers (pedestrians) + * Renamed vehicle.get_vehicle_control() to vehicle.get_control() to be consistent with walkers + * Added a few methods to manage an actor: + - set_velocity: for setting the linear velocity + - set_angular_velocity: for setting the angular velocity + - get_angular_velocity: for getting the angular velocity + - add_impulse: for applying an impulse (in world axis) + * Added support for gnss_sensor + * OpenDriveActor has been rewritten using the Waypoint API, this has fixed some bugs ## CARLA 0.9.2 diff --git a/Docs/build_system.md b/Docs/build_system.md index 21c13f8d4..b3fe24ab4 100644 --- a/Docs/build_system.md +++ b/Docs/build_system.md @@ -11,7 +11,7 @@ process. ![modules](img/modules.png) -In Linux, we compile CARLA and all the dependencies with clang-5.0 and C++14 +In Linux, we compile CARLA and all the dependencies with clang-6.0 and C++14 standard. We however link against different runtime C++ libraries depending on where the code going to be used, since all the code that is going to be linked with Unreal Engine needs to be compiled using `libc++`. @@ -26,9 +26,9 @@ make setup Get and compile dependencies - * llvm-5.0 (libc++ and libc++abi) + * llvm-6.0 (libc++ and libc++abi) * rpclib-2.2.1 (twice, with libstdc++ and libc++) - * boost-1.67 (headers only) + * boost-1.69 (headers only) * googletest-1.8.0 (with libc++) #### LibCarla @@ -53,7 +53,7 @@ Two configurations: #### CarlaUE4 and Carla plugin -Both compiled at the same step with Unreal Engine 4.19 build tool. They require +Both compiled at the same step with Unreal Engine 4.21 build tool. They require the `UE4_ROOT` environment variable set. Command diff --git a/Docs/cameras_and_sensors.md b/Docs/cameras_and_sensors.md index 48b56b1aa..7d3360344 100644 --- a/Docs/cameras_and_sensors.md +++ b/Docs/cameras_and_sensors.md @@ -18,6 +18,8 @@ blueprint = world.get_blueprint_library().find('sensor.camera.rgb') blueprint.set_attribute('image_size_x', '1920') blueprint.set_attribute('image_size_y', '1080') blueprint.set_attribute('fov', '110') +# Set the time in seconds between sensor captures +blueprint.set_attribute('sensor_tick', '1.0') # Provide the position of the sensor relative to the vehicle. transform = carla.Transform(carla.Location(x=0.8, z=1.7)) # Tell the world to spawn the sensor, don't forget to attach it to your vehicle actor. @@ -58,6 +60,9 @@ The "RGB" camera acts as a regular camera capturing images from the scene. | `image_size_y` | int | 600 | Image height in pixels | | `fov` | float | 90.0 | Field of view in degrees | | `enable_postprocess_effects` | bool | True | Whether the post-process effect in the scene affect the image | +| `sensor_tick` | float | 0.0 | Seconds between sensor captures (ticks) | + +`sensor_tick` tells how fast we want the sensor to capture the data. A value of 1.5 means that we want the sensor to capture data each second and a half. By default a value of 0.0 means as fast as possible. If `enable_postprocess_effects` is enabled, a set of post-process effects is applied to the image to create a more realistic feel @@ -95,6 +100,7 @@ pixel to the camera (also known as **depth buffer** or **z-buffer**). | `image_size_x` | int | 800 | Image width in pixels | | `image_size_y` | int | 600 | Image height in pixels | | `fov` | float | 90.0 | Field of view in degrees | +| `sensor_tick` | float | 0.0 | Seconds between sensor captures (ticks) | This sensor produces [`carla.Image`](python_api.md#carlaimagecarlasensordata) objects. @@ -132,6 +138,7 @@ pedestrians appear in a different color than vehicles. | `image_size_x` | int | 800 | Image width in pixels | | `image_size_y` | int | 600 | Image height in pixels | | `fov` | float | 90.0 | Field of view in degrees | +| `sensor_tick` | float | 0.0 | Seconds between sensor captures (ticks) | This sensor produces [`carla.Image`](python_api.md#carlaimagecarlasensordata) objects. @@ -197,6 +204,7 @@ supposed to generate this frame; `points_per_second / (FPS * channels)`. | `rotation_frequency` | float | 10.0 | Lidar rotation frequency | | `upper_fov` | float | 10.0 | Angle in degrees of the upper most laser | | `lower_fov` | float | -30.0 | Angle in degrees of the lower most laser | +| `sensor_tick` | float | 0.0 | Seconds between sensor captures (ticks) | This sensor produces [`carla.LidarMeasurement`](python_api.md#carlalidarmeasurementcarlasensordata) @@ -277,3 +285,23 @@ object for each lane marking crossed by the actor | `transform` | carla.Transform | Transform in world coordinates of the sensor at the time of the measurement | | `actor` | carla.Actor | Actor that invaded another lane ("self" actor) | | `crossed_lane_markings` | carla.LaneMarking list | List of lane markings that have been crossed | + +sensor.other.gnss +----------------- + +This sensor, when attached to an actor, reports its current gnss position. +The gnss position is internally calculated by adding the metric position to +an initial geo reference location defined within the OpenDRIVE map definition. + +This sensor produces +[`carla.GnssEvent`](python_api.md#carlagnsseventcarlasensordata) +objects. + +| Sensor data attribute | Type | Description | +| ---------------------- | ----------- | ----------- | +| `frame_number` | int | Frame count when the measurement took place | +| `transform` | carla.Transform | Transform in world coordinates of the sensor at the time of the measurement | +| `latitude` | double | Latitude position of the actor | +| `longitude` | double | Longitude position of the actor | +| `altitude` | double | Altitude of the actor | + diff --git a/Docs/coding_standard.md b/Docs/coding_standard.md index d8bcb483e..b2ebc12cd 100644 --- a/Docs/coding_standard.md +++ b/Docs/coding_standard.md @@ -27,7 +27,7 @@ C++ * Comments should not exceed 80 columns, code may exceed this limit a bit in rare occasions if it results in clearer code. * Compilation should not give any error or warning - (`clang++ -Wall -Wextra -std=C++14 -Wno-missing-braces`). + (`clang++-6.0 -Wall -Wextra -std=C++14 -Wno-missing-braces`). * Unreal C++ code (CarlaUE4 and Carla plugin) follow the [Unreal Engine's Coding Standard][ue4link] with the exception of using spaces instead of tabs. diff --git a/Docs/how_to_build_on_linux.md b/Docs/how_to_build_on_linux.md index 504b55880..82b62d2dc 100644 --- a/Docs/how_to_build_on_linux.md +++ b/Docs/how_to_build_on_linux.md @@ -8,20 +8,20 @@ Install the build tools and dependencies ``` sudo add-apt-repository ppa:ubuntu-toolchain-r/test sudo apt-get update -sudo apt-get install build-essential clang-5.0 lld-5.0 g++-7 cmake ninja-build python python-pip python-dev python3-dev python3-pip libpng16-dev libtiff5-dev libjpeg-dev tzdata sed curl wget unzip autoconf libtool +sudo apt-get install build-essential clang-6.0 lld-6.0 g++-7 cmake ninja-build python python-pip python-dev python3-dev python3-pip libpng16-dev libtiff5-dev libjpeg-dev tzdata sed curl wget unzip autoconf libtool pip2 install --user setuptools nose2 pip3 install --user setuptools nose2 ``` To avoid compatibility issues between Unreal Engine and the CARLA dependencies, the best configuration is to compile everything with the same compiler version -and C++ runtime library. We use clang 5.0 and LLVM's libc++. We recommend to +and C++ runtime library. We use clang 6.0 and LLVM's libc++. We recommend to change your default clang version to compile Unreal Engine and the CARLA dependencies ```sh -sudo update-alternatives --install /usr/bin/clang++ clang++ /usr/lib/llvm-5.0/bin/clang++ 101 -sudo update-alternatives --install /usr/bin/clang clang /usr/lib/llvm-5.0/bin/clang 101 +sudo update-alternatives --install /usr/bin/clang++ clang++ /usr/lib/llvm-6.0/bin/clang++ 102 +sudo update-alternatives --install /usr/bin/clang clang /usr/lib/llvm-6.0/bin/clang 102 ``` Build Unreal Engine @@ -32,13 +32,13 @@ Build Unreal Engine need to add your GitHub username when you sign up at [www.unrealengine.com](https://www.unrealengine.com). -Download and compile Unreal Engine 4.19. Here we will assume you install it at -`~/UnrealEngine_4.19", but you can install it anywhere, just replace the path +Download and compile Unreal Engine 4.21. Here we will assume you install it at +`~/UnrealEngine_4.21", but you can install it anywhere, just replace the path where necessary. ```sh -git clone --depth=1 -b 4.19 https://github.com/EpicGames/UnrealEngine.git ~/UnrealEngine_4.19 -cd ~/UnrealEngine_4.19 +git clone --depth=1 -b 4.21 https://github.com/EpicGames/UnrealEngine.git ~/UnrealEngine_4.21 +cd ~/UnrealEngine_4.21 ./Setup.sh && ./GenerateProjectFiles.sh && make ``` @@ -71,7 +71,7 @@ For CARLA to find your Unreal Engine's installation folder you need to set the following environment variable ```sh -export UE4_ROOT=~/UnrealEngine_4.19 +export UE4_ROOT=~/UnrealEngine_4.21 ``` You can also add this variable to your `~/.bashrc` or `~/.profile`. diff --git a/Docs/how_to_build_on_windows.md b/Docs/how_to_build_on_windows.md index fa3499b62..76ffe78dd 100644 --- a/Docs/how_to_build_on_windows.md +++ b/Docs/how_to_build_on_windows.md @@ -15,7 +15,7 @@ Also: -- [Unreal Engine](https://www.unrealengine.com/download) (v4.19.x) +- [Unreal Engine](https://www.unrealengine.com/download) (v4.21.x) - [Visual Studio](https://www.visualstudio.com/downloads/) (2017)

Environment Setup

diff --git a/Docs/how_to_make_a_new_map.md b/Docs/how_to_make_a_new_map.md index ed396ddfb..b667eac22 100644 --- a/Docs/how_to_make_a_new_map.md +++ b/Docs/how_to_make_a_new_map.md @@ -1,4 +1,4 @@ -

How to make a new map in CARLA with RoadRunner

+

How to make a new map with RoadRunner

![Town03](img/create_map_01.jpg) @@ -7,7 +7,7 @@ RoadRunner is a powerful software from Vector Zero to create 3D scenes. Using RoadRunner is easy, in a few steps you will be able to create an impressive scene. You can download a trial of RoadRunner at: -![vectorzero](img/logo_vectorzero.jpg) https://www.vectorzero.io/ +![vectorzero](img/logo_vectorzero.jpg) [https://www.vectorzero.io/](https://www.vectorzero.io/)

Step 1 - Create your map in RoadRunner:

@@ -84,7 +84,7 @@ and save it inside Game/Carla/Maps/ Once everything is loaded up you should end with several staticMeshes in the folder you have selected. Drag them all to the level and you will have -your map in Unreal! Congratulations!. +your map in Unreal! Congratulations!. The time taken by the loading process will depend on how many pieces you cut the map into. Be patient! All the pieces share the same center pivot, so if they are positioned in the same place they should fit as they did in the RoadRunner Editor. When @@ -143,8 +143,8 @@ for the meshes the last one was referencing.

Setting up the traffic based on OpenDrive:

!!! Note - the current version of CARLA needs a CarlaMapGenerator spawned in order to use the server autopilot functionality. From the current version it is also to drive all vehicles using the new client nvaigation stack. However, if you still want to use the server autopilot you will need to create a CarlaMapGenerator for your city --never above or under it. - + the current version of CARLA needs a CarlaMapGenerator spawned in order to use the server autopilot functionality. From the current version it is also to drive all vehicles using the new client nvaigation stack. However, if you still want to use the server autopilot you will need to create a CarlaMapGenerator for your city --never above or under it. + * You will need to place points for the vehicles to spawn. The actor that sets the spawn position of the vehicles is called VehicleSpawnPoint. CARLA vehicles must overlap with one of the RoutePlanner's trigger box for them to be @@ -161,7 +161,7 @@ for the meshes the last one was referencing. * Traffic lights must be placed in every crossing in whitch vehicles might conflict. theese are the childs of "TrafficLightBase" and are stored inside Game/Carla/Static/TrafficSigns/Streetlights_01. - + Again, remember this is just needed to use the server autopilot. We recommend to start using new the client driving stack provided from 0.9.2 version.

Working with BP_TrafficLight and BP_TrafficLightGroups:

diff --git a/Docs/index.md b/Docs/index.md index 609982360..dbbea3d97 100644 --- a/Docs/index.md +++ b/Docs/index.md @@ -24,6 +24,7 @@ * [Python API reference](python_api.md) * [Running without display and selecting GPUs](carla_headless.md) * [Running in a Docker](carla_docker.md) + * [How to make a new map with RoadRunner](how_to_make_a_new_map.md) * [How to link Epic's Automotive Materials](epic_automotive_materials.md)

Contributing

diff --git a/Docs/python_api.md b/Docs/python_api.md index 20d87d571..381c42b9d 100644 --- a/Docs/python_api.md +++ b/Docs/python_api.md @@ -106,7 +106,7 @@ - `bounding_box` - `apply_control(vehicle_control)` -- `get_vehicle_control()` +- `get_control()` - `set_autopilot(enabled=True)` ## `carla.TrafficLight(carla.Actor)` @@ -160,6 +160,12 @@ - `actor` - `crossed_lane_markings` +## `carla.GnssEvent(carla.SensorData)` + +- `latitude` +- `longitude` +- `altitude` + ## `carla.VehicleControl` - `throttle` @@ -243,6 +249,7 @@ Static presets - `pitch` - `yaw` - `roll` +- `get_forward_vector()` - `__eq__(other)` - `__ne__(other)` @@ -250,10 +257,10 @@ Static presets - `location` - `rotation` +- `transform(geom_object)` +- `get_forward_vector()` - `__eq__(other)` - `__ne__(other)` -- `transform_point` -- `transform_point_list` ## `carla.BoundingBox` diff --git a/Jenkinsfile b/Jenkinsfile index 8180d158f..bc106bac0 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -2,7 +2,7 @@ pipeline { agent any environment { - UE4_ROOT = '/var/lib/jenkins/UnrealEngine_4.19' + UE4_ROOT = '/var/lib/jenkins/UnrealEngine_4.21' } options { diff --git a/LibCarla/source/carla/PythonUtil.h b/LibCarla/source/carla/PythonUtil.h index 528bf8470..b667feca9 100644 --- a/LibCarla/source/carla/PythonUtil.h +++ b/LibCarla/source/carla/PythonUtil.h @@ -9,7 +9,14 @@ #include "carla/NonCopyable.h" #ifdef LIBCARLA_WITH_PYTHON_SUPPORT -# include +# if defined(__clang__) +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-register" +# endif +# include +# if defined(__clang__) +# pragma clang diagnostic pop +# endif #endif // LIBCARLA_WITH_PYTHON_SUPPORT namespace carla { diff --git a/LibCarla/source/carla/client/Actor.cpp b/LibCarla/source/carla/client/Actor.cpp index 8235d11fd..e66732f7b 100644 --- a/LibCarla/source/carla/client/Actor.cpp +++ b/LibCarla/source/carla/client/Actor.cpp @@ -24,6 +24,14 @@ namespace client { return GetEpisode().Lock()->GetActorVelocity(*this); } + geom::Vector3D Actor::GetAngularVelocity() const { + return GetEpisode().Lock()->GetActorAngularVelocity(*this); + } + + void Actor::SetAngularVelocity(const geom::Vector3D &vector) { + GetEpisode().Lock()->SetActorAngularVelocity(*this, vector); + } + geom::Vector3D Actor::GetAcceleration() const { return GetEpisode().Lock()->GetActorAcceleration(*this); } @@ -36,6 +44,14 @@ namespace client { GetEpisode().Lock()->SetActorTransform(*this, transform); } + void Actor::SetVelocity(const geom::Vector3D &vector) { + GetEpisode().Lock()->SetActorVelocity(*this, vector); + } + + void Actor::AddImpulse(const geom::Vector3D &vector) { + GetEpisode().Lock()->AddActorImpulse(*this, vector); + } + void Actor::SetSimulatePhysics(const bool enabled) { GetEpisode().Lock()->SetActorSimulatePhysics(*this, enabled); } diff --git a/LibCarla/source/carla/client/Actor.h b/LibCarla/source/carla/client/Actor.h index a985e8d2e..0a1cfa73c 100644 --- a/LibCarla/source/carla/client/Actor.h +++ b/LibCarla/source/carla/client/Actor.h @@ -46,6 +46,12 @@ namespace client { /// velocity received in the last tick. geom::Vector3D GetVelocity() const; + /// Return the current 3D angular velocity of the actor. + /// + /// @note This function does not call the simulator, it returns the + /// angular velocity received in the last tick. + geom::Vector3D GetAngularVelocity() const; + /// Return the current 3D acceleration of the actor. /// /// @note This function does not call the simulator, it returns the @@ -58,9 +64,18 @@ namespace client { /// Teleport and rotate the actor to @a transform. void SetTransform(const geom::Transform &transform); + /// Set the actor velocity. + void SetVelocity(const geom::Vector3D &vector); + + /// Add impulse to the actor. + void AddImpulse(const geom::Vector3D &vector); + /// Enable or disable physics simulation on this actor. void SetSimulatePhysics(bool enabled = true); + /// Set the angular velocity of the actor + void SetAngularVelocity(const geom::Vector3D &vector); + /// @warning This method only checks whether this instance of Actor has /// called the Destroy() method, it does not check whether the actor is /// actually alive in the simulator. diff --git a/LibCarla/source/carla/client/GnssSensor.cpp b/LibCarla/source/carla/client/GnssSensor.cpp new file mode 100644 index 000000000..d5e3fdd2d --- /dev/null +++ b/LibCarla/source/carla/client/GnssSensor.cpp @@ -0,0 +1,160 @@ +// Copyright (c) 2019 Intel Labs. +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "carla/client/GnssSensor.h" + +#include "carla/Logging.h" +#include "carla/client/Map.h" +#include "carla/client/detail/Simulator.h" +#include "carla/geom/Math.h" +#include "carla/sensor/data/GnssEvent.h" +#include "carla/StringUtil.h" + +#include +#include +#include +#include + +#if defined(_WIN32) && !defined(_USE_MATH_DEFINES) +#define _USE_MATH_DEFINES +#include // cmath is not enough for MSVC +#endif + +namespace carla { +namespace client { + + const double EARTH_RADIUS_EQUA = 6378137.0;// earth radius at equator [m] + + // inlined functions to avoid multiple definitions + + /// @brief convert latitude to scale, which is needed by mercator transformations + /// @param lat latitude in degrees (DEG) + /// @return scale factor + /// @note when converting from lat/lon -> mercator and back again, + /// or vice versa, use the same scale in both transformations! + inline double LatToScale (double lat) { + return cos(lat * geom::Math::pi() / 180.0); + } + + /// @brief converts lat/lon/scale to mx/my (mx/my in meters if correct scale is given) + /// + template + inline void LatLonToMercator (double lat, double lon, double scale, float_type &mx, float_type &my) { + mx = scale * lon * geom::Math::pi() * EARTH_RADIUS_EQUA / 180.0; + my = scale * EARTH_RADIUS_EQUA * log( tan((90.0+lat) * geom::Math::pi() / 360.0) ); + } + + /// @brief converts mx/my/scale to lat/lon (mx/my in meters if correct scale is given) + inline void MercatorToLatLon (double mx, double my, double scale, double &lat, double &lon) { + lon = mx * 180.0 / (geom::Math::pi() * EARTH_RADIUS_EQUA * scale); + lat = 360.0 * atan( exp(my/(EARTH_RADIUS_EQUA * scale)) ) / geom::Math::pi() - 90.0; + } + + /// @brief adds meters dx/dy to given lat/lon and returns new lat/lon + inline void LatLonAddMeters (double lat_start, double lon_start, double dx, double dy, double &lat_end, double &lon_end) { + double scale = LatToScale (lat_start); + double mx,my; + LatLonToMercator (lat_start, lon_start, scale, mx, my); + mx += dx; + my += dy; + MercatorToLatLon (mx, my, scale, lat_end, lon_end); + } + + GnssSensor::~GnssSensor() = default; + + void GnssSensor::Listen(CallbackFunctionType callback) { + if (_is_listening) { + log_error(GetDisplayId(), ": already listening"); + return; + } + + if (GetParent() == nullptr) { + throw std::runtime_error(GetDisplayId() + ": not attached to vehicle"); + return; + } + + SharedPtr map = GetWorld().GetMap(); + + DEBUG_ASSERT(map != nullptr); + + auto self = boost::static_pointer_cast(shared_from_this()); + + //parse geo reference string + _map_latitude = std::numeric_limits::quiet_NaN(); + _map_longitude = std::numeric_limits::quiet_NaN(); + std::vector geo_ref_splitted; + StringUtil::Split(geo_ref_splitted, map->GetGeoReference(), " "); + + for (auto element: geo_ref_splitted) { + std::vector geo_ref_key_value; + StringUtil::Split(geo_ref_key_value, element, "="); + if (geo_ref_key_value.size() != 2u) { + continue; + } + std::istringstream istr(geo_ref_key_value[1]); + istr.imbue(std::locale("C")); + if (geo_ref_key_value[0] == "+lat_0") { + istr >> _map_latitude; + } else if (geo_ref_key_value[0] == "+lon_0") { + istr >> _map_longitude; + } + if (istr.fail() || !istr.eof()) { + _map_latitude = std::numeric_limits::quiet_NaN(); + _map_longitude = std::numeric_limits::quiet_NaN(); + } + } + + + if (std::isnan(_map_latitude) || std::isnan(_map_longitude)) { + log_warning(GetDisplayId(), ": cannot parse georeference: '" + map->GetGeoReference() + "'. Using default values."); + _map_latitude = 42.0; + _map_longitude = 2.0; + } + + log_debug(GetDisplayId(), ": map geo reference: latitude ", _map_latitude, ", longitude ", _map_longitude); + + log_debug(GetDisplayId(), ": subscribing to tick event"); + GetEpisode().Lock()->RegisterOnTickEvent([ + cb=std::move(callback), + weak_self=WeakPtr(self)](const auto ×tamp) { + auto self = weak_self.lock(); + if (self != nullptr) { + auto data = self->TickGnssSensor(timestamp); + if (data != nullptr) { + cb(std::move(data)); + } + } + }); + _is_listening = true; + } + + SharedPtr GnssSensor::TickGnssSensor( + const Timestamp ×tamp) { + try { + const auto location = GetLocation(); + double current_lat, current_lon; + + LatLonAddMeters(_map_latitude, _map_longitude, location.x, location.y, current_lat, current_lon); + + return MakeShared( + timestamp.frame_count, + GetTransform(), + current_lat, + current_lon, + location.z); + } catch (const std::exception &e) { + /// @todo We need to unsubscribe the sensor. + // log_error("LaneDetector:", e.what()); + return nullptr; + } + + } + + void GnssSensor::Stop() { + _is_listening = false; + } + +} // namespace client +} // namespace carla diff --git a/LibCarla/source/carla/client/GnssSensor.h b/LibCarla/source/carla/client/GnssSensor.h new file mode 100644 index 000000000..33147d723 --- /dev/null +++ b/LibCarla/source/carla/client/GnssSensor.h @@ -0,0 +1,54 @@ +// Copyright (c) 2019 Intel Labs. +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "carla/client/Sensor.h" + +namespace carla { +namespace client { + + class Map; + class Vehicle; + + class GnssSensor final : public Sensor { + public: + + using Sensor::Sensor; + + ~GnssSensor(); + + /// Register a @a callback to be executed each time a new measurement is + /// received. + /// + /// @warning Calling this function on a sensor that is already listening + /// steals the data stream from the previously set callback. Note that + /// several instances of Sensor (even in different processes) may point to + /// the same sensor in the simulator. + void Listen(CallbackFunctionType callback) override; + + /// Stop listening for new measurements. + void Stop() override; + + /// Return whether this Sensor instance is currently listening to the + /// associated sensor in the simulator. + bool IsListening() const override { + return _is_listening; + } + + private: + + SharedPtr TickGnssSensor(const Timestamp ×tamp); + + double _map_latitude; + + double _map_longitude; + + bool _is_listening = false; + + }; + +} // namespace client +} // namespace carla diff --git a/LibCarla/source/carla/client/Map.cpp b/LibCarla/source/carla/client/Map.cpp index 534a4a95e..ba6cb0de3 100644 --- a/LibCarla/source/carla/client/Map.cpp +++ b/LibCarla/source/carla/client/Map.cpp @@ -86,5 +86,10 @@ namespace client { return _map->CalculateCrossedLanes(origin, destination); } + + std::string Map::GetGeoReference() const { + DEBUG_ASSERT(_map != nullptr); + return _map->GetData().GetGeoReference(); + } } // namespace client } // namespace carla diff --git a/LibCarla/source/carla/client/Map.h b/LibCarla/source/carla/client/Map.h index 8546aaeb7..b68022264 100644 --- a/LibCarla/source/carla/client/Map.h +++ b/LibCarla/source/carla/client/Map.h @@ -54,6 +54,8 @@ namespace client { const geom::Location &origin, const geom::Location &destination) const; + std::string GetGeoReference() const; + private: rpc::MapInfo _description; diff --git a/LibCarla/source/carla/client/Vehicle.cpp b/LibCarla/source/carla/client/Vehicle.cpp index 97a66ac05..bd4c98daf 100644 --- a/LibCarla/source/carla/client/Vehicle.cpp +++ b/LibCarla/source/carla/client/Vehicle.cpp @@ -22,7 +22,7 @@ namespace client { } } - Vehicle::Control Vehicle::GetVehicleControl() const { + Vehicle::Control Vehicle::GetControl() const { return GetEpisode().Lock()->GetActorDynamicState(*this).state.vehicle_control; } diff --git a/LibCarla/source/carla/client/Vehicle.h b/LibCarla/source/carla/client/Vehicle.h index a2f012381..fd0a2b03c 100644 --- a/LibCarla/source/carla/client/Vehicle.h +++ b/LibCarla/source/carla/client/Vehicle.h @@ -29,7 +29,7 @@ namespace client { /// /// @note This function does not call the simulator, it returns the Control /// received in the last tick. - Control GetVehicleControl() const; + Control GetControl() const; private: diff --git a/LibCarla/source/carla/client/Walker.cpp b/LibCarla/source/carla/client/Walker.cpp new file mode 100644 index 000000000..145f9bc5f --- /dev/null +++ b/LibCarla/source/carla/client/Walker.cpp @@ -0,0 +1,26 @@ +// 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/Walker.h" + +#include "carla/client/detail/Simulator.h" + +namespace carla { +namespace client { + + void Walker::ApplyControl(const Control &control) { + if (control != _control) { + GetEpisode().Lock()->ApplyControlToWalker(*this, control); + _control = control; + } + } + + Walker::Control Walker::GetWalkerControl() const { + return GetEpisode().Lock()->GetActorDynamicState(*this).state.walker_control; + } + +} // namespace client +} // namespace carla diff --git a/LibCarla/source/carla/client/Walker.h b/LibCarla/source/carla/client/Walker.h new file mode 100644 index 000000000..d4648116b --- /dev/null +++ b/LibCarla/source/carla/client/Walker.h @@ -0,0 +1,37 @@ +// 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/Actor.h" +#include "carla/rpc/WalkerControl.h" + +namespace carla { +namespace client { + + class Walker : public Actor { + public: + + using Control = rpc::WalkerControl; + + explicit Walker(ActorInitializer init) : Actor(std::move(init)) {} + + /// Apply @a control to this Walker. + void ApplyControl(const Control &control); + + /// Return the control last applied to this Walker. + /// + /// @note This function does not call the simulator, it returns the Control + /// received in the last tick. + Control GetWalkerControl() const; + + private: + + Control _control; + }; + +} // namespace client +} // namespace carla diff --git a/LibCarla/source/carla/client/detail/ActorFactory.cpp b/LibCarla/source/carla/client/detail/ActorFactory.cpp index 9a03e7e28..6f654b22c 100644 --- a/LibCarla/source/carla/client/detail/ActorFactory.cpp +++ b/LibCarla/source/carla/client/detail/ActorFactory.cpp @@ -10,9 +10,11 @@ #include "carla/StringUtil.h" #include "carla/client/Actor.h" #include "carla/client/LaneDetector.h" +#include "carla/client/GnssSensor.h" #include "carla/client/ServerSideSensor.h" #include "carla/client/TrafficLight.h" #include "carla/client/Vehicle.h" +#include "carla/client/Walker.h" #include "carla/client/World.h" #include "carla/client/detail/Client.h" @@ -72,10 +74,14 @@ namespace detail { auto init = ActorInitializer{description, episode, parent}; if (description.description.id == "sensor.other.lane_detector") { /// @todo return MakeActorImpl(std::move(init), gc); + } else if (description.description.id == "sensor.other.gnss") { /// @todo + return MakeActorImpl(std::move(init), gc); } else if (description.HasAStream()) { return MakeActorImpl(std::move(init), gc); } else if (StringUtil::StartsWith(description.description.id, "vehicle.")) { return MakeActorImpl(std::move(init), gc); + } else if (StringUtil::StartsWith(description.description.id, "walker.")) { + return MakeActorImpl(std::move(init), gc); } else if (StringUtil::StartsWith(description.description.id, "traffic.traffic_light")) { return MakeActorImpl(std::move(init), gc); } diff --git a/LibCarla/source/carla/client/detail/Client.cpp b/LibCarla/source/carla/client/detail/Client.cpp index 63e31732f..954412711 100644 --- a/LibCarla/source/carla/client/detail/Client.cpp +++ b/LibCarla/source/carla/client/detail/Client.cpp @@ -11,6 +11,7 @@ #include "carla/rpc/Client.h" #include "carla/rpc/DebugShape.h" #include "carla/rpc/VehicleControl.h" +#include "carla/rpc/WalkerControl.h" #include "carla/streaming/Client.h" #include @@ -139,8 +140,24 @@ namespace detail { _pimpl->AsyncCall("set_actor_autopilot", vehicle, enabled); } - void Client::ApplyControlToActor(const rpc::Actor &vehicle, const rpc::VehicleControl &control) { - _pimpl->AsyncCall("apply_control_to_actor", vehicle, control); + void Client::ApplyControlToVehicle(const rpc::Actor &vehicle, const rpc::VehicleControl &control) { + _pimpl->AsyncCall("apply_control_to_vehicle", vehicle, control); + } + + void Client::ApplyControlToWalker(const rpc::Actor &walker, const rpc::WalkerControl &control) { + _pimpl->AsyncCall("apply_control_to_walker", walker, control); + } + + void Client::SetActorVelocity(const rpc::Actor &actor, const geom::Vector3D &vector) { + _pimpl->AsyncCall("set_actor_velocity", actor, vector); + } + + void Client::SetActorAngularVelocity(const rpc::Actor &actor, const geom::Vector3D &vector) { + _pimpl->AsyncCall("set_actor_angular_velocity", actor, vector); + } + + void Client::AddActorImpulse(const rpc::Actor &actor, const geom::Vector3D &vector) { + _pimpl->AsyncCall("add_actor_impulse", actor, vector); } void Client::SubscribeToStream( diff --git a/LibCarla/source/carla/client/detail/Client.h b/LibCarla/source/carla/client/detail/Client.h index 006b501e5..3d891126a 100644 --- a/LibCarla/source/carla/client/detail/Client.h +++ b/LibCarla/source/carla/client/detail/Client.h @@ -28,6 +28,7 @@ namespace rpc { class ActorDescription; class DebugShape; class VehicleControl; + class WalkerControl; } namespace sensor { class SensorData; } namespace streaming { class Token; } @@ -98,10 +99,26 @@ namespace detail { const rpc::Actor &vehicle, bool enabled); - void ApplyControlToActor( + void ApplyControlToVehicle( const rpc::Actor &vehicle, const rpc::VehicleControl &control); + void ApplyControlToWalker( + const rpc::Actor &walker, + const rpc::WalkerControl &control); + + void SetActorVelocity( + const rpc::Actor &actor, + const geom::Vector3D &vector); + + void SetActorAngularVelocity( + const rpc::Actor &actor, + const geom::Vector3D &vector); + + void AddActorImpulse( + const rpc::Actor &actor, + const geom::Vector3D &vector); + void SubscribeToStream( const streaming::Token &token, std::function callback); diff --git a/LibCarla/source/carla/client/detail/EpisodeState.cpp b/LibCarla/source/carla/client/detail/EpisodeState.cpp index 914cf6b16..9bc1704c1 100644 --- a/LibCarla/source/carla/client/detail/EpisodeState.cpp +++ b/LibCarla/source/carla/client/detail/EpisodeState.cpp @@ -38,7 +38,7 @@ namespace detail { DEBUG_ONLY(auto result = ) next->_actors.emplace( actor.id, - ActorState{actor.transform, actor.velocity, acceleration, actor.state}); + ActorState{actor.transform, actor.velocity, actor.angular_velocity, acceleration, actor.state}); DEBUG_ASSERT(result.second); } return next; diff --git a/LibCarla/source/carla/client/detail/EpisodeState.h b/LibCarla/source/carla/client/detail/EpisodeState.h index 96c410d3b..cafdeceea 100644 --- a/LibCarla/source/carla/client/detail/EpisodeState.h +++ b/LibCarla/source/carla/client/detail/EpisodeState.h @@ -29,6 +29,7 @@ namespace detail { struct ActorState { geom::Transform transform; geom::Vector3D velocity; + geom::Vector3D angular_velocity; geom::Vector3D acceleration; sensor::data::ActorDynamicState::TypeDependentState state; }; diff --git a/LibCarla/source/carla/client/detail/Simulator.h b/LibCarla/source/carla/client/detail/Simulator.h index 82a929091..76aab1c45 100644 --- a/LibCarla/source/carla/client/detail/Simulator.h +++ b/LibCarla/source/carla/client/detail/Simulator.h @@ -13,6 +13,7 @@ #include "carla/client/Actor.h" #include "carla/client/GarbageCollectionPolicy.h" #include "carla/client/Vehicle.h" +#include "carla/client/Walker.h" #include "carla/client/detail/Client.h" #include "carla/client/detail/Episode.h" #include "carla/client/detail/EpisodeProxy.h" @@ -172,6 +173,22 @@ namespace detail { return GetActorDynamicState(actor).velocity; } + void SetActorVelocity(const Actor &actor, const geom::Vector3D &vector) { + _client.SetActorVelocity(actor.Serialize(), vector); + } + + geom::Vector3D GetActorAngularVelocity(const Actor &actor) const { + return GetActorDynamicState(actor).angular_velocity; + } + + void SetActorAngularVelocity(const Actor &actor, const geom::Vector3D &vector) { + _client.SetActorAngularVelocity(actor.Serialize(), vector); + } + + void AddActorImpulse(const Actor &actor, const geom::Vector3D &vector) { + _client.AddActorImpulse(actor.Serialize(), vector); + } + geom::Vector3D GetActorAcceleration(const Actor &actor) const { return GetActorDynamicState(actor).acceleration; } @@ -199,7 +216,11 @@ namespace detail { } void ApplyControlToVehicle(Vehicle &vehicle, const rpc::VehicleControl &control) { - _client.ApplyControlToActor(vehicle.Serialize(), control); + _client.ApplyControlToVehicle(vehicle.Serialize(), control); + } + + void ApplyControlToWalker(Walker &walker, const rpc::WalkerControl &control) { + _client.ApplyControlToWalker(walker.Serialize(), control); } /// @} diff --git a/LibCarla/source/carla/geom/BoundingBox.h b/LibCarla/source/carla/geom/BoundingBox.h index 9153293ca..05ac48bfd 100644 --- a/LibCarla/source/carla/geom/BoundingBox.h +++ b/LibCarla/source/carla/geom/BoundingBox.h @@ -11,6 +11,10 @@ #include "carla/geom/Location.h" #include "carla/geom/Vector3D.h" +#ifdef LIBCARLA_INCLUDED_FROM_UE4 +# include "Carla/Util/BoundingBox.h" +#endif // LIBCARLA_INCLUDED_FROM_UE4 + namespace carla { namespace geom { @@ -39,9 +43,9 @@ namespace geom { #ifdef LIBCARLA_INCLUDED_FROM_UE4 - BoundingBox(const FVector &Origin, const FVector &BoxExtent) - : location(Origin), - extent(1e-2f * BoxExtent.X, 1e-2f * BoxExtent.Y, 1e-2f * BoxExtent.Z) {} + BoundingBox(const FBoundingBox &Box) + : location(Box.Origin), + extent(1e-2f * Box.Extent.X, 1e-2f * Box.Extent.Y, 1e-2f * Box.Extent.Z) {} #endif // LIBCARLA_INCLUDED_FROM_UE4 diff --git a/LibCarla/source/carla/geom/Location.h b/LibCarla/source/carla/geom/Location.h index cbe6c87e9..813ce2582 100644 --- a/LibCarla/source/carla/geom/Location.h +++ b/LibCarla/source/carla/geom/Location.h @@ -19,17 +19,25 @@ namespace geom { class Location : public Vector3D { public: + // ========================================================================= + // -- Constructors --------------------------------------------------------- + // ========================================================================= + Location() = default; using Vector3D::Vector3D; - using Vector3D::x; - using Vector3D::y; - using Vector3D::z; + // ========================================================================= + // -- Other methods -------------------------------------------------------- + // ========================================================================= - using Vector3D::msgpack_pack; - using Vector3D::msgpack_unpack; - using Vector3D::msgpack_object; + double Distance(const Location &loc) const { + return Math::Distance(*this, loc); + } + + // ========================================================================= + // -- Arithmetic operators ------------------------------------------------- + // ========================================================================= Location &operator+=(const Location &rhs) { static_cast(*this) += rhs; @@ -67,6 +75,10 @@ namespace geom { return rhs; } + // ========================================================================= + // -- Comparison operators ------------------------------------------------- + // ========================================================================= + bool operator==(const Location &rhs) const { return static_cast(*this) == rhs; } @@ -75,9 +87,9 @@ namespace geom { return !(*this == rhs); } - double Distance(const Location &loc) const { - return Math::Distance(*this, loc); - } + // ========================================================================= + // -- Conversions to UE4 types --------------------------------------------- + // ========================================================================= #ifdef LIBCARLA_INCLUDED_FROM_UE4 diff --git a/LibCarla/source/carla/geom/Math.cpp b/LibCarla/source/carla/geom/Math.cpp index fa6d133bb..993497bb0 100644 --- a/LibCarla/source/carla/geom/Math.cpp +++ b/LibCarla/source/carla/geom/Math.cpp @@ -6,124 +6,136 @@ #include "carla/geom/Math.h" +#include "carla/geom/Rotation.h" + namespace carla { namespace geom { - /// Returns a pair containing: - /// - @b first: distance from v to p' where p' = p projected on segment (w - v) - /// - @b second: Euclidean distance from p to p' - /// @param p point to calculate distance - /// @param v first point of the segment - /// @param w second point of the segment - std::pair Math::DistSegmentPoint( - const Vector3D &p, - const Vector3D &v, - const Vector3D &w) { - const double l2 = DistanceSquared2D(v, w); - const double l = std::sqrt(l2); - if (l2 == 0.0) { - return std::make_pair(0.0, Distance2D(v, p)); - } - const double dot_p_w = Dot2D(p - v, w - v); - const double t = clamp01(dot_p_w / l2); - const Vector3D projection = v + t * (w - v); - return std::make_pair(t * l, Distance2D(projection, p)); + /// Returns a pair containing: + /// - @b first: distance from v to p' where p' = p projected on segment (w - + /// v) + /// - @b second: Euclidean distance from p to p' + /// @param p point to calculate distance + /// @param v first point of the segment + /// @param w second point of the segment + std::pair Math::DistSegmentPoint( + const Vector3D &p, + const Vector3D &v, + const Vector3D &w) { + const double l2 = DistanceSquared2D(v, w); + const double l = std::sqrt(l2); + if (l2 == 0.0) { + return std::make_pair(0.0, Distance2D(v, p)); } + const double dot_p_w = Dot2D(p - v, w - v); + const double t = clamp01(dot_p_w / l2); + const Vector3D projection = v + t * (w - v); + return std::make_pair(t * l, Distance2D(projection, p)); + } - Vector3D Math::RotatePointOnOrigin2D(Vector3D p, double angle) { - double s = std::sin(angle); - double c = std::cos(angle); - return Vector3D(p.x * c - p.y * s, p.x * s + p.y * c, 0.0); - } + Vector3D Math::RotatePointOnOrigin2D(Vector3D p, double angle) { + double s = std::sin(angle); + double c = std::cos(angle); + return Vector3D(p.x * c - p.y * s, p.x * s + p.y * c, 0.0); + } - /// Returns a pair containing: - /// - @b first: distance across the arc from start_pos to p' where p' = p projected on Arc - /// - @b second: Euclidean distance from p to p' - std::pair Math::DistArcPoint( - Vector3D p, - Vector3D start_pos, - double length, - double heading, // [radians] - double curvature) { + /// Returns a pair containing: + /// - @b first: distance across the arc from start_pos to p' where p' = p + /// projected on Arc + /// - @b second: Euclidean distance from p to p' + std::pair Math::DistArcPoint( + Vector3D p, + Vector3D start_pos, + double length, + double heading, // [radians] + double curvature) { - /// @todo: Because Unreal's coordinates, hacky way to correct - /// the -y, this must be changed in the future + /// @todo: Because Unreal's coordinates, hacky way to correct + /// the -y, this must be changed in the future + p.y = -p.y; + start_pos.y = -start_pos.y; + heading = -heading; + + // since this algorithm is working for positive curvatures, + // and we are only calculating distances, we can invert the y + // axis (along with the curvature and the heading), so if the + // curvature is negative, so the algorithm will work as expected + if (curvature < 0.0) { p.y = -p.y; start_pos.y = -start_pos.y; heading = -heading; - - // since this algorithm is working for positive curvatures, - // and we are only calculating distances, we can invert the y - // axis (along with the curvature and the heading), so if the - // curvature is negative, so the algorithm will work as expected - if (curvature < 0.0) { - p.y = -p.y; - start_pos.y = -start_pos.y; - heading = -heading; - curvature = -curvature; - } - - // transport point relative to the arc starting poistion and rotation - const Vector3D rotated_p(RotatePointOnOrigin2D(p - start_pos, -heading)); - - const double radius = 1.0 / curvature; - const Vector3D circ_center(0, radius, 0); - - // check if the point is in the center of the circle, so we know p - // is in the same distance of every possible point in the arc - if (rotated_p == circ_center) { - return std::make_pair(0.0, radius); - } - - // find intersection position using the unit vector from the center - // of the circle to the point and multiplying by the radius - const Vector3D intersection = ((rotated_p - circ_center).MakeUnitVector() * radius) + circ_center; - - // use the arc length to calculate the angle in the last point of it - // circumference of a circle = 2 * PI * r - // last_point_angle = (length / circumference) * 2 * PI - // so last_point_angle = length / radius - const double last_point_angle = length / radius; - - // move the point relative to the center of the circle and find - // the angle between the point and the center of coords in rad - double angle = std::atan2(intersection.y - radius, intersection.x) + pi_half(); - - if(angle < 0.0) { - angle += pi_double(); - } - - // see if the angle is between 0 and last_point_angle - DEBUG_ASSERT(angle >= 0.0); - if (angle <= last_point_angle) { - return std::make_pair( - angle * radius, - Distance2D(intersection, rotated_p)); - } - - // find the nearest point, start or end to intersection - const double start_dist = Distance2D(Vector3D(), rotated_p); - - const Vector3D end_pos( - radius * std::cos(last_point_angle - pi_half()), - radius * std::sin(last_point_angle - pi_half()) + circ_center.y, - 0.0); - const double end_dist = Distance2D(end_pos, rotated_p); - return (start_dist < end_dist) ? - std::make_pair(0.0, start_dist) : - std::make_pair(length, end_dist); + curvature = -curvature; } - bool Math::PointInRectangle( - const Vector3D &pos, - const Vector3D &extent, - double angle, // [radians] - const Vector3D &p) { - // Move p relative to pos's position and angle - Vector3D transf_p = RotatePointOnOrigin2D(p - pos, -angle); - return transf_p.x <= extent.x && transf_p.y <= extent.y && - transf_p.x >= -extent.x && transf_p.y >= -extent.y; + // transport point relative to the arc starting poistion and rotation + const Vector3D rotated_p(RotatePointOnOrigin2D(p - start_pos, -heading)); + + const double radius = 1.0 / curvature; + const Vector3D circ_center(0, radius, 0); + + // check if the point is in the center of the circle, so we know p + // is in the same distance of every possible point in the arc + if (rotated_p == circ_center) { + return std::make_pair(0.0, radius); } + // find intersection position using the unit vector from the center + // of the circle to the point and multiplying by the radius + const Vector3D intersection = ((rotated_p - circ_center).MakeUnitVector() * radius) + circ_center; + + // use the arc length to calculate the angle in the last point of it + // circumference of a circle = 2 * PI * r + // last_point_angle = (length / circumference) * 2 * PI + // so last_point_angle = length / radius + const double last_point_angle = length / radius; + + // move the point relative to the center of the circle and find + // the angle between the point and the center of coords in rad + double angle = std::atan2(intersection.y - radius, intersection.x) + pi_half(); + + if (angle < 0.0) { + angle += pi_double(); + } + + // see if the angle is between 0 and last_point_angle + DEBUG_ASSERT(angle >= 0.0); + if (angle <= last_point_angle) { + return std::make_pair( + angle * radius, + Distance2D(intersection, rotated_p)); + } + + // find the nearest point, start or end to intersection + const double start_dist = Distance2D(Vector3D(), rotated_p); + + const Vector3D end_pos( + radius * std::cos(last_point_angle - pi_half()), + radius *std::sin(last_point_angle - pi_half()) + circ_center.y, + 0.0); + const double end_dist = Distance2D(end_pos, rotated_p); + return (start_dist < end_dist) ? + std::make_pair(0.0, start_dist) : + std::make_pair(length, end_dist); + } + + bool Math::PointInRectangle( + const Vector3D &pos, + const Vector3D &extent, + double angle, // [radians] + const Vector3D &p) { + // Move p relative to pos's position and angle + Vector3D transf_p = RotatePointOnOrigin2D(p - pos, -angle); + return transf_p.x <= extent.x && transf_p.y <= extent.y && + transf_p.x >= -extent.x && transf_p.y >= -extent.y; + } + + Vector3D Math::GetForwardVector(const Rotation &rotation) { + const float cp = std::cos(to_radians(rotation.pitch)); + const float sp = std::sin(to_radians(rotation.pitch)); + const float cy = std::cos(to_radians(rotation.yaw)); + const float sy = std::sin(to_radians(rotation.yaw)); + return {cy * cp, sy * cp, sp}; + } + } // namespace geom -} // namespace carla \ No newline at end of file +} // namespace carla diff --git a/LibCarla/source/carla/geom/Math.h b/LibCarla/source/carla/geom/Math.h index df247b074..c076d755b 100644 --- a/LibCarla/source/carla/geom/Math.h +++ b/LibCarla/source/carla/geom/Math.h @@ -15,8 +15,11 @@ namespace carla { namespace geom { + class Rotation; + class Math { public: + static constexpr auto pi() { return 3.14159265358979323846264338327950288; } @@ -98,6 +101,9 @@ namespace geom { const Vector3D &, double, // [radians] const Vector3D &); + + /// Compute the unit vector pointing towards the X-axis of @a rotation. + static Vector3D GetForwardVector(const Rotation &rotation); }; } // namespace geom diff --git a/LibCarla/source/carla/geom/Rotation.h b/LibCarla/source/carla/geom/Rotation.h index 8bfd2cbc6..77832695e 100644 --- a/LibCarla/source/carla/geom/Rotation.h +++ b/LibCarla/source/carla/geom/Rotation.h @@ -7,6 +7,8 @@ #pragma once #include "carla/MsgPack.h" +#include "carla/geom/Math.h" +#include "carla/geom/Vector3D.h" #ifdef LIBCARLA_INCLUDED_FROM_UE4 # include "Math/Rotator.h" @@ -18,6 +20,22 @@ namespace geom { class Rotation { public: + // ========================================================================= + // -- Public data members -------------------------------------------------- + // ========================================================================= + + float pitch = 0.0f; + + float yaw = 0.0f; + + float roll = 0.0f; + + MSGPACK_DEFINE_ARRAY(pitch, yaw, roll); + + // ========================================================================= + // -- Constructors --------------------------------------------------------- + // ========================================================================= + Rotation() = default; Rotation(float p, float y, float r) @@ -25,9 +43,17 @@ namespace geom { yaw(y), roll(r) {} - float pitch = 0.0f; - float yaw = 0.0f; - float roll = 0.0f; + // ========================================================================= + // -- Other methods -------------------------------------------------------- + // ========================================================================= + + Vector3D GetForwardVector() const { + return Math::GetForwardVector(*this); + } + + // ========================================================================= + // -- Comparison operators ------------------------------------------------- + // ========================================================================= bool operator==(const Rotation &rhs) const { return (pitch == rhs.pitch) && (yaw == rhs.yaw) && (roll == rhs.roll); @@ -37,6 +63,10 @@ namespace geom { return !(*this == rhs); } + // ========================================================================= + // -- Conversions to UE4 types --------------------------------------------- + // ========================================================================= + #ifdef LIBCARLA_INCLUDED_FROM_UE4 Rotation(const FRotator &rotator) @@ -47,8 +77,6 @@ namespace geom { } #endif // LIBCARLA_INCLUDED_FROM_UE4 - - MSGPACK_DEFINE_ARRAY(pitch, yaw, roll); }; } // namespace geom diff --git a/LibCarla/source/carla/geom/Transform.h b/LibCarla/source/carla/geom/Transform.h index 38ac0a43d..ce27ed420 100644 --- a/LibCarla/source/carla/geom/Transform.h +++ b/LibCarla/source/carla/geom/Transform.h @@ -21,25 +21,35 @@ namespace geom { class Transform { public: + // ========================================================================= + // -- Public data members -------------------------------------------------- + // ========================================================================= + + Location location; + + Rotation rotation; + + MSGPACK_DEFINE_ARRAY(location, rotation); + + // ========================================================================= + // -- Constructors --------------------------------------------------------- + // ========================================================================= + Transform() = default; Transform(const Location &in_location, const Rotation &in_rotation) : location(in_location), rotation(in_rotation) {} - Location location; - Rotation rotation; + // ========================================================================= + // -- Other methods -------------------------------------------------------- + // ========================================================================= - bool operator==(const Transform &rhs) const { - return (location == rhs.location) && (rotation == rhs.rotation); - } - - bool operator!=(const Transform &rhs) const { - return !(*this == rhs); + Vector3D GetForwardVector() const { + return rotation.GetForwardVector(); } void TransformPoint(Vector3D &in_point) const { - // Rotate double cy = cos(Math::to_radians(rotation.yaw)); double sy = sin(Math::to_radians(rotation.yaw)); @@ -66,6 +76,22 @@ namespace geom { in_point = out_point; } + // ========================================================================= + // -- Comparison operators ------------------------------------------------- + // ========================================================================= + + bool operator==(const Transform &rhs) const { + return (location == rhs.location) && (rotation == rhs.rotation); + } + + bool operator!=(const Transform &rhs) const { + return !(*this == rhs); + } + + // ========================================================================= + // -- Conversions to UE4 types --------------------------------------------- + // ========================================================================= + #ifdef LIBCARLA_INCLUDED_FROM_UE4 Transform(const FTransform &transform) @@ -77,8 +103,6 @@ namespace geom { } #endif // LIBCARLA_INCLUDED_FROM_UE4 - - MSGPACK_DEFINE_ARRAY(location, rotation); }; } // namespace geom diff --git a/LibCarla/source/carla/geom/Vector3D.h b/LibCarla/source/carla/geom/Vector3D.h index a427c56f4..bf331a82c 100644 --- a/LibCarla/source/carla/geom/Vector3D.h +++ b/LibCarla/source/carla/geom/Vector3D.h @@ -17,10 +17,20 @@ namespace geom { class Vector3D { public: + // ========================================================================= + // -- Public data members -------------------------------------------------- + // ========================================================================= + float x = 0.0f; + float y = 0.0f; + float z = 0.0f; + // ========================================================================= + // -- Constructors --------------------------------------------------------- + // ========================================================================= + Vector3D() = default; Vector3D(float ix, float iy, float iz) @@ -28,6 +38,29 @@ namespace geom { y(iy), z(iz) {} + // ========================================================================= + // -- Other methods -------------------------------------------------------- + // ========================================================================= + + double SquaredLength() const { + return x * x + y * y + z * z; + } + + double Length() const { + return std::sqrt(SquaredLength()); + } + + Vector3D MakeUnitVector() const { + const double len = Length(); + DEBUG_ASSERT(len > std::numeric_limits::epsilon()); + double k = 1.0 / len; + return Vector3D(x * k, y * k, z * k); + } + + // ========================================================================= + // -- Arithmetic operators ------------------------------------------------- + // ========================================================================= + Vector3D &operator+=(const Vector3D &rhs) { x += rhs.x; y += rhs.y; @@ -86,6 +119,10 @@ namespace geom { return rhs; } + // ========================================================================= + // -- Comparison operators ------------------------------------------------- + // ========================================================================= + bool operator==(const Vector3D &rhs) const { return (x == rhs.x) && (y == rhs.y) && (z == rhs.z); } @@ -94,21 +131,35 @@ namespace geom { return !(*this == rhs); } - double SquaredLength() const { - return x * x + y * y + z * z; + // ========================================================================= + // -- Conversions to UE4 types --------------------------------------------- + // ========================================================================= + +#ifdef LIBCARLA_INCLUDED_FROM_UE4 + + Vector3D(const FVector &vector) + : Vector3D(vector.X, vector.Y, vector.Z) {} + + Vector3D &ToMeters(void) { // from centimeters to meters. + x *= 0.001f; + y *= 0.001f; + z *= 0.001f; + return *this; } - double Length() const { - return std::sqrt(SquaredLength()); + Vector3D &ToCentimeters(void) { // from meters to centimeters. + x *= 100.0f; + y *= 100.0f; + z *= 100.0f; + return *this; } - Vector3D MakeUnitVector() const { - const double len = Length(); - DEBUG_ASSERT(len > std::numeric_limits::epsilon()); - double k = 1.0 / len; - return Vector3D(x * k, y * k, z * k); + operator FVector() const { + return FVector{x, y, z}; } +#endif // LIBCARLA_INCLUDED_FROM_UE4 + // ========================================================================= /// @todo The following is copy-pasted from MSGPACK_DEFINE_ARRAY. /// This is a workaround for an issue in msgpack library. The diff --git a/LibCarla/source/carla/image/BoostGil.h b/LibCarla/source/carla/image/BoostGil.h index f3814c3a0..32639b552 100644 --- a/LibCarla/source/carla/image/BoostGil.h +++ b/LibCarla/source/carla/image/BoostGil.h @@ -13,7 +13,7 @@ # pragma clang diagnostic ignored "-Wunused-local-typedef" #endif -#include +#include #if defined(__clang__) # pragma clang diagnostic pop diff --git a/LibCarla/source/carla/image/ImageIOConfig.h b/LibCarla/source/carla/image/ImageIOConfig.h index 2d6c94a9f..b3c784929 100644 --- a/LibCarla/source/carla/image/ImageIOConfig.h +++ b/LibCarla/source/carla/image/ImageIOConfig.h @@ -47,15 +47,23 @@ # ifndef int_p_NULL # define int_p_NULL (int*)NULL # endif // int_p_NULL -# include +# if defined(__clang__) +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wignored-qualifiers" +# pragma clang diagnostic ignored "-Wparentheses" +# endif +# include +# if defined(__clang__) +# pragma clang diagnostic pop +# endif #endif #if LIBCARLA_IMAGE_WITH_JPEG_SUPPORT == true -# include +# include #endif #if LIBCARLA_IMAGE_WITH_TIFF_SUPPORT == true -# include +# include #endif #if defined(__clang__) @@ -85,6 +93,11 @@ namespace io { namespace detail { + template + struct is_write_supported { + static constexpr bool value = boost::gil::is_write_supported::type, IOTag>::value; + }; + struct io_png { static constexpr bool is_supported = has_png_support(); @@ -102,12 +115,12 @@ namespace detail { template static void read_image(Str &&in_filename, ImageT &image) { - boost::gil::png_read_and_convert_image(std::forward(in_filename), image); + boost::gil::read_and_convert_image(std::forward(in_filename), image, boost::gil::png_tag()); } template static void write_view(Str &&out_filename, const ViewT &view) { - boost::gil::png_write_view(std::forward(out_filename), view); + boost::gil::write_view(std::forward(out_filename), view, boost::gil::png_tag()); } #endif // LIBCARLA_IMAGE_WITH_PNG_SUPPORT @@ -131,21 +144,22 @@ namespace detail { template static void read_image(Str &&in_filename, ImageT &image) { - boost::gil::jpeg_read_image(std::forward(in_filename), image); + boost::gil::read_image(std::forward(in_filename), image, boost::gil::jpeg_tag()); } template - static typename std::enable_if::is_supported>::type + static typename std::enable_if::value>::type write_view(Str &&out_filename, const ViewT &view) { - using namespace boost::gil; - jpeg_write_view(std::forward(out_filename), view); + boost::gil::write_view(std::forward(out_filename), view, boost::gil::jpeg_tag()); } template - static typename std::enable_if::is_supported>::type + static typename std::enable_if::value>::type write_view(Str &&out_filename, const ViewT &view) { - using namespace boost::gil; - jpeg_write_view(std::forward(out_filename), color_converted_view(view)); + boost::gil::write_view( + std::forward(out_filename), + boost::gil::color_converted_view(view), + boost::gil::jpeg_tag()); } #endif // LIBCARLA_IMAGE_WITH_JPEG_SUPPORT @@ -168,21 +182,22 @@ namespace detail { template static void read_image(Str &&in_filename, ImageT &image) { - boost::gil::tiff_read_and_convert_image(std::forward(in_filename), image); + boost::gil::read_and_convert_image(std::forward(in_filename), image, boost::gil::tiff_tag()); } template - static typename std::enable_if::is_supported>::type + static typename std::enable_if::value>::type write_view(Str &&out_filename, const ViewT &view) { - using namespace boost::gil; - tiff_write_view(std::forward(out_filename), view); + boost::gil::write_view(std::forward(out_filename), view, boost::gil::tiff_tag()); } template - static typename std::enable_if::is_supported>::type + static typename std::enable_if::value>::type write_view(Str &&out_filename, const ViewT &view) { - using namespace boost::gil; - tiff_write_view(std::forward(out_filename), color_converted_view(view)); + boost::gil::write_view( + std::forward(out_filename), + boost::gil::color_converted_view(view), + boost::gil::tiff_tag()); } #endif // LIBCARLA_IMAGE_WITH_TIFF_SUPPORT diff --git a/LibCarla/source/carla/opendrive/OpenDrive.cpp b/LibCarla/source/carla/opendrive/OpenDrive.cpp index 885e542de..2a240dc42 100644 --- a/LibCarla/source/carla/opendrive/OpenDrive.cpp +++ b/LibCarla/source/carla/opendrive/OpenDrive.cpp @@ -107,6 +107,8 @@ namespace opendrive { return mapBuilder.Build(); } + mapBuilder.SetGeoReference(open_drive_road.geoReference); + // Generate road and junction information using junction_data_t = std::map>>; using road_data_t = std::map; diff --git a/LibCarla/source/carla/opendrive/parser/OpenDriveParser.h b/LibCarla/source/carla/opendrive/parser/OpenDriveParser.h index 19ec60eac..9a97668e9 100644 --- a/LibCarla/source/carla/opendrive/parser/OpenDriveParser.h +++ b/LibCarla/source/carla/opendrive/parser/OpenDriveParser.h @@ -86,6 +86,8 @@ struct OpenDriveParser { carla::opendrive::parser::JunctionParser::Parse(junction, out_open_drive_data.junctions); } + out_open_drive_data.geoReference = xmlDoc.child("OpenDRIVE").child("header").child_value("geoReference"); + return true; } }; diff --git a/LibCarla/source/carla/opendrive/types.h b/LibCarla/source/carla/opendrive/types.h index 98b02bfe7..4e99fcaf6 100644 --- a/LibCarla/source/carla/opendrive/types.h +++ b/LibCarla/source/carla/opendrive/types.h @@ -239,6 +239,7 @@ namespace types { ///////////////////////////////////////////////////////////////// struct OpenDriveData { + std::string geoReference; std::vector roads; std::vector junctions; }; diff --git a/LibCarla/source/carla/road/MapBuilder.h b/LibCarla/source/carla/road/MapBuilder.h index ab4c03b72..b57d63df1 100644 --- a/LibCarla/source/carla/road/MapBuilder.h +++ b/LibCarla/source/carla/road/MapBuilder.h @@ -23,6 +23,10 @@ namespace road { _map_data.SetJunctionInformation(junctionInfo); } + void SetGeoReference(const std::string &geoReference) { + _map_data.SetGeoReference(geoReference); + } + SharedPtr Build(); private: diff --git a/LibCarla/source/carla/road/MapData.h b/LibCarla/source/carla/road/MapData.h index 8d879c240..d2fb53baf 100644 --- a/LibCarla/source/carla/road/MapData.h +++ b/LibCarla/source/carla/road/MapData.h @@ -52,6 +52,10 @@ namespace road { return _junction_information; } + const std::string &GetGeoReference() const { + return _geo_reference; + } + auto GetRoadSegments() const { using const_ref = const element::RoadSegment &; auto get = [](auto &pair) -> const_ref { return *pair.second; }; @@ -70,6 +74,12 @@ namespace road { _junction_information = junctionInfo; } + void SetGeoReference(const std::string &geoReference) { + _geo_reference = geoReference; + } + + std::string _geo_reference; + std::vector _junction_information; std::unordered_map< diff --git a/LibCarla/source/carla/road/WaypointGenerator.cpp b/LibCarla/source/carla/road/WaypointGenerator.cpp index 593632100..1ecc1a48e 100644 --- a/LibCarla/source/carla/road/WaypointGenerator.cpp +++ b/LibCarla/source/carla/road/WaypointGenerator.cpp @@ -55,7 +55,7 @@ namespace road { waypoint.GetRoadSegment().GetPrevLane(this_lane_id); if (next_lanes.empty()) { - log_error("lane id =", this_lane_id, " road id=", this_road_id, ": missing next lanes"); + log_error("road id =", this_road_id, "lane id =", this_lane_id, ": missing next lanes"); } std::vector result; @@ -120,6 +120,40 @@ namespace road { return result; } + std::vector WaypointGenerator::GenerateLaneBegin( + const Map &map) { + std::vector result; + for (auto &&road_segment : map.GetData().GetRoadSegments()) { + ForEachDrivableLane(road_segment, 0.0, [&](auto lane_id) { + auto distance = lane_id < 0 ? 0.0 : road_segment.GetLength(); + auto this_waypoint = Waypoint( + map.shared_from_this(), + road_segment.GetId(), + lane_id, + distance); + result.push_back(this_waypoint); + }); + } + return result; + } + + std::vector WaypointGenerator::GenerateLaneEnd( + const Map &map) { + std::vector result; + for (auto &&road_segment : map.GetData().GetRoadSegments()) { + ForEachDrivableLane(road_segment, 0.0, [&](auto lane_id) { + auto distance = lane_id > 0 ? 0.0 : road_segment.GetLength(); + auto this_waypoint = Waypoint( + map.shared_from_this(), + road_segment.GetId(), + lane_id, + distance); + result.push_back(this_waypoint); + }); + } + return result; + } + std::vector> WaypointGenerator::GenerateTopology( const Map &map) { std::vector> result; diff --git a/LibCarla/source/carla/road/WaypointGenerator.h b/LibCarla/source/carla/road/WaypointGenerator.h index 2e6dd2cd4..23e8f044a 100644 --- a/LibCarla/source/carla/road/WaypointGenerator.h +++ b/LibCarla/source/carla/road/WaypointGenerator.h @@ -38,10 +38,19 @@ namespace road { const Map &map, double approx_distance); + /// Returns a list of waypoints at the beginning of each lane of the map. + static std::vector GenerateLaneBegin( + const Map &map); + + /// Returns a list of waypoints at the end of each lane of the map. + static std::vector GenerateLaneEnd( + const Map &map); + /// Generate the minimum set of waypoints that define the topology of @a /// map. The waypoints are placed at the entrance of each lane. static std::vector> GenerateTopology( const Map &map); + }; } // namespace road diff --git a/LibCarla/source/carla/rpc/Vector3D.h b/LibCarla/source/carla/rpc/Vector3D.h new file mode 100644 index 000000000..a15995ecf --- /dev/null +++ b/LibCarla/source/carla/rpc/Vector3D.h @@ -0,0 +1,17 @@ +// 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/Vector3D.h" + +namespace carla { +namespace rpc { + + using Vector3D = geom::Vector3D; + +} // namespace rpc +} // namespace carla diff --git a/LibCarla/source/carla/rpc/WalkerControl.h b/LibCarla/source/carla/rpc/WalkerControl.h new file mode 100644 index 000000000..165febc21 --- /dev/null +++ b/LibCarla/source/carla/rpc/WalkerControl.h @@ -0,0 +1,66 @@ +// 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" + +#ifdef LIBCARLA_INCLUDED_FROM_UE4 +# include "Carla/Walker/WalkerControl.h" +#endif // LIBCARLA_INCLUDED_FROM_UE4 + +namespace carla { +namespace rpc { + + class WalkerControl { + public: + + WalkerControl() = default; + + WalkerControl( + geom::Vector3D in_direction, + float in_speed, + bool in_jump) + : direction(in_direction), + speed(in_speed), + jump(in_jump) {} + + geom::Vector3D direction = {1.0f, 0.0f, 0.0f}; + + float speed = 0.0f; + + bool jump = false; + +#ifdef LIBCARLA_INCLUDED_FROM_UE4 + + WalkerControl(const FWalkerControl &Control) + : direction(Control.Direction.X, Control.Direction.Y, Control.Direction.Z), + speed(1e-2f * Control.Speed), + jump(Control.Jump) {} + + operator FWalkerControl() const { + FWalkerControl Control; + Control.Direction = {direction.x, direction.y, direction.z}; + Control.Speed = 1e2f * speed; + Control.Jump = jump; + return Control; + } + +#endif // LIBCARLA_INCLUDED_FROM_UE4 + + bool operator!=(const WalkerControl &rhs) const { + return direction != rhs.direction || speed != rhs.speed || jump != rhs.jump; + } + + bool operator==(const WalkerControl &rhs) const { + return !(*this != rhs); + } + + MSGPACK_DEFINE_ARRAY(direction, speed, jump); + }; + +} // namespace rpc +} // namespace carla diff --git a/LibCarla/source/carla/sensor/SensorRegistry.h b/LibCarla/source/carla/sensor/SensorRegistry.h index 3b4a8e917..4e7dcc79a 100644 --- a/LibCarla/source/carla/sensor/SensorRegistry.h +++ b/LibCarla/source/carla/sensor/SensorRegistry.h @@ -26,6 +26,7 @@ class ARayCastLidar; class ASceneCaptureCamera; class ASemanticSegmentationCamera; class AWorldObserver; +class AGnssSensor; namespace carla { namespace sensor { @@ -40,7 +41,8 @@ namespace sensor { std::pair, std::pair, std::pair, - std::pair + std::pair, + std::pair >; } // namespace sensor @@ -57,5 +59,6 @@ namespace sensor { #include "Carla/Sensor/SceneCaptureCamera.h" #include "Carla/Sensor/SemanticSegmentationCamera.h" #include "Carla/Sensor/WorldObserver.h" +#include "Carla/Sensor/GnssSensor.h" #endif // LIBCARLA_SENSOR_REGISTRY_WITH_SENSOR_INCLUDES diff --git a/LibCarla/source/carla/sensor/data/ActorDynamicState.h b/LibCarla/source/carla/sensor/data/ActorDynamicState.h index afa69a8b3..0c209931d 100644 --- a/LibCarla/source/carla/sensor/data/ActorDynamicState.h +++ b/LibCarla/source/carla/sensor/data/ActorDynamicState.h @@ -11,6 +11,7 @@ #include "carla/rpc/ActorId.h" #include "carla/rpc/TrafficLightState.h" #include "carla/rpc/VehicleControl.h" +#include "carla/rpc/WalkerControl.h" #include @@ -51,6 +52,29 @@ namespace detail { }; #pragma pack(pop) +#pragma pack(push, 1) + class PackedWalkerControl { + public: + + PackedWalkerControl() = default; + + PackedWalkerControl(const rpc::WalkerControl &control) + : direction{control.direction.x, control.direction.y, control.direction.z}, + speed(control.speed), + jump(control.jump) {} + + operator rpc::WalkerControl() const { + return {geom::Vector3D{direction[0u], direction[1u], direction[2u]}, speed, jump}; + } + + private: + + float direction[3u]; + float speed; + bool jump; + }; +#pragma pack(pop) + } // namespace detail #pragma pack(push, 1) @@ -64,16 +88,19 @@ namespace detail { geom::Vector3D velocity; + geom::Vector3D angular_velocity; + union TypeDependentState { rpc::TrafficLightState traffic_light_state; detail::PackedVehicleControl vehicle_control; + detail::PackedWalkerControl walker_control; } state; }; #pragma pack(pop) static_assert( - sizeof(ActorDynamicState) == 10u * sizeof(uint32_t) + sizeof(detail::PackedVehicleControl), + sizeof(ActorDynamicState) == 13u * sizeof(uint32_t) + sizeof(detail::PackedVehicleControl), "Invalid ActorDynamicState size!"); } // namespace data diff --git a/LibCarla/source/carla/sensor/data/GnssEvent.h b/LibCarla/source/carla/sensor/data/GnssEvent.h new file mode 100644 index 000000000..785c0be5f --- /dev/null +++ b/LibCarla/source/carla/sensor/data/GnssEvent.h @@ -0,0 +1,55 @@ +// Copyright (c) 2019 Intel Labs. +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "carla/road/element/LaneMarking.h" +#include "carla/sensor/SensorData.h" + +#include + +namespace carla { +namespace sensor { +namespace data { + + /// A change of gnss data + class GnssEvent : public SensorData { + public: + + explicit GnssEvent( + size_t frame_number, + const rpc::Transform &sensor_transform, + double lat, + double lon, + double alt) + : SensorData(frame_number, sensor_transform), + _lat(std::move(lat)), + _lon(std::move(lon)), + _alt(std::move(alt)) {} + + const double &GetLatitude() const { + return _lat; + } + + const double &GetLongitude() const { + return _lon; + } + + const double &GetAltitude() const { + return _alt; + } + + private: + + double _lat; + + double _lon; + + double _alt; + }; + +} // namespace data +} // namespace sensor +} // namespace carla diff --git a/LibCarla/source/compiler/disable-ue4-macros.h b/LibCarla/source/compiler/disable-ue4-macros.h index 87425d5ed..9c66b60bc 100644 --- a/LibCarla/source/compiler/disable-ue4-macros.h +++ b/LibCarla/source/compiler/disable-ue4-macros.h @@ -7,6 +7,8 @@ #ifndef LIBCARLA_INCLUDED_DISABLE_UE4_MACROS_HEADER #define LIBCARLA_INCLUDED_DISABLE_UE4_MACROS_HEADER +#include "Carla.h" + #ifndef BOOST_ERROR_CODE_HEADER_ONLY # define BOOST_ERROR_CODE_HEADER_ONLY #endif // BOOST_ERROR_CODE_HEADER_ONLY @@ -15,16 +17,6 @@ # define BOOST_NO_EXCEPTIONS #endif // BOOST_NO_EXCEPTIONS -// Suppress clang warning. -#if defined(__clang__) -# ifndef __cpp_coroutines -# define __cpp_coroutines 0 -# endif // __cpp_coroutines -# ifndef __cpp_noexcept_function_type -# define __cpp_noexcept_function_type 0 -# endif // __cpp_noexcept_function_type -#endif // defined(__clang__) - namespace boost { static inline void throw_exception(const std::exception &e) { @@ -56,6 +48,8 @@ namespace boost { #if defined(__clang__) # pragma clang diagnostic push # pragma clang diagnostic ignored "-Wmissing-braces" +# pragma clang diagnostic ignored "-Wunusable-partial-specialization" +# pragma clang diagnostic ignored "-Wundef" #endif #pragma push_macro("TEXT") diff --git a/LibCarla/source/test/test_geom.cpp b/LibCarla/source/test/test_geom.cpp index d6278d3c8..b9c2b62ec 100644 --- a/LibCarla/source/test/test_geom.cpp +++ b/LibCarla/source/test/test_geom.cpp @@ -11,6 +11,17 @@ #include #include +namespace carla { +namespace geom { + + std::ostream &operator<<(std::ostream &out, const Vector3D &vector3D) { + out << "{x=" << vector3D.x << ", y=" << vector3D.y << ", z=" << vector3D.z << '}'; + return out; + } + +} // namespace geom +} // namespace carla + using namespace carla::geom; TEST(geom, single_point_no_transform) { @@ -76,7 +87,6 @@ TEST(geom, single_point_translation_and_rotation) { ASSERT_NEAR(point.z, result_point.z, error); } - TEST(geom, distance) { constexpr double error = .01; ASSERT_NEAR(Math::Distance({0, 0, 0}, {0, 0, 0}), 0.0, error); @@ -138,6 +148,27 @@ TEST(geom, nearest_point_segment) { } } +TEST(geom, forward_vector) { + auto compare = [](Rotation rotation, Vector3D expected) { + constexpr float eps = 2.0f * std::numeric_limits::epsilon(); + auto result = rotation.GetForwardVector(); + EXPECT_TRUE( + (std::abs(expected.x - result.x) < eps) && + (std::abs(expected.y - result.y) < eps) && + (std::abs(expected.z - result.z) < eps)) + << "result = " << result << '\n' + << "expected = " << expected; + }; + // pitch yaw roll x y z + compare({ 0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}); + compare({ 0.0f, 0.0f, 123.0f}, {1.0f, 0.0f, 0.0f}); + compare({360.0f, 360.0f, 0.0f}, {1.0f, 0.0f, 0.0f}); + compare({ 0.0f, 90.0f, 0.0f}, {0.0f, 1.0f, 0.0f}); + compare({ 0.0f, -90.0f, 0.0f}, {0.0f,-1.0f, 0.0f}); + compare({ 90.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 1.0f}); + compare({180.0f, -90.0f, 0.0f}, {0.0f, 1.0f, 0.0f}); +} + TEST(geom, point_in_rectangle) { ASSERT_TRUE(Math::PointInRectangle( Vector3D(0, 0, 0), Vector3D(1, 1, 0), 0, Vector3D(0, 0, 0))); diff --git a/PythonAPI/automatic_control.py b/PythonAPI/automatic_control.py index 1bbca6f4f..ccc01a104 100644 --- a/PythonAPI/automatic_control.py +++ b/PythonAPI/automatic_control.py @@ -216,7 +216,7 @@ class KeyboardControl(object): self._control.gear = 1 if self._control.reverse else -1 elif event.key == K_m: self._control.manual_gear_shift = not self._control.manual_gear_shift - self._control.gear = world.vehicle.get_vehicle_control().gear + self._control.gear = world.vehicle.get_control().gear world.hud.notification( '%s Transmission' % ('Manual' if self._control.manual_gear_shift else 'Automatic')) elif self._control.manual_gear_shift and event.key == K_COMMA: @@ -284,7 +284,7 @@ class HUD(object): return t = world.vehicle.get_transform() v = world.vehicle.get_velocity() - c = world.vehicle.get_vehicle_control() + c = world.vehicle.get_control() heading = 'N' if abs(t.rotation.yaw) < 89.5 else '' heading += 'S' if abs(t.rotation.yaw) > 90.5 else '' heading += 'E' if 179.5 > t.rotation.yaw > 0.5 else '' diff --git a/PythonAPI/dynamic_weather.py b/PythonAPI/dynamic_weather.py index 7473ed6a6..2151e63e8 100755 --- a/PythonAPI/dynamic_weather.py +++ b/PythonAPI/dynamic_weather.py @@ -132,7 +132,7 @@ def main(): elapsed_time = 0.0 while True: - timestamp = world.wait_for_tick() + timestamp = world.wait_for_tick(seconds=30.0) elapsed_time += timestamp.delta_seconds if elapsed_time > update_freq: weather.tick(speed_factor * elapsed_time) diff --git a/PythonAPI/manual_control.py b/PythonAPI/manual_control.py index 11b999e0c..ab30e3b9b 100755 --- a/PythonAPI/manual_control.py +++ b/PythonAPI/manual_control.py @@ -136,15 +136,17 @@ def get_actor_display_name(actor, truncate=250): class World(object): - def __init__(self, carla_world, hud): + def __init__(self, carla_world, hud, actor_filter): self.world = carla_world self.hud = hud - self.vehicle = None + self.player = None self.collision_sensor = None self.lane_invasion_sensor = None + self.gnss_sensor = None self.camera_manager = None self._weather_presets = find_weather_presets() self._weather_index = 0 + self._actor_filter = actor_filter self.restart() self.world.on_tick(hud.on_world_tick) @@ -152,31 +154,32 @@ class World(object): # Keep same camera config if the camera manager exists. cam_index = self.camera_manager._index if self.camera_manager is not None else 0 cam_pos_index = self.camera_manager._transform_index if self.camera_manager is not None else 0 - # Get a random vehicle blueprint. - blueprint = random.choice(self.world.get_blueprint_library().filter('vehicle')) + # Get a random blueprint. + blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) blueprint.set_attribute('role_name', 'hero') if blueprint.has_attribute('color'): color = random.choice(blueprint.get_attribute('color').recommended_values) blueprint.set_attribute('color', color) - # Spawn the vehicle. - if self.vehicle is not None: - spawn_point = self.vehicle.get_transform() + # Spawn the player. + if self.player is not None: + spawn_point = self.player.get_transform() spawn_point.location.z += 2.0 spawn_point.rotation.roll = 0.0 spawn_point.rotation.pitch = 0.0 self.destroy() - self.vehicle = self.world.try_spawn_actor(blueprint, spawn_point) - while self.vehicle is None: + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + while self.player is None: spawn_points = self.world.get_map().get_spawn_points() spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() - self.vehicle = self.world.try_spawn_actor(blueprint, spawn_point) + self.player = self.world.try_spawn_actor(blueprint, spawn_point) # Set up the sensors. - self.collision_sensor = CollisionSensor(self.vehicle, self.hud) - self.lane_invasion_sensor = LaneInvasionSensor(self.vehicle, self.hud) - self.camera_manager = CameraManager(self.vehicle, self.hud) + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud) self.camera_manager._transform_index = cam_pos_index self.camera_manager.set_sensor(cam_index, notify=False) - actor_type = get_actor_display_name(self.vehicle) + actor_type = get_actor_display_name(self.player) self.hud.notification(actor_type) def next_weather(self, reverse=False): @@ -184,7 +187,7 @@ class World(object): self._weather_index %= len(self._weather_presets) preset = self._weather_presets[self._weather_index] self.hud.notification('Weather: %s' % preset[1]) - self.vehicle.get_world().set_weather(preset[0]) + self.player.get_world().set_weather(preset[0]) def tick(self, clock): self.hud.tick(self, clock) @@ -198,7 +201,8 @@ class World(object): self.camera_manager.sensor, self.collision_sensor.sensor, self.lane_invasion_sensor.sensor, - self.vehicle] + self.gnss_sensor.sensor, + self.player] for actor in actors: if actor is not None: actor.destroy() @@ -212,9 +216,16 @@ class World(object): class KeyboardControl(object): def __init__(self, world, start_in_autopilot): self._autopilot_enabled = start_in_autopilot - self._control = carla.VehicleControl() + if isinstance(world.player, carla.Vehicle): + self._control = carla.VehicleControl() + world.player.set_autopilot(self._autopilot_enabled) + elif isinstance(world.player, carla.Walker): + self._control = carla.WalkerControl() + self._autopilot_enabled = False + self._rotation = world.player.get_transform().rotation + else: + raise NotImplementedError("Actor type not supported") self._steer_cache = 0.0 - world.vehicle.set_autopilot(self._autopilot_enabled) world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) def parse_events(self, world, clock): @@ -242,26 +253,30 @@ class KeyboardControl(object): world.camera_manager.set_sensor(event.key - 1 - K_0) elif event.key == K_r: world.camera_manager.toggle_recording() - elif event.key == K_q: - self._control.gear = 1 if self._control.reverse else -1 - elif event.key == K_m: - self._control.manual_gear_shift = not self._control.manual_gear_shift - self._control.gear = world.vehicle.get_vehicle_control().gear - world.hud.notification('%s Transmission' % ('Manual' if self._control.manual_gear_shift else 'Automatic')) - elif self._control.manual_gear_shift and event.key == K_COMMA: - self._control.gear = max(-1, self._control.gear - 1) - elif self._control.manual_gear_shift and event.key == K_PERIOD: - self._control.gear = self._control.gear + 1 - elif event.key == K_p: - self._autopilot_enabled = not self._autopilot_enabled - world.vehicle.set_autopilot(self._autopilot_enabled) - world.hud.notification('Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) + if isinstance(self._control, carla.VehicleControl): + if event.key == K_q: + self._control.gear = 1 if self._control.reverse else -1 + elif event.key == K_m: + self._control.manual_gear_shift = not self._control.manual_gear_shift + self._control.gear = world.player.get_control().gear + world.hud.notification('%s Transmission' % ('Manual' if self._control.manual_gear_shift else 'Automatic')) + elif self._control.manual_gear_shift and event.key == K_COMMA: + self._control.gear = max(-1, self._control.gear - 1) + elif self._control.manual_gear_shift and event.key == K_PERIOD: + self._control.gear = self._control.gear + 1 + elif event.key == K_p: + self._autopilot_enabled = not self._autopilot_enabled + world.player.set_autopilot(self._autopilot_enabled) + world.hud.notification('Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) if not self._autopilot_enabled: - self._parse_keys(pygame.key.get_pressed(), clock.get_time()) - self._control.reverse = self._control.gear < 0 - world.vehicle.apply_control(self._control) + if isinstance(self._control, carla.VehicleControl): + self._parse_vehicle_keys(pygame.key.get_pressed(), clock.get_time()) + self._control.reverse = self._control.gear < 0 + elif isinstance(self._control, carla.WalkerControl): + self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time()) + world.player.apply_control(self._control) - def _parse_keys(self, keys, milliseconds): + def _parse_vehicle_keys(self, keys, milliseconds): self._control.throttle = 1.0 if keys[K_UP] or keys[K_w] else 0.0 steer_increment = 5e-4 * milliseconds if keys[K_LEFT] or keys[K_a]: @@ -275,6 +290,22 @@ class KeyboardControl(object): self._control.brake = 1.0 if keys[K_DOWN] or keys[K_s] else 0.0 self._control.hand_brake = keys[K_SPACE] + def _parse_walker_keys(self, keys, milliseconds): + self._control.speed = 0.0 + if keys[K_DOWN] or keys[K_s]: + self._control.speed = 0.0 + if keys[K_LEFT] or keys[K_a]: + self._control.speed = .01 + self._rotation.yaw -= 0.08 * milliseconds + if keys[K_RIGHT] or keys[K_d]: + self._control.speed = .01 + self._rotation.yaw += 0.08 * milliseconds + if keys[K_UP] or keys[K_w]: + self._control.speed = 5.556 if pygame.key.get_mods() & KMOD_SHIFT else 2.778 + self._control.jump = keys[K_SPACE] + self._rotation.yaw = round(self._rotation.yaw, 1) + self._control.direction = self._rotation.get_forward_vector() + @staticmethod def _is_quit_shortcut(key): return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) @@ -312,9 +343,9 @@ class HUD(object): def tick(self, world, clock): if not self._show_info: return - t = world.vehicle.get_transform() - v = world.vehicle.get_velocity() - c = world.vehicle.get_vehicle_control() + t = world.player.get_transform() + v = world.player.get_velocity() + c = world.player.get_control() heading = 'N' if abs(t.rotation.yaw) < 89.5 else '' heading += 'S' if abs(t.rotation.yaw) > 90.5 else '' heading += 'E' if 179.5 > t.rotation.yaw > 0.5 else '' @@ -328,32 +359,39 @@ class HUD(object): 'Server: % 16d FPS' % self.server_fps, 'Client: % 16d FPS' % clock.get_fps(), '', - 'Vehicle: % 20s' % get_actor_display_name(world.vehicle, truncate=20), + 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), 'Map: % 20s' % world.world.map_name, 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), '', 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)), u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (t.rotation.yaw, heading), 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (t.location.x, t.location.y)), + 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), 'Height: % 18.0f m' % t.location.z, - '', - ('Throttle:', c.throttle, 0.0, 1.0), - ('Steer:', c.steer, -1.0, 1.0), - ('Brake:', c.brake, 0.0, 1.0), - ('Reverse:', c.reverse), - ('Hand brake:', c.hand_brake), - ('Manual:', c.manual_gear_shift), - 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear), + ''] + if isinstance(c, carla.VehicleControl): + self._info_text += [ + ('Throttle:', c.throttle, 0.0, 1.0), + ('Steer:', c.steer, -1.0, 1.0), + ('Brake:', c.brake, 0.0, 1.0), + ('Reverse:', c.reverse), + ('Hand brake:', c.hand_brake), + ('Manual:', c.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] + elif isinstance(c, carla.WalkerControl): + self._info_text += [ + ('Speed:', c.speed, 0.0, 5.556), + ('Jump:', c.jump)] + self._info_text += [ '', 'Collision:', collision, '', - 'Number of vehicles: % 8d' % len(vehicles) - ] + 'Number of vehicles: % 8d' % len(vehicles)] if len(vehicles) > 1: self._info_text += ['Nearby vehicles:'] distance = lambda l: math.sqrt((l.x - t.location.x)**2 + (l.y - t.location.y)**2 + (l.z - t.location.z)**2) - vehicles = [(distance(x.get_location()), x) for x in vehicles if x.id != world.vehicle.id] + vehicles = [(distance(x.get_location()), x) for x in vehicles if x.id != world.player.id] for d, vehicle in sorted(vehicles): if d > 200.0: break @@ -531,6 +569,33 @@ class LaneInvasionSensor(object): text = ['%r' % str(x).split()[-1] for x in set(event.crossed_lane_markings)] self._hud.notification('Crossed line %s' % ' and '.join(text)) +# ============================================================================== +# -- GnssSensor -------------------------------------------------------- +# ============================================================================== + + +class GnssSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + self.lat = 0.0 + self.lon = 0.0 + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.gnss') + self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=1.0, z=2.8)), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) + + @staticmethod + def _on_gnss_event(weak_self, event): + self = weak_self() + if not self: + return + self.lat = event.latitude + self.lon = event.longitude + # ============================================================================== # -- CameraManager ------------------------------------------------------------- @@ -651,7 +716,7 @@ def game_loop(args): pygame.HWSURFACE | pygame.DOUBLEBUF) hud = HUD(args.width, args.height) - world = World(client.get_world(), hud) + world = World(client.get_world(), hud, args.filter) controller = KeyboardControl(world, args.autopilot) clock = pygame.time.Clock() @@ -704,6 +769,11 @@ def main(): metavar='WIDTHxHEIGHT', default='1280x720', help='window resolution (default: 1280x720)') + argparser.add_argument( + '--filter', + metavar='PATTERN', + default='vehicle.*', + help='actor filter (default: "vehicle.*")') args = argparser.parse_args() args.width, args.height = [int(x) for x in args.res.split('x')] diff --git a/PythonAPI/setup.py b/PythonAPI/setup.py index e70aa91a4..240475f86 100644 --- a/PythonAPI/setup.py +++ b/PythonAPI/setup.py @@ -25,7 +25,7 @@ def get_libcarla_extensions(): yield os.path.join(root, filename) if os.name == "posix": - if platform.dist()[0].lower() in ["ubuntu", "debian"]: + if platform.dist()[0].lower() in ["ubuntu", "debian", "deepin"]: pwd = os.path.dirname(os.path.realpath(__file__)) pylib = "libboost_python%d%d.a" % (sys.version_info.major, sys.version_info.minor) diff --git a/PythonAPI/source/libcarla/Actor.cpp b/PythonAPI/source/libcarla/Actor.cpp index 56b4fc3dd..182045e69 100644 --- a/PythonAPI/source/libcarla/Actor.cpp +++ b/PythonAPI/source/libcarla/Actor.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include @@ -58,9 +59,13 @@ void export_actor() { .def("get_location", &cc::Actor::GetLocation) .def("get_transform", &cc::Actor::GetTransform) .def("get_velocity", &cc::Actor::GetVelocity) + .def("get_angular_velocity", &cc::Actor::GetAngularVelocity) .def("get_acceleration", &cc::Actor::GetAcceleration) .def("set_location", &cc::Actor::SetLocation, (arg("location"))) .def("set_transform", &cc::Actor::SetTransform, (arg("transform"))) + .def("set_velocity", &cc::Actor::SetVelocity, (arg("vector"))) + .def("set_angular_velocity", &cc::Actor::SetAngularVelocity, (arg("vector"))) + .def("add_impulse", &cc::Actor::AddImpulse, (arg("vector"))) .def("set_simulate_physics", &cc::Actor::SetSimulatePhysics, (arg("enabled")=true)) .def("destroy", CALL_WITHOUT_GIL(cc::Actor, Destroy)) .def(self_ns::str(self_ns::self)) @@ -69,11 +74,18 @@ void export_actor() { class_, boost::noncopyable, boost::shared_ptr>("Vehicle", no_init) .add_property("bounding_box", CALL_RETURNING_COPY(cc::Vehicle, GetBoundingBox)) .def("apply_control", &cc::Vehicle::ApplyControl, (arg("control"))) - .def("get_vehicle_control", &cc::Vehicle::GetVehicleControl) + .def("get_control", &cc::Vehicle::GetControl) .def("set_autopilot", &cc::Vehicle::SetAutopilot, (arg("enabled")=true)) .def(self_ns::str(self_ns::self)) ; + class_, boost::noncopyable, boost::shared_ptr>("Walker", no_init) + .add_property("bounding_box", CALL_RETURNING_COPY(cc::Walker, GetBoundingBox)) + .def("apply_control", &cc::Walker::ApplyControl, (arg("control"))) + .def("get_control", &cc::Walker::GetWalkerControl) + .def(self_ns::str(self_ns::self)) + ; + enum_("TrafficLightState") .value("Off", cr::TrafficLightState::Off) .value("Red", cr::TrafficLightState::Red) diff --git a/PythonAPI/source/libcarla/Control.cpp b/PythonAPI/source/libcarla/Control.cpp index dc28734af..67913ae09 100644 --- a/PythonAPI/source/libcarla/Control.cpp +++ b/PythonAPI/source/libcarla/Control.cpp @@ -5,14 +5,18 @@ // For a copy, see . #include +#include #include namespace carla { namespace rpc { + static auto boolalpha(bool b) { + return b ? "True" : "False"; + }; + std::ostream &operator<<(std::ostream &out, const VehicleControl &control) { - auto boolalpha = [](bool b) { return b ? "True" : "False"; }; out << "VehicleControl(throttle=" << control.throttle << ", steer=" << control.steer << ", brake=" << control.brake @@ -23,12 +27,20 @@ namespace rpc { return out; } + std::ostream &operator<<(std::ostream &out, const WalkerControl &control) { + out << "WalkerControl(direction=" << control.direction + << ", speed=" << control.speed + << ", jump=" << boolalpha(control.jump) << ')'; + return out; + } + } // namespace rpc } // namespace carla void export_control() { using namespace boost::python; namespace cr = carla::rpc; + namespace cg = carla::geom; class_("VehicleControl") .def(init( @@ -50,4 +62,17 @@ void export_control() { .def("__ne__", &cr::VehicleControl::operator!=) .def(self_ns::str(self_ns::self)) ; + + class_("WalkerControl") + .def(init( + (arg("direction")=cg::Vector3D{1.0f, 0.0f, 0.0f}, + arg("speed")=0.0f, + arg("jump")=false))) + .def_readwrite("direction", &cr::WalkerControl::direction) + .def_readwrite("speed", &cr::WalkerControl::speed) + .def_readwrite("jump", &cr::WalkerControl::jump) + .def("__eq__", &cr::WalkerControl::operator==) + .def("__ne__", &cr::WalkerControl::operator!=) + .def(self_ns::str(self_ns::self)) + ; } diff --git a/PythonAPI/source/libcarla/Geom.cpp b/PythonAPI/source/libcarla/Geom.cpp index 7162481b5..0c4a30f16 100644 --- a/PythonAPI/source/libcarla/Geom.cpp +++ b/PythonAPI/source/libcarla/Geom.cpp @@ -100,24 +100,25 @@ class_>("Location") .def_readwrite("pitch", &cg::Rotation::pitch) .def_readwrite("yaw", &cg::Rotation::yaw) .def_readwrite("roll", &cg::Rotation::roll) + .def("get_forward_vector", &cg::Rotation::GetForwardVector) .def("__eq__", &cg::Rotation::operator==) .def("__ne__", &cg::Rotation::operator!=) .def(self_ns::str(self_ns::self)) ; class_("Transform") - .def(init( (arg("location")=cg::Location(), arg("rotation")=cg::Rotation()))) .def_readwrite("location", &cg::Transform::location) .def_readwrite("rotation", &cg::Transform::rotation) - .def("__eq__", &cg::Transform::operator==) - .def("__ne__", &cg::Transform::operator!=) .def("transform", &TransformList) .def("transform", +[](const cg::Transform &self, cg::Vector3D &location) { self.TransformPoint(location); return location; }, arg("in_point")) + .def("get_forward_vector", &cg::Transform::GetForwardVector) + .def("__eq__", &cg::Transform::operator==) + .def("__ne__", &cg::Transform::operator!=) .def(self_ns::str(self_ns::self)) ; diff --git a/PythonAPI/source/libcarla/Sensor.cpp b/PythonAPI/source/libcarla/Sensor.cpp index 9801343d6..06706ffa0 100644 --- a/PythonAPI/source/libcarla/Sensor.cpp +++ b/PythonAPI/source/libcarla/Sensor.cpp @@ -6,6 +6,7 @@ #include #include +#include #include #include #include @@ -39,4 +40,9 @@ void export_sensor() { ("LaneDetector", no_init) .def(self_ns::str(self_ns::self)) ; + + class_, boost::noncopyable, boost::shared_ptr> + ("GnssSensor", no_init) + .def(self_ns::str(self_ns::self)) + ; } diff --git a/PythonAPI/source/libcarla/SensorData.cpp b/PythonAPI/source/libcarla/SensorData.cpp index 32e5b1425..d93607219 100644 --- a/PythonAPI/source/libcarla/SensorData.cpp +++ b/PythonAPI/source/libcarla/SensorData.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include @@ -50,6 +51,15 @@ namespace data { return out; } + std::ostream &operator<<(std::ostream &out, const GnssEvent &meas) { + out << "GnssEvent(frame=" << meas.GetFrameNumber() + << ", lat=" << meas.GetLatitude() + << ", lon=" << meas.GetLongitude() + << ", alt=" << meas.GetAltitude() + << ')'; + return out; + } + } // namespace data } // namespace sensor } // namespace carla @@ -205,4 +215,11 @@ void export_sensor_data() { .add_property("crossed_lane_markings", CALL_RETURNING_COPY(csd::LaneInvasionEvent, GetCrossedLaneMarkings)) .def(self_ns::str(self_ns::self)) ; + + class_, boost::noncopyable, boost::shared_ptr>("GnssEvent", no_init) + .add_property("latitude", CALL_RETURNING_COPY(csd::GnssEvent, GetLatitude)) + .add_property("longitude", CALL_RETURNING_COPY(csd::GnssEvent, GetLongitude)) + .add_property("altitude", CALL_RETURNING_COPY(csd::GnssEvent, GetAltitude)) + .def(self_ns::str(self_ns::self)) + ; } diff --git a/PythonAPI/source/libcarla/World.cpp b/PythonAPI/source/libcarla/World.cpp index 93bb52373..202330786 100644 --- a/PythonAPI/source/libcarla/World.cpp +++ b/PythonAPI/source/libcarla/World.cpp @@ -94,7 +94,7 @@ void export_world() { .def("get_actors", CONST_CALL_WITHOUT_GIL(cc::World, GetActors)) .def("spawn_actor", SPAWN_ACTOR_WITHOUT_GIL(SpawnActor)) .def("try_spawn_actor", SPAWN_ACTOR_WITHOUT_GIL(TrySpawnActor)) - .def("wait_for_tick", &WaitForTick, (arg("seconds")=1.0)) + .def("wait_for_tick", &WaitForTick, (arg("seconds")=10.0)) .def("on_tick", &OnTick, (arg("callback"))) .def(self_ns::str(self_ns::self)) ; diff --git a/PythonAPI/source/libcarla/libcarla.cpp b/PythonAPI/source/libcarla/libcarla.cpp index 223104835..9349e36ad 100644 --- a/PythonAPI/source/libcarla/libcarla.cpp +++ b/PythonAPI/source/libcarla/libcarla.cpp @@ -119,12 +119,12 @@ static auto MakeCallback(boost::python::object callback) { }; } +#include "Geom.cpp" #include "Actor.cpp" #include "Blueprint.cpp" #include "Client.cpp" #include "Control.cpp" #include "Exception.cpp" -#include "Geom.cpp" #include "Map.cpp" #include "Sensor.cpp" #include "SensorData.cpp" diff --git a/Unreal/CarlaUE4/CarlaUE4.uproject b/Unreal/CarlaUE4/CarlaUE4.uproject index c7b4426fb..6106ad0f2 100644 --- a/Unreal/CarlaUE4/CarlaUE4.uproject +++ b/Unreal/CarlaUE4/CarlaUE4.uproject @@ -1,6 +1,6 @@ { "FileVersion": 3, - "EngineAssociation": "4.19", + "EngineAssociation": "4.21", "Category": "", "Description": "", "Modules": [ diff --git a/Unreal/CarlaUE4/Config/DefaultEngine.ini b/Unreal/CarlaUE4/Config/DefaultEngine.ini index db039fc6b..fef2e0538 100644 --- a/Unreal/CarlaUE4/Config/DefaultEngine.ini +++ b/Unreal/CarlaUE4/Config/DefaultEngine.ini @@ -28,13 +28,13 @@ r.DefaultFeature.AmbientOcclusionStaticFraction=False r.DefaultFeature.AutoExposure=False r.CustomDepth=3 r.Streaming.PoolSize=2000 -r.TextureStreaming=False +r.TextureStreaming=True [/Script/AIModule.AISense_Sight] bAutoRegisterAllPawnsAsSources=False bAutoRegisterNewPawnsAsSources=False -[/Script/Engine.RecastNavMesh] +[/Script/NavigationSystem.RecastNavMesh] RuntimeGeneration=Static [/Script/AIModule.CrowdManager] @@ -53,6 +53,7 @@ bEnablePCM=False bEnableStabilization=False bWarnMissingLocks=True bEnable2DPhysics=False +PhysicErrorCorrection=(PingExtrapolation=0.100000,PingLimit=100.000000,ErrorPerLinearDifference=1.000000,ErrorPerAngularDifference=1.000000,MaxRestoredStateError=1.000000,MaxLinearHardSnapDistance=400.000000,PositionLerp=0.000000,AngleLerp=0.400000,LinearVelocityCoefficient=100.000000,AngularVelocityCoefficient=10.000000,ErrorAccumulationSeconds=0.500000,ErrorAccumulationDistanceSq=15.000000,ErrorAccumulationSimilarity=100.000000) LockedAxis=Invalid DefaultDegreesOfFreedom=Full3D BounceThresholdVelocity=200.000000 @@ -69,6 +70,8 @@ bDefaultHasComplexCollision=True bSuppressFaceRemapTable=False bSupportUVFromHitResults=False bDisableActiveActors=False +bDisableKinematicStaticPairs=False +bDisableKinematicKinematicPairs=False bDisableCCD=False bEnableEnhancedDeterminism=False MaxPhysicsDeltaTime=0.333330 @@ -80,5 +83,14 @@ SyncSceneSmoothingFactor=0.000000 AsyncSceneSmoothingFactor=0.990000 InitialAverageFrameRate=0.016667 PhysXTreeRebuildRate=10 +DefaultBroadphaseSettings=(bUseMBPOnClient=False,bUseMBPOnServer=False,MBPBounds=(Min=(X=0.000000,Y=0.000000,Z=0.000000),Max=(X=0.000000,Y=0.000000,Z=0.000000),IsValid=0),MBPNumSubdivs=2) + +[/Script/LinuxTargetPlatform.LinuxTargetSettings] +SpatializationPlugin= +ReverbPlugin= +OcclusionPlugin= +-TargetedRHIs=SF_VULKAN_SM5 +-TargetedRHIs=GLSL_430 ++TargetedRHIs=GLSL_430 diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp index 0035bf94d..c6340eee7 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp @@ -218,6 +218,19 @@ static void AddRecommendedValuesForSensorRoleNames(FActorDefinition &Definition) AddRecommendedValuesForActorRoleName(Definition, {TEXT("front"), TEXT("back"), TEXT("left"), TEXT("right"), TEXT("front_left"), TEXT("front_right"), TEXT("back_left"), TEXT("back_right")}); } +static void AddVariationsForSensor(FActorDefinition &Def) +{ + FActorVariation Tick; + + Tick.Id = TEXT("sensor_tick"); + Tick.Type = EActorAttributeType::Float; + Tick.RecommendedValues = { TEXT("0.0f") }; + Tick.bRestrictToRecommended = false; + + Def.Variations.Emplace(Tick); +} + + FActorDefinition UActorBlueprintFunctionLibrary::MakeGenericSensorDefinition( const FString &Type, const FString &Id) @@ -247,6 +260,7 @@ void UActorBlueprintFunctionLibrary::MakeCameraDefinition( { FillIdAndTags(Definition, TEXT("sensor"), TEXT("camera"), Id); AddRecommendedValuesForSensorRoleNames(Definition); + AddVariationsForSensor(Definition); // FOV. FActorVariation FOV; FOV.Id = TEXT("fov"); @@ -297,6 +311,7 @@ void UActorBlueprintFunctionLibrary::MakeLidarDefinition( { FillIdAndTags(Definition, TEXT("sensor"), TEXT("lidar"), Id); AddRecommendedValuesForSensorRoleNames(Definition); + AddVariationsForSensor(Definition); // Number of channels. FActorVariation Channels; Channels.Id = TEXT("channels"); @@ -392,6 +407,40 @@ void UActorBlueprintFunctionLibrary::MakeVehicleDefinitions( FillActorDefinitionArray(ParameterArray, Definitions, &MakeVehicleDefinition); } +void UActorBlueprintFunctionLibrary::MakePedestrianDefinition( + const FPedestrianParameters &Parameters, + bool &Success, + FActorDefinition &Definition) +{ + /// @todo We need to validate here the params. + FillIdAndTags(Definition, TEXT("walker"), TEXT("pedestrian"), Parameters.Id); + AddRecommendedValuesForActorRoleName(Definition, {TEXT("pedestrian")}); + Definition.Class = Parameters.Class; + + auto GetGender = [](EPedestrianGender Value) { + switch (Value) + { + case EPedestrianGender::Female: return TEXT("female"); + case EPedestrianGender::Male: return TEXT("male"); + default: return TEXT("other"); + } + }; + + Definition.Attributes.Emplace(FActorAttribute{ + TEXT("gender"), + EActorAttributeType::String, + GetGender(Parameters.Gender)}); + + Success = CheckActorDefinition(Definition); +} + +void UActorBlueprintFunctionLibrary::MakePedestrianDefinitions( + const TArray &ParameterArray, + TArray &Definitions) +{ + FillActorDefinitionArray(ParameterArray, Definitions, &MakePedestrianDefinition); +} + /// ============================================================================ /// -- Helpers to retrieve attribute values ------------------------------------ /// ============================================================================ diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.h index 96028c4a9..6ea5af95a 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.h @@ -8,7 +8,8 @@ #include "Carla/Actor/ActorDefinition.h" #include "Carla/Actor/ActorDescription.h" -#include "Carla/Vehicle/CarlaWheeledVehicle.h" +#include "Carla/Actor/PedestrianParameters.h" +#include "Carla/Actor/VehicleParameters.h" #include "Kismet/BlueprintFunctionLibrary.h" @@ -17,30 +18,6 @@ class ASceneCaptureSensor; struct FLidarDescription; -USTRUCT(BlueprintType) -struct CARLA_API FVehicleParameters -{ - GENERATED_BODY() - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - FString Make; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - FString Model; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - TSubclassOf Class; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - int32 NumberOfWheels = 4; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - FString ObjectType; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - TArray RecommendedColors; -}; - UCLASS() class UActorBlueprintFunctionLibrary : public UBlueprintFunctionLibrary { @@ -103,6 +80,17 @@ public: const TArray &ParameterArray, TArray &Definitions); + UFUNCTION(Category = "Carla Actor", BlueprintCallable) + static void MakePedestrianDefinition( + const FPedestrianParameters &Parameters, + bool &Success, + FActorDefinition &Definition); + + UFUNCTION(Category = "Carla Actor", BlueprintCallable) + static void MakePedestrianDefinitions( + const TArray &ParameterArray, + TArray &Definitions); + /// @} /// ========================================================================== /// @name Helpers to retrieve attribute values diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp index c12301611..b25a65923 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp @@ -20,6 +20,10 @@ static FActorView::ActorType FActorRegistry_GetActorType(const FActorView &View) { return FActorView::ActorType::Vehicle; } + else if (nullptr != Cast(View.GetActor())) + { + return FActorView::ActorType::Walker; + } else if (nullptr != Cast(View.GetActor())) { return FActorView::ActorType::TrafficLight; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h index 61c69660c..abe6c46d7 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h @@ -21,6 +21,7 @@ public: enum class ActorType : uint8 { Other, Vehicle, + Walker, TrafficLight }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/PedestrianParameters.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/PedestrianParameters.h new file mode 100644 index 000000000..8dcabef4d --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/PedestrianParameters.h @@ -0,0 +1,37 @@ +// 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 "GameFramework/Character.h" + +#include "PedestrianParameters.generated.h" + +UENUM(BlueprintType) +enum class EPedestrianGender : uint8 +{ + Other UMETA(DisplayName = "Other"), + Female UMETA(DisplayName = "Female"), + Male UMETA(DisplayName = "Male"), + + SIZE UMETA(Hidden), + INVALID UMETA(Hidden) +}; + +USTRUCT(BlueprintType) +struct CARLA_API FPedestrianParameters +{ + GENERATED_BODY() + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString Id; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + TSubclassOf Class; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + EPedestrianGender Gender = EPedestrianGender::Other; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/VehicleParameters.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/VehicleParameters.h new file mode 100644 index 000000000..feadd267d --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/VehicleParameters.h @@ -0,0 +1,36 @@ +// 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/Vehicle/CarlaWheeledVehicle.h" + +#include "VehicleParameters.generated.h" + +USTRUCT(BlueprintType) +struct CARLA_API FVehicleParameters +{ + GENERATED_BODY() + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString Make; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString Model; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + TSubclassOf Class; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + int32 NumberOfWheels = 4; + + /// (OPTIONAL) Use for custom classification of vehicles. + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString ObjectType; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + TArray RecommendedColors; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/AgentComponent.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/AgentComponent.cpp index 9c29dffb8..d31d83560 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/AgentComponent.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Agent/AgentComponent.cpp @@ -26,7 +26,7 @@ UAgentComponent::UAgentComponent(const FObjectInitializer& ObjectInitializer) { bVisible = false; bHiddenInGame = true; - bShouldUpdatePhysicsVolume = false; + SetShouldUpdatePhysicsVolume(false); PrimaryComponentTick.bCanEverTick = false; } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.Build.cs b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.Build.cs index c3ad0988c..adbf1ee21 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.Build.cs +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.Build.cs @@ -8,6 +8,8 @@ public class Carla : ModuleRules { public Carla(ReadOnlyTargetRules Target) : base(Target) { + PrivatePCHHeaderFile = "Carla.h"; + PublicIncludePaths.AddRange( new string[] { // ... add public include paths required here ... @@ -36,11 +38,12 @@ public class Carla : ModuleRules "AIModule", "CoreUObject", "Engine", + "Foliage", + "ImageWriteQueue", + "Landscape", "PhysXVehicles", "Slate", - "SlateCore", - "Landscape", - "Foliage" + "SlateCore" // ... add private dependencies that you statically link with here ... } ); @@ -115,7 +118,6 @@ public class Carla : ModuleRules } else { - PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("c++abi"))); PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("rpc"))); if (UseDebugLibs(Target)) { diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaPlayerState.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaPlayerState.h index 45b83df9d..4d049c703 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaPlayerState.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaPlayerState.h @@ -70,7 +70,7 @@ public: /// @{ UFUNCTION(BlueprintCallable) - const FTransform &GetTransform() const + const FTransform &GetPlayerTransform() const { return Transform; } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/RoadMap.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/RoadMap.cpp index 2994f8f75..7b9211e26 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/RoadMap.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/RoadMap.cpp @@ -5,7 +5,9 @@ // For a copy, see . #include "Carla.h" -#include "RoadMap.h" +#include "Carla/MapGen/RoadMap.h" + +#include "Carla/Sensor/PixelReader.h" #include "FileHelper.h" #include "HighResScreenshot.h" @@ -253,19 +255,16 @@ bool URoadMap::SaveAsPNG(const FString &Folder, const FString &MapName) const return false; } - TArray BitMap; - for (auto Value : RoadMapData) { - BitMap.Emplace(FRoadMapPixelData(Value).EncodeAsColor()); - } - const FString ImagePath = FPaths::Combine(Folder, MapName + TEXT(".png")); const FString MetadataPath = FPaths::Combine(Folder, MapName + TEXT(".txt")); const FIntPoint DestSize(Width, Height); - FString ResultPath; - FHighResScreenshotConfig &HighResScreenshotConfig = GetHighResScreenshotConfig(); - HighResScreenshotConfig.SetHDRCapture(false); - HighResScreenshotConfig.SaveImage(ImagePath, BitMap, DestSize, &ResultPath); + TUniquePtr> PixelData = MakeUnique>(DestSize); + PixelData->Pixels.Reserve(RoadMapData.Num()); + for (auto Value : RoadMapData) { + PixelData->Pixels.Emplace(FRoadMapPixelData(Value).EncodeAsColor()); + } + FPixelReader::SavePixelsToDisk(std::move(PixelData), ImagePath); // Save metadata. FFormatNamedArguments Args; @@ -287,7 +286,7 @@ bool URoadMap::SaveAsPNG(const FString &Folder, const FString &MapName) const UE_LOG(LogCarla, Error, TEXT("Failed to save map metadata")); } - UE_LOG(LogCarla, Log, TEXT("Saved road map to \"%s\""), *ResultPath); + UE_LOG(LogCarla, Log, TEXT("Saved road map to \"%s\""), *ImagePath); return true; } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp index 56bf40d3b..247706ca6 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp @@ -11,66 +11,76 @@ #include "Util/OpenDrive.h" #include +#include #include #include +#include #include -#include - -TArray DirectedPointArray2FVectorArray( - const TArray &DirectedPoints) +static TArray WaypointVector2FVectorArray( + const std::vector &Waypoints, + const float TriggersHeight) { TArray Positions; - Positions.Reserve(DirectedPoints.Num()); - for (int i = 0; i < DirectedPoints.Num(); ++i) + Positions.Reserve(Waypoints.size()); + for (int i = 0; i < Waypoints.size(); ++i) { - Positions.Add(DirectedPoints[i].location); + // Add the trigger height because the z position of the points does not + // influence on the driver AI and is easy to visualize in the editor + Positions.Add(Waypoints[i].ComputeTransform().location + + FVector(0.f, 0.f, TriggersHeight)); } return Positions; } -AOpenDriveActor::AOpenDriveActor(const FObjectInitializer& ObjectInitializer) : - Super(ObjectInitializer) +AOpenDriveActor::AOpenDriveActor(const FObjectInitializer &ObjectInitializer) + : Super(ObjectInitializer) { PrimaryActorTick.bCanEverTick = false; // Structure to hold one-time initialization static struct FConstructorStatics { - // A helper class object we use to find target UTexture2D object in resource package + // A helper class object we use to find target UTexture2D object in resource + // package ConstructorHelpers::FObjectFinderOptional TextureObject; FName Category; FText Name; FConstructorStatics() - // Use helper class object to find the texture resource path - : TextureObject(TEXT("/Carla/Icons/OpenDriveActorIcon")) - , Category(TEXT("OpenDriveActor")) - , Name(NSLOCTEXT("SpriteCategory", "OpenDriveActor", "OpenDriveActor")) - { - } + // Use helper class object to find the texture resource path + : TextureObject(TEXT("/Carla/Icons/OpenDriveActorIcon")), + Category(TEXT("OpenDriveActor")), + Name(NSLOCTEXT("SpriteCategory", "OpenDriveActor", "OpenDriveActor")) + {} } ConstructorStatics; // We need a scene component to attach Icon sprite - USceneComponent* SceneComponent = ObjectInitializer.CreateDefaultSubobject(this, TEXT("SceneComp")); + USceneComponent *SceneComponent = + ObjectInitializer.CreateDefaultSubobject(this, TEXT("SceneComp")); RootComponent = SceneComponent; RootComponent->Mobility = EComponentMobility::Static; #if WITH_EDITORONLY_DATA - SpriteComponent = ObjectInitializer.CreateEditorOnlyDefaultSubobject(this, TEXT("Sprite")); + SpriteComponent = + ObjectInitializer.CreateEditorOnlyDefaultSubobject(this, TEXT("Sprite")); if (SpriteComponent) { - SpriteComponent->Sprite = ConstructorStatics.TextureObject.Get(); // Get the sprite texture from helper class object - SpriteComponent->SpriteInfo.Category = ConstructorStatics.Category; // Assign sprite category name - SpriteComponent->SpriteInfo.DisplayName = ConstructorStatics.Name; // Assign sprite display name - SpriteComponent->SetupAttachment(RootComponent); // Attach sprite to scene component + // Get the sprite texture from helper class object + SpriteComponent->Sprite = ConstructorStatics.TextureObject.Get(); + // Assign sprite category name + SpriteComponent->SpriteInfo.Category = ConstructorStatics.Category; + // Assign sprite display name + SpriteComponent->SpriteInfo.DisplayName = ConstructorStatics.Name; + // Attach sprite to scene component + SpriteComponent->SetupAttachment(RootComponent); SpriteComponent->Mobility = EComponentMobility::Static; - SpriteComponent->SetEditorScale(1.0f); + SpriteComponent->SetEditorScale(1.f); } #endif // WITH_EDITORONLY_DATA } #if WITH_EDITOR -void AOpenDriveActor::PostEditChangeProperty(struct FPropertyChangedEvent& Event) +void AOpenDriveActor::PostEditChangeProperty(struct FPropertyChangedEvent &Event) { Super::PostEditChangeProperty(Event); @@ -129,35 +139,21 @@ void AOpenDriveActor::PostEditChangeProperty(struct FPropertyChangedEvent& Event } #endif // WITH_EDITOR -ARoutePlanner *AOpenDriveActor::GenerateRoutePlanner(const TArray &DirectedPoints) -{ - using CarlaMath = carla::geom::Math; - - TArray Positions = DirectedPointArray2FVectorArray(DirectedPoints); - ARoutePlanner *RoutePlanner = GetWorld()->SpawnActor(); - - RoutePlanner->SetActorRotation(FRotator(0.0f, CarlaMath::to_degrees(DirectedPoints[0].tangent), 0.0f)); - RoutePlanner->SetActorLocation(DirectedPoints[0].location); - RoutePlanner->SetBoxExtent(FVector(70.0f, 70.0f, 50.0f)); - RoutePlanner->AddRoute(1.0f, Positions); - RoutePlanner->Init(); - RoutePlanners.Add(RoutePlanner); - return RoutePlanner; -} - void AOpenDriveActor::BuildRoutes() { + using CarlaMath = carla::geom::Math; using IdType = carla::road::element::id_type; + using Waypoint = carla::road::element::Waypoint; + using WaypointGen = carla::road::WaypointGenerator; std::string ParseError; - // NOTE(Andrei): As the OpenDrive file has the same name as level, - // build the path to the xodr file using the lavel name and the - // game content directory. - FString MapName = GetWorld()->GetMapName(); - FString XodrContent = FOpenDrive::Load(MapName); + // As the OpenDrive file has the same name as level, build the path to the + // xodr file using the lavel name and the game content directory. + const FString XodrContent = FOpenDrive::Load(GetWorld()->GetMapName()); - auto map_ptr = carla::opendrive::OpenDrive::Load(TCHAR_TO_UTF8(*XodrContent), + auto map_ptr = carla::opendrive::OpenDrive::Load( + TCHAR_TO_UTF8(*XodrContent), XmlInputType::CONTENT, &ParseError); @@ -168,63 +164,59 @@ void AOpenDriveActor::BuildRoutes() } const auto &map = map_ptr->GetData(); - std::vector JunctionInfo = map.GetJunctionInformation(); - // NOTE(Andrei): Build the roads that are not junctions - auto RoadIDsView = map.GetAllIds(); - std::vector roadIDs(RoadIDsView.begin(), RoadIDsView.end()); - std::sort(roadIDs.begin(), roadIDs.end()); + // List with waypoints, each one at the end of each lane of the map + const std::vector MapLaneBeginWaypoint = + WaypointGen::GenerateLaneEnd(*map_ptr); - for (auto &&id : roadIDs) + for (auto &&EndLaneWaypoint : MapLaneBeginWaypoint) { - GenerateWaypointsRoad(map.GetRoad(id)); - } + std::vector Successors = WaypointGen::GetSuccessors(EndLaneWaypoint); - // NOTE(Andrei): Build the roads that are junctions as one RoutePlanner - // can have more than one path that can be taken + // generate the RoutePlanner + ARoutePlanner *RoutePlanner = GetWorld()->SpawnActor(); + RoutePlanner->bIsIntersection = std::any_of(Successors.begin(), Successors.end(), [](auto w) { + return w.IsIntersection(); + }); + RoutePlanner->SetBoxExtent(FVector(70.f, 70.f, 50.f)); + RoutePlanner->SetActorRotation(EndLaneWaypoint.ComputeTransform().rotation); + RoutePlanner->SetActorLocation(EndLaneWaypoint.ComputeTransform().location + + FVector(0.f, 0.f, TriggersHeight)); - // junctionId roadID laneID - std::map>> Junctions; - - for (auto && Junction : JunctionInfo) - { - TArray> Waypoints; - - int FromRoadID = Junction.incomming_road; - int ToRoadID = Junction.connection_road; - int JunctonID = Junction.junction_id; - - GenerateWaypointsJunction(map.GetRoad(ToRoadID), Waypoints); - ARoutePlanner *routePlanner = nullptr; - - std::sort(Junction.from_lane.begin(), Junction.from_lane.end(), std::greater()); - - if (Junction.from_lane[0] < 0) + // fill the RoutePlanner with all the needed roads + for (auto &&Successor : Successors) { - std::reverse(Junction.from_lane.begin(), Junction.from_lane.end()); - } + const IdType RoadId = Successor.GetRoadId(); + const float MaxDist = map.GetRoad(RoadId)->GetLength(); - for (size_t n = 0; n < Junction.from_lane.size(); ++n) - { - int FromLaneID = Junction.from_lane[n]; - routePlanner = Junctions[JunctonID][FromRoadID][FromLaneID]; + std::vector Waypoints; - if (routePlanner == nullptr) + Waypoints.emplace_back(Successor); + + for (float Dist = RoadAccuracy; Dist < MaxDist; Dist += RoadAccuracy) { - routePlanner = GenerateRoutePlanner(Waypoints[n]); - routePlanner->SetSplineColor(FColor::MakeRandomColor()); - Junctions[JunctonID][FromRoadID][FromLaneID] = routePlanner; - } - else - { - routePlanner->AddRoute(1.0, DirectedPointArray2FVectorArray(Waypoints[n])); + const auto NewWaypoint = WaypointGen::GetNext(Successor, Dist); + + check(Dist < MaxDist); + check(NewWaypoint.size() == 1); + + Waypoints.emplace_back(NewWaypoint[0]); } + + // merge with the first waypoint of the next lane if needed + Waypoints.emplace_back(WaypointGen::GetNext( + Successor, CarlaMath::clamp(MaxDist - 0.1f, 0.f, MaxDist))[0]); + + check(Waypoints.size() >= 2); + + TArray Positions = WaypointVector2FVectorArray(Waypoints, TriggersHeight); + + RoutePlanner->AddRoute(1.f, Positions); + RoutePlanners.Add(RoutePlanner); } } } -/// Remove all the existing ARoutePlanner and VehicleSpawners previously -/// generated by this class to avoid overlapping void AOpenDriveActor::RemoveRoutes() { const int rp_num = RoutePlanners.Num(); @@ -238,171 +230,6 @@ void AOpenDriveActor::RemoveRoutes() RoutePlanners.Empty(); } -TArray AOpenDriveActor::GenerateLaneZeroPoints( - const RoadSegment *road) -{ - using RoadElevationInfo = carla::road::element::RoadElevationInfo; - - size_t LanesOffsetIndex = 0; - TArray LaneZeroPoints; - - const RoadGeneralInfo *generalInfo = - road->GetInfo(0.0); - std::vector> LanesOffset = generalInfo->GetLanesOffset(); - - for (float WaypointsOffset = 0.0f; WaypointsOffset < road->GetLength() + RoadAccuracy; WaypointsOffset += RoadAccuracy) - { - // NOTE(Andrei): Calculate the which laneOffset has to be used - if (LanesOffsetIndex < LanesOffset.size() - 1 && - WaypointsOffset >= LanesOffset[LanesOffsetIndex + 1].first) - { - ++LanesOffsetIndex; - } - - // NOTE(Andrei): Get waypoin at the offset, and invert the y axis - DirectedPoint Waypoint = road->GetDirectedPointIn(WaypointsOffset); - - // Elevate the generated road 1m because the triggers and visualization - Waypoint.location.z += 1; - - // NOTE(Andrei): Applyed the laneOffset of the lane section - Waypoint.ApplyLateralOffset(LanesOffset[LanesOffsetIndex].second); - - LaneZeroPoints.Add(Waypoint); - } - - return LaneZeroPoints; -} - -TArray> AOpenDriveActor::GenerateRightLaneWaypoints( - const RoadSegment *road, - const TArray &laneZeroPoints) -{ - const RoadInfoLane *lanesInfo = - road->GetInfo(0.0); - std::vector rightLanes = - lanesInfo->getLanesIDs(RoadInfoLane::which_lane_e::Right); - - TArray> retWaypoints; - double currentOffset = 0.0; - - for (size_t j = 0; j < rightLanes.size(); ++j) - { - const LaneInfo *laneInfo = lanesInfo->getLane(rightLanes[j]); - const float HalfWidth = laneInfo->_width * 0.5; - - currentOffset += HalfWidth; - if (laneInfo->_type == "driving") - { - TArray roadWaypoints; - for (int i = 0; i < laneZeroPoints.Num(); ++i) - { - DirectedPoint currentPoint = laneZeroPoints[i]; - currentPoint.ApplyLateralOffset(-currentOffset); - roadWaypoints.Add(currentPoint); - } - if (roadWaypoints.Num() >= 2) - { - retWaypoints.Add(roadWaypoints); - } - } - currentOffset += HalfWidth; - } - return retWaypoints; -} - -TArray> AOpenDriveActor::GenerateLeftLaneWaypoints( - const RoadSegment *road, - const TArray &laneZeroPoints) -{ - using CarlaMath = carla::geom::Math; - - const RoadInfoLane *lanesInfo = - road->GetInfo(0.0); - std::vector leftLanes = lanesInfo->getLanesIDs(RoadInfoLane::which_lane_e::Left); - - TArray> retWaypoints; - double currentOffset = 0.0; - - for (size_t j = 0; j < leftLanes.size(); ++j) - { - const LaneInfo *laneInfo = lanesInfo->getLane(leftLanes[j]); - const float HalfWidth = laneInfo->_width * 0.5; - - currentOffset += HalfWidth; - if (laneInfo->_type == "driving") - { - TArray roadWaypoints; - for (int i = 0; i < laneZeroPoints.Num(); ++i) - { - DirectedPoint currentPoint = laneZeroPoints[i]; - currentPoint.ApplyLateralOffset(currentOffset); - if (currentPoint.tangent + CarlaMath::pi() < CarlaMath::pi_double()) - { - currentPoint.tangent += CarlaMath::pi(); - } - else - { - currentPoint.tangent -= CarlaMath::pi(); - } - roadWaypoints.Add(currentPoint); - } - if (roadWaypoints.Num() >= 2) - { - Algo::Reverse(roadWaypoints); - retWaypoints.Add(roadWaypoints); - } - } - currentOffset += HalfWidth; - } - return retWaypoints; -} - -void AOpenDriveActor::GenerateWaypointsRoad(const RoadSegment *road) -{ - const RoadGeneralInfo *generalInfo = - road->GetInfo(0.0); - if (generalInfo->GetJunctionId() > -1) - { - return; - } - - TArray laneZeroPoints = GenerateLaneZeroPoints(road); - - TArray> rightLaneWaypoints = GenerateRightLaneWaypoints(road, laneZeroPoints); - TArray> leftLaneWaypoints = GenerateLeftLaneWaypoints(road, laneZeroPoints); - - for (int i = 0; i < rightLaneWaypoints.Num(); ++i) - { - GenerateRoutePlanner(rightLaneWaypoints[i]); - } - - for (int i = 0; i < leftLaneWaypoints.Num(); ++i) - { - GenerateRoutePlanner(leftLaneWaypoints[i]); - } -} - -void AOpenDriveActor::GenerateWaypointsJunction( - const RoadSegment *road, - TArray> &out_waypoints) -{ - const RoadGeneralInfo *generalInfo = - road->GetInfo(0.0); - if (generalInfo->GetJunctionId() == -1) - { - return; - } - - TArray laneZeroPoints = GenerateLaneZeroPoints(road); - out_waypoints = GenerateRightLaneWaypoints(road, laneZeroPoints); - - if (out_waypoints.Num() == 0) - { - out_waypoints = GenerateLeftLaneWaypoints(road, laneZeroPoints); - } -} - void AOpenDriveActor::DebugRoutes() const { for (int i = 0; i < RoutePlanners.Num(); ++i) @@ -416,9 +243,9 @@ void AOpenDriveActor::DebugRoutes() const void AOpenDriveActor::RemoveDebugRoutes() const { - #if WITH_EDITOR +#if WITH_EDITOR FlushPersistentDebugLines(GetWorld()); - #endif // WITH_EDITOR +#endif // WITH_EDITOR } void AOpenDriveActor::AddSpawners() @@ -427,11 +254,18 @@ void AOpenDriveActor::AddSpawners() { if (RoutePlanners[i] != nullptr) { - FTransform Trans = RoutePlanners[i]->GetActorTransform(); - AVehicleSpawnPoint *Spawner = GetWorld()->SpawnActor(); - Spawner->SetActorRotation(Trans.GetRotation()); - Spawner->SetActorLocation(Trans.GetTranslation() + FVector(0.0f, 0.0f, SpawnersHeight)); - VehicleSpawners.Add(Spawner); + if (!bOnIntersections && RoutePlanners[i]->bIsIntersection) + { + continue; + } + else + { + FTransform Trans = RoutePlanners[i]->GetActorTransform(); + AVehicleSpawnPoint *Spawner = GetWorld()->SpawnActor(); + Spawner->SetActorRotation(Trans.GetRotation()); + Spawner->SetActorLocation(Trans.GetTranslation() + FVector(0.f, 0.f, SpawnersHeight)); + VehicleSpawners.Add(Spawner); + } } } } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.h index ccc89ac2e..97f61e646 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.h @@ -28,10 +28,12 @@ class CARLA_API AOpenDriveActor : public AActor GENERATED_BODY() protected: - // A UBillboardComponent to hold Icon sprite - UBillboardComponent* SpriteComponent; - // Sprite for the Billboard Component - UTexture2D* SpriteTexture; + + /// A UBillboardComponent to hold Icon sprite + UBillboardComponent *SpriteComponent; + + /// Sprite for the Billboard Component + UTexture2D *SpriteTexture; private: @@ -42,17 +44,23 @@ private: TArray VehicleSpawners; #if WITH_EDITORONLY_DATA - /// Generate the road network using an OpenDrive file (named as the current .umap) + /// Generate the road network using an OpenDrive file (named as the current + /// .umap) UPROPERTY(Category = "Generate", EditAnywhere) bool bGenerateRoutes = false; #endif // WITH_EDITORONLY_DATA /// Distance between waypoints where the cars will drive UPROPERTY(Category = "Generate", EditAnywhere, meta = (ClampMin = "0.01", UIMin = "0.01")) - float RoadAccuracy = 2.0f; + float RoadAccuracy = 2.f; + + /// Trigger elevantion + UPROPERTY(Category = "Generate", EditAnywhere, meta = (ClampMin = "0.01", UIMin = "0.01")) + float TriggersHeight = 100.f; #if WITH_EDITORONLY_DATA - /// Remove the previously generated road network. Also, it will remove spawners if necessary + /// Remove the previously generated road network. Also, it will remove + /// spawners if necessary UPROPERTY(Category = "Generate", EditAnywhere) bool bRemoveRoutes = false; #endif // WITH_EDITORONLY_DATA @@ -61,9 +69,14 @@ private: UPROPERTY(Category = "Spawners", EditAnywhere) bool bAddSpawners = false; - /// Determine the height where the spawners will be placed, relative to each RoutePlanner + /// If true, spawners will be placed on junctions too UPROPERTY(Category = "Spawners", EditAnywhere) - float SpawnersHeight = 300.0; + bool bOnIntersections = false; + + /// Determine the height where the spawners will be placed, relative to each + /// RoutePlanner + UPROPERTY(Category = "Spawners", EditAnywhere) + float SpawnersHeight = 300.f; #if WITH_EDITORONLY_DATA /// Remove already placed spawners if necessary @@ -79,16 +92,12 @@ private: public: - using RoadSegment = carla::road::element::RoadSegment; - using DirectedPoint = carla::road::element::DirectedPoint; - using LaneInfo = carla::road::element::LaneInfo; - using RoadGeneralInfo = carla::road::element::RoadGeneralInfo; - using RoadInfoLane = carla::road::element::RoadInfoLane; - - AOpenDriveActor(const FObjectInitializer& ObjectInitializer); + AOpenDriveActor(const FObjectInitializer &ObjectInitializer); void BuildRoutes(); + /// Remove all the existing ARoutePlanner and VehicleSpawners previously + /// generated by this class to avoid overlapping void RemoveRoutes(); void DebugRoutes() const; @@ -96,29 +105,12 @@ public: void RemoveDebugRoutes() const; #if WITH_EDITOR - void PostEditChangeProperty(struct FPropertyChangedEvent& PropertyChangedEvent); + void PostEditChangeProperty(struct FPropertyChangedEvent &PropertyChangedEvent); + #endif // WITH_EDITOR - ARoutePlanner *GenerateRoutePlanner(const TArray &waypoints); - - TArray GenerateLaneZeroPoints( - const RoadSegment *road); - - TArray> GenerateRightLaneWaypoints( - const RoadSegment *road, - const TArray &laneZeroPoints); - - TArray> GenerateLeftLaneWaypoints( - const RoadSegment *road, - const TArray &laneZeroPoints); - - void GenerateWaypointsJunction( - const RoadSegment *road, - TArray> &waypoints); - - void GenerateWaypointsRoad(const RoadSegment *road); - void AddSpawners(); void RemoveSpawners(); + }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.cpp index 013482c3a..645011bd1 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.cpp @@ -10,6 +10,7 @@ #include "Carla/Actor/ActorBlueprintFunctionLibrary.h" #include "Carla/Actor/ActorRegistry.h" #include "Carla/Game/CarlaEpisode.h" +#include "Carla/Game/CarlaGameInstance.h" #include "Carla/Game/TheNewCarlaGameModeBase.h" ACollisionSensor::ACollisionSensor(const FObjectInitializer& ObjectInitializer) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.h index 994f4e196..a8a916278 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.h @@ -11,6 +11,7 @@ #include "CollisionSensor.generated.h" class UCarlaEpisode; +class UCarlaGameInstance; /// A sensor to register collisions. UCLASS() diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.cpp new file mode 100644 index 000000000..3b96c1e49 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.cpp @@ -0,0 +1,26 @@ +// Copyright (c) 2019 Intel Labs. +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "Carla.h" +#include "Carla/Sensor/GnssSensor.h" +#include "StaticMeshResources.h" + +FActorDefinition AGnssSensor::GetSensorDefinition() +{ + return UActorBlueprintFunctionLibrary::MakeGenericSensorDefinition(TEXT("other"), TEXT("gnss")); +} + +AGnssSensor::AGnssSensor(const FObjectInitializer &ObjectInitializer) + : Super(ObjectInitializer) +{ + PrimaryActorTick.bCanEverTick = false; + + auto MeshComp = CreateDefaultSubobject(TEXT("RootComponent")); + MeshComp->SetCollisionProfileName(UCollisionProfile::NoCollision_ProfileName); + MeshComp->bHiddenInGame = true; + MeshComp->CastShadow = false; + MeshComp->PostPhysicsComponentTick.bCanEverTick = false; + RootComponent = MeshComp; +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.h new file mode 100644 index 000000000..8a81a79a2 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.h @@ -0,0 +1,27 @@ +// Copyright (c) 2019 Intel Labs. +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Carla/Sensor/ShaderBasedSensor.h" + +#include "Carla/Actor/ActorDefinition.h" + +#include "GnssSensor.generated.h" + +/// Gnss sensor representation +/// The actual position calculation is done one client side +UCLASS() +class CARLA_API AGnssSensor : public ASensor +{ + GENERATED_BODY() + +public: + + static FActorDefinition GetSensorDefinition(); + + AGnssSensor(const FObjectInitializer &ObjectInitializer); + +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.cpp index a58ad2241..c4e813393 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.cpp @@ -8,6 +8,8 @@ #include "Carla/Sensor/PixelReader.h" #include "Engine/TextureRenderTarget2D.h" +#include "HighResScreenshot.h" +#include "Runtime/ImageWriteQueue/Public/ImageWriteQueue.h" // For now we only support Vulkan on Windows. #if PLATFORM_WINDOWS @@ -91,21 +93,38 @@ bool FPixelReader::WritePixelsToArray( return RTResource->ReadPixels(BitMap, ReadPixelFlags); } -bool FPixelReader::SavePixelsToDisk( +TUniquePtr> FPixelReader::DumpPixels( + UTextureRenderTarget2D &RenderTarget) +{ + const FIntPoint DestSize(RenderTarget.GetSurfaceWidth(), RenderTarget.GetSurfaceHeight()); + TUniquePtr> PixelData = MakeUnique>(DestSize); + if (!WritePixelsToArray(RenderTarget, PixelData->Pixels)) { + return nullptr; + } + return PixelData; +} + +TFuture FPixelReader::SavePixelsToDisk( UTextureRenderTarget2D &RenderTarget, const FString &FilePath) { - TArray OutBMP; - if (!WritePixelsToArray(RenderTarget, OutBMP)) { - return false; - } - for (FColor &color : OutBMP) { - color.A = 255u; - } - const FIntPoint DestSize(RenderTarget.GetSurfaceWidth(), RenderTarget.GetSurfaceHeight()); - FString ResultPath; + return SavePixelsToDisk(DumpPixels(RenderTarget), FilePath); +} + +TFuture FPixelReader::SavePixelsToDisk( + TUniquePtr> PixelData, + const FString &FilePath) +{ + TUniquePtr ImageTask = MakeUnique(); + ImageTask->PixelData = MoveTemp(PixelData); + ImageTask->Filename = FilePath; + ImageTask->Format = EImageFormat::PNG; + ImageTask->CompressionQuality = (int32)EImageCompressionQuality::Default; + ImageTask->bOverwriteFile = true; + ImageTask->PixelPreProcessors.Add(TAsyncAlphaWrite(255)); + FHighResScreenshotConfig &HighResScreenshotConfig = GetHighResScreenshotConfig(); - return HighResScreenshotConfig.SaveImage(FilePath, OutBMP, DestSize, &ResultPath); + return HighResScreenshotConfig.ImageWriteQueue->Enqueue(MoveTemp(ImageTask)); } void FPixelReader::WritePixelsToBuffer( diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.h index aeea7ad37..8104cb9ea 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.h @@ -8,6 +8,7 @@ #include "CoreGlobals.h" #include "Engine/TextureRenderTarget2D.h" +#include "Runtime/ImageWriteQueue/Public/ImagePixelData.h" #include #include @@ -32,21 +33,25 @@ public: UTextureRenderTarget2D &RenderTarget, TArray &BitMap); - /// Save the pixels in @a RenderTarget to disk. + /// Dump the pixels in @a RenderTarget. /// /// @pre To be called from game-thread. - static bool SavePixelsToDisk( + static TUniquePtr> DumpPixels( + UTextureRenderTarget2D &RenderTarget); + + /// Asynchronously save the pixels in @a RenderTarget to disk. + /// + /// @pre To be called from game-thread. + static TFuture SavePixelsToDisk( UTextureRenderTarget2D &RenderTarget, const FString &FilePath); - /// Copy the pixels in @a RenderTarget into @a Buffer. + /// Asynchronously save the pixels in @a PixelData to disk. /// - /// @pre To be called from render-thread. - static void WritePixelsToBuffer( - UTextureRenderTarget2D &RenderTarget, - carla::Buffer &Buffer, - uint32 Offset, - FRHICommandListImmediate &InRHICmdList); + /// @pre To be called from game-thread. + static TFuture SavePixelsToDisk( + TUniquePtr> PixelData, + const FString &FilePath); /// Convenience function to enqueue a render command that sends the pixels /// down the @a Sensor's data stream. It expects a sensor derived from @@ -58,6 +63,17 @@ public: /// @pre To be called from game-thread. template static void SendPixelsInRenderThread(TSensor &Sensor); + +private: + + /// Copy the pixels in @a RenderTarget into @a Buffer. + /// + /// @pre To be called from render-thread. + static void WritePixelsToBuffer( + UTextureRenderTarget2D &RenderTarget, + carla::Buffer &Buffer, + uint32 Offset, + FRHICommandListImmediate &InRHICmdList); }; // ============================================================================= diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureSensor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureSensor.h index e950b2759..94b336b42 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureSensor.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureSensor.h @@ -77,10 +77,10 @@ public: /// Use for debugging purposes only. UFUNCTION(BlueprintCallable) - bool SaveCaptureToDisk(const FString &FilePath) const + void SaveCaptureToDisk(const FString &FilePath) const { check(CaptureRenderTarget != nullptr); - return FPixelReader::SavePixelsToDisk(*CaptureRenderTarget, FilePath); + FPixelReader::SavePixelsToDisk(*CaptureRenderTarget, FilePath); } protected: diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Sensor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Sensor.h index 2464687ee..e8edaf5e7 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Sensor.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Sensor.h @@ -9,19 +9,29 @@ #include "GameFramework/Actor.h" #include "Carla/Actor/ActorDescription.h" +#include "Carla/Actor/ActorBlueprintFunctionLibrary.h" #include "Carla/Sensor/DataStream.h" #include "Sensor.generated.h" /// Base class for sensors. -UCLASS(Abstract, hidecategories=(Collision, Attachment, Actor)) +UCLASS(Abstract, hidecategories = (Collision, Attachment, Actor)) class CARLA_API ASensor : public AActor { GENERATED_BODY() public: - virtual void Set(const FActorDescription &) {} + virtual void Set(const FActorDescription &Description) + { + // set the tick interval of the sensor + if (Description.Variations.Contains("sensor_tick")) + { + SetActorTickInterval( + UActorBlueprintFunctionLibrary::ActorAttributeToFloat(Description.Variations["sensor_tick"], + 0.0f)); + } + } /// Replace the FDataStream associated with this sensor. /// diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp index a1b414798..e5b0706d0 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp @@ -8,6 +8,7 @@ #include "Carla/Sensor/WorldObserver.h" #include "Carla/Traffic/TrafficLightBase.h" +#include "Carla/Walker/WalkerController.h" #include "CoreGlobals.h" @@ -30,6 +31,15 @@ static auto AWorldObserver_GetActorState(const FActorView &View) state.vehicle_control = carla::rpc::VehicleControl{Vehicle->GetVehicleControl()}; } } + else if (AType::Walker == View.GetActorType()) + { + auto Walker = Cast(View.GetActor()); + auto Controller = Walker != nullptr ? Cast(Walker->GetController()) : nullptr; + if (Controller != nullptr) + { + state.walker_control = carla::rpc::WalkerControl{Controller->GetWalkerControl()}; + } + } else if (AType::TrafficLight == View.GetActorType()) { auto TrafficLight = Cast(View.GetActor()); @@ -70,10 +80,16 @@ static carla::Buffer AWorldObserver_Serialize( check(actor_view.GetActor() != nullptr); constexpr float TO_METERS = 1e-2; const auto velocity = TO_METERS * actor_view.GetActor()->GetVelocity(); + // get the angular velocity + const auto RootComponent = Cast(actor_view.GetActor()->GetRootComponent()); + FVector angularVelocity { 0.0f, 0.0f, 0.0f }; + if (RootComponent != nullptr) + angularVelocity = RootComponent->GetPhysicsAngularVelocityInDegrees(); ActorDynamicState info = { actor_view.GetActorId(), actor_view.GetActor()->GetActorTransform(), carla::geom::Vector3D{velocity.X, velocity.Y, velocity.Z}, + carla::geom::Vector3D{angularVelocity.X, angularVelocity.Y, angularVelocity.Z}, AWorldObserver_GetActorState(actor_view) }; write_data(info); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaEncoder.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaEncoder.cpp index b6b56f108..ccaa62c0e 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaEncoder.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaEncoder.cpp @@ -95,7 +95,7 @@ void FCarlaEncoder::Encode( Data.platform_timestamp = PlayerState.GetPlatformTimeStamp(); Data.game_timestamp = PlayerState.GetGameTimeStamp(); auto &Player = Data.player_measurements; - ::Encode(PlayerState.GetTransform(), Player.transform); + ::Encode(PlayerState.GetPlayerTransform(), Player.transform); ::Encode(PlayerState.GetBoundingBoxTransform(), Player.bounding_box.transform); ::Encode(PlayerState.GetBoundingBoxExtent() * TO_METERS, Player.bounding_box.extent); ::Encode(PlayerState.GetAcceleration() * TO_METERS, Player.acceleration); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp index 3bb302190..ba2df7292 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp @@ -8,9 +8,11 @@ #include "Carla/Server/TheNewCarlaServer.h" #include "Carla/Sensor/Sensor.h" +#include "Carla/Util/BoundingBoxCalculator.h" #include "Carla/Util/DebugShapeDrawer.h" #include "Carla/Util/OpenDrive.h" #include "Carla/Vehicle/CarlaWheeledVehicle.h" +#include "Carla/Walker/WalkerController.h" #include "GameFramework/SpectatorPawn.h" @@ -24,7 +26,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -112,30 +116,17 @@ private: ::AttachActors(Child.GetActor(), Parent.GetActor()); } - carla::geom::BoundingBox GetActorBoundingBox(const AActor &Actor) - { - /// @todo Bounding boxes only available for vehicles. - auto Vehicle = Cast(&Actor); - if (Vehicle != nullptr) - { - FVector Location = Vehicle->GetVehicleBoundingBoxTransform().GetTranslation(); - FVector Extent = Vehicle->GetVehicleBoundingBoxExtent(); - return {Location, Extent}; - } - return {}; - } - public: carla::rpc::Actor SerializeActor(FActorView ActorView) { carla::rpc::Actor Actor; Actor.id = ActorView.GetActorId(); - if (ActorView.IsValid()) + if (ActorView.IsValid() && !ActorView.GetActor()->IsPendingKill()) { Actor.parent_id = Episode->GetActorRegistry().Find(ActorView.GetActor()->GetOwner()).GetActorId(); Actor.description = *ActorView.GetActorDescription(); - Actor.bounding_box = GetActorBoundingBox(*ActorView.GetActor()); + Actor.bounding_box = UBoundingBoxCalculator::GetActorBoundingBox(ActorView.GetActor()); Actor.semantic_tags.reserve(ActorView.GetSemanticTags().Num()); using tag_t = decltype(Actor.semantic_tags)::value_type; for (auto &&Tag : ActorView.GetSemanticTags()) @@ -308,6 +299,82 @@ void FTheNewCarlaServer::FPimpl::BindActions() return {ActorView.GetActor()->GetActorTransform()}; }); + Server.BindSync("get_actor_velocity", [this](cr::Actor Actor) -> cr::Vector3D { + RequireEpisode(); + auto ActorView = Episode->GetActorRegistry().Find(Actor.id); + if (!ActorView.IsValid() || ActorView.GetActor()->IsPendingKill()) { + RespondErrorStr("unable to get actor velocity: actor not found"); + } + return cr::Vector3D(ActorView.GetActor()->GetRootComponent()->GetComponentVelocity()).ToMeters(); + }); + + Server.BindSync("get_actor_angular_velocity", [this](cr::Actor Actor) -> cr::Vector3D { + RequireEpisode(); + auto ActorView = Episode->GetActorRegistry().Find(Actor.id); + if (!ActorView.IsValid() || ActorView.GetActor()->IsPendingKill()) { + RespondErrorStr("unable to get actor angular velocity: actor not found"); + } + auto RootComponent = Cast(ActorView.GetActor()->GetRootComponent()); + if (RootComponent == nullptr) { + RespondErrorStr("unable to get actor angular velocity: not supported by actor"); + } + return cr::Vector3D(RootComponent->GetPhysicsAngularVelocityInDegrees()); + }); + + Server.BindSync("set_actor_angular_velocity", [this]( + cr::Actor Actor, + cr::Vector3D vector) { + RequireEpisode(); + auto ActorView = Episode->GetActorRegistry().Find(Actor.id); + if (!ActorView.IsValid() || ActorView.GetActor()->IsPendingKill()) { + RespondErrorStr("unable to set actor angular velocity: actor not found"); + } + auto RootComponent = Cast(ActorView.GetActor()->GetRootComponent()); + if (RootComponent == nullptr) { + RespondErrorStr("unable to set actor angular velocity: not supported by actor"); + } + RootComponent->SetPhysicsAngularVelocityInDegrees( + vector, + false, + "None"); + }); + + Server.BindSync("set_actor_velocity", [this]( + cr::Actor Actor, + cr::Vector3D vector) { + RequireEpisode(); + auto ActorView = Episode->GetActorRegistry().Find(Actor.id); + if (!ActorView.IsValid() || ActorView.GetActor()->IsPendingKill()) { + RespondErrorStr("unable to set actor velocity: actor not found"); + } + auto RootComponent = Cast(ActorView.GetActor()->GetRootComponent()); + if (RootComponent == nullptr) { + RespondErrorStr("unable to set actor velocity: not supported by actor"); + } + RootComponent->SetPhysicsLinearVelocity( + vector.ToCentimeters(), + false, + "None"); + }); + + Server.BindSync("add_actor_impulse", [this]( + cr::Actor Actor, + cr::Vector3D vector) { + RequireEpisode(); + auto ActorView = Episode->GetActorRegistry().Find(Actor.id); + if (!ActorView.IsValid() || ActorView.GetActor()->IsPendingKill()) { + RespondErrorStr("unable to add actor impulse: actor not found"); + } + auto RootComponent = Cast(ActorView.GetActor()->GetRootComponent()); + if (RootComponent == nullptr) { + RespondErrorStr("unable to add actor impulse: not supported by actor"); + } + RootComponent->AddImpulse( + vector.ToCentimeters(), + "None", + false); + }); + Server.BindSync("set_actor_location", [this]( cr::Actor Actor, cr::Location Location) { @@ -356,7 +423,7 @@ void FTheNewCarlaServer::FPimpl::BindActions() RootComponent->SetSimulatePhysics(bEnabled); }); - Server.BindSync("apply_control_to_actor", [this](cr::Actor Actor, cr::VehicleControl Control) { + Server.BindSync("apply_control_to_vehicle", [this](cr::Actor Actor, cr::VehicleControl Control) { RequireEpisode(); auto ActorView = Episode->GetActorRegistry().Find(Actor.id); if (!ActorView.IsValid() || ActorView.GetActor()->IsPendingKill()) { @@ -369,6 +436,23 @@ void FTheNewCarlaServer::FPimpl::BindActions() Vehicle->ApplyVehicleControl(Control); }); + Server.BindSync("apply_control_to_walker", [this](cr::Actor Actor, cr::WalkerControl Control) { + RequireEpisode(); + auto ActorView = Episode->GetActorRegistry().Find(Actor.id); + if (!ActorView.IsValid() || ActorView.GetActor()->IsPendingKill()) { + RespondErrorStr("unable to apply control: actor not found"); + } + auto Pawn = Cast(ActorView.GetActor()); + if (Pawn == nullptr) { + RespondErrorStr("unable to apply control: actor is not a walker"); + } + auto Controller = Cast(Pawn->GetController()); + if (Controller == nullptr) { + RespondErrorStr("unable to apply control: walker has an incompatible controller"); + } + Controller->ApplyWalkerControl(Control); + }); + Server.BindSync("set_actor_autopilot", [this](cr::Actor Actor, bool bEnabled) { RequireEpisode(); auto ActorView = Episode->GetActorRegistry().Find(Actor.id); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettingsDelegate.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettingsDelegate.h index 5a8a721d5..f7d401bc4 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettingsDelegate.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettingsDelegate.h @@ -6,12 +6,16 @@ #pragma once +#include "Carla/Settings/QualityLevelUE.h" + #include "CoreMinimal.h" #include "Engine/StaticMesh.h" #include "Engine/World.h" #include "CarlaSettingsDelegate.generated.h" +class UCarlaSettings; + /// Used to set settings for every actor that is spawned into the world. UCLASS(BlueprintType) class CARLA_API UCarlaSettingsDelegate : public UObject diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.cpp index ca52e007f..d7b0a2139 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.cpp @@ -57,12 +57,10 @@ ARoutePlanner::ARoutePlanner(const FObjectInitializer &ObjectInitializer) TriggerVolume->SetHiddenInGame(true); TriggerVolume->SetMobility(EComponentMobility::Static); TriggerVolume->SetCollisionProfileName(FName("OverlapAll")); - TriggerVolume->bGenerateOverlapEvents = true; + TriggerVolume->SetGenerateOverlapEvents(true); // Do not change default value here, our autopilot depends on this. TriggerVolume->SetBoxExtent(FVector{32.0f, 32.0f, 32.0f}); - - SplineColor = FColor::Black; } void ARoutePlanner::BeginDestroy() @@ -203,17 +201,29 @@ void ARoutePlanner::DrawRoutes() { for (int j = 0, lenNumPoints = Routes[i]->GetNumberOfSplinePoints() - 1; j < lenNumPoints; ++j) { - FVector p0 = Routes[i]->GetLocationAtSplinePoint(j + 0, ESplineCoordinateSpace::World); - FVector p1 = Routes[i]->GetLocationAtSplinePoint(j + 1, ESplineCoordinateSpace::World); + const FVector p0 = Routes[i]->GetLocationAtSplinePoint(j + 0, ESplineCoordinateSpace::World); + const FVector p1 = Routes[i]->GetLocationAtSplinePoint(j + 1, ESplineCoordinateSpace::World); - if (SplineColor == FColor::Black) + static const float MinThickness = 3.f; + static const float MaxThickness = 15.f; + + const float Dist = (float) j / (float) lenNumPoints; + const float OneMinusDist = 1.f - Dist; + const float Thickness = OneMinusDist * MaxThickness + MinThickness; + + if (bIsIntersection) { - float f = (float) j / (float) lenNumPoints; - DrawDebugLine(GetWorld(), p0, p1, FColor(255 * f, 255 - 255 * f, 0), true); + // from blue to black + DrawDebugLine( + GetWorld(), p0, p1, FColor(0, 0, 255 * OneMinusDist), + true, -1.f, 0, Thickness); } else { - DrawDebugLine(GetWorld(), p0, p1, SplineColor, true); + // from green to black + DrawDebugLine( + GetWorld(), p0, p1, FColor(0, 255 * OneMinusDist, 0), + true, -1.f, 0, Thickness); } } } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.h index a34bff259..1af540165 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.h @@ -35,11 +35,6 @@ public: TriggerVolume->SetBoxExtent(Extent); } - void SetSplineColor(FColor Color) - { - SplineColor = Color; - } - void DrawRoutes(); void AddRoute(float probability, const TArray &routePoints); @@ -50,6 +45,7 @@ protected: #if WITH_EDITOR virtual void PostEditChangeProperty(FPropertyChangedEvent &PropertyChangedEvent) override; + #endif // WITH_EDITOR virtual void BeginPlay() override; @@ -76,7 +72,6 @@ public: UPROPERTY(BlueprintReadWrite, Category = "Traffic Routes", EditAnywhere, EditFixedSize) TArray Probabilities; -private: - - FColor SplineColor; + UPROPERTY(BlueprintReadWrite, Category = "Traffic Routes", EditAnywhere, EditFixedSize) + bool bIsIntersection = false; }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBox.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBox.h new file mode 100644 index 000000000..60eb2221b --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBox.h @@ -0,0 +1,23 @@ +// 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 "BoundingBox.generated.h" + +USTRUCT(BlueprintType) +struct CARLA_API FBoundingBox +{ + GENERATED_BODY() + + /// Origin of the bounding box relative to its owner. + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FVector Origin = {0.0f, 0.0f, 0.0f}; + + /// Radii extent of the bounding box. + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FVector Extent = {0.0f, 0.0f, 0.0f}; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp new file mode 100644 index 000000000..49cdd790e --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp @@ -0,0 +1,44 @@ +// 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/BoundingBoxCalculator.h" + +#include "Carla/Vehicle/CarlaWheeledVehicle.h" + +#include "Components/CapsuleComponent.h" +#include "GameFramework/Character.h" + +FBoundingBox UBoundingBoxCalculator::GetActorBoundingBox(const AActor *Actor) +{ + if (Actor != nullptr) + { + // Vehicle. + auto Vehicle = Cast(Actor); + if (Vehicle != nullptr) + { + FVector Origin = Vehicle->GetVehicleBoundingBoxTransform().GetTranslation(); + FVector Extent = Vehicle->GetVehicleBoundingBoxExtent(); + return {Origin, Extent}; + } + // Walker. + auto Character = Cast(Actor); + if (Character != nullptr) + { + auto Capsule = Character->GetCapsuleComponent(); + if (Capsule != nullptr) + { + const auto Radius = Capsule->GetScaledCapsuleRadius(); + const auto HalfHeight = Capsule->GetScaledCapsuleHalfHeight(); + // Characters have the pivot point centered. + FVector Origin = {0.0f, 0.0f, 0.0f}; + FVector Extent = {Radius, Radius, HalfHeight}; + return {Origin, Extent}; + } + } + } + return {}; +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h new file mode 100644 index 000000000..816ed173c --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h @@ -0,0 +1,30 @@ +// 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/Util/BoundingBox.h" + +#include "Kismet/BlueprintFunctionLibrary.h" + +#include "BoundingBoxCalculator.generated.h" + +class AActor; + +UCLASS() +class CARLA_API UBoundingBoxCalculator : public UBlueprintFunctionLibrary +{ + GENERATED_BODY() + +public: + + /// Compute the bounding box of the given Carla actor. + /// + /// @warning If the actor type is not supported a default initialized bounding + /// box is returned. + UFUNCTION(Category = "Carla Actor", BlueprintCallable) + static FBoundingBox GetActorBoundingBox(const AActor *Actor); +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerControl.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerControl.h new file mode 100644 index 000000000..2efba7f8a --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerControl.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 + +#include "WalkerControl.generated.h" + +USTRUCT(BlueprintType) +struct CARLA_API FWalkerControl +{ + GENERATED_BODY() + + UPROPERTY(Category = "Walker Control", EditAnywhere, BlueprintReadWrite) + FVector Direction = {1.0f, 0.0f, 0.0f}; + + UPROPERTY(Category = "Walker Control", EditAnywhere, BlueprintReadWrite) + float Speed = 0.0f; + + UPROPERTY(Category = "Walker Control", EditAnywhere, BlueprintReadWrite) + bool Jump = false; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerController.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerController.cpp new file mode 100644 index 000000000..6b1b5d7e5 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerController.cpp @@ -0,0 +1,56 @@ +// 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/Walker/WalkerController.h" + +#include "GameFramework/CharacterMovementComponent.h" +#include "GameFramework/Pawn.h" + +AWalkerController::AWalkerController(const FObjectInitializer& ObjectInitializer) + : Super(ObjectInitializer) +{ + PrimaryActorTick.bCanEverTick = true; +} + +void AWalkerController::Possess(APawn *InPawn) +{ + Super::Possess(InPawn); + + auto *Character = Cast(InPawn); + if (Character == nullptr) + { + UE_LOG(LogCarla, Error, TEXT("Walker is not a character!")); + return; + } + + auto *MovementComponent = Character->GetCharacterMovement(); + if (MovementComponent == nullptr) + { + UE_LOG(LogCarla, Error, TEXT("Walker missing character movement component!")); + return; + } + + MovementComponent->MaxWalkSpeed = GetMaximumWalkSpeed(); + MovementComponent->JumpZVelocity = 500.0f; + Character->JumpMaxCount = 2; +} + +void AWalkerController::Tick(float DeltaSeconds) +{ + Super::Tick(DeltaSeconds); + + auto *Character = GetCharacter(); + + if (Character != nullptr) + { + Character->AddMovementInput(Control.Direction, Control.Speed / GetMaximumWalkSpeed()); + if (Control.Jump) + { + Character->Jump(); + } + } +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerController.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerController.h new file mode 100644 index 000000000..d41b2ee91 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerController.h @@ -0,0 +1,52 @@ +// 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/Walker/WalkerControl.h" + +#include "CoreMinimal.h" +#include "GameFramework/Controller.h" + +#include "WalkerController.generated.h" + +UCLASS() +class CARLA_API AWalkerController : public AController +{ + GENERATED_BODY() + +public: + + AWalkerController(const FObjectInitializer& ObjectInitializer); + + void Possess(APawn *InPawn) override; + + void Tick(float DeltaSeconds) override; + + /// Maximum walk speed allowed in centimetres per second. + UFUNCTION(BlueprintCallable) + float GetMaximumWalkSpeed() const + { + return 4096.0f; // ~147 km/h + } + + UFUNCTION(BlueprintCallable) + void ApplyWalkerControl(const FWalkerControl &InControl) + { + Control = InControl; + } + + UFUNCTION(BlueprintCallable) + const FWalkerControl &GetWalkerControl() const + { + return Control; + } + +private: + + UPROPERTY(VisibleAnywhere) + FWalkerControl Control; +}; diff --git a/Unreal/CarlaUE4/Source/CarlaUE4/CarlaUE4.Build.cs b/Unreal/CarlaUE4/Source/CarlaUE4/CarlaUE4.Build.cs index dc178de25..e8847e7ee 100644 --- a/Unreal/CarlaUE4/Source/CarlaUE4/CarlaUE4.Build.cs +++ b/Unreal/CarlaUE4/Source/CarlaUE4/CarlaUE4.Build.cs @@ -6,6 +6,8 @@ public class CarlaUE4 : ModuleRules { public CarlaUE4(ReadOnlyTargetRules Target) : base(Target) { + PrivatePCHHeaderFile = "CarlaUE4.h"; + PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore" }); PrivateDependencyModuleNames.AddRange(new string[] { }); diff --git a/Util/BuildTools/BuildCarlaUE4.bat b/Util/BuildTools/BuildCarlaUE4.bat index 8c1da15ae..2959ff8d8 100644 --- a/Util/BuildTools/BuildCarlaUE4.bat +++ b/Util/BuildTools/BuildCarlaUE4.bat @@ -23,7 +23,7 @@ set BUILD_CARLAUE4_EDITOR=false set LAUNCH_UE4_EDITOR=false set REMOVE_INTERMEDIATE=false -set UE_VERSION=4.19 +set UE_VERSION=4.21 :arg-parse if not "%1"=="" ( diff --git a/Util/BuildTools/BuildPythonAPI.sh b/Util/BuildTools/BuildPythonAPI.sh index 89671c58a..60d625ab7 100755 --- a/Util/BuildTools/BuildPythonAPI.sh +++ b/Util/BuildTools/BuildPythonAPI.sh @@ -2,8 +2,8 @@ source $(dirname "$0")/Environment.sh -export CC=clang-5.0 -export CXX=clang++-5.0 +export CC=clang-6.0 +export CXX=clang++-6.0 # ============================================================================== # -- Parse arguments ----------------------------------------------------------- diff --git a/Util/BuildTools/Package.bat b/Util/BuildTools/Package.bat index e93ee78b7..77cc49234 100644 --- a/Util/BuildTools/Package.bat +++ b/Util/BuildTools/Package.bat @@ -24,7 +24,7 @@ set DO_COPY_FILES=true set DO_TARBALL=true set DO_CLEAN=false -set UE_VERSION=4.19 +set UE_VERSION=4.21 :arg-parse if not "%1"=="" ( diff --git a/Util/BuildTools/Setup.sh b/Util/BuildTools/Setup.sh index 98c47e785..9c06dcdd8 100755 --- a/Util/BuildTools/Setup.sh +++ b/Util/BuildTools/Setup.sh @@ -4,14 +4,14 @@ # -- Set up environment -------------------------------------------------------- # ============================================================================== -command -v /usr/bin/clang++-5.0 >/dev/null 2>&1 || { - echo >&2 "clang 5.0 is required, but it's not installed."; - echo >&2 "make sure you build Unreal Engine with clang 5.0 too."; +command -v /usr/bin/clang++-6.0 >/dev/null 2>&1 || { + echo >&2 "clang 6.0 is required, but it's not installed."; + echo >&2 "make sure you build Unreal Engine with clang 6.0 too."; exit 1; } -export CC=/usr/bin/clang-5.0 -export CXX=/usr/bin/clang++-5.0 +export CC=/usr/bin/clang-6.0 +export CXX=/usr/bin/clang++-6.0 source $(dirname "$0")/Environment.sh @@ -22,7 +22,7 @@ pushd ${CARLA_BUILD_FOLDER} >/dev/null # -- Get and compile libc++ ---------------------------------------------------- # ============================================================================== -LLVM_BASENAME=llvm-5.0 +LLVM_BASENAME=llvm-6.0 LLVM_INCLUDE=${PWD}/${LLVM_BASENAME}-install/include/c++/v1 LLVM_LIBPATH=${PWD}/${LLVM_BASENAME}-install/lib @@ -34,9 +34,9 @@ else log "Retrieving libc++." - git clone --depth=1 -b release_50 https://github.com/llvm-mirror/llvm.git ${LLVM_BASENAME}-source - git clone --depth=1 -b release_50 https://github.com/llvm-mirror/libcxx.git ${LLVM_BASENAME}-source/projects/libcxx - git clone --depth=1 -b release_50 https://github.com/llvm-mirror/libcxxabi.git ${LLVM_BASENAME}-source/projects/libcxxabi + git clone --depth=1 -b release_60 https://github.com/llvm-mirror/llvm.git ${LLVM_BASENAME}-source + git clone --depth=1 -b release_60 https://github.com/llvm-mirror/libcxx.git ${LLVM_BASENAME}-source/projects/libcxx + git clone --depth=1 -b release_60 https://github.com/llvm-mirror/libcxxabi.git ${LLVM_BASENAME}-source/projects/libcxxabi log "Compiling libc++." @@ -58,8 +58,8 @@ else popd >/dev/null # Workaround, it seems LLVM 5.0 does not install these files. - cp -v ${LLVM_BASENAME}-build/include/c++/v1/cxxabi.h ${LLVM_INCLUDE} - cp -v ${LLVM_BASENAME}-build/include/c++/v1/__cxxabi_config.h ${LLVM_INCLUDE} + # cp -v ${LLVM_BASENAME}-build/include/c++/v1/cxxabi.h ${LLVM_INCLUDE} + # cp -v ${LLVM_BASENAME}-build/include/c++/v1/__cxxabi_config.h ${LLVM_INCLUDE} rm -Rf ${LLVM_BASENAME}-source ${LLVM_BASENAME}-build @@ -71,7 +71,8 @@ unset LLVM_BASENAME # -- Get boost includes -------------------------------------------------------- # ============================================================================== -BOOST_BASENAME=boost-1.67.0 +BOOST_VERSION=1.69.0 +BOOST_BASENAME="boost-${BOOST_VERSION}" BOOST_INCLUDE=${PWD}/${BOOST_BASENAME}-install/include BOOST_LIBPATH=${PWD}/${BOOST_BASENAME}-install/lib @@ -83,16 +84,15 @@ else rm -Rf ${BOOST_BASENAME}-source log "Retrieving boost." - wget https://dl.bintray.com/boostorg/release/1.67.0/source/boost_1_67_0.tar.gz + wget "https://dl.bintray.com/boostorg/release/${BOOST_VERSION}/source/boost_${BOOST_VERSION//./_}.tar.gz" log "Extracting boost." - tar -xzf boost_1_67_0.tar.gz + tar -xzf ${BOOST_BASENAME//[-.]/_}.tar.gz mkdir -p ${BOOST_BASENAME}-install/include - mv boost_1_67_0 ${BOOST_BASENAME}-source - # rm -Rf boost_1_67_0 + mv ${BOOST_BASENAME//[-.]/_} ${BOOST_BASENAME}-source pushd ${BOOST_BASENAME}-source >/dev/null - BOOST_TOOLSET="clang-5.0" + BOOST_TOOLSET="clang-6.0" BOOST_CFLAGS="-fPIC -std=c++14 -DBOOST_ERROR_CODE_HEADER_ONLY" py2="/usr/bin/env python2" @@ -114,13 +114,13 @@ else ./b2 toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CFLAGS}" --prefix="../${BOOST_BASENAME}-install" -j ${CARLA_BUILD_CONCURRENCY} stage release ./b2 toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CFLAGS}" --prefix="../${BOOST_BASENAME}-install" -j ${CARLA_BUILD_CONCURRENCY} install ./b2 toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CFLAGS}" --prefix="../${BOOST_BASENAME}-install" -j ${CARLA_BUILD_CONCURRENCY} --clean-all - + # Get rid of python2 build artifacts completely & do a clean build for python3 popd >/dev/null rm -Rf ${BOOST_BASENAME}-source - tar -xzf boost_1_67_0.tar.gz + tar -xzf ${BOOST_BASENAME//[-.]/_}.tar.gz mkdir -p ${BOOST_BASENAME}-install/include - mv boost_1_67_0 ${BOOST_BASENAME}-source + mv ${BOOST_BASENAME//[-.]/_} ${BOOST_BASENAME}-source pushd ${BOOST_BASENAME}-source >/dev/null py3="/usr/bin/env python3" @@ -145,6 +145,7 @@ else popd >/dev/null rm -Rf ${BOOST_BASENAME}-source + rm ${BOOST_BASENAME//[-.]/_}.tar.gz fi @@ -178,7 +179,7 @@ else log "Building rpclib with libc++." - # rpclib does not use any cmake 3.9 feature. + # rpclib does not use any cmake 3.9 feature. # As cmake 3.9 is not standard in Ubuntu 16.04, change cmake version to 3.5 sed -i s/"3.9.0"/"3.5.0"/g ${RPCLIB_BASENAME}-source/CMakeLists.txt @@ -276,7 +277,7 @@ set(CMAKE_CXX_COMPILER ${CXX}) set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -std=c++14 -pthread -fPIC" CACHE STRING "" FORCE) set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Werror -Wall -Wextra" CACHE STRING "" FORCE) # See https://bugs.llvm.org/show_bug.cgi?id=21629 -set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wno-missing-braces" CACHE STRING "" FORCE) +# set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wno-missing-braces" CACHE STRING "" FORCE) # @todo These flags need to be compatible with setup.py compilation. set(CMAKE_CXX_FLAGS_RELEASE_CLIENT "\${CMAKE_CXX_FLAGS_RELEASE} -DNDEBUG -g -fwrapv -O2 -Wall -Wstrict-prototypes -fno-strict-aliasing -Wdate-time -D_FORTIFY_SOURCE=2 -g -fstack-protector-strong -Wformat -Werror=format-security -fPIC -std=c++14 -Wno-missing-braces -DBOOST_ERROR_CODE_HEADER_ONLY -DLIBCARLA_ENABLE_LIFETIME_PROFILER -DLIBCARLA_WITH_PYTHON_SUPPORT" CACHE STRING "" FORCE) diff --git a/Util/BuildTools/Windows.mk b/Util/BuildTools/Windows.mk index 0c4434243..9571f0713 100644 --- a/Util/BuildTools/Windows.mk +++ b/Util/BuildTools/Windows.mk @@ -12,20 +12,20 @@ help: @type "${CARLA_BUILD_TOOLS_FOLDER}\Windows.mk.help" launch: LibCarla - @"${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.bat" --build --ue-version 4.19 + @"${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.bat" --build --ue-version 4.21 launch-editor: LibCarla @"${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.bat" --launch-editor package: PythonAPI - @"${CARLA_BUILD_TOOLS_FOLDER}/Package.bat" --ue-version 4.19 + @"${CARLA_BUILD_TOOLS_FOLDER}/Package.bat" --ue-version 4.21 docs: @doxygen @echo "Documentation index at ./Doxygen/html/index.html" clean: - @"${CARLA_BUILD_TOOLS_FOLDER}/Package.bat" --clean --ue-version 4.19 + @"${CARLA_BUILD_TOOLS_FOLDER}/Package.bat" --clean --ue-version 4.21 @"${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.bat" --clean @"${CARLA_BUILD_TOOLS_FOLDER}/BuildPythonAPI.bat" --clean @@ -34,7 +34,7 @@ clean: rebuild: setup @${CARLA_BUILD_TOOLS_FOLDER}/BuildLibCarla.bat" --rebuild @${CARLA_BUILD_TOOLS_FOLDER}/BuildPythonAPI.bat" --rebuild - @${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.bat" --rebuild --ue-version 4.19 + @${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.bat" --rebuild --ue-version 4.21 check: PythonAPI @echo "Not implemented!" @@ -43,7 +43,7 @@ benchmark: LibCarla @echo "Not implemented!" CarlaUE4Editor: LibCarla - @"${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.bat" --build-editor --ue-version 4.19 + @"${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.bat" --build-editor --ue-version 4.21 .PHONY: PythonAPI PythonAPI: LibCarla diff --git a/Util/InstallersWin/install_boost.bat b/Util/InstallersWin/install_boost.bat index 9e4089d09..b8b126aee 100644 --- a/Util/InstallersWin/install_boost.bat +++ b/Util/InstallersWin/install_boost.bat @@ -42,7 +42,7 @@ if [%B_TOOLSET%] == [] set B_TOOLSET=msvc-14.1 rem If is not set set the number of parallel jobs to the number of CPU threads if [%NUMBER_OF_ASYNC_JOBS%] == [] set NUMBER_OF_ASYNC_JOBS=%NUMBER_OF_PROCESSORS% -set B_VERSION=boost-1.67.0 +set B_VERSION=boost-1.69.0 set B_SRC=boost-src set B_SRC_DIR=%BUILD_DIR%%B_SRC% set B_INSTALL=boost-install diff --git a/mkdocs.yml b/mkdocs.yml index 7a3a42b96..e4d6bb107 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -19,6 +19,7 @@ pages: - 'Python API reference': 'python_api.md' - 'Running without display and selecting GPUs': 'carla_headless.md' - 'Running in a Docker': 'carla_docker.md' + - "How to make a new map with RoadRunner": 'how_to_make_a_new_map.md' - "How to link Epic's Automotive Materials": 'epic_automotive_materials.md' - Contributing: - 'Contribution guidelines': 'CONTRIBUTING.md'