From e6f294cdb3261d3ce21c5c67ad933cbd4dc36f54 Mon Sep 17 00:00:00 2001
From: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com>
Date: Tue, 6 Sep 2022 13:52:24 +0900
Subject: [PATCH] fix(elevation_map_loader): remove and create map bagfile if
 it cannot be loaded (#1772)

* fix(elevation_map_loader): remove and create map bagfile if it cannot be loaded

Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com>

* fix(elevation_map_loader): add dependencies

Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com>

* fix(elevation_map_loader): use snake case for variable

Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com>

Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com>
---
 perception/elevation_map_loader/package.xml   |  1 +
 .../src/elevation_map_loader_node.cpp         | 22 +++++++++++++++++--
 2 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml
index b60113c41c26f..1a727f591ffcd 100644
--- 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
index cac07dab64fa7..57f95eb7e1e89 100644
--- 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_);