Skip to content

Commit e68e9b8

Browse files
alek-kam-robotec-aialek-kam-robotec-ai
committed
Remove Raycaster-side publishing and introduce various fixes
Signed-off-by: Aleksander Kamiński <[email protected]>
1 parent c49f3fa commit e68e9b8

File tree

6 files changed

+28
-140
lines changed

6 files changed

+28
-140
lines changed

‎Code/Source/Lidar/LidarRaycaster.cpp‎

Lines changed: 3 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -90,12 +90,10 @@ namespace RGL
9090

9191
void LidarRaycaster::ConfigureRaycastResultFlags(ROS2::RaycastResultFlags flags)
9292
{
93-
m_rglRaycastResults.m_fields.clear();
94-
m_rglRaycastResults.m_xyz.clear();
95-
m_rglRaycastResults.m_distance.clear();
96-
93+
m_rglRaycastResults = {};
9794
m_raycastResults = ROS2::RaycastResults(flags);
9895

96+
m_rglRaycastResults.m_fields = { RGL_FIELD_IS_HIT_I32 };
9997
if (ROS2::IsFlagEnabled(ROS2::RaycastResultFlags::Point, flags))
10098
{
10199
m_rglRaycastResults.m_fields.push_back(RGL_FIELD_XYZ_VEC3_F32);
@@ -116,10 +114,9 @@ namespace RGL
116114
m_rglRaycastResults.m_fields.push_back(RGL_FIELD_ENTITY_ID_I32);
117115
}
118116

119-
m_graph.ConfigureYieldNodes(m_rglRaycastResults.m_fields.data(), m_rglRaycastResults.m_fields.size());
117+
m_graph.ConfigureFieldNodes(m_rglRaycastResults.m_fields.data(), m_rglRaycastResults.m_fields.size());
120118

121119
m_graph.SetIsCompactEnabled(ShouldEnableCompact());
122-
m_graph.SetIsPcPublishingEnabled(ShouldEnablePcPublishing());
123120
}
124121

125122
AZ::Outcome<ROS2::RaycastResults, const char*> LidarRaycaster::PerformRaycast(const AZ::Transform& lidarTransform)
@@ -131,11 +128,6 @@ namespace RGL
131128
const AZ::Matrix3x4 lidarPose = AZ::Matrix3x4::CreateFromTransform(lidarTransform);
132129

133130
m_graph.ConfigureLidarTransformNode(lidarPose);
134-
if (m_graph.IsPcPublishingEnabled())
135-
{
136-
// Transforms the obtained point-cloud from world to sensor frame of reference.
137-
m_graph.ConfigurePcTransformNode(lidarPose.GetInverseFull());
138-
}
139131

140132
m_graph.Run();
141133

@@ -219,19 +211,6 @@ namespace RGL
219211

220212
// We need to configure if points should be compacted to minimize the CPU operations when retrieving raycast results.
221213
m_graph.SetIsCompactEnabled(ShouldEnableCompact());
222-
m_graph.SetIsPcPublishingEnabled(ShouldEnablePcPublishing());
223-
}
224-
225-
void LidarRaycaster::ConfigurePointCloudPublisher(
226-
const AZStd::string& topicName, const AZStd::string& frameId, const ROS2::QoS& qosPolicy)
227-
{
228-
m_graph.ConfigurePcPublisherNode(topicName, frameId, qosPolicy);
229-
m_graph.SetIsPcPublishingEnabled(ShouldEnablePcPublishing());
230-
}
231-
232-
bool LidarRaycaster::CanHandlePublishing()
233-
{
234-
return m_graph.IsPcPublishingEnabled();
235214
}
236215

237216
AZStd::optional<size_t> LidarRaycaster::GetRglResultsSize(
@@ -284,19 +263,8 @@ namespace RGL
284263
return resultsSize;
285264
}
286265

