From 9b349282bb5a5b636a2e8c0247f6a99beeaf8b9e Mon Sep 17 00:00:00 2001 From: goes Date: Thu, 25 Apr 2024 15:53:22 +0800 Subject: [PATCH 1/6] provide validation_message.hpp Signed-off-by: goes --- nav2_amcl/src/amcl_node.cpp | 5 + nav2_costmap_2d/plugins/static_layer.cpp | 5 + .../include/nav2_util/validate_messages.hpp | 147 ++++++++++++++++++ 3 files changed, 157 insertions(+) create mode 100644 nav2_util/include/nav2_util/validate_messages.hpp 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..710b7318c8 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_message.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_ From 243d6ee5e0af53c3ddd9873eb6e2997039afdcd7 Mon Sep 17 00:00:00 2001 From: goes Date: Thu, 25 Apr 2024 16:39:45 +0800 Subject: [PATCH 2/6] fix typo Signed-off-by: goes --- nav2_costmap_2d/plugins/static_layer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 710b7318c8..cc90b8409b 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -45,7 +45,7 @@ #include "pluginlib/class_list_macros.hpp" #include "tf2/convert.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "nav2_util/validate_message.hpp" +#include "nav2_util/validate_messages.hpp" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer) From d6193417f38a6f54908ee9f83801f4994b327cd6 Mon Sep 17 00:00:00 2001 From: goes Date: Fri, 26 Apr 2024 14:22:11 +0800 Subject: [PATCH 3/6] add test_validation_messages.cpp Signed-off-by: goes --- nav2_util/test/CMakeLists.txt | 4 + nav2_util/test/test_validation_messages.cpp | 263 ++++++++++++++++++++ 2 files changed, 267 insertions(+) create mode 100644 nav2_util/test/test_validation_messages.cpp diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 14e1a361d3..4c0f274721 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/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}) \ 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..6e5087f36b --- /dev/null +++ b/nav2_util/test/test_validation_messages.cpp @@ -0,0 +1,263 @@ +// 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 "nav2_util/validate_messages.hpp" +#include + +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 + geometry_msgs::msg::Point invalid_point; + 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 + geometry_msgs::msg::Point invalid_point; + 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 + geometry_msgs::msg::Pose invalid_pose; + 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 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 = 1.0; + valid_origin.orientation.w = 1.0; + valid_map_meta_data.origin = valid_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; + 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_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; + std::vector invalid_data(100 * 100, 0); + invalid_occupancy_grid.data = invalid_data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); + + // Test invalid info message with wrong data size + nav_msgs::msg::OccupancyGrid invalid_occupancy_grid; + 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; + std::vector invalid_data(100 * 100, 0); + invalid_occupancy_grid.data = invalid_data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); + + // Test invalid OccupancyGrid message with wrong data size + nav_msgs::msg::OccupancyGrid invalid_occupancy_grid; + 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 From 0d315bcbd81a563fd80053319b5689f50adbc101 Mon Sep 17 00:00:00 2001 From: goes Date: Fri, 26 Apr 2024 14:25:20 +0800 Subject: [PATCH 4/6] change include-order Signed-off-by: goes --- nav2_util/test/test_validation_messages.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_util/test/test_validation_messages.cpp b/nav2_util/test/test_validation_messages.cpp index 6e5087f36b..0b5356d6f4 100644 --- a/nav2_util/test/test_validation_messages.cpp +++ b/nav2_util/test/test_validation_messages.cpp @@ -12,9 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. - -#include "nav2_util/validate_messages.hpp" #include +#include "nav2_util/validate_messages.hpp" TEST(ValidateMessagesTest, DoubleValueCheck) { // Test valid double value From dc997eb3036b010fe67a1ae2080cfe889f96eac1 Mon Sep 17 00:00:00 2001 From: goes Date: Fri, 26 Apr 2024 14:28:16 +0800 Subject: [PATCH 5/6] reformat Signed-off-by: goes --- nav2_util/test/test_validation_messages.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_util/test/test_validation_messages.cpp b/nav2_util/test/test_validation_messages.cpp index 0b5356d6f4..f53af7f0d4 100644 --- a/nav2_util/test/test_validation_messages.cpp +++ b/nav2_util/test/test_validation_messages.cpp @@ -100,7 +100,7 @@ TEST(ValidateMessagesTest, QuaternionCheck) 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) + invalid_quaternion.w = 0.5; // Invalid magnitude (should be 1.0) EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); // One NaN value @@ -224,13 +224,13 @@ TEST(ValidateMessagesTest, OccupancyGridCheck) { 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 + 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.header.frame_id = ""; // Incorrect id invalid_occupancy_grid.info.resolution = 0.05; invalid_occupancy_grid.info.width = 100; invalid_occupancy_grid.info.height = 100; @@ -254,7 +254,7 @@ TEST(ValidateMessagesTest, OccupancyGridCheck) { 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 + std::vector invalid_data(100 * 99, 0); // Incorrect data size invalid_occupancy_grid.data = invalid_data; EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); } From 3918de64a02c6cd85bfde2b9f288b0c081a82f4d Mon Sep 17 00:00:00 2001 From: goes Date: Fri, 26 Apr 2024 14:37:27 +0800 Subject: [PATCH 6/6] update test Signed-off-by: goes --- nav2_util/test/test_validation_messages.cpp | 39 +++++++-------------- 1 file changed, 12 insertions(+), 27 deletions(-) diff --git a/nav2_util/test/test_validation_messages.cpp b/nav2_util/test/test_validation_messages.cpp index f53af7f0d4..060ec11ede 100644 --- a/nav2_util/test/test_validation_messages.cpp +++ b/nav2_util/test/test_validation_messages.cpp @@ -73,13 +73,11 @@ TEST(ValidateMessagesTest, PointCheck) invalid_point.z = 3.0; EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); // Test invalid Point message with NaN value - geometry_msgs::msg::Point invalid_point; 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 - geometry_msgs::msg::Point invalid_point; invalid_point.x = 1.0; invalid_point.y = 2.0; invalid_point.z = std::numeric_limits::quiet_NaN(); @@ -149,7 +147,6 @@ TEST(ValidateMessagesTest, PoseCheck) invalid_pose.orientation.w = 0.0; EXPECT_FALSE(nav2_util::validateMsg(invalid_pose)); // Test invalid Pose message with invalid orientation - geometry_msgs::msg::Pose invalid_pose; invalid_pose.position.x = 1.0; invalid_pose.position.y = 2.0; invalid_pose.position.z = 3.0; @@ -183,30 +180,22 @@ TEST(ValidateMessagesTest, MapMetaDataCheck) { invalid_map_meta_data.resolution = 100.0; invalid_map_meta_data.width = 100; invalid_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 = 1.0; - valid_origin.orientation.w = 1.0; - valid_map_meta_data.origin = valid_origin; + 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; - 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; + invalid_map_meta_data.origin = valid_origin; EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); // Test invalid MapMetaData message with zero width @@ -234,22 +223,18 @@ TEST(ValidateMessagesTest, OccupancyGridCheck) { invalid_occupancy_grid.info.resolution = 0.05; invalid_occupancy_grid.info.width = 100; invalid_occupancy_grid.info.height = 100; - std::vector invalid_data(100 * 100, 0); - invalid_occupancy_grid.data = invalid_data; + invalid_occupancy_grid.data = data; EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); // Test invalid info message with wrong data size - nav_msgs::msg::OccupancyGrid invalid_occupancy_grid; 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; - std::vector invalid_data(100 * 100, 0); - invalid_occupancy_grid.data = invalid_data; + invalid_occupancy_grid.data = data; EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); // Test invalid OccupancyGrid message with wrong data size - nav_msgs::msg::OccupancyGrid invalid_occupancy_grid; invalid_occupancy_grid.header.frame_id = "map"; invalid_occupancy_grid.info.resolution = 0.05; invalid_occupancy_grid.info.width = 100;