now segmentation is optional

o3de · Jun 17, 2024 · c46a1f0 · c46a1f0
1 parent 548ffd4
commit c46a1f0
Show file tree

Hide file tree

Showing 3 changed files with 38 additions and 20 deletions.
diff --git a/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp
@@ -14,6 +14,11 @@
 
 namespace ROS2
 {
+    const AZStd::string LidarSensorConfiguration::unknownClassName = "unknown";
+    const AZ::Color LidarSensorConfiguration::unknownClassDefaultColor = AZ::Color(1.0f, 1.0f, 1.0f, 1.0f);
+    const AZStd::string LidarSensorConfiguration::groundClassName = "Ground";
+    const AZ::Color LidarSensorConfiguration::groundClassDefaultColor = AZ::Color(0.5f, 0.25f, 0.0f, 1.0f);
+
     void LidarSensorConfiguration::Reflect(AZ::ReflectContext* context)
     {
         LidarSegmentationClassConfiguration::Reflect(context);
@@ -27,6 +32,7 @@ namespace ROS2
                 ->Field("IgnoredLayerIndices", &LidarSensorConfiguration::m_ignoredCollisionLayers)
                 ->Field("ExcludedEntities", &LidarSensorConfiguration::m_excludedEntities)
                 ->Field("SegmentationClasses", &LidarSensorConfiguration::m_segmentationClasses)
+                ->Field("IsSegmentationEnabled", &LidarSensorConfiguration::m_isSegmentationEnabled)
                 ->Field("PointsAtMax", &LidarSensorConfiguration::m_addPointsAtMax);
 
             if (AZ::EditContext* ec = serializeContext->GetEditContext())
@@ -62,6 +68,12 @@ namespace ROS2
                     ->Attribute(AZ::Edit::Attributes::AutoExpand, true)
                     ->Attribute(AZ::Edit::Attributes::ContainerCanBeModified, true)
                     ->Attribute(AZ::Edit::Attributes::Visibility, &LidarSensorConfiguration::IsEntityExclusionVisible)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &LidarSensorConfiguration::m_isSegmentationEnabled,
+                        "Enable Segmentation",
+                        "Enable segmentation of point cloud")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, &LidarSensorConfiguration::IsSegmentationConfigurationVisible)
                     ->DataElement(
                         AZ::Edit::UIHandlers::Default,
                         &LidarSensorConfiguration::m_segmentationClasses,
@@ -74,7 +86,8 @@ namespace ROS2
                     ->Attribute(
                         AZ::Edit::Attributes::DefaultAsset,
                         AZStd::vector<LidarSegmentationClassConfiguration>(
-                            { { "unknown", 0, AZ::Colors::White }, { "Ground", 1, AZ::Colors::Brown } }))
+                        { { unknownClassName, unknownClassId, unknownClassDefaultColor },
+                          { groundClassName, groundClassId, groundClassDefaultColor } }))
                     ->Attribute(AZ::Edit::Attributes::ChangeNotify, &LidarSensorConfiguration::SegmentationClassesChangeNotify)
                     ->DataElement(
                         AZ::Edit::UIHandlers::Default,
@@ -91,35 +104,35 @@ namespace ROS2
         bool unknown_present = false;
         bool ground_present = false;
         bool changed = false;
-        for (size_t i = 0; i < m_segmentationClasses.size(); i++)
+        for (auto& m_segmentationClasse : m_segmentationClasses)
         {
-            if (m_segmentationClasses[i].m_className == "unknown")
+            if (m_segmentationClasse.m_className == unknownClassName)
             {
-                if (m_segmentationClasses[i].m_classId != 0)
+                if (m_segmentationClasse.m_classId != unknownClassId)
                 {
-                    m_segmentationClasses[i].m_classId = 0;
+                    m_segmentationClasse.m_classId = unknownClassId;
                     changed = true;
                 }
 
                 unknown_present = true;
             }
-            if (m_segmentationClasses[i].m_className == "Ground")
+            if (m_segmentationClasse.m_className == groundClassName)
             {
-                if (m_segmentationClasses[i].m_classId != 1)
+                if (m_segmentationClasse.m_classId != groundClassId)
                 {
-                    m_segmentationClasses[i].m_classId = 1;
+                    m_segmentationClasse.m_classId = groundClassId;
                     changed = true;
                 }
                 ground_present = true;
             }
         }
         if (!unknown_present)
         {
-            m_segmentationClasses.push_back({ "unknown", 0, AZ::Colors::White });
+            m_segmentationClasses.push_back({ unknownClassName, unknownClassId, unknownClassDefaultColor });
         }
         if (!ground_present)
         {
-            m_segmentationClasses.push_back({ "Ground", 1, AZ::Colors::Brown });
+            m_segmentationClasses.push_back({ groundClassName, groundClassId, groundClassDefaultColor });
         }
         if (changed)
         {

diff --git a/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.h b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.h
@@ -16,7 +16,6 @@
 #include "LidarTemplate.h"
 #include "LidarTemplateUtils.h"
 #include "LidarSegmentationClassConfiguration.h"
-#include "LmbrCentral/Scripting/TagComponentBus.h"
 
 namespace ROS2
 {
@@ -31,6 +30,15 @@ namespace ROS2
 
         //! Update the lidar system features based on the current lidar system selected.
         void FetchLidarImplementationFeatures();
+        static constexpr size_t maxClass = 256;
+        [[nodiscard]] AZStd::array<AZ::Color, maxClass> GenerateSegmentationColorsLookupTable() const;
+
+        static constexpr size_t unknownClassId = 0;
+        static const AZStd::string unknownClassName;
+        static const AZ::Color unknownClassDefaultColor;
+        static constexpr size_t groundClassId = 1;
+        static const AZStd::string groundClassName;
+        static const AZ::Color groundClassDefaultColor;
 
         LidarSystemFeatures m_lidarSystemFeatures;
 
@@ -42,11 +50,7 @@ namespace ROS2
         AZStd::vector<AZ::EntityId> m_excludedEntities;
 
         AZStd::vector<LidarSegmentationClassConfiguration> m_segmentationClasses;
-
-        static constexpr size_t maxClass = 256;
-
-        [[nodiscard]] AZStd::array<AZ::Color, maxClass> GenerateSegmentationColorsLookupTable() const;
-
+        bool m_isSegmentationEnabled = false;
         bool m_addPointsAtMax = false;
 
     private:

diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp
@@ -100,7 +100,7 @@ namespace ROS2
             const TopicConfiguration& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[PointCloudType];
             AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
             m_pointCloudPublisher = ros2Node->create_publisher<sensor_msgs::msg::PointCloud2>(fullTopic.data(), publisherConfig.GetQoS());
-            if (m_lidarCore.m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Segmentation) {
+            if (m_lidarCore.m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Segmentation && m_lidarCore.m_lidarConfiguration.m_isSegmentationEnabled) {
                  m_segmentationClassesPublisher = ros2Node->create_publisher<vision_msgs::msg::LabelInfo>(
                     ROS2Names::GetNamespacedName(GetNamespace(), "segmentation_classes").data(),
                     publisherConfig.GetQoS());
@@ -192,7 +192,8 @@ namespace ROS2
             message.point_step += fieldLength;
         }
         AZStd::array<AZ::Color, LidarSensorConfiguration::maxClass> colorLookupTable;
-        if (lastScanResults.m_classes.has_value())
+        const bool publishSegmentation = lastScanResults.m_classes.has_value() && m_lidarCore.m_lidarConfiguration.m_isSegmentationEnabled;
+        if (publishSegmentation)
         {
             colorLookupTable = m_lidarCore.m_lidarConfiguration.GenerateSegmentationColorsLookupTable();
 
@@ -240,7 +241,7 @@ namespace ROS2
                 nextFieldOffset += sizeof(int32_t);
             }
 
-            if (lastScanResults.m_classes.has_value())
+            if (publishSegmentation)
             {
                 const AZ::Color color = colorLookupTable[lastScanResults.m_classes.value()[i]];
                 for (int j = 0; j < 3; j++)
@@ -258,7 +259,7 @@ namespace ROS2
 
         m_pointCloudPublisher->publish(message);
 
-        if (m_segmentationClassesPublisher)
+        if (publishSegmentation && m_segmentationClassesPublisher)
         {
             vision_msgs::msg::LabelInfo segmentationClasses;
             for (const auto&segmentation_class : m_lidarCore.m_lidarConfiguration.m_segmentationClasses)