diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index b311b7ac5e..ba5c83447f 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/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; } diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 18cd51164f..cc90b8409b 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/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; diff --git a/nav2_util/include/nav2_util/validate_messages.hpp b/nav2_util/include/nav2_util/validate_messages.hpp new file mode 100644 index 0000000000..7ce942be42 --- /dev/null +++ b/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 +#include + +#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_ diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index b99e3d8371..12b4254ae6 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -58,3 +58,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}) \ No newline at end of file diff --git a/nav2_util/test/test_validation_messages.cpp b/nav2_util/test/test_validation_messages.cpp new file mode 100644 index 0000000000..060ec11ede --- /dev/null +++ b/nav2_util/test/test_validation_messages.cpp @@ -0,0 +1,247 @@ +// 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. + +#include +#include "nav2_util/validate_messages.hpp" + +TEST(ValidateMessagesTest, DoubleValueCheck) { + // Test valid double value + EXPECT_TRUE(nav2_util::validateMsg(3.14)); + // Test invalid double value (infinity) + EXPECT_FALSE(nav2_util::validateMsg(std::numeric_limits::infinity())); + // Test invalid double value (NaN) + EXPECT_FALSE(nav2_util::validateMsg(std::numeric_limits::quiet_NaN())); +} + +TEST(ValidateMessagesTest, TimeStampCheck) +{ + // Test valid time stamp + builtin_interfaces::msg::Time valid_time_stamp; + valid_time_stamp.sec = 123; + valid_time_stamp.nanosec = 456789; + EXPECT_TRUE(nav2_util::validateMsg(valid_time_stamp)); + // Test invalid time stamp (nanosec out of range) + builtin_interfaces::msg::Time invalid_time_stamp; + invalid_time_stamp.sec = 123; + invalid_time_stamp.nanosec = 1e9; // 1 second = 1e9 nanoseconds + EXPECT_FALSE(nav2_util::validateMsg(invalid_time_stamp)); +} + +TEST(ValidateMessagesTest, HeaderCheck) +{ + // Test valid header with non-empty frame_id + std_msgs::msg::Header valid_header; + valid_header.stamp.sec = 123; + valid_header.stamp.nanosec = 456789; + valid_header.frame_id = "map"; + EXPECT_TRUE(nav2_util::validateMsg(valid_header)); + // Test invalid header with empty frame_id + std_msgs::msg::Header invalid_header; + invalid_header.stamp.sec = 123; + invalid_header.stamp.nanosec = 456789; + invalid_header.frame_id = ""; + EXPECT_FALSE(nav2_util::validateMsg(invalid_header)); + invalid_header.stamp.sec = 123; + invalid_header.stamp.nanosec = 1e9; + invalid_header.frame_id = "map"; + EXPECT_FALSE(nav2_util::validateMsg(invalid_header)); +} + +TEST(ValidateMessagesTest, PointCheck) +{ + // Test valid Point message + geometry_msgs::msg::Point valid_point; + valid_point.x = 1.0; + valid_point.y = 2.0; + valid_point.z = 3.0; + EXPECT_TRUE(nav2_util::validateMsg(valid_point)); + // Test invalid Point message with NaN value + geometry_msgs::msg::Point invalid_point; + invalid_point.x = 1.0; + invalid_point.y = std::numeric_limits::quiet_NaN(); + invalid_point.z = 3.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); + // Test invalid Point message with NaN value + invalid_point.x = std::numeric_limits::quiet_NaN(); + invalid_point.y = 2.0; + invalid_point.z = 3.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); + // Test invalid Point message with NaN value + invalid_point.x = 1.0; + invalid_point.y = 2.0; + invalid_point.z = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); +} + +TEST(ValidateMessagesTest, QuaternionCheck) +{ + // Test valid Quaternion message + geometry_msgs::msg::Quaternion valid_quaternion; + valid_quaternion.x = 0.0; + valid_quaternion.y = 0.0; + valid_quaternion.z = 0.0; + valid_quaternion.w = 1.0; + EXPECT_TRUE(nav2_util::validateMsg(valid_quaternion)); + // Test invalid Quaternion message with invalid magnitude + geometry_msgs::msg::Quaternion invalid_quaternion; + invalid_quaternion.x = 0.1; + invalid_quaternion.y = 0.2; + invalid_quaternion.z = 0.3; + invalid_quaternion.w = 0.5; // Invalid magnitude (should be 1.0) + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + + // One NaN value + invalid_quaternion.x = 0.0; + invalid_quaternion.y = std::numeric_limits::quiet_NaN(); + invalid_quaternion.z = 0.0; + invalid_quaternion.w = 1.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + invalid_quaternion.x = std::numeric_limits::quiet_NaN(); + invalid_quaternion.y = 0.0; + invalid_quaternion.z = 0.0; + invalid_quaternion.w = 1.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + invalid_quaternion.x = 0.0; + invalid_quaternion.y = 0.0; + invalid_quaternion.z = std::numeric_limits::quiet_NaN(); + invalid_quaternion.w = 1.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + invalid_quaternion.x = 0.0; + invalid_quaternion.y = 0.0; + invalid_quaternion.z = 1.0; + invalid_quaternion.w = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); +} + +TEST(ValidateMessagesTest, PoseCheck) +{ + // Test valid Pose message + geometry_msgs::msg::Pose valid_pose; + valid_pose.position.x = 1.0; + valid_pose.position.y = 2.0; + valid_pose.position.z = 3.0; + valid_pose.orientation.x = 1.0; + valid_pose.orientation.y = 0.0; + valid_pose.orientation.z = 0.0; + valid_pose.orientation.w = 0.0; + EXPECT_TRUE(nav2_util::validateMsg(valid_pose)); + // Test invalid Pose message with invalid position + geometry_msgs::msg::Pose invalid_pose; + invalid_pose.position.x = 1.0; + invalid_pose.position.y = std::numeric_limits::quiet_NaN(); + invalid_pose.position.z = 3.0; + invalid_pose.orientation.x = 1.0; + invalid_pose.orientation.y = 0.0; + invalid_pose.orientation.z = 0.0; + invalid_pose.orientation.w = 0.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_pose)); + // Test invalid Pose message with invalid orientation + invalid_pose.position.x = 1.0; + invalid_pose.position.y = 2.0; + invalid_pose.position.z = 3.0; + invalid_pose.orientation.x = 0.1; + invalid_pose.orientation.y = 0.2; + invalid_pose.orientation.z = 0.3; + invalid_pose.orientation.w = 0.4; + EXPECT_FALSE(nav2_util::validateMsg(invalid_pose)); +} + + +TEST(ValidateMessagesTest, MapMetaDataCheck) { + // Test valid MapMetaData message + nav_msgs::msg::MapMetaData valid_map_meta_data; + valid_map_meta_data.resolution = 0.05; + valid_map_meta_data.width = 100; + valid_map_meta_data.height = 100; + geometry_msgs::msg::Pose valid_origin; + valid_origin.position.x = 0.0; + valid_origin.position.y = 0.0; + valid_origin.position.z = 0.0; + valid_origin.orientation.x = 0.0; + valid_origin.orientation.y = 0.0; + valid_origin.orientation.z = 0.0; + valid_origin.orientation.w = 1.0; + valid_map_meta_data.origin = valid_origin; + EXPECT_TRUE(nav2_util::validateMsg(valid_map_meta_data)); + + // Test invalid origin message + nav_msgs::msg::MapMetaData invalid_map_meta_data; + invalid_map_meta_data.resolution = 100.0; + invalid_map_meta_data.width = 100; + invalid_map_meta_data.height = 100; + geometry_msgs::msg::Pose invalid_origin; + invalid_origin.position.x = 0.0; + invalid_origin.position.y = 0.0; + invalid_origin.position.z = 0.0; + invalid_origin.orientation.x = 0.0; + invalid_origin.orientation.y = 0.0; + invalid_origin.orientation.z = 1.0; + invalid_origin.orientation.w = 1.0; + invalid_map_meta_data.origin = invalid_origin; + EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); + + // Test invalid resolution message + invalid_map_meta_data.resolution = std::numeric_limits::quiet_NaN(); + invalid_map_meta_data.width = 100; + invalid_map_meta_data.height = 100; + invalid_map_meta_data.origin = valid_origin; + EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); + + // Test invalid MapMetaData message with zero width + invalid_map_meta_data.resolution = 0.05; + invalid_map_meta_data.width = 0; + invalid_map_meta_data.height = 100; + invalid_map_meta_data.origin = valid_origin; + EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); +} + +TEST(ValidateMessagesTest, OccupancyGridCheck) { + // Test valid OccupancyGrid message + nav_msgs::msg::OccupancyGrid valid_occupancy_grid; + valid_occupancy_grid.header.frame_id = "map"; + valid_occupancy_grid.info.resolution = 0.05; + valid_occupancy_grid.info.width = 100; + valid_occupancy_grid.info.height = 100; + std::vector data(100 * 100, 0); // Initialize with zeros + valid_occupancy_grid.data = data; + EXPECT_TRUE(nav2_util::validateMsg(valid_occupancy_grid)); + + // Test invalid header message with wrong data size + nav_msgs::msg::OccupancyGrid invalid_occupancy_grid; + invalid_occupancy_grid.header.frame_id = ""; // Incorrect id + invalid_occupancy_grid.info.resolution = 0.05; + invalid_occupancy_grid.info.width = 100; + invalid_occupancy_grid.info.height = 100; + invalid_occupancy_grid.data = data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); + + // Test invalid info message with wrong data size + invalid_occupancy_grid.header.frame_id = "map"; + invalid_occupancy_grid.info.resolution = 0.05; + invalid_occupancy_grid.info.width = 0; // Incorrect width + invalid_occupancy_grid.info.height = 100; + invalid_occupancy_grid.data = data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); + + // Test invalid OccupancyGrid message with wrong data size + invalid_occupancy_grid.header.frame_id = "map"; + invalid_occupancy_grid.info.resolution = 0.05; + invalid_occupancy_grid.info.width = 100; + invalid_occupancy_grid.info.height = 100; + std::vector invalid_data(100 * 99, 0); // Incorrect data size + invalid_occupancy_grid.data = invalid_data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); +} + +// Add more test cases for other validateMsg functions if needed