Skip to content

Commit 8dcf206

Browse files
Ryanf55wep21
Ryanf55
authored and
wep21
committed
Fix LoadFromBag assumptions causing C++ exceptions during serialization (#438)
Fix LoadFromBag assumptions * Not all bags have only GridMap messages * Not all bags have GridMap on the right topic * Add test for trying to load a grid map from a bag that doesn't contain it on the expected topic * Add nullptr check on reader messages * Remove unused include * Don't skip reporting when tests fail
1 parent b234d1a commit 8dcf206

File tree

8 files changed

+152
-1
lines changed

8 files changed

+152
-1
lines changed

‎.github/workflows/colcon.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ jobs:
4444
run: |
4545
source /opt/ros/${{matrix.config.rosdistro}}/setup.bash
4646
source install/setup.bash
47-
colcon test --return-code-on-test-failure --paths src/grid_map/* --event-handlers=console_cohesion+
47+
colcon test --paths src/grid_map/* --event-handlers=console_cohesion+
4848
colcon test-result --all --verbose
4949
shell: bash
5050

‎grid_map_ros/CMakeLists.txt

+3
Original file line numberDiff line numberDiff line change
@@ -115,6 +115,9 @@ if(BUILD_TESTING)
115115
test/test_grid_map_ros.cpp
116116
test/GridMapRosTest.cpp
117117
)
118+
119+
file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/resource/double_chatter DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
120+
file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/resource/test_multitopic DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
118121
endif()
119122

120123
if(TARGET ${PROJECT_NAME}-test)

‎grid_map_ros/resource/double_chatter/double_chatter.db3

40 KB
Binary file not shown.

‎grid_map_ros/resource/double_chatter/metadata.yaml

+57
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
rosbag2_bagfile_information:
2+
version: 8
3+
storage_identifier: sqlite3
4+
duration:
5+
nanoseconds: 2844280786
6+
starting_time:
7+
nanoseconds_since_epoch: 1708069847130953754
8+
message_count: 25
9+
topics_with_message_count:
10+
- topic_metadata:
11+
name: /chatter1
12+
type: std_msgs/msg/String
13+
serialization_format: cdr
14+
offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false"
15+
type_description_hash: RIHS01_df668c740482bbd48fb39d76a70dfd4bd59db1288021743503259e948f6b1a18
16+
message_count: 3
17+
- topic_metadata:
18+
name: /chatter2
19+
type: std_msgs/msg/String
20+
serialization_format: cdr
21+
offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false"
22+
type_description_hash: RIHS01_df668c740482bbd48fb39d76a70dfd4bd59db1288021743503259e948f6b1a18
23+
message_count: 3
24+
- topic_metadata:
25+
name: /events/write_split
26+
type: rosbag2_interfaces/msg/WriteSplitEvent
27+
serialization_format: cdr
28+
offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false"
29+
type_description_hash: RIHS01_5ef58f7106a5cff8f5a794028c18117ee21015850ddcc567df449f41932960f8
30+
message_count: 0
31+
- topic_metadata:
32+
name: /parameter_events
33+
type: rcl_interfaces/msg/ParameterEvent
34+
serialization_format: cdr
35+
offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false"
36+
type_description_hash: RIHS01_043e627780fcad87a22d225bc2a037361dba713fca6a6b9f4b869a5aa0393204
37+
message_count: 0
38+
- topic_metadata:
39+
name: /rosout
40+
type: rcl_interfaces/msg/Log
41+
serialization_format: cdr
42+
offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false"
43+
type_description_hash: RIHS01_e28ce254ca8abc06abf92773b74602cdbf116ed34fbaf294fb9f81da9f318eac
44+
message_count: 19
45+
compression_format: ""
46+
compression_mode: ""
47+
relative_file_paths:
48+
- double_chatter.db3
49+
files:
50+
- path: double_chatter.db3
51+
starting_time:
52+
nanoseconds_since_epoch: 1708069847130953754
53+
duration:
54+
nanoseconds: 2844280786
55+
message_count: 25
56+
custom_data: ~
57+
ros_distro: rolling

‎grid_map_ros/resource/test_multitopic/metadata.yaml

+35
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
rosbag2_bagfile_information:
2+
version: 7
3+
storage_identifier: sqlite3
4+
duration:
5+
nanoseconds: 2640963805
6+
starting_time:
7+
nanoseconds_since_epoch: 1708420680345493502
8+
message_count: 2
9+
topics_with_message_count:
10+
- topic_metadata:
11+
name: /chatter
12+
type: std_msgs/msg/String
13+
serialization_format: cdr
14+
offered_qos_profiles: "- history: 1\n depth: 7\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false"
15+
type_description_hash: RIHS01_df668c740482bbd48fb39d76a70dfd4bd59db1288021743503259e948f6b1a18
16+
message_count: 1
17+
- topic_metadata:
18+
name: /grid_map
19+
type: grid_map_msgs/msg/GridMap
20+
serialization_format: cdr
21+
offered_qos_profiles: "- history: 1\n depth: 10\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false"
22+
type_description_hash: RIHS01_343b0e72887541beda6b035cc053f2d6fffaad9d6dcb2773c15a808dfca31fde
23+
message_count: 1
24+
compression_format: ""
25+
compression_mode: ""
26+
relative_file_paths:
27+
- test_multitopic_0.db3
28+
files:
29+
- path: test_multitopic_0.db3
30+
starting_time:
31+
nanoseconds_since_epoch: 1708420680345493502
32+
duration:
33+
nanoseconds: 2640963805
34+
message_count: 2
35+
custom_data: ~

‎grid_map_ros/resource/test_multitopic/test_multitopic_0.db3

32 KB
Binary file not shown.

‎grid_map_ros/src/GridMapRosConverter.cpp

+26
Original file line numberDiff line numberDiff line change
@@ -721,8 +721,34 @@ bool GridMapRosConverter::loadFromBag(
721721
grid_map_msgs::msg::GridMap extracted_gridmap_msg;
722722
rclcpp::Serialization<grid_map_msgs::msg::GridMap> serialization;
723723

724+
// Validate the received bag topic exists and
725+
// is of the correct type to prevent later serialization exception.
726+
const auto topic_metadata = reader.get_all_topics_and_types();
727+
bool topic_is_correct_type = false;
728+
for (const auto & m : topic_metadata) {
729+
if (m.name == topic && m.type == "grid_map_msgs/msg/GridMap") {
730+
topic_is_correct_type = true;
731+
}
732+
}
733+
if (!topic_is_correct_type) {
734+
RCLCPP_ERROR(
735+
rclcpp::get_logger(
736+
"loadFromBag"), "Bagfile does not contain a GridMap message on the expected topic '%s'",
737+
topic.c_str());
738+
return false;
739+
}
740+
724741
while (reader.has_next()) {
725742
auto bag_message = reader.read_next();
743+
if (bag_message == nullptr) {
744+
continue;
745+
}
746+
747+
// Only read messages on the correct topic.
748+
// https://github.com/ANYbotics/grid_map/issues/401
749+
if (bag_message->topic_name != topic) {
750+
continue;
751+
}
726752

727753
if (bag_message->serialized_data != NULL) {
728754
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);

‎grid_map_ros/test/GridMapRosTest.cpp

+30
Original file line numberDiff line numberDiff line change
@@ -132,6 +132,36 @@ TEST(RosbagHandling, saveLoadWithTime)
132132
rcpputils::fs::remove_all(dir);
133133
}
134134

135+
TEST(RosbagHandling, wrongTopicType)
136+
{
137+
// This test validates LoadFromBag will reject a topic of the wrong type.
138+
// See https://github.com/ANYbotics/grid_map/issues/401.
139+
140+
std::string pathToBag = "double_chatter";
141+
string topic = "/chatter1";
142+
GridMap gridMapOut;
143+
rcpputils::fs::path dir(pathToBag);
144+
145+
EXPECT_FALSE(GridMapRosConverter::loadFromBag(pathToBag, topic, gridMapOut));
146+
}
147+
148+
TEST(RosbagHandling, checkTopicTypes)
149+
{
150+
// This test validates loadFromBag will reject a topic of the wrong type or
151+
// non-existing topic and accept a correct topic.
152+
153+
std::string pathToBag = "test_multitopic";
154+
string topic_wrong = "/chatter";
155+
string topic_correct = "/grid_map";
156+
string topic_non_existing = "/grid_map_non_existing";
157+
GridMap gridMapOut;
158+
rcpputils::fs::path dir(pathToBag);
159+
160+
EXPECT_FALSE(GridMapRosConverter::loadFromBag(pathToBag, topic_wrong, gridMapOut));
161+
EXPECT_TRUE(GridMapRosConverter::loadFromBag(pathToBag, topic_correct, gridMapOut));
162+
EXPECT_FALSE(GridMapRosConverter::loadFromBag(pathToBag, topic_non_existing, gridMapOut));
163+
}
164+
135165
TEST(OccupancyGridConversion, withMove)
136166
{
137167
grid_map::GridMap map;

0 commit comments

Comments
 (0)