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 Signed-off-by: Ryan Friedman <[email protected]>
ANYbotics · Feb 16, 2024 · 44cda3b · 44cda3b
1 parent ef8a31b
commit 44cda3b
Show file tree

Hide file tree

Showing 5 changed files with 96 additions and 0 deletions.
diff --git a/grid_map_ros/CMakeLists.txt b/grid_map_ros/CMakeLists.txt
@@ -115,6 +115,8 @@ 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})
 endif()
 
 if(TARGET ${PROJECT_NAME}-test)

diff --git a/grid_map_ros/resource/double_chatter/double_chatter.db3 b/grid_map_ros/resource/double_chatter/double_chatter.db3
diff --git a/grid_map_ros/resource/double_chatter/metadata.yaml b/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
diff --git a/grid_map_ros/src/GridMapRosConverter.cpp b/grid_map_ros/src/GridMapRosConverter.cpp
@@ -721,8 +721,32 @@ 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);

diff --git a/grid_map_ros/test/GridMapRosTest.cpp b/grid_map_ros/test/GridMapRosTest.cpp
@@ -19,6 +19,7 @@
 #include <nav2_msgs/msg/costmap.hpp>
 #include <sensor_msgs/image_encodings.hpp>
 #include <rcpputils/filesystem_helper.hpp>
+#include <rosbag2_cpp/writer.hpp>
 
 // STD
 #include <string>
@@ -132,6 +133,18 @@ TEST(RosbagHandling, saveLoadWithTime)
   rcpputils::fs::remove_all(dir);
 }
 
+TEST(RosbagHandling, wrongTopicType)
+{
+  // This test validates LoadFromBag will reject a topic of the wrong type.
+
+  std::string pathToBag = "double_chatter";
+  string topic = "chatter1";
+  GridMap gridMapOut;
+  rcpputils::fs::path dir(pathToBag);
+
+  EXPECT_FALSE(GridMapRosConverter::loadFromBag(pathToBag, topic, gridMapOut));
+}
+
 TEST(OccupancyGridConversion, withMove)
 {
   grid_map::GridMap map;