Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix LoadFromBag assumptions causing C++ exceptions during serialization (backport #438) #468

Merged
merged 1 commit into from
Jul 31, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .github/workflows/colcon.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,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

3 changes: 3 additions & 0 deletions grid_map_ros/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
Binary file not shown.
57 changes: 57 additions & 0 deletions grid_map_ros/resource/double_chatter/metadata.yaml
Original file line number Diff line number Diff line change
@@ -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
35 changes: 35 additions & 0 deletions grid_map_ros/resource/test_multitopic/metadata.yaml
Original file line number Diff line number Diff line change
@@ -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: ~
Binary file not shown.
26 changes: 26 additions & 0 deletions grid_map_ros/src/GridMapRosConverter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
30 changes: 30 additions & 0 deletions grid_map_ros/test/GridMapRosTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down