From 6d9ed1e9351797d06d91d575dedd460fc27f64ba Mon Sep 17 00:00:00 2001 From: Anton Pechenko Date: Mon, 4 Dec 2017 18:06:01 +0300 Subject: [PATCH] lidar done, before merge master --- PythonClient/CarlaSettings.ini | 4 +-- PythonClient/carla/datastream.py | 30 +++-------------- PythonClient/carla_manual_control.py | 21 ++++++++++++ .../Carla/Source/Carla/Game/CarlaServer.cpp | 32 +++--------------- .../Plugins/Carla/Source/Carla/Lidar.cpp | 33 +------------------ .../Plugins/Carla/Source/Carla/Lidar.h | 2 -- .../Plugins/Carla/Source/Carla/LidarLaser.cpp | 18 +++++----- .../Plugins/Carla/Source/Carla/LidarLaser.h | 3 -- .../Source/Carla/Settings/LidarDescription.h | 17 ---------- .../source/carla/server/CarlaServer.cpp | 3 +- .../source/carla/server/ImagesMessage.cpp | 13 -------- .../source/carla/server/ImagesMessage.h | 20 ----------- .../carla/server/LidarMeasurementsMessage.cpp | 17 ---------- .../carla/server/LidarMeasurementsMessage.h | 31 ----------------- .../source/carla/server/MeasurementsMessage.h | 14 ++++---- .../source/test/Test_CarlaServer.cpp | 1 - .../source/test/Test_TCPServer.cpp | 2 -- 17 files changed, 51 insertions(+), 210 deletions(-) diff --git a/PythonClient/CarlaSettings.ini b/PythonClient/CarlaSettings.ini index 506f34182..50a1939d6 100644 --- a/PythonClient/CarlaSettings.ini +++ b/PythonClient/CarlaSettings.ini @@ -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 diff --git a/PythonClient/carla/datastream.py b/PythonClient/carla/datastream.py index 3e2d6fc0f..40f4cb876 100644 --- a/PythonClient/carla/datastream.py +++ b/PythonClient/carla/datastream.py @@ -98,20 +98,17 @@ class DataStream(object): pointer +=4 channels_count = struct.unpack(' 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): diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaServer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaServer.cpp index 50acbf3b8..461abe4bd 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaServer.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaServer.cpp @@ -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(cLidarMeasurement.channels_count); - for(int i=0; i(3 * 32 * 2); - size_t points_filled = 0; - for(int i=0; i 0) { cLidarMeasurement.horizontal_angle = uLidarSegment.HorizontalAngle; @@ -130,16 +106,18 @@ static void Set( total_points += points_count; } cLidarMeasurementData.points = MakeUnique(3 * total_points); + size_t points_filled = 0; for(int i=0; iGetComponentLocation(); - // 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; } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Lidar.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Lidar.h index d7f22fa63..9b4b1b007 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Lidar.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Lidar.h @@ -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); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/LidarLaser.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/LidarLaser.cpp index 4bf844f39..803273860 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/LidarLaser.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/LidarLaser.cpp @@ -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 + LidarBodyRot + ); 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; } } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/LidarLaser.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/LidarLaser.h index c7773f1cd..511870803 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/LidarLaser.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/LidarLaser.h @@ -6,9 +6,6 @@ class ALidar; -/** - * - */ class CARLA_API LidarLaser { public: diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/LidarDescription.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/LidarDescription.h index 9a9c88364..f1a39c231 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/LidarDescription.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/LidarDescription.h @@ -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 diff --git a/Util/CarlaServer/source/carla/server/CarlaServer.cpp b/Util/CarlaServer/source/carla/server/CarlaServer.cpp index 55c04f893..a86a1b05b 100644 --- a/Util/CarlaServer/source/carla/server/CarlaServer.cpp +++ b/Util/CarlaServer/source/carla/server/CarlaServer.cpp @@ -132,6 +132,7 @@ int32_t carla_write_measurements( return agent->WriteMeasurements( values, carla::const_array_view(images, number_of_images), - carla::const_array_view(lidar_measurements, number_of_lidar_measurements)).value(); + carla::const_array_view(lidar_measurements, number_of_lidar_measurements) + ).value(); } } diff --git a/Util/CarlaServer/source/carla/server/ImagesMessage.cpp b/Util/CarlaServer/source/carla/server/ImagesMessage.cpp index bb8a00dad..4bf732452 100644 --- a/Util/CarlaServer/source/carla/server/ImagesMessage.cpp +++ b/Util/CarlaServer/source/carla/server/ImagesMessage.cpp @@ -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(count); - // _capacity = count; - // } - // _size = count; - // } - } // namespace server } // namespace carla diff --git a/Util/CarlaServer/source/carla/server/ImagesMessage.h b/Util/CarlaServer/source/carla/server/ImagesMessage.h index 27052d211..078df93b7 100644 --- a/Util/CarlaServer/source/carla/server/ImagesMessage.h +++ b/Util/CarlaServer/source/carla/server/ImagesMessage.h @@ -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 images, unsigned char *buffer ); size_t GetSize(const_array_view images); - - // const_buffer buffer() const { - // return boost::asio::buffer(_buffer.get(), _size); - // } - - private: - - // void Reset(uint32_t count); - // - // std::unique_ptr _buffer = nullptr; - // - // uint32_t _size = 0u; - // - // uint32_t _capacity = 0u; }; } // namespace server diff --git a/Util/CarlaServer/source/carla/server/LidarMeasurementsMessage.cpp b/Util/CarlaServer/source/carla/server/LidarMeasurementsMessage.cpp index b3c1b2b01..4e2717bbe 100644 --- a/Util/CarlaServer/source/carla/server/LidarMeasurementsMessage.cpp +++ b/Util/CarlaServer/source/carla/server/LidarMeasurementsMessage.cpp @@ -2,7 +2,6 @@ #include "carla/server/LidarMeasurementsMessage.h" #include -#include #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(count); - // _capacity = count; - // } - // _size = count; - // } - } // namespace server } // namespace carla diff --git a/Util/CarlaServer/source/carla/server/LidarMeasurementsMessage.h b/Util/CarlaServer/source/carla/server/LidarMeasurementsMessage.h index 44f132ae1..919bf3a17 100644 --- a/Util/CarlaServer/source/carla/server/LidarMeasurementsMessage.h +++ b/Util/CarlaServer/source/carla/server/LidarMeasurementsMessage.h @@ -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 lidar_measurements, unsigned char *buffer ); size_t GetSize(const_array_view lidar_measurements); - - // const_buffer buffer() const { - // return boost::asio::buffer(_buffer.get(), _size); - // } - - private: - - // void Reset(uint32_t count); - // - // std::unique_ptr _buffer = nullptr; - // - // uint32_t _size = 0u; - // - // uint32_t _capacity = 0u; }; } // namespace server diff --git a/Util/CarlaServer/source/carla/server/MeasurementsMessage.h b/Util/CarlaServer/source/carla/server/MeasurementsMessage.h index 539293287..45a031058 100644 --- a/Util/CarlaServer/source/carla/server/MeasurementsMessage.h +++ b/Util/CarlaServer/source/carla/server/MeasurementsMessage.h @@ -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 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); diff --git a/Util/CarlaServer/source/test/Test_CarlaServer.cpp b/Util/CarlaServer/source/test/Test_CarlaServer.cpp index 735884f03..6f49be215 100644 --- a/Util/CarlaServer/source/test/Test_CarlaServer.cpp +++ b/Util/CarlaServer/source/test/Test_CarlaServer.cpp @@ -1,6 +1,5 @@ #include #include -#include #include diff --git a/Util/CarlaServer/source/test/Test_TCPServer.cpp b/Util/CarlaServer/source/test/Test_TCPServer.cpp index ef33951c7..26dd5a395 100644 --- a/Util/CarlaServer/source/test/Test_TCPServer.cpp +++ b/Util/CarlaServer/source/test/Test_TCPServer.cpp @@ -1,5 +1,3 @@ -#include - #include #include