Skip to content

Commit 4a42901

Browse files
adamdbrwadamdbrw
committed
Supporting multiple publisher (WIP)
Signed-off-by: Adam Dąbrowski <[email protected]>
1 parent f92cea0 commit 4a42901

9 files changed

+130
-28
lines changed

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

Lines changed: 17 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
#include "Lidar/ROS2LidarSensorComponent.h"
1010
#include "Frame/ROS2FrameComponent.h"
1111
#include "ROS2/ROS2Bus.h"
12+
#include "Utilities/ROS2Names.h"
1213

1314
#include <AzCore/Component/Entity.h>
1415
#include <AzCore/Serialization/SerializeContext.h>
@@ -43,8 +44,11 @@ namespace ROS2
4344
auto ros2Node = ROS2Interface::Get()->GetNode();
4445
auto config = GetConfiguration();
4546

46-
// TODO - also use QoS
47-
m_pointCloudPublisher = ros2Node->create_publisher<sensor_msgs::msg::PointCloud2>(GetFullTopic().data(), 10);
47+
AZ_Assert(config.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for lidar sensor");
48+
49+
const auto& publisherConfig = config.m_publishersConfigurations.front();
50+
AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
51+
m_pointCloudPublisher = ros2Node->create_publisher<sensor_msgs::msg::PointCloud2>(fullTopic.data(), publisherConfig.m_qos);
4852
}
4953

5054
void ROS2LidarSensorComponent::Deactivate()
@@ -95,4 +99,15 @@ namespace ROS2
9599

96100
m_pointCloudPublisher->publish(message);
97101
}
102+
103+
SensorConfiguration ROS2LidarSensorComponent::DefaultConfiguration() const
104+
{
105+
SensorConfiguration sc;
106+
PublisherConfiguration pc;
107+
pc.m_type = "sensor_msgs::msg::PointCloud2";
108+
pc.m_topic = "pc";
109+
sc.m_frequency = 10; // TODO - dependent on lidar type
110+
sc.m_publishersConfigurations = { pc };
111+
return sc;
112+
}
98113
} // namespace ROS2

‎Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ namespace ROS2
2929

3030
private:
3131
void FrequencyTick() override;
32+
SensorConfiguration DefaultConfiguration() const override;
3233

3334
LidarTemplate::LidarModel m_lidarModel = LidarTemplate::SickMRS6000;
3435
LidarRaycaster m_lidarRaycaster;

‎Gems/ROS2/Code/Source/Sensor/PublisherConfiguration.cpp‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
/*
2+
* Copyright (c) Contributors to the Open 3D Engine Project.
3+
* For complete copyright and license terms please see the LICENSE at the root of this distribution.
4+
*
5+
* SPDX-License-Identifier: Apache-2.0 OR MIT
6+
*
7+
*/
8+
9+
#pragma once
10+
11+
#include "PublisherConfiguration.h"
12+
#include <AzCore/Serialization/EditContext.h>
13+
#include <AzCore/Serialization/SerializeContext.h>
14+
15+
namespace ROS2
16+
{
17+
/// A struct used to create and configure publishers
18+
void PublisherConfiguration::Reflect(AZ::ReflectContext* context)
19+
{
20+
if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
21+
{
22+
serializeContext->Class<PublisherConfiguration>()
23+
->Version(1)
24+
->Field("Type", &PublisherConfiguration::m_type)
25+
->Field("Topic", &PublisherConfiguration::m_topic)
26+
// TODO - also enable QoS
27+
;
28+
29+
if (AZ::EditContext* ec = serializeContext->GetEditContext())
30+
{
31+
ec->Class<PublisherConfiguration>("ROS2 Publisher configuration", "Publisher configuration")
32+
->DataElement(AZ::Edit::UIHandlers::Default, &PublisherConfiguration::m_type, "Type", "Type of topic")
33+
->Attribute(AZ::Edit::Attributes::ReadOnly, true)
34+
->DataElement(AZ::Edit::UIHandlers::Default, &PublisherConfiguration::m_topic, "Topic", "Topic with no namespace")
35+
;
36+
}
37+
}
38+
};
39+
} // namespace ROS2
40+

