Adding nan twist rejection for velocity smoother and collision monitor

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -27,6 +27,7 @@
 #include "tf2_ros/transform_listener.h"
 
 #include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/robot_utils.hpp"
 #include "nav2_msgs/msg/collision_monitor_state.hpp"
 
 #include "nav2_collision_monitor/types.hpp"

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -160,6 +160,12 @@ CollisionMonitor::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
 
 void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg)
 {
+  // If message contains NaN or Inf, ignore
+  if (!nav2_util::validateTwist(*msg)) {
+    RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!");
+    return;
+  }
+
   process({msg->linear.x, msg->linear.y, msg->angular.z});
 }
 

nav2_util/include/nav2_util/robot_utils.hpp

@@ -21,6 +21,7 @@
 #include <memory>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
 #include "tf2/time.h"
 #include "tf2_ros/buffer.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
@@ -109,6 +110,13 @@ bool getTransform(
   const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
   tf2::Transform & tf2_transform);
 
+/**
+ * @brief Validates a twist message contains no nans or infs
+ * @param msg Twist message to validate
+ * @return True if valid, false if contains unactionable values
+ */
+bool validateTwist(const geometry_msgs::msg::Twist & msg);
+
 }  // end namespace nav2_util
 
 #endif  // NAV2_UTIL__ROBOT_UTILS_HPP_

nav2_util/src/robot_utils.cpp

@@ -15,6 +15,7 @@
 // limitations under the License.
 
 #include <string>
+#include <cmath>
 #include <memory>
 
 #include "tf2/convert.h"
@@ -142,4 +143,33 @@ bool getTransform(
   return true;
 }
 
+bool validateTwist(const geometry_msgs::msg::Twist & msg)
+{
+  if (std::isinf(msg.linear.x) || std::isnan(msg.linear.x)) {
+    return false;
+  }
+
+  if (std::isinf(msg.linear.y) || std::isnan(msg.linear.y)) {
+    return false;
+  }
+
+  if (std::isinf(msg.linear.z) || std::isnan(msg.linear.z)) {
+    return false;
+  }
+
+  if (std::isinf(msg.angular.x) || std::isnan(msg.angular.x)) {
+    return false;
+  }
+
+  if (std::isinf(msg.angular.y) || std::isnan(msg.angular.y)) {
+    return false;
+  }
+
+  if (std::isinf(msg.angular.z) || std::isnan(msg.angular.z)) {
+    return false;
+  }
+
+  return true;
+}
+
 }  // end namespace nav2_util

nav2_util/test/test_robot_utils.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <memory>
+#include <cmath>
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "tf2_ros/transform_listener.h"
@@ -32,3 +33,28 @@ TEST(RobotUtils, LookupExceptionError)
   global_pose.header.frame_id = "base_link";
   ASSERT_FALSE(nav2_util::transformPoseInTargetFrame(global_pose, global_pose, tf, "map", 0.1));
 }
+
+TEST(RobotUtils, validateTwist)
+{
+  geometry_msgs::msg::Twist msg;
+  EXPECT_TRUE(nav2_util::validateTwist(msg));
+
+  msg.linear.x = NAN;
+  EXPECT_FALSE(nav2_util::validateTwist(msg));
+  msg.linear.x = 1;
+  msg.linear.y = NAN;
+  EXPECT_FALSE(nav2_util::validateTwist(msg));
+  msg.linear.y = 1;
+  msg.linear.z = NAN;
+  EXPECT_FALSE(nav2_util::validateTwist(msg));
+
+  msg.linear.z = 1;
+  msg.angular.x = NAN;
+  EXPECT_FALSE(nav2_util::validateTwist(msg));
+  msg.angular.x = 1;
+  msg.angular.y = NAN;
+  EXPECT_FALSE(nav2_util::validateTwist(msg));
+  msg.angular.y = 1;
+  msg.angular.z = NAN;
+  EXPECT_FALSE(nav2_util::validateTwist(msg));
+}

nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp

@@ -25,6 +25,7 @@
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_util/node_utils.hpp"
 #include "nav2_util/odometry_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
 
 namespace nav2_velocity_smoother
 {

nav2_velocity_smoother/src/velocity_smoother.cpp

@@ -174,6 +174,12 @@ VelocitySmoother::on_shutdown(const rclcpp_lifecycle::State &)
 
 void VelocitySmoother::inputCommandCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
 {
+  // If message contains NaN or Inf, ignore
+  if (!nav2_util::validateTwist(*msg)) {
+    RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!");
+    return;
+  }
+
   command_ = msg;
   last_command_time_ = now();
 }