fix(elevation_map_loader): remove and create map bagfile if it cannot…

… be loaded (backport autowarefoundation#1772) (#218) fix(elevation_map_loader): remove and create map bagfile if it cannot be loaded (autowarefoundation#1772) * fix(elevation_map_loader): remove and create map bagfile if it cannot be loaded Signed-off-by: Shin-kyoto <[email protected]> * fix(elevation_map_loader): add dependencies Signed-off-by: Shin-kyoto <[email protected]> * fix(elevation_map_loader): use snake case for variable Signed-off-by: Shin-kyoto <[email protected]> Signed-off-by: Shin-kyoto <[email protected]> Signed-off-by: Shin-kyoto <[email protected]> Co-authored-by: Shintaro Tomie <[email protected]>
tier4 · Dec 23, 2022 · ec11dab · ec11dab
1 parent ac82f10
commit ec11dab
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 2 deletions.
diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml
@@ -19,6 +19,7 @@
   <depend>pcl_conversions</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>rosbag2_storage_default_plugins</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_autoware_utils</depend>

diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp
@@ -44,6 +44,7 @@
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <rosbag2_storage_default_plugins/sqlite/sqlite_statement_wrapper.hpp>
 
 ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & options)
 : Node("elevation_map_loader", options)
@@ -102,8 +103,25 @@ void ElevationMapLoaderNode::publish()
     RCLCPP_INFO(
       this->get_logger(), "Load elevation map from: %s",
       data_manager_.elevation_map_path_->c_str());
-    grid_map::GridMapRosConverter::loadFromBag(
-      *data_manager_.elevation_map_path_, "elevation_map", elevation_map_);
+
+    // Check if bag can be loaded
+    bool is_bag_loaded = false;
+    try {
+      is_bag_loaded = grid_map::GridMapRosConverter::loadFromBag(
+        *data_manager_.elevation_map_path_, "elevation_map", elevation_map_);
+    } catch (rosbag2_storage_plugins::SqliteException & e) {
+      is_bag_loaded = false;
+    }
+    if (!is_bag_loaded) {
+      // Delete directory including elevation map if bag is broken
+      RCLCPP_ERROR(
+        this->get_logger(), "Try to loading bag, but bag is broken. Remove %s",
+        data_manager_.elevation_map_path_->c_str());
+      std::filesystem::remove_all(data_manager_.elevation_map_path_->c_str());
+      // Create elevation map from pointcloud map if bag is broken
+      RCLCPP_INFO(this->get_logger(), "Create elevation map from pointcloud map ");
+      createElevationMap();
+    }
   }
 
   elevation_map_.setFrameId(map_frame_);