Skip to content

Commit 0ac49d0

Browse files
GoesMgoes
authored and
enricosutera
committed
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]> Signed-off-by: enricosutera <[email protected]>
1 parent 10d1fdf commit 0ac49d0

File tree

3 files changed

+155
-5
lines changed

3 files changed

+155
-5
lines changed

‎nav2_amcl/src/amcl_node.cpp‎

Lines changed: 2 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -524,11 +524,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha
524524

525525
RCLCPP_INFO(get_logger(), "initialPoseReceived");
526526

527-
if (msg->header.frame_id == "") {
528-
// This should be removed at some point
529-
RCLCPP_WARN(
530-
get_logger(),
531-
"Received initial pose with empty frame_id. You should always supply a frame_id.");
527+
if (!nav2_util::validateMsg(*msg)) {
528+
RCLCPP_ERROR(get_logger(), "Received initialpose message is malformed. Rejecting.");
532529
return;
533530
}
534531
if (nav2_util::strip_leading_slash(msg->header.frame_id) != global_frame_id_) {

‎nav2_util/include/nav2_util/validate_messages.hpp‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121

2222
#include "nav_msgs/msg/occupancy_grid.hpp"
2323
#include "nav_msgs/msg/odometry.hpp"
24+
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
2425

2526

2627
// @brief Validation Check
@@ -55,6 +56,20 @@ bool validateMsg(const double & num)
5556
return true;
5657
}
5758

59+
template<size_t N>
60+
bool validateMsg(const std::array<double, N> & msg)
61+
{
62+
/* @brief value check for double-array
63+
* like the field `covariance` used in the msg-type:
64+
* geometry_msgs::msg::PoseWithCovarianceStamped
65+
*/
66+
for (const auto & element : msg) {
67+
if (!validateMsg(element)) {return false;}
68+
}
69+
70+
return true;
71+
}
72+
5873
const int NSEC_PER_SEC = 1e9; // 1 second = 1e9 nanosecond
5974
bool validateMsg(const builtin_interfaces::msg::Time & msg)
6075
{
@@ -111,6 +126,23 @@ bool validateMsg(const geometry_msgs::msg::Pose & msg)
111126
return true;
112127
}
113128

129+
bool validateMsg(const geometry_msgs::msg::PoseWithCovariance & msg)
130+
{
131+
// check sub-type
132+
if (!validateMsg(msg.pose)) {return false;}
133+
if (!validateMsg(msg.covariance)) {return false;}
134+
135+
return true;
136+
}
137+
138+
bool validateMsg(const geometry_msgs::msg::PoseWithCovarianceStamped & msg)
139+
{
140+
// check sub-type
141+
if (!validateMsg(msg.header)) {return false;}
142+
if (!validateMsg(msg.pose)) {return false;}
143+
return true;
144+
}
145+
114146

115147
// Function to verify map meta information
116148
bool validateMsg(const nav_msgs::msg::MapMetaData & msg)

‎nav2_util/test/test_validation_messages.cpp‎

Lines changed: 121 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -244,4 +244,125 @@ TEST(ValidateMessagesTest, OccupancyGridCheck) {
244244
EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid));
245245
}
246246

