diff --git a/grid_map_ros/CMakeLists.txt b/grid_map_ros/CMakeLists.txt index b116a2325..31b6be135 100644 --- 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 new file mode 100644 index 000000000..10eb9a495 Binary files /dev/null and b/grid_map_ros/resource/double_chatter/double_chatter.db3 differ diff --git a/grid_map_ros/resource/double_chatter/metadata.yaml b/grid_map_ros/resource/double_chatter/metadata.yaml new file mode 100644 index 000000000..0216d7d10 --- /dev/null +++ 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 index 23a6edc47..66facc5fc 100644 --- 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 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 index 221629236..87bf1a72f 100644 --- a/grid_map_ros/test/GridMapRosTest.cpp +++ b/grid_map_ros/test/GridMapRosTest.cpp @@ -132,6 +132,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;