Merge branch 'master' into NoRenderingMode

This commit is contained in:
Manish 2019-01-24 14:44:01 +01:00
commit a892b8e666
89 changed files with 1632 additions and 388 deletions

View File

@ -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

View File

@ -1,8 +1,14 @@
## Latest changes
* 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 support for gnss_sensor
## CARLA 0.9.2

View File

@ -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

View File

@ -285,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 |

View File

@ -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.

View File

@ -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`.

View File

@ -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)
<h3>Environment Setup</h3>

View File

@ -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`

2
Jenkinsfile vendored
View File

@ -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 {

View File

@ -9,7 +9,14 @@
#include "carla/NonCopyable.h"
#ifdef LIBCARLA_WITH_PYTHON_SUPPORT
# include <boost/python.hpp>
# if defined(__clang__)
# pragma clang diagnostic push
# pragma clang diagnostic ignored "-Wdeprecated-register"
# endif
# include <boost/python.hpp>
# if defined(__clang__)
# pragma clang diagnostic pop
# endif
#endif // LIBCARLA_WITH_PYTHON_SUPPORT
namespace carla {

View File

@ -0,0 +1,160 @@
// Copyright (c) 2019 Intel Labs.
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#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 <exception>
#include <cmath>
#include <sstream>
#include <limits>
#if defined(_WIN32) && !defined(_USE_MATH_DEFINES)
#define _USE_MATH_DEFINES
#include <math.h> // 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<class float_type>
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> map = GetWorld().GetMap();
DEBUG_ASSERT(map != nullptr);
auto self = boost::static_pointer_cast<GnssSensor>(shared_from_this());
//parse geo reference string
_map_latitude = std::numeric_limits<double>::quiet_NaN();
_map_longitude = std::numeric_limits<double>::quiet_NaN();
std::vector<std::string> geo_ref_splitted;
StringUtil::Split(geo_ref_splitted, map->GetGeoReference(), " ");
for (auto element: geo_ref_splitted) {
std::vector<std::string> 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<double>::quiet_NaN();
_map_longitude = std::numeric_limits<double>::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<GnssSensor>(self)](const auto &timestamp) {
auto self = weak_self.lock();
if (self != nullptr) {
auto data = self->TickGnssSensor(timestamp);
if (data != nullptr) {
cb(std::move(data));
}
}
});
_is_listening = true;
}
SharedPtr<sensor::SensorData> GnssSensor::TickGnssSensor(
const Timestamp &timestamp) {
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<sensor::data::GnssEvent>(
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

View File

@ -0,0 +1,54 @@
// Copyright (c) 2019 Intel Labs.
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#pragma once
#include "carla/client/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<sensor::SensorData> TickGnssSensor(const Timestamp &timestamp);
double _map_latitude;
double _map_longitude;
bool _is_listening = false;
};
} // namespace client
} // namespace carla

View File

@ -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

View File

@ -54,6 +54,8 @@ namespace client {
const geom::Location &origin,
const geom::Location &destination) const;
std::string GetGeoReference() const;
private:
rpc::MapInfo _description;

View File

@ -22,7 +22,7 @@ namespace client {
}
}
Vehicle::Control Vehicle::GetVehicleControl() const {
Vehicle::Control Vehicle::GetControl() const {
return GetEpisode().Lock()->GetActorDynamicState(*this).state.vehicle_control;
}

View File

@ -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:

View File

@ -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 <https://opensource.org/licenses/MIT>.
#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

View File

@ -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 <https://opensource.org/licenses/MIT>.
#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

View File

@ -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<LaneDetector>(std::move(init), gc);
} else if (description.description.id == "sensor.other.gnss") { /// @todo
return MakeActorImpl<GnssSensor>(std::move(init), gc);
} else if (description.HasAStream()) {
return MakeActorImpl<ServerSideSensor>(std::move(init), gc);
} else if (StringUtil::StartsWith(description.description.id, "vehicle.")) {
return MakeActorImpl<Vehicle>(std::move(init), gc);
} else if (StringUtil::StartsWith(description.description.id, "walker.")) {
return MakeActorImpl<Walker>(std::move(init), gc);
} else if (StringUtil::StartsWith(description.description.id, "traffic.traffic_light")) {
return MakeActorImpl<TrafficLight>(std::move(init), gc);
}