287-
void LidarRaycaster::UpdatePublisherTimestamp(AZ::u64 timestampNanoseconds)
288-
{
289-
RGL_CHECK(rgl_scene_set_time(nullptr, timestampNanoseconds));
290-
}
291-
292266
bool LidarRaycaster::ShouldEnableCompact() const
293267
{
294268
return !m_raycastResults->IsFieldPresent<ROS2::RaycastResultFlags::Range>() && !m_isMaxRangeEnabled;
295269
}
296-
297-
bool LidarRaycaster::ShouldEnablePcPublishing() const
298-
{
299-
return m_graph.IsPublisherConfigured() && !m_raycastResults->IsFieldPresent<ROS2::RaycastResultFlags::Range>() &&
300-
!m_isMaxRangeEnabled;
301-
}
302270
} // namespace RGL

‎Code/Source/Lidar/LidarRaycaster.h‎

Lines changed: 0 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,6 @@ namespace RGL
3434
void ConfigureRayOrientations(const AZStd::vector<AZ::Vector3>& orientations) override;
3535
void ConfigureRayRange(ROS2::RayRange range) override;
3636
void ConfigureRaycastResultFlags(ROS2::RaycastResultFlags flags) override;
37-
bool CanHandlePublishing() override;
3837

3938
// Returns the size of required results.
4039
// If obtained result sizes do not match,
@@ -52,13 +51,6 @@ namespace RGL
5251
void UpdateNonHitValues();
5352
void ConfigureMaxRangePointAddition(bool addMaxRangePoints) override;
5453

55-
void ConfigurePointCloudPublisher(
56-
[[maybe_unused]] const AZStd::string& topicName,
57-
[[maybe_unused]] const AZStd::string& frameId,
58-
[[maybe_unused]] const ROS2::QoS& qosPolicy) override;
59-
60-
void UpdatePublisherTimestamp([[maybe_unused]] AZ::u64 timestampNanoseconds) override;
61-
6254
private:
6355
AZ::Uuid m_uuid;
6456

@@ -73,6 +65,5 @@ namespace RGL
7365
PipelineGraph m_graph;
7466

7567
[[nodiscard]] bool ShouldEnableCompact() const;
76-
[[nodiscard]] bool ShouldEnablePcPublishing() const;
7768
};
7869
} // namespace RGL

‎Code/Source/Lidar/LidarSystem.cpp‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -30,8 +30,7 @@ namespace RGL
3030

3131
using Features = ROS2::LidarSystemFeatures;
3232
static constexpr auto SupportedFeatures = aznumeric_cast<Features>(
33-
Features::EntityExclusion | Features::MaxRangePoints | Features::Noise | Features::PointcloudPublishing | Features::Intensity |
34-
Features::Segmentation);
33+
Features::EntityExclusion | Features::MaxRangePoints | Features::Noise | Features::Intensity | Features::Segmentation);
3534

3635
ROS2::LidarSystemRequestBus::Handler::BusConnect(AZ_CRC(name));
3736

‎Code/Source/Lidar/PipelineGraph.cpp‎

Lines changed: 19 additions & 76 deletions
Original file line numberDiff line numberDiff line change
@@ -27,16 +27,11 @@ namespace RGL
2727
RGL_CHECK(rgl_node_points_compact_by_field(&m_nodes.m_pointsCompact, RGL_FIELD_IS_HIT_I32));
2828
ConfigureAngularNoiseNode(0.0f);
2929
ConfigureDistanceNoiseNode(0.0f, 0.0f);
30-
ConfigureYieldNodes(DefaultFields.data(), DefaultFields.size());
31-
32-
ConfigurePcTransformNode(AZ::Matrix3x4::CreateIdentity());
33-
RGL_CHECK(rgl_node_points_format(&m_nodes.m_pcPublishFormat, DefaultFields.data(), aznumeric_cast<int32_t>(DefaultFields.size())));
30+
ConfigureFieldNodes(DefaultFields.data(), DefaultFields.size());
3431

3532
// Non-conditional connections
3633
RGL_CHECK(rgl_graph_node_add_child(m_nodes.m_rayPoses, m_nodes.m_rayRanges));
3734
RGL_CHECK(rgl_graph_node_add_child(m_nodes.m_rayRanges, m_nodes.m_lidarTransform));
38-
RGL_CHECK(rgl_graph_node_add_child(m_nodes.m_compactYield, m_nodes.m_pointsYield));
39-
RGL_CHECK(rgl_graph_node_add_child(m_nodes.m_pointCloudTransform, m_nodes.m_pcPublishFormat));
4035

