Merge pull request #692 from o3de/point-release/23103

23.10.3 Point Release

Signed-off-by: Nicholas Lawson <70027408+nick-l-o3de@users.noreply.github.com>

.gitattributes

@@ -124,7 +124,11 @@ Gems/AtomLyIntegration/CommonFeatures/Assets/Objects/Lucy/Lucy_low.fbx filter=lf
 Gems/AtomLyIntegration/CommonFeatures/Assets/Objects/Lucy/.wip/marmoset_bake.tbscene filter=lfs diff=lfs merge=lfs -text
 Gems/AtomLyIntegration/CommonFeatures/Assets/Objects/Lucy/.wip/Brass/brass_bake.spp filter=lfs diff=lfs merge=lfs -text
 Gems/AtomLyIntegration/CommonFeatures/Assets/Objects/Lucy/.wip/stone/stone_bake.spp filter=lfs diff=lfs merge=lfs -text
+Gems/ProteusRobot/docs/images/*.png filter= diff= merge= -text
+Gems/RosRobotSample/docs/images/*.png filter= diff= merge= -text
+Gems/WarehouseAutomation/docs/images/*.png filter= diff= merge= -text
 Gems/WarehouseAssets/docs/images/*.png filter= diff= merge= -text
+Gems/WarehouseSample/docs/images/*.png filter= diff= merge= -text
 Templates/Ros2FleetRobotTemplate/docs/images/*.png filter= diff= merge= -text
 Gems/ROS2/docs/**/*.png -filter -diff -merge
 Templates/Ros2ProjectTemplate/Screenshots/*.png filter= diff= merge= -text

Gems/ProteusRobot/README.md

@@ -0,0 +1,30 @@
+[![Apache License, Version 2.0][apache_shield]][apache]
+
+# Proteus Robot Gem for Open 3D Engine (O3DE)
+
+## Requirements
+- Any O3DE project with the [ROS 2 Gem](https://docs.o3de.org/docs/user-guide/interactivity/robotics/) enabled.
+
+Please refer to [O3DE documentation](https://docs.o3de.org/docs/user-guide/gems/) to learn more about Gems and about registering Gems in the system and O3DE projects.
+
+## Description
+This is an Asset Gem. It contains a simplified model of [Proteus Robot](https://robotsguide.com/robots/proteus) - an autonomous mobile robot that can pick up, transport, and drop off containers. It is delivered as a ready-to-use O3DE prefab, `Proteus.prefab`, containing visual models, physics and the following ROS 2 components:
+- `ROS2 Frame`
+- `ROS2 Robot Control`
+- `ROS2 Skid Steering Twist Control`
+- `ROS2 Lidar Sensor`
+
+Additionally, the model is equipped with links that are suitable for adding `Camera` and `Imu` sensors.
+
+The robot publishes Lidar Sensor's output on the`/base_link/pc` ROS 2 topic and can be driven using the `/base_link/cmd_vel` ROS 2 topic. An example of its use can be found in [ROS 2 Project Template](https://github.com/o3de/o3de-extras/tree/development/Templates/Ros2FleetRobotTemplate).
+
+## Screenshots
+![](docs/images/front.png)
+![](docs/images/back.png)
+
+## Acknowledgments
+This work is licensed under [Apache License, Version 2.0][apache]. You may elect at your option to use the [MIT License][mit] instead. Contributions must be made under both licenses.
+
+[apache]: https://opensource.org/licenses/Apache-2.0
+[mit]: https://opensource.org/licenses/MIT
+[apache_shield]: https://img.shields.io/badge/License-Apache_2.0-blue.svg

Gems/ProteusRobot/gem.json

@@ -2,10 +2,10 @@
     "gem_name": "ProteusRobot",
     "version": "1.0.0",
     "display_name": "Proteus Robot",
-    "license": "Apache-2.0",
+    "license": "Apache-2.0 or MIT",
     "license_url": "https://opensource.org/licenses/Apache-2.0",
     "origin": "RobotecAI",
-    "origin_url": "https://github.com/o3de/o3de-extras/tree/development/Gems/ProteusRobot",
+    "origin_url": "https://robotec.ai",
     "type": "Asset",
     "summary": "Proteus warehouse robot with Lidar sensor",
     "canonical_tags": [
@@ -18,15 +18,17 @@
         ""
     ],
     "icon_path": "preview.png",
-    "requirements": "Requires ROS 2 Gem for the Lidar to function",
+    "requirements": "Requires ROS 2 Gem",
     "documentation_url": "https://www.o3de.org/docs/user-guide/interactivity/robotics/project-configuration/#ros-2-project-templates",
-    "dependencies": [],
+    "dependencies": [
+        "ROS2==2.0.0"
+    ],
     "repo_uri": "https://raw.githubusercontent.com/o3de/o3de-extras/development",
-     "compatible_engines": [
+    "compatible_engines": [
         "o3de-sdk>=1.2.0",
         "o3de>=1.2.0"
     ],
     "engine_api_dependencies": [],
     "restricted": "ProteusRobot",
     "download_source_uri": "https://github.com/o3de/o3de-extras/releases/download/2.0/proteusrobot-1.0.0-gem.zip"
-}
+}

Gems/ROS2/CMakeLists.txt

@@ -10,6 +10,9 @@ if (NOT gem_name)
     set(gem_name "ROS2")
 endif()
 
+# Add pip requirements required for ros2 packages.
+update_pip_requirements(${CMAKE_CURRENT_LIST_DIR}/requirements.txt ROS2)
+
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/Code")
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} PARENT_SCOPE)
 

Gems/ROS2/Code/PythonTests/tests/SmokeTests_EnterGameModeWorks.py

@@ -66,9 +66,13 @@ def SmokeTest_EnterGameModeWorks():
     # Required for automated tests
     helper.init_idle()
 
-    # Open the level called "Warehouse".
-    # We use a warehouse level for a smoke test - it already has a robot prefab present
-    helper.open_level(level="Warehouse", directory='')
+    # Open the level called "DefaultLevel".
+    # We use a DefaultLevel level for a smoke test.
+    # ROS2 System Component should publish topics listed below regardless of level
+    # - /tf
+    # - /tf_static
+    # - /clock
+    helper.open_level(level="DefaultLevel", directory='')
 
     topics_before_game_mode = check_topics()
 

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

@@ -184,7 +184,9 @@ namespace ROS2
     void CameraSensor::RequestFrame(
         const AZ::Transform& cameraPose, AZStd::function<void(const AZ::RPI::AttachmentReadback::ReadbackResult& result)> callback)
     {
-        const AZ::Transform inverse = (cameraPose * AtomToRos).GetInverse();
+        const AZ::Transform cameraPoseNoScaling =
+            AZ::Transform::CreateFromQuaternionAndTranslation(cameraPose.GetRotation(), cameraPose.GetTranslation());
+        const AZ::Transform inverse = (cameraPoseNoScaling * AtomToRos).GetInverse();
         m_view->SetWorldToViewMatrix(AZ::Matrix4x4::CreateFromQuaternionAndTranslation(inverse.GetRotation(), inverse.GetTranslation()));
 
         AZ::Render::FrameCaptureOutcome captureOutcome;

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

@@ -142,7 +142,8 @@ namespace ROS2
         auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
         auto requestResults = sceneInterface->QuerySceneBatch(m_sceneHandle, requests);
         AZ_Assert(requestResults.size() == rayDirections.size(), "Request size should be equal to directions size");
-        const auto localTransform = lidarTransform.GetInverse();
+        const auto localTransform =
+            AZ::Transform::CreateFromQuaternionAndTranslation(lidarTransform.GetRotation(), lidarTransform.GetTranslation()).GetInverse();
         const float maxRange = m_addMaxRangePoints ? m_range : AZStd::numeric_limits<float>::infinity();
 
         for (int i = 0; i < requestResults.size(); i++)

Gems/ROS2/Code/Source/ROS2SystemComponent.cpp

@@ -5,6 +5,8 @@
  * SPDX-License-Identifier: Apache-2.0 OR MIT
  *
  */