247+
TEST(ValidateMessagesTest, PoseWithCovarianceCheck) {
248+
// Valid message
249+
geometry_msgs::msg::PoseWithCovariance validate_msg;
250+
validate_msg.covariance[0] = 0.25;
251+
// assign other covariance values...
252+
validate_msg.covariance[35] = 0.06853891909122467;
253+
254+
validate_msg.pose.position.x = 0.50010401010515571;
255+
validate_msg.pose.position.y = 1.7468730211257935;
256+
validate_msg.pose.position.z = 0.0;
257+
258+
validate_msg.pose.orientation.x = 0.9440542194053062;
259+
validate_msg.pose.orientation.y = 0.0;
260+
validate_msg.pose.orientation.z = 0.0;
261+
validate_msg.pose.orientation.w = -0.32979028309372299;
262+
263+
EXPECT_TRUE(nav2_util::validateMsg(validate_msg));
264+
265+
// Invalid messages
266+
geometry_msgs::msg::PoseWithCovariance invalidate_msg1;
267+
invalidate_msg1.covariance[0] = 0.25;
268+
// assign other covariance values...
269+
invalidate_msg1.covariance[7] = NAN;
270+
invalidate_msg1.covariance[9] = NAN;
271+
invalidate_msg1.covariance[35] = 0.06853891909122467;
272+
273+
invalidate_msg1.pose.position.x = 0.50010401010515571;
274+
invalidate_msg1.pose.position.y = 1.7468730211257935;
275+
invalidate_msg1.pose.position.z = 0.0;
276+
277+
invalidate_msg1.pose.orientation.x = 0.9440542194053062;
278+
invalidate_msg1.pose.orientation.y = 0.0;
279+
invalidate_msg1.pose.orientation.z = 0.0;
280+
invalidate_msg1.pose.orientation.w = -0.32979028309372299;
281+
282+
EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1));
283+
284+
geometry_msgs::msg::PoseWithCovariance invalidate_msg2;
285+
invalidate_msg2.covariance[0] = 0.25;
286+
// assign other covariance values...
287+
invalidate_msg2.covariance[35] = 0.06853891909122467;
288+
289+
invalidate_msg2.pose.position.x = NAN;
290+
invalidate_msg2.pose.position.y = 1.7468730211257935;
291+
invalidate_msg2.pose.position.z = 0.0;
292+
293+
invalidate_msg2.pose.orientation.x = 0.9440542194053062;
294+
invalidate_msg2.pose.orientation.y = 0.0;
295+
invalidate_msg2.pose.orientation.z = 0.0;
296+
invalidate_msg2.pose.orientation.w = -0.32979028309372299;
297+
298+
EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2));
299+
}
300+
301+
TEST(ValidateMessagesTest, PoseWithCovarianceStampedCheck) {
302+
// Valid message
303+
geometry_msgs::msg::PoseWithCovarianceStamped validate_msg;
304+
validate_msg.header.frame_id = "map";
305+
validate_msg.header.stamp.sec = 1711029956;
306+
validate_msg.header.stamp.nanosec = 146734875;
307+
308+
validate_msg.pose.covariance[0] = 0.25;
309+
// assign other covariance values...
310+
validate_msg.pose.covariance[35] = 0.06853891909122467;
311+
312+
validate_msg.pose.pose.position.x = 0.50010401010515571;
313+
validate_msg.pose.pose.position.y = 1.7468730211257935;
314+
validate_msg.pose.pose.position.z = 0.0;
315+
316+
validate_msg.pose.pose.orientation.x = 0.9440542194053062;
317+
validate_msg.pose.pose.orientation.y = 0.0;
318+
validate_msg.pose.pose.orientation.z = 0.0;
319+
validate_msg.pose.pose.orientation.w = -0.32979028309372299;
320+
321+
EXPECT_TRUE(nav2_util::validateMsg(validate_msg));
322+
323+
// Invalid messages
324+
geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg1;
325+
invalidate_msg1.header.frame_id = "map";
326+
invalidate_msg1.header.stamp.sec = 1711029956;
327+
invalidate_msg1.header.stamp.nanosec = 146734875;
328+
329+
invalidate_msg1.pose.covariance[0] = 0.25;
330+
// assign other covariance values...
331+
invalidate_msg1.pose.covariance[7] = NAN;
332+
invalidate_msg1.pose.covariance[9] = NAN;
333+
invalidate_msg1.pose.covariance[35] = 0.06853891909122467;
334+
335+
invalidate_msg1.pose.pose.position.x = 0.50010401010515571;
336+
invalidate_msg1.pose.pose.position.y = 1.7468730211257935;
337+
invalidate_msg1.pose.pose.position.z = 0.0;
338+
339+
invalidate_msg1.pose.pose.orientation.x = 0.9440542194053062;
340+
invalidate_msg1.pose.pose.orientation.y = 0.0;
341+
invalidate_msg1.pose.pose.orientation.z = 0.0;
342+
invalidate_msg1.pose.pose.orientation.w = -0.32979028309372299;
343+
344+
EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1));
345+
346+
geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg2;
347+
invalidate_msg2.header.frame_id = "";
348+
invalidate_msg2.header.stamp.sec = 1711029956;
349+
invalidate_msg2.header.stamp.nanosec = 146734875;
350+
351+
invalidate_msg2.pose.covariance[0] = 0.25;
352+
// assign other covariance values...
353+
invalidate_msg2.pose.covariance[35] = 0.06853891909122467;
354+
355+
invalidate_msg2.pose.pose.position.x = 0.50010401010515571;
356+
invalidate_msg2.pose.pose.position.y = 1.7468730211257935;
357+
invalidate_msg2.pose.pose.position.z = 0.0;
358+
359+
invalidate_msg2.pose.pose.orientation.x = 0.9440542194053062;
360+
invalidate_msg2.pose.pose.orientation.y = 0.0;
361+
invalidate_msg2.pose.pose.orientation.z = 0.0;
362+
invalidate_msg2.pose.pose.orientation.w = -0.32979028309372299;
363+
364+
EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2));
365+
}
366+
367+
247368
// Add more test cases for other validateMsg functions if needed

0 commit comments

Comments
 (0)