added 2 more scripts

scripts/bim2ros.py

@@ -0,0 +1,119 @@
+#!/usr/bin/python3
+import numpy as np
+import ifcopenshell
+import ifcopenshell.util.placement
+import ifcopenshell.util.selector
+import ifcopenshell.util.shape
+import ifcopenshell.geom
+import rospy
+from sensor_msgs.msg import PointCloud2
+from geometry_msgs.msg import PointStamped
+from sensor_msgs import point_cloud2
+from visualization_msgs.msg import Marker, MarkerArray
+import tf2_ros
+from tf2_geometry_msgs import do_transform_point
+
+element_occurrences = {}  # Global variable to store element occurrences
+
+def load_ifc_model(model_path):
+    rospy.loginfo("Loading IFC model...")
+    try:
+        model = ifcopenshell.open(model_path)
+        rospy.loginfo("IFC model loaded.")
+        return model
+    except Exception as e:
+        rospy.logerr(f"Failed to load IFC model: {e}")
+        return None
+
+def find_closest_objects_to_point_cloud(points):
+    rospy.loginfo("Finding closest objects to point cloud...")
+    global element_occurrences
+    for point in points:
+        index = point2grid(point[0], point[1], point[2], onedivres, grid_stepy, grid_stepz)
+        if loaded_data_zeros[index] == 0 and loaded_data[index]:
+            element_value = loaded_data[index]
+            element_occurrences[element_value] = element_occurrences.get(element_value, 0) + 1
+            loaded_data_zeros[index] = 1
+
+    for element_value, count in element_occurrences.items():
+        rospy.loginfo(f"Value {element_value}: {count} times")
+
+    return []
+
+def point2grid(x, y, z, onedivres, grid_stepy, grid_stepz):
+    index = int(np.floor(x * onedivres) + np.floor(y * onedivres) * grid_stepy + np.floor(z * onedivres) * grid_stepz)
+    return index
+
+def crop_cloud(cl, mindist=0.00, maxdist=100):
+    cldist = np.linalg.norm(cl[:, 0:2], axis=1)
+    cropped_cloud = cl[(cldist > mindist) & (cldist <= maxdist), :]
+    return cropped_cloud
+
+def save_element_occurrences(filename):
+    global element_occurrences
+    try:
+        with open(filename, 'w') as f:
+            for element_value, count in element_occurrences.items():
+                f.write(f"{element_value}: {count}\n")
+        rospy.loginfo(f"Element occurrences saved to {filename}")
+    except Exception as e:
+        rospy.logerr(f"Failed to save element occurrences: {e}")
+
+def pointcloud_callback(msg):
+    global element_occurrences
+    rospy.loginfo("Received point cloud message.")
+    points = point_cloud2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True)
+    pcl_example = [(point[0], point[1], point[2]) for point in points]
+    pcl_example = crop_cloud(np.array(pcl_example))
+    rospy.loginfo(f"Reduced number of LiDAR points to {len(pcl_example)}")
+
+    transformed_points = []
+
+    for point in pcl_example:
+        point_stamped = PointStamped()
+        point_stamped.header.stamp = rospy.Time.now()
+        point_stamped.header.frame_id = "velodyne_base_link"
+        point_stamped.point.x = point[0]
+        point_stamped.point.y = point[1]
+        point_stamped.point.z = point[2]
+        try:
+            transformed_point_c = tf_buffer.transform(point_stamped, target_frame='map', timeout=rospy.Duration(0.1))
+            transformed_points.append((transformed_point_c.point.x, transformed_point_c.point.y, transformed_point_c.point.z))
+        except tf2_ros.TransformException as ex:
+            rospy.logwarn(f"Transform failed: {ex}")
+
+    rospy.loginfo("Applied transformations.")
+    rospy.loginfo("Finding objects")
+
+    closest_objects = find_closest_objects_to_point_cloud(transformed_points)
+
+    # Trigger to save element_occurrences (example: save when point cloud processed)
+    save_element_occurrences("element_occurrences.txt")
+
+def main():
+    global tf_buffer, onedivres, grid_stepy, grid_stepz, loaded_data, loaded_data_zeros
+
+    rospy.init_node('ifc_processor')
+
+    tf_buffer = tf2_ros.Buffer()
+    tf_listener = tf2_ros.TransformListener(tf_buffer)
+
+    RES = 0.1
+    GRID_SIZEX = 220
+    GRID_SIZEY = 220
+    GRID_SIZEZ = 20
+    onedivres = 1 / RES
+    grid_stepy = GRID_SIZEX
+    grid_stepz = GRID_SIZEX * GRID_SIZEY
+
+    file = "/home/rva_container/rva_exchange/catkin_ws/src/Heuristic_path_planners/scripts/semantic_grid_ints.npy"
+    loaded_data = np.load(file)
+    file = "/home/rva_container/rva_exchange/catkin_ws/src/Heuristic_path_planners/scripts/semantic_grid_zeros.npy"
+    loaded_data_zeros = np.load(file)
+
+    rospy.Subscriber("velodyne_points", PointCloud2, pointcloud_callback)
+
+    rospy.spin()
+
+if __name__ == "__main__":
+    main()

