From 7ef4473b266802f06736150fdac987ba7cb0e47a Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Wed, 8 May 2024 23:23:30 +0800 Subject: [PATCH] msg validation check for `/initialpose` in `nav2_amcl` (#4301) * add validation check for PoseWithCovarianceStamped Signed-off-by: goes * remove rebundant check before Signed-off-by: goes * reformat Signed-off-by: goes * typo fixed Signed-off-by: goes * change the type-name Signed-off-by: goes * update test Signed-off-by: goes * reformat Signed-off-by: goes * . Signed-off-by: goes * add comment Signed-off-by: goes * update comment Signed-off-by: goes * change header Signed-off-by: goes * update test Signed-off-by: goes * typo fixed Signed-off-by: goes --------- Signed-off-by: goes Co-authored-by: goes --- nav2_amcl/src/amcl_node.cpp | 7 +- .../include/nav2_util/validate_messages.hpp | 32 +++++ nav2_util/test/test_validation_messages.cpp | 121 ++++++++++++++++++ 3 files changed, 155 insertions(+), 5 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index ba5c83447f..764a8f059f 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -524,11 +524,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha RCLCPP_INFO(get_logger(), "initialPoseReceived"); - if (msg->header.frame_id == "") { - // This should be removed at some point - RCLCPP_WARN( - get_logger(), - "Received initial pose with empty frame_id. You should always supply a frame_id."); + if (!nav2_util::validateMsg(*msg)) { + RCLCPP_ERROR(get_logger(), "Received initialpose message is malformed. Rejecting."); return; } if (nav2_util::strip_leading_slash(msg->header.frame_id) != global_frame_id_) { diff --git a/nav2_util/include/nav2_util/validate_messages.hpp b/nav2_util/include/nav2_util/validate_messages.hpp index 7ce942be42..58594f1ecd 100644 --- a/nav2_util/include/nav2_util/validate_messages.hpp +++ b/nav2_util/include/nav2_util/validate_messages.hpp @@ -21,6 +21,7 @@ #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" // @brief Validation Check @@ -55,6 +56,20 @@ bool validateMsg(const double & num) return true; } +template +bool validateMsg(const std::array & msg) +{ + /* @brief value check for double-array + * like the field `covariance` used in the msg-type: + * geometry_msgs::msg::PoseWithCovarianceStamped + */ + for (const auto & element : msg) { + if (!validateMsg(element)) {return false;} + } + + return true; +} + const int NSEC_PER_SEC = 1e9; // 1 second = 1e9 nanosecond bool validateMsg(const builtin_interfaces::msg::Time & msg) { @@ -111,6 +126,23 @@ bool validateMsg(const geometry_msgs::msg::Pose & msg) return true; } +bool validateMsg(const geometry_msgs::msg::PoseWithCovariance & msg) +{ + // check sub-type + if (!validateMsg(msg.pose)) {return false;} + if (!validateMsg(msg.covariance)) {return false;} + + return true; +} + +bool validateMsg(const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +{ + // check sub-type + if (!validateMsg(msg.header)) {return false;} + if (!validateMsg(msg.pose)) {return false;} + return true; +} + // Function to verify map meta information bool validateMsg(const nav_msgs::msg::MapMetaData & msg) diff --git a/nav2_util/test/test_validation_messages.cpp b/nav2_util/test/test_validation_messages.cpp index 060ec11ede..a7d8ed8904 100644 --- a/nav2_util/test/test_validation_messages.cpp +++ b/nav2_util/test/test_validation_messages.cpp @@ -244,4 +244,125 @@ TEST(ValidateMessagesTest, OccupancyGridCheck) { EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); } +TEST(ValidateMessagesTest, PoseWithCovarianceCheck) { + // Valid message + geometry_msgs::msg::PoseWithCovariance validate_msg; + validate_msg.covariance[0] = 0.25; + // assign other covariance values... + validate_msg.covariance[35] = 0.06853891909122467; + + validate_msg.pose.position.x = 0.50010401010515571; + validate_msg.pose.position.y = 1.7468730211257935; + validate_msg.pose.position.z = 0.0; + + validate_msg.pose.orientation.x = 0.9440542194053062; + validate_msg.pose.orientation.y = 0.0; + validate_msg.pose.orientation.z = 0.0; + validate_msg.pose.orientation.w = -0.32979028309372299; + + EXPECT_TRUE(nav2_util::validateMsg(validate_msg)); + + // Invalid messages + geometry_msgs::msg::PoseWithCovariance invalidate_msg1; + invalidate_msg1.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg1.covariance[7] = NAN; + invalidate_msg1.covariance[9] = NAN; + invalidate_msg1.covariance[35] = 0.06853891909122467; + + invalidate_msg1.pose.position.x = 0.50010401010515571; + invalidate_msg1.pose.position.y = 1.7468730211257935; + invalidate_msg1.pose.position.z = 0.0; + + invalidate_msg1.pose.orientation.x = 0.9440542194053062; + invalidate_msg1.pose.orientation.y = 0.0; + invalidate_msg1.pose.orientation.z = 0.0; + invalidate_msg1.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1)); + + geometry_msgs::msg::PoseWithCovariance invalidate_msg2; + invalidate_msg2.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg2.covariance[35] = 0.06853891909122467; + + invalidate_msg2.pose.position.x = NAN; + invalidate_msg2.pose.position.y = 1.7468730211257935; + invalidate_msg2.pose.position.z = 0.0; + + invalidate_msg2.pose.orientation.x = 0.9440542194053062; + invalidate_msg2.pose.orientation.y = 0.0; + invalidate_msg2.pose.orientation.z = 0.0; + invalidate_msg2.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2)); +} + +TEST(ValidateMessagesTest, PoseWithCovarianceStampedCheck) { + // Valid message + geometry_msgs::msg::PoseWithCovarianceStamped validate_msg; + validate_msg.header.frame_id = "map"; + validate_msg.header.stamp.sec = 1711029956; + validate_msg.header.stamp.nanosec = 146734875; + + validate_msg.pose.covariance[0] = 0.25; + // assign other covariance values... + validate_msg.pose.covariance[35] = 0.06853891909122467; + + validate_msg.pose.pose.position.x = 0.50010401010515571; + validate_msg.pose.pose.position.y = 1.7468730211257935; + validate_msg.pose.pose.position.z = 0.0; + + validate_msg.pose.pose.orientation.x = 0.9440542194053062; + validate_msg.pose.pose.orientation.y = 0.0; + validate_msg.pose.pose.orientation.z = 0.0; + validate_msg.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_TRUE(nav2_util::validateMsg(validate_msg)); + + // Invalid messages + geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg1; + invalidate_msg1.header.frame_id = "map"; + invalidate_msg1.header.stamp.sec = 1711029956; + invalidate_msg1.header.stamp.nanosec = 146734875; + + invalidate_msg1.pose.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg1.pose.covariance[7] = NAN; + invalidate_msg1.pose.covariance[9] = NAN; + invalidate_msg1.pose.covariance[35] = 0.06853891909122467; + + invalidate_msg1.pose.pose.position.x = 0.50010401010515571; + invalidate_msg1.pose.pose.position.y = 1.7468730211257935; + invalidate_msg1.pose.pose.position.z = 0.0; + + invalidate_msg1.pose.pose.orientation.x = 0.9440542194053062; + invalidate_msg1.pose.pose.orientation.y = 0.0; + invalidate_msg1.pose.pose.orientation.z = 0.0; + invalidate_msg1.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1)); + + geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg2; + invalidate_msg2.header.frame_id = ""; + invalidate_msg2.header.stamp.sec = 1711029956; + invalidate_msg2.header.stamp.nanosec = 146734875; + + invalidate_msg2.pose.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg2.pose.covariance[35] = 0.06853891909122467; + + invalidate_msg2.pose.pose.position.x = 0.50010401010515571; + invalidate_msg2.pose.pose.position.y = 1.7468730211257935; + invalidate_msg2.pose.pose.position.z = 0.0; + + invalidate_msg2.pose.pose.orientation.x = 0.9440542194053062; + invalidate_msg2.pose.pose.orientation.y = 0.0; + invalidate_msg2.pose.pose.orientation.z = 0.0; + invalidate_msg2.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2)); +} + + // Add more test cases for other validateMsg functions if needed