diff --git a/autoware_iv_internal_api_adaptor/script/map_hash_generator b/autoware_iv_internal_api_adaptor/script/map_hash_generator new file mode 100755 index 0000000..5fb5755 --- /dev/null +++ b/autoware_iv_internal_api_adaptor/script/map_hash_generator @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 + +# Copyright 2021 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import hashlib +import pathlib + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSProfile +from tier4_external_api_msgs.msg import MapHash +from tier4_external_api_msgs.msg import ResponseStatus +from tier4_external_api_msgs.srv import GetTextFile + + +class MapHashGenerator(Node): + def __init__(self): + super().__init__("map_hash_generator") + self.lanelet_path = self.declare_parameter("lanelet2_map_path", "").value + self.lanelet_text = self.load_lanelet_text(self.lanelet_path) + self.lanelet_hash = self.generate_lanelet_file_hash(self.lanelet_text) + + self.pcd_map_path = self.declare_parameter("pointcloud_map_path", "").value + self.pcd_map_hash = self.generate_pcd_file_hash(self.pcd_map_path) + + qos_profile = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + msg = MapHash() + msg.lanelet = self.lanelet_hash + msg.pcd = self.pcd_map_hash + self.pub = self.create_publisher(MapHash, "/api/autoware/get/map/info/hash", qos_profile) + self.pub.publish(msg) + + self.srv = self.create_service( + GetTextFile, "/api/autoware/get/map/lanelet/xml", self.on_get_lanelet_xml + ) # noqa: E501 + + def on_get_lanelet_xml(self, request, response): + response.status.code = ResponseStatus.SUCCESS + response.file.text = self.lanelet_text + return response + + @staticmethod + def load_lanelet_text(path): + path = pathlib.Path(path) + return path.read_text() if path.is_file() else "" + + @staticmethod + def generate_lanelet_file_hash(data): + return hashlib.sha256(data.encode()).hexdigest() if data else "" + + def update_hash(self, m, path): + try: + with open(path, "rb") as pcd_file: + m.update(pcd_file.read()) + except FileNotFoundError as e: + self.get_logger().error(e) + return False + return True + + def generate_pcd_file_hash(self, path): + path = pathlib.Path(path) + if path.is_file(): + if not path.suffix == ".pcd": + self.get_logger().error(f"[{path}] is not pcd file") + return "" + m = hashlib.sha256() + if not self.update_hash(m, path): + return "" + return m.hexdigest() + + if path.is_dir(): + m = hashlib.sha256() + for pcd_file_path in sorted(path.iterdir()): + if not pcd_file_path.suffix == ".pcd": + continue + if not self.update_hash(m, pcd_file_path): + return "" + if m.hexdigest() == hashlib.sha256().hexdigest(): + self.get_logger().error(f"there are no pcd files in [{path}]") + return "" + return m.hexdigest() + + self.get_logger().error(f"[{path}] is neither file nor directory") + return "" + + +def main(args=None): + rclpy.init(args=args) + node = MapHashGenerator() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + pass