+#include <signal.h>
+
 #include <Lidar/LidarCore.h>
 #include <ROS2/Clock/PhysicallyStableClock.h>
 #include <ROS2/Communication/PublisherConfiguration.h>
@@ -47,7 +49,10 @@ namespace ROS2
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
-                ec->Class<ROS2SystemComponent>("ROS2 System Component", "[Description of functionality provided by this System Component]")
+                ec->Class<ROS2SystemComponent>(
+                      "ROS 2 System Component",
+                      "This component is responsible for creating ROS 2 node and executor, provides ROS 2 interfaces, manages ROS 2 clock and "
+                      "publishes transforms.")
                     ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
                     ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("System"))
                     ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
@@ -82,7 +87,6 @@ namespace ROS2
         {
             ROS2Interface::Register(this);
         }
-        return;
     }
 
     ROS2SystemComponent::~ROS2SystemComponent()
@@ -97,6 +101,14 @@ namespace ROS2
     void ROS2SystemComponent::Init()
     {
         rclcpp::init(0, 0);
+
+        // handle signals, e.g. via `Ctrl+C` hotkey or `kill` command 
+        auto handler = [](int sig){
+            rclcpp::shutdown(); // shutdown rclcpp
+            std::raise(sig); // shutdown o3de
+            };
+        signal(SIGINT, handler);
+        signal(SIGTERM, handler);
     }
 
     void ROS2SystemComponent::InitClock()

