RobotImporter: add Lidar2D and sdf::GPU_LIDAR support (o3de#590)

* RobotImporter: Lidar2D support added
* add sdf::GPU_LIDAR to RGL mapping; lidar fixes

---------

Signed-off-by: Jan Hanca <[email protected]>

Author: jhanca-robotecai

Gems/ROS2/Code/Source/Lidar/LidarSystem.h

@@ -26,9 +26,9 @@ namespace ROS2
         void Activate();
         void Deactivate();
 
-    private:
         static constexpr const char* SystemName = "Scene Queries";
 
+    private:
         // LidarSystemRequestBus overrides
         LidarId CreateLidar(AZ::EntityId lidarEntityId) override;
         void DestroyLidar(LidarId lidarId) override;

Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp

@@ -6,6 +6,7 @@
  *
  */
 
+#include <Lidar/ROS2Lidar2DSensorComponent.h>
 #include <Lidar/ROS2LidarSensorComponent.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
 #include <RobotImporter/SDFormat/ROS2SensorHooks.h>
@@ -19,7 +20,7 @@ namespace ROS2::SDFormat
     SensorImporterHook ROS2SensorHooks::ROS2LidarSensor()
     {
         SensorImporterHook importerHook;
-        importerHook.m_sensorTypes = AZStd::unordered_set<sdf::SensorType>{ sdf::SensorType::LIDAR };
+        importerHook.m_sensorTypes = AZStd::unordered_set<sdf::SensorType>{ sdf::SensorType::LIDAR, sdf::SensorType::GPU_LIDAR };
         importerHook.m_supportedSensorParams = AZStd::unordered_set<AZStd::string>{
             ">update_rate",
             ">lidar>scan>horizontal>samples",
@@ -51,28 +52,53 @@ namespace ROS2::SDFormat
                 return AZ::Failure(AZStd::string("Failed to read parsed SDFormat data of %s Lidar sensor", sdfSensor.Name().c_str()));
             }
 
+            const bool is2DLidar = (lidarSensor->VerticalScanSamples() == 1);
+
             SensorConfiguration sensorConfiguration;
             sensorConfiguration.m_frequency = sdfSensor.UpdateRate();
-            const AZStd::string messageType = "sensor_msgs::msg::PointCloud2";
-            Utils::AddTopicConfiguration(sensorConfiguration, "pc", messageType, messageType);
+            const AZStd::string messageType = is2DLidar ? "sensor_msgs::msg::LaserScan" : "sensor_msgs::msg::PointCloud2";
+            const AZStd::string messageTopic = is2DLidar ? "scan" : "pc";
+            Utils::AddTopicConfiguration(sensorConfiguration, messageTopic, messageType, messageType);
 
-            LidarSensorConfiguration lidarConfiguration;
-            lidarConfiguration.m_lidarParameters.m_model = LidarTemplate::LidarModel::Custom3DLidar;
+            LidarSensorConfiguration lidarConfiguration{ is2DLidar ? LidarTemplateUtils::Get2DModels()
+                                                                   : LidarTemplateUtils::Get3DModels() };
             lidarConfiguration.m_lidarParameters.m_name = AZStd::string(sdfSensor.Name().c_str());
             lidarConfiguration.m_lidarParameters.m_minHAngle = lidarSensor->HorizontalScanMinAngle().Degree();
             lidarConfiguration.m_lidarParameters.m_maxHAngle = lidarSensor->HorizontalScanMaxAngle().Degree();
             lidarConfiguration.m_lidarParameters.m_minVAngle = lidarSensor->VerticalScanMinAngle().Degree();
             lidarConfiguration.m_lidarParameters.m_maxVAngle = lidarSensor->VerticalScanMaxAngle().Degree();
-            lidarConfiguration.m_lidarParameters.m_layers = lidarSensor->HorizontalScanSamples();
-            lidarConfiguration.m_lidarParameters.m_numberOfIncrements = lidarSensor->VerticalScanSamples();
+            lidarConfiguration.m_lidarParameters.m_numberOfIncrements = lidarSensor->HorizontalScanSamples();
+            lidarConfiguration.m_lidarParameters.m_layers = lidarSensor->VerticalScanSamples();
             lidarConfiguration.m_lidarParameters.m_minRange = lidarSensor->RangeMin();
             lidarConfiguration.m_lidarParameters.m_maxRange = lidarSensor->RangeMax();
 
+            const auto lidarSystems = LidarRegistrarInterface::Get()->GetRegisteredLidarSystems();
+            if (!lidarSystems.empty())
+            {
+                const AZStd::string query = (sdfSensor.Type() == sdf::SensorType::GPU_LIDAR ? "RobotecGPULidar" : LidarSystem::SystemName);
+                if (const auto it = AZStd::find(lidarSystems.begin(), lidarSystems.end(), query); it != lidarSystems.end())
+                {
+                    lidarConfiguration.m_lidarSystem = *it;
+                }
+            }
+
+            if (lidarConfiguration.m_lidarSystem.empty())
+            {
+                AZ_Warning("ROS2LidarSensorHook", false, "Lidar System in imported robot not set.");
+                AZ_Warning(
+                    "ROS2LidarSensorHook",
+                    sdfSensor.Type() != sdf::SensorType::GPU_LIDAR,
+                    "GPU Lidar requires RGL Gem, see https://github.com/RobotecAI/o3de-rgl-gem for more details.\n");
+            }
+
             // Create required components
             Utils::CreateComponent<ROS2FrameComponent>(entity);
 
             // Create Lidar component
-            if (Utils::CreateComponent<ROS2LidarSensorComponent>(entity, sensorConfiguration, lidarConfiguration))
+            const auto lidarComponent = is2DLidar
+                ? Utils::CreateComponent<ROS2Lidar2DSensorComponent>(entity, sensorConfiguration, lidarConfiguration)
+                : Utils::CreateComponent<ROS2LidarSensorComponent>(entity, sensorConfiguration, lidarConfiguration);
+            if (lidarComponent)
             {
                 return AZ::Success();
             }