Some improvements to lidar: (#88)

* Some improvements to lidar:
- Lidar scene is no longer the default, but it is the scene of entity which contains the lidar component. This is more correct.
- Renamed self detection entity to lidar transparent entity since self detection has a broader meaning in robotics and could cause confusion.
- Moved base class Activate to the end of Lidar Activate since we don't want to subscribe to Tick Bus before all is set.
- Added some documentation to new API

* Separate out code and default to DefaultPhysicsScene
if entity scene is invalid

Signed-off-by: Adam Dąbrowski <[email protected]>

Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp

@@ -11,13 +11,25 @@
 #include <AzCore/std/smart_ptr/make_shared.h>
 #include <AzFramework/Physics/Common/PhysicsSceneQueries.h>
 #include <AzFramework/Physics/PhysicsScene.h>
+#include <AzFramework/Physics/PhysicsSystem.h>
 
 namespace ROS2
 {
+    void LidarRaycaster::SetRaycasterScene(const AzPhysics::SceneHandle& handle)
+    {
+        m_sceneHandle = handle;
+    }
+
     // A simplified, non-optimized first version. TODO - generalize results (fields)
     AZStd::vector<AZ::Vector3> LidarRaycaster::PerformRaycast(const AZ::Vector3& start, const AZStd::vector<AZ::Vector3>& directions, float distance)
     {
         AZStd::vector<AZ::Vector3> results;
+        if (m_sceneHandle == AzPhysics::InvalidSceneHandle)
+        {
+            AZ_Warning("LidarRaycaster", false, "No valid scene handle");
+            return results;
+        }
+
         AzPhysics::SceneQueryRequests requests;
         requests.reserve(directions.size());
         for (const AZ::Vector3& direction : directions)
@@ -27,27 +39,23 @@ namespace ROS2
             request->m_direction = direction;
             request->m_distance = distance;
             request->m_reportMultipleHits = false;
-            request->m_filterCallback = [selfCollider = m_selfColliderEntityId](const AzPhysics::SimulatedBody* simBody, const Physics::Shape *) {
-                if (simBody->GetEntityId() == selfCollider)
-                {
-                    return AzPhysics::SceneQuery::QueryHitType::None;
-                } 
-                else 
-                {
-                    return AzPhysics::SceneQuery::QueryHitType::Block;
-                }
-            };
+            request->m_filterCallback = [transparentEntityId = m_lidarTransparentEntityId]
+                    (const AzPhysics::SimulatedBody* simBody, const Physics::Shape*)
+                    {
+                        if (simBody->GetEntityId() == transparentEntityId)
+                        {
+                            return AzPhysics::SceneQuery::QueryHitType::None;
+                        }
+                        else
+                        {
+                            return AzPhysics::SceneQuery::QueryHitType::Block;
+                        }
+                    };
             requests.emplace_back(AZStd::move(request));
         }
 
         auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
-        AzPhysics::SceneHandle sceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
-        if (sceneHandle == AzPhysics::InvalidSceneHandle)
-        {
-            AZ_Warning("LidarRaycaster", false, "No valid scene handle");
-            return results;
-        }
-        auto requestResults = sceneInterface->QuerySceneBatch(sceneHandle, requests);
+        auto requestResults = sceneInterface->QuerySceneBatch(m_sceneHandle, requests);
         for (const auto& requestResult : requestResults)
         {   // TODO - check flag for SceneQuery::ResultFlags::Position
             if (requestResult.m_hits.size() > 0)
@@ -58,7 +66,8 @@ namespace ROS2
         return results;
     }
 
-    void LidarRaycaster::setSelfColliderEntity(const AZ::EntityId &selfColliderEntity) {
-        m_selfColliderEntityId = selfColliderEntity;
+    void LidarRaycaster::setLidarTransparentEntity(const AZ::EntityId& lidarTransparentEntityId)
+    {
+        m_lidarTransparentEntityId = lidarTransparentEntityId;
     }
-} // namespace ROS2
+} // namespace ROS2

Gems/ROS2/Code/Source/Lidar/LidarRaycaster.h

@@ -10,6 +10,7 @@
 #include <AzCore/Component/EntityId.h>
 #include <AzCore/Math/Vector3.h>
 #include <AzCore/std/containers/vector.h>
+#include <AzFramework/Physics/PhysicsScene.h>
 
 // TODO - switch to interface
 namespace ROS2
@@ -18,6 +19,15 @@ namespace ROS2
     class LidarRaycaster
     {
     public:
+
+        //! Set the Scene for the ray-casting.
+        //! This should be the scene with the Entity that holds the sensor.
+        //! @code
+        //! auto sceneHandle = AZ::RPI::Scene::GetSceneForEntityId(GetEntityId());
+        //! @endcode
+        //! @param scene that will be subject to ray-casting.
+        void SetRaycasterScene(const AzPhysics::SceneHandle& handle);
+
         //! Perform raycast against the current scene.
         //! @param start Starting point of rays. This is a simplification since there can be multiple starting points
         //! in real sensors.
@@ -29,9 +39,17 @@ namespace ROS2
         // TODO - customized settings. Encapsulate in lidar definition and pass in constructor, update transform.
         AZStd::vector<AZ::Vector3> PerformRaycast(const AZ::Vector3& start, const AZStd::vector<AZ::Vector3>& directions, float distance);
 
-        void setSelfColliderEntity(const AZ::EntityId &selfColliderEntity);
+        //! Set the lidar entity to use to filter out this lidar's collider(s) from ray-casting scene.
+        //! For example, if a lidar prefab contains a physical model with a collider, we would like ray-casts to
+        //! ignore it completely. This is a simplification due to lack of handling of transparency (e.g. lidar front glass).
+        //! @param lidarTransparentEntity entity to ignore in ray-casts. Note that all colliders within the entity will be ignored.
+        //! @note in robotics, self-detection (or ego-detection) is means detecting any part of the robot.
+        //! As such, it should be simulated to match the real data (which will also initially include self-detection).
+        void setLidarTransparentEntity(const AZ::EntityId& lidarTransparentEntityId);
 
     private:
-        AZ::EntityId m_selfColliderEntityId;
+        AzPhysics::SceneHandle m_sceneHandle;
+        AZ::EntityId m_lidarTransparentEntityId;
     };
 }  // namespace ROS2
