Migrate ROS 1 elevation_mapping_cupy to ROS 2 (Humble) with Existing pybind11 Integration

README.md

@@ -1,3 +1,20 @@
+## ROS2 Elevation Mapping Cupy 
+ **Status**: Image and Pointcloud subscribers done
+### Features 
+- **Point cloud-based map update**: *Functional*
+- **Image-based map update**: *Functional*
+
+
+![HILS in Jetson AGX Orin]https://github.com/user-attachments/assets/120d7f41-e54a-4fdf-af30-3e09cd5061a1
+
+
+### Dependencies -
+- **ROS 2 Humble**
+- **CUDA 12.4**
+- **PyTorch 2.4.0**
+
+![Elevation Map in ROS 2 Humble with Gazebo ](https://github.com/user-attachments/assets/0dd9ebbe-a90d-486f-9871-81921308fab9)
+
 # Elevation Mapping cupy
 
 ![python tests](https://github.com/leggedrobotics/elevation_mapping_cupy/actions/workflows/python-tests.yml/badge.svg)

elevation_map_msgs/CMakeLists.txt

@@ -1,32 +1,55 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(elevation_map_msgs)
 
-find_package(catkin REQUIRED)
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)  # Added std_msgs
 
-find_package(catkin REQUIRED COMPONENTS
-  geometry_msgs
-  message_generation
+set(msg_files
+  "msg/Statistics.msg"
+  "msg/ChannelInfo.msg"
 )
 
-add_message_files(
-  FILES
-  Statistics.msg
-  ChannelInfo.msg
+set(srv_files
+  "srv/CheckSafety.srv"
+  "srv/Initialize.srv"
 )
 
-## Generate services in the 'srv' folder
-add_service_files(
-  FILES
-  CheckSafety.srv
-  Initialize.srv
+rosidl_generate_interfaces(${PROJECT_NAME}
+  ${msg_files}
+  ${srv_files}
+  DEPENDENCIES geometry_msgs std_msgs  std_msgs # Added std_msgs
 )
 
+ament_export_dependencies(rosidl_default_runtime)
 
-## Generate added messages and services with any dependencies listed here
-generate_messages(
-  DEPENDENCIES
-  geometry_msgs
-)
+ament_package()
+
+# cmake_minimum_required(VERSION 3.8)
+# project(elevation_map_msgs)
+
+# if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+#   add_compile_options(-Wall -Wextra -Wpedantic)
+# endif()
+
+# # find dependencies
+# find_package(ament_cmake REQUIRED)
+# find_package(rosidl_default_generators REQUIRED)
+# find_package(builtin_interfaces REQUIRED)
+# find_package(std_msgs REQUIRED)
+# find_package(geometry_msgs REQUIRED)
+# find_package(action_msgs REQUIRED)
+
+# rosidl_generate_interfaces(${PROJECT_NAME}
+#   "msg/Statistics.msg"
+#   "msg/ChannelInfo.msg"
+#   "srv/CheckSafety.srv"
+#   "srv/Initialize.srv"
+#   DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs
+# )
+
+# ament_export_dependencies(rosidl_default_runtime)
+
+# ament_package()
 
-catkin_package(
-)

elevation_map_msgs/msg/ChannelInfo.msg

@@ -1,2 +1,2 @@
-Header header
+std_msgs/Header header
 string[] channels       # channel names for each layer

elevation_map_msgs/msg/Statistics.msg

@@ -1,2 +1,2 @@
- Header header
- float64 pointcloud_process_fps
+std_msgs/Header header
+float64 pointcloud_process_fps

elevation_map_msgs/package.xml

@@ -1,19 +1,30 @@
 <?xml version="1.0"?>
-<package>
+<package format="3">
     <name>elevation_map_msgs</name>
     <version>0.0.0</version>
     <description>ROS Message definitions for elevation mapping.</description>
     <maintainer email="[email protected]">Takahiro Miki</maintainer>
     <author email="[email protected]">Takahiro Miki</author>
     <license>MIT</license>
 
-    <buildtool_depend>catkin</buildtool_depend>
-
+    <!-- Build and Runtime Dependencies -->
+    <buildtool_depend>ament_cmake</buildtool_depend>
+
     <build_depend>geometry_msgs</build_depend>
-    <build_depend>message_generation</build_depend>
+    <build_depend>rosidl_default_generators</build_depend>
+    <build_depend>std_msgs</build_depend>
+    <member_of_group>rosidl_interface_packages</member_of_group>
+
+    <exec_depend>geometry_msgs</exec_depend>
+    <exec_depend>rosidl_default_runtime</exec_depend>
+    <exec_depend>std_msgs</exec_depend>
 
-    <run_depend>geometry_msgs</run_depend>
+    <!-- Testing Dependencies -->
+    <test_depend>ament_lint_auto</test_depend>
+    <test_depend>ament_lint_common</test_depend>
 
     <export>
+        <build_type>ament_cmake</build_type>
+        <member_of_group>rosidl_interface_packages</member_of_group>
     </export>
 </package>

elevation_mapping_cupy/CMakeLists.txt

@@ -1,80 +1,141 @@
-# %Tag(FULLTEXT)%
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.8)
 project(elevation_mapping_cupy)
 
+# # Enable C++11 (or higher if needed for ROS 2 and pybind11)
+# set(CMAKE_CXX_STANDARD 11)
+# set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# 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)
-find_package(PythonLibs 3 REQUIRED)
+find_package(pybind11 CONFIG REQUIRED)
+find_package(Eigen3 REQUIRED)
 find_package(OpenCV REQUIRED)
 
-if(PYTHONLIBS_FOUND)
-  message(STATUS "Using Python Libraries at: " ${PYTHON_LIBRARIES})
-  message(STATUS "Using Python include directories at: " ${PYTHON_INCLUDE_DIRS})
-else()
-  message(WARNING "Could not find Python Libraries")
-endif()
+# Find pybind11
 
-find_package(Eigen3 REQUIRED)
+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_package(catkin REQUIRED
-  roscpp
-  rospy
-  tf
-  tf_conversions
-  sensor_msgs
+# Find ROS 2 dependencies
+find_package(message_filters REQUIRED)
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(grid_map_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(elevation_map_msgs REQUIRED)
+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)
+
+
+_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
+  rclpy
   std_msgs
+  std_srvs
+  builtin_interfaces
   geometry_msgs
+  sensor_msgs
   elevation_map_msgs
   grid_map_msgs
   grid_map_ros
   image_transport
   pcl_ros
-  pybind11_catkin
-)
-
-catkin_package(
-  INCLUDE_DIRS
-    include
-    ${EIGEN3_INCLUDE_DIRS}
-  CATKIN_DEPENDS
-    roscpp
-    rospy
-    tf
-    tf_conversions
-    sensor_msgs
-    std_msgs
-    geometry_msgs
-    elevation_map_msgs
-    grid_map_ros
-    image_transport
-    pcl_ros
-    pybind11_catkin
+  message_filters
+  tf2_eigen
 )
 
+# Include directories
 include_directories(
-  include
+  include    
   ${PYTHON_INCLUDE_DIRS}
-  ${catkin_INCLUDE_DIRS}
-  ${EIGEN3_INCLUDE_DIRS}
+  ${Eigen3_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
+  ${pybind11_INCLUDE_DIRS}
 )
 
+# Declare C++ library
 add_library(elevation_mapping_ros
     src/elevation_mapping_wrapper.cpp
-    src/elevation_mapping_ros.cpp)
+    src/elevation_mapping_ros.cpp
+)
 
-target_link_libraries(elevation_mapping_ros ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+# Link the library with necessary dependencies
+target_link_libraries(elevation_mapping_ros ${PYTHON_LIBRARIES} ${OpenCV_LIBRARIES} pybind11::embed)
+ament_target_dependencies(elevation_mapping_ros ${dependencies})
 
+# Declare C++ executable
 add_executable(elevation_mapping_node src/elevation_mapping_node.cpp)
-target_link_libraries(elevation_mapping_node elevation_mapping_ros)
 
-catkin_python_setup()
+# Link the executable with the library and dependencies
+target_link_libraries(elevation_mapping_node elevation_mapping_ros ${OpenCV_LIBRARIES} pybind11::embed)
+ament_target_dependencies(elevation_mapping_node ${dependencies})
+
+
+
+# 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 
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 
 install(
-  TARGETS elevation_mapping_ros elevation_mapping_node
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+  TARGETS elevation_mapping_ros 
+  DESTINATION lib/${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+
+
+
+install(PROGRAMS  
+  DESTINATION lib/${PROJECT_NAME}
+)
 
+# Install launch and config directories
 install(
   DIRECTORY launch config
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+  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/build/lib/elevation_mapping_cupy/__init__.py

@@ -0,0 +1,4 @@
+# from .parameter import Parameter
+# from .elevation_mapping import ElevationMap
+# from .kernels import custom_image_kernels, custom_kernels, custom_semantic_kernels
+