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 @@ pcl_conversions rclcpp rclcpp_components + rosbag2_storage_default_plugins tf2_geometry_msgs tf2_ros tier4_autoware_utils 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 #include +#include 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_);