View File

@ -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 <thread>
@ -139,8 +140,12 @@ 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::SubscribeToStream(

View File

@ -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,14 @@ 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 SubscribeToStream(
const streaming::Token &token,
std::function<void(Buffer)> callback);

View File

@ -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"
@ -199,7 +200,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);
}
/// @}

View File

@ -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

View File

@ -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<Vector3D &>(*this) += rhs;
@ -67,6 +75,10 @@ namespace geom {
return rhs;
}
// =========================================================================
// -- Comparison operators -------------------------------------------------
// =========================================================================
bool operator==(const Location &rhs) const {
return static_cast<const Vector3D &>(*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

View File

@ -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<double, double> 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<double, double> 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<double, double> 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<double, double> 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
} // namespace carla

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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<double>::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,6 @@ namespace geom {
return !(*this == rhs);
}
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<double>::epsilon());
double k = 1.0 / len;
return Vector3D(x * k, y * k, z * k);
}
// =========================================================================
/// @todo The following is copy-pasted from MSGPACK_DEFINE_ARRAY.
/// This is a workaround for an issue in msgpack library. The

View File

@ -13,7 +13,7 @@
# pragma clang diagnostic ignored "-Wunused-local-typedef"
#endif
#include <boost/gil/gil_all.hpp>
#include <boost/gil.hpp>
#if defined(__clang__)
# pragma clang diagnostic pop

View File

