Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[DO NOT MERGE] Simplify result processing #40

Merged
merged 2 commits into from
Aug 14, 2024
Merged

[DO NOT MERGE] Simplify result processing #40

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
dbb932d
Simplify result processing
alek-kam-robotec-ai Aug 7, 2024
3d5afba
Adjust to ROS2 gem changes
alek-kam-robotec-ai Aug 8, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
71 changes: 25 additions & 46 deletions Code/Source/Lidar/LidarRaycaster.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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<float>::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<bool>(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<float>::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;
}

Expand All @@ -222,10 +185,26 @@ namespace RGL
}
}

void LidarRaycaster::UpdateNonHitValues()
{
float minRangeNonHitValue = -AZStd::numeric_limits<float>::infinity();
float maxRangeNonHitValue = AZStd::numeric_limits<float>::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());
Expand Down
6 changes: 3 additions & 3 deletions Code/Source/Lidar/LidarRaycaster.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,7 @@ namespace RGL
protected:
// LidarRaycasterRequestBus overrides
void ConfigureRayOrientations(const AZStd::vector<AZ::Vector3>& 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;

Expand All @@ -44,6 +43,7 @@ namespace RGL
[[maybe_unused]] float distanceNoiseStdDevBase,
[[maybe_unused]] float distanceNoiseStdDevRisePerMeter) override;
void ExcludeEntities(const AZStd::vector<AZ::EntityId>& excludedEntities) override;
void UpdateNonHitValues();
void ConfigureMaxRangePointAddition(bool addMaxRangePoints) override;

void ConfigurePointCloudPublisher(
Expand All @@ -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<float, float> m_range{ 0.0f, 1.0f };
AZStd::optional<ROS2::RayRange> m_range{};
AZStd::vector<AZ::Matrix3x4> m_rayTransforms{ AZ::Matrix3x4::CreateIdentity() };

PipelineGraph::RaycastResults m_rglRaycastResults;
Expand Down
11 changes: 8 additions & 3 deletions Code/Source/Lidar/PipelineGraph.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down Expand Up @@ -91,9 +91,9 @@ namespace RGL
RGL_CHECK(rgl_node_rays_from_mat3x4f(&m_nodes.m_rayPoses, rayPoses.data(), aznumeric_cast<int32_t>(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));
}

Expand Down Expand Up @@ -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);
Expand Down
3 changes: 2 additions & 1 deletion Code/Source/Lidar/PipelineGraph.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,13 +62,14 @@ namespace RGL
}

void ConfigureRayPosesNode(const AZStd::vector<rgl_mat3x4f>& 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);
Expand Down