Skip to content

Commit 8f00360

Browse files
GoesMgoes
GoesM
and
goes
authored
provide message validation check API (#4276)
* provide validation_message.hpp Signed-off-by: goes <GoesM@buaa.edu.cn> * fix typo Signed-off-by: goes <GoesM@buaa.edu.cn> * add test_validation_messages.cpp Signed-off-by: goes <GoesM@buaa.edu.cn> * change include-order Signed-off-by: goes <GoesM@buaa.edu.cn> * reformat Signed-off-by: goes <GoesM@buaa.edu.cn> * update test Signed-off-by: goes <GoesM@buaa.edu.cn> --------- Signed-off-by: goes <GoesM@buaa.edu.cn> Co-authored-by: goes <GoesM@buaa.edu.cn>
1 parent c6ccd8e commit 8f00360

File tree

5 files changed

+408
-0
lines changed

5 files changed

+408
-0
lines changed

‎nav2_amcl/src/amcl_node.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,7 @@
4949
#pragma GCC diagnostic pop
5050

5151
#include "nav2_amcl/portable_utils.hpp"
52+
#include "nav2_util/validate_messages.hpp"
5253

5354
using namespace std::placeholders;
5455
using rcl_interfaces::msg::ParameterType;
@@ -1390,6 +1391,10 @@ void
13901391
AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
13911392
{
13921393
RCLCPP_DEBUG(get_logger(), "AmclNode: A new map was received.");
1394+
if (!nav2_util::validateMsg(*msg)) {
1395+
RCLCPP_ERROR(get_logger(), "Received map message is malformed. Rejecting.");
1396+
return;
1397+
}
13931398
if (first_map_only_ && first_map_received_) {
13941399
return;
13951400
}

‎nav2_costmap_2d/plugins/static_layer.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,7 @@
4545
#include "pluginlib/class_list_macros.hpp"
4646
#include "tf2/convert.h"
4747
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
48+
#include "nav2_util/validate_messages.hpp"
4849

4950
PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer)
5051

@@ -277,6 +278,10 @@ StaticLayer::interpretValue(unsigned char value)
277278
void
278279
StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map)
279280
{
281+
if (!nav2_util::validateMsg(*new_map)) {
282+
RCLCPP_ERROR(logger_, "Received map message is malformed. Rejecting.");
283+
return;
284+
}
280285
if (!map_received_) {
281286
processMap(*new_map);
282287
map_received_ = true;

‎nav2_util/include/nav2_util/validate_messages.hpp‎

Lines changed: 147 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,147 @@
1+
// Copyright (c) 2024 GoesM
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
16+
#ifndef NAV2_UTIL__VALIDATE_MESSAGES_HPP_
17+
#define NAV2_UTIL__VALIDATE_MESSAGES_HPP_
18+
19+
#include <cmath>
20+
#include <iostream>
21+
22+
#include "nav_msgs/msg/occupancy_grid.hpp"
23+
#include "nav_msgs/msg/odometry.hpp"
24+
25+
26+
// @brief Validation Check
27+
// Check recieved message is safe or not for the nav2-system
28+
// For each msg-type known in nav2, we could check it as following:
29+
// if(!validateMsg()) RCLCPP_ERROR(,"malformed msg. Rejecting.")
30+
//
31+
// Workflow of validateMsg():
32+
// if here's a sub-msg-type in the recieved msg,
33+
// the content of sub-msg would be checked as sub-msg-type
34+
// then, check the whole recieved msg.
35+
//
36+
// Following conditions are involved in check:
37+
// 1> Value Check: to avoid damaged value like like `nan`, `INF`, empty string and so on
38+
// 2> Logic Check: to avoid value with bad logic,
39+
// like the size of `map` should be equal to `height*width`
40+
// 3> Any other needed condition could be joint here in future
41+
42+
namespace nav2_util
43+
{
44+
45+
46+
bool validateMsg(const double & num)
47+
{
48+
/* @brief double/float value check
49+
* if here'a need to check message validation
50+
* it should be avoid to use double value like `nan`, `inf`
51+
* otherwise, we regard it as an invalid message
52+
*/
53+
if (std::isinf(num)) {return false;}
54+
if (std::isnan(num)) {return false;}
55+
return true;
56+
}
57+
58+
const int NSEC_PER_SEC = 1e9; // 1 second = 1e9 nanosecond
59+
bool validateMsg(const builtin_interfaces::msg::Time & msg)
60+
{
61+
if (msg.nanosec >= NSEC_PER_SEC) {
62+
return false; // invalid nanosec-stamp
63+
}
64+
return true;
65+
}
66+
67+
bool validateMsg(const std_msgs::msg::Header & msg)
68+
{
69+
// check sub-type
70+
if (!validateMsg(msg.stamp)) {return false;}
71+
72+
/* @brief frame_id check
73+
* if here'a need to check message validation
74+
* it should at least have a non-empty frame_id
75+
* otherwise, we regard it as an invalid message
76+
*/
77+
if (msg.frame_id.empty()) {return false;}
78+
return true;
79+
}
80+
81+
bool validateMsg(const geometry_msgs::msg::Point & msg)
82+
{
83+
// check sub-type
84+
if (!validateMsg(msg.x)) {return false;}
85+
if (!validateMsg(msg.y)) {return false;}
86+
if (!validateMsg(msg.z)) {return false;}
87+
return true;
88+
}
89+
90+
const double epsilon = 1e-4;
91+
bool validateMsg(const geometry_msgs::msg::Quaternion & msg)
92+
{
93+
// check sub-type
94+
if (!validateMsg(msg.x)) {return false;}
95+
if (!validateMsg(msg.y)) {return false;}
96+
if (!validateMsg(msg.z)) {return false;}
97+
if (!validateMsg(msg.w)) {return false;}
98+
99+
if (abs(msg.x * msg.x + msg.y * msg.y + msg.z * msg.z + msg.w * msg.w - 1.0) >= epsilon) {
100+
return false;
101+
}
102+
103+
return true;
104+
}
105+
106+
bool validateMsg(const geometry_msgs::msg::Pose & msg)
107+
{
108+
// check sub-type
109+
if (!validateMsg(msg.position)) {return false;}
110+
if (!validateMsg(msg.orientation)) {return false;}
111+
return true;
112+
}
113+
114+
115+
// Function to verify map meta information
116+
bool validateMsg(const nav_msgs::msg::MapMetaData & msg)
117+
{
118+
// check sub-type
119+
if (!validateMsg(msg.origin)) {return false;}
120+
if (!validateMsg(msg.resolution)) {return false;}
121+
122+
// logic check
123+
// 1> we don't need an empty map
124+
if (msg.height == 0 || msg.width == 0) {return false;}
125+
return true;
126+
}
127+
128+
// for msg-type like map, costmap and others as `OccupancyGrid`
129+
bool validateMsg(const nav_msgs::msg::OccupancyGrid & msg)
130+
{
131+
// check sub-type
132+
if (!validateMsg(msg.header)) {return false;}
133+
// msg.data : @todo any check for it ?
134+
if (!validateMsg(msg.info)) {return false;}
135+
136+
// check logic
137+
if (msg.data.size() != msg.info.width * msg.info.height) {
138+
return false; // check map-size
139+
}
140+
return true;
141+
}
142+
143+
144+
} // namespace nav2_util
145+
146+
147+
#endif // NAV2_UTIL__VALIDATE_MESSAGES_HPP_

‎nav2_util/test/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -56,3 +56,7 @@ target_link_libraries(test_twist_publisher ${library_name})
5656
ament_add_gtest(test_twist_subscriber test_twist_subscriber.cpp)
5757
ament_target_dependencies(test_twist_subscriber rclcpp_lifecycle)
5858
target_link_libraries(test_twist_subscriber ${library_name})
59+
60+
ament_add_gtest(test_validation_messages test_validation_messages.cpp)
61+
ament_target_dependencies(test_validation_messages rclcpp_lifecycle)
62+
target_link_libraries(test_validation_messages ${library_name})

0 commit comments

Comments
 (0)