‎Gems/ROS2/Code/Source/Sensor/PublisherConfiguration.h‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
/*
2+
* Copyright (c) Contributors to the Open 3D Engine Project.
3+
* For complete copyright and license terms please see the LICENSE at the root of this distribution.
4+
*
5+
* SPDX-License-Identifier: Apache-2.0 OR MIT
6+
*
7+
*/
8+
9+
#pragma once
10+
11+
#include <AzCore/RTTI/RTTI.h>
12+
#include <AzCore/Serialization/SerializeContext.h>
13+
#include <AzCore/std/string/string.h>
14+
#include <rclcpp/qos.hpp>
15+
16+
namespace ROS2
17+
{
18+
/// A struct used to create and configure publishers
19+
struct PublisherConfiguration
20+
{
21+
public:
22+
AZ_RTTI(PublisherConfiguration, "{{7F875348-F2F9-404A-841E-D9A749EA4E79}}");
23+
PublisherConfiguration() = default;
24+
virtual ~PublisherConfiguration() = default;
25+
static void Reflect(AZ::ReflectContext* context);
26+
27+
AZStd::string m_type = "std_msgs::msg::Empty";
28+
AZStd::string m_topic = "default_topic";
29+
rclcpp::QoS m_qos = 10;
30+
};
31+
} // namespace ROS2
32+

‎Gems/ROS2/Code/Source/Sensor/ROS2SensorComponent.cpp‎

Lines changed: 13 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,11 @@
1414

1515
namespace ROS2
1616
{
17+
void ROS2SensorComponent::Init()
18+
{
19+
m_sensorConfiguration = DefaultConfiguration();
20+
}
21+
1722
void ROS2SensorComponent::Activate()
1823
{
1924
AZ::TickBus::Handler::BusConnect();
@@ -57,11 +62,6 @@ namespace ROS2
5762
return ros2Frame->GetNamespace();
5863
};
5964

60-
AZStd::string ROS2SensorComponent::GetFullTopic() const
61-
{
62-
return ROS2Names::GetNamespacedName(GetNamespace(), m_sensorConfiguration.m_topic);
63-
}
64-
6565
AZStd::string ROS2SensorComponent::GetFrameID() const
6666
{
6767
auto ros2Frame = GetEntity()->FindComponent<ROS2FrameComponent>();
@@ -99,4 +99,12 @@ namespace ROS2
9999
// Note that sensor frequency can be limited by simulation tick rate (if higher sensor Hz is desired).
100100
FrequencyTick();
101101
}
102+
103+
SensorConfiguration ROS2SensorComponent::DefaultConfiguration() const
104+
{
105+
SensorConfiguration sc;
106+
PublisherConfiguration pc;
107+
sc.m_publishersConfigurations = { pc };
108+
return sc;
109+
}
102110
} // namespace ROS2

‎Gems/ROS2/Code/Source/Sensor/ROS2SensorComponent.h‎

Lines changed: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@ namespace ROS2
2525
AZ_COMPONENT(ROS2SensorComponent, "{91BCC1E9-6D93-4466-9CDB-E73D497C6B5E}", AZ::Component);
2626

2727
// AZ::Component interface implementation.
28+
void Init() override;
2829
void Activate() override;
2930
void Deactivate() override;
3031

@@ -34,15 +35,12 @@ namespace ROS2
3435
static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
3536

3637
protected:
37-
// Getters which include namespaces
38-
AZStd::string GetFullTopic() const; // TODO - support multiple topics per sensor
39-
AZStd::string GetFrameID() const;
40-
38+
AZStd::string GetNamespace() const;
39+
AZStd::string GetFrameID() const; // includes namespace
4140
const SensorConfiguration& GetConfiguration() const;
4241

4342
private:
44-
AZStd::string GetNamespace() const;
45-
43+
virtual SensorConfiguration DefaultConfiguration() const; // Override to provide default sensor config
4644
virtual void FrequencyTick() { }; // Override to implement sensor behavior
4745

4846
// TODO - Editor component: validation of fields, constraints between values and so on

‎Gems/ROS2/Code/Source/Sensor/SensorConfiguration.cpp‎