Gems/ROS2/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.cpp

@@ -12,6 +12,7 @@
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
 #include <AzFramework/Physics/RigidBodyBus.h>
+#include <AzFramework/Physics/SimulatedBodies/RigidBody.h>
 
 namespace ROS2
 {
@@ -34,12 +35,58 @@ namespace ROS2
 
     void RigidBodyTwistControlComponent::Activate()
     {
+        AZ::TickBus::Handler::BusConnect();
         TwistNotificationBus::Handler::BusConnect(GetEntityId());
     }
 
     void RigidBodyTwistControlComponent::Deactivate()
     {
         TwistNotificationBus::Handler::BusDisconnect();
+        if (m_sceneFinishSimHandler.IsConnected())
+        {
+            m_sceneFinishSimHandler.Disconnect();
+        }
+        if (AZ::TickBus::Handler::BusIsConnected())
+        {
+            AZ::TickBus::Handler::BusDisconnect();
+        }
+    }
+
+    void RigidBodyTwistControlComponent::OnTick([[maybe_unused]]float deltaTime, [[maybe_unused]]AZ::ScriptTimePoint time)
+    {
+        AzPhysics::SceneInterface* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
+        AZ_Assert(sceneInterface, "No scene interface");
+        if (!sceneInterface)
+        {
+            return;
+        }
+        AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
+        AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
+
+        AzPhysics::RigidBody* rigidBody = nullptr;
+        Physics::RigidBodyRequestBus::EventResult(rigidBody, GetEntityId(), &Physics::RigidBodyRequests::GetRigidBody);
+        AZ_Warning("RigidBodyTwistControlComponent", rigidBody, "No rigid body found for entity %s", GetEntity()->GetName().c_str());
+        if (!rigidBody)
+        {
+            return;
+        }
+        m_bodyHandle = rigidBody->m_bodyHandle;
+        m_sceneFinishSimHandler = AzPhysics::SceneEvents::OnSceneSimulationFinishHandler(
+                [this, sceneInterface]([[maybe_unused]] AzPhysics::SceneHandle sceneHandle, float fixedDeltaTime)
+                {
+                    auto* rigidBody = sceneInterface->GetSimulatedBodyFromHandle(sceneHandle, m_bodyHandle);
+                    AZ_Assert(sceneInterface, "No body found for previously given handle");
+
+                    // Convert local steering to world frame
+                    const AZ::Transform robotTransform = rigidBody->GetTransform();
+                    const auto linearVelocityGlobal = robotTransform.TransformVector(m_linearVelocityLocal);
+                    const auto angularVelocityGlobal = robotTransform.TransformVector(m_angularVelocityLocal);
+                    Physics::RigidBodyRequestBus::Event(GetEntityId(), &Physics::RigidBodyRequests::SetLinearVelocity, linearVelocityGlobal);
+                    Physics::RigidBodyRequestBus::Event(GetEntityId(), &Physics::RigidBodyRequests::SetAngularVelocity, angularVelocityGlobal);
+                },
+                aznumeric_cast<int32_t>(AzPhysics::SceneEvents::PhysicsStartFinishSimulationPriority::Components));
+        sceneInterface->RegisterSceneSimulationFinishHandler(defaultSceneHandle, m_sceneFinishSimHandler);
+        AZ::TickBus::Handler::BusDisconnect();
     }
 
     void RigidBodyTwistControlComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
@@ -50,21 +97,7 @@ namespace ROS2
 
     void RigidBodyTwistControlComponent::TwistReceived(const AZ::Vector3& linear, const AZ::Vector3& angular)
     {
-        auto thisEntityId = GetEntityId();
-        AZ::Vector3 currentLinearVelocity;
-        Physics::RigidBodyRequestBus::EventResult(currentLinearVelocity, thisEntityId, &Physics::RigidBodyRequests::GetLinearVelocity);
-
-        // Convert local steering to world frame
-        AZ::Transform robotTransform;
-        AZ::TransformBus::EventResult(robotTransform, thisEntityId, &AZ::TransformBus::Events::GetWorldTM);
-        auto transformedLinearVelocity = robotTransform.TransformVector(linear);
-
-        // Overwrite control velocities on two axis
-        currentLinearVelocity.SetX(transformedLinearVelocity.GetX());
-        currentLinearVelocity.SetY(transformedLinearVelocity.GetY());
-
-        // Reapply desired velocities
-        Physics::RigidBodyRequestBus::Event(thisEntityId, &Physics::RigidBodyRequests::SetLinearVelocity, currentLinearVelocity);
-        Physics::RigidBodyRequestBus::Event(thisEntityId, &Physics::RigidBodyRequests::SetAngularVelocity, angular);
+        m_linearVelocityLocal = linear;
+        m_angularVelocityLocal = angular;
     }
 } // namespace ROS2

