Port lidar to new sensor system
This commit is contained in:
parent
c84eb298e7
commit
11167181f2
|
@ -15,9 +15,11 @@
|
||||||
|
|
||||||
// 1. Include the serializer here.
|
// 1. Include the serializer here.
|
||||||
#include "carla/sensor/s11n/ImageSerializer.h"
|
#include "carla/sensor/s11n/ImageSerializer.h"
|
||||||
|
#include "carla/sensor/s11n/LidarSerializer.h"
|
||||||
|
|
||||||
// 2. Add a forward-declaration of the sensor here.
|
// 2. Add a forward-declaration of the sensor here.
|
||||||
class ADepthCamera;
|
class ADepthCamera;
|
||||||
|
class ARayCastLidar;
|
||||||
class ASceneCaptureCamera;
|
class ASceneCaptureCamera;
|
||||||
class ASemanticSegmentationCamera;
|
class ASemanticSegmentationCamera;
|
||||||
|
|
||||||
|
@ -30,7 +32,8 @@ namespace sensor {
|
||||||
using SensorRegistry = CompositeSerializer<
|
using SensorRegistry = CompositeSerializer<
|
||||||
std::pair<ASceneCaptureCamera *, s11n::ImageSerializer>,
|
std::pair<ASceneCaptureCamera *, s11n::ImageSerializer>,
|
||||||
std::pair<ADepthCamera *, s11n::ImageSerializer>,
|
std::pair<ADepthCamera *, s11n::ImageSerializer>,
|
||||||
std::pair<ASemanticSegmentationCamera *, s11n::ImageSerializer>
|
std::pair<ASemanticSegmentationCamera *, s11n::ImageSerializer>,
|
||||||
|
std::pair<ARayCastLidar *, s11n::LidarSerializer>
|
||||||
>;
|
>;
|
||||||
|
|
||||||
} // namespace sensor
|
} // namespace sensor
|
||||||
|
@ -42,6 +45,7 @@ namespace sensor {
|
||||||
|
|
||||||
// 4. Include the sensor here.
|
// 4. Include the sensor here.
|
||||||
#include "Carla/Sensor/DepthCamera.h"
|
#include "Carla/Sensor/DepthCamera.h"
|
||||||
|
#include "Carla/Sensor/RayCastLidar.h"
|
||||||
#include "Carla/Sensor/SceneCaptureCamera.h"
|
#include "Carla/Sensor/SceneCaptureCamera.h"
|
||||||
#include "Carla/Sensor/SemanticSegmentationCamera.h"
|
#include "Carla/Sensor/SemanticSegmentationCamera.h"
|
||||||
|
|
||||||
|
|
|
@ -16,7 +16,7 @@ namespace carla {
|
||||||
namespace sensor {
|
namespace sensor {
|
||||||
namespace data {
|
namespace data {
|
||||||
|
|
||||||
template <size_t Offset, typename T>
|
template <typename T>
|
||||||
class Array : public SensorData {
|
class Array : public SensorData {
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
@ -28,11 +28,11 @@ namespace data {
|
||||||
using reference = typename std::iterator_traits<iterator>::reference;
|
using reference = typename std::iterator_traits<iterator>::reference;
|
||||||
|
|
||||||
iterator begin() {
|
iterator begin() {
|
||||||
return reinterpret_cast<iterator>(_message.begin() + Offset);
|
return reinterpret_cast<iterator>(_message.begin() + _offset);
|
||||||
}
|
}
|
||||||
|
|
||||||
const_iterator cbegin() const {
|
const_iterator cbegin() const {
|
||||||
return reinterpret_cast<const_iterator>(_message.begin() + Offset);
|
return reinterpret_cast<const_iterator>(_message.begin() + _offset);
|
||||||
}
|
}
|
||||||
|
|
||||||
const_iterator begin() const {
|
const_iterator begin() const {
|
||||||
|
@ -77,11 +77,19 @@ namespace data {
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
explicit Array(DataMessage message)
|
explicit Array(size_t offset, DataMessage message)
|
||||||
: SensorData(message),
|
: SensorData(message),
|
||||||
_message(std::move(message)) {
|
_message(std::move(message)) {
|
||||||
DEBUG_ASSERT(_message.size() >= Offset);
|
SetOffset(offset);
|
||||||
DEBUG_ASSERT((_message.size() - Offset) % sizeof(T) == 0u);
|
}
|
||||||
|
|
||||||
|
explicit Array(DataMessage message)
|
||||||
|
: Array(0u, std::move(message)) {}
|
||||||
|
|
||||||
|
void SetOffset(size_t offset) {
|
||||||
|
DEBUG_ASSERT(_message.size() >= _offset);
|
||||||
|
DEBUG_ASSERT((_message.size() - _offset) % sizeof(T) == 0u);
|
||||||
|
_offset = offset;
|
||||||
}
|
}
|
||||||
|
|
||||||
const DataMessage &GetMessage() const {
|
const DataMessage &GetMessage() const {
|
||||||
|
@ -90,6 +98,8 @@ namespace data {
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
size_t _offset;
|
||||||
|
|
||||||
DataMessage _message;
|
DataMessage _message;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -15,8 +15,8 @@ namespace sensor {
|
||||||
namespace data {
|
namespace data {
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
class ImageTmpl : public Array<s11n::ImageSerializer::header_offset, T> {
|
class ImageTmpl : public Array<T> {
|
||||||
using Super = Array<s11n::ImageSerializer::header_offset, T>;
|
using Super = Array<T>;
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
using Serializer = s11n::ImageSerializer;
|
using Serializer = s11n::ImageSerializer;
|
||||||
|
@ -24,28 +24,28 @@ namespace data {
|
||||||
friend Serializer;
|
friend Serializer;
|
||||||
|
|
||||||
explicit ImageTmpl(DataMessage message)
|
explicit ImageTmpl(DataMessage message)
|
||||||
: Super(std::move(message)) {
|
: Super(Serializer::header_offset, std::move(message)) {
|
||||||
DEBUG_ASSERT(GetWidth() * GetHeight() == Super::size());
|
DEBUG_ASSERT(GetWidth() * GetHeight() == Super::size());
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
const auto &GetHeader() const {
|
const auto &GetHeader() const {
|
||||||
return Serializer::DeserializeHeader(Super::GetMessage());
|
return Serializer::DeserializeHeader(Super::GetMessage());
|
||||||
}
|
}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
auto GetWidth() const {
|
auto GetWidth() const {
|
||||||
return GetHeader().width;
|
return GetHeader().width;
|
||||||
}
|
}
|
||||||
|
|
||||||
auto GetHeight() const {
|
auto GetHeight() const {
|
||||||
return GetHeader().height;
|
return GetHeader().height;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t GetFOVAngle() const {
|
auto GetFOVAngle() const {
|
||||||
return GetHeader().fov_angle;
|
return GetHeader().fov_angle;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1,55 @@
|
||||||
|
// 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/Debug.h"
|
||||||
|
#include "carla/rpc/Location.h"
|
||||||
|
#include "carla/sensor/data/Array.h"
|
||||||
|
#include "carla/sensor/s11n/LidarSerializer.h"
|
||||||
|
|
||||||
|
namespace carla {
|
||||||
|
namespace sensor {
|
||||||
|
namespace data {
|
||||||
|
|
||||||
|
class LidarMeasurement : public Array<rpc::Location> {
|
||||||
|
static_assert(sizeof(rpc::Location) == 3u * sizeof(float), "Location size missmatch");
|
||||||
|
using Super = Array<rpc::Location>;
|
||||||
|
protected:
|
||||||
|
|
||||||
|
using Serializer = s11n::LidarSerializer;
|
||||||
|
|
||||||
|
friend Serializer;
|
||||||
|
|
||||||
|
explicit LidarMeasurement(DataMessage message)
|
||||||
|
: Super(std::move(message)) {
|
||||||
|
Super::SetOffset(Serializer::GetHeaderOffset(Super::GetMessage()));
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
auto GetHeader() const {
|
||||||
|
return Serializer::DeserializeHeader(Super::GetMessage());
|
||||||
|
}
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
auto GetHorizontalAngle() const {
|
||||||
|
return GetHeader().GetHorizontalAngle();
|
||||||
|
}
|
||||||
|
|
||||||
|
auto GetChannelCount() const {
|
||||||
|
return GetHeader().GetChannelCount();
|
||||||
|
}
|
||||||
|
|
||||||
|
auto GetPointCount(size_t channel) const {
|
||||||
|
return GetHeader().GetPointCount(channel);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace data
|
||||||
|
} // namespace sensor
|
||||||
|
} // namespace carla
|
|
@ -0,0 +1,98 @@
|
||||||
|
// 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/rpc/Location.h"
|
||||||
|
|
||||||
|
#include <cstdint>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
namespace carla {
|
||||||
|
namespace sensor {
|
||||||
|
namespace s11n {
|
||||||
|
|
||||||
|
/// Helper class to store and serialize the data generated by a Lidar.
|
||||||
|
///
|
||||||
|
/// The header of a Lidar measurement consists of an array of uint32_t's in
|
||||||
|
/// the following layout
|
||||||
|
///
|
||||||
|
/// {
|
||||||
|
/// Horizontal angle (float),
|
||||||
|
/// Channel count,
|
||||||
|
/// Point count of channel 0,
|
||||||
|
/// ...
|
||||||
|
/// Point count of channel n,
|
||||||
|
/// }
|
||||||
|
///
|
||||||
|
/// The points are stored in an array of floats
|
||||||
|
///
|
||||||
|
/// {
|
||||||
|
/// X0, Y0, Z0,
|
||||||
|
/// ...
|
||||||
|
/// Xn, Yn, Zn,
|
||||||
|
/// }
|
||||||
|
///
|
||||||
|
/// @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 LidarMeasurement {
|
||||||
|
static_assert(sizeof(float) == sizeof(uint32_t), "Invalid float size");
|
||||||
|
|
||||||
|
friend class LidarSerializer;
|
||||||
|
friend class LidarHeaderView;
|
||||||
|
|
||||||
|
enum Index : size_t {
|
||||||
|
HorizontalAngle,
|
||||||
|
ChannelCount,
|
||||||
|
SIZE
|
||||||
|
};
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
explicit LidarMeasurement(uint32_t ChannelCount = 0u)
|
||||||
|
: _header(Index::SIZE + ChannelCount, 0u) {
|
||||||
|
_header[Index::ChannelCount] = ChannelCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
LidarMeasurement &operator=(LidarMeasurement &&) = default;
|
||||||
|
|
||||||
|
float GetHorizontalAngle() const {
|
||||||
|
return reinterpret_cast<const float &>(_header[Index::HorizontalAngle]);
|
||||||
|
}
|
||||||
|
|
||||||
|
void SetHorizontalAngle(float angle) {
|
||||||
|
_header[Index::HorizontalAngle] = reinterpret_cast<const uint32_t &>(angle);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t GetChannelCount() const {
|
||||||
|
return _header[Index::ChannelCount];
|
||||||
|
}
|
||||||
|
|
||||||
|
void Reset(uint32_t total_point_count) {
|
||||||
|
std::memset(_header.data() + Index::SIZE, 0, sizeof(uint32_t) * GetChannelCount());
|
||||||
|
_points.clear();
|
||||||
|
_points.reserve(3u * total_point_count);
|
||||||
|
}
|
||||||
|
|
||||||
|
void WritePoint(uint32_t channel, rpc::Location point) {
|
||||||
|
DEBUG_ASSERT(GetChannelCount() > channel);
|
||||||
|
_header[Index::SIZE + channel] += 1u;
|
||||||
|
_points.emplace_back(point.x);
|
||||||
|
_points.emplace_back(point.y);
|
||||||
|
_points.emplace_back(point.z);
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
std::vector<uint32_t> _header;
|
||||||
|
|
||||||
|
std::vector<float> _points;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace s11n
|
||||||
|
} // namespace sensor
|
||||||
|
} // namespace carla
|
|
@ -0,0 +1,22 @@
|
||||||
|
// 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/sensor/s11n/LidarSerializer.h"
|
||||||
|
|
||||||
|
#include "carla/sensor/data/LidarMeasurement.h"
|
||||||
|
|
||||||
|
namespace carla {
|
||||||
|
namespace sensor {
|
||||||
|
namespace s11n {
|
||||||
|
|
||||||
|
SharedPtr<SensorData> LidarSerializer::Deserialize(DataMessage message) {
|
||||||
|
return SharedPtr<data::LidarMeasurement>(
|
||||||
|
new data::LidarMeasurement{std::move(message)});
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace s11n
|
||||||
|
} // namespace sensor
|
||||||
|
} // namespace carla
|
|
@ -0,0 +1,96 @@
|
||||||
|
// 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/Debug.h"
|
||||||
|
#include "carla/Memory.h"
|
||||||
|
#include "carla/sensor/DataMessage.h"
|
||||||
|
#include "carla/sensor/s11n/LidarMeasurement.h"
|
||||||
|
|
||||||
|
namespace carla {
|
||||||
|
namespace sensor {
|
||||||
|
|
||||||
|
class SensorData;
|
||||||
|
|
||||||
|
namespace s11n {
|
||||||
|
|
||||||
|
// ===========================================================================
|
||||||
|
// -- LidarHeaderView --------------------------------------------------------
|
||||||
|
// ===========================================================================
|
||||||
|
|
||||||
|
class LidarHeaderView {
|
||||||
|
using Index = LidarMeasurement::Index;
|
||||||
|
public:
|
||||||
|
|
||||||
|
float GetHorizontalAngle() const {
|
||||||
|
return reinterpret_cast<const float &>(_begin[Index::HorizontalAngle]);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t GetChannelCount() const {
|
||||||
|
return _begin[Index::ChannelCount];
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t GetPointCount(size_t channel) const {
|
||||||
|
DEBUG_ASSERT(channel < GetChannelCount());
|
||||||
|
return _begin[Index::SIZE + channel];
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
friend class LidarSerializer;
|
||||||
|
|
||||||
|
explicit LidarHeaderView(const uint32_t *begin) : _begin(begin) {
|
||||||
|
DEBUG_ASSERT(_begin != nullptr);
|
||||||
|
}
|
||||||
|
|
||||||
|
const uint32_t *_begin;
|
||||||
|
};
|
||||||
|
|
||||||
|
// ===========================================================================
|
||||||
|
// -- LidarSerializer --------------------------------------------------------
|
||||||
|
// ===========================================================================
|
||||||
|
|
||||||
|
class LidarSerializer {
|
||||||
|
public:
|
||||||
|
|
||||||
|
static LidarHeaderView DeserializeHeader(const DataMessage &message) {
|
||||||
|
return LidarHeaderView{reinterpret_cast<const uint32_t *>(message.begin())};
|
||||||
|
}
|
||||||
|
|
||||||
|
static size_t GetHeaderOffset(const DataMessage &message) {
|
||||||
|
auto View = DeserializeHeader(message);
|
||||||
|
return sizeof(uint32_t) * (View.GetChannelCount() + LidarMeasurement::Index::SIZE);
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename Sensor>
|
||||||
|
static Buffer Serialize(
|
||||||
|
const Sensor &sensor,
|
||||||
|
const LidarMeasurement &measurement,
|
||||||
|
Buffer bitmap);
|
||||||
|
|
||||||
|
static SharedPtr<SensorData> Deserialize(DataMessage message);
|
||||||
|
};
|
||||||
|
|
||||||
|
// ===========================================================================
|
||||||
|
// -- LidarSerializer implementation -----------------------------------------
|
||||||
|
// ===========================================================================
|
||||||
|
|
||||||
|
template <typename Sensor>
|
||||||
|
inline Buffer LidarSerializer::Serialize(
|
||||||
|
const Sensor &,
|
||||||
|
const LidarMeasurement &measurement,
|
||||||
|
Buffer output) {
|
||||||
|
std::array<boost::asio::const_buffer, 2u> seq = {
|
||||||
|
boost::asio::buffer(measurement._header),
|
||||||
|
boost::asio::buffer(measurement._points)};
|
||||||
|
output.copy_from(seq);
|
||||||
|
return output;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace s11n
|
||||||
|
} // namespace sensor
|
||||||
|
} // namespace carla
|
|
@ -7,6 +7,7 @@
|
||||||
#include "Carla.h"
|
#include "Carla.h"
|
||||||
#include "Carla/Actor/ActorBlueprintFunctionLibrary.h"
|
#include "Carla/Actor/ActorBlueprintFunctionLibrary.h"
|
||||||
|
|
||||||
|
#include "Carla/Sensor/LidarDescription.h"
|
||||||
#include "Carla/Sensor/SceneCaptureSensor.h"
|
#include "Carla/Sensor/SceneCaptureSensor.h"
|
||||||
#include "Carla/Util/ScopedStack.h"
|
#include "Carla/Util/ScopedStack.h"
|
||||||
|
|
||||||
|
@ -186,6 +187,13 @@ bool UActorBlueprintFunctionLibrary::CheckActorDefinitions(const TArray<FActorDe
|
||||||
/// -- Helpers to create actor definitions -------------------------------------
|
/// -- Helpers to create actor definitions -------------------------------------
|
||||||
/// ============================================================================
|
/// ============================================================================
|
||||||
|
|
||||||
|
template <typename... TStrs>
|
||||||
|
static void FillIdAndTags(FActorDefinition &Def, TStrs &&... Strings)
|
||||||
|
{
|
||||||
|
Def.Id = JoinStrings(TEXT("."), std::forward<TStrs>(Strings)...).ToLower();
|
||||||
|
Def.Tags = JoinStrings(TEXT(","), std::forward<TStrs>(Strings)...).ToLower();
|
||||||
|
}
|
||||||
|
|
||||||
FActorDefinition UActorBlueprintFunctionLibrary::MakeCameraDefinition(
|
FActorDefinition UActorBlueprintFunctionLibrary::MakeCameraDefinition(
|
||||||
const FString &Id,
|
const FString &Id,
|
||||||
const bool bEnableModifyingPostProcessEffects)
|
const bool bEnableModifyingPostProcessEffects)
|
||||||
|
@ -203,8 +211,7 @@ void UActorBlueprintFunctionLibrary::MakeCameraDefinition(
|
||||||
bool &Success,
|
bool &Success,
|
||||||
FActorDefinition &Definition)
|
FActorDefinition &Definition)
|
||||||
{
|
{
|
||||||
Definition.Id = JoinStrings(TEXT("."), TEXT("sensor"), Id).ToLower();
|
FillIdAndTags(Definition, TEXT("sensor"), TEXT("camera"), Id);
|
||||||
Definition.Tags = JoinStrings(TEXT(","), TEXT("sensor"), Id).ToLower();
|
|
||||||
// FOV.
|
// FOV.
|
||||||
FActorVariation FOV;
|
FActorVariation FOV;
|
||||||
FOV.Id = TEXT("fov");
|
FOV.Id = TEXT("fov");
|
||||||
|
@ -238,15 +245,66 @@ void UActorBlueprintFunctionLibrary::MakeCameraDefinition(
|
||||||
Success = CheckActorDefinition(Definition);
|
Success = CheckActorDefinition(Definition);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
FActorDefinition UActorBlueprintFunctionLibrary::MakeLidarDefinition(
|
||||||
|
const FString &Id)
|
||||||
|
{
|
||||||
|
FActorDefinition Definition;
|
||||||
|
bool Success;
|
||||||
|
MakeLidarDefinition(Id, Success, Definition);
|
||||||
|
check(Success);
|
||||||
|
return Definition;
|
||||||
|
}
|
||||||
|
|
||||||
|
void UActorBlueprintFunctionLibrary::MakeLidarDefinition(
|
||||||
|
const FString &Id,
|
||||||
|
bool &Success,
|
||||||
|
FActorDefinition &Definition)
|
||||||
|
{
|
||||||
|
FillIdAndTags(Definition, TEXT("sensor"), TEXT("lidar"), Id);
|
||||||
|
// Number of channels.
|
||||||
|
FActorVariation Channels;
|
||||||
|
Channels.Id = TEXT("channels");
|
||||||
|
Channels.Type = EActorAttributeType::Int;
|
||||||
|
Channels.RecommendedValues = { TEXT("32") };
|
||||||
|
// Range.
|
||||||
|
FActorVariation Range;
|
||||||
|
Range.Id = TEXT("range");
|
||||||
|
Range.Type = EActorAttributeType::Float;
|
||||||
|
Range.RecommendedValues = { TEXT("5000.0") };
|
||||||
|
// Points per second.
|
||||||
|
FActorVariation PointsPerSecond;
|
||||||
|
PointsPerSecond.Id = TEXT("points_per_second");
|
||||||
|
PointsPerSecond.Type = EActorAttributeType::Int;
|
||||||
|
PointsPerSecond.RecommendedValues = { TEXT("56000") };
|
||||||
|
// Frequency.
|
||||||
|
FActorVariation Frequency;
|
||||||
|
Frequency.Id = TEXT("rotation_frequency");
|
||||||
|
Frequency.Type = EActorAttributeType::Float;
|
||||||
|
Frequency.RecommendedValues = { TEXT("10.0") };
|
||||||
|
// Upper FOV limit.
|
||||||
|
FActorVariation UpperFOV;
|
||||||
|
UpperFOV.Id = TEXT("upper_fov");
|
||||||
|
UpperFOV.Type = EActorAttributeType::Float;
|
||||||
|
UpperFOV.RecommendedValues = { TEXT("10.0") };
|
||||||
|
// Lower FOV limit.
|
||||||
|
FActorVariation LowerFOV;
|
||||||
|
LowerFOV.Id = TEXT("lower_fov");
|
||||||
|
LowerFOV.Type = EActorAttributeType::Float;
|
||||||
|
LowerFOV.RecommendedValues = { TEXT("-30.0") };
|
||||||
|
|
||||||
|
Definition.Variations = {Channels, Range, PointsPerSecond, Frequency, UpperFOV, LowerFOV};
|
||||||
|
|
||||||
|
Success = CheckActorDefinition(Definition);
|
||||||
|
}
|
||||||
|
|
||||||
void UActorBlueprintFunctionLibrary::MakeVehicleDefinition(
|
void UActorBlueprintFunctionLibrary::MakeVehicleDefinition(
|
||||||
const FVehicleParameters &Parameters,
|
const FVehicleParameters &Parameters,
|
||||||
bool &Success,
|
bool &Success,
|
||||||
FActorDefinition &Definition)
|
FActorDefinition &Definition)
|
||||||
{
|
{
|
||||||
/// @todo We need to validate here the params.
|
/// @todo We need to validate here the params.
|
||||||
Definition.Id = JoinStrings(TEXT("."), Parameters.Make, Parameters.Model).ToLower();
|
FillIdAndTags(Definition, TEXT("vehicle"), Parameters.Make, Parameters.Model);
|
||||||
Definition.Class = Parameters.Class;
|
Definition.Class = Parameters.Class;
|
||||||
Definition.Tags = JoinStrings(TEXT(","), TEXT("vehicle"), Parameters.Make, Parameters.Model).ToLower();
|
|
||||||
FActorVariation Colors;
|
FActorVariation Colors;
|
||||||
Colors.Id = TEXT("color");
|
Colors.Id = TEXT("color");
|
||||||
Colors.Type = EActorAttributeType::RGBColor;
|
Colors.Type = EActorAttributeType::RGBColor;
|
||||||
|
@ -443,7 +501,7 @@ FColor UActorBlueprintFunctionLibrary::RetrieveActorAttributeToColor(
|
||||||
check((ActorPtr != nullptr) && !ActorPtr->IsPendingKill());
|
check((ActorPtr != nullptr) && !ActorPtr->IsPendingKill());
|
||||||
#endif // WITH_EDITOR
|
#endif // WITH_EDITOR
|
||||||
|
|
||||||
void UActorBlueprintFunctionLibrary::SetActor(
|
void UActorBlueprintFunctionLibrary::SetCamera(
|
||||||
const FActorDescription &Description,
|
const FActorDescription &Description,
|
||||||
ASceneCaptureSensor *Camera)
|
ASceneCaptureSensor *Camera)
|
||||||
{
|
{
|
||||||
|
@ -462,4 +520,22 @@ void UActorBlueprintFunctionLibrary::SetActor(
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void UActorBlueprintFunctionLibrary::SetLidar(
|
||||||
|
const FActorDescription &Description,
|
||||||
|
FLidarDescription &Lidar)
|
||||||
|
{
|
||||||
|
Lidar.Channels =
|
||||||
|
RetrieveActorAttributeToInt("channels", Description.Variations, Lidar.Channels);
|
||||||
|
Lidar.Range =
|
||||||
|
RetrieveActorAttributeToFloat("range", Description.Variations, Lidar.Range);
|
||||||
|
Lidar.PointsPerSecond =
|
||||||
|
RetrieveActorAttributeToInt("points_per_second", Description.Variations, Lidar.PointsPerSecond);
|
||||||
|
Lidar.RotationFrequency =
|
||||||
|
RetrieveActorAttributeToFloat("rotation_frequency", Description.Variations, Lidar.RotationFrequency);
|
||||||
|
Lidar.UpperFovLimit =
|
||||||
|
RetrieveActorAttributeToFloat("upper_fov", Description.Variations, Lidar.UpperFovLimit);
|
||||||
|
Lidar.LowerFovLimit =
|
||||||
|
RetrieveActorAttributeToFloat("lower_fov", Description.Variations, Lidar.LowerFovLimit);
|
||||||
|
}
|
||||||
|
|
||||||
#undef CARLA_ABFL_CHECK_ACTOR
|
#undef CARLA_ABFL_CHECK_ACTOR
|
||||||
|
|
|
@ -15,6 +15,7 @@
|
||||||
#include "ActorBlueprintFunctionLibrary.generated.h"
|
#include "ActorBlueprintFunctionLibrary.generated.h"
|
||||||
|
|
||||||
class ASceneCaptureSensor;
|
class ASceneCaptureSensor;
|
||||||
|
struct FLidarDescription;
|
||||||
|
|
||||||
USTRUCT(BlueprintType)
|
USTRUCT(BlueprintType)
|
||||||
struct CARLA_API FVehicleParameters
|
struct CARLA_API FVehicleParameters
|
||||||
|
@ -75,6 +76,15 @@ public:
|
||||||
bool &Success,
|
bool &Success,
|
||||||
FActorDefinition &Definition);
|
FActorDefinition &Definition);
|
||||||
|
|
||||||
|
static FActorDefinition MakeLidarDefinition(
|
||||||
|
const FString &Id);
|
||||||
|
|
||||||
|
UFUNCTION(Category = "Carla Actor", BlueprintCallable)
|
||||||
|
static void MakeLidarDefinition(
|
||||||
|
const FString &Id,
|
||||||
|
bool &Success,
|
||||||
|
FActorDefinition &Definition);
|
||||||
|
|
||||||
UFUNCTION(Category = "Carla Actor", BlueprintCallable)
|
UFUNCTION(Category = "Carla Actor", BlueprintCallable)
|
||||||
static void MakeVehicleDefinition(
|
static void MakeVehicleDefinition(
|
||||||
const FVehicleParameters &Parameters,
|
const FVehicleParameters &Parameters,
|
||||||
|
@ -144,7 +154,9 @@ public:
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
UFUNCTION(Category = "Carla Actor", BlueprintCallable)
|
UFUNCTION(Category = "Carla Actor", BlueprintCallable)
|
||||||
static void SetActor(const FActorDescription &Description, ASceneCaptureSensor *Camera);
|
static void SetCamera(const FActorDescription &Description, ASceneCaptureSensor *Camera);
|
||||||
|
|
||||||
|
static void SetLidar(const FActorDescription &Description, FLidarDescription &Lidar);
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
};
|
};
|
||||||
|
|
|
@ -1,101 +0,0 @@
|
||||||
// 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 <compiler/disable-ue4-macros.h>
|
|
||||||
#include <carla/Buffer.h>
|
|
||||||
#include <compiler/enable-ue4-macros.h>
|
|
||||||
|
|
||||||
#include "Containers/Array.h"
|
|
||||||
|
|
||||||
/// Stores the data generated by ALidar. To be used by ALidar solely.
|
|
||||||
///
|
|
||||||
/// The header consists of an array of uint32's in the following layout
|
|
||||||
///
|
|
||||||
/// {
|
|
||||||
/// Horizontal angle (float),
|
|
||||||
/// Channel count,
|
|
||||||
/// Point count of channel 0,
|
|
||||||
/// ...
|
|
||||||
/// Point count of channel n,
|
|
||||||
/// }
|
|
||||||
///
|
|
||||||
/// The points are stored in an array of floats
|
|
||||||
///
|
|
||||||
/// {
|
|
||||||
/// X0, Y0, Z0,
|
|
||||||
/// ...
|
|
||||||
/// Xn, Yn, Zn,
|
|
||||||
/// }
|
|
||||||
///
|
|
||||||
class FLidarMeasurement {
|
|
||||||
static_assert(sizeof(float) == sizeof(uint32), "Invalid float size");
|
|
||||||
public:
|
|
||||||
|
|
||||||
explicit FLidarMeasurement(uint32 ChannelCount = 0u)
|
|
||||||
{
|
|
||||||
Header.AddDefaulted(2u + ChannelCount);
|
|
||||||
Header[1] = ChannelCount;
|
|
||||||
}
|
|
||||||
|
|
||||||
FLidarMeasurement &operator=(FLidarMeasurement &&Other)
|
|
||||||
{
|
|
||||||
Header = std::move(Other.Header);
|
|
||||||
Points = std::move(Other.Points);
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
float GetHorizontalAngle() const
|
|
||||||
{
|
|
||||||
return reinterpret_cast<const float &>(Header[0]);
|
|
||||||
}
|
|
||||||
|
|
||||||
void SetHorizontalAngle(float HorizontalAngle)
|
|
||||||
{
|
|
||||||
Header[0] = reinterpret_cast<const uint32 &>(HorizontalAngle);
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32 GetChannelCount() const
|
|
||||||
{
|
|
||||||
return Header[1];
|
|
||||||
}
|
|
||||||
|
|
||||||
void Reset(uint32 TotalPointCount)
|
|
||||||
{
|
|
||||||
std::memset(Header.GetData() + 2u, 0, sizeof(uint32) * GetChannelCount());
|
|
||||||
Points.Reset(3u * TotalPointCount);
|
|
||||||
}
|
|
||||||
|
|
||||||
void WritePoint(uint32 Channel, const FVector &Point)
|
|
||||||
{
|
|
||||||
check(GetChannelCount() > Channel);
|
|
||||||
Header[2u + Channel] += 1u;
|
|
||||||
constexpr float TO_METERS = 1e-2f;
|
|
||||||
Points.Emplace(TO_METERS * Point.X);
|
|
||||||
Points.Emplace(TO_METERS * Point.Y);
|
|
||||||
Points.Emplace(TO_METERS * Point.Z);
|
|
||||||
}
|
|
||||||
|
|
||||||
void CopyToBuffer(carla::Buffer &Buffer) const
|
|
||||||
{
|
|
||||||
/// @todo This should be moved to its own serializer.
|
|
||||||
std::array<boost::asio::const_buffer, 2u> BufSequence = {{
|
|
||||||
boost::asio::buffer(
|
|
||||||
reinterpret_cast<const unsigned char *>(Header.GetData()),
|
|
||||||
sizeof(uint32) * Header.Num()),
|
|
||||||
boost::asio::buffer(
|
|
||||||
reinterpret_cast<const unsigned char *>(Points.GetData()),
|
|
||||||
sizeof(float) * Points.Num())}};
|
|
||||||
Buffer.copy_from(BufSequence);
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
TArray<uint32> Header;
|
|
||||||
|
|
||||||
TArray<float> Points;
|
|
||||||
};
|
|
|
@ -5,14 +5,21 @@
|
||||||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||||
|
|
||||||
#include "Carla.h"
|
#include "Carla.h"
|
||||||
#include "Carla/Sensor/RayTraceLidar.h"
|
#include "Carla/Sensor/RayCastLidar.h"
|
||||||
|
|
||||||
|
#include "Carla/Actor/ActorBlueprintFunctionLibrary.h"
|
||||||
|
|
||||||
#include "DrawDebugHelpers.h"
|
#include "DrawDebugHelpers.h"
|
||||||
#include "Engine/CollisionProfile.h"
|
#include "Engine/CollisionProfile.h"
|
||||||
#include "Runtime/Engine/Classes/Kismet/KismetMathLibrary.h"
|
#include "Runtime/Engine/Classes/Kismet/KismetMathLibrary.h"
|
||||||
#include "StaticMeshResources.h"
|
#include "StaticMeshResources.h"
|
||||||
|
|
||||||
ARayTraceLidar::ARayTraceLidar(const FObjectInitializer& ObjectInitializer)
|
FActorDefinition ARayCastLidar::GetSensorDefinition()
|
||||||
|
{
|
||||||
|
return UActorBlueprintFunctionLibrary::MakeLidarDefinition(TEXT("ray_cast"));
|
||||||
|
}
|
||||||
|
|
||||||
|
ARayCastLidar::ARayCastLidar(const FObjectInitializer& ObjectInitializer)
|
||||||
: Super(ObjectInitializer)
|
: Super(ObjectInitializer)
|
||||||
{
|
{
|
||||||
PrimaryActorTick.bCanEverTick = true;
|
PrimaryActorTick.bCanEverTick = true;
|
||||||
|
@ -25,14 +32,22 @@ ARayTraceLidar::ARayTraceLidar(const FObjectInitializer& ObjectInitializer)
|
||||||
RootComponent = MeshComp;
|
RootComponent = MeshComp;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ARayTraceLidar::SetLidar(const FLidarDescription &LidarDescription)
|
void ARayCastLidar::Set(const FActorDescription &ActorDescription)
|
||||||
|
{
|
||||||
|
Super::Set(ActorDescription);
|
||||||
|
FLidarDescription LidarDescription;
|
||||||
|
UActorBlueprintFunctionLibrary::SetLidar(ActorDescription, LidarDescription);
|
||||||
|
Set(LidarDescription);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ARayCastLidar::Set(const FLidarDescription &LidarDescription)
|
||||||
{
|
{
|
||||||
Description = LidarDescription;
|
Description = LidarDescription;
|
||||||
LidarMeasurement = FLidarMeasurement(Description.Channels);
|
LidarMeasurement = FLidarMeasurement(Description.Channels);
|
||||||
CreateLasers();
|
CreateLasers();
|
||||||
}
|
}
|
||||||
|
|
||||||
void ARayTraceLidar::CreateLasers()
|
void ARayCastLidar::CreateLasers()
|
||||||
{
|
{
|
||||||
const auto NumberOfLasers = Description.Channels;
|
const auto NumberOfLasers = Description.Channels;
|
||||||
check(NumberOfLasers > 0u);
|
check(NumberOfLasers > 0u);
|
||||||
|
@ -48,20 +63,17 @@ void ARayTraceLidar::CreateLasers()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ARayTraceLidar::Tick(const float DeltaTime)
|
void ARayCastLidar::Tick(const float DeltaTime)
|
||||||
{
|
{
|
||||||
Super::Tick(DeltaTime);
|
Super::Tick(DeltaTime);
|
||||||
|
|
||||||
ReadPoints(DeltaTime);
|
ReadPoints(DeltaTime);
|
||||||
|
|
||||||
/// @todo Here we send the data.
|
auto &Stream = GetDataStream();
|
||||||
// auto &Stream = GetDataStream();
|
Stream.Send_GameThread(*this, LidarMeasurement, Stream.PopBufferFromPool());
|
||||||
// auto Buffer = Stream.PopBufferFromPool();
|
|
||||||
// LidarMeasurement.CopyToBuffer(Buffer);
|
|
||||||
// Stream.Send_GameThread(*this, std::move(Buffer));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ARayTraceLidar::ReadPoints(const float DeltaTime)
|
void ARayCastLidar::ReadPoints(const float DeltaTime)
|
||||||
{
|
{
|
||||||
const uint32 ChannelCount = Description.Channels;
|
const uint32 ChannelCount = Description.Channels;
|
||||||
const uint32 PointsToScanWithOneLaser =
|
const uint32 PointsToScanWithOneLaser =
|
||||||
|
@ -103,7 +115,7 @@ void ARayTraceLidar::ReadPoints(const float DeltaTime)
|
||||||
LidarMeasurement.SetHorizontalAngle(HorizontalAngle);
|
LidarMeasurement.SetHorizontalAngle(HorizontalAngle);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ARayTraceLidar::ShootLaser(const uint32 Channel, const float HorizontalAngle, FVector &XYZ) const
|
bool ARayCastLidar::ShootLaser(const uint32 Channel, const float HorizontalAngle, FVector &XYZ) const
|
||||||
{
|
{
|
||||||
const float VerticalAngle = LaserAngles[Channel];
|
const float VerticalAngle = LaserAngles[Channel];
|
||||||
|
|
|
@ -9,21 +9,30 @@
|
||||||
#include "Carla/Sensor/Sensor.h"
|
#include "Carla/Sensor/Sensor.h"
|
||||||
|
|
||||||
#include "Carla/Sensor/LidarDescription.h"
|
#include "Carla/Sensor/LidarDescription.h"
|
||||||
#include "Carla/Sensor/LidarMeasurement.h"
|
|
||||||
|
|
||||||
#include "RayTraceLidar.generated.h"
|
#include <compiler/disable-ue4-macros.h>
|
||||||
|
#include <carla/sensor/s11n/LidarMeasurement.h>
|
||||||
|
#include <compiler/enable-ue4-macros.h>
|
||||||
|
|
||||||
/// A ray-trace based Lidar sensor.
|
#include "RayCastLidar.generated.h"
|
||||||
|
|
||||||
|
/// A ray-cast based Lidar sensor.
|
||||||
UCLASS()
|
UCLASS()
|
||||||
class CARLA_API ARayTraceLidar : public ASensor
|
class CARLA_API ARayCastLidar : public ASensor
|
||||||
{
|
{
|
||||||
GENERATED_BODY()
|
GENERATED_BODY()
|
||||||
|
|
||||||
|
using FLidarMeasurement = carla::sensor::s11n::LidarMeasurement;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
ARayTraceLidar(const FObjectInitializer &ObjectInitializer);
|
static FActorDefinition GetSensorDefinition();
|
||||||
|
|
||||||
void SetLidar(const FLidarDescription &LidarDescription);
|
ARayCastLidar(const FObjectInitializer &ObjectInitializer);
|
||||||
|
|
||||||
|
void Set(const FActorDescription &Description) override;
|
||||||
|
|
||||||
|
void Set(const FLidarDescription &LidarDescription);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
|
@ -78,7 +78,7 @@ ASceneCaptureSensor::ASceneCaptureSensor(const FObjectInitializer& ObjectInitial
|
||||||
void ASceneCaptureSensor::Set(const FActorDescription &Description)
|
void ASceneCaptureSensor::Set(const FActorDescription &Description)
|
||||||
{
|
{
|
||||||
Super::Set(Description);
|
Super::Set(Description);
|
||||||
UActorBlueprintFunctionLibrary::SetActor(Description, this);
|
UActorBlueprintFunctionLibrary::SetCamera(Description, this);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ASceneCaptureSensor::SetImageSize(uint32 InWidth, uint32 InHeight)
|
void ASceneCaptureSensor::SetImageSize(uint32 InWidth, uint32 InHeight)
|
||||||
|
|
Loading…
Reference in New Issue