Remove duplicate code

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;
   }

nav2_controller/src/controller_server.cpp

@@ -758,6 +758,10 @@
 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));
   }

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
 

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