p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

lidar done, before merge master

This commit is contained in:
Anton Pechenko 2017-12-04 18:06:01 +03:00
parent 39886a7e29
commit 6d9ed1e935
17 changed files with 51 additions and 210 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
PythonClient/CarlaSettings.ini
View File

@ -42,7 +42,7 @@ Channels = 32
; Measure distance
Range = 5000
; Points generated by all lasers per second
PointsPerSecond = 56000
PointsPerSecond = 100000
; Lidar rotation frequency
RotationFrequency = 10
; Upper laser angle, counts from horizontal,
@ -58,6 +58,6 @@ LidarPositionX = 0
LidarPositionY = 0
LidarPositionZ = 250
; Rotation relative to the player.
LidarRotationPitch = -10
LidarRotationPitch = 0
LidarRotationRoll = 0
LidarRotationYaw = 0

30
PythonClient/carla/datastream.py
View File

@ -98,20 +98,17 @@ class DataStream(object):
pointer +=4
channels_count = struct.unpack('<L',imagedata[pointer:(pointer+4)])[0]
pointer +=4
print('--- pointcloud {} {}'.format(horizontal_angle, channels_count))
points_count_by_channel_size = channels_count * 4
points_count_by_channel_bytes = imagedata[pointer:(pointer + points_count_by_channel_size)]
points_count_by_channel = np.frombuffer(points_count_by_channel_bytes, dtype=np.dtype('uint32'))
pointer += points_count_by_channel_size
print('--- points counts {} {}'.format(points_count_by_channel_size, points_count_by_channel))
points_in_one_channel = points_count_by_channel[0]
points_size = channels_count * points_in_one_channel * 3 * 8
points = np.frombuffer(imagedata[pointer:(pointer + points_size)], dtype='float')
points = np.reshape(points, (channels_count, points_in_one_channel, 3))
pointer += points_size
print('--- points {} {}'.format(points_size, points))
lidar_measurement = {
'horizontal_angle' : horizontal_angle,
@ -120,26 +117,6 @@ class DataStream(object):
'points' : points
}
# points = []
# for points_count in points_count_by_channel.tolist():
# points_size = points_count * 8
# points.append(np.frombuffer(image_bytes[pointer: (pointer + points_size)], dtype='float'))
# image_size = width*height*4
#
# image_bytes = imagedata[pointer:(pointer+image_size)]
#
#
# dt = np.dtype("uint8")
#
# new_image =np.frombuffer(image_bytes,dtype=dt)
#
# new_image = np.reshape(new_image,(self._image_y,self._image_x,4)) # TODO: make this generic
#
# pointer += image_size
return lidar_measurement, im_type, pointer
@ -183,11 +160,12 @@ class DataStream(object):
meas_dict.update({'Labels':[]})
meas_dict.update({'Lidars':[]})
pointer = 0
while pointer < len(imagedata):
im_type = self._read_data_type(imagedata, pointer)
print('--- image type: {}'.format(im_type))
if im_type == 0:
image,im_type,pointer = self._read_image(imagedata,pointer)
@ -210,8 +188,8 @@ class DataStream(object):
logging.debug("RECEIVED scene_seg")
if im_type == 10:
image,im_type,pointer = self._read_lidar_measurement(imagedata,pointer)
# meas_dict['Labels'].append(image)
lidar_measurement,im_type,pointer = self._read_lidar_measurement(imagedata,pointer)
meas_dict['Lidars'].append(lidar_measurement)
logging.debug("RECEIVED lidar_measurement")
meas_dict.update({'WallTime':measurements.platform_timestamp})

21
PythonClient/carla_manual_control.py
View File

@ -183,6 +183,7 @@ class App(object):
self.img_vec = measurements['BGRA']
self.depth_vec = measurements['Depth']
self.labels_vec = measurements['Labels']
self.lidars_vec = measurements['Lidars']
if time.time() - self.prev_time > 1.:
message = 'Step {step} ({fps:.1f} FPS): '
@ -252,6 +253,26 @@ class App(object):
self._display_surf.blit(surface, (x_pos, auxImgYPos))
x_pos += f
if self.lidars_vec:
lidar_data = np.array(self.lidars_vec[0]['points'][:, :, :2])
lidar_data /= 50.0
lidar_data += 100.0
lidar_data = np.fabs(lidar_data)
lidar_data = lidar_data.astype(np.int32)
lidar_data = np.reshape(lidar_data, (-1, 2))
#draw lidar
lidar_img_size = (200, 200, 3)
lidar_img = np.zeros(lidar_img_size)
lidar_img[tuple(lidar_data.T)] = (255, 255, 255)
surface = pygame.surfarray.make_surface(
lidar_img
)
# surface = pygame.transform.scale(surface, (200, 200))
self._display_surf.blit(surface, (10, 10))
pygame.display.flip()
def on_cleanup(self):