@ -47,15 +47,23 @@
# ifndef int_p_NULL
# define int_p_NULL (int*)NULL
# endif // int_p_NULL
# include <boost/gil/extension/io/png_io.hpp>
# if defined(__clang__)
# pragma clang diagnostic push
# pragma clang diagnostic ignored "-Wignored-qualifiers"
# pragma clang diagnostic ignored "-Wparentheses"
# endif
# include <boost/gil/extension/io/png.hpp>
# if defined(__clang__)
# pragma clang diagnostic pop
# endif
#endif
#if LIBCARLA_IMAGE_WITH_JPEG_SUPPORT == true
# include <boost/gil/extension/io/jpeg_io.hpp>
# include <boost/gil/extension/io/jpeg.hpp>
#endif
#if LIBCARLA_IMAGE_WITH_TIFF_SUPPORT == true
# include <boost/gil/extension/io/tiff_io.hpp>
# include <boost/gil/extension/io/tiff.hpp>
#endif
#if defined(__clang__)
@ -85,6 +93,11 @@ namespace io {
namespace detail {
template <typename ViewT, typename IOTag>
struct is_write_supported {
static constexpr bool value = boost::gil::is_write_supported<typename boost::gil::get_pixel_type<ViewT>::type, IOTag>::value;
};
struct io_png {
static constexpr bool is_supported = has_png_support();
@ -102,12 +115,12 @@ namespace detail {
template <typename Str, typename ImageT>
static void read_image(Str &&in_filename, ImageT &image) {
boost::gil::png_read_and_convert_image(std::forward<Str>(in_filename), image);
boost::gil::read_and_convert_image(std::forward<Str>(in_filename), image, boost::gil::png_tag());
}
template <typename Str, typename ViewT>
static void write_view(Str &&out_filename, const ViewT &view) {
boost::gil::png_write_view(std::forward<Str>(out_filename), view);
boost::gil::write_view(std::forward<Str>(out_filename), view, boost::gil::png_tag());
}
#endif // LIBCARLA_IMAGE_WITH_PNG_SUPPORT
@ -131,21 +144,22 @@ namespace detail {
template <typename Str, typename ImageT>
static void read_image(Str &&in_filename, ImageT &image) {
boost::gil::jpeg_read_image(std::forward<Str>(in_filename), image);
boost::gil::read_image(std::forward<Str>(in_filename), image, boost::gil::jpeg_tag());
}
template <typename Str, typename ViewT>
static typename std::enable_if<boost::gil::jpeg_write_support<ViewT>::is_supported>::type
static typename std::enable_if<is_write_supported<ViewT, boost::gil::jpeg_tag>::value>::type
write_view(Str &&out_filename, const ViewT &view) {
using namespace boost::gil;
jpeg_write_view(std::forward<Str>(out_filename), view);
boost::gil::write_view(std::forward<Str>(out_filename), view, boost::gil::jpeg_tag());
}
template <typename Str, typename ViewT>
static typename std::enable_if<!boost::gil::jpeg_write_support<ViewT>::is_supported>::type
static typename std::enable_if<!is_write_supported<ViewT, boost::gil::jpeg_tag>::value>::type
write_view(Str &&out_filename, const ViewT &view) {
using namespace boost::gil;
jpeg_write_view(std::forward<Str>(out_filename), color_converted_view<rgb8_pixel_t>(view));
boost::gil::write_view(
std::forward<Str>(out_filename),
boost::gil::color_converted_view<boost::gil::rgb8_pixel_t>(view),
boost::gil::jpeg_tag());
}
#endif // LIBCARLA_IMAGE_WITH_JPEG_SUPPORT
@ -168,21 +182,22 @@ namespace detail {
template <typename Str, typename ImageT>
static void read_image(Str &&in_filename, ImageT &image) {
boost::gil::tiff_read_and_convert_image(std::forward<Str>(in_filename), image);
boost::gil::read_and_convert_image(std::forward<Str>(in_filename), image, boost::gil::tiff_tag());
}
template <typename Str, typename ViewT>
static typename std::enable_if<boost::gil::tiff_write_support<ViewT>::is_supported>::type
static typename std::enable_if<is_write_supported<ViewT, boost::gil::tiff_tag>::value>::type
write_view(Str &&out_filename, const ViewT &view) {
using namespace boost::gil;
tiff_write_view(std::forward<Str>(out_filename), view);
boost::gil::write_view(std::forward<Str>(out_filename), view, boost::gil::tiff_tag());
}
template <typename Str, typename ViewT>
static typename std::enable_if<!boost::gil::tiff_write_support<ViewT>::is_supported>::type
static typename std::enable_if<!is_write_supported<ViewT, boost::gil::tiff_tag>::value>::type
write_view(Str &&out_filename, const ViewT &view) {
using namespace boost::gil;
tiff_write_view(std::forward<Str>(out_filename), color_converted_view<rgb8_pixel_t>(view));
boost::gil::write_view(
std::forward<Str>(out_filename),
boost::gil::color_converted_view<boost::gil::rgb8_pixel_t>(view),
boost::gil::tiff_tag());
}
#endif // LIBCARLA_IMAGE_WITH_TIFF_SUPPORT

View File

@ -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<int, std::map<int, std::vector<carla::road::lane_junction_t>>>;
using road_data_t = std::map<int, carla::opendrive::types::RoadInformation *>;

View File

@ -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;
}
};

View File

@ -239,6 +239,7 @@ namespace types {
/////////////////////////////////////////////////////////////////
struct OpenDriveData {
std::string geoReference;
std::vector<RoadInformation> roads;
std::vector<Junction> junctions;
};

View File

@ -23,6 +23,10 @@ namespace road {
_map_data.SetJunctionInformation(junctionInfo);
}
void SetGeoReference(const std::string &geoReference) {
_map_data.SetGeoReference(geoReference);
}
SharedPtr<Map> Build();
private:

View File

@ -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<lane_junction_t> _junction_information;
std::unordered_map<

View File

@ -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 <https://opensource.org/licenses/MIT>.
#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

View File

@ -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<ADepthCamera *, s11n::ImageSerializer>,
std::pair<ASemanticSegmentationCamera *, s11n::ImageSerializer>,
std::pair<ARayCastLidar *, s11n::LidarSerializer>,
std::pair<ACollisionSensor *, s11n::CollisionEventSerializer>
std::pair<ACollisionSensor *, s11n::CollisionEventSerializer>,
std::pair<AGnssSensor *, s11n::ImageSerializer>
>;
} // 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

View File

@ -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 <cstdint>
@ -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)
@ -67,6 +91,7 @@ namespace detail {
union TypeDependentState {
rpc::TrafficLightState traffic_light_state;
detail::PackedVehicleControl vehicle_control;
detail::PackedWalkerControl walker_control;
} state;
};

View File

@ -0,0 +1,55 @@
// Copyright (c) 2019 Intel Labs.
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#pragma once
#include "carla/road/element/LaneMarking.h"
#include "carla/sensor/SensorData.h"
#include <vector>
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

View File

@ -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")

View File

@ -11,6 +11,17 @@
#include <carla/geom/Transform.h>
#include <limits>
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<float>::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)));

