diff --git a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h b/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h index 48a723a73..7dfce43ed 100644 --- a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h +++ b/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h @@ -101,6 +101,14 @@ namespace ROS2 AZStd::vector m_intensities; }; + //! Structure used to describe both minimal and maximal + //! ray travel distance in meters. + struct RayRange + { + float m_min{ 0.0f }; + float m_max{ 0.0f }; + }; + //! Interface class that allows for communication with a single Lidar instance. class LidarRaycasterRequests { @@ -112,16 +120,9 @@ namespace ROS2 //! vector in the positive z direction first by the y, next by the z axis. The x axis is currently not included in calculations. virtual void ConfigureRayOrientations(const AZStd::vector& orientations) = 0; - //! Configures ray maximum travel distance. - //! @param range Ray range in meters. - virtual void ConfigureRayRange(float range) = 0; - - //! Configures ray minimum travel distance. - //! @param range Ray range in meters. - virtual void ConfigureMinimumRayRange(float range) - { - AZ_Assert(false, "This Lidar Implementation does not support minimum ray range configurations!"); - } + //! Configures ray range. + //! @param range Ray range. + virtual void ConfigureRayRange(RayRange range) = 0; //! Configures result flags. //! @param flags Raycast result flags define set of data types returned by lidar. @@ -212,11 +213,6 @@ namespace ROS2 protected: ~LidarRaycasterRequests() = default; - static void ValidateRayRange([[maybe_unused]] float range) - { - AZ_Assert(range > 0.0f, "Provided ray range was of incorrect value: Ray range value must be greater than zero.") - } - static void ValidateRayOrientations([[maybe_unused]] const AZStd::vector& orientations) { AZ_Assert(!orientations.empty(), "Provided ray orientations were of incorrect value: Ray orientations must not be empty.") diff --git a/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp b/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp index 07504b8f8..8ed211800 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp @@ -58,10 +58,8 @@ namespace ROS2 LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayOrientations, m_lastRotations); LidarRaycasterRequestBus::Event( m_lidarRaycasterId, - &LidarRaycasterRequestBus::Events::ConfigureMinimumRayRange, - m_lidarConfiguration.m_lidarParameters.m_minRange); - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarConfiguration.m_lidarParameters.m_maxRange); + &LidarRaycasterRequestBus::Events::ConfigureRayRange, + RayRange{ m_lidarConfiguration.m_lidarParameters.m_minRange, m_lidarConfiguration.m_lidarParameters.m_maxRange }); if ((m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Noise) && m_lidarConfiguration.m_lidarParameters.m_isNoiseEnabled) diff --git a/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp b/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp index 0207d3f7b..710d41b89 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp @@ -44,7 +44,6 @@ namespace ROS2 , m_sceneEntityId{ lidarRaycaster.m_sceneEntityId } , m_sceneHandle{ lidarRaycaster.m_sceneHandle } , m_resultFlags{ lidarRaycaster.m_resultFlags } - , m_minRange{ lidarRaycaster.m_minRange } , m_range{ lidarRaycaster.m_range } , m_addMaxRangePoints{ lidarRaycaster.m_addMaxRangePoints } , m_rayRotations{ AZStd::move(lidarRaycaster.m_rayRotations) } @@ -72,17 +71,11 @@ namespace ROS2 } } - void LidarRaycaster::ConfigureRayRange(float range) + void LidarRaycaster::ConfigureRayRange(RayRange range) { - ValidateRayRange(range); m_range = range; } - void LidarRaycaster::ConfigureMinimumRayRange(float range) - { - m_minRange = range; - } - void LidarRaycaster::ConfigureRaycastResultFlags(RaycastResultFlags flags) { m_resultFlags = flags; @@ -101,7 +94,7 @@ namespace ROS2 AZStd::shared_ptr request = AZStd::make_shared(); request->m_start = lidarPosition; request->m_direction = direction; - request->m_distance = m_range; + request->m_distance = m_range->m_max; request->m_reportMultipleHits = false; request->m_filterCallback = [ignoredCollisionLayers = this->m_ignoredCollisionLayers]( @@ -122,7 +115,7 @@ namespace ROS2 RaycastResult LidarRaycaster::PerformRaycast(const AZ::Transform& lidarTransform) { AZ_Assert(!m_rayRotations.empty(), "Ray poses are not configured. Unable to Perform a raycast."); - AZ_Assert(m_range > 0.0f, "Ray range is not configured. Unable to Perform a raycast."); + AZ_Assert(m_range.has_value(), "Ray range is not configured. Unable to Perform a raycast."); if (m_sceneHandle == AzPhysics::InvalidSceneHandle) { @@ -149,13 +142,13 @@ namespace ROS2 AZ_Assert(requestResults.size() == rayDirections.size(), "Request size should be equal to directions size"); const auto localTransform = AZ::Transform::CreateFromQuaternionAndTranslation(lidarTransform.GetRotation(), lidarTransform.GetTranslation()).GetInverse(); - const float maxRange = m_addMaxRangePoints ? m_range : AZStd::numeric_limits::infinity(); + const float maxRange = m_addMaxRangePoints ? m_range->m_max : AZStd::numeric_limits::infinity(); for (int i = 0; i < requestResults.size(); i++) { const auto& requestResult = requestResults[i]; float hitRange = requestResult ? requestResult.m_hits[0].m_distance : maxRange; - if (hitRange < m_minRange) + if (hitRange < m_range->m_min) { hitRange = -AZStd::numeric_limits::infinity(); } diff --git a/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.h b/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.h index 500506f5c..dbec76dc0 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.h +++ b/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.h @@ -28,8 +28,7 @@ namespace ROS2 protected: // LidarRaycasterRequestBus overrides void ConfigureRayOrientations(const AZStd::vector& orientations) override; - void ConfigureRayRange(float range) override; - void ConfigureMinimumRayRange(float range) override; + void ConfigureRayRange(RayRange range) override; void ConfigureRaycastResultFlags(RaycastResultFlags flags) override; RaycastResult PerformRaycast(const AZ::Transform& lidarTransform) override; @@ -46,8 +45,7 @@ namespace ROS2 AzPhysics::SceneHandle m_sceneHandle{ AzPhysics::InvalidSceneHandle }; RaycastResultFlags m_resultFlags{ RaycastResultFlags::Points }; - float m_minRange{ 0.0f }; - float m_range{ 1.0f }; + AZStd::optional m_range{}; bool m_addMaxRangePoints{ false }; AZStd::vector m_rayRotations{ { AZ::Quaternion::CreateZero() } }; diff --git a/Gems/ROS2/Code/Source/Lidar/LidarTemplate.cpp b/Gems/ROS2/Code/Source/Lidar/LidarTemplate.cpp index 1bcd10f28..8d44449fd 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarTemplate.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarTemplate.cpp @@ -98,11 +98,9 @@ namespace ROS2 ->Attribute(AZ::Edit::Attributes::Max, 90.0f) ->Attribute(AZ::Edit::Attributes::Visibility, &LidarTemplate::IsLayersVisible) ->DataElement(AZ::Edit::UIHandlers::Default, &LidarTemplate::m_minRange, "Min range", "Minimum beam range [m]") - ->Attribute(AZ::Edit::Attributes::Min, 0.0f) - ->Attribute(AZ::Edit::Attributes::Max, 1000.0f) + ->Attribute(AZ::Edit::Attributes::ChangeValidate, &LidarTemplate::ValidateMinRange) ->DataElement(AZ::Edit::UIHandlers::Default, &LidarTemplate::m_maxRange, "Max range", "Maximum beam range [m]") - ->Attribute(AZ::Edit::Attributes::Min, 0.001f) - ->Attribute(AZ::Edit::Attributes::Max, 1000.0f) + ->Attribute(AZ::Edit::Attributes::ChangeValidate, &LidarTemplate::ValidateMaxRange) ->DataElement( AZ::Edit::UIHandlers::Default, &LidarTemplate::m_isNoiseEnabled, @@ -124,4 +122,41 @@ namespace ROS2 { return m_showNoiseConfig && m_isNoiseEnabled; } + + AZ::Outcome LidarTemplate::ValidateRange(float minRange, float maxRange) + { + if (0.0f <= minRange && minRange <= maxRange) + { + return AZ::Success(); + } + + return AZ::Failure(AZStd::string::format( + "Provided ray range (%.2f, %.2f) was of incorrect value: Max range must be greater than or equal to min range, and both must be greater " + "than zero.", + minRange, + maxRange)); + } + + AZ::Outcome LidarTemplate::ValidateMinRange(void* newValue, const AZ::TypeId& valueType) const + { + if (azrtti_typeid() != valueType) + { + AZ_Assert(false, "Unexpected value type"); + return AZ::Failure(AZStd::string("Unexpectedly received a non-float type for the min range!")); + } + + return ValidateRange(*reinterpret_cast(newValue), m_maxRange); + } + + AZ::Outcome LidarTemplate::ValidateMaxRange(void* newValue, const AZ::TypeId& valueType) const + { + if (azrtti_typeid() != valueType) + { + AZ_Assert(false, "Unexpected value type"); + return AZ::Failure(AZStd::string("Unexpectedly received a non-float type for the max range!")); + } + + return ValidateRange(m_minRange, *reinterpret_cast(newValue)); + } + } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarTemplate.h b/Gems/ROS2/Code/Source/Lidar/LidarTemplate.h index 867d7df3d..421362382 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarTemplate.h +++ b/Gems/ROS2/Code/Source/Lidar/LidarTemplate.h @@ -81,5 +81,10 @@ namespace ROS2 private: bool IsLayersVisible() const; [[nodiscard]] bool IsNoiseConfigVisible() const; + + static AZ::Outcome ValidateRange(float minRange, float maxRange); + + AZ::Outcome ValidateMinRange(void* newValue, const AZ::TypeId& valueType) const; + AZ::Outcome ValidateMaxRange(void* newValue, const AZ::TypeId& valueType) const; }; } // namespace ROS2