32
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaServer.cpp
View File

@ -92,30 +92,6 @@ static void Set(
carla_lidar_measurement_data &cLidarMeasurementData)
{
cLidarMeasurement.horizontal_angle = 0;
cLidarMeasurement.channels_count = 32;
cLidarMeasurementData.points_count_by_channel = MakeUnique<uint32_t[]>(cLidarMeasurement.channels_count);
for(int i=0; i<cLidarMeasurement.channels_count; i++)
{
cLidarMeasurementData.points_count_by_channel[i] = 2;
}
cLidarMeasurementData.points = MakeUnique<double[]>(3 * 32 * 2);
size_t points_filled = 0;
for(int i=0; i<cLidarMeasurement.channels_count; i++)
{
size_t points_count = cLidarMeasurementData.points_count_by_channel[i];
for(int pi=0; pi<points_count; pi++)
{
cLidarMeasurementData.points[3 * pi + 3 * points_filled] = 1 + 3 * pi;
cLidarMeasurementData.points[3 * pi + 1 + 3 * points_filled] = 2 + 3 * pi;
cLidarMeasurementData.points[3 * pi + 2 + 3 * points_filled] = 3 + 3 * pi;
}
points_filled += points_count;
}
cLidarMeasurement.points_count_by_channel = cLidarMeasurementData.points_count_by_channel.Get();
cLidarMeasurement.data = cLidarMeasurementData.points.Get();
return;
if (uLidarSegment.LidarLasersSegments.Num() > 0) {
cLidarMeasurement.horizontal_angle = uLidarSegment.HorizontalAngle;
@ -130,16 +106,18 @@ static void Set(
total_points += points_count;
}
cLidarMeasurementData.points = MakeUnique<double[]>(3 * total_points);
size_t points_filled = 0;
for(int i=0; i<cLidarMeasurement.channels_count; i++)
{
size_t points_count = cLidarMeasurementData.points_count_by_channel[i];
auto& laser_points = uLidarSegment.LidarLasersSegments[i].Points;
for(int pi=0; pi<points_count; pi++)
{
cLidarMeasurementData.points[pi] = laser_points[pi].X;
cLidarMeasurementData.points[pi + 1] = laser_points[pi].Y;
cLidarMeasurementData.points[pi + 2] = laser_points[pi].Z;
cLidarMeasurementData.points[3 * (pi + points_filled)] = laser_points[pi].X;
cLidarMeasurementData.points[3 * (pi + points_filled) + 1] = laser_points[pi].Y;
cLidarMeasurementData.points[3 * (pi + points_filled) + 2] = laser_points[pi].Z;
}
points_filled += points_count;
}
cLidarMeasurement.points_count_by_channel = cLidarMeasurementData.points_count_by_channel.Get();

33
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Lidar.cpp
View File

@ -23,22 +23,6 @@ ALidar::ALidar()
void ALidar::Set(const FLidarDescription &LidarDescription)
{
// UE_LOG(LogCarla, Log, TEXT("--- Lidar settings --------------------------"));
// UE_LOG(LogCarla, Log, TEXT("pos x %f"), LidarDescription.Position.X);
// UE_LOG(LogCarla, Log, TEXT("pos y %f"), LidarDescription.Position.Y);
// UE_LOG(LogCarla, Log, TEXT("pos z %f"), LidarDescription.Position.Z);
// UE_LOG(LogCarla, Log, TEXT("rot p %f"), LidarDescription.Rotation.Pitch);
// UE_LOG(LogCarla, Log, TEXT("rot r %f"), LidarDescription.Rotation.Roll);
// UE_LOG(LogCarla, Log, TEXT("rot y %f"), LidarDescription.Rotation.Yaw);
//
// UE_LOG(LogCarla, Log, TEXT("ch %d"), LidarDescription.Channels);
// UE_LOG(LogCarla, Log, TEXT("r %f"), LidarDescription.Range);
// UE_LOG(LogCarla, Log, TEXT("pts %f"), LidarDescription.PointsPerSecond);
// UE_LOG(LogCarla, Log, TEXT("freq %f"), LidarDescription.RotationFrequency);
// UE_LOG(LogCarla, Log, TEXT("upper l %f"), LidarDescription.UpperFovLimit);
// UE_LOG(LogCarla, Log, TEXT("lower l %f"), LidarDescription.LowerFovLimit);
// UE_LOG(LogCarla, Log, TEXT("d %d"), LidarDescription.ShowDebugPoints?1:0);
Channels = LidarDescription.Channels;
Range = LidarDescription.Range;
PointsPerSecond = LidarDescription.PointsPerSecond;
@ -66,25 +50,11 @@ void ALidar::BeginPlay()
Super::BeginPlay();
}
// Called every frame
// void ALidar::Tick(float DeltaTime)
// {
// Super::Tick(DeltaTime);
// }
void ALidar::ReadPoints(float DeltaTime, FCapturedLidarSegment& LidarSegmentData)
{
// UE_LOG(LogCarla, Log, TEXT("--- Lidar tick %d"), ShowDebugPoints?1:0);
// FVector LidarBodyLoc = RootComponent->GetComponentLocation();
// UE_LOG(LogCarla, Log, TEXT("--- location: %f %f %f"), lidar_body_loc.X, lidar_body_loc.Y, lidar_body_loc.Z);
// UE_LOG(LogCarla, Log, TEXT("--- actor rotation: %s"), *GetActorRotation().ToString());
// UE_LOG(LogCarla, Log, TEXT("--- root rotation: %s"), *RootComponent->GetComponentRotation().ToString());
int PointsToScanWithOneLaser = int(FMath::RoundHalfFromZero(PointsPerSecond * DeltaTime / float(Channels)));
// float HorizontalAngle = CurrentHorizontalAngle;
float AngleDistanceOfTick = RotationFrequency * 360 * DeltaTime;
// PrintString(FString::Printf(TEXT("tick %f %f %d"), DeltaTime, angle_distance_of_tick, points_to_scan_with_one_laser));
float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser;
LidarSegmentData.LidarLasersSegments.Empty();
@ -107,7 +77,6 @@ void ALidar::ReadPoints(float DeltaTime, FCapturedLidarSegment& LidarSegmentData
}
}
// lidar_body_->AddLocalRotation(FRotator(0, angle_distance_of_tick, 0), false);
CurrentHorizontalAngle += AngleDistanceOfTick;
CurrentHorizontalAngle = fmod(CurrentHorizontalAngle + AngleDistanceOfTick, 360.0f);
LidarSegmentData.HorizontalAngle = CurrentHorizontalAngle;
}

2
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Lidar.h
View File

@ -30,8 +30,6 @@ protected:
void CreateLasers();
public:
// Called every frame
// virtual void Tick(float DeltaTime) override;
/** Capture lidar segment points produced by DeltaTime */
void ReadPoints(float DeltaTime, FCapturedLidarSegment& LidarSegmentData);

16
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/LidarLaser.cpp
View File

@ -21,22 +21,18 @@ bool LidarLaser::Measure(ALidar* Lidar, float HorizontalAngle, FVector& XYZ, boo
FVector LidarBodyLoc = Lidar->GetActorLocation();
FRotator LidarBodyRot = Lidar->GetActorRotation();
FRotator LaserRot (VerticalAngle, HorizontalAngle, 0);
// float InPitch, float InYaw, float InRoll
FRotator LaserRot (VerticalAngle, HorizontalAngle, 0); // float InPitch, float InYaw, float InRoll
FRotator ResultRot = UKismetMathLibrary::ComposeRotators(
LaserRot,
// UKismetMathLibrary::ComposeRotators(
// FRotator(25, 0, 0),
LidarBodyRot
// )
); //up, no change, no change
);
FVector EndTrace = Lidar->Range * UKismetMathLibrary::GetForwardVector(ResultRot) + LidarBodyLoc;
Lidar->GetWorld()->LineTraceSingleByChannel(
HitInfo,
LidarBodyLoc,
EndTrace,
ECC_Visibility,
ECC_MAX,
TraceParams,
FCollisionResponseParams::DefaultResponseParam
);
@ -56,9 +52,15 @@ bool LidarLaser::Measure(ALidar* Lidar, float HorizontalAngle, FVector& XYZ, boo
}
XYZ = LidarBodyLoc - HitInfo.ImpactPoint;
XYZ = UKismetMathLibrary::RotateAngleAxis(
XYZ,
- LidarBodyRot.Yaw + 90,
FVector(0, 0, 1)
);
return true;
} else {
XYZ = FVector(0, 0, 0);
return false;
}
}