View File

@ -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 ''

View File

@ -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)

View File

@ -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')]

View File

@ -7,6 +7,7 @@
#include <carla/client/Actor.h>
#include <carla/client/TrafficLight.h>
#include <carla/client/Vehicle.h>
#include <carla/client/Walker.h>
#include <carla/rpc/TrafficLightState.h>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
@ -69,11 +70,18 @@ void export_actor() {
class_<cc::Vehicle, bases<cc::Actor>, boost::noncopyable, boost::shared_ptr<cc::Vehicle>>("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_<cc::Walker, bases<cc::Actor>, boost::noncopyable, boost::shared_ptr<cc::Walker>>("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_<cr::TrafficLightState>("TrafficLightState")
.value("Off", cr::TrafficLightState::Off)
.value("Red", cr::TrafficLightState::Red)

View File

@ -5,14 +5,18 @@
// For a copy, see <https://opensource.org/licenses/MIT>.
#include <carla/rpc/VehicleControl.h>
#include <carla/rpc/WalkerControl.h>
#include <ostream>
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_<cr::VehicleControl>("VehicleControl")
.def(init<float, float, float, bool, bool, bool, int>(
@ -50,4 +62,17 @@ void export_control() {
.def("__ne__", &cr::VehicleControl::operator!=)
.def(self_ns::str(self_ns::self))
;
class_<cr::WalkerControl>("WalkerControl")
.def(init<cg::Vector3D, float, bool>(
(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))
;
}

View File

@ -100,24 +100,25 @@ class_<cg::Location, bases<cg::Vector3D>>("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_<cg::Transform>("Transform")
.def(init<cg::Location, cg::Rotation>(
(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))
;

View File

@ -6,6 +6,7 @@
#include <carla/PythonUtil.h>
#include <carla/client/ClientSideSensor.h>
#include <carla/client/GnssSensor.h>
#include <carla/client/LaneDetector.h>
#include <carla/client/Sensor.h>
#include <carla/client/ServerSideSensor.h>
@ -39,4 +40,9 @@ void export_sensor() {
("LaneDetector", no_init)
.def(self_ns::str(self_ns::self))
;
class_<cc::GnssSensor, bases<cc::Sensor>, boost::noncopyable, boost::shared_ptr<cc::GnssSensor>>
("GnssSensor", no_init)
.def(self_ns::str(self_ns::self))
;
}

View File

@ -14,6 +14,7 @@
#include <carla/sensor/data/Image.h>
#include <carla/sensor/data/LaneInvasionEvent.h>
#include <carla/sensor/data/LidarMeasurement.h>
#include <carla/sensor/data/GnssEvent.h>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
@ -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_<csd::GnssEvent, bases<cs::SensorData>, boost::noncopyable, boost::shared_ptr<csd::GnssEvent>>("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))
;
}

View File

@ -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))
;

View File

@ -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"

View File

@ -1,6 +1,6 @@
{
"FileVersion": 3,
"EngineAssociation": "4.19",
"EngineAssociation": "4.21",
"Category": "",
"Description": "",
"Modules": [

View File

@ -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

View File

@ -407,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<FPedestrianParameters> &ParameterArray,
TArray<FActorDefinition> &Definitions)
{
FillActorDefinitionArray(ParameterArray, Definitions, &MakePedestrianDefinition);
}
/// ============================================================================
/// -- Helpers to retrieve attribute values ------------------------------------
/// ============================================================================

View File

@ -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<ACarlaWheeledVehicle> Class;
UPROPERTY(EditAnywhere, BlueprintReadWrite)
int32 NumberOfWheels = 4;
UPROPERTY(EditAnywhere, BlueprintReadWrite)
FString ObjectType;
UPROPERTY(EditAnywhere, BlueprintReadWrite)
TArray<FColor> RecommendedColors;
};
UCLASS()
class UActorBlueprintFunctionLibrary : public UBlueprintFunctionLibrary
{
@ -103,6 +80,17 @@ public:
const TArray<FVehicleParameters> &ParameterArray,
TArray<FActorDefinition> &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<FPedestrianParameters> &ParameterArray,
TArray<FActorDefinition> &Definitions);
/// @}
/// ==========================================================================
/// @name Helpers to retrieve attribute values

View File

@ -20,6 +20,10 @@ static FActorView::ActorType FActorRegistry_GetActorType(const FActorView &View)
{
return FActorView::ActorType::Vehicle;
}
else if (nullptr != Cast<ACharacter>(View.GetActor()))
{
return FActorView::ActorType::Walker;
}
else if (nullptr != Cast<ATrafficLightBase>(View.GetActor()))
{
return FActorView::ActorType::TrafficLight;

View File

@ -21,6 +21,7 @@ public:
enum class ActorType : uint8 {
Other,
Vehicle,
Walker,
TrafficLight
};

View File

@ -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 <https://opensource.org/licenses/MIT>.
#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<ACharacter> Class;
UPROPERTY(EditAnywhere, BlueprintReadWrite)
EPedestrianGender Gender = EPedestrianGender::Other;
};

View File

@ -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 <https://opensource.org/licenses/MIT>.
#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<ACarlaWheeledVehicle> Class;
UPROPERTY(EditAnywhere, BlueprintReadWrite)
int32 NumberOfWheels = 4;
/// (OPTIONAL) Use for custom classification of vehicles.
UPROPERTY(EditAnywhere, BlueprintReadWrite)
FString ObjectType;
UPROPERTY(EditAnywhere, BlueprintReadWrite)
TArray<FColor> RecommendedColors;
};

View File

@ -26,7 +26,7 @@ UAgentComponent::UAgentComponent(const FObjectInitializer& ObjectInitializer)
{
bVisible = false;
bHiddenInGame = true;
bShouldUpdatePhysicsVolume = false;
SetShouldUpdatePhysicsVolume(false);
PrimaryComponentTick.bCanEverTick = false;
}

View File

@ -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))
{

View File

@ -70,7 +70,7 @@ public:
/// @{
UFUNCTION(BlueprintCallable)
const FTransform &GetTransform() const
const FTransform &GetPlayerTransform() const
{
return Transform;
}

View File

@ -5,7 +5,9 @@
// For a copy, see <https://opensource.org/licenses/MIT>.
#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<FColor> 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<TImagePixelData<FColor>> PixelData = MakeUnique<TImagePixelData<FColor>>(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;
}

View File

@ -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)

View File

@ -11,6 +11,7 @@
#include "CollisionSensor.generated.h"
class UCarlaEpisode;
class UCarlaGameInstance;
/// A sensor to register collisions.
UCLASS()

View File

@ -0,0 +1,26 @@
// Copyright (c) 2019 Intel Labs.
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "Carla.h"
#include "Carla/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<UStaticMeshComponent>(TEXT("RootComponent"));
MeshComp->SetCollisionProfileName(UCollisionProfile::NoCollision_ProfileName);
MeshComp->bHiddenInGame = true;
MeshComp->CastShadow = false;
MeshComp->PostPhysicsComponentTick.bCanEverTick = false;
RootComponent = MeshComp;
}