Lines changed: 12 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -16,26 +16,30 @@ namespace ROS2
1616
{
1717
void SensorConfiguration::Reflect(AZ::ReflectContext* context)
1818
{
19+
PublisherConfiguration::Reflect(context);
1920
if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
2021
{
22+
serializeContext->RegisterGenericType<AZStd::vector<PublisherConfiguration>>();
2123
serializeContext->Class<SensorConfiguration>()
2224
->Version(1)
23-
->Field("Topic", &SensorConfiguration::m_topic)
25+
->Field("Visualise", &SensorConfiguration::m_visualise)
2426
->Field("Publishing Enabled", &SensorConfiguration::m_publishingEnabled)
2527
->Field("Frequency (HZ)", &SensorConfiguration::m_frequency)
26-
->Field("Visualise", &SensorConfiguration::m_visualise)
28+
->Field("Publishers", &SensorConfiguration::m_publishersConfigurations)
2729
;
2830

2931
if (AZ::EditContext* ec = serializeContext->GetEditContext())
3032
{
3133
ec->Class<SensorConfiguration>("ROS2 Sensor Component", "[Base component for sensors]")
32-
->DataElement(AZ::Edit::UIHandlers::Default, &SensorConfiguration::m_topic, "Topic", "Topic")
33-
->DataElement(AZ::Edit::UIHandlers::Default, &SensorConfiguration::m_publishingEnabled, "Publishing Enabled", "Publishing Enabled")
34-
->DataElement(AZ::Edit::UIHandlers::Default, &SensorConfiguration::m_frequency, "Frequency", "Frequency (HZ)")
35-
->DataElement(AZ::Edit::UIHandlers::Default, &SensorConfiguration::m_visualise, "Visualise", "Visualise")
36-
;
34+
->DataElement(AZ::Edit::UIHandlers::Default, &SensorConfiguration::m_visualise, "Visualise", "Visualise")
35+
->DataElement(AZ::Edit::UIHandlers::Default, &SensorConfiguration::m_publishingEnabled, "Publishing Enabled", "Toggle publishing for topic")
36+
->DataElement(AZ::Edit::UIHandlers::Default, &SensorConfiguration::m_frequency, "Frequency", "Frequency of publishing")
37+
->Attribute(AZ::Edit::Attributes::Min, 1)
38+
->Attribute(AZ::Edit::Attributes::Max, 100)
39+
->DataElement(AZ::Edit::UIHandlers::Default, &SensorConfiguration::m_publishersConfigurations, "Publishers", "Publishers")
40+
->Attribute(AZ::Edit::Attributes::ContainerCanBeModified, false)
41+
;
3742
}
3843
}
3944
}
4045
} // namespace ROS2
41-

‎Gems/ROS2/Code/Source/Sensor/SensorConfiguration.h‎

Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,9 @@
1010

1111
#include <AzCore/RTTI/RTTI.h>
1212
#include <AzCore/Serialization/SerializeContext.h>
13+
#include <AzCore/std/smart_ptr/shared_ptr.h>
14+
#include <AzCore/std/containers/vector.h>
15+
#include "PublisherConfiguration.h"
1316

1417
namespace ROS2
1518
{
@@ -18,18 +21,17 @@ namespace ROS2
1821
{
1922
public:
2023
AZ_RTTI(SensorConfiguration, "{4755363D-0B5A-42D7-BBEF-152D87BA10D7}");
21-
2224
SensorConfiguration() = default;
2325
virtual ~SensorConfiguration() = default;
24-
2526
static void Reflect(AZ::ReflectContext* context);
2627

27-
// TODO - publishing-related data
28-
AZStd::string m_topic = "default_topic"; // TODO - apply namespace, default to standard names per type, validation
29-
bool m_publishingEnabled = true;
30-
float m_frequency = 10;
31-
// TODO - add QoS here (struct, mapped to ros2 QoS).
28+
// Will typically be 1-3 elements (3 max for known sensors).
29+
AZStd::vector<PublisherConfiguration> m_publishersConfigurations;
3230

31+
// TODO - consider moving frequency, publishingEnabled to publisherConfiguration if any sensor has
32+
// a couple of publishers for which we want different values of these fields
33+
float m_frequency = 10;
34+
bool m_publishingEnabled = true;
3335
bool m_visualise = true;
3436
};
3537
} // namespace ROS2

‎Gems/ROS2/Code/ros2_files.cmake‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,8 @@ set(FILES
1919
Source/ROS2ModuleInterface.h
2020
Source/ROS2SystemComponent.cpp
2121
Source/ROS2SystemComponent.h
22+
Source/Sensor/PublisherConfiguration.cpp
23+
Source/Sensor/PublisherConfiguration.h
2224
Source/Sensor/ROS2SensorComponent.cpp
2325
Source/Sensor/ROS2SensorComponent.h
2426
Source/Sensor/SensorConfiguration.cpp

0 commit comments

Comments
 (0)