3
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/LidarLaser.h
View File

@ -6,9 +6,6 @@
class ALidar;
/**
*
*/
class CARLA_API LidarLaser
{
public:

17
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/LidarDescription.h
View File

@ -51,20 +51,3 @@ struct FLidarDescription
UPROPERTY(Category = "Lidar Description", EditDefaultsOnly)
FRotator Rotation = {0.0f, 0.0f, 0.0f};
};
// Parameters of known lidars
// Velodyne HDL-32E
// +/- 2 cm accuracy
// 32 Channels
// 80m-100m Range
// 700,000 Points per Second
// 360° Horizontal FOV
// +10° to -30° Vertical FOV
// Velodyne VLP-16
// 16 Channels
// 100m Range
// 300,000 Points per Second
// 360° Horizontal FOV
// +/- 15° Vertical FOV

3
Util/CarlaServer/source/carla/server/CarlaServer.cpp
View File

@ -132,6 +132,7 @@ int32_t carla_write_measurements(
return agent->WriteMeasurements(
values,
carla::const_array_view<carla_image>(images, number_of_images),
carla::const_array_view<carla_lidar_measurement>(lidar_measurements, number_of_lidar_measurements)).value();
carla::const_array_view<carla_lidar_measurement>(lidar_measurements, number_of_lidar_measurements)
).value();
}
}