Gems/ROS2/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.h

@@ -9,14 +9,16 @@
 
 #include <AzCore/Component/Component.h>
 #include <ROS2/RobotControl/Twist/TwistBus.h>
-
+#include <AzFramework/Physics/PhysicsSystem.h>
+#include <AzCore/Component/TickBus.h>
 namespace ROS2
 {
     //! A component with a simple handler for Twist type of control (linear and angular velocities).
     //! Velocities are directly applied to a selected body.
     class RigidBodyTwistControlComponent
         : public AZ::Component
         , private TwistNotificationBus::Handler
+        , private AZ::TickBus::Handler
     {
     public:
         AZ_COMPONENT(RigidBodyTwistControlComponent, "{D994FE1A-AA6A-42B9-8B8E-B3B375891F5B}", AZ::Component);
@@ -36,5 +38,15 @@ namespace ROS2
         // TwistNotificationBus::Handler overrides
         void TwistReceived(const AZ::Vector3& linear, const AZ::Vector3& angular) override;
         //////////////////////////////////////////////////////////////////////////
+
+        //////////////////////////////////////////////////////////////////////////
+        // AZ::TickBus::Handler overrides
+        void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
+        //////////////////////////////////////////////////////////////////////////
+
+        AZ::Vector3 m_linearVelocityLocal {AZ::Vector3::CreateZero()}; //!< Linear velocity in local frame
+        AZ::Vector3 m_angularVelocityLocal {AZ::Vector3::CreateZero()}; //!< Angular velocity in local frame
+        AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_sceneFinishSimHandler; //!< Handler called after every physics sub-step
+        AzPhysics::SimulatedBodyHandle m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle; //!< Handle to the body to apply velocities to
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.cpp

@@ -40,7 +40,7 @@ namespace ROS2
         for (size_t si = 0; si < link->SensorCount(); ++si)
         {
             const auto* sensor = link->SensorByIndex(si);
-            const bool success = AddSensor(entityId, sensor);
+            [[maybe_unused]] const bool success = AddSensor(entityId, sensor);
             AZ_Warning("SensorMaker", success, "Cannot find a sensor hook for sensor %d", sensor->Type());
         }
     }

Gems/ROS2/requirements.txt

@@ -0,0 +1,2 @@
+pyyaml==5.3
+lark==1.1.1

Gems/RosRobotSample/README.md

@@ -1,9 +1,34 @@
-# ROS 2 Robot Sample
+[![Apache License, Version 2.0][apache_shield]][apache]
 
-This gem contains a sample robot asset, ready to be used with the ROS 2 Gem. 
+# Ros Robot Sample Gem for Open 3D Engine (O3DE)
 
-The robot is delivered as a ready to use prefab containing models, physics and following ROS 2 components:
-- Frame
-- Robot Control
-- Twist Control
-- Lidar Sensor
+## Requirements
+- Any O3DE project with the [ROS 2 Gem](https://docs.o3de.org/docs/user-guide/interactivity/robotics/) enabled.
+
+Please refer to [O3DE documentation](https://docs.o3de.org/docs/user-guide/gems/) to learn more about Gems and about registering Gems in the system and O3DE projects.
+
+## Description
+This is an Asset Gem. It contains the [Husarion ROSbot XL](https://husarion.com/manuals/rosbot-xl/) robot - an autonomous mobile robot platform developed by [Husarion](https://husarion.com). The robot is delivered as a ready-to-use prefab, `ROSbot.prefab`, containing visual models, physics and the following ROS 2 components:
+- `ROS2 Frame`
+- `ROS2 Robot Control`
+- `ROS2 Skid Steering Twist Control`
+
+Additionally, the model is equipped with links that are suitable for adding `Camera` and `Imu` sensors. There are two extra O3DE prefabs with 2D and 3D LiDAR sensors attached: `ROSBot_slamtec.prefab` and `ROSbot_velodyne.prefab` with Slamtec RPLIDAR S1 and Velodyne Puck VLP-16 sensors respectively. The components are visualized with the corresponding meshes. 
+
+The robot publishes Lidar Sensor's output on the`/base_link/pc` ROS 2 topic and can be driven using the `/base_link/cmd_vel` ROS 2 topic. An example of its use can be found in [ROS 2 Project Template](https://github.com/o3de/o3de-extras/tree/development/Templates/Ros2ProjectTemplate).
+
+## Screenshots
+![](docs/images/front.png)
+From left: `ROSbot_velodyne.prefab`, `ROSBot_slamtec.prefab`, and `ROSbot.prefab`
+
+![](docs/images/back.png)
+From left: `ROSbot.prefab`, `ROSBot_slamtec.prefab`, and `ROSbot_velodyne.prefab`
+
+## Acknowledgments
+This work is licensed under [Apache License, Version 2.0][apache]. You may elect at your option to use the [MIT License][mit] instead. Contributions must be made under both licenses.
+
+Models were created based on STL and URDF files kindly shared by Husarion.
+
+[apache]: https://opensource.org/licenses/Apache-2.0
+[mit]: https://opensource.org/licenses/MIT
+[apache_shield]: https://img.shields.io/badge/License-Apache_2.0-blue.svg

Gems/RosRobotSample/gem.json

@@ -3,10 +3,10 @@
     "display_name": "ROS Robot Sample",
     "license": "Apache-2.0 or MIT",
     "license_url": "https://opensource.org/licenses/Apache-2.0",
-    "origin": "Ros2WarehouseDemo",
-    "origin_url": "https://github.com/o3de/o3de-extras/tree/main/Gems/RosRobotSample",
+    "origin": "RobotecAI",
+    "origin_url": "https://robotec.ai",
     "type": "Asset",
-    "summary": "This project contains a sample robot asset",
+    "summary": "Husarion ROSbot XL robot assets.",
     "canonical_tags": [
         "Gem"
     ],
@@ -17,10 +17,10 @@
         "Linux"
     ],
     "icon_path": "preview.png",
-    "requirements": "",
+    "requirements": "Requires ROS 2 Gem",
     "documentation_url": "",
     "dependencies": [
-        "ROS2"
+        "ROS2==2.0.0"
     ],
     "compatible_engines": [
         "o3de-sdk>=1.2.0",
@@ -30,4 +30,4 @@
     "restricted": "",
     "download_source_uri": "https://github.com/o3de/o3de-extras/releases/download/2.0/rosrobotsample-1.0.0-gem.zip",
     "version": "1.0.0"
-}
+}