diff --git a/nav2_behaviors/README.md b/nav2_behaviors/README.md
index 5ecd14af5c..fdc24a26b3 100644
--- a/nav2_behaviors/README.md
+++ b/nav2_behaviors/README.md
@@ -13,3 +13,6 @@ The value of the centralized behavior server is to **share resources** amongst s
 See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-behavior-server.html) for additional parameter descriptions and a [tutorial about writing behaviors](https://navigation.ros.org/plugin_tutorials/docs/writing_new_behavior_plugin.html).
 
 See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins.
+
+The `TimedBehavior` template makes use of a [nav2_util::TwistPublisher](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).
+The `AssistedTeleop` behavior makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).
\ No newline at end of file
diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp
index 33f9ef7762..772bb894a0 100644
--- a/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp
+++ b/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp
@@ -23,6 +23,7 @@
 #include "std_msgs/msg/empty.hpp"
 #include "nav2_behaviors/timed_behavior.hpp"
 #include "nav2_msgs/action/assisted_teleop.hpp"
+#include "nav2_util/twist_subscriber.hpp"
 
 namespace nav2_behaviors
 {
@@ -82,12 +83,6 @@ class AssistedTeleop : public TimedBehavior<AssistedTeleopAction>
     const geometry_msgs::msg::Twist & twist,
     double projection_time);
 
-  /**
-   * @brief Callback function for velocity subscriber
-   * @param msg received Twist message
-   */
-  void teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
-
   /**
    * @brief Callback function to preempt assisted teleop
    * @param msg empty message
@@ -100,11 +95,11 @@ class AssistedTeleop : public TimedBehavior<AssistedTeleopAction>
   double projection_time_;
   double simulation_time_step_;
 
-  geometry_msgs::msg::Twist teleop_twist_;
+  geometry_msgs::msg::TwistStamped teleop_twist_;
   bool preempt_teleop_{false};
 
   // subscribers
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr vel_sub_;
+  std::unique_ptr<nav2_util::TwistSubscriber> vel_sub_;
   rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr preempt_teleop_sub_;
 
   rclcpp::Duration command_time_allowance_{0, 0};
diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp
index cd0a167428..0b74a04462 100644
--- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp
+++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp
@@ -24,6 +24,7 @@
 #include "nav2_msgs/action/drive_on_heading.hpp"
 #include "nav2_msgs/action/back_up.hpp"
 #include "nav2_util/node_utils.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
 
 namespace nav2_behaviors
 {
@@ -125,17 +126,19 @@ class DriveOnHeading : public TimedBehavior<ActionT>
       return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE};
     }
 
-    auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
-    cmd_vel->linear.y = 0.0;
-    cmd_vel->angular.z = 0.0;
-    cmd_vel->linear.x = command_speed_;
+    auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
+    cmd_vel->header.stamp = this->clock_->now();
+    cmd_vel->header.frame_id = this->robot_base_frame_;
+    cmd_vel->twist.linear.y = 0.0;
+    cmd_vel->twist.angular.z = 0.0;
+    cmd_vel->twist.linear.x = command_speed_;
 
     geometry_msgs::msg::Pose2D pose2d;
     pose2d.x = current_pose.pose.position.x;
     pose2d.y = current_pose.pose.position.y;
     pose2d.theta = tf2::getYaw(current_pose.pose.orientation);
 
-    if (!isCollisionFree(distance, cmd_vel.get(), pose2d)) {
+    if (!isCollisionFree(distance, cmd_vel->twist, pose2d)) {
       this->stopRobot();
       RCLCPP_WARN(this->logger_, "Collision Ahead - Exiting DriveOnHeading");
       return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD};
@@ -162,7 +165,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
    */
   bool isCollisionFree(
     const double & distance,
-    geometry_msgs::msg::Twist * cmd_vel,
+    const geometry_msgs::msg::Twist & cmd_vel,
     geometry_msgs::msg::Pose2D & pose2d)
   {
     // Simulate ahead by simulate_ahead_time_ in this->cycle_frequency_ increments
@@ -174,7 +177,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     bool fetch_data = true;
 
     while (cycle_count < max_cycle_count) {
-      sim_position_change = cmd_vel->linear.x * (cycle_count / this->cycle_frequency_);
+      sim_position_change = cmd_vel.linear.x * (cycle_count / this->cycle_frequency_);
       pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta);
       pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta);
       cycle_count++;
diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp
index 01358ddbd3..eeb8446222 100644
--- a/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp
+++ b/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp
@@ -22,6 +22,7 @@
 #include "nav2_behaviors/timed_behavior.hpp"
 #include "nav2_msgs/action/spin.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
 
 namespace nav2_behaviors
 {
@@ -79,7 +80,7 @@ class Spin : public TimedBehavior<SpinAction>
    */
   bool isCollisionFree(
     const double & distance,
-    geometry_msgs::msg::Twist * cmd_vel,
+    const geometry_msgs::msg::Twist & cmd_vel,
     geometry_msgs::msg::Pose2D & pose2d);
 
   SpinAction::Feedback::SharedPtr feedback_;
diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
index d01e14179f..eab6330d50 100644
--- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
+++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
@@ -29,8 +29,9 @@
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/create_timer_ros.h"
 #include "geometry_msgs/msg/twist.hpp"
-#include "nav2_util/simple_action_server.hpp"
 #include "nav2_util/robot_utils.hpp"
+#include "nav2_util/twist_publisher.hpp"
+#include "nav2_util/simple_action_server.hpp"
 #include "nav2_core/behavior.hpp"
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
@@ -150,7 +151,7 @@ class TimedBehavior : public nav2_core::Behavior
     local_collision_checker_ = local_collision_checker;
     global_collision_checker_ = global_collision_checker;
 
-    vel_pub_ = node->template create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
+    vel_pub_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel", 1);
 
     onConfigure();
   }
@@ -185,7 +186,7 @@ class TimedBehavior : public nav2_core::Behavior
   rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
 
   std::string behavior_name_;
-  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
+  std::unique_ptr<nav2_util::TwistPublisher> vel_pub_;
   std::shared_ptr<ActionServer> action_server_;
   std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> local_collision_checker_;
   std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> global_collision_checker_;
@@ -289,10 +290,12 @@ class TimedBehavior : public nav2_core::Behavior
   // Stop the robot with a commanded velocity
   void stopRobot()
   {
-    auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
-    cmd_vel->linear.x = 0.0;
-    cmd_vel->linear.y = 0.0;
-    cmd_vel->angular.z = 0.0;
+    auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
+    cmd_vel->header.frame_id = robot_base_frame_;
+    cmd_vel->header.stamp = clock_->now();
+    cmd_vel->twist.linear.x = 0.0;
+    cmd_vel->twist.linear.y = 0.0;
+    cmd_vel->twist.angular.z = 0.0;
 
     vel_pub_->publish(std::move(cmd_vel));
   }
diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp
index a0fc972999..9e85229a3c 100644
--- a/nav2_behaviors/plugins/assisted_teleop.cpp
+++ b/nav2_behaviors/plugins/assisted_teleop.cpp
@@ -50,11 +50,14 @@ void AssistedTeleop::onConfigure()
   std::string cmd_vel_teleop;
   node->get_parameter("cmd_vel_teleop", cmd_vel_teleop);
 
-  vel_sub_ = node->create_subscription<geometry_msgs::msg::Twist>(
+  vel_sub_ = std::make_unique<nav2_util::TwistSubscriber>(
+    node,
     cmd_vel_teleop, rclcpp::SystemDefaultsQoS(),
-    std::bind(
-      &AssistedTeleop::teleopVelocityCallback,
-      this, std::placeholders::_1));
+    [&](geometry_msgs::msg::Twist::SharedPtr msg) {
+      teleop_twist_.twist = *msg;
+    }, [&](geometry_msgs::msg::TwistStamped::SharedPtr msg) {
+      teleop_twist_ = *msg;
+    });
 
   preempt_teleop_sub_ = node->create_subscription<std_msgs::msg::Empty>(
     "preempt_teleop", rclcpp::SystemDefaultsQoS(),
@@ -73,7 +76,7 @@ ResultStatus AssistedTeleop::onRun(const std::shared_ptr<const AssistedTeleopAct
 
 void AssistedTeleop::onActionCompletion(std::shared_ptr<AssistedTeleopActionResult>/*result*/)
 {
-  teleop_twist_ = geometry_msgs::msg::Twist();
+  teleop_twist_ = geometry_msgs::msg::TwistStamped();
   preempt_teleop_ = false;
 }
 
@@ -115,11 +118,11 @@ ResultStatus AssistedTeleop::onCycleUpdate()
   projected_pose.y = current_pose.pose.position.y;
   projected_pose.theta = tf2::getYaw(current_pose.pose.orientation);
 
-  geometry_msgs::msg::Twist scaled_twist = teleop_twist_;
+  auto scaled_twist = std::make_unique<geometry_msgs::msg::TwistStamped>(teleop_twist_);
   for (double time = simulation_time_step_; time < projection_time_;
     time += simulation_time_step_)
   {
-    projected_pose = projectPose(projected_pose, teleop_twist_, simulation_time_step_);
+    projected_pose = projectPose(projected_pose, teleop_twist_.twist, simulation_time_step_);
 
     if (!local_collision_checker_->isCollisionFree(projected_pose)) {
       if (time == simulation_time_step_) {
@@ -128,9 +131,9 @@ ResultStatus AssistedTeleop::onCycleUpdate()
           *clock_,
           1000,
           behavior_name_.c_str() << " collided on first time step, setting velocity to zero");
-        scaled_twist.linear.x = 0.0f;
-        scaled_twist.linear.y = 0.0f;
-        scaled_twist.angular.z = 0.0f;
+        scaled_twist->twist.linear.x = 0.0f;
+        scaled_twist->twist.linear.y = 0.0f;
+        scaled_twist->twist.angular.z = 0.0f;
         break;
       } else {
         RCLCPP_DEBUG_STREAM_THROTTLE(
@@ -139,9 +142,9 @@ ResultStatus AssistedTeleop::onCycleUpdate()
           1000,
           behavior_name_.c_str() << " collision approaching in " << time << " seconds");
         double scale_factor = time / projection_time_;
-        scaled_twist.linear.x *= scale_factor;
-        scaled_twist.linear.y *= scale_factor;
-        scaled_twist.angular.z *= scale_factor;
+        scaled_twist->twist.linear.x *= scale_factor;
+        scaled_twist->twist.linear.y *= scale_factor;
+        scaled_twist->twist.angular.z *= scale_factor;
         break;
       }
     }
@@ -171,11 +174,6 @@ geometry_msgs::msg::Pose2D AssistedTeleop::projectPose(
   return projected_pose;
 }
 
-void AssistedTeleop::teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
-{
-  teleop_twist_ = *msg;
-}
-
 void AssistedTeleop::preemptTeleopCallback(const std_msgs::msg::Empty::SharedPtr)
 {
   preempt_teleop_ = true;
diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp
index 55a7872247..5965675f64 100644
--- a/nav2_behaviors/plugins/spin.cpp
+++ b/nav2_behaviors/plugins/spin.cpp
@@ -138,15 +138,17 @@ ResultStatus Spin::onCycleUpdate()
   double vel = sqrt(2 * rotational_acc_lim_ * remaining_yaw);
   vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);
 
-  auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
-  cmd_vel->angular.z = copysign(vel, cmd_yaw_);
+  auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
+  cmd_vel->header.frame_id = robot_base_frame_;
+  cmd_vel->header.stamp = clock_->now();
+  cmd_vel->twist.angular.z = copysign(vel, cmd_yaw_);
 
   geometry_msgs::msg::Pose2D pose2d;
   pose2d.x = current_pose.pose.position.x;
   pose2d.y = current_pose.pose.position.y;
   pose2d.theta = tf2::getYaw(current_pose.pose.orientation);
 
-  if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) {
+  if (!isCollisionFree(relative_yaw_, cmd_vel->twist, pose2d)) {
     stopRobot();
     RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin");
     return ResultStatus{Status::FAILED, SpinActionResult::COLLISION_AHEAD};
@@ -159,7 +161,7 @@ ResultStatus Spin::onCycleUpdate()
 
 bool Spin::isCollisionFree(
   const double & relative_yaw,
-  geometry_msgs::msg::Twist * cmd_vel,
+  const geometry_msgs::msg::Twist & cmd_vel,
   geometry_msgs::msg::Pose2D & pose2d)
 {
   // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments
@@ -170,7 +172,7 @@ bool Spin::isCollisionFree(
   bool fetch_data = true;
 
   while (cycle_count < max_cycle_count) {
-    sim_position_change = cmd_vel->angular.z * (cycle_count / cycle_frequency_);
+    sim_position_change = cmd_vel.angular.z * (cycle_count / cycle_frequency_);
     pose2d.theta = init_pose.theta + sim_position_change;
     cycle_count++;
 
diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md
index 1c5b32dc7d..4af1e890da 100644
--- a/nav2_collision_monitor/README.md
+++ b/nav2_collision_monitor/README.md
@@ -88,3 +88,5 @@ The zones around the robot and the data sources are the same as for the Collisio
 ### Configuration
 
 Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/collision_monitor/configuring-collision-detector-node.html).
+
+The `CollisionMonitor` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
index 09a1365809..7362b4baeb 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
@@ -22,12 +22,15 @@
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
 
 #include "tf2/time.h"
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
 
 #include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/twist_publisher.hpp"
+#include "nav2_util/twist_subscriber.hpp"
 #include "nav2_msgs/msg/collision_monitor_state.hpp"
 
 #include "nav2_collision_monitor/types.hpp"
@@ -95,13 +98,15 @@ class CollisionMonitor : public nav2_util::LifecycleNode
    * @brief Callback for input cmd_vel
    * @param msg Input cmd_vel message
    */
-  void cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg);
+  void cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped::SharedPtr msg);
+  void cmdVelInCallbackUnstamped(geometry_msgs::msg::Twist::SharedPtr msg);
   /**
    * @brief Publishes output cmd_vel. If robot was stopped more than stop_pub_timeout_ seconds,
    * quit to publish 0-velocity.
    * @param robot_action Robot action to publish
+   * @param header TwistStamped header to use
    */
-  void publishVelocity(const Action & robot_action);
+  void publishVelocity(const Action & robot_action, const std_msgs::msg::Header & header);
 
   /**
    * @brief Supporting routine obtaining all ROS-parameters
@@ -145,8 +150,9 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   /**
    * @brief Main processing routine
    * @param cmd_vel_in Input desired robot velocity
+   * @param header Twist header
    */
-  void process(const Velocity & cmd_vel_in);
+  void process(const Velocity & cmd_vel_in, const std_msgs::msg::Header & header);
 
   /**
    * @brief Processes the polygon of STOP, SLOWDOWN and LIMIT action type
@@ -203,10 +209,10 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   std::vector<std::shared_ptr<Source>> sources_;
 
   // Input/output speed controls
-  /// @beirf Input cmd_vel subscriber
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_in_sub_;
+  /// @brief Input cmd_vel subscriber
+  std::unique_ptr<nav2_util::TwistSubscriber> cmd_vel_in_sub_;
   /// @brief Output cmd_vel publisher
-  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_out_pub_;
+  std::unique_ptr<nav2_util::TwistPublisher> cmd_vel_out_pub_;
 
   /// @brief CollisionMonitor state publisher
   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionMonitorState>::SharedPtr
diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
index 29e3f3031d..e3018e1020 100644
--- a/nav2_collision_monitor/src/collision_monitor_node.cpp
+++ b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -63,11 +63,15 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/)
     return nav2_util::CallbackReturn::FAILURE;
   }
 
-  cmd_vel_in_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
-    cmd_vel_in_topic, 1,
-    std::bind(&CollisionMonitor::cmdVelInCallback, this, std::placeholders::_1));
-  cmd_vel_out_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
-    cmd_vel_out_topic, 1);
+  cmd_vel_in_sub_ = std::make_unique<nav2_util::TwistSubscriber>(
+    shared_from_this(),
+    cmd_vel_in_topic,
+    1,
+    std::bind(&CollisionMonitor::cmdVelInCallbackUnstamped, this, std::placeholders::_1),
+    std::bind(&CollisionMonitor::cmdVelInCallbackStamped, this, std::placeholders::_1));
+
+  auto node = shared_from_this();
+  cmd_vel_out_pub_ = std::make_unique<nav2_util::TwistPublisher>(node, cmd_vel_out_topic, 1);
 
   if (!state_topic.empty()) {
     state_pub_ = this->create_publisher<nav2_msgs::msg::CollisionMonitorState>(
@@ -77,7 +81,6 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/)
   collision_points_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
     "~/collision_points_marker", 1);
 
-  auto node = shared_from_this();
   nav2_util::declare_parameter_if_not_declared(
     node, "use_realtime_priority", rclcpp::ParameterValue(false));
   bool use_realtime_priority = false;
@@ -180,7 +183,7 @@ CollisionMonitor::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
-void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg)
+void CollisionMonitor::cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped::SharedPtr msg)
 {
   // If message contains NaN or Inf, ignore
   if (!nav2_util::validateTwist(*msg)) {
@@ -188,10 +191,18 @@ void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPt
     return;
   }
 
-  process({msg->linear.x, msg->linear.y, msg->angular.z});
+  process({msg->twist.linear.x, msg->twist.linear.y, msg->twist.angular.z}, msg->header);
+}
+
+void CollisionMonitor::cmdVelInCallbackUnstamped(geometry_msgs::msg::Twist::SharedPtr msg)
+{
+  auto twist_stamped = std::make_shared<geometry_msgs::msg::TwistStamped>();
+  twist_stamped->twist = *msg;
+  cmdVelInCallbackStamped(twist_stamped);
 }
 
-void CollisionMonitor::publishVelocity(const Action & robot_action)
+void CollisionMonitor::publishVelocity(
+  const Action & robot_action, const std_msgs::msg::Header & header)
 {
   if (robot_action.req_vel.isZero()) {
     if (!robot_action_prev_.req_vel.isZero()) {
@@ -204,11 +215,11 @@ void CollisionMonitor::publishVelocity(const Action & robot_action)
     }
   }
 
-  std::unique_ptr<geometry_msgs::msg::Twist> cmd_vel_out_msg =
-    std::make_unique<geometry_msgs::msg::Twist>();
-  cmd_vel_out_msg->linear.x = robot_action.req_vel.x;
-  cmd_vel_out_msg->linear.y = robot_action.req_vel.y;
-  cmd_vel_out_msg->angular.z = robot_action.req_vel.tw;
+  auto cmd_vel_out_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
+  cmd_vel_out_msg->header = header;
+  cmd_vel_out_msg->twist.linear.x = robot_action.req_vel.x;
+  cmd_vel_out_msg->twist.linear.y = robot_action.req_vel.y;
+  cmd_vel_out_msg->twist.angular.z = robot_action.req_vel.tw;
   // linear.z, angular.x and angular.y will remain 0.0
 
   cmd_vel_out_pub_->publish(std::move(cmd_vel_out_msg));
@@ -379,7 +390,7 @@ bool CollisionMonitor::configureSources(
   return true;
 }
 
-void CollisionMonitor::process(const Velocity & cmd_vel_in)
+void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg::Header & header)
 {
   // Current timestamp for all inner routines prolongation
   rclcpp::Time curr_time = this->now();
@@ -473,7 +484,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
   }
 
   // Publish required robot velocity
-  publishVelocity(robot_action);
+  publishVelocity(robot_action, header);
 
   // Publish polygons for better visualization
   publishPolygons();
diff --git a/nav2_controller/README.md b/nav2_controller/README.md
index 3a77144f16..98b76c3355 100644
--- a/nav2_controller/README.md
+++ b/nav2_controller/README.md
@@ -7,3 +7,5 @@ An execution module implementing the `nav2_msgs::action::FollowPath` action serv
 See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available controller plugins. 
 
 See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-controller-server.html) for additional parameter descriptions and a [tutorial about writing controller plugins](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html).
+
+The `ControllerServer` makes use of a [nav2_util::TwistPublisher](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).
diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp
index 992c8fab04..0f250dee14 100644
--- a/nav2_controller/include/nav2_controller/controller_server.hpp
+++ b/nav2_controller/include/nav2_controller/controller_server.hpp
@@ -33,6 +33,7 @@
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_util/simple_action_server.hpp"
 #include "nav2_util/robot_utils.hpp"
+#include "nav2_util/twist_publisher.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
 
@@ -229,7 +230,7 @@ class ControllerServer : public nav2_util::LifecycleNode
 
   // Publishers and subscribers
   std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
-  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_publisher_;
+  std::unique_ptr<nav2_util::TwistPublisher> vel_publisher_;
   rclcpp::Subscription<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_sub_;
 
   // Progress Checker Plugin
diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
index df337762c8..17c3b510dc 100644
--- a/nav2_controller/src/controller_server.cpp
+++ b/nav2_controller/src/controller_server.cpp
@@ -215,7 +215,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
     "Controller Server has %s controllers available.", controller_ids_concat_.c_str());
 
   odom_sub_ = std::make_unique<nav_2d_utils::OdomSubscriber>(node);
-  vel_publisher_ = create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
+  vel_publisher_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel", 1);
 
   double action_server_result_timeout;
   get_parameter("action_server_result_timeout", action_server_result_timeout);
@@ -599,6 +599,8 @@ void ControllerServer::computeAndPublishVelocity()
       nav_2d_utils::twist2Dto3D(twist),
       goal_checkers_[current_goal_checker_].get());
     last_valid_cmd_time_ = now();
+    cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID();
+    cmd_vel_2d.header.stamp = last_valid_cmd_time_;
     // Only no valid control exception types are valid to attempt to have control patience, as
     // other types will not be resolved with more attempts
   } catch (nav2_core::NoValidControl & e) {
@@ -697,7 +699,7 @@ void ControllerServer::updateGlobalPath()
 
 void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity)
 {
-  auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>(velocity.twist);
+  auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>(velocity);
   if (vel_publisher_->is_activated() && vel_publisher_->get_subscription_count() > 0) {
     vel_publisher_->publish(std::move(cmd_vel));
   }
diff --git a/nav2_util/README.md b/nav2_util/README.md
index 844370983c..adf71e61c2 100644
--- a/nav2_util/README.md
+++ b/nav2_util/README.md
@@ -7,6 +7,25 @@ The `nav2_util` package contains utilities abstracted from individual packages w
 - Simplified service clients
 - Simplified action servers
 - Transformation and robot pose helpers
+- Twist Subscriber and Twist Publisher
 
 The long-term aim is for these utilities to find more permanent homes in other packages (within and outside of Nav2) or migrate to the raw tools made available in ROS 2.
- 
\ No newline at end of file
+
+## Twist Publisher and Twist Subscriber for commanded velocities
+
+### Background 
+
+The Twist Publisher and Twist Subscriber are utility classes to assist NAV2 transition from 
+[Twist](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/Twist.msg) to [TwistStamped](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/TwistStamped.msg).
+
+Details on the migration are found in [#1594](https://github.com/ros-planning/navigation2/issues/1594).
+
+Certain applications of NAV2, such as in ROS Aerial mandate the usage of `TwistStamped`, while many other applications still use `Twist`.
+
+The utility has the following effect:
+* Allows use of either `Twist` or `TwistStamped`, controlled by ROS parameter `enable_stamped_cmd_vel`
+* Preserves existing topic names without duplication of data
+
+Every node in `nav2` that subscribes or publishes velocity commands with `Twist` now supports this optional behavior.
+The behavior up through ROS 2 Iron is preserved - using `Twist`. In a future ROS 2 version, when enough of the
+ROS ecosystem has moved to `TwistStamped`, the default may change. 
diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp
index bcd2d2ae83..3cc8f87267 100644
--- a/nav2_util/include/nav2_util/robot_utils.hpp
+++ b/nav2_util/include/nav2_util/robot_utils.hpp
@@ -22,6 +22,7 @@
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
 #include "tf2/time.h"
 #include "tf2_ros/buffer.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
@@ -103,7 +104,8 @@ bool getTransform(
  * @param msg Twist message to validate
  * @return True if valid, false if contains unactionable values
  */
-bool validateTwist(const geometry_msgs::msg::Twist & msg);
+[[nodiscard]] bool validateTwist(const geometry_msgs::msg::Twist & msg);
+[[nodiscard]] bool validateTwist(const geometry_msgs::msg::TwistStamped & msg);
 
 }  // end namespace nav2_util
 
diff --git a/nav2_util/include/nav2_util/twist_publisher.hpp b/nav2_util/include/nav2_util/twist_publisher.hpp
new file mode 100644
index 0000000000..b86fdee5e7
--- /dev/null
+++ b/nav2_util/include/nav2_util/twist_publisher.hpp
@@ -0,0 +1,133 @@
+// Copyright (C) 2023 Ryan Friedman
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#ifndef NAV2_UTIL__TWIST_PUBLISHER_HPP_
+#define NAV2_UTIL__TWIST_PUBLISHER_HPP_
+
+
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "rclcpp/parameter_service.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
+
+#include "lifecycle_node.hpp"
+#include "node_utils.hpp"
+
+namespace nav2_util
+{
+
+/**
+ * @class nav2_util::TwistPublisher
+ * @brief A simple wrapper on a Twist publisher that provides either Twist or TwistStamped
+ *
+ * The default is to publish Twist to preserve backwards compatibility, but it can be overridden
+ * using the "enable_stamped_cmd_vel" parameter to publish TwistStamped.
+ *
+ */
+
+class TwistPublisher
+{
+public:
+  /**
+  * @brief A constructor
+  * @param nh The node
+  * @param topic publisher topic name
+  * @param qos publisher quality of service
+  */
+  explicit TwistPublisher(
+    nav2_util::LifecycleNode::SharedPtr node,
+    const std::string & topic,
+    const rclcpp::QoS & qos)
+  : topic_(topic)
+  {
+    using nav2_util::declare_parameter_if_not_declared;
+    declare_parameter_if_not_declared(
+      node, "enable_stamped_cmd_vel",
+      rclcpp::ParameterValue{false});
+    node->get_parameter("enable_stamped_cmd_vel", is_stamped_);
+    if (is_stamped_) {
+      twist_stamped_pub_ = node->create_publisher<geometry_msgs::msg::TwistStamped>(
+        topic_,
+        qos);
+    } else {
+      twist_pub_ = node->create_publisher<geometry_msgs::msg::Twist>(
+        topic_,
+        qos);
+    }
+  }
+
+  void on_activate()
+  {
+    if (is_stamped_) {
+      twist_stamped_pub_->on_activate();
+    } else {
+      twist_pub_->on_activate();
+    }
+  }
+
+  void on_deactivate()
+  {
+    if (is_stamped_) {
+      twist_stamped_pub_->on_deactivate();
+    } else {
+      twist_pub_->on_deactivate();
+    }
+  }
+
+  [[nodiscard]] bool is_activated() const
+  {
+    if (is_stamped_) {
+      return twist_stamped_pub_->is_activated();
+    } else {
+      return twist_pub_->is_activated();
+    }
+  }
+
+  void publish(std::unique_ptr<geometry_msgs::msg::TwistStamped> velocity)
+  {
+    if (is_stamped_) {
+      twist_stamped_pub_->publish(std::move(velocity));
+    } else {
+      auto twist_msg = std::make_unique<geometry_msgs::msg::Twist>(velocity->twist);
+      twist_pub_->publish(std::move(twist_msg));
+    }
+  }
+
+  [[nodiscard]] size_t get_subscription_count() const
+  {
+    if (is_stamped_) {
+      return twist_stamped_pub_->get_subscription_count();
+    } else {
+      return twist_pub_->get_subscription_count();
+    }
+  }
+
+protected:
+  std::string topic_;
+  bool is_stamped_;
+  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
+  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::TwistStamped>::SharedPtr
+    twist_stamped_pub_;
+};
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__TWIST_PUBLISHER_HPP_
diff --git a/nav2_util/include/nav2_util/twist_subscriber.hpp b/nav2_util/include/nav2_util/twist_subscriber.hpp
new file mode 100644
index 0000000000..01f9ca25fe
--- /dev/null
+++ b/nav2_util/include/nav2_util/twist_subscriber.hpp
@@ -0,0 +1,153 @@
+// Copyright (C) 2023 Ryan Friedman
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#ifndef NAV2_UTIL__TWIST_SUBSCRIBER_HPP_
+#define NAV2_UTIL__TWIST_SUBSCRIBER_HPP_
+
+
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "rclcpp/parameter_service.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
+
+#include "lifecycle_node.hpp"
+#include "node_utils.hpp"
+
+namespace nav2_util
+{
+
+/**
+ * @class nav2_util::TwistSubscriber
+ * @brief A simple wrapper on a Twist subscriber that receives either
+ *        geometry_msgs::msg::TwistStamped or geometry_msgs::msg::Twist
+ *
+ * @note Usage:
+ * The default behavior is to subscribe to Twist, which preserves backwards compatibility
+ * with ROS distributions up to Iron.
+ * The behavior can be overridden using the "enable_stamped_cmd_vel" parameter.
+ * By setting that to "True", the TwistSubscriber class would subscribe to TwistStamped.
+ *
+ * @note Why use Twist Stamped over Twist?
+ * Twist has been used widely in many ROS applications, typically for body-frame velocity control,
+ * and between ROS nodes on the same computer. Many ROS interfaces are moving to using TwistStamped
+ * because it is more robust for stale data protection. This protection is especially important
+ * when sending velocity control over lossy communication links.
+ * An example application where this matters is a drone with a Linux computer running a ROS
+ * controller that sends Twist commands to an embedded autopilot. If the autopilot failed to
+ * recognize a highly latent connection, it could result in instability or a crash because of the
+ * decreased phase margin for control.
+ * TwistStamped also has a frame ID, allowing explicit control for multiple frames, rather than
+ * relying on an assumption of body-frame control or having to create a different topic.
+ * Adding a header is low-cost for most ROS applications; the header can be set to an empty string
+ * if bandwidth is of concern.
+ *
+ * @note Implementation Design Notes:
+ * Compared to the naive approach of setting up one subscriber for each message type,
+ * only one subscriber is created at a time; the other is nullptr.
+ * This reduces RAM usage and ROS discovery traffic.
+ * This approach allows NAV2 libraries to be flexible in which Twist message they support,
+ * while maintaining a stable API in a ROS distribution.
+ *
+ */
+
+class TwistSubscriber
+{
+public:
+  /**
+  * @brief A constructor that supports either Twist and TwistStamped
+  * @param node The node to add the Twist subscriber to
+  * @param topic The subscriber topic name
+  * @param qos The subscriber quality of service
+  * @param TwistCallback The subscriber callback for Twist messages
+  * @param TwistStampedCallback The subscriber callback for TwistStamped messages
+  */
+  template<typename TwistCallbackT,
+    typename TwistStampedCallbackT
+  >
+  explicit TwistSubscriber(
+    nav2_util::LifecycleNode::SharedPtr node,
+    const std::string & topic,
+    const rclcpp::QoS & qos,
+    TwistCallbackT && TwistCallback,
+    TwistStampedCallbackT && TwistStampedCallback
+  )
+  {
+    nav2_util::declare_parameter_if_not_declared(
+      node, "enable_stamped_cmd_vel",
+      rclcpp::ParameterValue(false));
+    node->get_parameter("enable_stamped_cmd_vel", is_stamped_);
+    if (is_stamped_) {
+      twist_stamped_sub_ = node->create_subscription<geometry_msgs::msg::TwistStamped>(
+        topic,
+        qos,
+        std::forward<TwistStampedCallbackT>(TwistStampedCallback));
+    } else {
+      twist_sub_ = node->create_subscription<geometry_msgs::msg::Twist>(
+        topic,
+        qos,
+        std::forward<TwistCallbackT>(TwistCallback));
+    }
+  }
+
+  /**
+  * @brief A constructor that only supports TwistStamped
+  * @param node The node to add the TwistStamped subscriber to
+  * @param topic The subscriber topic name
+  * @param qos The subscriber quality of service
+  * @param TwistStampedCallback The subscriber callback for TwistStamped messages
+  * @throw std::invalid_argument When configured with an invalid ROS parameter
+  */
+  template<typename TwistStampedCallbackT>
+  explicit TwistSubscriber(
+    nav2_util::LifecycleNode::SharedPtr node,
+    const std::string & topic,
+    const rclcpp::QoS & qos,
+    TwistStampedCallbackT && TwistStampedCallback
+  )
+  {
+    nav2_util::declare_parameter_if_not_declared(
+      node, "enable_stamped_cmd_vel",
+      rclcpp::ParameterValue(false));
+    node->get_parameter("enable_stamped_cmd_vel", is_stamped_);
+    if (is_stamped_) {
+      twist_stamped_sub_ = node->create_subscription<geometry_msgs::msg::TwistStamped>(
+        topic,
+        qos,
+        std::forward<TwistStampedCallbackT>(TwistStampedCallback));
+    } else {
+      throw std::invalid_argument(
+              "enable_stamped_cmd_vel must be true when using this constructor!");
+    }
+  }
+
+protected:
+  //! @brief The user-configured value for ROS parameter enable_stamped_cmd_vel
+  bool is_stamped_{false};
+  //! @brief The subscription when using Twist
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_ {nullptr};
+  //! @brief The subscription when using TwistStamped
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_stamped_sub_ {nullptr};
+};
+
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__TWIST_SUBSCRIBER_HPP_
diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp
index e6a6e10cf4..6933b71694 100644
--- a/nav2_util/src/robot_utils.cpp
+++ b/nav2_util/src/robot_utils.cpp
@@ -172,4 +172,9 @@ bool validateTwist(const geometry_msgs::msg::Twist & msg)
   return true;
 }
 
+bool validateTwist(const geometry_msgs::msg::TwistStamped & msg)
+{
+  return validateTwist(msg.twist);
+}
+
 }  // end namespace nav2_util
diff --git a/nav2_util/test/test_twist_publisher.cpp b/nav2_util/test/test_twist_publisher.cpp
new file mode 100644
index 0000000000..94361e94aa
--- /dev/null
+++ b/nav2_util/test/test_twist_publisher.cpp
@@ -0,0 +1,101 @@
+// Copyright (C) 2023 Ryan Friedman
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <string>
+
+#include "nav2_util/twist_publisher.hpp"
+#include "nav2_util/lifecycle_utils.hpp"
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
+
+using nav2_util::startup_lifecycle_nodes;
+using nav2_util::reset_lifecycle_nodes;
+
+TEST(TwistPublisher, Unstamped)
+{
+  rclcpp::init(0, nullptr);
+  auto pub_node = std::make_shared<nav2_util::LifecycleNode>("pub_node", "");
+  pub_node->configure();
+  auto vel_publisher = std::make_unique<nav2_util::TwistPublisher>(pub_node, "cmd_vel", 1);
+  ASSERT_EQ(vel_publisher->get_subscription_count(), 0);
+  EXPECT_FALSE(vel_publisher->is_activated());
+  pub_node->activate();
+  EXPECT_TRUE(vel_publisher->is_activated());
+  vel_publisher->on_activate();
+  auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node->get_node_base_interface());});
+
+  auto sub_node = std::make_shared<nav2_util::LifecycleNode>("sub_node", "");
+  sub_node->configure();
+  sub_node->activate();
+
+  auto pub_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
+  pub_msg->twist.linear.x = 42.0;
+  auto pub_msg_copy = pub_msg->twist;
+
+  geometry_msgs::msg::Twist sub_msg {};
+  auto my_sub = sub_node->create_subscription<geometry_msgs::msg::Twist>(
+    "cmd_vel", 10,
+    [&](const geometry_msgs::msg::Twist msg) {sub_msg = msg;});
+
+  vel_publisher->publish(std::move(pub_msg));
+  rclcpp::spin_some(sub_node->get_node_base_interface());
+
+  EXPECT_EQ(pub_msg_copy.linear.x, sub_msg.linear.x);
+  EXPECT_EQ(vel_publisher->get_subscription_count(), 1);
+  pub_node->deactivate();
+  sub_node->deactivate();
+  rclcpp::shutdown();
+  // // Have to join thread after rclcpp is shut down otherwise test hangs.
+  pub_thread.join();
+}
+
+TEST(TwistPublisher, Stamped)
+{
+  rclcpp::init(0, nullptr);
+  auto pub_node = std::make_shared<nav2_util::LifecycleNode>("pub_node", "");
+  pub_node->declare_parameter("enable_stamped_cmd_vel", true);
+  pub_node->configure();
+  auto vel_publisher = std::make_unique<nav2_util::TwistPublisher>(pub_node, "cmd_vel", 1);
+  ASSERT_EQ(vel_publisher->get_subscription_count(), 0);
+  EXPECT_FALSE(vel_publisher->is_activated());
+  pub_node->activate();
+  EXPECT_TRUE(vel_publisher->is_activated());
+  auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node->get_node_base_interface());});
+
+  auto sub_node = std::make_shared<nav2_util::LifecycleNode>("sub_node", "");
+  sub_node->configure();
+  sub_node->activate();
+
+  auto pub_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
+  pub_msg->twist.linear.x = 42.0;
+  pub_msg->header.frame_id = "foo";
+  auto pub_msg_copy = *pub_msg;
+
+  geometry_msgs::msg::TwistStamped sub_msg {};
+  auto my_sub = sub_node->create_subscription<geometry_msgs::msg::TwistStamped>(
+    "cmd_vel", 10,
+    [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;});
+
+  vel_publisher->publish(std::move(pub_msg));
+  rclcpp::spin_some(sub_node->get_node_base_interface());
+  ASSERT_EQ(vel_publisher->get_subscription_count(), 1);
+  EXPECT_EQ(pub_msg_copy, sub_msg);
+  pub_node->deactivate();
+  sub_node->deactivate();
+  rclcpp::shutdown();
+  // Have to join thread after rclcpp is shut down otherwise test hangs.
+  pub_thread.join();
+}
diff --git a/nav2_util/test/test_twist_subscriber.cpp b/nav2_util/test/test_twist_subscriber.cpp
new file mode 100644
index 0000000000..5672887368
--- /dev/null
+++ b/nav2_util/test/test_twist_subscriber.cpp
@@ -0,0 +1,97 @@
+// Copyright (C) 2023 Ryan Friedman
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <string>
+
+#include "nav2_util/twist_subscriber.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+
+
+TEST(TwistSubscriber, Unstamped)
+{
+  rclcpp::init(0, nullptr);
+  auto sub_node = std::make_shared<nav2_util::LifecycleNode>("sub_node", "");
+  sub_node->configure();
+  sub_node->activate();
+
+  geometry_msgs::msg::TwistStamped sub_msg {};
+  auto vel_subscriber = std::make_unique<nav2_util::TwistSubscriber>(
+    sub_node, "cmd_vel", 1,
+    [&](const geometry_msgs::msg::Twist msg) {sub_msg.twist = msg;},
+    [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;}
+  );
+
+  auto pub_node = std::make_shared<nav2_util::LifecycleNode>("pub_node", "");
+  pub_node->configure();
+
+  geometry_msgs::msg::TwistStamped pub_msg {};
+  pub_msg.twist.linear.x = 42.0;
+
+  auto vel_pub =
+    pub_node->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
+
+  pub_node->activate();
+  vel_pub->on_activate();
+
+  vel_pub->publish(pub_msg.twist);
+  rclcpp::spin_some(sub_node->get_node_base_interface());
+  ASSERT_EQ(vel_pub->get_subscription_count(), 1);
+  EXPECT_EQ(pub_msg, sub_msg);
+
+  pub_node->deactivate();
+  sub_node->deactivate();
+  rclcpp::shutdown();
+}
+
+TEST(TwistSubscriber, Stamped)
+{
+  rclcpp::init(0, nullptr);
+  auto sub_node = std::make_shared<nav2_util::LifecycleNode>("sub_node", "");
+  sub_node->declare_parameter("enable_stamped_cmd_vel", true);
+  sub_node->configure();
+  sub_node->activate();
+
+  geometry_msgs::msg::TwistStamped sub_msg {};
+  auto vel_subscriber = std::make_unique<nav2_util::TwistSubscriber>(
+    sub_node, "cmd_vel", 1,
+    [&](const geometry_msgs::msg::Twist msg) {sub_msg.twist = msg;},
+    [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;}
+  );
+
+  auto pub_node = std::make_shared<nav2_util::LifecycleNode>("pub_node", "");
+  pub_node->configure();
+
+  geometry_msgs::msg::TwistStamped pub_msg {};
+  pub_msg.twist.linear.x = 42.0;
+
+  auto vel_pub =
+    pub_node->create_publisher<geometry_msgs::msg::TwistStamped>("cmd_vel", 1);
+
+  pub_node->activate();
+  vel_pub->on_activate();
+
+  vel_pub->publish(pub_msg);
+  rclcpp::spin_some(sub_node->get_node_base_interface());
+  ASSERT_EQ(vel_pub->get_subscription_count(), 1);
+  EXPECT_EQ(pub_msg, sub_msg);
+
+  pub_node->deactivate();
+  sub_node->deactivate();
+  rclcpp::shutdown();
+}
diff --git a/nav2_velocity_smoother/README.md b/nav2_velocity_smoother/README.md
index 6ffbe5d372..a9554997e2 100644
--- a/nav2_velocity_smoother/README.md
+++ b/nav2_velocity_smoother/README.md
@@ -59,14 +59,15 @@ velocity_smoother:
    	max_decel: [-2.5, 0.0, -3.2]  # Maximum deceleration, ordered [Ax, Ay, Aw]
    	odom_topic: "odom"  # Topic of odometry to use for estimating current velocities
    	odom_duration: 0.1  # Period of time (s) to sample odometry information in for velocity estimation
