diff --git a/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.cpp b/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.cpp
index 623c0828..8ac29530 100644
--- a/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.cpp
+++ b/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.cpp
@@ -26,7 +26,7 @@
 #include <ROS2/Utilities/ROS2Conversions.h>
 #include <Source/RigidBodyComponent.h>
 #include <imgui/imgui.h>
-#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_listener.hpp>
 
 namespace ROS2PoseControl
 {
@@ -135,7 +135,7 @@ namespace ROS2PoseControl
             m_poseSubscription = ros2Node->create_subscription<geometry_msgs::msg::PoseStamped>(
                 namespacedTopicName.data(),
                 m_configuration.m_poseTopicConfiguration.GetQoS(),
-                [this](geometry_msgs::msg::PoseStamped::SharedPtr msg)
+                [this](geometry_msgs::msg::PoseStamped::ConstSharedPtr msg)
                 {
                     OnPoseMessageReceived(msg);
                 }
@@ -287,7 +287,7 @@ namespace ROS2PoseControl
         return AZ::Success(AZ::Transform::CreateFromQuaternionAndTranslation(rotation, translation));
     }
 
-    AZ::Outcome<AZ::Transform, void> ROS2PoseControl::GetTransformFromPoseStamped(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
+    AZ::Outcome<AZ::Transform, void> ROS2PoseControl::GetTransformFromPoseStamped(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg)
     {
         if (!msg)
         {
@@ -314,7 +314,7 @@ namespace ROS2PoseControl
         return AZ::Success(transform);
     }
 
-    void ROS2PoseControl::OnPoseMessageReceived(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
+    void ROS2PoseControl::OnPoseMessageReceived(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg)
     {
         if (m_configuration.m_tracking_mode != TrackingMode::PoseMessages)
         {
diff --git a/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.h b/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.h
index 12cfea81..7dd761ad 100644
--- a/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.h
+++ b/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.h
@@ -24,8 +24,8 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <rclcpp/publisher.hpp>
 #include <rclcpp/subscription.hpp>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.hpp>
+#include <tf2_ros/transform_listener.hpp>
 
 namespace ROS2PoseControl
 {
@@ -77,7 +77,8 @@ namespace ROS2PoseControl
         //! transform is assumed to be in the global frame.
         //! @param msg The PoseStamped message.
         //! @return The world transform translated using the global frame and the pose stamped.
-        [[nodiscard]] AZ::Outcome<AZ::Transform, void> GetTransformFromPoseStamped(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
+        [[nodiscard]] AZ::Outcome<AZ::Transform, void> GetTransformFromPoseStamped(
+            const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg);
 
         //! Removes the tilt from a transform by projecting the transform's forward vector onto the gravity direction and creating a new
         //! basis from the projected forward vector and the gravity direction.
@@ -94,7 +95,7 @@ namespace ROS2PoseControl
 
         //! Applies the given transform to the entity. Run when a transform is received from a PoseStamped message.
         //! @param transform The transform to apply.
-        void OnPoseMessageReceived(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
+        void OnPoseMessageReceived(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg);
 
         //! Applies the given transform to the entity. Will check if removing tilt, applying an offset, and clamping to the ground are
         //! enabled in the configuration and add those modifications to the input transform.
diff --git a/Gems/ROS2PoseControl/gem.json b/Gems/ROS2PoseControl/gem.json
index 0933de7f..c7aa8a73 100644
--- a/Gems/ROS2PoseControl/gem.json
+++ b/Gems/ROS2PoseControl/gem.json
@@ -1,6 +1,6 @@
 {
     "gem_name": "ROS2PoseControl",
-    "version": "4.0.0",
+    "version": "4.0.1",
     "display_name": "ROS2PoseControl",
     "license": "Apache-2.0",
     "license_url": "https://opensource.org/licenses/Apache-2.0",