From e7df24db0332c4f19e29b74092bba17fed8f374a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 26 Jun 2023 16:05:36 -0700 Subject: [PATCH] Adding nan twist rejection for velocity smoother and collision monitor (#3658) * adding nan twist rejection for velocity smoother and collision monitor * deref --- .../collision_monitor_node.hpp | 2 ++ .../src/collision_monitor_node.cpp | 6 ++++ nav2_util/include/nav2_util/robot_utils.hpp | 9 ++++++ nav2_util/src/robot_utils.cpp | 30 +++++++++++++++++++ nav2_util/test/test_robot_utils.cpp | 26 ++++++++++++++++ .../velocity_smoother.hpp | 1 + .../src/velocity_smoother.cpp | 6 ++++ 7 files changed, 80 insertions(+) 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 2dc2995856..e26ca55495 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 @@ -27,6 +27,8 @@ #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" #include "nav2_collision_monitor/polygon.hpp" diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 939555bf3d..69523e3c14 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -148,6 +148,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}); } diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index baead03cba..e653e39a25 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -20,6 +20,8 @@ #include #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" #include "rclcpp/rclcpp.hpp" @@ -107,6 +109,13 @@ bool getTransform( const std::shared_ptr 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_ diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 37ab261230..20791e8293 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -15,6 +15,7 @@ // limitations under the License. #include +#include #include #include "nav2_util/robot_utils.hpp" @@ -141,4 +142,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 diff --git a/nav2_util/test/test_robot_utils.cpp b/nav2_util/test/test_robot_utils.cpp index bd277515ab..d8922868fb 100644 --- a/nav2_util/test/test_robot_utils.cpp +++ b/nav2_util/test/test_robot_utils.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #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)); +} 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 c3e7835c6b..a059e74076 100644 --- a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp +++ b/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 { diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index aa5fbec783..2596c073ba 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/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(); }