provide message validation check API

nav2_amcl/src/amcl_node.cpp

@@ -49,6 +49,7 @@
 #pragma GCC diagnostic pop
 
 #include "nav2_amcl/portable_utils.hpp"
+#include "nav2_util/validate_messages.hpp"
 
 using namespace std::placeholders;
 using rcl_interfaces::msg::ParameterType;
@@ -1390,6 +1391,10 @@ void
 AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
 {
   RCLCPP_DEBUG(get_logger(), "AmclNode: A new map was received.");
+  if (!nav2_util::validateMsg(*msg)) {
+    RCLCPP_ERROR(get_logger(), "Received map message is malformed. Rejecting.");
+    return;
+  }
   if (first_map_only_ && first_map_received_) {
     return;
   }

nav2_costmap_2d/plugins/static_layer.cpp

@@ -45,6 +45,7 @@
 #include "pluginlib/class_list_macros.hpp"
 #include "tf2/convert.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_util/validate_messages.hpp"
 
 PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer)
 
@@ -277,6 +278,10 @@ StaticLayer::interpretValue(unsigned char value)
 void
 StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map)
 {
+  if (!nav2_util::validateMsg(*new_map)) {
+    RCLCPP_ERROR(logger_, "Received map message is malformed. Rejecting.");
+    return;
+  }
   if (!map_received_) {
     processMap(*new_map);
     map_received_ = true;

nav2_util/include/nav2_util/validate_messages.hpp

@@ -0,0 +1,147 @@
+// Copyright (c) 2024 GoesM
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#ifndef  NAV2_UTIL__VALIDATE_MESSAGES_HPP_
+#define  NAV2_UTIL__VALIDATE_MESSAGES_HPP_
+
+#include <cmath>
+#include <iostream>
+
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+
+
+// @brief Validation Check
+//  Check recieved message is safe or not for the nav2-system
+//  For each msg-type known in nav2, we could check it as following:
+//  if(!validateMsg()) RCLCPP_ERROR(,"malformed msg. Rejecting.")
+//
+//  Workflow of validateMsg():
+//     if here's a sub-msg-type in the recieved msg,
+//        the content of sub-msg would be checked as sub-msg-type
+//     then, check the whole recieved msg.
+//
+//  Following conditions are involved in check:
+//     1> Value Check: to avoid damaged value like like `nan`, `INF`, empty string and so on
+//     2> Logic Check: to avoid value with bad logic,
+//             like the size of `map` should be equal to `height*width`
+//     3> Any other needed condition could be joint here in future
+
+namespace nav2_util
+{
+
+
+bool validateMsg(const double & num)
+{
+  /*  @brief double/float value check
+   *  if here'a need to check message validation
+   *  it should be avoid to use double value like `nan`, `inf`
+   *  otherwise, we regard it as an invalid message
+   */
+  if (std::isinf(num)) {return false;}
+  if (std::isnan(num)) {return false;}
+  return true;
+}
+
+const int NSEC_PER_SEC = 1e9;  // 1 second = 1e9 nanosecond
+bool validateMsg(const builtin_interfaces::msg::Time & msg)
+{
+  if (msg.nanosec >= NSEC_PER_SEC) {
+    return false;                                      // invalid nanosec-stamp
+  }
+  return true;
+}
+
+bool validateMsg(const std_msgs::msg::Header & msg)
+{
+  //  check sub-type
+  if (!validateMsg(msg.stamp)) {return false;}
+
+  /*  @brief frame_id check
+   *  if here'a need to check message validation
+   *  it should at least have a non-empty frame_id
+   *  otherwise, we regard it as an invalid message
+   */
+  if (msg.frame_id.empty()) {return false;}
+  return true;
+}
+
+bool validateMsg(const geometry_msgs::msg::Point & msg)
+{
+  //  check sub-type
+  if (!validateMsg(msg.x)) {return false;}
+  if (!validateMsg(msg.y)) {return false;}
+  if (!validateMsg(msg.z)) {return false;}
+  return true;
+}
+
+const double epsilon = 1e-4;
+bool validateMsg(const geometry_msgs::msg::Quaternion & msg)
+{
+  //  check sub-type
+  if (!validateMsg(msg.x)) {return false;}
+  if (!validateMsg(msg.y)) {return false;}
+  if (!validateMsg(msg.z)) {return false;}
+  if (!validateMsg(msg.w)) {return false;}
+
+  if (abs(msg.x * msg.x + msg.y * msg.y + msg.z * msg.z + msg.w * msg.w - 1.0) >= epsilon) {
+    return false;
+  }
+
+  return true;
+}
+
+bool validateMsg(const geometry_msgs::msg::Pose & msg)
+{
+  // check sub-type
+  if (!validateMsg(msg.position)) {return false;}
+  if (!validateMsg(msg.orientation)) {return false;}
+  return true;
+}
+
+
+// Function to verify map meta information
+bool validateMsg(const nav_msgs::msg::MapMetaData & msg)
+{
+  // check sub-type
+  if (!validateMsg(msg.origin)) {return false;}
+  if (!validateMsg(msg.resolution)) {return false;}
+
+  // logic check
+  // 1> we don't need an empty map
+  if (msg.height == 0 || msg.width == 0) {return false;}
+  return true;
+}
+
+// for msg-type like map, costmap and others as `OccupancyGrid`
+bool validateMsg(const nav_msgs::msg::OccupancyGrid & msg)
+{
+  // check sub-type
+  if (!validateMsg(msg.header)) {return false;}
+  // msg.data :  @todo any check for it ?
+  if (!validateMsg(msg.info)) {return false;}
+
+  // check logic
+  if (msg.data.size() != msg.info.width * msg.info.height) {
+    return false;                                                          // check map-size
+  }
+  return true;
+}
+
+
+}  //  namespace nav2_util
+
+
+#endif  // NAV2_UTIL__VALIDATE_MESSAGES_HPP_

nav2_util/test/CMakeLists.txt

@@ -56,3 +56,7 @@ target_link_libraries(test_twist_publisher ${library_name})
 ament_add_gtest(test_twist_subscriber test_twist_subscriber.cpp)
 ament_target_dependencies(test_twist_subscriber rclcpp_lifecycle)
 target_link_libraries(test_twist_subscriber ${library_name})
+
+ament_add_gtest(test_validation_messages test_validation_messages.cpp)
+ament_target_dependencies(test_validation_messages rclcpp_lifecycle)
+target_link_libraries(test_validation_messages ${library_name})