Fix LoadFromBag assumptions causing C++ exceptions during serializati…

…on (#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

(cherry picked from commit 25a1ea5)

.github/workflows/colcon.yaml

@@ -43,7 +43,7 @@ jobs:
       run: |
         source /opt/ros/${{matrix.config.rosdistro}}/setup.bash
         source install/setup.bash
-        colcon test --return-code-on-test-failure --paths src/grid_map/* --event-handlers=console_cohesion+
+        colcon test --paths src/grid_map/* --event-handlers=console_cohesion+
         colcon test-result --all --verbose
       shell: bash
 

grid_map_ros/CMakeLists.txt

@@ -115,6 +115,9 @@ if(BUILD_TESTING)
     test/test_grid_map_ros.cpp
     test/GridMapRosTest.cpp
   )
+
+  file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/resource/double_chatter DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+  file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/resource/test_multitopic DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
 endif()
 
 if(TARGET ${PROJECT_NAME}-test)

grid_map_ros/resource/double_chatter/metadata.yaml

@@ -0,0 +1,57 @@
+rosbag2_bagfile_information:
+  version: 8
+  storage_identifier: sqlite3
+  duration:
+    nanoseconds: 2844280786
+  starting_time:
+    nanoseconds_since_epoch: 1708069847130953754
+  message_count: 25
+  topics_with_message_count:
+    - topic_metadata:
+        name: /chatter1
+        type: std_msgs/msg/String
+        serialization_format: cdr
+        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"
+        type_description_hash: RIHS01_df668c740482bbd48fb39d76a70dfd4bd59db1288021743503259e948f6b1a18
+      message_count: 3
+    - topic_metadata:
+        name: /chatter2
+        type: std_msgs/msg/String
+        serialization_format: cdr
+        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"
+        type_description_hash: RIHS01_df668c740482bbd48fb39d76a70dfd4bd59db1288021743503259e948f6b1a18
+      message_count: 3
+    - topic_metadata:
+        name: /events/write_split
+        type: rosbag2_interfaces/msg/WriteSplitEvent
+        serialization_format: cdr
+        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"
+        type_description_hash: RIHS01_5ef58f7106a5cff8f5a794028c18117ee21015850ddcc567df449f41932960f8
+      message_count: 0
+    - topic_metadata:
+        name: /parameter_events
+        type: rcl_interfaces/msg/ParameterEvent
+        serialization_format: cdr
+        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"
+        type_description_hash: RIHS01_043e627780fcad87a22d225bc2a037361dba713fca6a6b9f4b869a5aa0393204
+      message_count: 0
+    - topic_metadata:
+        name: /rosout
+        type: rcl_interfaces/msg/Log
+        serialization_format: cdr
+        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"
+        type_description_hash: RIHS01_e28ce254ca8abc06abf92773b74602cdbf116ed34fbaf294fb9f81da9f318eac
+      message_count: 19
+  compression_format: ""
+  compression_mode: ""
+  relative_file_paths:
+    - double_chatter.db3
+  files:
+    - path: double_chatter.db3
+      starting_time:
+        nanoseconds_since_epoch: 1708069847130953754
+      duration:
+        nanoseconds: 2844280786
+      message_count: 25
+  custom_data: ~
+  ros_distro: rolling

grid_map_ros/resource/test_multitopic/metadata.yaml

@@ -0,0 +1,35 @@
+rosbag2_bagfile_information:
+  version: 7
+  storage_identifier: sqlite3
+  duration:
+    nanoseconds: 2640963805
+  starting_time:
+    nanoseconds_since_epoch: 1708420680345493502
+  message_count: 2
+  topics_with_message_count:
+    - topic_metadata:
+        name: /chatter
+        type: std_msgs/msg/String
+        serialization_format: cdr
+        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"
+        type_description_hash: RIHS01_df668c740482bbd48fb39d76a70dfd4bd59db1288021743503259e948f6b1a18
+      message_count: 1
+    - topic_metadata:
+        name: /grid_map
+        type: grid_map_msgs/msg/GridMap
+        serialization_format: cdr
+        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"
+        type_description_hash: RIHS01_343b0e72887541beda6b035cc053f2d6fffaad9d6dcb2773c15a808dfca31fde
+      message_count: 1
+  compression_format: ""
+  compression_mode: ""
+  relative_file_paths:
+    - test_multitopic_0.db3
+  files:
+    - path: test_multitopic_0.db3
+      starting_time:
+        nanoseconds_since_epoch: 1708420680345493502
+      duration:
+        nanoseconds: 2640963805
+      message_count: 2
+  custom_data: ~

grid_map_ros/src/GridMapRosConverter.cpp

@@ -721,8 +721,34 @@ bool GridMapRosConverter::loadFromBag(
   grid_map_msgs::msg::GridMap extracted_gridmap_msg;
   rclcpp::Serialization<grid_map_msgs::msg::GridMap> serialization;
 
+  // Validate the received bag topic exists and
+  // is of the correct type to prevent later serialization exception.
+  const auto topic_metadata = reader.get_all_topics_and_types();
+  bool topic_is_correct_type = false;
+  for (const auto & m : topic_metadata) {
+    if (m.name == topic && m.type == "grid_map_msgs/msg/GridMap") {
+      topic_is_correct_type = true;
+    }
+  }
+  if (!topic_is_correct_type) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger(
+        "loadFromBag"), "Bagfile does not contain a GridMap message on the expected topic '%s'",
+        topic.c_str());
+    return false;
+  }
+
   while (reader.has_next()) {
     auto bag_message = reader.read_next();
+    if (bag_message == nullptr) {
+      continue;
+    }
+
+    // Only read messages on the correct topic.
+    // https://github.com/ANYbotics/grid_map/issues/401
+    if (bag_message->topic_name != topic) {
+      continue;
+    }
 
     if (bag_message->serialized_data != NULL) {
       rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);

grid_map_ros/test/GridMapRosTest.cpp

@@ -132,6 +132,36 @@ TEST(RosbagHandling, saveLoadWithTime)
   rcpputils::fs::remove_all(dir);
 }
 
+TEST(RosbagHandling, wrongTopicType)
+{
+  // This test validates LoadFromBag will reject a topic of the wrong type.
+  // See https://github.com/ANYbotics/grid_map/issues/401.
+
+  std::string pathToBag = "double_chatter";
+  string topic = "/chatter1";
+  GridMap gridMapOut;
+  rcpputils::fs::path dir(pathToBag);
+
+  EXPECT_FALSE(GridMapRosConverter::loadFromBag(pathToBag, topic, gridMapOut));
+}
+
+TEST(RosbagHandling, checkTopicTypes)
+{
+  // This test validates loadFromBag will reject a topic of the wrong type or
+  // non-existing topic and accept a correct topic.
+
+  std::string pathToBag = "test_multitopic";
+  string topic_wrong = "/chatter";
+  string topic_correct = "/grid_map";
+  string topic_non_existing = "/grid_map_non_existing";
+  GridMap gridMapOut;
+  rcpputils::fs::path dir(pathToBag);
+
+  EXPECT_FALSE(GridMapRosConverter::loadFromBag(pathToBag, topic_wrong, gridMapOut));
+  EXPECT_TRUE(GridMapRosConverter::loadFromBag(pathToBag, topic_correct, gridMapOut));
+  EXPECT_FALSE(GridMapRosConverter::loadFromBag(pathToBag, topic_non_existing, gridMapOut));
+}
+
 TEST(OccupancyGridConversion, withMove)
 {
   grid_map::GridMap map;