diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
index a19bb285b839ff7be1776cc4488f279db6c038f1..8cf04d9d164043b1d9f60d3846524747e48495fa 100644
--- a/nav2_amcl/src/amcl_node.cpp
+++ b/nav2_amcl/src/amcl_node.cpp
@@ -53,6 +53,7 @@ using namespace std::chrono_literals;
 
 namespace nav2_amcl
 {
+using nav2_util::geometry_utils::orientationAroundZAxis;
 
 AmclNode::AmclNode()
 : nav2_util::LifecycleNode("amcl", "", true)
@@ -243,8 +244,7 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/)
     msg->pose.pose.position.x = initial_pose_x_;
     msg->pose.pose.position.y = initial_pose_y_;
     msg->pose.pose.position.z = initial_pose_z_;
-    msg->pose.pose.orientation =
-      nav2_util::geometry_utils::orientationAroundZAxis(initial_pose_yaw_);
+    msg->pose.pose.orientation = orientationAroundZAxis(initial_pose_yaw_);
 
     initialPoseReceived(msg);
   } else if (init_pose_received_on_inactive) {
@@ -723,16 +723,13 @@ bool AmclNode::updateFilter(
   //
   // Construct min and max angles of laser, in the base_link frame.
   // Here we set the roll pich yaw of the lasers.  We assume roll and pich are zero.
-  tf2::Quaternion q;
-  q.setRPY(0.0, 0.0, laser_scan->angle_min);
   geometry_msgs::msg::QuaternionStamped min_q, inc_q;
   min_q.header.stamp = laser_scan->header.stamp;
   min_q.header.frame_id = nav2_util::strip_leading_slash(laser_scan->header.frame_id);
-  tf2::impl::Converter<false, true>::convert(q, min_q.quaternion);
+  min_q.quaternion = orientationAroundZAxis(laser_scan->angle_min);
 
-  q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
   inc_q.header = min_q.header;
-  tf2::impl::Converter<false, true>::convert(q, inc_q.quaternion);
+  inc_q.quaternion = orientationAroundZAxis(laser_scan->angle_min + laser_scan->angle_increment);
   try {
     tf_buffer_->transform(min_q, min_q, base_frame_id_);
     tf_buffer_->transform(inc_q, inc_q, base_frame_id_);
@@ -797,9 +794,7 @@ AmclNode::publishParticleCloud(const pf_sample_set_t * set)
     cloud_msg.poses[i].position.x = set->samples[i].pose.v[0];
     cloud_msg.poses[i].position.y = set->samples[i].pose.v[1];
     cloud_msg.poses[i].position.z = 0;
-    tf2::Quaternion q;
-    q.setRPY(0, 0, set->samples[i].pose.v[2]);
-    tf2::impl::Converter<false, true>::convert(q, cloud_msg.poses[i].orientation);
+    cloud_msg.poses[i].orientation = orientationAroundZAxis(set->samples[i].pose.v[2]);
   }
   particlecloud_pub_->publish(cloud_msg);
 }
@@ -867,9 +862,7 @@ AmclNode::publishAmclPose(
   // Copy in the pose
   p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
   p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
-  tf2::Quaternion q;
-  q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
-  tf2::impl::Converter<false, true>::convert(q, p.pose.pose.orientation);
+  p.pose.pose.orientation = orientationAroundZAxis(hyps[max_weight_hyp].pf_pose_mean.v[2]);
   // Copy in the covariance, converting from 3-D to 6-D
   pf_sample_set_t * set = pf_->sets + pf_->current_set;
   for (int i = 0; i < 2; i++) {
diff --git a/nav2_costmap_2d/test/integration/test_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_collision_checker.cpp
index 243d709832f8e30e6023944db5801646ba08eae9..07a55a00b3343b07306161308d393fe18ac18d3c 100644
--- a/nav2_costmap_2d/test/integration/test_collision_checker.cpp
+++ b/nav2_costmap_2d/test/integration/test_collision_checker.cpp
@@ -35,9 +35,11 @@
 #pragma GCC diagnostic ignored "-Wpedantic"
 #include "tf2/utils.h"
 #pragma GCC diagnostic pop
+#include "nav2_util/geometry_utils.hpp"
 
 using namespace std::chrono_literals;
 using namespace std::placeholders;
+using nav2_util::geometry_utils::orientationAroundZAxis;
 
 class RclCppFixture
 {
@@ -198,9 +200,7 @@ protected:
     current_pose_.pose.position.x = x_;
     current_pose_.pose.position.y = y_;
     current_pose_.pose.position.z = 0;
-    tf2::Quaternion q;
-    q.setRPY(0, 0, yaw_);
-    current_pose_.pose.orientation = tf2::toMsg(q);
+    current_pose_.pose.orientation = orientationAroundZAxis(yaw_);
   }
 
   void publishFootprint()
diff --git a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp
index 4b1e062c929030cf363a5b8c8ce4093949ec56f7..8a6da00676bcc60888e2bdf37a6878eaada745d6 100644
--- a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp
+++ b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp
@@ -39,9 +39,11 @@
 #pragma GCC diagnostic ignored "-Wpedantic"
 #include "tf2/utils.h"
 #pragma GCC diagnostic pop
+#include "nav2_util/geometry_utils.hpp"
 
 namespace nav_2d_utils
 {
+using nav2_util::geometry_utils::orientationAroundZAxis;
 
 geometry_msgs::msg::Twist twist2Dto3D(const nav_2d_msgs::msg::Twist2D & cmd_vel_2d)
 {
@@ -87,9 +89,7 @@ geometry_msgs::msg::Pose pose2DToPose(const geometry_msgs::msg::Pose2D & pose2d)
   geometry_msgs::msg::Pose pose;
   pose.position.x = pose2d.x;
   pose.position.y = pose2d.y;
-  tf2::Quaternion q;
-  q.setRPY(0, 0, pose2d.theta);
-  pose.orientation = tf2::toMsg(q);
+  pose.orientation = orientationAroundZAxis(pose2d.theta);
   return pose;
 }
 
@@ -111,9 +111,7 @@ geometry_msgs::msg::PoseStamped pose2DToPoseStamped(
   pose.header.stamp = stamp;
   pose.pose.position.x = pose2d.x;
   pose.pose.position.y = pose2d.y;
-  tf2::Quaternion q;
-  q.setRPY(0, 0, pose2d.theta);
-  pose.pose.orientation = tf2::toMsg(q);
+  pose.pose.orientation = orientationAroundZAxis(pose2d.theta);
   return pose;
 }
 
diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp
index 458c1bd9eb892cf5e7037fd6f39d015ab188d7e0..c161d5056c9f283ad4b49472541014791fcb1372 100644
--- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp
+++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp
@@ -63,9 +63,6 @@ protected:
 
   // Also, for convenience, this client supports invoking the NavigateToPose action
   rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SharedPtr navigate_action_client_;
-
-  // A convenience function to convert from an algle to a Quaternion
-  geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle);
 };
 
 }  // namespace nav2_lifecycle_manager
diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp
index d9b7a828ac2b34569d4cbd7898d006a305720f35..5e5a03d974964ec64b42c1d19ca8d1b5b3bf935e 100644
--- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp
+++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp
@@ -18,9 +18,11 @@
 #include <memory>
 
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "nav2_util/geometry_utils.hpp"
 
 namespace nav2_lifecycle_manager
 {
+using nav2_util::geometry_utils::orientationAroundZAxis;
 
 LifecycleManagerClient::LifecycleManagerClient()
 {
@@ -54,14 +56,6 @@ LifecycleManagerClient::shutdown()
   callService(shutdown_client_, "lifecycle_manager/shutdown");
 }
 
-geometry_msgs::msg::Quaternion
-LifecycleManagerClient::orientationAroundZAxis(double angle)
-{
-  tf2::Quaternion q;
-  q.setRPY(0, 0, angle);  // void returning function
-  return tf2::toMsg(q);
-}
-
 void
 LifecycleManagerClient::set_initial_pose(double x, double y, double theta)
 {
diff --git a/nav2_map_server/src/occ_grid_loader.cpp b/nav2_map_server/src/occ_grid_loader.cpp
index d751d6855cdde3618b017a79d29da73fb5ffbb8f..dd960a95316ada144e7042375b5f1b61c7c2ef27 100644
--- a/nav2_map_server/src/occ_grid_loader.cpp
+++ b/nav2_map_server/src/occ_grid_loader.cpp
@@ -40,11 +40,14 @@
 #include "Magick++.h"
 #include "tf2/LinearMath/Quaternion.h"
 #include "yaml-cpp/yaml.h"
+#include "nav2_util/geometry_utils.hpp"
 
 using namespace std::chrono_literals;
 
 namespace nav2_map_server
 {
+using nav2_util::geometry_utils::orientationAroundZAxis;
+
 OccGridLoader::OccGridLoader(
   rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & yaml_filename)
 : node_(node), yaml_filename_(yaml_filename)
@@ -221,12 +224,7 @@ void OccGridLoader::loadMapFromFile(const LoadParameters & loadParameters)
   msg.info.origin.position.x = loadParameters.origin[0];
   msg.info.origin.position.y = loadParameters.origin[1];
   msg.info.origin.position.z = 0.0;
-  tf2::Quaternion q;
-  q.setRPY(0, 0, loadParameters.origin[2]);
-  msg.info.origin.orientation.x = q.x();
-  msg.info.origin.orientation.y = q.y();
-  msg.info.origin.orientation.z = q.z();
-  msg.info.origin.orientation.w = q.w();
+  msg.info.origin.orientation = orientationAroundZAxis(loadParameters.origin[2]);
 
   // Allocate space to hold the data
   msg.data.resize(msg.info.width * msg.info.height);
diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
index 7668542be8bb8402983a5912972878b20f53aff5..4269d72828dda72fa10572cfa8c26354979ae1f0 100644
--- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
+++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
@@ -28,6 +28,7 @@
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "rviz_common/panel.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "nav2_util/geometry_utils.hpp"
 
 class QPushButton;
 
@@ -60,8 +61,6 @@ private:
   void onCancelButtonPressed();
   void timerEvent(QTimerEvent * event) override;
 
-  geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle);
-
   // Call to send NavigateToPose action request for goal pose
   void startNavigation(geometry_msgs::msg::PoseStamped pose);
   using GoalHandle = rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>;
diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp
index e846421401cdc1639d0ef1d5bccde2b38370bd15..cdf84ce3c264775fde3aa4ca45848ee30d52071c 100644
--- a/nav2_rviz_plugins/src/nav2_panel.cpp
+++ b/nav2_rviz_plugins/src/nav2_panel.cpp
@@ -26,6 +26,7 @@ using namespace std::chrono_literals;
 
 namespace nav2_rviz_plugins
 {
+using nav2_util::geometry_utils::orientationAroundZAxis;
 
 // Define global GoalPoseUpdater so that the nav2 GoalTool plugin can access to update goal pose
 GoalPoseUpdater GoalUpdater;
@@ -180,14 +181,6 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame)
   startNavigation(pose);
 }
 
-geometry_msgs::msg::Quaternion
-Nav2Panel::orientationAroundZAxis(double angle)
-{
-  tf2::Quaternion q;
-  q.setRPY(0, 0, angle);  // void returning function
-  return tf2::toMsg(q);
-}
-
 void
 Nav2Panel::onCancelButtonPressed()
 {
diff --git a/nav2_util/src/costmap.cpp b/nav2_util/src/costmap.cpp
index 3af0f6e850b94e9ec68cf33fddfe541ccf22dd98..59925e838bf57419c949210a6d4e7d0f01577f7b 100644
--- a/nav2_util/src/costmap.cpp
+++ b/nav2_util/src/costmap.cpp
@@ -16,11 +16,13 @@
 #include <algorithm>
 #include "nav2_util/costmap.hpp"
 #include "tf2/LinearMath/Quaternion.h"
+#include "nav2_util/geometry_utils.hpp"
 
 using std::vector;
 
 namespace nav2_util
 {
+using nav2_util::geometry_utils::orientationAroundZAxis;
 
 const Costmap::CostValue Costmap::no_information = 255;
 const Costmap::CostValue Costmap::lethal_obstacle = 254;
@@ -95,13 +97,7 @@ void Costmap::set_test_costmap(const TestCostmap & testCostmapType)
 
   // Define map rotation
   // Provided as yaw with counterclockwise rotation, with yaw = 0 meaning no rotation
-
-  tf2::Quaternion quaternion;
-  quaternion.setRPY(0.0, 0.0, 0.0);  // set roll, pitch, yaw
-  costmap_properties_.origin.orientation.x = quaternion.x();
-  costmap_properties_.origin.orientation.y = quaternion.y();
-  costmap_properties_.origin.orientation.z = quaternion.z();
-  costmap_properties_.origin.orientation.w = quaternion.w();
+  costmap_properties_.origin.orientation = orientationAroundZAxis(0.0);
 
   costs_ = get_test_data(testCostmapType);