diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
index c336c59782c..472a7d255e3 100644
--- a/nav2_collision_monitor/src/collision_monitor_node.cpp
+++ b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -188,7 +188,7 @@ CollisionMonitor::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
 void CollisionMonitor::cmdVelInCallbackStamped(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;
   }
diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
index eeb02a554cb..a0b78c04f09 100644
--- a/nav2_controller/src/controller_server.cpp
+++ b/nav2_controller/src/controller_server.cpp
@@ -758,6 +758,10 @@ void ControllerServer::updateGlobalPath()
 void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity)
 {
   auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>(velocity);
+  if (!nav2_util::validateTwist(cmd_vel->twist)) {
+    RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!");
+    return;
+  }
   if (vel_publisher_->is_activated() && vel_publisher_->get_subscription_count() > 0) {
     vel_publisher_->publish(std::move(cmd_vel));
   }
diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp
index e76a2cce708..cffbe80a26b 100644
--- a/nav2_util/include/nav2_util/robot_utils.hpp
+++ b/nav2_util/include/nav2_util/robot_utils.hpp
@@ -146,7 +146,6 @@ bool getTransform(
  * @return True if valid, false if contains unactionable values
  */
 [[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/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp
index 736709ce641..e9e26a9fdb5 100644
--- a/nav2_util/src/robot_utils.cpp
+++ b/nav2_util/src/robot_utils.cpp
@@ -202,9 +202,4 @@ 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