Simplify and improve lidar range configuration (o3de#744)

Signed-off-by: Aleksander Kamiński <[email protected]> Signed-off-by: Aleksander Kamiński <[email protected]> Co-authored-by: Adam Dąbrowski <[email protected]>
RobotecAI · Aug 22, 2024 · 2e3bac4 · 2e3bac4
1 parent 23e0d7b
commit 2e3bac4
Show file tree

Hide file tree

Showing 6 changed files with 64 additions and 39 deletions.
diff --git 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<float> 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<AZ::Vector3>& 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<AZ::Vector3>& 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
@@ -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
@@ -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) }
@@ -67,17 +66,11 @@ namespace ROS2
         m_rayRotations = orientations;
     }
 
-    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;
@@ -96,7 +89,7 @@ namespace ROS2
             AZStd::shared_ptr<AzPhysics::RayCastRequest> request = AZStd::make_shared<AzPhysics::RayCastRequest>();
             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](
@@ -117,7 +110,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)
         {
@@ -144,13 +137,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<float>::infinity();
+        const float maxRange = m_addMaxRangePoints ? m_range->m_max : AZStd::numeric_limits<float>::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<float>::infinity();
             }

diff --git a/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.h b/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.h
@@ -27,8 +27,7 @@ namespace ROS2
     protected:
         // LidarRaycasterRequestBus overrides
         void ConfigureRayOrientations(const AZStd::vector<AZ::Vector3>& 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;
@@ -45,8 +44,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<RayRange> m_range{};
         bool m_addMaxRangePoints{ false };
         AZStd::vector<AZ::Vector3> m_rayRotations{ { AZ::Vector3::CreateZero() } };
 

diff --git 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<void, AZStd::string> 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<void, AZStd::string> LidarTemplate::ValidateMinRange(void* newValue, const AZ::TypeId& valueType) const
+    {
+        if (azrtti_typeid<float>() != 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<float*>(newValue), m_maxRange);
+    }
+
+    AZ::Outcome<void, AZStd::string> LidarTemplate::ValidateMaxRange(void* newValue, const AZ::TypeId& valueType) const
+    {
+        if (azrtti_typeid<float>() != 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<float*>(newValue));
+    }
+
 } // namespace ROS2
diff --git 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<void, AZStd::string> ValidateRange(float minRange, float maxRange);
+
+        AZ::Outcome<void, AZStd::string> ValidateMinRange(void* newValue, const AZ::TypeId& valueType) const;
+        AZ::Outcome<void, AZStd::string> ValidateMaxRange(void* newValue, const AZ::TypeId& valueType) const;
     };
 } // namespace ROS2