msg validation check for /initialpose in nav2_amcl (#4301)

* add validation check for PoseWithCovarianceStamped Signed-off-by: goes <[email protected]> * remove rebundant check before Signed-off-by: goes <[email protected]> * reformat Signed-off-by: goes <[email protected]> * typo fixed Signed-off-by: goes <[email protected]> * change the type-name Signed-off-by: goes <[email protected]> * update test Signed-off-by: goes <[email protected]> * reformat Signed-off-by: goes <[email protected]> * . Signed-off-by: goes <[email protected]> * add comment Signed-off-by: goes <[email protected]> * update comment Signed-off-by: goes <[email protected]> * change header Signed-off-by: goes <[email protected]> * update test Signed-off-by: goes <[email protected]> * typo fixed Signed-off-by: goes <[email protected]> --------- Signed-off-by: goes <[email protected]> Co-authored-by: goes <[email protected]>
ros-navigation · May 8, 2024 · 7ef4473 · 7ef4473
1 parent 5913178
commit 7ef4473
Show file tree

Hide file tree

Showing 3 changed files with 155 additions and 5 deletions.
diff --git 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
@@ -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<size_t N>
+bool validateMsg(const std::array<double, N> & 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
@@ -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