13
Util/CarlaServer/source/carla/server/ImagesMessage.cpp
View File

@ -44,11 +44,8 @@ namespace server {
unsigned char *buffer
) {
long buffer_size = GetSizeOfBuffer(images);
// Reset(sizeof(uint32_t) + buffer_size); // header + buffer.
auto begin = buffer;
// auto begin = _buffer.get();
// begin += WriteSizeToBuffer(begin, buffer_size);
for (const auto &image : images) {
begin += WriteSizeToBuffer(begin, image.width);
begin += WriteSizeToBuffer(begin, image.height);
@ -56,18 +53,8 @@ namespace server {
begin += WriteImageToBuffer(begin, image);
}
DEBUG_ASSERT(std::distance(buffer, begin) == buffer_size);
// DEBUG_ASSERT(std::distance(_buffer.get(), begin) == _size);
return buffer_size;
}
// void ImagesMessage::Reset(const uint32_t count) {
// if (_capacity < count) {
// log_info("allocating image buffer of", count, "bytes");
// _buffer = std::make_unique<unsigned char[]>(count);
// _capacity = count;
// }
// _size = count;
// }
} // namespace server
} // namespace carla

20
Util/CarlaServer/source/carla/server/ImagesMessage.h
View File

@ -31,32 +31,12 @@ namespace server {
class ImagesMessage : private NonCopyable {
public:
/// Allocates a new buffer if the capacity is not enough to hold the images,
/// but it does not allocate a smaller one if the capacity is greater than
/// the size of the images.
///
/// @note The expected usage of this class is to mantain a constant size
/// buffer of images, so memory allocation occurs only once.
size_t Write(
const_array_view<carla_image> images,
unsigned char *buffer
);
size_t GetSize(const_array_view<carla_image> images);
// const_buffer buffer() const {
// return boost::asio::buffer(_buffer.get(), _size);
// }
private:
// void Reset(uint32_t count);
//
// std::unique_ptr<unsigned char[]> _buffer = nullptr;
//
// uint32_t _size = 0u;
//
// uint32_t _capacity = 0u;
};
} // namespace server

17
Util/CarlaServer/source/carla/server/LidarMeasurementsMessage.cpp
View File

