diff --git a/LibCarla/source/carla/sensor/s11n/LidarMeasurement.h b/LibCarla/source/carla/sensor/s11n/LidarData.h similarity index 85% rename from LibCarla/source/carla/sensor/s11n/LidarMeasurement.h rename to LibCarla/source/carla/sensor/s11n/LidarData.h index 790781ba6..d29942c8d 100644 --- a/LibCarla/source/carla/sensor/s11n/LidarMeasurement.h +++ b/LibCarla/source/carla/sensor/s11n/LidarData.h @@ -36,9 +36,6 @@ namespace s11n { /// Xn, Yn, Zn, In /// } /// - /// @warning WritePoint should be called sequentially in the order in which - /// the points are going to be stored, i.e., starting at channel zero and - /// increasing steadily. class LidarDetection { public: @@ -64,7 +61,7 @@ namespace s11n { } }; - class LidarMeasurement { + class LidarData { static_assert(sizeof(float) == sizeof(uint32_t), "Invalid float size"); static const int SizeLidarDetection = 4; @@ -78,12 +75,12 @@ namespace s11n { }; public: - explicit LidarMeasurement(uint32_t ChannelCount = 0u) + explicit LidarData(uint32_t ChannelCount = 0u) : _header(Index::SIZE + ChannelCount, 0u) { _header[Index::ChannelCount] = ChannelCount; } - LidarMeasurement &operator=(LidarMeasurement &&) = default; + LidarData &operator=(LidarData &&) = default; float GetHorizontalAngle() const { return reinterpret_cast(_header[Index::HorizontalAngle]); @@ -97,16 +94,14 @@ namespace s11n { return _header[Index::ChannelCount]; } - void Reset(uint32_t channels, uint32_t channel_point_count) { + void Reset(uint32_t channel_point_count) { std::memset(_header.data() + Index::SIZE, 0, sizeof(uint32_t) * GetChannelCount()); - _points.clear(); - _points.reserve(SizeLidarDetection * channels * channel_point_count); - - _aux_points.resize(channels); + _max_channel_points = channel_point_count; + _aux_points.resize(GetChannelCount()); for (auto& aux : _aux_points) { aux.clear(); - aux.reserve(channel_point_count); + aux.reserve(_max_channel_points); } } @@ -117,6 +112,7 @@ namespace s11n { void SaveDetections() { _points.clear(); + _points.reserve(SizeLidarDetection * GetChannelCount() * _max_channel_points); for (auto idxChannel = 0u; idxChannel < GetChannelCount(); ++idxChannel) { _header[Index::SIZE + idxChannel] = static_cast(_aux_points.size()); @@ -129,10 +125,12 @@ namespace s11n { } } - private: + protected: std::vector _header; std::vector> _aux_points; + uint32_t _max_channel_points; + private: std::vector _points; }; diff --git a/LibCarla/source/carla/sensor/s11n/LidarRawData.h b/LibCarla/source/carla/sensor/s11n/LidarRawData.h index 940d03021..cec1d8084 100644 --- a/LibCarla/source/carla/sensor/s11n/LidarRawData.h +++ b/LibCarla/source/carla/sensor/s11n/LidarRawData.h @@ -105,13 +105,11 @@ namespace s11n { return _header[Index::ChannelCount]; } - void Reset(uint32_t channels, uint32_t channel_point_count) { + void Reset(uint32_t channel_point_count) { std::memset(_header.data() + Index::SIZE, 0, sizeof(uint32_t) * GetChannelCount()); - _ser_points.clear(); - _ser_points.reserve(channels * channel_point_count); - - _aux_points.resize(channels); + _max_channel_points = channel_point_count; + _aux_points.resize(GetChannelCount()); for (auto& aux : _aux_points) { aux.clear(); aux.reserve(channel_point_count); @@ -125,6 +123,7 @@ namespace s11n { void SaveDetections() { _ser_points.clear(); + _ser_points.reserve(GetChannelCount() * _max_channel_points); for (auto idxChannel = 0u; idxChannel < GetChannelCount(); ++idxChannel) { _header[Index::SIZE + idxChannel] = static_cast(_aux_points.size()); @@ -132,9 +131,12 @@ namespace s11n { } } - private: + protected: std::vector _header; std::vector> _aux_points; + uint32_t _max_channel_points; + + private: std::vector _ser_points; }; diff --git a/LibCarla/source/carla/sensor/s11n/LidarSerializer.h b/LibCarla/source/carla/sensor/s11n/LidarSerializer.h index 252963828..ce1efd1d2 100644 --- a/LibCarla/source/carla/sensor/s11n/LidarSerializer.h +++ b/LibCarla/source/carla/sensor/s11n/LidarSerializer.h @@ -9,7 +9,7 @@ #include "carla/Debug.h" #include "carla/Memory.h" #include "carla/sensor/RawData.h" -#include "carla/sensor/s11n/LidarMeasurement.h" +#include "carla/sensor/s11n/LidarData.h" namespace carla { namespace sensor { @@ -24,7 +24,7 @@ namespace s11n { /// A view over the header of a Lidar measurement. class LidarHeaderView { - using Index = LidarMeasurement::Index; + using Index = LidarData::Index; public: float GetHorizontalAngle() const { @@ -65,13 +65,13 @@ namespace s11n { static size_t GetHeaderOffset(const RawData &data) { auto View = DeserializeHeader(data); - return sizeof(uint32_t) * (View.GetChannelCount() + LidarMeasurement::Index::SIZE); + return sizeof(uint32_t) * (View.GetChannelCount() + LidarData::Index::SIZE); } template static Buffer Serialize( const Sensor &sensor, - const LidarMeasurement &measurement, + const LidarData &measurement, Buffer &&output); static SharedPtr Deserialize(RawData &&data); @@ -84,11 +84,11 @@ namespace s11n { template inline Buffer LidarSerializer::Serialize( const Sensor &, - const LidarMeasurement &measurement, + const LidarData &data, Buffer &&output) { std::array seq = { - boost::asio::buffer(measurement._header), - boost::asio::buffer(measurement._points)}; + boost::asio::buffer(data._header), + boost::asio::buffer(data._points)}; output.copy_from(seq); return std::move(output); } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp index 7e7b73392..e66770749 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp @@ -44,7 +44,7 @@ void ARayCastLidar::Set(const FActorDescription &ActorDescription) void ARayCastLidar::Set(const FLidarDescription &LidarDescription) { Description = LidarDescription; - LidarMeasurement = FLidarMeasurement(Description.Channels); + LidarData = FLidarData(Description.Channels); CreateLasers(); } @@ -76,7 +76,7 @@ void ARayCastLidar::Tick(const float DeltaTime) ReadPoints(DeltaTime); auto DataStream = GetDataStream(*this); - DataStream.Send(*this, LidarMeasurement, DataStream.PopBufferFromPool()); + DataStream.Send(*this, LidarData, DataStream.PopBufferFromPool()); } void ARayCastLidar::ReadPoints(const float DeltaTime) @@ -99,11 +99,11 @@ void ARayCastLidar::ReadPoints(const float DeltaTime) check(ChannelCount == LaserAngles.Num()); const float CurrentHorizontalAngle = carla::geom::Math::ToDegrees( - LidarMeasurement.GetHorizontalAngle()); + LidarData.GetHorizontalAngle()); const float AngleDistanceOfTick = Description.RotationFrequency * 360.0f * DeltaTime; const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser; - LidarMeasurement.Reset(ChannelCount, PointsToScanWithOneLaser); + LidarData.Reset(PointsToScanWithOneLaser); GetWorld()->GetPhysicsScene()->GetPxScene()->lockRead(); @@ -116,19 +116,19 @@ void ARayCastLidar::ReadPoints(const float DeltaTime) const float Angle = CurrentHorizontalAngle + AngleDistanceOfLaserMeasure * idxPtsOneLaser; if (ShootLaser(idxChannel, Angle, Point, Intensity)) { Mutex.Lock(); - LidarMeasurement.WritePointAsync(idxChannel, {Point, Intensity}); + LidarData.WritePointAsync(idxChannel, {Point, Intensity}); Mutex.Unlock(); } }); }); GetWorld()->GetPhysicsScene()->GetPxScene()->unlockRead(); - LidarMeasurement.SaveDetections(); + LidarData.SaveDetections(); const float HorizontalAngle = carla::geom::Math::ToRadians( std::fmod(CurrentHorizontalAngle + AngleDistanceOfTick, 360.0f)); - LidarMeasurement.SetHorizontalAngle(HorizontalAngle); + LidarData.SetHorizontalAngle(HorizontalAngle); } float ARayCastLidar::ComputeIntensity(const FVector &LidarBodyLoc, const FHitResult& HitInfo) const diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.h index 7a3a81c96..98daa2e5a 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.h @@ -12,7 +12,7 @@ #include "Carla/Sensor/LidarDescription.h" #include -#include +#include #include #include "RayCastLidar.generated.h" @@ -23,7 +23,7 @@ class CARLA_API ARayCastLidar : public ASensor { GENERATED_BODY() - using FLidarMeasurement = carla::sensor::s11n::LidarMeasurement; + using FLidarData = carla::sensor::s11n::LidarData; public: @@ -58,7 +58,7 @@ private: TArray LaserAngles; - FLidarMeasurement LidarMeasurement; + FLidarData LidarData; /// Enable/Disable general dropoff of lidar points bool DropOffGenActive; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastRawLidar.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastRawLidar.cpp index c38b7e96a..0f5ff3f48 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastRawLidar.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastRawLidar.cpp @@ -98,7 +98,7 @@ void ARayCastRawLidar::ReadPoints(const float DeltaTime) const float AngleDistanceOfTick = Description.RotationFrequency * 360.0f * DeltaTime; const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser; - LidarData.Reset(ChannelCount, PointsToScanWithOneLaser); + LidarData.Reset(PointsToScanWithOneLaser); GetWorld()->GetPhysicsScene()->GetPxScene()->lockRead(); ParallelFor(ChannelCount, [&](int32 idxChannel) {