fix: added validation and tests to range sensor message types for nav2_collision_monitor (#6231)

* fix: added validation and tests to range sensor message types for nav2_collision_monitor

Signed-off-by: Mann Bhanushali <bhanushalimann@gmail.com>

* fix: Addressed Review changes for nav2_collision_monitor range message validation tests.

Signed-off-by: Mann Bhanushali <bhanushalimann@gmail.com>

* fix: Added unit tests for nav2_collision_monitor range validation updates

Signed-off-by: Mann Bhanushali <bhanushalimann@gmail.com>

---------

Signed-off-by: Mann Bhanushali <bhanushalimann@gmail.com>

Author: Innomer

nav2_collision_monitor/src/range.cpp

@@ -21,11 +21,14 @@
 #include "tf2/transform_datatypes.hpp"
 
 #include "nav2_ros_common/node_utils.hpp"
+#include "nav2_ros_common/validate_messages.hpp"
 #include "nav2_util/robot_utils.hpp"
 
 namespace nav2_collision_monitor
 {
 
+constexpr size_t MAX_RANGE_DATA_POINTS = 1e4;
+
 Range::Range(
   const nav2::LifecycleNode::WeakPtr & node,
   const std::string & source_name,
@@ -89,6 +92,16 @@ bool Range::getData(
     return false;
   }
 
+  const size_t point_count = static_cast<size_t>(
+    std::ceil(static_cast<double>(data_->field_of_view) / obstacles_angle_)) + 1;
+  if (point_count > MAX_RANGE_DATA_POINTS) {
+    RCLCPP_ERROR(
+      logger_,
+      "[%s]: Range data would generate %zu points, exceeding the limit of %zu. Ignoring...",
+      source_name_.c_str(), point_count, MAX_RANGE_DATA_POINTS);
+    return false;
+  }
+
   tf2::Transform tf_transform;
   if (!getTransform(curr_time, data_->header, tf_transform)) {
     return false;
@@ -139,10 +152,23 @@ void Range::getParameters(std::string & source_topic)
 
   obstacles_angle_ = node->declare_or_get_parameter(
     source_name_ + ".obstacles_angle", M_PI / 180);
+
+  if (!std::isfinite(obstacles_angle_) || obstacles_angle_ <= 0.0) {
+    throw std::runtime_error{
+            "Range source " + source_name_ + " has invalid obstacles_angle parameter"};
+  }
 }
 
 void Range::dataCallback(sensor_msgs::msg::Range::ConstSharedPtr msg)
 {
+  if (!nav2::validateMsg(*msg)) {
+    RCLCPP_ERROR(
+      logger_,
+      "[%s]: Malformed range message. Rejecting...",
+      source_name_.c_str());
+    return;
+  }
+
   data_ = msg;
 }
 

nav2_collision_monitor/test/sources_test.cpp

@@ -368,6 +368,21 @@ class RangeWrapper : public nav2_collision_monitor::Range
   {
     return data_ != nullptr;
   }
+
+  void setData(sensor_msgs::msg::Range::ConstSharedPtr msg)
+  {
+    data_ = msg;
+  }
+
+  void setObstaclesAngle(const double obstacles_angle)
+  {
+    obstacles_angle_ = obstacles_angle;
+  }
+
+  void processData(sensor_msgs::msg::Range::ConstSharedPtr msg)
+  {
+    dataCallback(msg);
+  }
 };  // RangeWrapper
 
 class PolygonWrapper : public nav2_collision_monitor::PolygonSource
@@ -886,6 +901,94 @@ TEST_F(Tester, testIgnoreTimeShift)
   checkPolygon(data);
 }
 
+TEST_F(Tester, testRangeGeneratedPointLimit)
+{
+  rclcpp::Time curr_time = test_node_->now();
+
+  createSources(false);
+
+  auto msg = std::make_shared<sensor_msgs::msg::Range>();
+  msg->header.frame_id = BASE_FRAME_ID;
+  msg->header.stamp = curr_time;
+  msg->radiation_type = sensor_msgs::msg::Range::ULTRASOUND;
+  msg->field_of_view = M_PI;
+  msg->min_range = 0.1;
+  msg->max_range = 1.1;
+  msg->range = 1.0;
+
+  range_->setData(msg);
+  range_->setObstaclesAngle(M_PI / 1e5);
+
+  std::vector<nav2_collision_monitor::Point> data;
+  EXPECT_FALSE(range_->getData(curr_time, data));
+  EXPECT_TRUE(data.empty());
+
+  range_->setData(msg);
+  range_->setObstaclesAngle(M_PI / 999.0);
+
+  data.clear();
+  EXPECT_TRUE(range_->getData(curr_time, data));
+  EXPECT_EQ(data.size(), 1000u);
+}
+
+TEST_F(Tester, testRangeObstaclesAngleValidation)
+{
+  const std::string valid_range_name = "ValidRange";
+
+  test_node_->declare_parameter(
+    std::string(RANGE_NAME) + ".topic", rclcpp::ParameterValue(RANGE_TOPIC));
+  test_node_->declare_parameter(
+    std::string(RANGE_NAME) + ".obstacles_angle", rclcpp::ParameterValue(0.0));
+
+  range_ = std::make_shared<RangeWrapper>(
+    test_node_, RANGE_NAME, tf_buffer_,
+    BASE_FRAME_ID, GLOBAL_FRAME_ID,
+    TRANSFORM_TOLERANCE, DATA_TIMEOUT, true);
+
+  EXPECT_THROW(range_->configure(), std::runtime_error);
+
+  test_node_->declare_parameter(
+    valid_range_name + ".topic", rclcpp::ParameterValue(RANGE_TOPIC));
+  test_node_->declare_parameter(
+    valid_range_name + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 180.0));
+  range_ = std::make_shared<RangeWrapper>(
+    test_node_, valid_range_name, tf_buffer_,
+    BASE_FRAME_ID, GLOBAL_FRAME_ID,
+    TRANSFORM_TOLERANCE, DATA_TIMEOUT, true);
+
+  EXPECT_NO_THROW(range_->configure());
+}
+
+TEST_F(Tester, testRangeMessageValidation)
+{
+  createSources();
+
+  auto msg = std::make_shared<sensor_msgs::msg::Range>();
+  msg->header.frame_id = SOURCE_FRAME_ID;
+  msg->header.stamp = test_node_->now();
+  msg->radiation_type = sensor_msgs::msg::Range::ULTRASOUND;
+  msg->field_of_view = 0.0;
+  msg->min_range = 0.1;
+  msg->max_range = 1.1;
+  msg->range = 1.0;
+
+  range_->processData(msg);
+
+  EXPECT_FALSE(range_->dataReceived());
+
+  msg->header.frame_id = SOURCE_FRAME_ID;
+  msg->header.stamp = test_node_->now();
+  msg->radiation_type = sensor_msgs::msg::Range::ULTRASOUND;
+  msg->field_of_view = M_PI / 10;
+  msg->min_range = 0.1;
+  msg->max_range = 1.1;
+  msg->range = 1.0;
+
+  range_->processData(msg);
+
+  EXPECT_TRUE(range_->dataReceived());
+}
+
 TEST_F(Tester, testPointCloudMinRange)
 {
   rclcpp::Time curr_time = test_node_->now();

nav2_ros_common/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(rcl_interfaces REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
@@ -39,6 +40,7 @@ target_link_libraries(${PROJECT_NAME} INTERFACE
   pluginlib::pluginlib
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
+  ${sensor_msgs_TARGETS}
   tf2_ros::tf2_ros
   rclcpp_action::rclcpp_action
   tf2_geometry_msgs::tf2_geometry_msgs
@@ -80,6 +82,7 @@ ament_export_dependencies(
   rclcpp
   rclcpp_action
   rclcpp_lifecycle
+  sensor_msgs
   tf2
   tf2_geometry_msgs
   tf2_ros

nav2_ros_common/include/nav2_ros_common/validate_messages.hpp

@@ -24,6 +24,7 @@
 #include "nav_msgs/msg/odometry.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
 #include "map_msgs/msg/occupancy_grid_update.hpp"
+#include "sensor_msgs/msg/range.hpp"
 
 
 // @brief Validation Check
@@ -61,6 +62,7 @@ bool validateMsg(const double & num)
 const double MAX_COVARIANCE = 1e9;
 const double MIN_COVARIANCE = 0;
 const double MIN_MAP_RESOLUTION = 1e-6;
+const double MAX_RANGE_FIELD_OF_VIEW = M_PI;
 
 template<size_t N>
 bool validateMsg(const std::array<double, N> & msg)
@@ -229,6 +231,25 @@ bool validateMsg(const map_msgs::msg::OccupancyGridUpdate & msg)
   return true;
 }
 
+bool validateMsg(const sensor_msgs::msg::Range & msg)
+{
+  if (!validateMsg(msg.header)) {return false;}
+  if (!validateMsg(static_cast<double>(msg.field_of_view))) {return false;}
+  if (!validateMsg(static_cast<double>(msg.min_range))) {return false;}
+  if (!validateMsg(static_cast<double>(msg.max_range))) {return false;}
+  if (!validateMsg(static_cast<double>(msg.range))) {return false;}
+
+  if (msg.field_of_view <= 0.0 || msg.field_of_view > MAX_RANGE_FIELD_OF_VIEW) {
+    return false;
+  }
+
+  if (msg.min_range < 0.0 || msg.max_range <= msg.min_range) {
+    return false;
+  }
+
+  return true;
+}
+
 }  //  namespace nav2
 
 #endif  // NAV2_ROS_COMMON__VALIDATE_MESSAGES_HPP_

nav2_ros_common/package.xml

@@ -20,6 +20,7 @@
   <depend>nav2_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>map_msgs</depend>
+  <depend>sensor_msgs</depend>
   <depend>rcl_interfaces</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>

nav2_ros_common/test/CMakeLists.txt

@@ -124,5 +124,6 @@ target_link_libraries(test_validation_messages
   ${tf2_geometry_msgs_TARGETS}
   pluginlib::pluginlib
   ${builtin_interfaces_TARGETS}
+  ${sensor_msgs_TARGETS}
   ${std_msgs_TARGETS}
 )

nav2_ros_common/test/test_validation_messages.cpp

@@ -18,6 +18,7 @@
 #include <limits>
 
 #include "nav2_ros_common/validate_messages.hpp"
+#include "sensor_msgs/msg/range.hpp"
 
 TEST(ValidateMessagesTest, DoubleValueCheck) {
   // Test valid double value
@@ -481,5 +482,51 @@ TEST(ValidateMessagesTest, PoseWithCovarianceStampedCheck) {
   EXPECT_FALSE(nav2::validateMsg(invalidate_msg2));
 }
 
+TEST(ValidateMessagesTest, RangeCheck)
+{
+  sensor_msgs::msg::Range valid_range;
+  valid_range.header.frame_id = "range_sensor";
+  valid_range.header.stamp.sec = 123;
+  valid_range.header.stamp.nanosec = 456789;
+  valid_range.radiation_type = sensor_msgs::msg::Range::ULTRASOUND;
+  valid_range.field_of_view = M_PI / 2;
+  valid_range.min_range = 0.1;
+  valid_range.max_range = 2.0;
+  valid_range.range = 1.0;
+  EXPECT_TRUE(nav2::validateMsg(valid_range));
+
+  auto invalid_range = valid_range;
+  invalid_range.field_of_view = std::numeric_limits<float>::quiet_NaN();
+  EXPECT_FALSE(nav2::validateMsg(invalid_range));
+
+  invalid_range = valid_range;
+  invalid_range.field_of_view = std::numeric_limits<float>::infinity();
+  EXPECT_FALSE(nav2::validateMsg(invalid_range));
+
+  invalid_range = valid_range;
+  invalid_range.field_of_view = 0.0;
+  EXPECT_FALSE(nav2::validateMsg(invalid_range));
+
+  invalid_range = valid_range;
+  invalid_range.field_of_view = -0.1;
+  EXPECT_FALSE(nav2::validateMsg(invalid_range));
+
+  invalid_range = valid_range;
+  invalid_range.field_of_view = static_cast<float>(M_PI + 0.1);
+  EXPECT_FALSE(nav2::validateMsg(invalid_range));
+
+  invalid_range = valid_range;
+  invalid_range.range = std::numeric_limits<float>::quiet_NaN();
+  EXPECT_FALSE(nav2::validateMsg(invalid_range));
+
+  invalid_range = valid_range;
+  invalid_range.min_range = -0.1;
+  EXPECT_FALSE(nav2::validateMsg(invalid_range));
+
+  invalid_range = valid_range;
+  invalid_range.max_range = invalid_range.min_range;
+  EXPECT_FALSE(nav2::validateMsg(invalid_range));
+}
+
 
 // Add more test cases for other validateMsg functions if needed