4136
InitializeConditionalConnections();
4237
}
@@ -61,14 +56,6 @@ namespace RGL
6156
// one (or two) rgl_graph_destroy API call(s).
6257
SetIsNoiseEnabled(true);
6358
SetIsCompactEnabled(true);
64-
if (IsPublisherConfigured())
65-
{
66-
SetIsPcPublishingEnabled(true);
67-
}
68-
else
69-
{
70-
rgl_graph_destroy(m_nodes.m_pointCloudTransform);
71-
}
7259

7360
rgl_graph_destroy(m_nodes.m_rayPoses);
7461
}
@@ -77,10 +64,7 @@ namespace RGL
7764
{
7865
return IsFeatureEnabled(PipelineFeatureFlags::PointsCompact);
7966
}
80-
bool PipelineGraph::IsPcPublishingEnabled() const
81-
{
82-
return IsFeatureEnabled(PipelineFeatureFlags::PointCloudPublishing);
83-
}
67+
8468
bool PipelineGraph::IsNoiseEnabled() const
8569
{
8670
return IsFeatureEnabled(PipelineFeatureFlags::Noise);
@@ -97,11 +81,10 @@ namespace RGL
9781
RGL_CHECK(rgl_node_rays_set_range(&m_nodes.m_rayRanges, &range, 1));
9882
}
9983

100-
void PipelineGraph::ConfigureYieldNodes(const rgl_field_t* fields, size_t size)
84+
void PipelineGraph::ConfigureFieldNodes(const rgl_field_t* fields, size_t size)
10185
{
10286
RGL_CHECK(rgl_node_points_yield(&m_nodes.m_pointsYield, fields, aznumeric_cast<int32_t>(size)));
10387
RGL_CHECK(rgl_node_points_yield(&m_nodes.m_rayTraceYield, fields, aznumeric_cast<int32_t>(size)));
104-
RGL_CHECK(rgl_node_points_yield(&m_nodes.m_compactYield, fields, aznumeric_cast<int32_t>(size)));
10588
}
10689

10790
void PipelineGraph::ConfigureLidarTransformNode(const AZ::Matrix3x4& lidarTransform)
@@ -110,12 +93,6 @@ namespace RGL
11093
RGL_CHECK(rgl_node_rays_transform(&m_nodes.m_lidarTransform, &RglLidarTransform));
11194
}
11295

113-
void PipelineGraph::ConfigurePcTransformNode(const AZ::Matrix3x4& pcTransform)
114-
{
115-
const rgl_mat3x4f rglPcTransform = Utils::RglMat3x4FromAzMatrix3x4(pcTransform);
116-
RGL_CHECK(rgl_node_points_transform(&m_nodes.m_pointCloudTransform, &rglPcTransform));
117-
}
118-
11996
void PipelineGraph::ConfigureAngularNoiseNode(float angularNoiseStdDev)
12097
{
12198
RGL_CHECK(rgl_node_gaussian_noise_angular_ray(&m_nodes.m_angularNoise, 0.0f, angularNoiseStdDev, RGL_AXIS_Z));
@@ -127,27 +104,6 @@ namespace RGL
127104
rgl_node_gaussian_noise_distance(&m_nodes.m_distanceNoise, 0.0f, distanceNoiseStdDevBase, distanceNoiseStdDevRisePerMeter));
128105
}
129106