+	enable_stamped_cmd_vel: false # Whether to stamp the velocity. True uses TwistStamped. False uses Twist
 ```
 
 ## Topics
 
 | Topic            | Type                    | Use                           |
 |------------------|-------------------------|-------------------------------|
-| smoothed_cmd_vel | geometry_msgs/Twist     | Publish smoothed velocities   |
-| cmd_vel          | geometry_msgs/Twist     | Subscribe to input velocities |
+| smoothed_cmd_vel | geometry_msgs/Twist or  geometry_msgs/TwistStamped | Publish smoothed velocities   |
+| cmd_vel          | geometry_msgs/Twist or  geometry_msgs/TwistStamped | Subscribe to input velocities |
 
 
 ## Install
@@ -85,3 +86,5 @@ When in doubt, open-loop is a reasonable choice for most users.
 The minimum and maximum velocities for rotation (e.g. ``Vw``) represent left and right turns. While we make it possible to specify these separately, most users would be wise to set these values the same (but signed) for rotation. Additionally, the parameters are signed, so it is important to specify maximum deceleration with negative signs to represent deceleration. Minimum velocities with negatives when moving backward, so backward movement can be restricted by setting this to ``0``.
 
 Deadband velocities are minimum thresholds, below which we set its value to `0`. This can be useful when your robot's breaking torque from stand still is non-trivial so sending very small values will pull high amounts of current.
+
+The `VelocitySmoother` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).
\ No newline at end of file
diff --git a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp
index a059e74076..f69a827a7d 100644
--- a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp
+++ b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp
@@ -26,6 +26,10 @@
 #include "nav2_util/node_utils.hpp"
 #include "nav2_util/odometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
+#include "nav2_util/twist_publisher.hpp"
+#include "nav2_util/twist_subscriber.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
 
 namespace nav2_velocity_smoother
 {
@@ -114,6 +118,7 @@ class VelocitySmoother : public nav2_util::LifecycleNode
    * @param msg Twist message
    */
   void inputCommandCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
+  void inputCommandStampedCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
 
   /**
    * @brief Main worker timer function
@@ -129,14 +134,13 @@ class VelocitySmoother : public nav2_util::LifecycleNode
 
   // Network interfaces
   std::unique_ptr<nav2_util::OdomSmoother> odom_smoother_;
-  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr
-    smoothed_cmd_pub_;
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
+  std::unique_ptr<nav2_util::TwistPublisher> smoothed_cmd_pub_;
+  std::unique_ptr<nav2_util::TwistSubscriber> cmd_sub_;
   rclcpp::TimerBase::SharedPtr timer_;
 
   rclcpp::Clock::SharedPtr clock_;
-  geometry_msgs::msg::Twist last_cmd_;
-  geometry_msgs::msg::Twist::SharedPtr command_;
+  geometry_msgs::msg::TwistStamped last_cmd_;
+  geometry_msgs::msg::TwistStamped::SharedPtr command_;
 
   // Parameters
   double smoothing_frequency_;
diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp
index d2dad65913..02fb390cc0 100644
--- a/nav2_velocity_smoother/src/velocity_smoother.cpp
+++ b/nav2_velocity_smoother/src/velocity_smoother.cpp
@@ -128,10 +128,13 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &)
   }
 
   // Setup inputs / outputs
-  smoothed_cmd_pub_ = create_publisher<geometry_msgs::msg::Twist>("cmd_vel_smoothed", 1);
-  cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>(
+  smoothed_cmd_pub_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel_smoothed", 1);
+  cmd_sub_ = std::make_unique<nav2_util::TwistSubscriber>(
+    node,
     "cmd_vel", rclcpp::QoS(1),
-    std::bind(&VelocitySmoother::inputCommandCallback, this, std::placeholders::_1));
+    std::bind(&VelocitySmoother::inputCommandCallback, this, std::placeholders::_1),
+    std::bind(&VelocitySmoother::inputCommandStampedCallback, this, std::placeholders::_1)
+  );
 
   declare_parameter_if_not_declared(node, "use_realtime_priority", rclcpp::ParameterValue(false));
   bool use_realtime_priority = false;
@@ -199,16 +202,29 @@ VelocitySmoother::on_shutdown(const rclcpp_lifecycle::State &)
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
-void VelocitySmoother::inputCommandCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
+void VelocitySmoother::inputCommandStampedCallback(
+  const geometry_msgs::msg::TwistStamped::SharedPtr msg)
 {
   // If message contains NaN or Inf, ignore
-  if (!nav2_util::validateTwist(*msg)) {
+  if (!nav2_util::validateTwist(msg->twist)) {
     RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!");
     return;
   }
 
   command_ = msg;
-  last_command_time_ = now();
+  if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0) {
+    last_command_time_ = now();
+  } else {
+    last_command_time_ = msg->header.stamp;
+  }
+}
+
+void VelocitySmoother::inputCommandCallback(
+  geometry_msgs::msg::Twist::SharedPtr msg)
+{
+  auto twist_stamped = std::make_shared<geometry_msgs::msg::TwistStamped>();
+  twist_stamped->twist = *msg;
+  inputCommandStampedCallback(twist_stamped);
 }
 
 double VelocitySmoother::findEtaConstraint(
@@ -272,31 +288,38 @@ void VelocitySmoother::smootherTimer()
     return;
   }
 
-  auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
+  auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
+  cmd_vel->header = command_->header;
 
   // Check for velocity timeout. If nothing received, publish zeros to apply deceleration
   if (now() - last_command_time_ > velocity_timeout_) {
-    if (last_cmd_ == geometry_msgs::msg::Twist() || stopped_) {
+    if (last_cmd_ == geometry_msgs::msg::TwistStamped() || stopped_) {
       stopped_ = true;
       return;
     }
-    *command_ = geometry_msgs::msg::Twist();
+    *command_ = geometry_msgs::msg::TwistStamped();
   }
 
   stopped_ = false;
 
   // Get current velocity based on feedback type
-  geometry_msgs::msg::Twist current_;
+  geometry_msgs::msg::TwistStamped current_;
   if (open_loop_) {
     current_ = last_cmd_;
   } else {
-    current_ = odom_smoother_->getTwist();
+    current_ = odom_smoother_->getTwistStamped();
   }
 
   // Apply absolute velocity restrictions to the command
-  command_->linear.x = std::clamp(command_->linear.x, min_velocities_[0], max_velocities_[0]);
-  command_->linear.y = std::clamp(command_->linear.y, min_velocities_[1], max_velocities_[1]);
-  command_->angular.z = std::clamp(command_->angular.z, min_velocities_[2], max_velocities_[2]);
+  command_->twist.linear.x = std::clamp(
+    command_->twist.linear.x, min_velocities_[0],
+    max_velocities_[0]);
+  command_->twist.linear.y = std::clamp(
+    command_->twist.linear.y, min_velocities_[1],
+    max_velocities_[1]);
+  command_->twist.angular.z = std::clamp(
+    command_->twist.angular.z, min_velocities_[2],
+    max_velocities_[2]);
 
   // Find if any component is not within the acceleration constraints. If so, store the most
   // significant scale factor to apply to the vector <dvx, dvy, dvw>, eta, to reduce all axes
@@ -308,37 +331,39 @@ void VelocitySmoother::smootherTimer()
     double curr_eta = -1.0;
 
     curr_eta = findEtaConstraint(
-      current_.linear.x, command_->linear.x, max_accels_[0], max_decels_[0]);
+      current_.twist.linear.x, command_->twist.linear.x, max_accels_[0], max_decels_[0]);
     if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) {
       eta = curr_eta;
     }
 
     curr_eta = findEtaConstraint(
-      current_.linear.y, command_->linear.y, max_accels_[1], max_decels_[1]);
+      current_.twist.linear.y, command_->twist.linear.y, max_accels_[1], max_decels_[1]);
     if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) {
       eta = curr_eta;
     }
 
     curr_eta = findEtaConstraint(
-      current_.angular.z, command_->angular.z, max_accels_[2], max_decels_[2]);
+      current_.twist.angular.z, command_->twist.angular.z, max_accels_[2], max_decels_[2]);
     if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) {
       eta = curr_eta;
     }
   }
 
-  cmd_vel->linear.x = applyConstraints(
-    current_.linear.x, command_->linear.x, max_accels_[0], max_decels_[0], eta);
-  cmd_vel->linear.y = applyConstraints(
-    current_.linear.y, command_->linear.y, max_accels_[1], max_decels_[1], eta);
-  cmd_vel->angular.z = applyConstraints(
-    current_.angular.z, command_->angular.z, max_accels_[2], max_decels_[2], eta);
+  cmd_vel->twist.linear.x = applyConstraints(
+    current_.twist.linear.x, command_->twist.linear.x, max_accels_[0], max_decels_[0], eta);
+  cmd_vel->twist.linear.y = applyConstraints(
+    current_.twist.linear.y, command_->twist.linear.y, max_accels_[1], max_decels_[1], eta);
+  cmd_vel->twist.angular.z = applyConstraints(
+    current_.twist.angular.z, command_->twist.angular.z, max_accels_[2], max_decels_[2], eta);
   last_cmd_ = *cmd_vel;
 
   // Apply deadband restrictions & publish
-  cmd_vel->linear.x = fabs(cmd_vel->linear.x) < deadband_velocities_[0] ? 0.0 : cmd_vel->linear.x;
-  cmd_vel->linear.y = fabs(cmd_vel->linear.y) < deadband_velocities_[1] ? 0.0 : cmd_vel->linear.y;
-  cmd_vel->angular.z = fabs(cmd_vel->angular.z) <
-    deadband_velocities_[2] ? 0.0 : cmd_vel->angular.z;
+  cmd_vel->twist.linear.x =
+    fabs(cmd_vel->twist.linear.x) < deadband_velocities_[0] ? 0.0 : cmd_vel->twist.linear.x;
+  cmd_vel->twist.linear.y =
+    fabs(cmd_vel->twist.linear.y) < deadband_velocities_[1] ? 0.0 : cmd_vel->twist.linear.y;
+  cmd_vel->twist.angular.z =
+    fabs(cmd_vel->twist.angular.z) < deadband_velocities_[2] ? 0.0 : cmd_vel->twist.angular.z;
 
   smoothed_cmd_pub_->publish(std::move(cmd_vel));
 }
diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp
index c58e20b22c..25754d0f35 100644
--- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp
+++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp
@@ -23,6 +23,7 @@
 #include "nav2_velocity_smoother/velocity_smoother.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "geometry_msgs/msg/twist.hpp"
+#include "nav2_util/twist_subscriber.hpp"
 
 using namespace std::chrono_literals;
 
@@ -47,7 +48,7 @@ class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother
 
   bool isOdomSmoother() {return odom_smoother_ ? true : false;}
   bool hasCommandMsg() {return last_command_time_.nanoseconds() != 0;}
-  geometry_msgs::msg::Twist::SharedPtr lastCommandMsg() {return command_;}
+  geometry_msgs::msg::TwistStamped::SharedPtr lastCommandMsg() {return command_;}
 
   void sendCommandMsg(geometry_msgs::msg::Twist::SharedPtr msg) {inputCommandCallback(msg);}
 };
@@ -66,11 +67,14 @@ TEST(VelocitySmootherTest, openLoopTestTimer)
   smoother->activate(state);
 
   std::vector<double> linear_vels;
-  auto subscription = smoother->create_subscription<geometry_msgs::msg::Twist>(
+  auto subscription = nav2_util::TwistSubscriber(
+    smoother,
     "cmd_vel_smoothed",
     1,
     [&](geometry_msgs::msg::Twist::SharedPtr msg) {
       linear_vels.push_back(msg->linear.x);
+    }, [&](geometry_msgs::msg::TwistStamped::SharedPtr msg) {
+      linear_vels.push_back(msg->twist.linear.x);
     });
 
   // Send a velocity command
@@ -117,11 +121,14 @@ TEST(VelocitySmootherTest, approxClosedLoopTestTimer)
   smoother->activate(state);
 
   std::vector<double> linear_vels;
-  auto subscription = smoother->create_subscription<geometry_msgs::msg::Twist>(
+  auto subscription = nav2_util::TwistSubscriber(
+    smoother,
     "cmd_vel_smoothed",
     1,
     [&](geometry_msgs::msg::Twist::SharedPtr msg) {
       linear_vels.push_back(msg->linear.x);
+    }, [&](geometry_msgs::msg::TwistStamped::SharedPtr msg) {
+      linear_vels.push_back(msg->twist.linear.x);
     });
 
   auto odom_pub = smoother->create_publisher<nav_msgs::msg::Odometry>("odom", 1);
@@ -566,7 +573,7 @@ TEST(VelocitySmootherTest, testCommandCallback)
   rclcpp::spin_some(smoother->get_node_base_interface());
 
   EXPECT_TRUE(smoother->hasCommandMsg());
-  EXPECT_EQ(smoother->lastCommandMsg()->linear.x, 100.0);
+  EXPECT_EQ(smoother->lastCommandMsg()->twist.linear.x, 100.0);
 }
 
 TEST(VelocitySmootherTest, testClosedLoopSub)