@ -2,7 +2,6 @@
#include "carla/server/LidarMeasurementsMessage.h"
#include <cstring>
#include <iostream>
#include "carla/Debug.h"
#include "carla/Logging.h"
@ -46,9 +45,7 @@ namespace server {
static size_t WriteLidarMeasurementToBuffer(unsigned char *buffer, const carla_lidar_measurement &lidar_measurement) {
const auto points_counts_size = GetSizeOfLidarPointsCounts(lidar_measurement);
std::cout << "--- points_counts_size: " << points_counts_size << std::endl;
const auto points_size = GetSizeOfLidarPoints(lidar_measurement);
std::cout << "--- points_size: " << points_size << std::endl;
DEBUG_ASSERT(lidar_measurement.points_count_by_channel != nullptr);
DEBUG_ASSERT(lidar_measurement.data != nullptr);
std::memcpy(buffer, lidar_measurement.points_count_by_channel, points_counts_size);
@ -65,12 +62,8 @@ namespace server {
unsigned char *buffer
) {
long buffer_size = GetSizeOfBuffer(lidar_measurements);
// Reset(sizeof(uint32_t) + buffer_size); // header + buffer
std::cout << "--- buffer size: " << buffer_size << std::endl;
auto begin = buffer;
// auto begin = _buffer.get();
// begin += WriteIntToBuffer(begin, buffer_size);
for (const auto &lidar_measurement : lidar_measurements) {
begin += WriteDoubleToBuffer(begin, lidar_measurement.horizontal_angle);
begin += WriteIntToBuffer(begin, 10); // type of lidar message
@ -78,18 +71,8 @@ namespace server {
begin += WriteLidarMeasurementToBuffer(begin, lidar_measurement);
}
DEBUG_ASSERT(std::distance(buffer, begin) == buffer_size);
// DEBUG_ASSERT(std::distance(_buffer.get(), begin) == _size);
return buffer_size;
}
// void LidarMeasurementsMessage::Reset(const uint32_t count) {
// if (_capacity < count) {
// log_info("allocating image buffer of", count, "bytes");
// _buffer = std::make_unique<unsigned char[]>(count);
// _capacity = count;
// }
// _size = count;
// }
} // namespace server
} // namespace carla

31
Util/CarlaServer/source/carla/server/LidarMeasurementsMessage.h
View File

@ -12,46 +12,15 @@
namespace carla {
namespace server {
/// Encodes the given images as binary array to be sent to the client.
///
/// The message consists of an array of uint32's in the following layout
///
/// {
/// total size,
/// width, height, type, color[0], color[1],..., <- first image
/// width, height, type, color[0], color[1],..., <- second image
/// ...
/// }
///
class LidarMeasurementsMessage : private NonCopyable {
public:
/// Allocates a new buffer if the capacity is not enough to hold the images,
/// but it does not allocate a smaller one if the capacity is greater than
/// the size of the images.
///
/// @note The expected usage of this class is to mantain a constant size
/// buffer of images, so memory allocation occurs only once.
size_t Write(
const_array_view<carla_lidar_measurement> lidar_measurements,
unsigned char *buffer
);
size_t GetSize(const_array_view<carla_lidar_measurement> lidar_measurements);
// const_buffer buffer() const {
// return boost::asio::buffer(_buffer.get(), _size);
// }
private:
// void Reset(uint32_t count);
//
// std::unique_ptr<unsigned char[]> _buffer = nullptr;
//
// uint32_t _size = 0u;
//
// uint32_t _capacity = 0u;
};
} // namespace server

14
Util/CarlaServer/source/carla/server/MeasurementsMessage.h
View File

@ -18,6 +18,12 @@ namespace server {
class MeasurementsMessage : private NonCopyable {
public:
/// Allocates a new buffer if the capacity is not enough to hold the images and
/// lidar measurements, but it does not allocate a smaller one if the capacity is
/// greater than the size of the images.
///
/// @note The expected usage of this class is to mantain a constant size
/// buffer of images, so memory allocation occurs only once.
void Write(
const carla_measurements &measurements,
const_array_view<carla_image> images,
@ -43,14 +49,6 @@ namespace server {
return boost::asio::buffer(_buffer.get(), _size);
}
// const_buffer images() const {
// return _images.buffer();
// }
// const_buffer lidar_measurements() const {
// return _lidar_measurements.buffer();
// }
protected:
void Reset(uint32_t count);

1
Util/CarlaServer/source/test/Test_CarlaServer.cpp
View File

@ -1,6 +1,5 @@
#include <atomic>
#include <future>
#include <iostream>
#include <gtest/gtest.h>

2
Util/CarlaServer/source/test/Test_TCPServer.cpp
View File

@ -1,5 +1,3 @@
#include <iostream>
#include <future>
#include <gtest/gtest.h>