Merge branch 'o3de:development' into development

Author: arturkamieniecki

.gitattributes

@@ -1,4 +1,3 @@
-
 #
 # Git LFS (see https://git-lfs.github.com/)
 #
@@ -130,3 +129,4 @@ Templates/Ros2FleetRobotTemplate/docs/images/*.png filter= diff= merge= -text
 Gems/ROS2/docs/**/*.png -filter -diff -merge
 Templates/Ros2ProjectTemplate/Screenshots/*.png filter= diff= merge= -text
 Templates/Ros2FleetRobotTemplate/Screenshots/*.png filter= diff= merge= -text
+*.pgm filter=lfs diff=lfs merge=lfs -text

Gems/ROS2/Code/CMakeLists.txt

@@ -140,9 +140,6 @@ if(PAL_TRAIT_BUILD_HOST_TOOLS)
                 3rdParty::sdformat
     )
 
-    find_package(urdfdom)
-    target_link_libraries(${gem_name}.Editor.Static PUBLIC urdfdom::urdfdom_model)
-
     ly_add_target(
         NAME ${gem_name}.Editor GEM_MODULE
         NAMESPACE Gem

Gems/ROS2/Code/FindROS2.cmake

@@ -5,9 +5,10 @@
 
 # Note that this does not find any ros2 package in particular, but determines whether a distro is sourced properly
 # Can be extended to handle supported / unsupported distros
+message(STATUS "Ros Distro is \"$ENV{ROS_DISTRO}\"")
 if (NOT DEFINED ENV{ROS_DISTRO} OR NOT DEFINED ENV{AMENT_PREFIX_PATH})
     message(WARNING, "To build ROS2 Gem a ROS distribution needs to be sourced, but none detected")
     set(ROS2_FOUND FALSE)
     return()
 endif()
-set(ROS2_FOUND TRUE)
+set(ROS2_FOUND TRUE)

Gems/ROS2/Code/Include/ROS2/Frame/NamespaceConfiguration.h

@@ -41,6 +41,11 @@ namespace ROS2
         void PopulateNamespace(bool isRoot, const AZStd::string& entityName);
         AZStd::string GetNamespace(const AZStd::string& parentNamespace) const;
 
+        //! Update namespace and strategy.
+        //! @param ns Desired namespace.
+        //! @param strategy Namespace strategy.
+        void SetNamespace(const AZStd::string& ns, NamespaceStrategy strategy);
+
     private:
         AZStd::string m_namespace;
         NamespaceStrategy m_namespaceStrategy = NamespaceStrategy::Default;

Gems/ROS2/Code/Include/ROS2/Frame/ROS2FrameComponent.h

@@ -74,6 +74,11 @@ namespace ROS2
         //! @return The name of the global frame with namespace attached. It is typically "odom", "map", "world".
         AZStd::string GetGlobalFrameName() const;
 
+        //! Updates the namespace and namespace strategy of the underlying namespace configuration
+        //! @param ns Namespace to set.
+        //! @param strategy Namespace strategy to use.
+        void UpdateNamespaceConfiguration(const AZStd::string& ns, NamespaceConfiguration::NamespaceStrategy strategy);
+
     private:
         //////////////////////////////////////////////////////////////////////////
         // AZ::TickBus::Handler overrides

Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h

@@ -152,10 +152,9 @@ namespace ROS2
             AZ_Assert(false, "This Lidar Implementation does not support noise!");
         }
 
-        //! Configures Layer ignoring parameters
-        //! @param ignoreLayer Should a specified collision layer be ignored?
-        //! @param layerIndex Index of collision layer to be ignored.
-        virtual void ConfigureLayerIgnoring([[maybe_unused]] bool ignoreLayer, [[maybe_unused]] AZ::u32 layerIndex)
+        //! Configures which collision layers should be ignored.
+        //! @param layerIndices Indices of collision layers to be ignored.
+        virtual void ConfigureIgnoredCollisionLayers([[maybe_unused]] const AZStd::unordered_set<AZ::u32>& layerIndices)
         {
             AZ_Assert(false, "This Lidar Implementation does not support collision layer configurations!");
         }

Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponent.h

@@ -18,6 +18,7 @@ namespace ROS2
     //! Captures common behavior of ROS2 sensor Components.
     //! Sensors acquire data from the simulation engine and publish it to ROS2 ecosystem.
     //! Derive this Component to implement a new ROS2 sensor. Each sensor Component requires ROS2FrameComponent.
+    //! For high frequency sensors also derive PhysicsCallbackHandler.
     class ROS2SensorComponent
         : public AZ::Component
         , public AZ::TickBus::Handler

Gems/ROS2/Code/Source/Utilities/PhysicsCallbackHandler.h renamed to Gems/ROS2/Code/Include/ROS2/Utilities/PhysicsCallbackHandler.h

@@ -13,31 +13,26 @@
 
 namespace ROS2::Utils
 {
-    //! Helper class that register OnSceneSimulationFinishHandler and retrieve handle to Simulated Body from the Default Scene.
+    //! A class that registers physics tick callbacks from the Default Scene.
     class PhysicsCallbackHandler
     {
     protected:
-        //! Install to default physics scene callbacks, when the scene is created, the @member m_bodyHandle is populated with the handle to
-        //! simulated body.
-        //! @param m_entityId Entity id to get m_bodyHandle to.
-        void InstallPhysicalCallback(AZ::EntityId m_entityId);
+        //! Install default physics scene callbacks
+        void InstallPhysicalCallback();
 
         //! Removes all attached callbacks
         void RemovePhysicalCallback();
 
         //! Called multiple times per frame after every inner loop of physics engine.
-        //! It virtual version of  callback AzPhysics::SceneEvents::OnSceneSimulationFinishHandler.
+        //! It's a virtual version of callback AzPhysics::SceneEvents::OnSceneSimulationFinishHandler.
         //! @param sceneHandle - scene handle, only handle to Default Scene is expected
         //! @param deltaTime - update of simulated time in seconds.
-        virtual void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) {};
+        virtual void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime){};
 
-        //! Callback called on begging of the first physical simulation.
-        //! inner loop of physics engine.
+        //! Callback called on beginning of the first physical simulation inner loop of physics engine.
         //! @param sceneHandle - scene handle, only handle to Default Scene is expected
-        virtual void OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle) {};
+        virtual void OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle){};
 
-        //! Handler to simulated physical body
-        AzPhysics::SimulatedBodyHandle m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle;
     private:
         AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_onSceneSimulationEvent;
         AzPhysics::SceneEvents::OnSceneSimulationStartHandler m_onSceneSimulationStart;

Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp

@@ -133,57 +133,53 @@ namespace ROS2
         const AZ::u32 stateBefore = debugDisplay.GetState();
         AZ::Transform transform = GetEntity()->GetTransform()->GetWorldTM();
 
+        const float distance = m_cameraSensorConfiguration.m_farClipDistance * 0.1f;
+        const float tangent = static_cast<float>(tan(0.5f * AZ::DegToRad(m_cameraSensorConfiguration.m_verticalFieldOfViewDeg)));
+
+        float height = distance * tangent;
+        float width = height * m_cameraSensorConfiguration.m_width / m_cameraSensorConfiguration.m_height;
+
+        AZ::Vector3 farPoints[4];
+        farPoints[0] = AZ::Vector3(width, height, distance);
+        farPoints[1] = AZ::Vector3(-width, height, distance);
+        farPoints[2] = AZ::Vector3(-width, -height, distance);
+        farPoints[3] = AZ::Vector3(width, -height, distance);
+
+        AZ::Vector3 nearPoints[4];
+        nearPoints[0] = farPoints[0].GetNormalizedSafe() * m_cameraSensorConfiguration.m_nearClipDistance;
+        nearPoints[1] = farPoints[1].GetNormalizedSafe() * m_cameraSensorConfiguration.m_nearClipDistance;
+        nearPoints[2] = farPoints[2].GetNormalizedSafe() * m_cameraSensorConfiguration.m_nearClipDistance;
+        nearPoints[3] = farPoints[3].GetNormalizedSafe() * m_cameraSensorConfiguration.m_nearClipDistance;
+
         // dimension of drawing
-        const float arrowRise = 1.1f;
-        const float arrowSize = 0.5f;
-        const float frustumScale = 0.1f;
-
-        transform.SetUniformScale(frustumScale);
-        debugDisplay.DepthTestOff();
-        const float vertical = 0.5f * AZStd::tan((AZ::DegToRad(m_cameraSensorConfiguration.m_verticalFieldOfViewDeg * 0.5f)));
-        const float horizontal = m_cameraSensorConfiguration.m_height * vertical / m_cameraSensorConfiguration.m_width;
-
-        // frustum drawing
-        const AZ::Vector3 p1(-vertical, -horizontal, 1.f);
-        const AZ::Vector3 p2(vertical, -horizontal, 1.f);
-        const AZ::Vector3 p3(vertical, horizontal, 1.f);
-        const AZ::Vector3 p4(-vertical, horizontal, 1.f);
-        const AZ::Vector3 p0(0, 0, 0);
-        const AZ::Vector3 py(0.1, 0, 0);
-
-        const auto pt1 = transform.TransformPoint(p1);
-        const auto pt2 = transform.TransformPoint(p2);
-        const auto pt3 = transform.TransformPoint(p3);
-        const auto pt4 = transform.TransformPoint(p4);
-        const auto pt0 = transform.TransformPoint(p0);
-        const auto ptz = transform.TransformPoint(AZ::Vector3::CreateAxisZ(0.2f));
-        const auto pty = transform.TransformPoint(AZ::Vector3::CreateAxisY(0.2f));
-        const auto ptx = transform.TransformPoint(AZ::Vector3::CreateAxisX(0.2f));
+        const float arrowRise = 0.11f;
+        const float arrowSize = 0.05f;
+
+        const AZ::Vector3 pt0(0, 0, 0);
+        const auto ptz = AZ::Vector3::CreateAxisZ(0.2f);
+        const auto pty = AZ::Vector3::CreateAxisY(0.2f);
+        const auto ptx = AZ::Vector3::CreateAxisX(0.2f);
+
+        debugDisplay.PushMatrix(transform);
 
         debugDisplay.SetColor(AZ::Color(0.f, 1.f, 1.f, 1.f));
-        debugDisplay.SetLineWidth(1);
-        debugDisplay.DrawLine(pt1, pt2);
-        debugDisplay.DrawLine(pt2, pt3);
-        debugDisplay.DrawLine(pt3, pt4);
-        debugDisplay.DrawLine(pt4, pt1);
-        debugDisplay.DrawLine(pt1, pt0);
-        debugDisplay.DrawLine(pt2, pt0);
-        debugDisplay.DrawLine(pt3, pt0);
-        debugDisplay.DrawLine(pt4, pt0);
+        debugDisplay.DrawLine(nearPoints[0], farPoints[0]);
+        debugDisplay.DrawLine(nearPoints[1], farPoints[1]);
+        debugDisplay.DrawLine(nearPoints[2], farPoints[2]);
+        debugDisplay.DrawLine(nearPoints[3], farPoints[3]);
+        debugDisplay.DrawPolyLine(nearPoints, AZ_ARRAY_SIZE(nearPoints));
+        debugDisplay.DrawPolyLine(farPoints, AZ_ARRAY_SIZE(farPoints));
 
         // up-arrow drawing
-        const AZ::Vector3 pa1(-arrowSize * vertical, -arrowRise * horizontal, 1.f);
-        const AZ::Vector3 pa2(arrowSize * vertical, -arrowRise * horizontal, 1.f);
-        const AZ::Vector3 pa3(0, (-arrowRise - arrowSize) * horizontal, 1.f);
-        const auto pat1 = transform.TransformPoint(pa1);
-        const auto pat2 = transform.TransformPoint(pa2);
-        const auto pat3 = transform.TransformPoint(pa3);
+        const AZ::Vector3 pa1(-arrowSize * height, -arrowRise * width, 1.f);
+        const AZ::Vector3 pa2(arrowSize * height, -arrowRise * width, 1.f);
+        const AZ::Vector3 pa3(0, (-arrowRise - arrowSize) * width, 1.f);
 
         debugDisplay.SetColor(AZ::Color(0.f, 0.6f, 1.f, 1.f));
         debugDisplay.SetLineWidth(1);
-        debugDisplay.DrawLine(pat1, pat2);
-        debugDisplay.DrawLine(pat2, pat3);
-        debugDisplay.DrawLine(pat3, pat1);
+        debugDisplay.DrawLine(pa1, pa2);
+        debugDisplay.DrawLine(pa2, pa3);
+        debugDisplay.DrawLine(pa3, pa1);
 
         // coordinate system drawing
         debugDisplay.SetColor(AZ::Color(1.f, 0.f, 0.f, 1.f));
@@ -198,6 +194,8 @@ namespace ROS2
         debugDisplay.SetLineWidth(2);
         debugDisplay.DrawLine(ptz, pt0);
 
+        debugDisplay.PopMatrix();
+
         debugDisplay.SetState(stateBefore);
     }
 

Gems/ROS2/Code/Source/Frame/NamespaceConfiguration.cpp

@@ -63,6 +63,13 @@ namespace ROS2
         return ROS2Names::GetNamespacedName(parentNamespace, m_namespace);
     }
 
+    void NamespaceConfiguration::SetNamespace(const AZStd::string& ns, NamespaceStrategy strategy)
+    {
+        m_namespace = ns;
+        m_namespaceStrategy = strategy;
+        UpdateNamespace();
+    }
+
     bool NamespaceConfiguration::IsNamespaceCustom() const
     {
         return m_namespaceStrategy == NamespaceConfiguration::NamespaceStrategy::Custom;