130-
void PipelineGraph::ConfigurePcPublisherNode(const AZStd::string& topicName, const AZStd::string& frameId, const ROS2::QoS& qosPolicy)
131-
{
132-
const bool FirstConfiguration = !IsPublisherConfigured();
133-
134-
RGL_CHECK(rgl_node_points_ros2_publish_with_qos(
135-
&m_nodes.m_pointCloudPublish,
136-
topicName.c_str(),
137-
frameId.c_str(),
138-
static_cast<rgl_qos_policy_reliability_t>(static_cast<int>(qosPolicy.GetQoS().reliability())),
139-
static_cast<rgl_qos_policy_durability_t>(static_cast<int>(qosPolicy.GetQoS().durability())),
140-
static_cast<rgl_qos_policy_history_t>(static_cast<int>(qosPolicy.GetQoS().history())),
141-
qosPolicy.GetQoS().depth()));
142-
143-
if (FirstConfiguration)
144-
{
145-
// clang-format off
146-
AddConditionalConnection(m_nodes.m_pcPublishFormat, m_nodes.m_pointCloudPublish, [](const PipelineGraph& graph){ return graph.IsPcPublishingEnabled(); });
147-
// clang-format on
148-
}
149-
}
150-
151107
void PipelineGraph::ConfigureRaytraceNodeNonHits(float minRangeNonHitValue, float maxRangeNonHitValue)
152108
{
153109
RGL_CHECK(rgl_node_raytrace_configure_non_hits(m_nodes.m_rayTrace, minRangeNonHitValue, maxRangeNonHitValue));
@@ -158,16 +114,6 @@ namespace RGL
158114
SetIsFeatureEnabled(PipelineFeatureFlags::PointsCompact, value);
159115
}
160116

161-
void PipelineGraph::SetIsPcPublishingEnabled(bool value)
162-
{
163-
if (value && !IsPublisherConfigured())
164-
{
165-
AZ_Assert(false, "Trying to enable publishing without the publisher node configured.");
166-
return;
167-
}
168-
SetIsFeatureEnabled(PipelineFeatureFlags::PointCloudPublishing, value);
169-
}
170-
171117
void PipelineGraph::SetIsNoiseEnabled(bool value)
172118
{
173119
SetIsFeatureEnabled(PipelineFeatureFlags::Noise, value);
@@ -197,9 +143,16 @@ namespace RGL
197143
case RGL_FIELD_ENTITY_ID_I32:
198144
success = success && GetResult(results.m_packedRglEntityId, RGL_FIELD_ENTITY_ID_I32);
199145
break;
146+
case RGL_FIELD_IS_HIT_I32: // Not needed.
147+
continue;
200148
default:
201149
success = false;
202-
AZ_Assert(false, "Invalid result field type!");
150+
AZ_Assert(false, AZStd::string::format("Invalid result field type with RGL id %i!", field).c_str());
151+
break;
152+
}
153+
154+
if (!success)
155+
{
203156
break;
204157
}
205158
}
@@ -238,23 +191,9 @@ namespace RGL
238191
return graph.IsCompactEnabled();
239192
};
240193

241-
const ConditionType PublishingCondition = [](const PipelineGraph& graph)
242-
{
243-
return graph.IsPcPublishingEnabled();
244-
};
245-
246-
// clang-format off
247194
AddConditionalNode(m_nodes.m_angularNoise, m_nodes.m_lidarTransform, m_nodes.m_rayTrace, NoiseCondition);
248195
AddConditionalNode(m_nodes.m_distanceNoise, m_nodes.m_rayTrace, m_nodes.m_rayTraceYield, NoiseCondition);
249-
AddConditionalNode(m_nodes.m_pointsCompact, m_nodes.m_rayTraceYield, m_nodes.m_compactYield, CompactCondition);
250-
AddConditionalConnection(m_nodes.m_compactYield, m_nodes.m_pointCloudTransform, PublishingCondition);
251-
// clang-format on
252-
if (IsPublisherConfigured())
253-
{
254-
// clang-format off
255-
AddConditionalConnection( m_nodes.m_pcPublishFormat, m_nodes.m_pointCloudPublish, PublishingCondition);
256-
// clang-format on
257-
}
196+
AddConditionalNode(m_nodes.m_pointsCompact, m_nodes.m_rayTraceYield, m_nodes.m_pointsYield, CompactCondition);
258197
}
259198

260199
void PipelineGraph::UpdateConnections()
@@ -269,9 +208,13 @@ namespace RGL
269208
{
270209
AddConditionalConnection(parent, node, condition);
271210
AddConditionalConnection(node, child, condition);
272-
// clang-format off
273-
AddConditionalConnection(parent, child, [condition](const PipelineGraph& pipelineGraph){ return !condition(pipelineGraph); });
274-
// clang-format on
211+
AddConditionalConnection(
212+
parent,
213+
child,
214+
[condition](const PipelineGraph& pipelineGraph)
215+
{
216+
return !condition(pipelineGraph);
217+
});
275218
}
276219

