diff --git a/Code/Source/Lidar/LidarRaycaster.cpp b/Code/Source/Lidar/LidarRaycaster.cpp index 5edc31a..bc0e7cd 100644 --- a/Code/Source/Lidar/LidarRaycaster.cpp +++ b/Code/Source/Lidar/LidarRaycaster.cpp @@ -77,18 +77,13 @@ namespace RGL m_graph.ConfigureRayPosesNode(rglRayTransforms); } - void LidarRaycaster::ConfigureRayRange(float range) + void LidarRaycaster::ConfigureRayRange(ROS2::RayRange range) { - ValidateRayRange(range); - m_range.second = range; - // We set the graph-side value of min range to zero to be able to distinguish rays below min range from the ones above max range. - m_graph.ConfigureRayRangesNode(0.0f, m_range.second); - } + m_range = range; - void LidarRaycaster::ConfigureMinimumRayRange(float range) - { - m_range.first = range; - // We omit updating the graph-side value of min range to be able to distinguish rays below min range from the ones above max range. + UpdateNonHitValues(); + + m_graph.ConfigureRayRangesNode(range.m_min, range.m_max); } void LidarRaycaster::ConfigureRaycastResultFlags(ROS2::RaycastResultFlags flags) @@ -123,6 +118,7 @@ namespace RGL ROS2::RaycastResult LidarRaycaster::PerformRaycast(const AZ::Transform& lidarTransform) { + AZ_Assert(m_range.has_value(), "Programmer error. Raycaster range not configured."); RGLInterface::Get()->UpdateScene(); const AZ::Matrix3x4 lidarPose = AZ::Matrix3x4::CreateFromTransform(lidarTransform); @@ -155,54 +151,21 @@ namespace RGL m_raycastResults.m_ranges.resize(m_rglRaycastResults.m_distance.size()); } - size_t usedPointIndex = 0LU; const size_t resultsSize = pointsExpected ? m_raycastResults.m_points.size() : m_raycastResults.m_ranges.size(); - const float maxRange = m_isMaxRangeEnabled ? m_range.second : AZStd::numeric_limits::infinity(); for (size_t resultIndex = 0LU; resultIndex < resultsSize; ++resultIndex) { + m_raycastResults.m_intensities[resultIndex] = m_rglRaycastResults.m_intensity[resultIndex]; if (pointsExpected) { - const bool isHit = m_graph.IsCompactEnabled() || aznumeric_cast(m_rglRaycastResults.m_isHit[resultIndex]); - if (isHit) - { - m_raycastResults.m_points[usedPointIndex] = Utils::AzVector3FromRglVec3f(m_rglRaycastResults.m_xyz[resultIndex]); - m_raycastResults.m_intensities[usedPointIndex] = m_rglRaycastResults.m_intensity[resultIndex]; - } - else if (m_isMaxRangeEnabled) - { - const AZ::Vector4 maxVector = lidarPose * m_rayTransforms[resultIndex] * AZ::Vector4(0.0f, 0.0f, m_range.second, 1.0f); - m_raycastResults.m_points[usedPointIndex] = maxVector.GetAsVector3(); - } - - if (isHit || m_isMaxRangeEnabled) - { - ++usedPointIndex; - } + m_raycastResults.m_points[resultIndex] = Utils::AzVector3FromRglVec3f(m_rglRaycastResults.m_xyz[resultIndex]); } if (distanceExpected) { - float distance = m_rglRaycastResults.m_distance[resultIndex]; - if (distance < m_range.first) - { - distance = -AZStd::numeric_limits::infinity(); - } - else if (distance > m_range.second) - { - distance = maxRange; - } - - m_raycastResults.m_ranges[resultIndex] = distance; - m_raycastResults.m_intensities[resultIndex] = m_rglRaycastResults.m_intensity[resultIndex]; + m_raycastResults.m_ranges[resultIndex] = m_rglRaycastResults.m_distance[resultIndex]; } } - if (pointsExpected) - { - m_raycastResults.m_points.resize(usedPointIndex); - m_raycastResults.m_intensities.resize(usedPointIndex); - } - return m_raycastResults; } @@ -222,10 +185,26 @@ namespace RGL } } + void LidarRaycaster::UpdateNonHitValues() + { + float minRangeNonHitValue = -AZStd::numeric_limits::infinity(); + float maxRangeNonHitValue = AZStd::numeric_limits::infinity(); + + if (m_isMaxRangeEnabled && m_range.has_value()) + { + minRangeNonHitValue = m_range.value().m_min; + maxRangeNonHitValue = m_range.value().m_max; + } + + m_graph.ConfigureRaytraceNodeNonHits(minRangeNonHitValue, maxRangeNonHitValue); + } + void LidarRaycaster::ConfigureMaxRangePointAddition(bool addMaxRangePoints) { m_isMaxRangeEnabled = addMaxRangePoints; + UpdateNonHitValues(); + // We need to configure if points should be compacted to minimize the CPU operations when retrieving raycast results. m_graph.SetIsCompactEnabled(ShouldEnableCompact()); m_graph.SetIsPcPublishingEnabled(ShouldEnablePcPublishing()); diff --git a/Code/Source/Lidar/LidarRaycaster.h b/Code/Source/Lidar/LidarRaycaster.h index 6de2195..c38b842 100644 --- a/Code/Source/Lidar/LidarRaycaster.h +++ b/Code/Source/Lidar/LidarRaycaster.h @@ -32,8 +32,7 @@ namespace RGL protected: // LidarRaycasterRequestBus overrides void ConfigureRayOrientations(const AZStd::vector& orientations) override; - void ConfigureRayRange(float range) override; - void ConfigureMinimumRayRange(float range) override; + void ConfigureRayRange(ROS2::RayRange range) override; void ConfigureRaycastResultFlags(ROS2::RaycastResultFlags flags) override; bool CanHandlePublishing() override; @@ -44,6 +43,7 @@ namespace RGL [[maybe_unused]] float distanceNoiseStdDevBase, [[maybe_unused]] float distanceNoiseStdDevRisePerMeter) override; void ExcludeEntities(const AZStd::vector& excludedEntities) override; + void UpdateNonHitValues(); void ConfigureMaxRangePointAddition(bool addMaxRangePoints) override; void ConfigurePointCloudPublisher( @@ -59,7 +59,7 @@ namespace RGL bool m_isMaxRangeEnabled{ false }; //!< Determines whether max range point addition is enabled. ROS2::RaycastResultFlags m_resultFlags{ ROS2::RaycastResultFlags::Points }; - AZStd::pair m_range{ 0.0f, 1.0f }; + AZStd::optional m_range{}; AZStd::vector m_rayTransforms{ AZ::Matrix3x4::CreateIdentity() }; PipelineGraph::RaycastResults m_rglRaycastResults; diff --git a/Code/Source/Lidar/PipelineGraph.cpp b/Code/Source/Lidar/PipelineGraph.cpp index 0464487..3e8eacf 100644 --- a/Code/Source/Lidar/PipelineGraph.cpp +++ b/Code/Source/Lidar/PipelineGraph.cpp @@ -20,7 +20,7 @@ namespace RGL { PipelineGraph::PipelineGraph() { - ConfigureRayPosesNode({Utils::IdentityTransform}); + ConfigureRayPosesNode({ Utils::IdentityTransform }); ConfigureRayRangesNode(0.0f, 1.0f); ConfigureLidarTransformNode(AZ::Matrix3x4::CreateIdentity()); RGL_CHECK(rgl_node_raytrace(&m_nodes.m_rayTrace, nullptr)); @@ -91,9 +91,9 @@ namespace RGL RGL_CHECK(rgl_node_rays_from_mat3x4f(&m_nodes.m_rayPoses, rayPoses.data(), aznumeric_cast(rayPoses.size()))); } - void PipelineGraph::ConfigureRayRangesNode(float minRange, float maxRange) + void PipelineGraph::ConfigureRayRangesNode(float min, float max) { - rgl_vec2f range = { .value = { minRange, maxRange } }; + const rgl_vec2f range = { .value = { min, max } }; RGL_CHECK(rgl_node_rays_set_range(&m_nodes.m_rayRanges, &range, 1)); } @@ -148,6 +148,11 @@ namespace RGL } } + void PipelineGraph::ConfigureRaytraceNodeNonHits(float minRangeNonHitValue, float maxRangeNonHitValue) + { + RGL_CHECK(rgl_node_raytrace_configure_non_hits(m_nodes.m_rayTrace, minRangeNonHitValue, maxRangeNonHitValue)); + } + void PipelineGraph::SetIsCompactEnabled(bool value) { SetIsFeatureEnabled(PipelineFeatureFlags::PointsCompact, value); diff --git a/Code/Source/Lidar/PipelineGraph.h b/Code/Source/Lidar/PipelineGraph.h index 17612ae..c7131c0 100644 --- a/Code/Source/Lidar/PipelineGraph.h +++ b/Code/Source/Lidar/PipelineGraph.h @@ -62,13 +62,14 @@ namespace RGL } void ConfigureRayPosesNode(const AZStd::vector& rayPoses); - void ConfigureRayRangesNode(float minRange, float maxRange); + void ConfigureRayRangesNode(float min, float max); void ConfigureYieldNodes(const rgl_field_t* fields, size_t size); void ConfigureLidarTransformNode(const AZ::Matrix3x4& lidarTransform); void ConfigurePcTransformNode(const AZ::Matrix3x4& pcTransform); void ConfigureAngularNoiseNode(float angularNoiseStdDev); void ConfigureDistanceNoiseNode(float distanceNoiseStdDevBase, float distanceNoiseStdDevRisePerMeter); void ConfigurePcPublisherNode(const AZStd::string& topicName, const AZStd::string& frameId, const ROS2::QoS& qosPolicy); + void ConfigureRaytraceNodeNonHits(float minRangeNonHitValue, float maxRangeNonHitValue); void SetIsCompactEnabled(bool value); void SetIsPcPublishingEnabled(bool value);