FromROS2Pose conversion

Signed-off-by: Paweł Lech <[email protected]>

Gems/ROS2/Code/Include/ROS2/Utilities/ROS2Conversions.h

@@ -25,5 +25,8 @@ namespace ROS2
         static geometry_msgs::msg::Point ToROS2Point(const AZ::Vector3& azvector);
         static geometry_msgs::msg::Quaternion ToROS2Quaternion(const AZ::Quaternion& azquaternion);
         static geometry_msgs::msg::Pose ToROS2Pose(const AZ::Transform& aztransform);
+        static AZ::Transform FromROS2Pose(const geometry_msgs::msg::Pose& ros2pose);
+        static AZ::Vector3 FromROS2Point(const geometry_msgs::msg::Point& ros2point);
+        static AZ::Quaternion FromROS2Quaternion(const geometry_msgs::msg::Quaternion& ros2quaternion);
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Utilities/ROS2Conversions.cpp

@@ -51,4 +51,24 @@ namespace ROS2
         pose.orientation = ToROS2Quaternion(aztransform.GetRotation());
         return pose;
     }
+
+    AZ::Transform ROS2Conversions::FromROS2Pose(const geometry_msgs::msg::Pose& ros2pose)
+    {
+        return { FromROS2Point(ros2pose.position), FromROS2Quaternion(ros2pose.orientation), 1.0f };
+    }
+
+    AZ::Vector3 ROS2Conversions::FromROS2Point(const geometry_msgs::msg::Point& ros2point)
+    {
+        return AZ::Vector3(ros2point.x, ros2point.y, ros2point.z);
+    }
+
+    AZ::Quaternion ROS2Conversions::FromROS2Quaternion(const geometry_msgs::msg::Quaternion& ros2quaternion)
+    {
+        AZ::Quaternion azquaternion;
+        azquaternion.SetX(ros2quaternion.x);
+        azquaternion.SetY(ros2quaternion.y);
+        azquaternion.SetZ(ros2quaternion.z);
+        azquaternion.SetW(ros2quaternion.w);
+        return azquaternion;
+    }
 } // namespace ROS2