277220
void PipelineGraph::AddConditionalConnection(rgl_node_t parent, rgl_node_t child, const ConditionType& condition)

‎Code/Source/Lidar/PipelineGraph.h‎

Lines changed: 4 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -28,12 +28,12 @@ namespace RGL
2828
class PipelineGraph
2929
{
3030
private:
31-
static constexpr AZStd::array DefaultFields{ RGL_FIELD_IS_HIT_I32, RGL_FIELD_XYZ_VEC3_F32, RGL_FIELD_INTENSITY_F32 };
31+
static constexpr AZStd::array DefaultFields{ RGL_FIELD_XYZ_VEC3_F32 };
3232

3333
public:
3434
struct RaycastResults
3535
{
36-
AZStd::vector<rgl_field_t> m_fields{ DefaultFields.data(), DefaultFields.data() + DefaultFields.size() };
36+
AZStd::vector<rgl_field_t> m_fields;
3737
AZStd::vector<rgl_vec3f> m_xyz;
3838
AZStd::vector<float> m_distance;
3939
AZStd::vector<float> m_intensity;
@@ -44,8 +44,7 @@ namespace RGL
4444
{
4545
rgl_node_t m_rayPoses{ nullptr }, m_rayRanges{ nullptr }, m_lidarTransform{ nullptr }, m_angularNoise{ nullptr },
4646
m_rayTrace{ nullptr }, m_distanceNoise{ nullptr }, m_rayTraceYield{ nullptr }, m_pointsCompact{ nullptr },
47-
m_compactYield{ nullptr }, m_pointsYield{ nullptr }, m_pointCloudTransform{ nullptr }, m_pcPublishFormat{ nullptr },
48-
m_pointCloudPublish{ nullptr };
47+
m_pointsYield{ nullptr };
4948
};
5049

5150
PipelineGraph();
@@ -54,25 +53,17 @@ namespace RGL
5453
~PipelineGraph();
5554

5655
[[nodiscard]] bool IsCompactEnabled() const;
57-
[[nodiscard]] bool IsPcPublishingEnabled() const;
5856
[[nodiscard]] bool IsNoiseEnabled() const;
59-
[[nodiscard]] bool IsPublisherConfigured() const
60-
{
61-
return m_nodes.m_pointCloudPublish;
62-
}
6357

6458
void ConfigureRayPosesNode(const AZStd::vector<rgl_mat3x4f>& rayPoses);
6559
void ConfigureRayRangesNode(float min, float max);
66-
void ConfigureYieldNodes(const rgl_field_t* fields, size_t size);
60+
void ConfigureFieldNodes(const rgl_field_t* fields, size_t size);
6761
void ConfigureLidarTransformNode(const AZ::Matrix3x4& lidarTransform);
68-
void ConfigurePcTransformNode(const AZ::Matrix3x4& pcTransform);
6962
void ConfigureAngularNoiseNode(float angularNoiseStdDev);
7063
void ConfigureDistanceNoiseNode(float distanceNoiseStdDevBase, float distanceNoiseStdDevRisePerMeter);
71-
void ConfigurePcPublisherNode(const AZStd::string& topicName, const AZStd::string& frameId, const ROS2::QoS& qosPolicy);
7264
void ConfigureRaytraceNodeNonHits(float minRangeNonHitValue, float maxRangeNonHitValue);
7365

7466
void SetIsCompactEnabled(bool value);
75-
void SetIsPcPublishingEnabled(bool value);
7667
void SetIsNoiseEnabled(bool value);
7768

7869
void Run();

‎static/PipelineGraph.mmd‎

Lines changed: 1 addition & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -8,9 +8,5 @@ flowchart TD
88
RT -->|Noise disabled| DNY[Yield Node]
99
DN --> DNY
1010
DNY -->|Compact enabled| PC[Points Compact]
11-
DNY -->|Compact disabled| PCY[Yield Node]
11+
DNY -->|Compact disabled| PCY[Points Yield]
1212
PC --> PCY
13-
PCY --> PY[Points Yield]
14-
PCY -->|Publishing enabled| PT[Points Transform]
15-
PT --> PF2[Points Format]
16-
PF2 --> PCP[Point Cloud Publish]

0 commit comments

Comments
 (0)