+

Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp

@@ -17,6 +17,7 @@
 #include <Atom/RPI.Public/AuxGeom/AuxGeomFeatureProcessorInterface.h>
 #include <Atom/RPI.Public/RPISystemInterface.h>
 #include <Atom/RPI.Public/Scene.h>
+#include <AzFramework/Physics/PhysicsSystem.h>
 
 namespace ROS2
 {
@@ -33,7 +34,7 @@ namespace ROS2
             serialize->Class<ROS2LidarSensorComponent, ROS2SensorComponent>()
                 ->Version(1)
                 ->Field("lidarModel", &ROS2LidarSensorComponent::m_lidarModel)
-                ->Field("selfCollisionEntityId", &ROS2LidarSensorComponent::m_selfColliderEntityId)
+                ->Field("LidarTransparentEntityId", &ROS2LidarSensorComponent::m_lidarTransparentEntityId)
                 ;
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
@@ -44,8 +45,9 @@ namespace ROS2
                     ->DataElement(AZ::Edit::UIHandlers::ComboBox, &ROS2LidarSensorComponent::m_lidarModel, "Lidar Model", "Lidar model")
                         ->EnumAttribute(LidarTemplate::LidarModel::Generic3DLidar, "Generic Lidar")
                         // TODO - show lidar template field values (read only) - see Reflect for LidarTemplate
-                    ->DataElement(AZ::Edit::UIHandlers::EntityId, &ROS2LidarSensorComponent::m_selfColliderEntityId, "Self collision Entity",
-                                  "Entity to be transparent for the lidar. If not set, use this entity")
+                    ->DataElement(AZ::Edit::UIHandlers::EntityId, &ROS2LidarSensorComponent::m_lidarTransparentEntityId,
+                                  "Lidar-transparent Entity",
+                                  "Entity to be transparent for the lidar. If not set, use this component's entity")
                     ;
             }
         }
@@ -82,9 +84,23 @@ namespace ROS2
         }
     }
 
+    void ROS2LidarSensorComponent::SetPhysicsScene()
+    {
+        auto* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get();
+        auto foundBody = physicsSystem->FindAttachedBodyHandleFromEntityId(GetEntityId());
+        auto lidarPhysicsSceneHandle = foundBody.first;
+        if (foundBody.first == AzPhysics::InvalidSceneHandle)
+        {
+            auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
+            lidarPhysicsSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
+        }
+
+        AZ_Assert(lidarPhysicsSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid physics scene handle for entity");
+        m_lidarRaycaster.SetRaycasterScene(lidarPhysicsSceneHandle);
+    }
+
     void ROS2LidarSensorComponent::Activate()
     {
-        ROS2SensorComponent::Activate();
         auto ros2Node = ROS2Interface::Get()->GetNode();
         AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for lidar sensor");
 
@@ -92,20 +108,22 @@ namespace ROS2
         AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
         m_pointCloudPublisher = ros2Node->create_publisher<sensor_msgs::msg::PointCloud2>(fullTopic.data(), publisherConfig.GetQoS());
 
+        SetPhysicsScene();
         if (m_sensorConfiguration.m_visualise)
         {
-            auto defaultScene = AZ::RPI::Scene::GetSceneForEntityId(GetEntityId());
-            m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(defaultScene);
+            auto* entityScene = AZ::RPI::Scene::GetSceneForEntityId(GetEntityId());
+            m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene);
         }
 
-        if (m_selfColliderEntityId.IsValid())
+        if (m_lidarTransparentEntityId.IsValid())
         {
-            m_lidarRaycaster.setSelfColliderEntity(m_selfColliderEntityId);
+            m_lidarRaycaster.setLidarTransparentEntity(m_lidarTransparentEntityId);
         } 
         else 
         {
-            m_lidarRaycaster.setSelfColliderEntity(GetEntityId());
+            m_lidarRaycaster.setLidarTransparentEntity(GetEntityId());
         }
+        ROS2SensorComponent::Activate();
     }
 
     void ROS2LidarSensorComponent::Deactivate()

Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h

@@ -36,6 +36,7 @@ namespace ROS2
     private:
         void FrequencyTick() override;
         void Visualise() override;
+        void SetPhysicsScene();
 
         LidarTemplate::LidarModel m_lidarModel = LidarTemplate::Generic3DLidar;
         LidarRaycaster m_lidarRaycaster;
@@ -47,7 +48,7 @@ namespace ROS2
 
         AZStd::vector<AZ::Vector3> m_lastScanResults;
 
-        // EntityId for self collision filter
-        AZ::EntityId m_selfColliderEntityId;
+        // EntityId to ignore in lidar simulation (e.g. do not detect lidar own physical collider)
+        AZ::EntityId m_lidarTransparentEntityId;
     };
 }  // namespace ROS2