Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

msg validation check for /initialpose in nav2_amcl #4301

Merged
merged 13 commits into from
May 8, 2024
Merged

msg validation check for /initialpose in nav2_amcl #4301

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
7ebe2d4
add validation check for PoseWithCovarianceStamped
May 6, 2024
27d42a0
remove rebundant check before
May 6, 2024
b3e8d55
reformat
May 6, 2024
9538f42
typo fixed
May 6, 2024
8a9f3d1
change the type-name
May 7, 2024
daf257f
update test
May 7, 2024
857264d
reformat
May 7, 2024
86ddcbd
.
May 7, 2024
c3e56c8
add comment
May 8, 2024
f9dfc7d
update comment
May 8, 2024
5e1788b
change header
May 8, 2024
15268ac
update test
May 8, 2024
9db4821
typo fixed
May 8, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 2 additions & 5 deletions nav2_amcl/src/amcl_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_) {
Expand Down
32 changes: 32 additions & 0 deletions nav2_util/include/nav2_util/validate_messages.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -55,6 +56,20 @@ bool validateMsg(const double & num)
return true;
}

template<size_t N>
bool validateMsg(const std::array<double, N> & msg)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Vector?

Copy link
Contributor Author

@GoesM GoesM May 7, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I noticed YAML::List is always transformed into std::array in ROS, which is defined in geometry_msg:

  using _pose_type =
    geometry_msgs::msg::Pose_<ContainerAllocator>;
  _pose_type pose;
  using _covariance_type =
    std::array<double, 36>;
  _covariance_type covariance;

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah got it, this is for the covariance checking 👍 Can you add a comment for this one that this is what this is for? I'll forget in a year and when I look over this again I'll wonder again why its not a vector 😆

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

of course 😆

{
/* @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)
{
Expand Down Expand Up @@ -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)
Expand Down
121 changes: 121 additions & 0 deletions nav2_util/test/test_validation_messages.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Loading