scripts/put_semantic_info_in_pcl.py

@@ -0,0 +1,100 @@
+import multiprocessing
+import ifcopenshell
+import ifcopenshell.geom
+import numpy as np
+from collections import defaultdict
+import open3d as o3d
+import json
+
+# Constants
+FILENAME = 'nube_atlas_recortado.pcd'
+RES = 0.1
+GRID_SIZEX = 220
+GRID_SIZEY = 220
+GRID_SIZEZ = 20
+
+# IFC File Handling
+def setup_ifc_geometry(ifc_file_path):
+    ifc_file = ifcopenshell.open(ifc_file_path)
+    settings = ifcopenshell.geom.settings()
+    settings.set(settings.DISABLE_OPENING_SUBTRACTIONS, False)
+    settings.set(settings.USE_WORLD_COORDS, True)
+    return ifc_file, settings
+
+def initialize_iterator(settings, ifc_file):
+    iterator = ifcopenshell.geom.iterator(settings, ifc_file, multiprocessing.cpu_count())
+    tree = ifcopenshell.geom.tree()
+    if iterator.initialize():
+        while True:
+            shape = iterator.get_native()
+            tree.add_element(shape)
+            if not iterator.next():
+                break
+    return tree
+
+# Point Cloud Processing
+def load_point_cloud(filename):
+    pcd = o3d.io.read_point_cloud(filename)
+    return np.asarray(pcd.points)
+
+def point2grid(x, y, z, onedivres, grid_stepy, grid_stepz):
+    index = np.floor(x * onedivres) + np.floor(y * onedivres) * grid_stepy + np.floor(z * onedivres) * grid_stepz
+    return int(index)
+
+# Grid Initialization and Processing
+def process_points(points, tree, onedivres, grid_stepy, grid_stepz):
+    result_dict = defaultdict(int)
+    size = int(np.floor(GRID_SIZEX * onedivres) * np.floor(GRID_SIZEY * onedivres) * np.floor(GRID_SIZEZ * onedivres))
+
+    # Create grids for storing assigned integers and zeros
+    semantic_grid_ints = np.zeros(size)
+    semantic_grid_zeros = np.zeros(size)
+
+
+    global_id_to_int = {}
+    current_int = 0
+
+    for x, z, y in points:
+        search_point = (float(x), float(y) - 68.6, float(z) - 1)
+        elements = tree.select(search_point, extend=0.01)
+
+        if elements:
+            for item in elements:
+                result_dict[item.GlobalId] += 1
+
+                if item.GlobalId not in global_id_to_int:
+                    global_id_to_int[item.GlobalId] = current_int
+                    current_int += 1
+
+                assigned_int = global_id_to_int[item.GlobalId]
+                index = point2grid(x, y, z, onedivres, grid_stepy, grid_stepz)
+                semantic_grid_ints[index] = assigned_int
+                print(f'{x},{y},{z}')
+
+    return result_dict, global_id_to_int, semantic_grid_ints, semantic_grid_zeros
+
+# Saving Results
+def save_results(global_id_to_int, result_dict, semantic_grid_ints, semantic_grid_zeros):
+    with open('global_id_mapping.json', 'w') as file:
+        json.dump(global_id_to_int, file, indent=4)
+
+    with open('result_dict.json', 'w') as file:
+        json.dump(result_dict, file, indent=4)
+
+    # Save the semantic grids as npy files
+    np.save('semantic_grid_ints.npy', semantic_grid_ints)
+    np.save('semantic_grid_zeros.npy', semantic_grid_zeros)
+
+# Main Execution
+if __name__ == "__main__":
+    ifc_file_path = 'models/model.ifc'
+    ifc_file, settings = setup_ifc_geometry(ifc_file_path)
+    tree = initialize_iterator(settings, ifc_file)
+
+    points = load_point_cloud(FILENAME)
+    onedivres = 1 / RES
+    grid_stepy = GRID_SIZEX
+    grid_stepz = GRID_SIZEX * GRID_SIZEY
+
+    result_dict, global_id_to_int, semantic_grid_ints, semantic_grid_zeros = process_points(points, tree, onedivres, grid_stepy, grid_stepz)
+    save_results(global_id_to_int, result_dict, semantic_grid_ints, semantic_grid_zeros)