Got python version working again and cpp version building.

Still need to test cpp version

Author: jwag

elevation_mapping_cupy/CMakeLists.txt

@@ -5,10 +5,16 @@ project(elevation_mapping_cupy)
 # set(CMAKE_CXX_STANDARD 11)
 # set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
 # Compiler options
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
+
 # Additional dependencies
 # find_package(Python COMPONENTS Interpreter Development)
 find_package(PythonInterp 3 REQUIRED)
@@ -17,17 +23,17 @@ find_package(Eigen3 REQUIRED)
 find_package(OpenCV REQUIRED)
 
 # Find pybind11
-
 message([MAIN] "Found pybind11 v${pybind11_VERSION}: ${pybind11_INCLUDE_DIRS}")
 message([MAIN] "pybind11_INCLUDE_DIRS = ${pybind11_INCLUDE_DIRS}")
 message([MAIN] "pybind11_LIBRARIES = ${pybind11_LIBRARIES}")
 
 # Find ROS 2 dependencies
-find_package(message_filters REQUIRED)
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclpy REQUIRED)
 find_package(builtin_interfaces REQUIRED)
+find_package(message_filters REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(sensor_msgs REQUIRED)
@@ -38,13 +44,9 @@ find_package(grid_map_ros REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(pcl_ros REQUIRED)
 find_package(tf2_eigen REQUIRED)
-find_package(ament_cmake_python REQUIRED)
 find_package(python_cmake_module REQUIRED)
 find_package(point_cloud_transport REQUIRED)
 
-_ament_cmake_python_register_environment_hook()
-ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR script/${PROJECT_NAME})
-
 # List dependencies for ament_target_dependencies
 set(dependencies
   rclcpp
@@ -92,52 +94,39 @@ target_link_libraries(elevation_mapping_node elevation_mapping_ros ${OpenCV_LIBR
 ament_target_dependencies(elevation_mapping_node ${dependencies})
 
 
+# Install targets Not sure if these other argrs are necessary
+# install(
+#   TARGETS elevation_mapping_ros elevation_mapping_node 
+#   DESTINATION lib/${PROJECT_NAME}
+#   ARCHIVE DESTINATION lib
+#   LIBRARY DESTINATION lib
+#   RUNTIME DESTINATION bin
+# )
 
-# Install targets
-install(
-  TARGETS  elevation_mapping_node
-  DESTINATION lib/${PROJECT_NAME}
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
-)
-
-
-install(TARGETS 
-  elevation_mapping_node 
+install(TARGETS elevation_mapping_ros elevation_mapping_node 
   DESTINATION lib/${PROJECT_NAME}
 )
 
+# install(PROGRAMS  
+#   DESTINATION lib/${PROJECT_NAME}
+# )
 
+# Install launch, config, and rviz directories
 install(
-  TARGETS elevation_mapping_ros 
-  DESTINATION lib/${PROJECT_NAME}
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
+  DIRECTORY launch config rviz
+  DESTINATION share/${PROJECT_NAME}
 )
 
+# TODO: Understand if this line is necessary
+# _ament_cmake_python_register_environment_hook()
+# For use as a Python module outside of ROS 2
+ament_python_install_package(${PROJECT_NAME})
 
-
-
-install(PROGRAMS  
+# Install the Python ROS 2 modules
+install(PROGRAMS
+  scripts/elevation_mapping_node.py
   DESTINATION lib/${PROJECT_NAME}
 )
 
-# Install launch and config directories
-install(
-  DIRECTORY launch config
-  DESTINATION share/${PROJECT_NAME}
-)
-
-
-
-_ament_cmake_python_register_environment_hook()
-
-
-# ament_python_install_package(script/${PROJECT_NAME})
-# ament_python_install_package(script/${PROJECT_NAME})
-
-
 # Ament package declaration
 ament_package()

elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py

@@ -5,8 +5,6 @@
 import os
 from typing import List, Any, Tuple, Union
 
-import rclpy  # Import rclpy for ROS 2 logging
-from ament_index_python.packages import get_package_prefix
 import numpy as np
 import threading
 import subprocess
@@ -17,7 +15,6 @@
 )
 from elevation_mapping_cupy.parameter import Parameter
 
-
 from elevation_mapping_cupy.kernels import (
     add_points_kernel,
     add_color_kernel,
@@ -44,6 +41,7 @@
 )
 
 import cupy as cp
+import rclpy  # Import rclpy for ROS 2 logging
 
 xp = cp
 pool = cp.cuda.MemoryPool(cp.cuda.malloc_managed)
@@ -61,6 +59,7 @@ def __init__(self, param: Parameter):
         """
         # Initialize the ROS logger
         self.logger = rclpy.logging.get_logger('elevation_map')
+
         self.param = param
         self.data_type = self.param.data_type
         self.resolution = param.resolution
@@ -101,29 +100,24 @@ def __init__(self, param: Parameter):
         self.additive_mean_error = 0.0
 
         self.compile_kernels()
-        self.logger.info("Finished compiling kernels.")
+
         self.compile_image_kernels()
-        self.logger.info("Finished compiling image kernels.")
+
         self.semantic_map.initialize_fusion()
-        self.logger.info("Finished compiling semantic_map kernels.")
 
         weight_file = subprocess.getoutput('echo "' + param.weight_file + '"')
-        package_prefix = get_package_prefix('elevation_mapping_cupy')
-        self.logger.info("weight file : ." + str(package_prefix) + str(weight_file))
-        param.load_weights(package_prefix+weight_file)
+        param.load_weights(weight_file)
 
         if param.use_chainer:
             self.traversability_filter = get_filter_chainer(param.w1, param.w2, param.w3, param.w_out)
         else:
             self.traversability_filter = get_filter_torch(param.w1, param.w2, param.w3, param.w_out)
-        
         self.untraversable_polygon = xp.zeros((1, 2))
 
         # Plugins
         self.plugin_manager = PluginManager(cell_n=self.cell_n)
         plugin_config_file = subprocess.getoutput('echo "' + param.plugin_config_file + '"')
-        self.logger.info("plugin file : ." + str(package_prefix) + str(plugin_config_file))
-        self.plugin_manager.load_plugin_settings(package_prefix+plugin_config_file)
+        self.plugin_manager.load_plugin_settings(plugin_config_file)
 
         self.map_initializer = MapInitializer(self.initial_variance, param.initialized_variance, xp=cp, method="points")
 
@@ -359,7 +353,7 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori
         error = cp.array([0.0], dtype=cp.float32)
         error_cnt = cp.array([0], dtype=cp.float32)
         points = points_all[:, :3]
-        # additional_fusion = self.get_fusion_of_pcl(channels)
+
         with self.map_lock:
             self.shift_translation_to_map_center(t)
 
@@ -982,8 +976,7 @@ def initialize_map(self, points, method="cubic"):
         plugin_config_file="../config/plugin_config.yaml",
     )
     param.additional_layers = ["rgb", "grass", "tree", "people"]
-    # param.fusion_algorithms = ["color", "class_bayesian", "class_bayesian", "class_bayesian"]
-    param.fusion_algorithms = ["image_color", "pointcloud_color", "pointcloud_class_average"]
+    param.fusion_algorithms = ["color", "class_bayesian", "class_bayesian", "class_bayesian"]
     param.update()
     elevation = ElevationMap(param)
     layers = [

elevation_mapping_cupy/launch/elevation_mapping_turtle.launch.py

@@ -47,7 +47,7 @@ def generate_launch_description():
     )
     elevation_mapping_node = Node(
         package='elevation_mapping_cupy',
-        executable='elevation_mapping_node',
+        executable='elevation_mapping_node.py',
         name='elevation_mapping_node',
         output='screen',
         parameters=[

elevation_mapping_cupy/package.xml

@@ -5,6 +5,7 @@
     <description>Elevation mapping on GPU</description>
     <maintainer email="[email protected]">Takahiro Miki</maintainer>
     <license>MIT</license>
+
     <buildtool_depend>ament_cmake</buildtool_depend>
     <buildtool_depend>ament_cmake_python</buildtool_depend>
 
@@ -15,7 +16,8 @@
     <depend>std_srvs</depend>    
     <depend>sensor_msgs</depend>
     <depend>geometry_msgs</depend>
-    <depend>point_cloud_transport</depend>    
+    <!-- Might need to remove point_cloud_transport if not used -->
+    <depend>point_cloud_transport</depend>
     <depend>message_filters</depend>
     <depend>elevation_map_msgs</depend>
     <depend>grid_map_msgs</depend>
@@ -28,7 +30,6 @@
     <!-- Numpy version needs to be <2.0.0 -->
     <depend>numpy</depend>
     <depend>tf2_ros</depend>
-    <depend>message_filters</depend>
     <depend>cv_bridge</depend>
     <depend>ament_index_python</depend>
     <depend>tf_transformations</depend>
@@ -47,15 +48,14 @@
     <depend>simple-parsing</depend>
 
     <!-- Export dependencies -->
-    <!-- Might need to change to ament_cmake_python -->
-    <build_export_depend>ament_python</build_export_depend>
-    <exec_depend>ament_python</exec_depend>
+    <!-- <build_export_depend>ament_cmake_python</build_export_depend> -->
+    <!-- <exec_depend>ament_cmake_python</exec_depend> -->
 
     <test_depend>ament_lint_auto</test_depend>
     <test_depend>ament_lint_common</test_depend>
 
     <export>
-        <build_type>ament_python</build_type>
+        <!-- <build_type>ament_cmake_python</build_type> -->
         <build_type>ament_cmake</build_type>
     </export>