diff --git a/.github/workflows/colcon.yaml b/.github/workflows/colcon.yaml index e753651de..258c505d6 100644 --- a/.github/workflows/colcon.yaml +++ b/.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 diff --git a/grid_map_ros/CMakeLists.txt b/grid_map_ros/CMakeLists.txt index b116a2325..2a75ce805 100644 --- a/grid_map_ros/CMakeLists.txt +++ b/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) 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/resource/test_multitopic/metadata.yaml b/grid_map_ros/resource/test_multitopic/metadata.yaml new file mode 100644 index 000000000..5e74e8863 --- /dev/null +++ b/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: ~ \ No newline at end of file diff --git a/grid_map_ros/resource/test_multitopic/test_multitopic_0.db3 b/grid_map_ros/resource/test_multitopic/test_multitopic_0.db3 new file mode 100644 index 000000000..2820fc4cd Binary files /dev/null and b/grid_map_ros/resource/test_multitopic/test_multitopic_0.db3 differ diff --git a/grid_map_ros/src/GridMapRosConverter.cpp b/grid_map_ros/src/GridMapRosConverter.cpp index 23a6edc47..8ec6efada 100644 --- a/grid_map_ros/src/GridMapRosConverter.cpp +++ b/grid_map_ros/src/GridMapRosConverter.cpp @@ -721,8 +721,34 @@ 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..e48fc350d 100644 --- a/grid_map_ros/test/GridMapRosTest.cpp +++ b/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;