Merge pull request #1 from ctu-mrs/intensityLidar

Intensity lidar

cfg/unreal_simulator.cfg

@@ -32,6 +32,20 @@ lidar_segmented = gen.add_group("Segmented LiDAR");
 lidar_segmented.add("lidar_seg_enabled", bool_t, 0, "LiDAR segmented enabled", True)
 lidar_segmented.add("lidar_seg_rate", double_t, 0, "LiDAR segmented rate", 1.0, 1.0, 200.0)
 
+lidar_intensity = gen.add_group("Intensity LiDAR");
+
+lidar_intensity.add("lidar_int_enabled", bool_t, 0, "LiDAR intensity enabled", True)
+lidar_intensity.add("lidar_int_rate", double_t, 0, "LiDAR intensity rate", 1.0, 1.0, 200.0)
+lidar_intensity.add("lidar_int_noise_enabled", bool_t, 0, "LiDAR intensity noise enabled", True)
+lidar_intensity.add("lidar_int_value_grass", double_t, 0, "LiDAR intensity value grass", 0.0, 0.0, 255.0)
+lidar_intensity.add("lidar_int_value_road", double_t, 0, "LiDAR intensity value road", 0.0, 0.0, 255.0)
+lidar_intensity.add("lidar_int_value_building", double_t, 0, "LiDAR intensity value building", 0.0, 0.0, 255.0)
+lidar_intensity.add("lidar_int_value_tree", double_t, 0, "LiDAR intensity value tree", 0.0, 0.0, 255.0)
+lidar_intensity.add("lidar_int_value_fence", double_t, 0, "LiDAR intensity value fence", 0.0, 0.0, 255.0)
+lidar_intensity.add("lidar_int_value_dirt_road", double_t, 0, "LiDAR intensity value dirt road", 0.0, 0.0, 255.0)
+lidar_intensity.add("lidar_int_value_other", double_t, 0, "LiDAR intensity value other", 0.0, 0.0, 255.0)
+lidar_intensity.add("lidar_int_std_at_1m", double_t, 0, "LiDAR intensity STD at 1m", 0.0, 0.01, 1.0)
+lidar_intensity.add("lidar_int_std_slope", double_t, 0, "LiDAR intensity STD slope", 0.0, 0.01, 1.0)
 rgb = gen.add_group("RGB");
 
 rgb.add("rgb_enabled", bool_t, 0, "RGB enabled", False)

config/unreal_simulator.yaml

@@ -80,6 +80,25 @@ sensors:
       enabled: false
       rate: 10.0 # [Hz]
 
+    lidar_intensity:
+      enabled: false
+      rate: 10.0 # [Hz]
+
+      values:
+        grass: 50
+        road: 7
+        tree: 90
+        building: 70
+        fence: 40
+        dirt_road: 30
+        other: 155
+
+      noise:
+        enabled: true
+        std_at_1m: 0.59 # [-]
+        std_slope: 0.81 # [-] # multiplies std_at_1m for each meter of measured distances
+
+
   rgb:
     enabled: false
 

include/ueds_connector/data_types.h

@@ -67,6 +67,21 @@ struct LidarSegData
   }
 };
 
+struct LidarIntData
+{
+  LidarIntData() = default;
+
+  double      distance;
+  double      directionX;
+  double      directionY;
+  double      directionZ;
+  int      intensity;
+  std::string toString() const {
+    return "(distance: " + std::to_string(distance) + ", directionX: " + std::to_string(directionX) + ", directionY: " + std::to_string(directionY) +
+           ", directionZ: " + std::to_string(directionZ) + ", intensity: " + std::to_string(intensity) + ")";
+  }
+};
+
 struct LidarConfig
 {
   LidarConfig() = default;

include/ueds_connector/serialization/serializable_shared.h

@@ -104,7 +104,8 @@ enum MessageType : unsigned short
   get_lidar_seg                   = 19,
   set_location_and_rotation_async = 20,
   get_crash_state                 = 21,
-  get_rangefinder_data            = 22
+  get_lidar_int                   = 22,
+  get_rangefinder_data            = 23,
 };
 
 /* struct LidarConfig //{ */
@@ -561,7 +562,7 @@ namespace GetRangefinderData
       archive(cereal::base_class<Common::NetworkResponse>(this), range);
     }
   };
-}  // namespace GetRangefinderData  
+}  // namespace GetRangefinderData
 
 /* GetLidarData //{ */
 
@@ -657,6 +658,53 @@ struct Response : public Common::NetworkResponse
 
 //}
 
+/* GetLidarIntData //{ */
+
+namespace GetLidarIntData
+{
+struct LidarIntData
+{
+  LidarIntData() = default;
+
+  double distance;
+  double directionX;
+  double directionY;
+  double directionZ;
+  int    intensity;
+  template <class Archive>
+  void serialize(Archive& archive) {
+    archive(distance, directionX, directionY, directionZ, intensity);
+  }
+};
+
+struct Request : public Common::NetworkRequest
+{
+  Request() : Common::NetworkRequest(static_cast<unsigned short>(MessageType::get_lidar_int)) {
+  }
+};
+
+struct Response : public Common::NetworkResponse
+{
+  Response() : Common::NetworkResponse(static_cast<unsigned short>(MessageType::get_lidar_int)) {
+  }
+  explicit Response(bool _status) : Common::NetworkResponse(MessageType::get_lidar_int, _status) {
+  }
+
+  double startX;
+  double startY;
+  double startZ;
+
+  std::vector<LidarIntData> lidarIntData;
+
+  template <class Archive>
+  void serialize(Archive& archive) {
+    archive(cereal::base_class<Common::NetworkResponse>(this), startX, startY, startZ, lidarIntData);
+  }
+};
+}  // namespace GetLidarIntData
+
+//}
+
 /* GetLidarConfig //{ */
 
 namespace GetLidarConfig
