Added the ability for lidars to ignore multiple layers (#430)

Added the ability for lidars to ignore multiple layers

---------

Signed-off-by: Antoni Puch <[email protected]>

Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h

@@ -152,10 +152,9 @@ namespace ROS2
             AZ_Assert(false, "This Lidar Implementation does not support noise!");
         }
 
-        //! Configures Layer ignoring parameters
-        //! @param ignoreLayer Should a specified collision layer be ignored?
-        //! @param layerIndex Index of collision layer to be ignored.
-        virtual void ConfigureLayerIgnoring([[maybe_unused]] bool ignoreLayer, [[maybe_unused]] AZ::u32 layerIndex)
+        //! Configures which collision layers should be ignored.
+        //! @param layerIndices Indices of collision layers to be ignored.
+        virtual void ConfigureIgnoredCollisionLayers([[maybe_unused]] const AZStd::unordered_set<AZ::u32>& layerIndices)
         {
             AZ_Assert(false, "This Lidar Implementation does not support collision layer configurations!");
         }

Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.cpp

@@ -21,8 +21,7 @@ namespace ROS2
                 ->Field("lidarModel", &Lidar2DSensorConfiguration::m_lidarModel)
                 ->Field("lidarImplementation", &Lidar2DSensorConfiguration::m_lidarSystem)
                 ->Field("LidarParameters", &Lidar2DSensorConfiguration::m_lidarParameters)
-                ->Field("IgnoreLayer", &Lidar2DSensorConfiguration::m_ignoreLayer)
-                ->Field("IgnoredLayerIndex", &Lidar2DSensorConfiguration::m_ignoredLayerIndex)
+                ->Field("IgnoredLayerIndices", &Lidar2DSensorConfiguration::m_ignoredCollisionLayers)
                 ->Field("ExcludedEntities", &Lidar2DSensorConfiguration::m_excludedEntities)
                 ->Field("PointsAtMax", &Lidar2DSensorConfiguration::m_addPointsAtMax);
 
@@ -46,17 +45,11 @@ namespace ROS2
                         "Lidar parameters",
                         "Configuration of Custom lidar")
                     ->Attribute(AZ::Edit::Attributes::Visibility, &Lidar2DSensorConfiguration::IsConfigurationVisible)
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::ComboBox,
-                        &Lidar2DSensorConfiguration::m_ignoreLayer,
-                        "Ignore layer",
-                        "Should we ignore selected layer index")
-                    ->Attribute(AZ::Edit::Attributes::Visibility, &Lidar2DSensorConfiguration::IsIgnoredLayerConfigurationVisible)
                     ->DataElement(
                         AZ::Edit::UIHandlers::Default,
-                        &Lidar2DSensorConfiguration::m_ignoredLayerIndex,
-                        "Ignored layer index",
-                        "Layer index to ignore")
+                        &Lidar2DSensorConfiguration::m_ignoredCollisionLayers,
+                        "Ignored collision layers",
+                        "Indices of collision layers to ignore")
                     ->Attribute(AZ::Edit::Attributes::Visibility, &Lidar2DSensorConfiguration::IsIgnoredLayerConfigurationVisible)
                     ->DataElement(
                         AZ::Edit::UIHandlers::Default,

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

@@ -33,8 +33,7 @@ namespace ROS2
         LidarTemplate::LidarModel m_lidarModel = LidarTemplate::LidarModel::Custom2DLidar;
         LidarTemplate m_lidarParameters = LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel::Custom2DLidar);
 
-        bool m_ignoreLayer = false;
-        AZ::u32 m_ignoredLayerIndex = 0;
+        AZStd::unordered_set<AZ::u32> m_ignoredCollisionLayers;
         AZStd::vector<AZ::EntityId> m_excludedEntities;
 
         bool m_addPointsAtMax = false;

Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp

@@ -48,8 +48,7 @@ namespace ROS2
         , m_range{ lidarRaycaster.m_range }
         , m_addMaxRangePoints{ lidarRaycaster.m_addMaxRangePoints }
         , m_rayRotations{ AZStd::move(lidarRaycaster.m_rayRotations) }
-        , m_ignoreLayer{ lidarRaycaster.m_ignoreLayer }
-        , m_ignoredLayerIndex{ lidarRaycaster.m_ignoredLayerIndex }
+        , m_ignoredCollisionLayers{ lidarRaycaster.m_ignoredCollisionLayers }
     {
         lidarRaycaster.BusDisconnect();
         lidarRaycaster.m_busId = LidarId::CreateNull();
@@ -99,18 +98,17 @@ namespace ROS2
             request->m_direction = direction;
             request->m_distance = m_range;
             request->m_reportMultipleHits = false;
-            request->m_filterCallback = [ignoredLayerIndex = this->m_ignoredLayerIndex, ignoreLayer = this->m_ignoreLayer](
-                                            const AzPhysics::SimulatedBody* simBody, const Physics::Shape* shape)
+
+            request->m_filterCallback =
+                [ignoredCollisionLayers = this->m_ignoredCollisionLayers](const AzPhysics::SimulatedBody* simBody, const Physics::Shape* shape)
             {
-                if (ignoreLayer && (shape->GetCollisionLayer().GetIndex() == ignoredLayerIndex))
+                if (ignoredCollisionLayers.contains(shape->GetCollisionLayer().GetIndex()))
                 {
                     return AzPhysics::SceneQuery::QueryHitType::None;
                 }
-                else
-                {
-                    return AzPhysics::SceneQuery::QueryHitType::Block;
-                }
+                return AzPhysics::SceneQuery::QueryHitType::Block;
             };
+
             requests.emplace_back(AZStd::move(request));
         }
         return requests;
