diff --git a/CHANGELOG.md b/CHANGELOG.md index 844adcd89..f84aeb1a5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,11 @@ ## Latest + * Added new sensor: Inertial measurement unit (IMU) * Now all the camera-based sensors are provided with an additional parametrized lens distortion shader + * API changes: + - Lidar: `range` is now set in meters, not in centimeters + - Lidar: `horizontal_angle` is now received in radians, not in degrees + * API extensions: + - Added `carla.IMUMeasurement` * Updated manual_control.py with a lens disortion effect example * Exposed rgb camera attributes: exposure, depth of field, tonemapper, color correction, and chromatic aberration * Fixed pylint for python3 in travis diff --git a/Docs/asset_packages_for_dist.md b/Docs/asset_packages_for_dist.md index c2063c207..d17ecce43 100644 --- a/Docs/asset_packages_for_dist.md +++ b/Docs/asset_packages_for_dist.md @@ -26,14 +26,15 @@ And for the **props** folder, we will need the following files: Additionally, we have to create a **JSON file inside the package** that will contain information about its assets. The file extension must be `.json`. We recommend the JSON file to have the same -name as the package name in order to keep it organized. +name as the package name in order to keep it organized. **Please, keep in mind that the name of this file will used as the name of the distribution package**. The content of this JSON file should be similar to the following: ```json { - "maps": [{ + "maps": [ + { "name": "MyTown01", "source": "./MapToImport01/MapToImport01.fbx", "use_carla_materials": true, @@ -46,7 +47,8 @@ The content of this JSON file should be similar to the following: "xodr": "./MapToImport02/MapToImport02.xodr" } ], - "props": [{ + "props": [ + { "name": "MyProp01", "size": "medium", "source": "./AssetsToImport/PropToImport01/PropToImport01.fbx", @@ -100,7 +102,7 @@ name and, if so, it will create separate mesh files inside Unreal for each diffe - `Vegetation` - `Vehicles` - `Walls` - + Note that if the tag is not spelled correctly, it will interpret it as `None` value by default. To sum up, the `Import` folder should have this similar structure: diff --git a/Docs/cameras_and_sensors.md b/Docs/cameras_and_sensors.md index 4c537bbcc..95a16c5f8 100644 --- a/Docs/cameras_and_sensors.md +++ b/Docs/cameras_and_sensors.md @@ -304,7 +304,7 @@ supposed to generate this frame; `points_per_second / (FPS * channels)`. | Blueprint attribute | Type | Default | Description | | -------------------- | ---- | ------- | ----------- | | `channels` | int | 32 | Number of lasers | -| `range` | float | 1000 | Maximum measurement distance in centimeters | +| `range` | float | 10.0 | Maximum measurement distance in meters _(<=0.9.6: is in centimeters)_ | | `points_per_second` | int | 56000 | Points generated by all lasers per second | | `rotation_frequency` | float | 10.0 | Lidar rotation frequency | | `upper_fov` | float | 10.0 | Angle in degrees of the upper most laser | @@ -322,7 +322,7 @@ objects. | `frame` | int | Frame number when the measurement took place | | `timestamp` | double | Timestamp of the measurement in simulation seconds since the beginning of the episode | | `transform` | carla.Transform | Transform in world coordinates of the sensor at the time of the measurement | -| `horizontal_angle` | float | Angle in XY plane of the lidar this frame (in degrees) | +| `horizontal_angle` | float | Angle in XY plane of the lidar this frame (in radians) | | `channels` | int | Number of channels (lasers) of the lidar | | `get_point_count(channel)` | int | Number of points per channel captured this frame | | `raw_data` | bytes | Array of 32-bits floats (XYZ of each point) | @@ -457,3 +457,23 @@ objects. | `actor` | carla.Actor | Actor that detected the obstacle ("self" actor) | | `other_actor` | carla.Actor | Actor detected as obstacle | | `distance` | float | Distance from actor to other_actor | + +sensor.other.imu +---------------- + +This sensor, when attached to an actor, the user can access to it's accelerometer, gyroscope and compass. + +

Output attributes

+ +This sensor produces +[`carla.IMUMeasurement`](python_api.md#carla.IMUMeasurement) +objects. + +| Sensor data attribute | Type | Description | +| --------------------- | --------------- | ----------- | +| `frame` | int | Frame number when the measurement took place | +| `timestamp` | double | Timestamp of the measurement in simulation seconds since the beginning of the episode | +| `transform` | carla.Transform | Transform in world | +| `accelerometer` | carla.Vector3D | Measures linear acceleration in `m/s^2` | +| `gyroscope` | carla.Vector3D | Measures angular velocity in `rad/sec` | +| `compass` | float | Orientation with respect to the North (`(0.0, -1.0, 0.0)` in Unreal) in radians | diff --git a/Docs/python_api.md b/Docs/python_api.md index 276c6cfc6..9990caf41 100644 --- a/Docs/python_api.md +++ b/Docs/python_api.md @@ -1317,19 +1317,35 @@ Return number of [carla.ActorSnapshot](#carla.ActorSnapshot) present in this [ca --- -## carla.GnssEvent([carla.SensorData](#carla.SensorData)) _class_ +## carla.GnssMeasurement([carla.SensorData](#carla.SensorData)) _class_ Gnss sensor data.

Instance Variables

-- **latitude** (_float_) +- **latitude** (_float_) North/South value of a point on the map. -- **longitude** (_float_) +- **longitude** (_float_) West/East value of a point on the map. -- **altitude** (_float_) +- **altitude** (_float_) Height regarding ground level.

Methods

-- **\__str__**(**self**) +- **\__str__**(**self**) + +--- + +## carla.IMUMeasurement([carla.SensorData](#carla.SensorData)) _class_ +IMU sensor data regarding the sensor World's transformation. + +

Instance Variables

+- **accelerometer** (_[carla.Vector3D](#carla.Vector3D)_) +Measures linear acceleration in `m/s^2`. +- **gyroscope** (_[carla.Vector3D](#carla.Vector3D)_) +Measures angular velocity in `rad/sec`. +- **compass** (_float_) +Orientation with respect to the North (`(0.0, -1.0, 0.0)` in Unreal) in radians. + +

Methods

+- **\__str__**(**self**) --- @@ -1387,7 +1403,7 @@ Lidar sensor measurement data.

Instance Variables

- **horizontal_angle** (_float_) -Horizontal angle that the Lidar has rotated at the time of the measurement. +Horizontal angle that the Lidar has rotated at the time of the measurement (in radians). - **channels** (_int_) Number of lasers. - **raw_data** (_bytes_) diff --git a/LibCarla/source/carla/geom/Math.h b/LibCarla/source/carla/geom/Math.h index e3659a24b..d87a5ac05 100644 --- a/LibCarla/source/carla/geom/Math.h +++ b/LibCarla/source/carla/geom/Math.h @@ -27,6 +27,12 @@ namespace geom { return static_cast(3.14159265358979323846264338327950288); } + template + static constexpr T Pi2() { + static_assert(std::is_floating_point::value, "type must be floating point"); + return static_cast(static_cast(2) * Pi()); + } + template static constexpr T ToDegrees(T rad) { static_assert(std::is_floating_point::value, "type must be floating point"); diff --git a/LibCarla/source/carla/geom/Vector3D.h b/LibCarla/source/carla/geom/Vector3D.h index 6f1606619..8fdc3f54c 100644 --- a/LibCarla/source/carla/geom/Vector3D.h +++ b/LibCarla/source/carla/geom/Vector3D.h @@ -137,6 +137,12 @@ namespace geom { #ifdef LIBCARLA_INCLUDED_FROM_UE4 + /// These 2 methods are explicitly deleted to avoid creating them by other users, + /// unlike locations, some vectors have units and some don't, by removing + /// these methods we found several places were the conversion from cm to m was missing + Vector3D(const FVector &v) = delete; + Vector3D& operator=(const FVector &rhs) = delete; + /// Return a Vector3D converted from centimeters to meters. Vector3D ToMeters() const { return *this * 1e-2f; diff --git a/LibCarla/source/carla/pointcloud/PointCloudIO.cpp b/LibCarla/source/carla/pointcloud/PointCloudIO.cpp index 30c3d4e66..6b9930bdf 100644 --- a/LibCarla/source/carla/pointcloud/PointCloudIO.cpp +++ b/LibCarla/source/carla/pointcloud/PointCloudIO.cpp @@ -14,7 +14,7 @@ namespace pointcloud { void PointCloudIO::WriteHeader(std::ostream &out, size_t number_of_points) { out << "ply\n" "format ascii 1.0\n" - "element vertex " << number_of_points << "\n" + "element vertex " << std::to_string(number_of_points) << "\n" "property float32 x\n" "property float32 y\n" "property float32 z\n" diff --git a/LibCarla/source/carla/sensor/SensorRegistry.h b/LibCarla/source/carla/sensor/SensorRegistry.h index 67976e513..31d1cb188 100644 --- a/LibCarla/source/carla/sensor/SensorRegistry.h +++ b/LibCarla/source/carla/sensor/SensorRegistry.h @@ -17,6 +17,7 @@ #include "carla/sensor/s11n/CollisionEventSerializer.h" #include "carla/sensor/s11n/EpisodeStateSerializer.h" #include "carla/sensor/s11n/ImageSerializer.h" +#include "carla/sensor/s11n/IMUSerializer.h" #include "carla/sensor/s11n/LidarSerializer.h" #include "carla/sensor/s11n/NoopSerializer.h" #include "carla/sensor/s11n/ObstacleDetectionEventSerializer.h" @@ -25,6 +26,7 @@ class ACollisionSensor; class ADepthCamera; class AGnssSensor; +class AInertialMeasurementUnit; class ALaneInvasionSensor; class AObstacleDetectionSensor; class ARayCastLidar; @@ -46,6 +48,7 @@ namespace sensor { std::pair, std::pair, std::pair, + std::pair, std::pair, std::pair, std::pair, @@ -67,6 +70,7 @@ namespace sensor { #include "Carla/Sensor/RayCastLidar.h" #include "Carla/Sensor/SceneCaptureCamera.h" #include "Carla/Sensor/SemanticSegmentationCamera.h" +#include "Carla/Sensor/InertialMeasurementUnit.h" #include "Carla/Sensor/WorldObserver.h" #include "Carla/Sensor/GnssSensor.h" #include "Carla/Sensor/LaneInvasionSensor.h" diff --git a/LibCarla/source/carla/sensor/data/CollisionEvent.h b/LibCarla/source/carla/sensor/data/CollisionEvent.h index 090948674..3374eaafa 100644 --- a/LibCarla/source/carla/sensor/data/CollisionEvent.h +++ b/LibCarla/source/carla/sensor/data/CollisionEvent.h @@ -17,7 +17,7 @@ namespace sensor { namespace data { /// A registered collision. - class CollisionEvent : public SensorData { + class CollisionEvent : public SensorData { using Super = SensorData; protected: diff --git a/LibCarla/source/carla/sensor/data/IMUMeasurement.h b/LibCarla/source/carla/sensor/data/IMUMeasurement.h new file mode 100644 index 000000000..92cae4c69 --- /dev/null +++ b/LibCarla/source/carla/sensor/data/IMUMeasurement.h @@ -0,0 +1,56 @@ +// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "carla/geom/Vector3D.h" +#include "carla/sensor/s11n/IMUSerializer.h" +#include "carla/sensor/SensorData.h" + +namespace carla { +namespace sensor { +namespace data { + + class IMUMeasurement : public SensorData { + protected: + + using Super = SensorData; + + using Serializer = s11n::IMUSerializer; + + friend Serializer; + + explicit IMUMeasurement(const RawData &data) + : Super(data), + _accelerometer(Serializer::DeserializeRawData(data).accelerometer), + _gyroscope(Serializer::DeserializeRawData(data).gyroscope), + _compass(Serializer::DeserializeRawData(data).compass) {} + + public: + + geom::Vector3D GetAccelerometer() const { + return _accelerometer; + } + + geom::Vector3D GetGyroscope() const { + return _gyroscope; + } + + float GetCompass() const { + return _compass; + } + + private: + + geom::Vector3D _accelerometer; + geom::Vector3D _gyroscope; + float _compass; + + }; + +} // namespace data +} // namespace sensor +} // namespace carla diff --git a/LibCarla/source/carla/sensor/s11n/IMUSerializer.cpp b/LibCarla/source/carla/sensor/s11n/IMUSerializer.cpp new file mode 100644 index 000000000..f89e5b542 --- /dev/null +++ b/LibCarla/source/carla/sensor/s11n/IMUSerializer.cpp @@ -0,0 +1,20 @@ +// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "carla/sensor/s11n/IMUSerializer.h" +#include "carla/sensor/data/IMUMeasurement.h" + +namespace carla { +namespace sensor { +namespace s11n { + + SharedPtr IMUSerializer::Deserialize(RawData &&data) { + return SharedPtr(new data::IMUMeasurement(std::move(data))); + } + +} // namespace s11n +} // namespace sensor +} // namespace carla diff --git a/LibCarla/source/carla/sensor/s11n/IMUSerializer.h b/LibCarla/source/carla/sensor/s11n/IMUSerializer.h new file mode 100644 index 000000000..ee0bec53d --- /dev/null +++ b/LibCarla/source/carla/sensor/s11n/IMUSerializer.h @@ -0,0 +1,60 @@ +// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +// #include "carla/Buffer.h" +#include "carla/Memory.h" +#include "carla/sensor/RawData.h" + +namespace carla { +namespace sensor { + + class SensorData; + +namespace s11n { + + class IMUSerializer + { + public: + + struct Data { + + geom::Vector3D accelerometer; + + geom::Vector3D gyroscope; + + float compass; + + MSGPACK_DEFINE_ARRAY(accelerometer, gyroscope, compass) + }; + + template + static Buffer Serialize( + const SensorT &sensor, + const geom::Vector3D &accelerometer, + const geom::Vector3D &gyroscope, + const float compass); + + static Data DeserializeRawData(const RawData &message) { + return MsgPack::UnPack(message.begin(), message.size()); + } + + static SharedPtr Deserialize(RawData &&data); + }; + + template + inline Buffer IMUSerializer::Serialize( + const SensorT &, + const geom::Vector3D &accelerometer, + const geom::Vector3D &gyroscope, + const float compass) { + return MsgPack::Pack(Data{accelerometer, gyroscope, compass}); + } + +} // namespace s11n +} // namespace sensor +} // namespace carla diff --git a/PythonAPI/carla/source/libcarla/SensorData.cpp b/PythonAPI/carla/source/libcarla/SensorData.cpp index cf617b1f3..db620f757 100644 --- a/PythonAPI/carla/source/libcarla/SensorData.cpp +++ b/PythonAPI/carla/source/libcarla/SensorData.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -27,50 +28,60 @@ namespace sensor { namespace data { std::ostream &operator<<(std::ostream &out, const Image &image) { - out << "Image(frame=" << image.GetFrame() - << ", timestamp=" << image.GetTimestamp() - << ", size=" << image.GetWidth() << 'x' << image.GetHeight() + out << "Image(frame=" << std::to_string(image.GetFrame()) + << ", timestamp=" << std::to_string(image.GetTimestamp()) + << ", size=" << std::to_string(image.GetWidth()) << 'x' << std::to_string(image.GetHeight()) << ')'; return out; } std::ostream &operator<<(std::ostream &out, const LidarMeasurement &meas) { - out << "LidarMeasurement(frame=" << meas.GetFrame() - << ", timestamp=" << meas.GetTimestamp() - << ", number_of_points=" << meas.size() + out << "LidarMeasurement(frame=" << std::to_string(meas.GetFrame()) + << ", timestamp=" << std::to_string(meas.GetTimestamp()) + << ", number_of_points=" << std::to_string(meas.size()) << ')'; return out; } std::ostream &operator<<(std::ostream &out, const CollisionEvent &meas) { - out << "CollisionEvent(frame=" << meas.GetFrame() - << ", timestamp=" << meas.GetTimestamp() + out << "CollisionEvent(frame=" << std::to_string(meas.GetFrame()) + << ", timestamp=" << std::to_string(meas.GetTimestamp()) << ", other_actor=" << meas.GetOtherActor() << ')'; return out; } std::ostream &operator<<(std::ostream &out, const ObstacleDetectionEvent &meas) { - out << "ObstacleDetectionEvent(frame=" << meas.GetFrame() - << ", timestamp=" << meas.GetTimestamp() + out << "ObstacleDetectionEvent(frame=" << std::to_string(meas.GetFrame()) + << ", timestamp=" << std::to_string(meas.GetTimestamp()) << ", other_actor=" << meas.GetOtherActor() << ')'; return out; } std::ostream &operator<<(std::ostream &out, const LaneInvasionEvent &meas) { - out << "LaneInvasionEvent(frame=" << meas.GetFrame() - << ", timestamp=" << meas.GetTimestamp() + out << "LaneInvasionEvent(frame=" << std::to_string(meas.GetFrame()) + << ", timestamp=" << std::to_string(meas.GetTimestamp()) << ')'; return out; } std::ostream &operator<<(std::ostream &out, const GnssEvent &meas) { - out << "GnssEvent(frame=" << meas.GetFrame() - << ", timestamp=" << meas.GetTimestamp() - << ", lat=" << meas.GetLatitude() - << ", lon=" << meas.GetLongitude() - << ", alt=" << meas.GetAltitude() + out << "GnssEvent(frame=" << std::to_string(meas.GetFrame()) + << ", timestamp=" << std::to_string(meas.GetTimestamp()) + << ", lat=" << std::to_string(meas.GetLatitude()) + << ", lon=" << std::to_string(meas.GetLongitude()) + << ", alt=" << std::to_string(meas.GetAltitude()) + << ')'; + return out; + } + + std::ostream &operator<<(std::ostream &out, const IMUMeasurement &meas) { + out << "IMUMeasurement(frame=" << std::to_string(meas.GetFrame()) + << ", timestamp=" << std::to_string(meas.GetTimestamp()) + << ", accelerometer=" << meas.GetAccelerometer() + << ", gyroscope=" << meas.GetGyroscope() + << ", compass=" << std::to_string(meas.GetCompass()) << ')'; return out; } @@ -235,4 +246,11 @@ void export_sensor_data() { .add_property("altitude", &csd::GnssEvent::GetAltitude) .def(self_ns::str(self_ns::self)) ; + + class_, boost::noncopyable, boost::shared_ptr>("IMUMeasurement", no_init) + .add_property("accelerometer", &csd::IMUMeasurement::GetAccelerometer) + .add_property("gyroscope", &csd::IMUMeasurement::GetGyroscope) + .add_property("compass", &csd::IMUMeasurement::GetCompass) + .def(self_ns::str(self_ns::self)) + ; } diff --git a/PythonAPI/docs/sensor_data.yml b/PythonAPI/docs/sensor_data.yml index cdf961cdf..4d3aa94db 100644 --- a/PythonAPI/docs/sensor_data.yml +++ b/PythonAPI/docs/sensor_data.yml @@ -115,7 +115,7 @@ - var_name: horizontal_angle type: float doc: > - Horizontal angle that the Lidar has rotated at the time of the measurement + Horizontal angle that the Lidar has rotated at the time of the measurement (in radians) - var_name: channels type: int doc: > @@ -128,7 +128,7 @@ methods: - def_name: get_point_count params: - - param_name: channel + - param_name: channel type: int doc: > Retrieve the number of points that are generated by this channel. @@ -138,7 +138,7 @@ # -------------------------------------- - def_name: save_to_disk params: - - param_name: path + - param_name: path type: str doc: > Save point cloud to disk @@ -209,7 +209,7 @@ # - METHODS ---------------------------- methods: - def_name: __str__ - doc: > + doc: > # -------------------------------------- - class_name: LaneInvasionEvent @@ -230,10 +230,10 @@ # - METHODS ---------------------------- methods: - def_name: __str__ - doc: > + doc: > # -------------------------------------- - - class_name: GnssEvent + - class_name: GnssMeasurement parent: carla.SensorData # - DESCRIPTION ------------------------ doc: > @@ -243,11 +243,11 @@ - var_name: latitude type: float doc: > - North/South value of a point on the map. + North/South value of a point on the map. - var_name: longitude type: float doc: > - West/East value of a point on the map. + West/East value of a point on the map. - var_name: altitude type: float doc: > @@ -255,6 +255,31 @@ # - METHODS ---------------------------- methods: - def_name: __str__ - doc: > + doc: > + # -------------------------------------- + + - class_name: IMUMeasurement + parent: carla.SensorData + # - DESCRIPTION ------------------------ + doc: > + IMU sensor data regarding the sensor World's transformation + # - PROPERTIES ------------------------- + instance_variables: + - var_name: accelerometer + type: carla.Vector3D + doc: > + Measures linear acceleration in `m/s^2`. + - var_name: gyroscope + type: carla.Vector3D + doc: > + Measures angular velocity in `rad/sec`. + - var_name: compass + type: float + doc: > + Orientation with respect to the North (`(0.0, -1.0, 0.0)` in Unreal) in radians. + # - METHODS ---------------------------- + methods: + - def_name: __str__ + doc: > # -------------------------------------- ... diff --git a/PythonAPI/examples/automatic_control.py b/PythonAPI/examples/automatic_control.py index 89f82e4aa..f189c9e35 100755 --- a/PythonAPI/examples/automatic_control.py +++ b/PythonAPI/examples/automatic_control.py @@ -649,7 +649,7 @@ class CameraManager(object): bp.set_attribute('image_size_x', str(hud.dim[0])) bp.set_attribute('image_size_y', str(hud.dim[1])) elif item[0].startswith('sensor.lidar'): - bp.set_attribute('range', '5000') + bp.set_attribute('range', '50') item.append(bp) self.index = None diff --git a/PythonAPI/examples/manual_control.py b/PythonAPI/examples/manual_control.py index c72f089c1..692f7014c 100755 --- a/PythonAPI/examples/manual_control.py +++ b/PythonAPI/examples/manual_control.py @@ -718,7 +718,7 @@ class CameraManager(object): for attr_name, attr_value in item[3].items(): bp.set_attribute(attr_name, attr_value) elif item[0].startswith('sensor.lidar'): - bp.set_attribute('range', '5000') + bp.set_attribute('range', '50') item.append(bp) self.index = None diff --git a/PythonAPI/examples/manual_control_steeringwheel.py b/PythonAPI/examples/manual_control_steeringwheel.py index 022704685..af16c56a9 100755 --- a/PythonAPI/examples/manual_control_steeringwheel.py +++ b/PythonAPI/examples/manual_control_steeringwheel.py @@ -694,7 +694,7 @@ class CameraManager(object): bp.set_attribute('image_size_x', str(hud.dim[0])) bp.set_attribute('image_size_y', str(hud.dim[1])) elif item[0].startswith('sensor.lidar'): - bp.set_attribute('range', '5000') + bp.set_attribute('range', '50') item.append(bp) self.index = None diff --git a/PythonAPI/util/lane_explorer.py b/PythonAPI/util/lane_explorer.py index 2ce29ec40..58b31a3be 100755 --- a/PythonAPI/util/lane_explorer.py +++ b/PythonAPI/util/lane_explorer.py @@ -44,7 +44,7 @@ def draw_transform(debug, trans, col=carla.Color(255, 0, 0), lt=-1): x=trans.location.x + math.cos(pitch_in_rad) * math.cos(yaw_in_rad), y=trans.location.y + math.cos(pitch_in_rad) * math.sin(yaw_in_rad), z=trans.location.z + math.sin(pitch_in_rad)) - debug.draw_arrow(trans.location, p1, thickness=0.05, arrow_size=1.0, color=col, life_time=lt) + debug.draw_arrow(trans.location, p1, thickness=0.05, arrow_size=0.1, color=col, life_time=lt) def draw_waypoint_union(debug, w0, w1, color=carla.Color(255, 0, 0), lt=5): @@ -75,6 +75,10 @@ def main(): default=2000, type=int, help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-i', '--info', + action='store_true', + help='Show text information') argparser.add_argument( '-x', default=0.0, @@ -142,13 +146,13 @@ def main(): potential_w.remove(next_w) # Render some nice information, notice that you can't see the strings if you are using an editor camera - draw_waypoint_info(debug, current_w, trail_life_time) + if args.info: + draw_waypoint_info(debug, current_w, trail_life_time) draw_waypoint_union(debug, current_w, next_w, cyan if current_w.is_junction else green, trail_life_time) draw_transform(debug, current_w.transform, white, trail_life_time) # print the remaining waypoints for p in potential_w: - debug.draw_string(p.transform.location, str(p.lane_id), False, orange, trail_life_time) draw_waypoint_union(debug, current_w, p, red, trail_life_time) draw_transform(debug, p.transform, white, trail_life_time) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp index 39e3ad934..e37ab39e4 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp @@ -657,7 +657,7 @@ void UActorBlueprintFunctionLibrary::MakeLidarDefinition( FActorVariation Range; Range.Id = TEXT("range"); Range.Type = EActorAttributeType::Float; - Range.RecommendedValues = { TEXT("1000.0") }; + Range.RecommendedValues = { TEXT("10.0") }; // 10 meters // Points per second. FActorVariation PointsPerSecond; PointsPerSecond.Id = TEXT("points_per_second"); @@ -1188,10 +1188,11 @@ void UActorBlueprintFunctionLibrary::SetLidar( const FActorDescription &Description, FLidarDescription &Lidar) { + constexpr float TO_CENTIMETERS = 1e2; Lidar.Channels = RetrieveActorAttributeToInt("channels", Description.Variations, Lidar.Channels); Lidar.Range = - RetrieveActorAttributeToFloat("range", Description.Variations, Lidar.Range); + RetrieveActorAttributeToFloat("range", Description.Variations, 10.0f) * TO_CENTIMETERS; Lidar.PointsPerSecond = RetrieveActorAttributeToInt("points_per_second", Description.Variations, Lidar.PointsPerSecond); Lidar.RotationFrequency = diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h index d876ae5ac..0764b1468 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h @@ -17,7 +17,8 @@ public: using IdType = uint32; - enum class ActorType : uint8 { + enum class ActorType : uint8 + { Other, Vehicle, Walker, diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.cpp new file mode 100644 index 000000000..326db8319 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.cpp @@ -0,0 +1,130 @@ +// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "Carla.h" +#include "InertialMeasurementUnit.h" + +#include +#include "carla/geom/Math.h" +#include "carla/geom/Vector3D.h" +#include + +#include "Carla/Vehicle/CarlaWheeledVehicle.h" +#include "Carla/Sensor/WorldObserver.h" +#include "Carla/Actor/ActorBlueprintFunctionLibrary.h" + +// Based on OpenDRIVE's lon and lat +const FVector AInertialMeasurementUnit::CarlaNorthVector = + FVector(0.0f, -1.0f, 0.0f); + +AInertialMeasurementUnit::AInertialMeasurementUnit( + const FObjectInitializer &ObjectInitializer) + : Super(ObjectInitializer) +{ + PrimaryActorTick.bCanEverTick = true; + PrimaryActorTick.TickGroup = TG_PostPhysics; +} + +FActorDefinition AInertialMeasurementUnit::GetSensorDefinition() +{ + return UActorBlueprintFunctionLibrary::MakeGenericSensorDefinition( + TEXT("other"), + TEXT("imu")); +} + +void AInertialMeasurementUnit::Set(const FActorDescription &ActorDescription) +{ + Super::Set(ActorDescription); + // Fill the parameters that the user requested + // Not currently needed in this sensor +} + +void AInertialMeasurementUnit::SetOwner(AActor *Owner) +{ + Super::SetOwner(Owner); +} + +// Copy of FWorldObserver_GetAngularVelocity but using radiants +static FVector carla_GetActorAngularVelocityInRadians(AActor &Actor) +{ + const auto RootComponent = Cast(Actor.GetRootComponent()); + const FVector AngularVelocity = + RootComponent != nullptr ? + RootComponent->GetPhysicsAngularVelocityInRadians() : + FVector { 0.0f, 0.0f, 0.0f }; + + return AngularVelocity; +} + +void AInertialMeasurementUnit::Tick(float DeltaTime) +{ + Super::Tick(DeltaTime); + + namespace cg = carla::geom; + + // Accelerometer measures linear acceleration in m/s2 + constexpr float TO_METERS = 1e-2; + + ACarlaWheeledVehicle *vehicle = Cast(GetOwner()); + const FVector CurrentVelocity = vehicle->GetVehicleForwardSpeed() * vehicle->GetVehicleOrientation(); + float CurrentSimulationTime = GetWorld()->GetTimeSeconds(); + FVector FVectorAccelerometer = + TO_METERS * (CurrentVelocity - PrevVelocity) / DeltaTime; + + PrevVelocity = CurrentVelocity; + + FQuat ImuRotation = GetRootComponent()->GetComponentTransform().GetRotation(); + FVectorAccelerometer = ImuRotation.UnrotateVector(FVectorAccelerometer); + + // Cast from FVector to our Vector3D to correctly send the data in m/s2 + const cg::Vector3D Accelerometer ( + FVectorAccelerometer.X, + FVectorAccelerometer.Y, + FVectorAccelerometer.Z + ); + + // Gyroscope measures angular velocity in rad/sec + const FVector AngularVelocity = + carla_GetActorAngularVelocityInRadians(*GetOwner()); + + const FQuat SensorLocalRotation = + RootComponent->GetRelativeTransform().GetRotation(); + + const FVector FVectorGyroscope = + SensorLocalRotation.RotateVector(AngularVelocity); + + // Cast from FVector to our Vector3D to correctly send the data in rad/s + const cg::Vector3D Gyroscope ( + FVectorGyroscope.X, + FVectorGyroscope.Y, + FVectorGyroscope.Z + ); + + // Magnetometer: orientation with respect to the North in rad + const FVector ForwVect = GetActorForwardVector().GetSafeNormal2D(); + float Compass = std::acos(FVector::DotProduct(CarlaNorthVector, ForwVect)); + + // Keep the angle between [0, 2pi) + if (FVector::CrossProduct(CarlaNorthVector, ForwVect).Z > 0.0f) + { + Compass = cg::Math::Pi2() - Compass; + } + + auto Stream = GetDataStream(*this); + Stream.Send( + *this, + Accelerometer, + Gyroscope, + Compass); +} + +void AInertialMeasurementUnit::BeginPlay() +{ + Super::BeginPlay(); + + constexpr float TO_METERS = 1e-2; + PrevVelocity = GetOwner()->GetVelocity(); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.h new file mode 100644 index 000000000..e161f8042 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.h @@ -0,0 +1,42 @@ +// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Carla/Sensor/Sensor.h" + +#include "Carla/Actor/ActorDefinition.h" +#include "Carla/Actor/ActorDescription.h" + +#include "InertialMeasurementUnit.generated.h" + +UCLASS() +class CARLA_API AInertialMeasurementUnit : public ASensor +{ + GENERATED_BODY() + +public: + + AInertialMeasurementUnit(const FObjectInitializer &ObjectInitializer); + + static FActorDefinition GetSensorDefinition(); + + void Set(const FActorDescription &ActorDescription) override; + + void SetOwner(AActor *Owner) override; + + void Tick(float DeltaTime) override; + + /// Based on OpenDRIVE's lon and lat, North is in (0.0f, -1.0f, 0.0f) + static const FVector CarlaNorthVector; + +private: + + void BeginPlay() override; + + FVector PrevVelocity; + +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarDescription.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarDescription.h index 3c5260f7b..2e45b556f 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarDescription.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarDescription.h @@ -19,7 +19,7 @@ struct CARLA_API FLidarDescription /// Measure distance in centimeters. UPROPERTY(EditAnywhere) - float Range = 5000.0f; + float Range = 1000.0f; /// Points generated by all lasers per second. UPROPERTY(EditAnywhere) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp index 7093239cf..5e5ce464c 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp @@ -6,9 +6,12 @@ #include "Carla.h" #include "Carla/Sensor/RayCastLidar.h" - #include "Carla/Actor/ActorBlueprintFunctionLibrary.h" +#include +#include "carla/geom/Math.h" +#include + #include "DrawDebugHelpers.h" #include "Engine/CollisionProfile.h" #include "Runtime/Engine/Classes/Kismet/KismetMathLibrary.h" @@ -84,7 +87,8 @@ void ARayCastLidar::ReadPoints(const float DeltaTime) check(ChannelCount == LaserAngles.Num()); - const float CurrentHorizontalAngle = LidarMeasurement.GetHorizontalAngle(); + const float CurrentHorizontalAngle = carla::geom::Math::ToDegrees( + LidarMeasurement.GetHorizontalAngle()); const float AngleDistanceOfTick = Description.RotationFrequency * 360.0f * DeltaTime; const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser; @@ -103,7 +107,8 @@ void ARayCastLidar::ReadPoints(const float DeltaTime) } } - const float HorizontalAngle = std::fmod(CurrentHorizontalAngle + AngleDistanceOfTick, 360.0f); + const float HorizontalAngle = carla::geom::Math::ToRadians( + std::fmod(CurrentHorizontalAngle + AngleDistanceOfTick, 360.0f)); LidarMeasurement.SetHorizontalAngle(HorizontalAngle); }