@@ -969,7 +1017,7 @@ struct Response : public Common::NetworkResponse
   Response() : Common::NetworkResponse(static_cast<unsigned short>(MessageType::set_forest_hilly_level)){};
   explicit Response(bool _status) : Common::NetworkResponse(MessageType::set_forest_hilly_level, _status){};
 };
-};  // namespace SetForestHilly  
+};  // namespace SetForestHilly
 
 
 namespace SpawnDrone
@@ -1010,20 +1058,20 @@ namespace SpawnDroneAtLocation
       archive(cereal::base_class<Common::NetworkRequest>(this), x, y, z);
     }
   };
-  
+
   struct Response : public Common::NetworkResponse
   {
     Response() : Common::NetworkResponse(static_cast<unsigned short>(MessageType::spawn_drone_at_location)){};
     explicit Response(bool _status) : Common::NetworkResponse(MessageType::spawn_drone, _status){};
-    
+
     int port;
 
     template <class Archive>
     void serialize(Archive& archive) {
       archive(cereal::base_class<Common::NetworkResponse>(this), port);
     }
   };
-};  // namespace SpawnDrone 
+};  // namespace SpawnDrone
 
 namespace RemoveDrone
 {
@@ -1107,7 +1155,7 @@ enum GraphicsSettingsEnum : unsigned short
   EPIC = 3,
   CINEMATIC = 4
 };
-  
+
 namespace SetGraphicsSettings
 {
   struct Request : public Common::NetworkRequest
@@ -1127,7 +1175,7 @@ namespace SetGraphicsSettings
     Response() : Common::NetworkResponse(static_cast<unsigned short>(MessageType::set_graphics_settings)){};
     explicit Response(bool _status) : Common::NetworkResponse(MessageType::set_graphics_settings, _status){};
   };
-  
+
 }// namespace SetGraphicsSettings
 
 enum WorldLevelEnum : unsigned short

include/ueds_connector/ueds_connector.h

@@ -46,6 +46,8 @@ class UedsConnector : public SocketClient {
   std::tuple<bool, std::vector<LidarData>, Coordinates> GetLidarData();
 
   std::tuple<bool, std::vector<LidarSegData>, Coordinates> GetLidarSegData();
+
+  std::tuple<bool, std::vector<LidarIntData>, Coordinates> GetLidarIntData();
 
   std::pair<bool, LidarConfig> GetLidarConfig();
 

src/ueds_connector/ueds_connector.cpp

@@ -9,6 +9,7 @@ using kissnet::socket_status;
 using ueds_connector::Coordinates;
 using ueds_connector::LidarConfig;
 using ueds_connector::LidarData;
+using ueds_connector::LidarIntData;
 using ueds_connector::LidarSegData;
 using ueds_connector::RgbCameraConfig;
 using ueds_connector::Rotation;
@@ -262,6 +263,7 @@ std::tuple<bool> UedsConnector::SetLocationAndRotationAsync(const Coordinates& c
 
 std::tuple<bool, std::vector<LidarData>, Coordinates> UedsConnector::GetLidarData() {
 
+  /* std::cout << "GetLidarData()" << std::endl; */
   Serializable::Drone::GetLidarData::Request request{};
 
   Serializable::Drone::GetLidarData::Response response{};
@@ -338,6 +340,43 @@ std::tuple<bool, std::vector<LidarSegData>, Coordinates> UedsConnector::GetLidar
 }
 //}
 
+/* getLidarIntData() //{ */
+
+std::tuple<bool, std::vector<LidarIntData>, Coordinates> UedsConnector::GetLidarIntData() {
+
+  /* std::cout << "GetLidarIntData()" << std::endl; */
+
+  Serializable::Drone::GetLidarIntData::Request request{};
+
+  Serializable::Drone::GetLidarIntData::Response response{};
+  const auto                                     status  = Request(request, response);
+  const auto                                     success = status && response.status;
+  std::vector<LidarIntData>                      lidarIntData;
+  Coordinates                                    start{};
+
+  if (success) {
+
+    const auto arrSize = response.lidarIntData.size();
+    lidarIntData.resize(arrSize);
+
+    for (size_t i = 0; i < arrSize; i++) {
+      lidarIntData[i]            = LidarIntData{};
+      lidarIntData[i].distance   = response.lidarIntData[i].distance;
+      lidarIntData[i].directionX = response.lidarIntData[i].directionX;
+      lidarIntData[i].directionY = response.lidarIntData[i].directionY;
+      lidarIntData[i].directionZ = response.lidarIntData[i].directionZ;
+      lidarIntData[i].intensity  = response.lidarIntData[i].intensity;
+    }
+
+    start.x = response.startX;
+    start.y = response.startY;
+    start.z = response.startZ;
+  }
+
+  return std::make_tuple(success, lidarIntData, start);
+}
+//}
+
 /* getLidarConfig() //{ */
 
 std::pair<bool, LidarConfig> UedsConnector::GetLidarConfig() {