View File

@ -0,0 +1,27 @@
// Copyright (c) 2019 Intel Labs.
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#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);
};

View File

@ -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<TImagePixelData<FColor>> FPixelReader::DumpPixels(
UTextureRenderTarget2D &RenderTarget)
{
const FIntPoint DestSize(RenderTarget.GetSurfaceWidth(), RenderTarget.GetSurfaceHeight());
TUniquePtr<TImagePixelData<FColor>> PixelData = MakeUnique<TImagePixelData<FColor>>(DestSize);
if (!WritePixelsToArray(RenderTarget, PixelData->Pixels)) {
return nullptr;
}
return PixelData;
}
TFuture<bool> FPixelReader::SavePixelsToDisk(
UTextureRenderTarget2D &RenderTarget,
const FString &FilePath)
{
TArray<FColor> 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<bool> FPixelReader::SavePixelsToDisk(
TUniquePtr<TImagePixelData<FColor>> PixelData,
const FString &FilePath)
{
TUniquePtr<FImageWriteTask> ImageTask = MakeUnique<FImageWriteTask>();
ImageTask->PixelData = MoveTemp(PixelData);
ImageTask->Filename = FilePath;
ImageTask->Format = EImageFormat::PNG;
ImageTask->CompressionQuality = (int32)EImageCompressionQuality::Default;
ImageTask->bOverwriteFile = true;
ImageTask->PixelPreProcessors.Add(TAsyncAlphaWrite<FColor>(255));
FHighResScreenshotConfig &HighResScreenshotConfig = GetHighResScreenshotConfig();
return HighResScreenshotConfig.SaveImage(FilePath, OutBMP, DestSize, &ResultPath);
return HighResScreenshotConfig.ImageWriteQueue->Enqueue(MoveTemp(ImageTask));
}
void FPixelReader::WritePixelsToBuffer(

View File

@ -8,6 +8,7 @@
#include "CoreGlobals.h"
#include "Engine/TextureRenderTarget2D.h"
#include "Runtime/ImageWriteQueue/Public/ImagePixelData.h"
#include <compiler/disable-ue4-macros.h>
#include <carla/Buffer.h>
@ -32,21 +33,25 @@ public:
UTextureRenderTarget2D &RenderTarget,
TArray<FColor> &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<TImagePixelData<FColor>> DumpPixels(
UTextureRenderTarget2D &RenderTarget);
/// Asynchronously save the pixels in @a RenderTarget to disk.
///
/// @pre To be called from game-thread.
static TFuture<bool> 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<bool> SavePixelsToDisk(
TUniquePtr<TImagePixelData<FColor>> 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 <typename TSensor>
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);
};
// =============================================================================

View File

@ -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:

View File

@ -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<APawn>(View.GetActor());
auto Controller = Walker != nullptr ? Cast<AWalkerController>(Walker->GetController()) : nullptr;
if (Controller != nullptr)
{
state.walker_control = carla::rpc::WalkerControl{Controller->GetWalkerControl()};
}
}
else if (AType::TrafficLight == View.GetActorType())
{
auto TrafficLight = Cast<ATrafficLightBase>(View.GetActor());

View File

@ -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);