@@ -127,7 +125,6 @@ namespace ROS2
         }
 
         const AZStd::vector<AZ::Vector3> rayDirections = LidarTemplateUtils::RotationsToDirections(m_rayRotations, lidarTransform);
-
         AzPhysics::SceneQueryRequests requests = prepareRequests(lidarTransform, rayDirections);
 
         RaycastResult results;
@@ -179,10 +176,9 @@ namespace ROS2
         return results;
     }
 
-    void LidarRaycaster::ConfigureLayerIgnoring(bool ignoreLayer, AZ::u32 layerIndex)
+    void LidarRaycaster::ConfigureIgnoredCollisionLayers(const AZStd::unordered_set<AZ::u32>& layerIndices)
     {
-        m_ignoreLayer = ignoreLayer;
-        m_ignoredLayerIndex = layerIndex;
+        m_ignoredCollisionLayers = layerIndices;
     }
     void LidarRaycaster::ConfigureMaxRangePointAddition(bool addMaxRangePoints)
     {

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

@@ -33,7 +33,7 @@ namespace ROS2
 
         RaycastResult PerformRaycast(const AZ::Transform& lidarTransform) override;
 
-        void ConfigureLayerIgnoring(bool ignoreLayer, AZ::u32 layerIndex) override;
+        void ConfigureIgnoredCollisionLayers(const AZStd::unordered_set<AZ::u32>& layerIndices) override;
         void ConfigureMaxRangePointAddition(bool addMaxRangePoints) override;
 
     private:
@@ -50,7 +50,6 @@ namespace ROS2
         bool m_addMaxRangePoints{ false };
         AZStd::vector<AZ::Vector3> m_rayRotations{ { AZ::Vector3::CreateZero() } };
 
-        bool m_ignoreLayer{ false };
-        AZ::u32 m_ignoredLayerIndex{ 0 };
+        AZStd::unordered_set<AZ::u32> m_ignoredCollisionLayers;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp

@@ -21,8 +21,7 @@ namespace ROS2
                 ->Field("lidarModel", &LidarSensorConfiguration::m_lidarModel)
                 ->Field("lidarImplementation", &LidarSensorConfiguration::m_lidarSystem)
                 ->Field("LidarParameters", &LidarSensorConfiguration::m_lidarParameters)
-                ->Field("IgnoreLayer", &LidarSensorConfiguration::m_ignoreLayer)
-                ->Field("IgnoredLayerIndex", &LidarSensorConfiguration::m_ignoredLayerIndex)
+                ->Field("IgnoredLayerIndices", &LidarSensorConfiguration::m_ignoredCollisionLayers)
                 ->Field("ExcludedEntities", &LidarSensorConfiguration::m_excludedEntities)
                 ->Field("PointsAtMax", &LidarSensorConfiguration::m_addPointsAtMax);
 
@@ -50,17 +49,11 @@ namespace ROS2
                         "Lidar parameters",
                         "Configuration of Custom lidar")
                     ->Attribute(AZ::Edit::Attributes::Visibility, &LidarSensorConfiguration::IsConfigurationVisible)
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::ComboBox,
-                        &LidarSensorConfiguration::m_ignoreLayer,
-                        "Ignore layer",
-                        "Should we ignore selected layer index")
-                    ->Attribute(AZ::Edit::Attributes::Visibility, &LidarSensorConfiguration::IsIgnoredLayerConfigurationVisible)
                     ->DataElement(
                         AZ::Edit::UIHandlers::Default,
-                        &LidarSensorConfiguration::m_ignoredLayerIndex,
-                        "Ignored layer index",
-                        "Layer index to ignore")
+                        &LidarSensorConfiguration::m_ignoredCollisionLayers,
+                        "Ignored collision layers",
+                        "Indices of collision layers to ignore")
                     ->Attribute(AZ::Edit::Attributes::Visibility, &LidarSensorConfiguration::IsIgnoredLayerConfigurationVisible)
                     ->DataElement(
                         AZ::Edit::UIHandlers::Default,

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

@@ -33,8 +33,7 @@ namespace ROS2
         LidarTemplate::LidarModel m_lidarModel = LidarTemplate::LidarModel::Custom3DLidar;
         LidarTemplate m_lidarParameters = LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel::Custom3DLidar);
 
-        bool m_ignoreLayer = false;
-        AZ::u32 m_ignoredLayerIndex = 0;
+        AZStd::unordered_set<AZ::u32> m_ignoredCollisionLayers;
         AZStd::vector<AZ::EntityId> m_excludedEntities;
 
         bool m_addPointsAtMax = false;

Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp

@@ -96,10 +96,7 @@ namespace ROS2
         if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers)
         {
             LidarRaycasterRequestBus::Event(
-                m_lidarRaycasterId,
-                &LidarRaycasterRequestBus::Events::ConfigureLayerIgnoring,
-                m_lidarConfiguration.m_ignoreLayer,
-                m_lidarConfiguration.m_ignoredLayerIndex);
+                m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureIgnoredCollisionLayers, m_lidarConfiguration.m_ignoredCollisionLayers);
         }
 
         if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion)

Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp

@@ -91,10 +91,7 @@ namespace ROS2
         if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers)
         {
             LidarRaycasterRequestBus::Event(
-                m_lidarRaycasterId,
-                &LidarRaycasterRequestBus::Events::ConfigureLayerIgnoring,
-                m_lidarConfiguration.m_ignoreLayer,
-                m_lidarConfiguration.m_ignoredLayerIndex);
+                m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureIgnoredCollisionLayers, m_lidarConfiguration.m_ignoredCollisionLayers);
         }
 
         if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion)