Switch to map for publishers, fixes #27

Signed-off-by: Adam Dąbrowski <[email protected]>
RobotecAI · Jan 4, 2023 · 163b34c · 163b34c
1 parent f513870
commit 163b34c
Show file tree

Hide file tree

Showing 5 changed files with 27 additions and 23 deletions.
diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp
@@ -18,6 +18,11 @@
 
 namespace ROS2
 {
+    namespace Internal
+    {
+        const char* kPointCloudType = "sensor_msgs::msg::PointCloud2";
+    }
+
     void ROS2LidarSensorComponent::Reflect(AZ::ReflectContext* context)
     {
         if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
@@ -40,11 +45,13 @@ namespace ROS2
 
     ROS2LidarSensorComponent::ROS2LidarSensorComponent()
     {   // TODO - replace with EditorComponent behavior
-        PublisherConfiguration pc;
-        pc.m_type = "sensor_msgs::msg::PointCloud2";
-        pc.m_topic = "pc";
+        auto pc = AZStd::make_shared<PublisherConfiguration>();
+        auto type = Internal::kPointCloudType;
+        pc->m_type = type;
+        pc->m_topic = "pc";
         m_sensorConfiguration.m_frequency = 10; // TODO - dependent on lidar type
-        m_sensorConfiguration.m_publishersConfigurations = { pc };
+
+        m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(type, pc));
     }
 
     void ROS2LidarSensorComponent::Activate()
@@ -53,9 +60,9 @@ namespace ROS2
         auto ros2Node = ROS2Interface::Get()->GetNode();
         AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for lidar sensor");
 
-        const auto& publisherConfig = m_sensorConfiguration.m_publishersConfigurations.front();
-        AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
-        m_pointCloudPublisher = ros2Node->create_publisher<sensor_msgs::msg::PointCloud2>(fullTopic.data(), publisherConfig.m_qos);
+        const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kPointCloudType];
+        AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig->m_topic);
+        m_pointCloudPublisher = ros2Node->create_publisher<sensor_msgs::msg::PointCloud2>(fullTopic.data(), publisherConfig->m_qos);
     }
 
     void ROS2LidarSensorComponent::Deactivate()

diff --git a/Gems/ROS2/Code/Source/Sensor/PublisherConfiguration.cpp b/Gems/ROS2/Code/Source/Sensor/PublisherConfiguration.cpp
@@ -19,7 +19,6 @@ namespace ROS2
     {
         if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
         {
-            serializeContext->RegisterGenericType<AZStd::vector<PublisherConfiguration>>();
             serializeContext->Class<PublisherConfiguration>()
                 ->Version(1)
                 ->Field("Type", &PublisherConfiguration::m_type)
@@ -29,8 +28,10 @@ namespace ROS2
 
             if (AZ::EditContext* ec = serializeContext->GetEditContext())
             {
-                ec->Class<PublisherConfiguration>("ROS2 Publisher configuration", "Publisher configuration")
+                ec->Class<PublisherConfiguration>("Publisher configuration", "")
                     ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->DataElement(AZ::Edit::UIHandlers::Default, &PublisherConfiguration::m_type, "Type", "Type of topic messages")
+                        ->Attribute(AZ::Edit::Attributes::ReadOnly, true)
                     ->DataElement(AZ::Edit::UIHandlers::Default, &PublisherConfiguration::m_topic, "Topic", "Topic with no namespace")
                     ;
             }

diff --git a/Gems/ROS2/Code/Source/Sensor/SensorConfiguration.cpp b/Gems/ROS2/Code/Source/Sensor/SensorConfiguration.cpp
@@ -19,8 +19,10 @@ namespace ROS2
         PublisherConfiguration::Reflect(context);
         if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
         {
+            serializeContext->RegisterGenericType<AZStd::shared_ptr<PublisherConfiguration>>();
+            serializeContext->RegisterGenericType<AZStd::map<AZStd::string, AZStd::shared_ptr<PublisherConfiguration>>>();
             serializeContext->Class<SensorConfiguration>()
-                ->Version(1)
+                ->Version(2)
                 ->Field("Visualise", &SensorConfiguration::m_visualise)
                 ->Field("Publishing Enabled", &SensorConfiguration::m_publishingEnabled)
                 ->Field("Frequency (HZ)", &SensorConfiguration::m_frequency)
@@ -35,16 +37,12 @@ namespace ROS2
                     ->DataElement(AZ::Edit::UIHandlers::Default, &SensorConfiguration::m_publishingEnabled, "Publishing Enabled", "Toggle publishing for topic")
                     ->DataElement(AZ::Edit::UIHandlers::Default, &SensorConfiguration::m_frequency, "Frequency", "Frequency of publishing")
                     ->DataElement(AZ::Edit::UIHandlers::Default, &SensorConfiguration::m_publishersConfigurations, "Publishers", "Publishers")
-                        ->Attribute(AZ::Edit::Attributes::ContainerCanBeModified, false)
                         ->Attribute(AZ::Edit::Attributes::AutoExpand, true)
-                        ->Attribute(AZ::Edit::Attributes::IndexedChildNameLabelOverride, &SensorConfiguration::GetPublisherLabel)
+                        ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly)
+                        ->Attribute(AZ::Edit::Attributes::ContainerCanBeModified, false)
+                        ->ElementAttribute(AZ::Edit::Attributes::AutoExpand, true)
                     ;
             }
         }
     }
-
-    AZStd::string SensorConfiguration::GetPublisherLabel(int index) const
-    {
-        return m_publishersConfigurations[index].m_type;
-    }
 }  // namespace ROS2
diff --git a/Gems/ROS2/Code/Source/Sensor/SensorConfiguration.h b/Gems/ROS2/Code/Source/Sensor/SensorConfiguration.h
@@ -8,10 +8,11 @@
 
 #pragma once
 
+#include <AzCore/Memory/SystemAllocator.h>
 #include <AzCore/RTTI/RTTI.h>
 #include <AzCore/Serialization/SerializeContext.h>
 #include <AzCore/std/smart_ptr/shared_ptr.h>
-#include <AzCore/std/containers/vector.h>
+#include <AzCore/std/containers/map.h>
 #include "PublisherConfiguration.h"
 
 namespace ROS2
@@ -20,22 +21,20 @@ namespace ROS2
     struct SensorConfiguration
     {
     public:
+        AZ_CLASS_ALLOCATOR(SensorConfiguration, AZ::SystemAllocator, 0);
         AZ_RTTI(SensorConfiguration, "{4755363D-0B5A-42D7-BBEF-152D87BA10D7}");
         SensorConfiguration() = default;
         virtual ~SensorConfiguration() = default;
         static void Reflect(AZ::ReflectContext* context);
 
         // Will typically be 1-3 elements (3 max for known sensors).
-        AZStd::vector<PublisherConfiguration> m_publishersConfigurations;
+        AZStd::map<AZStd::string, AZStd::shared_ptr<PublisherConfiguration>> m_publishersConfigurations;
 
         // TODO - consider moving frequency, publishingEnabled to publisherConfiguration if any sensor has
         // a couple of publishers for which we want different values of these fields
         float m_frequency = 10;
         bool m_publishingEnabled = true;
         bool m_visualise = true;
-
-    private:
-        AZStd::string GetPublisherLabel(int index) const;
     };
 }  // namespace ROS2
 
diff --git a/Gems/ROS2/Code/Source/Utilities/ROS2Names.cpp b/Gems/ROS2/Code/Source/Utilities/ROS2Names.cpp
@@ -16,7 +16,6 @@ namespace ROS2
         if (ns.empty())
         {
             return name;
-
         }
 
         return AZStd::string::format("%s/%s", ns.c_str(), name.c_str());;