View File

@ -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"
@ -25,6 +27,7 @@
#include <carla/rpc/Server.h>
#include <carla/rpc/Transform.h>
#include <carla/rpc/VehicleControl.h>
#include <carla/rpc/WalkerControl.h>
#include <carla/rpc/WeatherParameters.h>
#include <carla/streaming/Server.h>
#include <compiler/enable-ue4-macros.h>
@ -112,30 +115,17 @@ private:
::AttachActors(Child.GetActor(), Parent.GetActor());
}
carla::geom::BoundingBox GetActorBoundingBox(const AActor &Actor)
{
/// @todo Bounding boxes only available for vehicles.
auto Vehicle = Cast<ACarlaWheeledVehicle>(&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())
@ -356,7 +346,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 +359,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<APawn>(ActorView.GetActor());
if (Pawn == nullptr) {
RespondErrorStr("unable to apply control: actor is not a walker");
}
auto Controller = Cast<AWalkerController>(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);

View File

@ -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

View File

@ -57,7 +57,7 @@ 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});

View File

@ -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 <https://opensource.org/licenses/MIT>.
#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};
};

View File

@ -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 <https://opensource.org/licenses/MIT>.
#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<ACarlaWheeledVehicle>(Actor);
if (Vehicle != nullptr)
{
FVector Origin = Vehicle->GetVehicleBoundingBoxTransform().GetTranslation();
FVector Extent = Vehicle->GetVehicleBoundingBoxExtent();
return {Origin, Extent};
}
// Walker.
auto Character = Cast<ACharacter>(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 {};
}

View File

@ -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 <https://opensource.org/licenses/MIT>.
#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);
};

View File

@ -0,0 +1,24 @@
// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
// de Barcelona (UAB).
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#pragma once
#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;
};

View File

@ -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 <https://opensource.org/licenses/MIT>.
#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<ACharacter>(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();
}
}
}

View File

@ -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 <https://opensource.org/licenses/MIT>.
#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;
};

View File

@ -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[] { });

View File

@ -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"=="" (

View File

@ -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 -----------------------------------------------------------

View File

@ -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"=="" (

View File

@ -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)

View File

@ -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

View File

@ -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