diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json
index 8ddd64fe..85fda906 100644
--- a/.devcontainer/devcontainer.json
+++ b/.devcontainer/devcontainer.json
@@ -1,7 +1,9 @@
 {
-  "name": "vsgraphs_ros1",
-  "remoteUser": "${env.USERNAME}",
-  "service": "vsgraphs",
+  "name": "vsgraphs_ros_jazzy",
+  "remoteUser": "asier", // Once the container is running, attach to it as the user asier
+  //only works if that user actually exists inside the container.
+  // "remoteUser": "${env.USERNAME}",
+  "service": "vsgraphs_ros2",
   "customizations": {
     "vscode": {
       "extensions": [
@@ -10,12 +12,12 @@
         "ms-vscode.cpptools",
         "ms-vscode.cpptools-themes",
         "ms-vscode.cpptools-extension-pack",
-        "ms-iot.vscode-ros",
+        //     "ms-iot.vscode-ros",
         "ms-vscode.cmake-tools",
-        "esbenp.prettier-vscode",
-        "GitHub.copilot",
-        "eamodio.gitlens",
-        "PKief.material-icon-theme"
+        // "esbenp.prettier-vscode",
+        "GitHub.copilot"
+        // "eamodio.gitlens",
+        // "PKief.material-icon-theme"
       ],
       "settings": {
         "files.associations": {
@@ -27,6 +29,6 @@
       }
     }
   },
-  "workspaceFolder": "/workspace",
+  "workspaceFolder": "/home/asier/ros2_ws",
   "dockerComposeFile": "../docker/docker-compose.yml"
-}
+}
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f802147e..75cc4893 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8)
+cmake_minimum_required(VERSION 3.5)  # ros2 change
 project(orb_slam3_ros)
 
 IF(NOT CMAKE_BUILD_TYPE)
@@ -13,22 +13,30 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall   -O3")
 # set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native")
 # set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native")
 
-# Check C++14 or C++0x support
-include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-
-if(COMPILER_SUPPORTS_CXX14)
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
-  add_definitions(-DCOMPILEDWITHC11)
-  message(STATUS "Using flag -std=c++14.")
-elseif(COMPILER_SUPPORTS_CXX0X)
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-  add_definitions(-DCOMPILEDWITHC0X)
-  message(STATUS "Using flag -std=c++0x.")
-else()
-  message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
 endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# # Check C++14 or C++0x support
+# include(CheckCXXCompilerFlag)
+# CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
+# CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+
+# if(COMPILER_SUPPORTS_CXX14)
+#   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+#   add_definitions(-DCOMPILEDWITHC11)
+#   message(STATUS "Using flag -std=c++14.")
+# elseif(COMPILER_SUPPORTS_CXX0X)
+#   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+#   add_definitions(-DCOMPILEDWITHC0X)
+#   message(STATUS "Using flag -std=c++0x.")
+# else()
+#   message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
+# endif()
+
 
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
 
@@ -40,57 +48,51 @@ endif()
 MESSAGE("OPENCV VERSION:")
 MESSAGE(${OpenCV_VERSION})
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-  cv_bridge
-  image_transport
-  geometry_msgs
-  sensor_msgs
-  nav_msgs
-  std_msgs
-  message_filters
-  roscpp
-  rospy
-  tf
-  tf2
-  message_generation
-  aruco_ros
-  aruco_msgs
-  backward_ros
-  rviz_visual_tools
-  pcl_ros
-  segmenter_ros
-)
+## Find ament macros and libraries
+## Use 'find_package' for ROS 2 packages instead of 'catkin'
+## Find ament and ROS2 packages
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(rosidl_default_runtime REQUIRED)
+# find_package(aruco_ros REQUIRED)  # Comment out - check if ROS2 equivalent exists
+# find_package(aruco_msgs REQUIRED)  # Comment out - check if ROS2 equivalent exists
+find_package(backward_ros REQUIRED)  # Comment out - check if ROS2 equivalent exists
+find_package(rviz_visual_tools REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(segmenter_ros REQUIRED)  # Comment out - check if ROS2 equivalent exists
+
 
 find_package(Eigen3 3.1.0 REQUIRED)
 find_package(Pangolin REQUIRED)
+# find_package(PCL REQUIRED)
+find_package(OpenGL REQUIRED)
+find_package(GLEW REQUIRED)
+
 find_package(PCL REQUIRED)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
 
-add_service_files(
-  FILES
-  SaveMap.srv
-)
 
-# Generate messages in the 'msg' folder
-add_message_files(
-  FILES
-  vSGraphs_WallData.msg
-  vSGraphs_RoomData.msg
-  vSGraphs_AllWallsData.msg
-  vSGraphs_AllDetectdetRooms.msg
-)
-
-generate_messages(
-  DEPENDENCIES
-  std_msgs
-  geometry_msgs
-)
-
-catkin_package (
-  CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs cv_bridge image_transport tf sensor_msgs dynamic_reconfigure message_runtime backward_ros rviz_visual_tools
-  LIBRARIES {PROJECT_NAME} libDBoW2
+# Generate services
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/SaveMap.srv"
+  "msg/VSGraphsWallData.msg"
+  "msg/VSGraphsRoomData.msg"
+  "msg/VSGraphsAllWallsData.msg"
+  "msg/VSGraphsAllDetectdetRooms.msg"
+  DEPENDENCIES std_msgs geometry_msgs sensor_msgs
 )
 
 ## Specify additional locations of header files
@@ -105,18 +107,24 @@ include_directories(
   ${PROJECT_SOURCE_DIR}/orb_slam3/Thirdparty/Sophus
   ${EIGEN3_INCLUDE_DIR}
   ${Pangolin_INCLUDE_DIRS}
-  ${catkin_INCLUDE_DIRS}
   ${PCL_INCLUDE_DIRS}
+  ${segmenter_ros_INCLUDE_DIRS}
 )
+include_directories(${OPENGL_INCLUDE_DIRS})
 
+# ROS2 uses ament_target_dependencies instead of catkin_INCLUDE_DIRS
 link_directories(${PCL_LIBRARY_DIRS})
 add_definitions(${PCL_DEFINITIONS})
 
+# Include third-party CMakeLists
 include(${PROJECT_SOURCE_DIR}/orb_slam3/Thirdparty/DBoW2/CMakeLists.txt)
 include(${PROJECT_SOURCE_DIR}/orb_slam3/Thirdparty/g2o/CMakeLists.txt)
 
+# Set library output directory
 set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/orb_slam3/lib)
-add_library(${PROJECT_NAME} SHARED
+
+# Create the main library
+add_library(${PROJECT_NAME}_lib SHARED
   orb_slam3/src/System.cc
   orb_slam3/src/Tracking.cc
   orb_slam3/src/LocalMapping.cc
@@ -201,7 +209,7 @@ add_library(${PROJECT_NAME} SHARED
   orb_slam3/include/SemanticsManager.h
 )
 
-target_link_libraries(${PROJECT_NAME}
+target_link_libraries(${PROJECT_NAME}_lib 
   ${OpenCV_LIBS}
   ${EIGEN3_LIBS}
   ${Pangolin_LIBRARIES}
@@ -213,62 +221,87 @@ target_link_libraries(${PROJECT_NAME}
   -lcrypto
 )
 
-## ROS node
-add_executable(ros_mono
-  src/ros_mono.cc
-  src/common.cc
-)
-target_link_libraries(ros_mono
-  ${PROJECT_NAME}
-  ${catkin_LIBRARIES}
+# ROS2 specific: Use ament_target_dependencies for ROS2 packages
+ament_target_dependencies(${PROJECT_NAME}_lib 
+  rclcpp
+  std_msgs
+  cv_bridge
+  sensor_msgs
+  geometry_msgs
+  tf2
+  tf2_ros
+  nav_msgs
+  message_filters
+  image_transport
+  pcl_ros
+  rviz_visual_tools
+  # segmenter_ros
 )
 
-## ROS node
-add_executable(ros_mono_inertial 
-  src/ros_mono_inertial.cc
-  src/common.cc
-)
-target_link_libraries(ros_mono_inertial 
-  ${PROJECT_NAME}
-  ${catkin_LIBRARIES}
-)
 
-## ROS node
-add_executable(ros_stereo 
-  src/ros_stereo.cc
+## ROS2 node
+add_executable(ros_rgbd
+  src/ros_rgbd.cc
   src/common.cc
 )
-target_link_libraries(ros_stereo
-  ${PROJECT_NAME}
-  ${catkin_LIBRARIES}
-)
-
-## ROS node
-add_executable(ros_stereo_inertial
-  src/ros_stereo_inertial.cc
-  src/common.cc
+target_link_libraries(ros_rgbd
+  ${PROJECT_NAME}_lib
 )
-target_link_libraries(ros_stereo_inertial 
-  ${PROJECT_NAME}
-  ${catkin_LIBRARIES}
+ament_target_dependencies(ros_rgbd
+  rclcpp
+  sensor_msgs
+  geometry_msgs
+  cv_bridge
+  std_msgs
+  nav_msgs
+  message_filters
+  tf2
+  tf2_ros
+  segmenter_ros
 )
 
-## ROS node
-add_executable(ros_rgbd
-  src/ros_rgbd.cc
-  src/common.cc
+## ROS2 node
+# add_executable(ros_rgbd_inertial
+#   src/ros_rgbd_inertial.cc
+#   src/common.cc
+# )
+
+# target_link_libraries(ros_rgbd_inertial
+#   ${PROJECT_NAME}_lib
+# )
+# ament_target_dependencies(ros_rgbd_inertial
+#   rclcpp
+#   sensor_msgs
+#   geometry_msgs
+#   cv_bridge
+#   std_msgs
+#   nav_msgs
+#   message_filters
+#   tf2
+#   tf2_ros
+#   segmenter_ros
+# )
+
+# Make sure executables can use the custom messages
+rosidl_target_interfaces(ros_rgbd ${PROJECT_NAME} "rosidl_typesupport_cpp")
+# rosidl_target_interfaces(ros_rgbd_inertial ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+install(
+  DIRECTORY launch config orb_slam3/Vocabulary
+  DESTINATION share/${PROJECT_NAME}
 )
-target_link_libraries(ros_rgbd
-  ${PROJECT_NAME}
-  ${catkin_LIBRARIES}
+
+# Install the library
+install(TARGETS ${PROJECT_NAME}_lib
+  DESTINATION lib/${PROJECT_NAME}
 )
 
-## ROS node
-add_executable(ros_rgbd_inertial
-  src/ros_rgbd_inertial.cc
-  src/common.cc
+# Install executables
+install(TARGETS
+  ros_rgbd
+  # ros_rgbd_inertial
+  DESTINATION lib/${PROJECT_NAME}
 )
-target_link_libraries(ros_rgbd_inertial
-  ${PROJECT_NAME}
-  ${catkin_LIBRARIES}
-)
\ No newline at end of file
+
+# IMPORTANT: This must be the very last line in CMakeLists.txt
+ament_package()
\ No newline at end of file
diff --git a/config/Visualization/vsgraphs_rgbd2.rviz b/config/Visualization/vsgraphs_rgbd2.rviz
new file mode 100644
index 00000000..71ae1925
--- /dev/null
+++ b/config/Visualization/vsgraphs_rgbd2.rviz
@@ -0,0 +1,180 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 157
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+      Splitter Ratio: 0.5
+    Tree Height: 646
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Raw Frames
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /camera/color/image_raw
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Tracked Frames
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /RGBD/tracking_image
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Segmented Frames
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /camera/color/image_segment_vis
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 255; 255; 255
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 12.543999671936035
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.2303985208272934
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.7353980541229248
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000035efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002d2000000c7000000000000000000000001000001000000035efc0200000008fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb000000140052006100770020004600720061006d00650073010000003b000000ce0000001600fffffffb0000000a0049006d00610067006500000000e5000000a50000000000000000fb0000001c0054007200610063006b006500640020004600720061006d00650073010000010f000000cf0000001600fffffffb0000000a0049006d006100670065010000010d000000d40000000000000000fb00000020005300650067006d0065006e0074006500640020004600720061006d0065007301000001e4000000d40000001600fffffffb0000000a0056006900650077007301000002be000000db000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004d80000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Raw Frames:
+    collapsed: false
+  Segmented Frames:
+    collapsed: false
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Tracked Frames:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1850
+  X: 1990
+  Y: 27
diff --git a/config/Visualization/vsgraphs_rgbd_asier.rviz b/config/Visualization/vsgraphs_rgbd_asier.rviz
new file mode 100644
index 00000000..897dba6b
--- /dev/null
+++ b/config/Visualization/vsgraphs_rgbd_asier.rviz
@@ -0,0 +1,109 @@
+Panels:
+  - Class: rviz_common/Displays
+    Name: Displays
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Name: Tool Properties
+  - Class: rviz_common/Views
+    Name: Views
+  - Class: rviz_common/Time
+    Name: Time
+
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz_default_plugins/Grid
+      Name: Grid
+      Enabled: true
+      Plane: XY
+      Cell Size: 1
+      Plane Cell Count: 20
+      Reference Frame: map
+    - Class: rviz_default_plugins/TF
+      Name: TF
+      Enabled: true
+    - Class: rviz_default_plugins/Image
+      Name: Tracking img
+      Enabled: true
+      Image Topic: /orb_slam3/tracking_image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+    - Class: rviz_default_plugins/Image
+      Name: Segmented img
+      Enabled: true
+      Image Topic: /camera/color/image_segment_vis
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+    - Class: rviz_default_plugins/PointCloud2
+      Name: all points
+      Enabled: true
+      Topic: /orb_slam3/all_points
+      Style: Flat Squares
+      Size (m): 0.02
+      Use Fixed Frame: true
+      Use rainbow: true
+    - Class: rviz_default_plugins/MarkerArray
+      Name: keyframes
+      Enabled: true
+      Marker Topic: /orb_slam3/kf_markers
+      Queue Size: 100
+    - Class: rviz_default_plugins/Path
+      Name: trajectory
+      Enabled: true
+      Topic: /orb_slam3_ros/trajectory
+      Line Style: Billboards
+      Line Width: 0.01
+    - Class: rviz_default_plugins/MarkerArray
+      Name: markers
+      Enabled: true
+      Marker Topic: /orb_slam3/fiducial_markers
+      Queue Size: 100
+    - Class: rviz_default_plugins/MarkerArray
+      Name: doors
+      Enabled: true
+      Marker Topic: /orb_slam3/doors
+      Queue Size: 100
+    - Class: rviz_default_plugins/MarkerArray
+      Name: planes
+      Enabled: true
+      Marker Topic: /orb_slam3/planes
+      Queue Size: 100
+    - Class: rviz_default_plugins/MarkerArray
+      Name: rooms
+      Enabled: true
+      Marker Topic: /orb_slam3/rooms
+      Queue Size: 100
+    - Class: rviz_default_plugins/PointCloud2
+      Name: segmented planes
+      Enabled: true
+      Topic: /orb_slam3/plane_point_clouds
+      Style: Flat Squares
+      Size (m): 0.01
+      Use Fixed Frame: true
+      Use rainbow: true
+    - Class: rviz_default_plugins/MarkerArray
+      Name: Camera Pose
+      Enabled: true
+      Marker Topic: /orb_slam3/camera_pose_vis
+      Queue Size: 100
+
+  Global Options:
+    Background Color: 255; 255; 255
+    Fixed Frame: map
+    Frame Rate: 30
+
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Name: Current View
+      Distance: 10
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.5
+        Y: -2.2
+        Z: 2.0
+      Pitch: 0.54
+      Yaw: 1.47
\ No newline at end of file
diff --git a/doc/ROS.md b/doc/ROS.md
index 841361a4..0c540084 100644
--- a/doc/ROS.md
+++ b/doc/ROS.md
@@ -14,7 +14,7 @@ vS-Graphs interfaces with several ROS topics to process sensor data, publish sem
 | `/camera/color/image_segment`        | `segmenter_ros::SegmenterDataMsg`           | Semantic segmentation results                          |
 | `/camera/color/image_segment_vis`    | `segmenter_ros::VSGraphDataMsg`             | Visualized semantic segmentation frame                 |
 | `/voxblox_skeletonizer/sparse_graph` | `visualization_msgs::MarkerArray`           | Sparse graph representation of the free-space topology |
-| `/gnn_room_detector`                 | `orb_slam3_ros::vSGraphs_AllDetectdetRooms` | GNN-based room segmentation output                     |
+| `/gnn_room_detector`                 | `orb_slam3_ros::VSGraphsAllDetectdetRooms` | GNN-based room segmentation output                     |
 
 > 💡 To customize input topics and other parameters at launch time, please refer to the [⚙️ vS-Graphs Launch Arguments](../launch/README.md) page for a full list and descriptions.
 
@@ -37,6 +37,6 @@ vS-Graphs interfaces with several ROS topics to process sensor data, publish sem
 | `/orb_slam3/plane_point_clouds`     | `sensor_msgs/PointCloud2`             | Point clouds of detected building components              |
 | `/orb_slam3/segmented_point_clouds` | `sensor_msgs/PointCloud2`             | Point cloud after semantic segmentation                   |
 | `/orb_slam3/freespace_clusters`     | `sensor_msgs/PointCloud2`             | Clustered free-space regions used for structural analysis |
-| `/orb_slam3/all_mapped_walls`       | `orb_slam3_ros/vSGraphs_AllWallsData` | All wall segments identified (GNN-based room detection)   |
+| `/orb_slam3/all_mapped_walls`       | `orb_slam3_ros/VSGraphsAllWallsData` | All wall segments identified (GNN-based room detection)   |
 | `/orb_slam3/rooms`                  | `visualization_msgs/MarkerArray`      | Detected structural elements (rooms and corridors)        |
 | `/orb_slam3/camera_pose_vis`        | `visualization_msgs/MarkerArray`      | Visualization markers for current camera pose             |
diff --git a/docker/Jazzy.Dockerfile b/docker/Jazzy.Dockerfile
new file mode 100644
index 00000000..5737a170
--- /dev/null
+++ b/docker/Jazzy.Dockerfile
@@ -0,0 +1,267 @@
+# FROM nvidia/cuda:12.9.0-cudnn-devel-ubuntu24.04
+FROM nvcr.io/nvidia/cuda-dl-base:25.04-cuda12.9-devel-ubuntu24.04
+ARG DEBIAN_FRONTEND=noninteractive
+
+### FIX MPI ISSUE ###
+RUN mkdir -p /opt/hpcx/ompi/lib/x86_64-linux-gnu
+RUN ln -s /opt/hpcx/ompi /opt/hpcx/ompi/lib/x86_64-linux-gnu/openmpi
+RUN dpkg-reconfigure libc-bin
+
+# User and group setup
+ARG USERNAME=user
+ARG USER_UID=1000
+ARG USER_GID=$USER_UID
+
+# Deletes user if already in container
+RUN if id -u $USER_UID ; then userdel "$(id -un $USER_UID)" ; fi
+
+##### Environment variables #####
+ENV CUDA_HOME=/usr/local/cuda
+ENV LANG=en_US.UTF-8
+ENV LC_ALL=en_US.UTF-8
+ENV ROS_DISTRO=jazzy
+
+##### Essential packages #####
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    python3-pip \
+    python-is-python3 \
+    git \
+    openssh-client \
+    wget \
+    vim \
+    curl \
+    libeigen3-dev \
+    build-essential \
+    locales \
+    software-properties-common \
+    lsb-release \
+    gnupg2 && \
+    locale-gen en_US en_US.UTF-8 && \
+    update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
+
+##### ROS 2 Jazzy setup #####
+
+# Add universe repository
+RUN add-apt-repository universe
+
+# Install ros-apt-source package
+RUN ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}') && \
+    curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo $VERSION_CODENAME)_all.deb" && \
+    apt install -y /tmp/ros2-apt-source.deb
+
+# Install dev tools and ROS
+RUN apt update && apt upgrade -y && \
+    apt install -y ros-dev-tools ros-${ROS_DISTRO}-desktop
+
+# Source ROS setup globally
+RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /etc/bash.bashrc
+
+# rosdep initialization (should run as root before switching user)
+RUN rosdep init && rosdep update
+
+##### Clean up to reduce image size
+RUN rm -rf /var/lib/apt/lists/* /tmp/*
+
+# Create new user
+RUN groupadd --gid $USER_GID $USERNAME \
+    && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \
+    && echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers
+
+# ##### Python environment setup #####
+# # PyTorch and related libraries - networkx needs to be installed first because of version issue
+# # RUN pip3 install networkx==3.1
+# # RUN pip3 install torch==2.0.1+cu118 -f  https://download.pytorch.org/whl/torch_stable.html
+# # RUN pip3 install torchvision==0.15.2+cu118 --extra-index-url https://download.pytorch.org/whl/cu118
+
+# RUN pip3 install --break-system-packages networkx==3.1
+# RUN pip3 install --break-system-packages torch==2.3.1+cu121 -f https://download.pytorch.org/whl/torch_stable.html
+# # RUN pip3 install --break-system-packages torchvision==0.16.1+cu121 --extra-index-url https://download.pytorch.org/whl/cu121
+# RUN pip3 install --break-system-packages torchvision==0.18.1+cu121 --extra-index-url https://download.pytorch.org/whl/cu121
+
+# # Replace your current PyTorch and torchvision install lines in your Dockerfile with the following: 
+# # RUN pip3 install --break-system-packages torch==1.10.0+cu113 torchvision==0.11.1+cu113 -f https://download.pytorch.org/whl/torch_stable.html
+
+
+# # detectron and CLIP
+# # Compute Capability 7.5 for T600 (SnT laptop) and 7.0 for V100 (HPC - Iris)
+# ARG TORCH_CUDA_ARCH_LIST="7.5;7.0+PTX"
+# ENV FORCE_CUDA="1"
+# RUN pip3 install --break-system-packages 'git+https://github.com/facebookresearch/detectron2.git'
+# RUN pip3 install --break-system-packages 'git+https://github.com/openai/CLIP.git'
+ENV PIP_BREAK_SYSTEM_PACKAGES=1
+
+RUN pip3 install networkx==3.1
+# RUN pip3 install torch==2.0.1+cu118 -f  https://download.pytorch.org/whl/torch_stable.html
+# RUN pip3 install torchvision==0.15.2+cu118 --extra-index-url https://download.pytorch.org/whl/cu118
+
+RUN pip3 install --extra-index-url https://download.pytorch.org/whl/cu121 \
+    torch \
+    torchvision
+
+# detectron and CLIP
+# Compute Capability 7.5 for T600 (SnT laptop) and 7.0 for V100 (HPC - Iris)
+ARG TORCH_CUDA_ARCH_LIST="7.5;7.0+PTX"
+ENV FORCE_CUDA="1"
+RUN pip3 install 'git+https://github.com/facebookresearch/detectron2.git'
+RUN pip3 install 'git+https://github.com/openai/CLIP.git'
+
+
+##### SSH keys for GitHub #####
+
+# Define the SSH keys as build arguments for latter mounting
+RUN mkdir -p -m 0600 ~/.ssh && ssh-keyscan github.com >> ~/.ssh/known_hosts
+
+##### Clone repositories #####
+# Pangolin
+# RUN apt-get install libepoxy-dev -y
+# RUN apt-get update && apt-get install -y libepoxy-dev
+# WORKDIR /opt/
+# # RUN git clone --branch v0.8 --depth 1 https://github.com/stevenlovegrove/Pangolin.git && \
+# RUN git clone https://github.com/stevenlovegrove/Pangolin.git && \
+#     cd Pangolin && \
+#     mkdir build && cd build && \
+#     cmake .. && \
+#     make -j && \
+#     make install
+
+# Pangolin
+# RUN apt-get update && apt-get install libepoxy-dev -y
+# WORKDIR /opt/
+# RUN git clone --branch v0.8 --depth 1 https://github.com/stevenlovegrove/Pangolin.git && \
+#     cd Pangolin && \
+#     mkdir build && cd build && \
+#     cmake .. && \
+#     make -j && \
+#     make install
+
+# Pangolin - Use a more recent version that works with modern compilers
+RUN apt-get update && apt-get install -y \
+    libepoxy-dev \
+    libgl1-mesa-dev \
+    libglu1-mesa-dev \
+    freeglut3-dev \
+    libglew-dev \
+    cmake \
+    build-essential \
+    git
+
+WORKDIR /opt/
+RUN git clone --branch v0.9.1 --depth 1 https://github.com/stevenlovegrove/Pangolin.git && \
+    cd Pangolin && \
+    mkdir build && cd build && \
+    cmake .. && \
+    make -j && \
+    make install
+
+# Cmake
+ARG version=3.22
+ARG build=1
+WORKDIR /tmp
+RUN wget https://cmake.org/files/v$version/cmake-$version.$build.tar.gz
+
+RUN tar -xzvf cmake-$version.$build.tar.gz
+WORKDIR /tmp/cmake-$version.$build
+RUN ./bootstrap
+RUN make -j8
+RUN make install
+
+# # ROS packages: visual sgraphs, semantic segmenter, aruco ros
+RUN mkdir -p /workspace/src
+WORKDIR /workspace/src/
+
+# # Mount the SSH keys for cloning private repositories
+# RUN --mount=type=ssh git clone git@github.com:snt-arg/visual_sgraphs.git
+# RUN --mount=type=ssh git clone git@github.com:snt-arg/scene_segment_ros.git
+# RUN --mount=type=ssh git clone git@github.com:snt-arg/scene_segment_ros2.git
+# RUN --mount=type=ssh git clone -b noetic-devel git@github.com:pal-robotics/aruco_ros.git
+# RUN --mount=type=ssh git clone -b humble-devel git@github.com:pal-robotics/aruco_ros.git
+
+# # other libraries
+WORKDIR /workspace/src/visual_sgraphs/docker
+# RUN pip3 install -r requirements.txt
+# WORKDIR /workspace/src/
+
+RUN pip3 install --break-system-packages --ignore-installed typing_extensions \
+    networkx==3.1 \
+    ultralytics==8.0.120 \
+    matplotlib>=3.2.2 \
+    opencv-python==4.6.0.66 \
+    Pillow>=7.1.2 \
+    PyYAML>=5.3.1 \
+    requests>=2.23.0 \
+    scipy>=1.4.1 \
+    tqdm>=4.64.0 \
+    pandas>=1.1.4 \
+    seaborn>=0.11.0 \
+    gradio==4.11.0 \
+    wandb \
+    transformers \
+    ftfy \
+    regex \
+    timm \
+    evo
+WORKDIR /workspace/src/
+
+# # for ROS package: mav_voxblox_planning
+# RUN --mount=type=ssh git clone git@github.com:snt-arg/mav_voxblox_planning.git
+# RUN --mount=type=ssh wstool init . ./mav_voxblox_planning/install/install_ssh.rosinstall
+# RUN --mount=type=ssh wstool update
+
+# # download the yoso checkpoint
+# RUN wget https://github.com/hujiecpp/YOSO/releases/download/v0.1/yoso_res50_coco.pth
+# RUN mv yoso_res50_coco.pth /workspace/src/scene_segment_ros/include/
+
+# # build the workspace
+WORKDIR /workspace/
+# RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && catkin build -j12 -DCMAKE_BUILD_TYPE=Release && rosclean purge -y"
+RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release && rosdep update"
+# RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --no-warn-unused-cli && rosdep update"
+
+
+##### Miscalleanous #####
+RUN ldconfig
+# RUN echo 'export PS1="[\u@\h \W] 🐳 "' >> /home/asier/.bashrc
+RUN echo 'export PS1="[\u@\h \W] 🐳 "' >> /home/$USERNAME/.bashrc
+
+#### For cmakelist of vsual_sgraphs #####
+ENV DEBIAN_FRONTEND=noninteractive
+RUN apt-get update && apt-get install -y ros-jazzy-rviz-visual-tools
+# RUN sudo apt update && sudo apt install ros-jazzy-rviz-visual-tools
+RUN sudo apt install ros-jazzy-pcl-ros
+
+### New installations here:
+RUN apt-get update && \
+    apt-get install -y ros-jazzy-depth-image-proc ros-jazzy-backward-ros
+# colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
+#  ros2 launch orb_slam3_ros vs_graphs.launch.py 
+# sudo apt install ros-jazzy-depth-image-proc
+# sudo apt install ros-jazzy-backward-ros
+# export MAKEFLAGS="-j 12
+
+
+
+##### Clean up #####
+# remove the apt list files
+RUN rm -rf /var/lib/apt/lists/*
+
+# remove packages no longer needed
+RUN apt-get clean && apt-get autoremove -y
+
+# remove the ssh keys
+RUN rm -rf /root/.ssh/
+
+##### Build entrypoint #####
+RUN echo "#!/bin/bash" >> /entrypoint.sh \
+    && echo "echo \"source /opt/ros/$ROS_DISTRO/setup.bash\" >> ~/.bashrc" >> /entrypoint.sh \
+    && echo "echo \"source /home/$USERNAME/ros2_ws/install/setup.bash\" >> ~/.bashrc" >> /entrypoint.sh \
+    && echo 'exec "$@"' >> /entrypoint.sh \
+    && chmod a+x /entrypoint.sh
+
+WORKDIR /workspace/
+
+ENTRYPOINT ["/entrypoint.sh"]
+
+USER $USERNAME
+CMD ["/bin/bash"]
+SHELL ["/bin/bash"]
+
diff --git a/docker/Noetic.Dockerfile b/docker/Noetic.Dockerfile
deleted file mode 100644
index 9bdf196c..00000000
--- a/docker/Noetic.Dockerfile
+++ /dev/null
@@ -1,153 +0,0 @@
-FROM nvidia/cuda:11.8.0-cudnn8-devel-ubuntu20.04
-ARG DEBIAN_FRONTEND=noninteractive
-
-# User and group setup
-ARG USERNAME=user
-ARG USER_UID=1000
-ARG USER_GID=$USER_UID
-
-# Deletes user if already in container
-RUN if id -u $USER_UID ; then userdel `id -un $USER_UID` ; fi
-
-##### Environment variables #####
-ENV CUDA_HOME=/usr/local/cuda
- 
-##### Essential packages #####
-RUN apt-get update && apt-get install -y \
-    python3-pip \
-    python-is-python3 \
-    git \
-    openssh-client \
-    wget \
-    vim \
-    curl \
-    libeigen3-dev \
-    build-essential
- 
-##### Install ROS 1 - Noetic #####
-# setup environment
-ENV ROS_DISTRO=noetic
- 
-# setup keys
-RUN echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list
-RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add -
- 
-# install ros packages
-RUN apt-get update && apt-get install -y \
-    ros-noetic-desktop-full
- 
-# ROS related packages
-RUN apt-get install -y \
-    python3-rosdep \
-    python3-rosinstall \
-    python3-rosinstall-generator \
-    python3-wstool \
-    python3-catkin-tools \
-    ros-noetic-pcl-ros \
-    ros-noetic-backward-ros \
-    ros-noetic-rviz-visual-tools \
-    ros-noetic-hector-trajectory-server
- 
-# ROS dependencies
-RUN rosdep init
-RUN rosdep update
-
-# Create new user
-RUN groupadd --gid $USER_GID $USERNAME \
-    && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \
-    && echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers
- 
-##### Python environment setup #####
-# PyTorch and related libraries - networkx needs to be installed first because of version issue
-RUN pip3 install networkx==3.1
-RUN pip3 install torch==2.0.1+cu118 -f  https://download.pytorch.org/whl/torch_stable.html
-RUN pip3 install torchvision==0.15.2+cu118 --extra-index-url https://download.pytorch.org/whl/cu118
- 
-# detectron and CLIP
-# Compute Capability 7.5 for T600 (SnT laptop) and 7.0 for V100 (HPC - Iris)
-ARG TORCH_CUDA_ARCH_LIST="7.5;7.0+PTX"
-ENV FORCE_CUDA="1"
-RUN pip3 install 'git+https://github.com/facebookresearch/detectron2.git'
-RUN pip3 install 'git+https://github.com/openai/CLIP.git'
- 
-##### SSH keys for GitHub #####
- 
-# Define the SSH keys as build arguments for latter mounting
-RUN mkdir -p -m 0600 ~/.ssh && ssh-keyscan github.com >> ~/.ssh/known_hosts
- 
-##### Clone repositories #####
-# Pangolin
-RUN apt-get install libepoxy-dev -y
-WORKDIR /opt/
-RUN git clone --branch v0.8 --depth 1 https://github.com/stevenlovegrove/Pangolin.git && \
-    cd Pangolin && \
-    mkdir build && cd build && \
-    cmake .. && \
-    make -j && \
-    make install
- 
-# Cmake
-ARG version=3.22
-ARG build=1
-WORKDIR /tmp
-RUN wget https://cmake.org/files/v$version/cmake-$version.$build.tar.gz
-RUN tar -xzvf cmake-$version.$build.tar.gz
-WORKDIR /tmp/cmake-$version.$build
-RUN ./bootstrap
-RUN make -j8
-RUN make install
- 
-# ROS packages: visual sgraphs, semantic segmenter, aruco ros
-RUN mkdir -p /workspace/src
-WORKDIR /workspace/src/
- 
-# Mount the SSH keys for cloning private repositories
-RUN --mount=type=ssh git clone git@github.com:snt-arg/visual_sgraphs.git
-RUN --mount=type=ssh git clone git@github.com:snt-arg/scene_segment_ros.git
-RUN --mount=type=ssh git clone -b noetic-devel git@github.com:pal-robotics/aruco_ros.git
-
-# other libraries
-WORKDIR /workspace/src/visual_sgraphs/docker
-RUN pip3 install -r requirements.txt
-WORKDIR /workspace/src/
- 
-# for ROS package: mav_voxblox_planning
-RUN --mount=type=ssh git clone git@github.com:snt-arg/mav_voxblox_planning.git
-RUN --mount=type=ssh wstool init . ./mav_voxblox_planning/install/install_ssh.rosinstall
-RUN --mount=type=ssh wstool update
- 
-# download the yoso checkpoint
-RUN wget https://github.com/hujiecpp/YOSO/releases/download/v0.1/yoso_res50_coco.pth
-RUN mv yoso_res50_coco.pth /workspace/src/scene_segment_ros/include/
- 
-# build the workspace
-WORKDIR /workspace/
-RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && catkin build -j12 -DCMAKE_BUILD_TYPE=Release && rosclean purge -y"
- 
-##### Miscalleanous #####
-RUN ldconfig
- 
-##### Clean up #####
-# remove the apt list files
-RUN rm -rf /var/lib/apt/lists/*
- 
-# remove packages no longer needed
-RUN apt-get clean && apt-get autoremove -y
- 
-# remove the ssh keys
-RUN rm -rf /root/.ssh/
- 
-##### Build entrypoint #####
-RUN echo "#!/bin/bash" >> /entrypoint.sh \
-    && echo "echo \"source /opt/ros/$ROS_DISTRO/setup.bash\" >> ~/.bashrc" >> /entrypoint.sh \
-    && echo "echo \"source /workspace/devel/setup.bash\" >> ~/.bashrc" >> /entrypoint.sh \
-    && echo 'exec "$@"' >> /entrypoint.sh \
-    && chmod a+x /entrypoint.sh
- 
-WORKDIR /workspace/
- 
-ENTRYPOINT ["/entrypoint.sh"]
-
-USER $USERNAME
-CMD ["/bin/bash"]
-SHELL ["/bin/bash"]
\ No newline at end of file
diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml
index 7327027a..7b8f2a61 100644
--- a/docker/docker-compose.yml
+++ b/docker/docker-compose.yml
@@ -1,16 +1,16 @@
 services:
-  vsgraphs:
-    image: vsgraphs
+  vsgraphs_ros2:
+    image: vsgraphs_ros2
     build:
       context: .
-      dockerfile: Noetic.Dockerfile
+      dockerfile: Jazzy.Dockerfile
       ssh:
         - default
       args:
         USERNAME: $USERNAME
         USER_UID: 1000
         USER_GID: 1000
-    container_name: vsgraphs
+    container_name: vsgraphs_ros2
     environment:
       DISPLAY: $DISPLAY
       XAUTHORITY: $XAUTHORITY
@@ -36,21 +36,31 @@ services:
       - ~/.gitconfig:/root/.gitconfig_local
       # Dev
       - /dev:/dev:rw
+      # persistent bash history and config from local to container
+      # - ~/.bash_history:/root/.bash_history
+      # - "/home/asier/.bash_history:/root/.bash_history"
+      # - container_home_cache:/root/.cache  # Optional: persist cache
+
       # Working Directories (** MODIFY **)
-      - "[visual_sgraphs-path]:/workspace/src/visual_sgraphs:rw"
+      - "/home/asier/mydockers/ubuntu24/ros_ws:/home/asier/ros2_ws"
       # Data Directories (** MODIFY **)
-      - "[dataset-path]:/root/datasets"
+      - "/home/asier/Documents/datasets:/home/asier/datasets"
+
     network_mode: "host"
-    command:
-      tail -f /dev/null
-      # Used to set correct U/G(ID) for shared folder
-    depends_on:
-      - volume-init
-  volume-init:
-    image: busybox
-    command: ["chown", "-R", "1000:1000", "/workspace/src/visual_sgraphs"]
-    volumes:
-      # Working Directories (** MODIFY **)
-      - "[visual_sgraphs-path]:/workspace/src/visual_sgraphs:rw"
-    entrypoint: ""
-    restart: "no"
+    command: tail -f /dev/null # Used to set correct U/G(ID) for shared folder
+  #   depends_on:
+  #     - volume-init
+  # # volume-init:
+  #   image: busybox
+  #   command: ["chown", "-R", "1000:1000", "/home/asier/ros2_ws"]
+  #   volumes:
+  #     # Working Directories (** MODIFY **)
+  #     - "/home/asier/mydockers/ubuntu24/ros_ws:/home/asier/ros2_ws"
+  #   entrypoint: ""
+  #   restart: "no"
+  # volumes:
+  # bash_history:
+  #   driver: local
+  # container_home_cache:
+  #   driver: local
+ 
diff --git a/evaluation/generate_kf_pose_txt_files.py b/evaluation/generate_kf_pose_txt_files.py
index dbbb7bf8..bf242c5a 100644
--- a/evaluation/generate_kf_pose_txt_files.py
+++ b/evaluation/generate_kf_pose_txt_files.py
@@ -1,19 +1,19 @@
 #!/usr/bin/env python3
 
-'''
- * This file is part of Visual S-Graphs (vS-Graphs).
- * Copyright (C) 2023-2025 SnT, University of Luxembourg
- *
- * 📝 Authors: Ali Tourani, Saad Ejaz, Hriday Bavle, Jose Luis Sanchez-Lopez, and Holger Voos
- *
- * vS-Graphs is free software: you can redistribute it and/or modify it under the terms
- * of the GNU General Public License as published by the Free Software Foundation, either
- * version 3 of the License, or (at your option) any later version.
- *
- * This software is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
- * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details: https://www.gnu.org/licenses/
-'''
+"""
+* This file is part of Visual S-Graphs (vS-Graphs).
+* Copyright (C) 2023-2025 SnT, University of Luxembourg
+*
+* 📝 Authors: Ali Tourani, Saad Ejaz, Hriday Bavle, Jose Luis Sanchez-Lopez, and Holger Voos
+*
+* vS-Graphs is free software: you can redistribute it and/or modify it under the terms
+* of the GNU General Public License as published by the Free Software Foundation, either
+* version 3 of the License, or (at your option) any later version.
+*
+* This software is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
+* without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+* See the GNU General Public License for more details: https://www.gnu.org/licenses/
+"""
 
 import sys
 import rospy
@@ -37,6 +37,7 @@
 print("Creating txt file for adding SLAM poses ...")
 slam_pose_file_path = f"{files_path}/slam_pose_{slam_method}_{dataset_seq}.txt"
 
+
 def write_pose_file(file_path, poses):
     with open(file_path, "w") as pose_file:
         pose_file.write("#timestamp tx ty tz qx qy qz qw\n")
@@ -65,4 +66,4 @@ def subscribers():
 
 
 if __name__ == "__main__":
-    subscribers()
\ No newline at end of file
+    subscribers()
diff --git a/evaluation/generate_pose_txt_files.py b/evaluation/generate_pose_txt_files.py
index fa2e94cf..74fe08f6 100644
--- a/evaluation/generate_pose_txt_files.py
+++ b/evaluation/generate_pose_txt_files.py
@@ -1,19 +1,19 @@
 #!/usr/bin/env python3
 
-'''
- * This file is part of Visual S-Graphs (vS-Graphs).
- * Copyright (C) 2023-2025 SnT, University of Luxembourg
- *
- * 📝 Authors: Ali Tourani, Saad Ejaz, Hriday Bavle, Jose Luis Sanchez-Lopez, and Holger Voos
- *
- * vS-Graphs is free software: you can redistribute it and/or modify it under the terms
- * of the GNU General Public License as published by the Free Software Foundation, either
- * version 3 of the License, or (at your option) any later version.
- *
- * This software is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
- * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details: https://www.gnu.org/licenses/
-'''
+"""
+* This file is part of Visual S-Graphs (vS-Graphs).
+* Copyright (C) 2023-2025 SnT, University of Luxembourg
+*
+* 📝 Authors: Ali Tourani, Saad Ejaz, Hriday Bavle, Jose Luis Sanchez-Lopez, and Holger Voos
+*
+* vS-Graphs is free software: you can redistribute it and/or modify it under the terms
+* of the GNU General Public License as published by the Free Software Foundation, either
+* version 3 of the License, or (at your option) any later version.
+*
+* This software is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
+* without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+* See the GNU General Public License for more details: https://www.gnu.org/licenses/
+"""
 
 import sys
 import rospy
@@ -37,12 +37,10 @@
 
 # Creating a txt file that will contain poses
 print("Creating txt files for adding SLAM and Ground-Truth poses ...")
-gt_pose_file = open(
-    f"{files_path}/gt_pose_{slam_method}_{dataset_seq}.txt", "w+")
+gt_pose_file = open(f"{files_path}/gt_pose_{slam_method}_{dataset_seq}.txt", "w+")
 gt_pose_file.write("#timestamp tx ty tz qx qy qz qw\n")
 gt_pose_file.flush()
-slam_pose_file = open(
-    f"{files_path}/slam_pose_{slam_method}_{dataset_seq}.txt", "w+")
+slam_pose_file = open(f"{files_path}/slam_pose_{slam_method}_{dataset_seq}.txt", "w+")
 slam_pose_file.write("#timestamp tx ty tz qx qy qz qw\n")
 slam_pose_file.flush()
 
@@ -59,9 +57,22 @@ def groundtruthPoseCallback(groundtruth_pose_msg):
     rw = groundtruth_pose_msg.pose.orientation.w
     # Write to the Ground-Truth file
     gt_pose_file.write(
-        str(time) + " " + str(tx) + " " + str(ty) + " "
-        + str(tz) + " " + str(rx) + " " + str(ry) + " "
-        + str(rz) + " " + str(rw) + "\n"
+        str(time)
+        + " "
+        + str(tx)
+        + " "
+        + str(ty)
+        + " "
+        + str(tz)
+        + " "
+        + str(rx)
+        + " "
+        + str(ry)
+        + " "
+        + str(rz)
+        + " "
+        + str(rw)
+        + "\n"
     )
     gt_pose_file.flush()
 
@@ -78,9 +89,22 @@ def slamPoseCallback(slam_pose_msg):
     odom_rw = slam_pose_msg.pose.orientation.w
     # Write to the SLAM file
     slam_pose_file.write(
-        str(time) + " " + str(odom_x) + " " + str(odom_y) + " "
-        + str(odom_z) + " " + str(odom_rx) + " " + str(odom_ry) + " "
-        + str(odom_rz) + " " + str(odom_rw) + "\n"
+        str(time)
+        + " "
+        + str(odom_x)
+        + " "
+        + str(odom_y)
+        + " "
+        + str(odom_z)
+        + " "
+        + str(odom_rx)
+        + " "
+        + str(odom_ry)
+        + " "
+        + str(odom_rz)
+        + " "
+        + str(odom_rw)
+        + "\n"
     )
     slam_pose_file.flush()
 
@@ -88,8 +112,7 @@ def slamPoseCallback(slam_pose_msg):
 def subscribers():
     rospy.init_node("text_file_generator", anonymous=True)
     # Subscriber to the ground-truth topic
-    rospy.Subscriber(gt_pose_topic,
-                     ModelStates, groundtruthPoseCallback)
+    rospy.Subscriber(gt_pose_topic, ModelStates, groundtruthPoseCallback)
     # Subscriber to the SLAM topic
     rospy.Subscriber(slam_pose_topic, PoseStamped, slamPoseCallback)
     rospy.spin()
diff --git a/include/common.h b/include/common.h
index da4c2475..34a9c775 100644
--- a/include/common.h
+++ b/include/common.h
@@ -30,52 +30,77 @@
 #include <mutex>
 #include <Eigen/Dense>
 
-#include <ros/ros.h>
-#include <ros/time.h>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/time.hpp>
 #include <opencv2/opencv.hpp>
-#include <cv_bridge/cv_bridge.h>
+#include <cv_bridge/cv_bridge.hpp>
 #include <opencv2/core/core.hpp>
 #include <opencv2/core/eigen.hpp>
-#include <tf/transform_broadcaster.h>
-#include <image_transport/image_transport.h>
-
-#include <std_msgs/Header.h>
-#include <std_msgs/UInt64.h>
-#include <sensor_msgs/Imu.h>
-#include <nav_msgs/Odometry.h>
-#include <pcl/common/common.h>
-#include <pcl/PCLPointCloud2.h>
-#include <pcl/common/distances.h>
-#include <pcl/filters/voxel_grid.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <visualization_msgs/Marker.h>
-#include <segmenter_ros/VSGraphDataMsg.h>
-#include <segmenter_ros/SegmenterDataMsg.h>
-#include <nav_msgs/Path.h>
-#include <visualization_msgs/MarkerArray.h>
+// #include <tf/transform_broadcaster.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <image_transport/image_transport.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <tf2_ros/transform_listener.h>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <tf2/transform_datatypes.h>
+#include <tf2/time.h>  // for tf2::durationFromSec
+
+#include <tf2_ros/buffer.h>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> // For doTransform
+// #include <tf2_geometry_msgs/tf2_geometry_msgs.h> // Make sure this include is present
+// #include <tf2/tf2.h>
+#include <functional>
+
+// #include <std_msgs/Header.hpp>
+#include <std_msgs/msg/header.hpp>
+#include <std_msgs/msg/u_int64.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+// #include <pcl_ros/common/common.hpp>
+// #include <pcl/PCLPointCloud2.hpp>
+// #include <pcl/common/distances.hpp>
+// #include <pcl/filters/voxel_grid.hpp>
+//Tranfororm previos pcl headers for ros2
+#include <pcl_ros/transforms.hpp>      // From your pcl_ros/ folder
+// #include <pcl_ros/point_cloud.hpp>    // From your pcl_ros/ folder (if needed)
 #include <pcl_conversions/pcl_conversions.h>
-#include <rviz_visual_tools/rviz_visual_tools.h>
+#include <pcl_ros/pcl_node.hpp>       // From your pcl_ros/ folder (if needed)
+
+// #include <sensor_msgs/PointCloud2.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+// #include <visualization_msgs/Marker.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include "sensor_msgs/msg/image.hpp"
+
+#include <segmenter_ros/msg/vs_graph_data_msg.hpp>
+#include <segmenter_ros/msg/segmenter_data_msg.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+#include <rviz_visual_tools/rviz_visual_tools.hpp>
 
-#include <message_filters/subscriber.h>
-#include <message_filters/time_synchronizer.h>
-#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/subscriber.hpp>
+#include <message_filters/time_synchronizer.hpp>
+#include <message_filters/sync_policies/approximate_time.hpp>
 
 // This file is created automatically, see here http://wiki.ros.org/ROS/Tutorials/CreatingMsgAndSrv#Creating_a_srv
-#include <orb_slam3_ros/SaveMap.h>
+#include <orb_slam3_ros/srv/save_map.hpp>
 
 // Transformation process
-#include <pcl_ros/transforms.h>
-#include <tf/transform_listener.h>
-#include <tf/transform_datatypes.h>
+#include <pcl_ros/transforms.hpp>
+#include <tf2/transform_datatypes.h>
 #include <tf2_ros/static_transform_broadcaster.h>
 
+
 // ORB-SLAM3-specific libraries
 #include "System.h"
 #include "ImuTypes.h"
 #include "Types/SystemParams.h"
 
 // ArUco-ROS library
-#include <aruco_msgs/MarkerArray.h>
+// #include <aruco_msgs/MarkerArray.h>
 
 // Semantics
 #include "Semantic/Door.h"
@@ -83,8 +108,10 @@
 #include "Semantic/Marker.h"
 
 // Custom Messages
-#include <orb_slam3_ros/vSGraphs_AllWallsData.h>
-#include <orb_slam3_ros/vSGraphs_AllDetectdetRooms.h>
+#include <orb_slam3_ros/msg/vs_graphs_all_walls_data.hpp>
+#include <orb_slam3_ros/msg/vs_graphs_wall_data.hpp>
+#include <orb_slam3_ros/msg/vs_graphs_room_data.hpp>
+#include <orb_slam3_ros/msg/vs_graphs_all_detectdet_rooms.hpp>
 
 using json = nlohmann::json;
 
@@ -106,38 +133,60 @@ extern std::vector<std::vector<Eigen::Vector3d>> skeletonClusterPoints;
 // List of GNN-based room candidates
 extern std::vector<ORB_SLAM3::Room *> gnnRoomCandidates;
 
-extern ros::Publisher pubKFImage;
-extern ros::Publisher pubAllWalls;
-extern ros::Time lastPlanePublishTime;
-extern image_transport::Publisher pubTrackingImage;
-extern ros::Publisher pubCameraPose, pubCameraPoseVis, pubOdometry, pubKeyFrameMarker;
-extern ros::Publisher pubTrackedMappoints, pubAllMappoints, pubSegmentedPointcloud;
-
-struct MapPointStruct
-{
+// extern ros::Publisher pubKFImage;
+// extern ros::Publisher pubAllWalls;
+// extern rclcpp::Time lastPlanePublishTime;
+// extern image_transport::Publisher pubTrackingImage;
+// extern ros::Publisher pubCameraPose, pubCameraPoseVis, pubOdometry, pubKeyFrameMarker;
+// extern ros::Publisher pubTrackedMappoints, pubAllMappoints, pubSegmentedPointcloud;
+extern rclcpp::Publisher<segmenter_ros::msg::VSGraphDataMsg>::SharedPtr pubKFImage;
+extern rclcpp::Publisher<orb_slam3_ros::msg::VSGraphsAllWallsData>::SharedPtr pubAllWalls;
+
+extern std::shared_ptr<image_transport::Publisher> pubTrackingImage;
+extern rclcpp::Time lastPlanePublishTime;
+
+extern rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pubCameraPose;
+extern rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pubCameraPoseVis;
+extern rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdometry;
+extern rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pubKeyFrameMarker;
+extern rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubTrackedMappoints;
+extern rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubAllMappoints;
+extern rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubSegmentedPointcloud;
+
+
+// struct MapPointStruct
+// {
+//     int clusterId;
+//     Eigen::Vector3f coordinates;
+
+//     MapPointStruct(Eigen::Vector3f coords) : coordinates(coords), clusterId(-1) {}
+// };
+class MapPointStruct {
     int clusterId;
     Eigen::Vector3f coordinates;
-
-    MapPointStruct(Eigen::Vector3f coords) : coordinates(coords), clusterId(-1) {}
+    MapPointStruct(Eigen::Vector3f coords) : clusterId(-1), coordinates(coords) {}
 };
 
-void setupServices(ros::NodeHandle &, std::string);
-void publishTopics(ros::Time, Eigen::Vector3f = Eigen::Vector3f::Zero());
-void setupPublishers(ros::NodeHandle &, image_transport::ImageTransport &, std::string);
-
-void publishTrackingImage(cv::Mat, ros::Time);
-void publishCameraPose(Sophus::SE3f, ros::Time);
-void publishRooms(std::vector<ORB_SLAM3::Room *>, ros::Time);
-void publishDoors(std::vector<ORB_SLAM3::Door *>, ros::Time);
-void publishPlanes(std::vector<ORB_SLAM3::Plane *>, ros::Time);
-void publishTFTransform(Sophus::SE3f, string, string, ros::Time);
-void publishAllPoints(std::vector<ORB_SLAM3::MapPoint *>, ros::Time);
-void publishTrackedPoints(std::vector<ORB_SLAM3::MapPoint *>, ros::Time);
-void publishFiducialMarkers(std::vector<ORB_SLAM3::Marker *>, ros::Time);
-void publishSegmentedCloud(std::vector<ORB_SLAM3::KeyFrame *>, ros::Time);
-void publishKeyFrameImages(std::vector<ORB_SLAM3::KeyFrame *>, ros::Time);
-void publishKeyFrameMarkers(std::vector<ORB_SLAM3::KeyFrame *>, ros::Time);
-void publishBodyOdometry(Sophus::SE3f, Eigen::Vector3f, Eigen::Vector3f, ros::Time);
+// void setupServices(ros::NodeHandle &, std::string);
+void setupServices(std::shared_ptr<rclcpp::Node>, const std::string&);
+void publishTopics(rclcpp::Time, Eigen::Vector3f = Eigen::Vector3f::Zero());
+// void setupPublishers(ros::NodeHandle &, image_transport::ImageTransport &, std::string);
+//void setupPublishers(std::shared_ptr<rclcpp::Node>, image_transport::ImageTransport&, const std::string&);
+void setupPublishers(std::shared_ptr<rclcpp::Node> node, std::shared_ptr<image_transport::ImageTransport> image_transport, const std::string &node_name);
+
+void publishTrackingImage(cv::Mat, rclcpp::Time);
+void publishCameraPose(Sophus::SE3f, rclcpp::Time);
+void publishRooms(std::vector<ORB_SLAM3::Room *>, rclcpp::Time);
+void publishDoors(std::vector<ORB_SLAM3::Door *>, rclcpp::Time);
+void publishPlanes(std::vector<ORB_SLAM3::Plane *>, rclcpp::Time);
+void publishTFTransform(Sophus::SE3f, string, string, rclcpp::Time);
+void publishAllPoints(std::vector<ORB_SLAM3::MapPoint *>, rclcpp::Time);
+void publishTrackedPoints(std::vector<ORB_SLAM3::MapPoint *>, rclcpp::Time);
+void publishFiducialMarkers(std::vector<ORB_SLAM3::Marker *>, rclcpp::Time);
+void publishSegmentedCloud(std::vector<ORB_SLAM3::KeyFrame *>, rclcpp::Time);
+void publishKeyFrameImages(std::vector<ORB_SLAM3::KeyFrame *>, rclcpp::Time);
+void publishKeyFrameMarkers(std::vector<ORB_SLAM3::KeyFrame *>, rclcpp::Time);
+void publishBodyOdometry(Sophus::SE3f, Eigen::Vector3f, Eigen::Vector3f, rclcpp::Time);
 
 /**
  * @brief Publishes all mapped walls to detect possible rooms.
@@ -145,13 +194,24 @@ void publishBodyOdometry(Sophus::SE3f, Eigen::Vector3f, Eigen::Vector3f, ros::Ti
  * @param walls The vector of mapped walls to be published.
  * @param time The timestamp for the message.
  */
-void publishAllMappedWalls(std::vector<ORB_SLAM3::Plane *>, ros::Time);
+void publishAllMappedWalls(std::vector<ORB_SLAM3::Plane *>, rclcpp::Time);
 
 void clearKFClsClouds(std::vector<ORB_SLAM3::KeyFrame *>);
 
-bool saveMapService(orb_slam3_ros::SaveMap::Request &, orb_slam3_ros::SaveMap::Response &);
-bool saveTrajectoryService(orb_slam3_ros::SaveMap::Request &, orb_slam3_ros::SaveMap::Response &);
-bool saveMapPointsAsPCDService(orb_slam3_ros::SaveMap::Request &, orb_slam3_ros::SaveMap::Response &);
+// bool saveMapService(orb_slam3_ros::SaveMap::Request &, orb_slam3_ros::SaveMap::Response &);
+// bool saveTrajectoryService(orb_slam3_ros::SaveMap::Request &, orb_slam3_ros::SaveMap::Response &);
+// bool saveMapPointsAsPCDService(orb_slam3_ros::SaveMap::Request &, orb_slam3_ros::SaveMap::Response &);
+void saveMapService(
+  std::shared_ptr<orb_slam3_ros::srv::SaveMap::Request> request,
+  std::shared_ptr<orb_slam3_ros::srv::SaveMap::Response> response);
+
+void saveTrajectoryService(
+  std::shared_ptr<orb_slam3_ros::srv::SaveMap::Request> request,
+  std::shared_ptr<orb_slam3_ros::srv::SaveMap::Response> response);
+
+void saveMapPointsAsPCDService(
+  std::shared_ptr<orb_slam3_ros::srv::SaveMap::Request> request,
+  std::shared_ptr<orb_slam3_ros::srv::SaveMap::Response> response);
 
 /**
  * @brief Converts a SE3f to a cv::Mat
@@ -165,7 +225,7 @@ cv::Mat SE3fToCvMat(Sophus::SE3f data);
  *
  * @param data The SE3f data to be converted
  */
-tf::Transform SE3fToTFTransform(Sophus::SE3f data);
+tf2::Transform SE3fToTFTransform(Sophus::SE3f data);
 
 /**
  * @brief Converts a vector of MapPoints to a PointCloud2 message
@@ -173,7 +233,8 @@ tf::Transform SE3fToTFTransform(Sophus::SE3f data);
  * @param mapPoints The vector of MapPoints to be converted
  * @param msgTime The timestamp for the PointCloud2 message
  */
-sensor_msgs::PointCloud2 mapPointToPointcloud(std::vector<ORB_SLAM3::MapPoint *> mapPoints, ros::Time msgTime);
+// sensor_msgs::PointCloud2 mapPointToPointcloud(std::vector<ORB_SLAM3::MapPoint *> mapPoints, rclcpp::Time msgTime);
+sensor_msgs::msg::PointCloud2 mapPointToPointcloud(std::vector<ORB_SLAM3::MapPoint *> mapPoints, rclcpp::Time msgTime);
 
 /**
  * @brief Publishes a static transformation (TF) between two coordinate frames and define a
@@ -183,7 +244,7 @@ sensor_msgs::PointCloud2 mapPointToPointcloud(std::vector<ORB_SLAM3::MapPoint *>
  * @param childFrameId The child frame ID for the static transformation
  * @param msgTime The timestamp for the transformation message
  */
-void publishStaticTFTransform(string parentFrameId, string childFrameId, ros::Time msgTime);
+void publishStaticTFTransform(string parentFrameId, string childFrameId, rclcpp::Time msgTime);
 
 /**
  * @brief Publishes the free space clusters obtained from `voxblox_skeleton` as a PointCloud2 message
@@ -191,13 +252,13 @@ void publishStaticTFTransform(string parentFrameId, string childFrameId, ros::Ti
  * @param skeletonClusterPoints The list of free space cluster points
  * @param msgTime The timestamp for the PointCloud2 message
  */
-void publishFreeSpaceClusters(std::vector<std::vector<Eigen::Vector3d>>, ros::Time);
+void publishFreeSpaceClusters(std::vector<std::vector<Eigen::Vector3d>>, rclcpp::Time);
 
 /**
  * @brief Adds the markers to the buffer to be processed
  * @param markerArray The array of markers received from `aruco_ros`
  */
-void addMarkersToBuffer(const aruco_msgs::MarkerArray &markerArray);
+// void addMarkersToBuffer(const aruco_msgs::MarkerArray &markerArray);
 
 /**
  * @brief Avoids adding duplicate markers to the buffer by checking the timestamp
@@ -209,10 +270,12 @@ std::pair<double, std::vector<ORB_SLAM3::Marker *>> findNearestMarker(double fra
  * @brief Gets skeleton voxels from `voxblox_skeleton` to be processed
  * @param skeletonArray The array of skeleton voxels received
  */
-void setVoxbloxSkeletonCluster(const visualization_msgs::MarkerArray &skeletonArray);
+// void setVoxbloxSkeletonCluster(const visualization_msgs::msg::MarkerArray &skeletonArray);
+void setVoxbloxSkeletonCluster(const visualization_msgs::msg::MarkerArray &skeletonArray);
 
 /**
  * @brief Gets the set of room candidates detected by the GNN-based room detection module
  * @param msgGNNRooms The message containing the detected room candidates
  */
-void setGNNBasedRoomCandidates(const orb_slam3_ros::vSGraphs_AllDetectdetRooms &msgGNNRooms);
\ No newline at end of file
+// void setGNNBasedRoomCandidates(const orb_slam3_ros::msg::VSGraphsAllDetectdetRooms &msgGNNRooms);
+void setGNNBasedRoomCandidates(const orb_slam3_ros::msg::VSGraphsAllDetectdetRooms &msgGNNRooms);
\ No newline at end of file
diff --git a/launch/vs_graphs.launch b/launch/vs_graphs.launch
deleted file mode 100644
index c2eb5225..00000000
--- a/launch/vs_graphs.launch
+++ /dev/null
@@ -1,72 +0,0 @@
-<launch>
-    <!-- Global arguments -->
-    <arg name="offline" default="true" />
-    <arg name="launch_rviz" default="true" />
-    <arg name="colored_pointcloud" default="true" />
-    <arg name="visualize_segmented_scene" default="true" />
-
-    <!-- Topics -->
-    <arg name="camera_frame" default="camera" />
-    <arg name="sensor_config" default="RealSense_D435i" />
-    <arg name="rgb_image_topic" default="/camera/color/image_raw" />
-    <arg name="rgb_camera_info_topic" default="/camera/color/camera_info" />
-    <arg name="depth_image_topic" default="/camera/aligned_depth_to_color/image_raw" />
-
-    <!-- Use simulation time if offline -->
-    <param name="use_sim_time" value="$(arg offline)" />
-
-    <!-- VS-Graphs Node -->
-    <node name="orb_slam3" pkg="orb_slam3_ros" cwd="node" type="ros_rgbd" output="screen">
-        <!-- Remaps -->
-        <remap from="/camera/rgb/image_raw" to="$(arg rgb_image_topic)"/>
-        <remap from="/camera/depth_registered/image_raw" to="$(arg depth_image_topic)"/>
-        <!-- Files -->
-        <param name="voc_file" type="string" value="$(find orb_slam3_ros)/orb_slam3/Vocabulary/ORBvoc.txt.bin"/>
-        <param name="settings_file" type="string" value="$(find orb_slam3_ros)/config/RGB-D/$(arg sensor_config).yaml"/>
-        <param name="sys_params_file" type="string" value="$(find orb_slam3_ros)/config/system_params.yaml"/>
-        <!-- Environment -->
-        <param name="static_transform" value="true" />
-        <param name="roll" type="double" value="0.0" />
-        <param name="yaw" type="double" value="1.5697" />
-        <param name="pitch" type="double" value="-1.5697" />
-        <!-- Reference Frames -->
-        <param name="frame_map" value="map" />
-        <param name="cam_frame_id" value="camera" />
-        <param name="world_frame_id" value="world" />
-        <!-- Visualization -->
-        <param name="enable_pangolin" value="false" />
-        <param name="publish_pointclouds" value="true" />
-        <param name="colored_pointcloud" value="$(arg colored_pointcloud)" />
-    </node>
-
-    <!-- Static Transforms -->
-    <node pkg="tf" type="static_transform_publisher" name="bc_to_se" args="0 -3 0 0 0 0 plane room 10" />
-    <node pkg="tf" type="static_transform_publisher" name="world_to_bc" args="0 -5 0 0 0 0 world plane 10" />
-    <node pkg="tf" type="static_transform_publisher" name="camera_to_camera_optical" args="0 0 0 0 0 0 camera camera_color_optical_frame 10" />
-
-    <!-- Trajectory Path Server -->
-    <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_orb_slam3" output="screen" ns="orb_slam3_ros" >
-        <param name="/target_frame_name" value="/map" />
-        <param name="/source_frame_name" value="/$(arg camera_frame)" />
-        <param name="/trajectory_update_rate" value="20.0" />
-        <param name="/trajectory_publish_rate" value="20.0" />
-    </node>
-    
-    <!-- RViz -->
-    <node if="$(arg launch_rviz)" name="rviz" pkg="rviz" type="rviz" args="-d $(find orb_slam3_ros)/config/Visualization/vsgraphs_rgbd.rviz" output="screen" />
-
-    <!-- Depth to Point Cloud -->
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" />
-    <node pkg="nodelet" type="nodelet" name="nodelet1" args="load depth_image_proc/point_cloud_xyzrgb nodelet_manager">
-        <remap from="rgb/camera_info" to="$(arg rgb_camera_info_topic)"/>
-        <remap from="rgb/image_rect_color" to="$(arg rgb_image_topic)"/>
-        <remap from="depth_registered/image_rect" to="$(arg depth_image_topic)"/>
-        <remap from="depth_registered/points" to="/camera/depth/points"/>
-    </node>
-
-    <!-- Semantic Scene Segmenter Node -->
-    <node name="segmenter_ros" pkg="segmenter_ros" type="segmenter_yoso.py" output="screen">
-        <rosparam command="load" file="$(find segmenter_ros)/config/cfg_yoso.yaml" />
-        <param name="visualize" value="$(arg visualize_segmented_scene)" />
-    </node>
-</launch>
diff --git a/launch/vs_graphs.launch.py b/launch/vs_graphs.launch.py
new file mode 100644
index 00000000..0231b395
--- /dev/null
+++ b/launch/vs_graphs.launch.py
@@ -0,0 +1,230 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.conditions import IfCondition
+from ament_index_python.packages import get_package_share_directory
+import os.path
+
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            # Global arguments
+            DeclareLaunchArgument("offline", default_value="true"),
+            DeclareLaunchArgument("launch_rviz", default_value="true"),
+            DeclareLaunchArgument("colored_pointcloud", default_value="true"),
+            DeclareLaunchArgument("visualize_segmented_scene", default_value="true"),
+            # Topics
+            DeclareLaunchArgument("camera_frame", default_value="camera"),
+            DeclareLaunchArgument("sensor_config", default_value="RealSense_D435i"),
+            DeclareLaunchArgument(
+                "rgb_image_topic", default_value="/camera/color/image_raw"
+            ),
+            DeclareLaunchArgument(
+                "rgb_camera_info_topic", default_value="/camera/color/camera_info"
+            ),
+            DeclareLaunchArgument(
+                "depth_image_topic",
+                default_value="/camera/aligned_depth_to_color/image_raw",
+            ),
+            # VS-Graphs Node
+            Node(
+                package="orb_slam3_ros",
+                executable="ros_rgbd",
+                # name="orb_slam3",
+                output="screen",
+                parameters=[
+                    {"use_sim_time": LaunchConfiguration("offline")},
+                    {
+                        "voc_file": LaunchConfiguration(
+                            "voc_file",
+                            default=[
+                                # LaunchConfiguration(
+                                #     "find_package_share", default="orb_slam3_ros"
+                                # ),
+                                get_package_share_directory("orb_slam3_ros"),
+                                "/Vocabulary/ORBvoc.txt.bin",
+                            ],
+                        )
+                    },
+                    {
+                        "settings_file": LaunchConfiguration(
+                            "settings_file",
+                            default=[
+                                # LaunchConfiguration(
+                                #     "find_package_share", default="orb_slam3_ros"
+                                # ),
+                                # "/home/asier/ros2_ws/install/orb_slam3_ros/share/orb_slam3_ros/",
+                                get_package_share_directory("orb_slam3_ros"),
+                                "/config/RGB-D/",
+                                LaunchConfiguration("sensor_config"),
+                                ".yaml",
+                            ],
+                        )
+                    },
+                    {
+                        "sys_params_file": LaunchConfiguration(
+                            "sys_params_file",
+                            default=[
+                                # LaunchConfiguration(
+                                #     "find_package_share", default="orb_slam3_ros"
+                                # ),
+                                get_package_share_directory("orb_slam3_ros"),
+                                "/config/system_params.yaml",
+                            ],
+                        )
+                    },
+                    {"static_transform": True},
+                    {"roll": 0.0},
+                    {"yaw": 1.5697},
+                    {"pitch": -1.5697},
+                    {"frame_map": "map"},
+                    {"cam_frame_id": "camera"},
+                    {"world_frame_id": "world"},
+                    {"enable_pangolin": False},
+                    {"publish_pointclouds": True},
+                    {"colored_pointcloud": LaunchConfiguration("colored_pointcloud")},
+                ],
+                remappings=[
+                    ("/camera/rgb/image_raw", LaunchConfiguration("rgb_image_topic")),
+                    (
+                        "/camera/depth_registered/image_raw",
+                        LaunchConfiguration("depth_image_topic"),
+                    ),
+                ],
+            ),
+            # Static Transforms (use ROS 2 static_transform_publisher)
+            Node(
+                package="tf2_ros",
+                executable="static_transform_publisher",
+                name="bc_to_se",
+                arguments=["0", "-3", "0", "0", "0", "0", "plane", "room"],
+            ),
+            Node(
+                package="tf2_ros",
+                executable="static_transform_publisher",
+                name="world_to_bc",
+                arguments=["0", "-5", "0", "0", "0", "0", "world", "plane"],
+            ),
+            Node(
+                package="tf2_ros",
+                executable="static_transform_publisher",
+                name="camera_to_camera_optical",
+                arguments=[
+                    "0",
+                    "0",
+                    "0",
+                    "0",
+                    "0",
+                    "0",
+                    "camera",
+                    "camera_color_optical_frame",
+                ],
+            ),
+            # # Trajectory Path Server (if available in ROS 2)
+            # Node(
+            #     package="hector_trajectory_server",
+            #     executable="hector_trajectory_server",
+            #     name="trajectory_server_orb_slam3",
+            #     namespace="orb_slam3_ros",
+            #     output="screen",
+            #     parameters=[
+            #         {"target_frame_name": "/map"},
+            #         {"source_frame_name": LaunchConfiguration("camera_frame")},
+            #         {"trajectory_update_rate": 20.0},
+            #         {"trajectory_publish_rate": 20.0},
+            #     ],
+            # ),
+            # RViz
+            Node(
+                condition=IfCondition(LaunchConfiguration("launch_rviz")),
+                package="rviz2",
+                executable="rviz2",
+                name="rviz",
+                arguments=[
+                    "-d",
+                    [
+                        # LaunchConfiguration(
+                        #     "find_package_share", default="orb_slam3_ros"
+                        # ),
+                        get_package_share_directory("orb_slam3_ros"),
+                        "/config/Visualization/vsgraphs_rgbd2.rviz",
+                        # get_package_share_directory("orb_slam3_ros"),
+                        # "/config/Visualization/",
+                        # LaunchConfiguration("vsgraphs_rgbd"),
+                        # ".rviz",
+                    ],
+                ],
+                output="screen",
+            ),
+            # # Depth to Point Cloud (nodelet is not used in ROS 2, use component or node directly)
+            # Node(
+            #     package="depth_image_proc",
+            #     executable="point_cloud_xyzrgb",
+            #     name="point_cloud_xyzrgb",
+            #     remappings=[
+            #         ("rgb/camera_info", LaunchConfiguration("rgb_camera_info_topic")),
+            #         ("rgb/image_rect_color", LaunchConfiguration("rgb_image_topic")),
+            #         (
+            #             "depth_registered/image_rect",
+            #             LaunchConfiguration("depth_image_topic"),
+            #         ),
+            #         ("depth_registered/points", "/camera/depth/points"),
+            #     ],
+            # ),
+            ComposableNodeContainer(
+                name="depth_image_proc_container",
+                package="rclcpp_components",
+                namespace="",
+                executable="component_container",
+                composable_node_descriptions=[
+                    ComposableNode(
+                        package="depth_image_proc",
+                        plugin="depth_image_proc::PointCloudXyzrgbNode",
+                        name="point_cloud_xyzrgb_node",
+                        remappings=[
+                            (
+                                "rgb/camera_info",
+                                LaunchConfiguration("rgb_camera_info_topic"),
+                            ),
+                            (
+                                "rgb/image_rect_color",
+                                LaunchConfiguration("rgb_image_topic"),
+                            ),
+                            (
+                                "depth_registered/image_rect",
+                                LaunchConfiguration("depth_image_topic"),
+                            ),
+                            ("points", "/camera/depth/points"),
+                        ],
+                    ),
+                ],
+            ),
+            # Semantic Scene Segmenter Node
+            Node(
+                package="segmenter_ros",
+                executable="segmenter_yoso.py",
+                name="segmenter_ros",
+                output="screen",
+                parameters=[
+                    {"visualize": LaunchConfiguration("visualize_segmented_scene")}
+                ],
+                arguments=[
+                    "--ros-args",
+                    "--params-file",
+                    [
+                        # LaunchConfiguration(
+                        #     "find_package_share", default="segmenter_ros"
+                        # ),
+                        get_package_share_directory("segmenter_ros"),
+                        "/config/cfg_yoso.yaml",
+                    ],
+                ],
+            ),
+        ]
+    )
diff --git a/msg/VSGraphsAllDetectdetRooms.msg b/msg/VSGraphsAllDetectdetRooms.msg
new file mode 100644
index 00000000..e993e6b6
--- /dev/null
+++ b/msg/VSGraphsAllDetectdetRooms.msg
@@ -0,0 +1,4 @@
+std_msgs/Header header
+
+# Set of vSGraphs_RoomData.msg
+VSGraphsRoomData[] rooms
\ No newline at end of file
diff --git a/msg/VSGraphsAllWallsData.msg b/msg/VSGraphsAllWallsData.msg
new file mode 100644
index 00000000..37e5a2db
--- /dev/null
+++ b/msg/VSGraphsAllWallsData.msg
@@ -0,0 +1,7 @@
+# std_msgs/Header header
+# # Set of vSGraphs_WallData.msg
+# vSGraphs_WallData[] walls
+
+std_msgs/Header header
+# Set of wall data messages
+VSGraphsWallData[] walls
\ No newline at end of file
diff --git a/msg/VSGraphsRoomData.msg b/msg/VSGraphsRoomData.msg
new file mode 100644
index 00000000..7a172e83
--- /dev/null
+++ b/msg/VSGraphsRoomData.msg
@@ -0,0 +1,5 @@
+# Header header
+std_msgs/Header header
+int32 id
+int32[] wall_ids
+geometry_msgs/Vector3 centroid
diff --git a/msg/VSGraphsWallData.msg b/msg/VSGraphsWallData.msg
new file mode 100644
index 00000000..0fb8274c
--- /dev/null
+++ b/msg/VSGraphsWallData.msg
@@ -0,0 +1,12 @@
+# Header header
+
+# int32 id
+# float32 length
+# geometry_msgs/Vector3 normal
+# geometry_msgs/Vector3 centroid
+
+std_msgs/Header header
+int32 id
+float32 length
+geometry_msgs/Vector3 normal
+geometry_msgs/Vector3 centroid
\ No newline at end of file
diff --git a/msg/vSGraphs_AllDetectdetRooms.msg b/msg/vSGraphs_AllDetectdetRooms.msg
deleted file mode 100644
index d31a1571..00000000
--- a/msg/vSGraphs_AllDetectdetRooms.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-Header header
-
-# Set of vSGraphs_RoomData.msg
-vSGraphs_RoomData[] rooms
\ No newline at end of file
diff --git a/msg/vSGraphs_AllWallsData.msg b/msg/vSGraphs_AllWallsData.msg
deleted file mode 100644
index c82c4d59..00000000
--- a/msg/vSGraphs_AllWallsData.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-Header header
-
-# Set of vSGraphs_WallData.msg
-vSGraphs_WallData[] walls
\ No newline at end of file
diff --git a/msg/vSGraphs_RoomData.msg b/msg/vSGraphs_RoomData.msg
deleted file mode 100644
index eea8c31d..00000000
--- a/msg/vSGraphs_RoomData.msg
+++ /dev/null
@@ -1,5 +0,0 @@
-Header header
-
-int32 id
-int32[] wallIds
-geometry_msgs/Vector3 centroid
\ No newline at end of file
diff --git a/msg/vSGraphs_WallData.msg b/msg/vSGraphs_WallData.msg
deleted file mode 100644
index 52a3824e..00000000
--- a/msg/vSGraphs_WallData.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-Header header
-
-int32 id
-float32 length
-geometry_msgs/Vector3 normal
-geometry_msgs/Vector3 centroid
\ No newline at end of file
diff --git a/orb_slam3/include/Geometric/Plane.h b/orb_slam3/include/Geometric/Plane.h
index 8a0a5e3e..d770ebab 100644
--- a/orb_slam3/include/Geometric/Plane.h
+++ b/orb_slam3/include/Geometric/Plane.h
@@ -26,6 +26,7 @@
 #include <pcl/common/io.h>
 #include <pcl/common/centroid.h>
 #include <pcl/octree/octree_search.h>
+#include <boost/shared_ptr.hpp>
 
 namespace ORB_SLAM3
 {
diff --git a/orb_slam3/include/LoopClosing.h b/orb_slam3/include/LoopClosing.h
index bb15f529..65fa6716 100644
--- a/orb_slam3/include/LoopClosing.h
+++ b/orb_slam3/include/LoopClosing.h
@@ -221,7 +221,8 @@ namespace ORB_SLAM3
         // Fix scale in the stereo/RGB-D case
         bool mbFixScale;
 
-        bool mnFullBAIdx;
+        // bool mnFullBAIdx;
+        unsigned int mnFullBAIdx;
 
         vector<double> vdPR_CurrentTime;
         vector<double> vdPR_MatchedTime;
diff --git a/orb_slam3/src/Geometric/Plane.cc b/orb_slam3/src/Geometric/Plane.cc
index 13e7cd53..216da14b 100644
--- a/orb_slam3/src/Geometric/Plane.cc
+++ b/orb_slam3/src/Geometric/Plane.cc
@@ -14,12 +14,17 @@
 */
 
 #include "Geometric/Plane.h"
+#include <boost/shared_ptr.hpp>
+#include <boost/make_shared.hpp>
+#include <pcl/octree/octree_search.h>
 
 namespace ORB_SLAM3
 {
     Plane::Plane()
     {
-        planeCloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZRGBA>>();
+        // planeCloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZRGBA>>();
+        planeCloud = std::make_shared<pcl::PointCloud<pcl::PointXYZRGBA>>();
+
         octree = boost::make_shared<pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGBA>>(SystemParams::GetParams()->refine_map_points.octree.resolution);
         centroid.setZero();
         excludedFromAssoc = false;
diff --git a/package.xml b/package.xml
index fa216582..03b79dd0 100644
--- a/package.xml
+++ b/package.xml
@@ -1,50 +1,40 @@
 <?xml version="1.0"?>
-<package>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypedPath="file"?>
+<package format="3">
   <name>orb_slam3_ros</name>
   <version>0.0.1</version>
   <description>Visual S-Graphs (vS-Graphs) ROS-based Implementation.</description>
   <maintainer email="a.tourani1991@gmail.com">Ali Tourani</maintainer>
-
   <license>BSD</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <build_depend>roscpp</build_depend>
-  <build_depend>rospy</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <build_depend>nav_msgs</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>tf2_geometry_msgs</build_depend>
-  <build_depend>cv_bridge</build_depend>
-  <build_depend>image_transport</build_depend>
-  <build_depend>tf</build_depend>
-  <build_depend>tf2_ros</build_depend>
-  <build_depend>dynamic_reconfigure</build_depend>
-  <build_depend>message_generation</build_depend>
-  <build_depend>aruco_ros</build_depend>
-  <build_depend>aruco_msgs</build_depend>
-  <build_depend>backward_ros</build_depend>
-  <build_depend>rviz_visual_tools</build_depend>
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <!-- <depend>aruco_ros</depend>
+  <depend>aruco_msgs</depend> -->
+  <depend>backward_ros</depend>
+  <depend>rviz_visual_tools</depend>
+  <depend>segmenter_ros</depend>
 
-  <run_depend>roscpp</run_depend>
-  <run_depend>rospy</run_depend>
-  <run_depend>std_msgs</run_depend>
-  <run_depend>nav_msgs</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>geometry_msgs</run_depend>
-  <run_depend>tf2_geometry_msgs</run_depend>
-  <run_depend>cv_bridge</run_depend>
-  <run_depend>image_transport</run_depend>
-  <run_depend>tf</run_depend>
-  <run_depend>tf2_ros</run_depend>
-  <run_depend>dynamic_reconfigure</run_depend>
-  <run_depend>message_runtime</run_depend>
-  <run_depend>aruco_ros</run_depend>
-  <run_depend>aruco_msgs</run_depend>
-  <run_depend>backward_ros</run_depend>
-  <run_depend>rviz_visual_tools</run_depend>
-  <export>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
 
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
   </export>
 </package>
\ No newline at end of file
diff --git a/src/AR/ros_mono_ar.cc b/src/AR/ros_mono_ar.cc
index 0a03521b..2aa77a07 100644
--- a/src/AR/ros_mono_ar.cc
+++ b/src/AR/ros_mono_ar.cc
@@ -18,7 +18,7 @@
 #include <fstream>
 #include <chrono>
 
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 #include <cv_bridge/cv_bridge.h>
 
 #include <opencv2/core/core.hpp>
diff --git a/src/common.cc b/src/common.cc
index a29e70b2..766e0ae2 100644
--- a/src/common.cc
+++ b/src/common.cc
@@ -21,6 +21,7 @@
 */
 
 #include "common.h"
+#include <rclcpp/rclcpp.hpp>
 
 // Variables for ORB-SLAM3
 ORB_SLAM3::System *pSLAM;
@@ -30,119 +31,247 @@ ORB_SLAM3::System::eSensor sensorType = ORB_SLAM3::System::NOT_SET;
 bool colorPointcloud = true;
 double roll = 0, pitch = 0, yaw = 0;
 bool pubStaticTransform, pubPointClouds;
-image_transport::Publisher pubTrackingImage;
+// image_transport::Publisher pubTrackingImage;
+std::shared_ptr<image_transport::Publisher> pubTrackingImage;
 std::vector<ORB_SLAM3::Room *> gnnRoomCandidates;
-rviz_visual_tools::RvizVisualToolsPtr visualTools;
-std::shared_ptr<tf::TransformListener> transformListener;
+// rviz_visual_tools::RvizVisualToolsPtr visualTools;
+// std::shared_ptr<tf2::TransformListener> transformListener;
+// std::vector<std::vector<ORB_SLAM3::Marker *>> markersBuffer;
+// std::vector<std::vector<Eigen::Vector3d>> skeletonClusterPoints;
+// std::string world_frame_id, cam_frame_id, imu_frame_id, frameMap, frameBC, frameSE;
+std::shared_ptr<rviz_visual_tools::RvizVisualTools> visualTools;
+std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
+std::shared_ptr<tf2_ros::TransformListener> tfListener_{nullptr};
+
+std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
+std::unique_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster;
 std::vector<std::vector<ORB_SLAM3::Marker *>> markersBuffer;
 std::vector<std::vector<Eigen::Vector3d>> skeletonClusterPoints;
 std::string world_frame_id, cam_frame_id, imu_frame_id, frameMap, frameBC, frameSE;
-ros::Publisher pubAllWalls, pubAllMappoints, pubFiducialMarker, pubRoom, pubFreespaceCluster;
-ros::Publisher pubTrackedMappoints, pubSegmentedPointcloud, pubPlanePointcloud, pubPlaneLabel, pubDoor;
-ros::Publisher pubCameraPose, pubCameraPoseVis, pubOdometry, pubKeyFrameMarker, pubKFImage, pubKeyFrameList;
-ros::Time lastPlanePublishTime = ros::Time(0);
-
-bool saveMapService(orb_slam3_ros::SaveMap::Request &req, orb_slam3_ros::SaveMap::Response &res)
+// ros::Publisher pubAllWalls, pubAllMappoints, pubFiducialMarker, pubRoom, pubFreespaceCluster;
+// ros::Publisher pubTrackedMappoints, pubSegmentedPointcloud, pubPlanePointcloud, pubPlaneLabel, pubDoor;
+// ros::Publisher pubCameraPose, pubCameraPoseVis, pubOdometry, pubKeyFrameMarker, pubKFImage, pubKeyFrameList;
+rclcpp::Time lastPlanePublishTime = rclcpp::Time(0);
+
+rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubAllMappoints;
+rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubTrackedMappoints;
+rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubSegmentedPointcloud;
+rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubPlanePointcloud;
+rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubFreespaceCluster;
+rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pubCameraPose;
+rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubKeyFrameList;
+rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdometry;
+rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pubKeyFrameMarker;
+rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pubCameraPoseVis;
+rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pubPlaneLabel;
+rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pubDoor;
+rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pubFiducialMarker;
+rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pubRoom;
+rclcpp::Publisher<segmenter_ros::msg::VSGraphDataMsg>::SharedPtr pubKFImage;
+rclcpp::Publisher<orb_slam3_ros::msg::VSGraphsAllWallsData>::SharedPtr pubAllWalls;
+
+// bool saveMapService(vs_graphs::SaveMap::Request &req, vs_graphs::SaveMap::Response &res)
+// {
+//     res.success = pSLAM->SaveMap(req.name);
+
+//     if (res.success)
+//         ROS_INFO("Map was saved as %s.osa", req.name.c_str());
+//     else
+//         ROS_ERROR("Map could not be saved.");
+
+//     return res.success;
+// }
+void saveMapService(
+    std::shared_ptr<orb_slam3_ros::srv::SaveMap::Request> req,
+    std::shared_ptr<orb_slam3_ros::srv::SaveMap::Response> res)
 {
-    res.success = pSLAM->SaveMap(req.name);
+    res->success = pSLAM->SaveMap(req->name);
 
-    if (res.success)
-        ROS_INFO("Map was saved as %s.osa", req.name.c_str());
+    if (res->success)
+        RCLCPP_INFO(rclcpp::get_logger("visual_sgraphs"), "Map was saved as %s.osa", req->name.c_str());
     else
-        ROS_ERROR("Map could not be saved.");
-
-    return res.success;
+        RCLCPP_ERROR(rclcpp::get_logger("visual_sgraphs"), "Map could not be saved.");
 }
 
-bool saveMapPointsAsPCDService(orb_slam3_ros::SaveMap::Request &req, orb_slam3_ros::SaveMap::Response &res)
+// bool saveMapPointsAsPCDService(vs_graphs::SaveMap::Request &req, vs_graphs::SaveMap::Response &res)
+// {
+//     res.success = pSLAM->SaveMapPointsAsPCD(req.name);
+
+//     if (res.success)
+//         ROS_INFO("Map points were saved as %s.pcd", req.name.c_str());
+//     else
+//         ROS_ERROR("Map points could not be saved.");
+
+//     return res.success;
+// }
+
+void saveMapPointsAsPCDService(
+    std::shared_ptr<orb_slam3_ros::srv::SaveMap::Request> req,
+    std::shared_ptr<orb_slam3_ros::srv::SaveMap::Response> res)
 {
-    res.success = pSLAM->SaveMapPointsAsPCD(req.name);
+    res->success = pSLAM->SaveMapPointsAsPCD(req->name);
 
-    if (res.success)
-        ROS_INFO("Map points were saved as %s.pcd", req.name.c_str());
+    if (res->success)
+        RCLCPP_INFO(rclcpp::get_logger("visual_sgraphs"), "Map points were saved as %s.pcd", req->name.c_str());
     else
-        ROS_ERROR("Map points could not be saved.");
-
-    return res.success;
+        RCLCPP_ERROR(rclcpp::get_logger("visual_sgraphs"), "Map points could not be saved.");
 }
 
-bool saveTrajectoryService(orb_slam3_ros::SaveMap::Request &req, orb_slam3_ros::SaveMap::Response &res)
+// bool saveTrajectoryService(vs_graphs::SaveMap::Request &req, vs_graphs::SaveMap::Response &res)
+// {
+//     const string cameraTrajectoryFile = req.name + "_cam_traj.txt";
+//     const string keyframeTrajectoryFile = req.name + "_kf_traj.txt";
+
+//     try
+//     {
+//         pSLAM->SaveTrajectoryEuRoC(cameraTrajectoryFile);
+//         pSLAM->SaveKeyFrameTrajectoryEuRoC(keyframeTrajectoryFile);
+//         res.success = true;
+//     }
+//     catch (const std::exception &e)
+//     {
+//         std::cerr << e.what() << std::endl;
+//         res.success = false;
+//     }
+//     catch (...)
+//     {
+//         std::cerr << "Unknows exeption" << std::endl;
+//         res.success = false;
+//     }
+
+//     if (!res.success)
+//         ROS_ERROR("Estimated trajectory could not be saved.");
+
+//     return res.success;
+// }
+void saveTrajectoryService(
+    std::shared_ptr<orb_slam3_ros::srv::SaveMap::Request> req,
+    std::shared_ptr<orb_slam3_ros::srv::SaveMap::Response> res)
 {
-    const string cameraTrajectoryFile = req.name + "_cam_traj.txt";
-    const string keyframeTrajectoryFile = req.name + "_kf_traj.txt";
+    const std::string cameraTrajectoryFile = req->name + "_cam_traj.txt";
+    const std::string keyframeTrajectoryFile = req->name + "_kf_traj.txt";
 
     try
     {
         pSLAM->SaveTrajectoryEuRoC(cameraTrajectoryFile);
         pSLAM->SaveKeyFrameTrajectoryEuRoC(keyframeTrajectoryFile);
-        res.success = true;
+        res->success = true;
     }
     catch (const std::exception &e)
     {
         std::cerr << e.what() << std::endl;
-        res.success = false;
+        res->success = false;
     }
     catch (...)
     {
-        std::cerr << "Unknows exeption" << std::endl;
-        res.success = false;
+        std::cerr << "Unknown exception" << std::endl;
+        res->success = false;
     }
 
-    if (!res.success)
-        ROS_ERROR("Estimated trajectory could not be saved.");
-
-    return res.success;
+    if (!res->success)
+        RCLCPP_ERROR(rclcpp::get_logger("visual_sgraphs"), "Estimated trajectory could not be saved.");
 }
 
-void setupServices(ros::NodeHandle &nodeHandler, std::string node_name)
+// void setupServices(ros::NodeHandle &nodeHandler, std::string node_name)
+// {
+//     static ros::ServiceServer save_map_service = nodeHandler.advertiseService(node_name + "/save_map", saveMapService);
+//     static ros::ServiceServer save_traj_service = nodeHandler.advertiseService(node_name + "/save_traj", saveTrajectoryService);
+//     static ros::ServiceServer save_map_points_service = nodeHandler.advertiseService(node_name + "/save_map_points", saveMapPointsAsPCDService);
+// }
+
+void setupServices(std::shared_ptr<rclcpp::Node> node, const std::string &node_name)
 {
-    static ros::ServiceServer save_map_service = nodeHandler.advertiseService(node_name + "/save_map", saveMapService);
-    static ros::ServiceServer save_traj_service = nodeHandler.advertiseService(node_name + "/save_traj", saveTrajectoryService);
-    static ros::ServiceServer save_map_points_service = nodeHandler.advertiseService(node_name + "/save_map_points", saveMapPointsAsPCDService);
+    node->create_service<orb_slam3_ros::srv::SaveMap>(
+        node_name + "/save_map", &saveMapService);
+    node->create_service<orb_slam3_ros::srv::SaveMap>(
+        node_name + "/save_map_points", &saveMapPointsAsPCDService);
+    node->create_service<orb_slam3_ros::srv::SaveMap>(
+        node_name + "/save_traj", &saveTrajectoryService);
 }
 
-void setupPublishers(ros::NodeHandle &nodeHandler, image_transport::ImageTransport &image_transport, std::string node_name)
+// void setupPublishers(ros::NodeHandle &nodeHandler, image_transport::ImageTransport &image_transport, std::string node_name)
+// {
+//     // Basic
+//     pubTrackingImage = image_transport.advertise(node_name + "/tracking_image", 1);
+//     pubKeyFrameList = nodeHandler.advertise<nav_msgs::Path>(node_name + "/keyframe_list", 2);
+//     pubAllMappoints = nodeHandler.advertise<sensor_msgs::PointCloud2>(node_name + "/all_points", 1);
+//     pubCameraPose = nodeHandler.advertise<geometry_msgs::msg::PoseStamped>(node_name + "/camera_pose", 1);
+//     pubKFImage = nodeHandler.advertise<segmenter_ros::VSGraphDataMsg>(node_name + "/keyframe_image", 50); // rate of keyframe generation is higher
+//     pubTrackedMappoints = nodeHandler.advertise<sensor_msgs::PointCloud2>(node_name + "/tracked_points", 1);
+//     pubKeyFrameMarker = nodeHandler.advertise<visualization_msgs::msg::MarkerArray>(node_name + "/kf_markers", 1);
+//     pubFreespaceCluster = nodeHandler.advertise<sensor_msgs::PointCloud2>(node_name + "/freespace_clusters", 1);
+//     pubCameraPoseVis = nodeHandler.advertise<visualization_msgs::msg::MarkerArray>(node_name + "/camera_pose_vis", 1);
+
+//     // Building Components
+//     pubDoor = nodeHandler.advertise<visualization_msgs::msg::MarkerArray>(node_name + "/doors", 1);
+//     pubPlaneLabel = nodeHandler.advertise<visualization_msgs::msg::MarkerArray>(node_name + "/plane_labels", 1);
+//     pubPlanePointcloud = nodeHandler.advertise<sensor_msgs::PointCloud2>(node_name + "/plane_point_clouds", 1);
+//     pubFiducialMarker = nodeHandler.advertise<visualization_msgs::msg::MarkerArray>(node_name + "/fiducial_markers", 1);
+//     pubSegmentedPointcloud = nodeHandler.advertise<sensor_msgs::PointCloud2>(node_name + "/segmented_point_clouds", 1);
+//     // [Building Components] Publishing Walls for GNN-based Room Detection
+//     pubAllWalls = nodeHandler.advertise<vs_graphs::VSGraphsAllWallsData>(node_name + "/all_mapped_walls", 10);
+
+//     // Structural Elements
+//     pubRoom = nodeHandler.advertise<visualization_msgs::msg::MarkerArray>(node_name + "/rooms", 1);
+
+//     // Get body odometry if IMU data is also available
+//     if (sensorType == ORB_SLAM3::System::IMU_MONOCULAR || sensorType == ORB_SLAM3::System::IMU_STEREO ||
+//         sensorType == ORB_SLAM3::System::IMU_RGBD)
+//         pubOdometry = nodeHandler.advertise< nav_msgs::msg::Odometry>(node_name + "/body_odom", 1);
+
+//     // Showing planes using RViz Visual Tools
+//     visualTools = std::make_shared<rviz_visual_tools::RvizVisualTools>(
+//         frameBC, "/plane_visuals", nodeHandler);
+//     visualTools->setAlpha(0.5);
+//     visualTools->loadMarkerPub();
+//     visualTools->deleteAllMarkers();
+//     visualTools->enableBatchPublishing();
+
+//     transformListener = std::make_shared<tf::TransformListener>();
+// }
+
+void setupPublishers(std::shared_ptr<rclcpp::Node> node, std::shared_ptr<image_transport::ImageTransport> image_transport, const std::string &node_name)
 {
     // Basic
-    pubTrackingImage = image_transport.advertise(node_name + "/tracking_image", 1);
-    pubKeyFrameList = nodeHandler.advertise<nav_msgs::Path>(node_name + "/keyframe_list", 2);
-    pubAllMappoints = nodeHandler.advertise<sensor_msgs::PointCloud2>(node_name + "/all_points", 1);
-    pubCameraPose = nodeHandler.advertise<geometry_msgs::PoseStamped>(node_name + "/camera_pose", 1);
-    pubKFImage = nodeHandler.advertise<segmenter_ros::VSGraphDataMsg>(node_name + "/keyframe_image", 50); // rate of keyframe generation is higher
-    pubTrackedMappoints = nodeHandler.advertise<sensor_msgs::PointCloud2>(node_name + "/tracked_points", 1);
-    pubKeyFrameMarker = nodeHandler.advertise<visualization_msgs::MarkerArray>(node_name + "/kf_markers", 1);
-    pubFreespaceCluster = nodeHandler.advertise<sensor_msgs::PointCloud2>(node_name + "/freespace_clusters", 1);
-    pubCameraPoseVis = nodeHandler.advertise<visualization_msgs::MarkerArray>(node_name + "/camera_pose_vis", 1);
+    pubTrackingImage = std::make_shared<image_transport::Publisher>(image_transport->advertise(node_name + "/tracking_image", 1));
+    pubKeyFrameList = node->create_publisher<nav_msgs::msg::Path>(node_name + "/keyframe_list", 2);
+    pubAllMappoints = node->create_publisher<sensor_msgs::msg::PointCloud2>(node_name + "/all_points", 1);
+    pubCameraPose = node->create_publisher<geometry_msgs::msg::PoseStamped>(node_name + "/camera_pose", 1);
+    pubKFImage = node->create_publisher<segmenter_ros::msg::VSGraphDataMsg>(node_name + "/keyframe_image", 50);
+    pubTrackedMappoints = node->create_publisher<sensor_msgs::msg::PointCloud2>(node_name + "/tracked_points", 1);
+    pubKeyFrameMarker = node->create_publisher<visualization_msgs::msg::MarkerArray>(node_name + "/kf_markers", 1);
+    pubFreespaceCluster = node->create_publisher<sensor_msgs::msg::PointCloud2>(node_name + "/freespace_clusters", 1);
+    pubCameraPoseVis = node->create_publisher<visualization_msgs::msg::MarkerArray>(node_name + "/camera_pose_vis", 1);
 
     // Building Components
-    pubDoor = nodeHandler.advertise<visualization_msgs::MarkerArray>(node_name + "/doors", 1);
-    pubPlaneLabel = nodeHandler.advertise<visualization_msgs::MarkerArray>(node_name + "/plane_labels", 1);
-    pubPlanePointcloud = nodeHandler.advertise<sensor_msgs::PointCloud2>(node_name + "/plane_point_clouds", 1);
-    pubFiducialMarker = nodeHandler.advertise<visualization_msgs::MarkerArray>(node_name + "/fiducial_markers", 1);
-    pubSegmentedPointcloud = nodeHandler.advertise<sensor_msgs::PointCloud2>(node_name + "/segmented_point_clouds", 1);
-    // [Building Components] Publishing Walls for GNN-based Room Detection
-    pubAllWalls = nodeHandler.advertise<orb_slam3_ros::vSGraphs_AllWallsData>(node_name + "/all_mapped_walls", 10);
+    pubDoor = node->create_publisher<visualization_msgs::msg::MarkerArray>(node_name + "/doors", 1);
+    pubPlaneLabel = node->create_publisher<visualization_msgs::msg::MarkerArray>(node_name + "/plane_labels", 1);
+    pubPlanePointcloud = node->create_publisher<sensor_msgs::msg::PointCloud2>(node_name + "/plane_point_clouds", 1);
+    pubFiducialMarker = node->create_publisher<visualization_msgs::msg::MarkerArray>(node_name + "/fiducial_markers", 1);
+    pubSegmentedPointcloud = node->create_publisher<sensor_msgs::msg::PointCloud2>(node_name + "/segmented_point_clouds", 1);
+    pubAllWalls = node->create_publisher<orb_slam3_ros::msg::VSGraphsAllWallsData>(node_name + "/all_mapped_walls", 10);
 
     // Structural Elements
-    pubRoom = nodeHandler.advertise<visualization_msgs::MarkerArray>(node_name + "/rooms", 1);
+    pubRoom = node->create_publisher<visualization_msgs::msg::MarkerArray>(node_name + "/rooms", 1);
 
     // Get body odometry if IMU data is also available
     if (sensorType == ORB_SLAM3::System::IMU_MONOCULAR || sensorType == ORB_SLAM3::System::IMU_STEREO ||
         sensorType == ORB_SLAM3::System::IMU_RGBD)
-        pubOdometry = nodeHandler.advertise<nav_msgs::Odometry>(node_name + "/body_odom", 1);
+        pubOdometry = node->create_publisher<nav_msgs::msg::Odometry>(node_name + "/body_odom", 1);
 
-    // Showing planes using RViz Visual Tools
     visualTools = std::make_shared<rviz_visual_tools::RvizVisualTools>(
-        frameBC, "/plane_visuals", nodeHandler);
+    frameBC, "/plane_visuals", node);
     visualTools->setAlpha(0.5);
     visualTools->loadMarkerPub();
     visualTools->deleteAllMarkers();
     visualTools->enableBatchPublishing();
 
-    transformListener = std::make_shared<tf::TransformListener>();
+    // transformListener = std::make_shared<tf::TransformListener>(); 
+    tfBuffer_ = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+    tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
 }
 
-void publishTopics(ros::Time msgTime, Eigen::Vector3f Wbb)
+void publishTopics(rclcpp::Time msgTime, Eigen::Vector3f Wbb)
 {
     Sophus::SE3f Twc = pSLAM->GetCamTwc();
 
@@ -201,9 +330,36 @@ void publishTopics(ros::Time msgTime, Eigen::Vector3f Wbb)
     }
 }
 
-void publishBodyOdometry(Sophus::SE3f Twb_SE3f, Eigen::Vector3f Vwb_E3f, Eigen::Vector3f ang_vel_body, ros::Time msgTime)
+// void publishBodyOdometry(Sophus::SE3f Twb_SE3f, Eigen::Vector3f Vwb_E3f, Eigen::Vector3f ang_vel_body, rclcpp::Time msgTime)
+// {
+//      nav_msgs::msg::Odometry odom_msg;
+//     odom_msg.child_frame_id = imu_frame_id;
+//     odom_msg.header.frame_id = world_frame_id;
+//     odom_msg.header.stamp = msgTime;
+
+//     odom_msg.pose.pose.position.x = Twb_SE3f.translation().x();
+//     odom_msg.pose.pose.position.y = Twb_SE3f.translation().y();
+//     odom_msg.pose.pose.position.z = Twb_SE3f.translation().z();
+
+//     odom_msg.pose.pose.orientation.w = Twb_SE3f.unit_quaternion().coeffs().w();
+//     odom_msg.pose.pose.orientation.x = Twb_SE3f.unit_quaternion().coeffs().x();
+//     odom_msg.pose.pose.orientation.y = Twb_SE3f.unit_quaternion().coeffs().y();
+//     odom_msg.pose.pose.orientation.z = Twb_SE3f.unit_quaternion().coeffs().z();
+
+//     odom_msg.twist.twist.linear.x = Vwb_E3f.x();
+//     odom_msg.twist.twist.linear.y = Vwb_E3f.y();
+//     odom_msg.twist.twist.linear.z = Vwb_E3f.z();
+
+//     odom_msg.twist.twist.angular.x = ang_vel_body.x();
+//     odom_msg.twist.twist.angular.y = ang_vel_body.y();
+//     odom_msg.twist.twist.angular.z = ang_vel_body.z();
+
+//     pubOdometry.publish(odom_msg);
+// }
+
+void publishBodyOdometry(Sophus::SE3f Twb_SE3f, Eigen::Vector3f Vwb_E3f, Eigen::Vector3f ang_vel_body, rclcpp::Time msgTime)
 {
-    nav_msgs::Odometry odom_msg;
+    nav_msgs::msg::Odometry odom_msg;
     odom_msg.child_frame_id = imu_frame_id;
     odom_msg.header.frame_id = world_frame_id;
     odom_msg.header.stamp = msgTime;
@@ -225,12 +381,61 @@ void publishBodyOdometry(Sophus::SE3f Twb_SE3f, Eigen::Vector3f Vwb_E3f, Eigen::
     odom_msg.twist.twist.angular.y = ang_vel_body.y();
     odom_msg.twist.twist.angular.z = ang_vel_body.z();
 
-    pubOdometry.publish(odom_msg);
+    pubOdometry->publish(odom_msg);
 }
 
-void publishCameraPose(Sophus::SE3f Tcw_SE3f, ros::Time msgTime)
+// void publishCameraPose(Sophus::SE3f Tcw_SE3f, rclcpp::Time msgTime)
+// {
+//     geometry_msgs::msg::PoseStamped poseMsg;
+//     poseMsg.header.frame_id = cam_frame_id;
+//     poseMsg.header.stamp = msgTime;
+
+//     poseMsg.pose.position.x = Tcw_SE3f.translation().x();
+//     poseMsg.pose.position.y = Tcw_SE3f.translation().y();
+//     poseMsg.pose.position.z = Tcw_SE3f.translation().z();
+
+//     poseMsg.pose.orientation.w = Tcw_SE3f.unit_quaternion().coeffs().w();
+//     poseMsg.pose.orientation.x = Tcw_SE3f.unit_quaternion().coeffs().x();
+//     poseMsg.pose.orientation.y = Tcw_SE3f.unit_quaternion().coeffs().y();
+//     poseMsg.pose.orientation.z = Tcw_SE3f.unit_quaternion().coeffs().z();
+
+//     pubCameraPose.publish(poseMsg);
+
+//     // Add a marker for visualization
+//     visualization_msgs::msg::Marker cameraVisual;
+//     visualization_msgs::msg::MarkerArray cameraVisualList;
+
+//     cameraVisual.id = 1;
+//     cameraVisual.color.a = 0.7;
+//     cameraVisual.scale.x = 0.5;
+//     cameraVisual.scale.y = 0.5;
+//     cameraVisual.scale.z = 0.5;
+//     cameraVisual.ns = "camera_pose";
+//     cameraVisual.header.stamp = msgTime;
+//     cameraVisual.action = cameraVisual.ADD;
+//     cameraVisual.lifetime = rclcpp::Duration::from_seconds(0);
+//     cameraVisual.header.frame_id = world_frame_id;
+//     cameraVisual.mesh_use_embedded_materials = true;
+//     cameraVisual.type = visualization_msgs::msg::Marker::MESH_RESOURCE;
+//     cameraVisual.mesh_resource =
+//         "package://vs_graphs/config/Assets/camera.dae";
+
+//     cameraVisual.pose.position.x = Tcw_SE3f.translation().x();
+//     cameraVisual.pose.position.y = Tcw_SE3f.translation().y();
+//     cameraVisual.pose.position.z = Tcw_SE3f.translation().z();
+//     cameraVisual.pose.orientation.x = Tcw_SE3f.unit_quaternion().x();
+//     cameraVisual.pose.orientation.y = Tcw_SE3f.unit_quaternion().y();
+//     cameraVisual.pose.orientation.z = Tcw_SE3f.unit_quaternion().z();
+//     cameraVisual.pose.orientation.w = Tcw_SE3f.unit_quaternion().w();
+
+//     cameraVisualList.markers.push_back(cameraVisual);
+
+//     pubCameraPoseVis.publish(cameraVisualList);
+// }
+
+void publishCameraPose(Sophus::SE3f Tcw_SE3f, rclcpp::Time msgTime)
 {
-    geometry_msgs::PoseStamped poseMsg;
+    geometry_msgs::msg::PoseStamped poseMsg;
     poseMsg.header.frame_id = cam_frame_id;
     poseMsg.header.stamp = msgTime;
 
@@ -243,11 +448,11 @@ void publishCameraPose(Sophus::SE3f Tcw_SE3f, ros::Time msgTime)
     poseMsg.pose.orientation.y = Tcw_SE3f.unit_quaternion().coeffs().y();
     poseMsg.pose.orientation.z = Tcw_SE3f.unit_quaternion().coeffs().z();
 
-    pubCameraPose.publish(poseMsg);
+    pubCameraPose->publish(poseMsg);
 
     // Add a marker for visualization
-    visualization_msgs::Marker cameraVisual;
-    visualization_msgs::MarkerArray cameraVisualList;
+    visualization_msgs::msg::Marker cameraVisual;
+    visualization_msgs::msg::MarkerArray cameraVisualList;
 
     cameraVisual.id = 1;
     cameraVisual.color.a = 0.7;
@@ -257,12 +462,12 @@ void publishCameraPose(Sophus::SE3f Tcw_SE3f, ros::Time msgTime)
     cameraVisual.ns = "camera_pose";
     cameraVisual.header.stamp = msgTime;
     cameraVisual.action = cameraVisual.ADD;
-    cameraVisual.lifetime = ros::Duration();
+    cameraVisual.lifetime = rclcpp::Duration::from_seconds(0);
     cameraVisual.header.frame_id = world_frame_id;
     cameraVisual.mesh_use_embedded_materials = true;
-    cameraVisual.type = visualization_msgs::Marker::MESH_RESOURCE;
+    cameraVisual.type = visualization_msgs::msg::Marker::MESH_RESOURCE;
     cameraVisual.mesh_resource =
-        "package://orb_slam3_ros/config/Assets/camera.dae";
+        "package://vs_graphs/config/Assets/camera.dae";
 
     cameraVisual.pose.position.x = Tcw_SE3f.translation().x();
     cameraVisual.pose.position.y = Tcw_SE3f.translation().y();
@@ -274,45 +479,64 @@ void publishCameraPose(Sophus::SE3f Tcw_SE3f, ros::Time msgTime)
 
     cameraVisualList.markers.push_back(cameraVisual);
 
-    pubCameraPoseVis.publish(cameraVisualList);
+    pubCameraPoseVis->publish(cameraVisualList);
 }
 
-void publishTFTransform(Sophus::SE3f T_SE3f, string frame_id, string child_frame_id, ros::Time msgTime)
+// void publishTFTransform(Sophus::SE3f T_SE3f, string frame_id, string child_frame_id, rclcpp::Time msgTime)
+// {
+//     tf::Transform tf_transform = SE3fToTFTransform(T_SE3f);
+//     static tf::TransformBroadcaster tf_broadcaster;
+//     tf_broadcaster.sendTransform(tf::StampedTransform(tf_transform, msgTime, frame_id, child_frame_id));
+// }
+
+void publishTFTransform(Sophus::SE3f T_SE3f, std::string frame_id, std::string child_frame_id, rclcpp::Time msgTime)
 {
-    tf::Transform tf_transform = SE3fToTFTransform(T_SE3f);
-    static tf::TransformBroadcaster tf_broadcaster;
-    tf_broadcaster.sendTransform(tf::StampedTransform(tf_transform, msgTime, frame_id, child_frame_id));
+    geometry_msgs::msg::TransformStamped transformStamped;
+    transformStamped.header.stamp = msgTime;
+    transformStamped.header.frame_id = frame_id;
+    transformStamped.child_frame_id = child_frame_id;
+
+    // Fill translation
+    transformStamped.transform.translation.x = T_SE3f.translation().x();
+    transformStamped.transform.translation.y = T_SE3f.translation().y();
+    transformStamped.transform.translation.z = T_SE3f.translation().z();
+
+    // Fill rotation
+    transformStamped.transform.rotation.x = T_SE3f.unit_quaternion().x();
+    transformStamped.transform.rotation.y = T_SE3f.unit_quaternion().y();
+    transformStamped.transform.rotation.z = T_SE3f.unit_quaternion().z();
+    transformStamped.transform.rotation.w = T_SE3f.unit_quaternion().w();
+
+    if (tf_broadcaster) {
+        tf_broadcaster->sendTransform(transformStamped);
+    }
 }
 
-void publishStaticTFTransform(string parentFrameId, string childFrameId, ros::Time msgTime)
+void publishStaticTFTransform(std::string parentFrameId, std::string childFrameId, rclcpp::Time msgTime)
 {
-    // Variables
     tf2::Quaternion quat;
-    geometry_msgs::TransformStamped transformStamped;
-    static tf2_ros::StaticTransformBroadcaster broadcaster;
+    geometry_msgs::msg::TransformStamped transformStamped;
 
-    // Set the values of the transform message
     transformStamped.header.stamp = msgTime;
     transformStamped.child_frame_id = childFrameId;
     transformStamped.header.frame_id = parentFrameId;
 
-    // Set the translation of the transform to (0,0,0)
     transformStamped.transform.translation.x = 0;
     transformStamped.transform.translation.y = 0;
     transformStamped.transform.translation.z = 0;
 
-    // Set the rotation of the transform to a fixed value
     quat.setRPY(roll, pitch, yaw);
     transformStamped.transform.rotation.x = quat.x();
     transformStamped.transform.rotation.y = quat.y();
     transformStamped.transform.rotation.z = quat.z();
     transformStamped.transform.rotation.w = quat.w();
 
-    // Publish the static transform using the broadcaster
-    broadcaster.sendTransform(transformStamped);
+    if (static_tf_broadcaster) {
+        static_tf_broadcaster->sendTransform(transformStamped);
+    }
 }
 
-void publishFreeSpaceClusters(std::vector<std::vector<Eigen::Vector3d>> clusterPoints, ros::Time msgTime)
+void publishFreeSpaceClusters(std::vector<std::vector<Eigen::Vector3d>> clusterPoints, rclcpp::Time msgTime)
 {
     // Check if the cluster points are empty
     if (clusterPoints.empty())
@@ -343,7 +567,8 @@ void publishFreeSpaceClusters(std::vector<std::vector<Eigen::Vector3d>> clusterP
         }
         // Increment the color index
         colorIndex += 1;
-        if (colorIndex == fixedColors.size())
+        // if (colorIndex == fixedColors.size())
+        if (colorIndex == static_cast<int>(fixedColors.size()))
             colorIndex = 0;
     }
 
@@ -352,7 +577,7 @@ void publishFreeSpaceClusters(std::vector<std::vector<Eigen::Vector3d>> clusterP
         return;
 
     // Convert the point cloud to a PointCloud2 message
-    sensor_msgs::PointCloud2 cloudMsg;
+    sensor_msgs::msg::PointCloud2 cloudMsg;
     pcl::toROSMsg(*freeSpaceCloud, cloudMsg);
 
     // Set message header
@@ -360,10 +585,10 @@ void publishFreeSpaceClusters(std::vector<std::vector<Eigen::Vector3d>> clusterP
     cloudMsg.header.frame_id = world_frame_id;
 
     // Publish the point cloud
-    pubFreespaceCluster.publish(cloudMsg);
+    pubFreespaceCluster->publish(cloudMsg);
 }
 
-void publishKeyFrameImages(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec, ros::Time msgTime)
+void publishKeyFrameImages(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec, rclcpp::Time msgTime)
 {
     // Check all keyframes and publish the ones that have not been published for Semantic Segmentation yet
     for (auto &keyframe : keyframe_vec)
@@ -372,29 +597,28 @@ void publishKeyFrameImages(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec, ros:
             continue;
 
         // Create an object of VSGraphDataMsg
-        segmenter_ros::VSGraphDataMsg vsGraphPublisher = segmenter_ros::VSGraphDataMsg();
-
-        std_msgs::Header header;
+        segmenter_ros::msg::VSGraphDataMsg vsGraphPublisher = segmenter_ros::msg::VSGraphDataMsg();
+        std_msgs::msg::Header header;
         header.stamp = msgTime;
         header.frame_id = world_frame_id;
-        std_msgs::UInt64 kfId;
+        std_msgs::msg::UInt64 kfId;
         kfId.data = keyframe->mnId;
-        const sensor_msgs::ImagePtr rendered_image_msg =
+        const sensor_msgs::msg::Image::SharedPtr rendered_image_msg =
             cv_bridge::CvImage(header, "bgr8", keyframe->mImage).toImageMsg();
 
         vsGraphPublisher.header = header;
-        vsGraphPublisher.keyFrameId = kfId;
-        vsGraphPublisher.keyFrameImage = *rendered_image_msg;
+        vsGraphPublisher.key_frame_id = kfId;
+        vsGraphPublisher.key_frame_image = *rendered_image_msg;
 
-        pubKFImage.publish(vsGraphPublisher);
+        pubKFImage->publish(vsGraphPublisher);
         keyframe->isPublished = true;
     }
 }
 
-void publishAllMappedWalls(std::vector<ORB_SLAM3::Plane *> walls, ros::Time msgTime)
+void publishAllMappedWalls(std::vector<ORB_SLAM3::Plane *> walls, rclcpp::Time msgTime)
 {
-    orb_slam3_ros::vSGraphs_AllWallsData wallDataMsg;
-
+    // vs_graphs::VSGraphsAllWallsData wallDataMsg;
+    orb_slam3_ros::msg::VSGraphsAllWallsData wallDataMsg;
     // Fill the data message with wall information
     wallDataMsg.header.stamp = msgTime;
     wallDataMsg.header.frame_id = world_frame_id;
@@ -417,7 +641,7 @@ void publishAllMappedWalls(std::vector<ORB_SLAM3::Plane *> walls, ros::Time msgT
         }
 
         // Fill the wall data
-        orb_slam3_ros::vSGraphs_WallData wallData;
+        orb_slam3_ros::msg::VSGraphsWallData wallData;
 
         wallData.length = length;
         wallData.id = wall->getId();
@@ -433,7 +657,7 @@ void publishAllMappedWalls(std::vector<ORB_SLAM3::Plane *> walls, ros::Time msgT
     }
 
     // Publish all mapped walls
-    pubAllWalls.publish(wallDataMsg);
+    pubAllWalls->publish(wallDataMsg);
 }
 
 void clearKFClsClouds(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec)
@@ -442,7 +666,7 @@ void clearKFClsClouds(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec)
         keyframe->clearClsClouds();
 }
 
-void publishSegmentedCloud(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec, ros::Time msgTime)
+void publishSegmentedCloud(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec, rclcpp::Time msgTime)
 {
     // get the latest processed keyframe
     ORB_SLAM3::KeyFrame *thisKF = nullptr;
@@ -491,50 +715,50 @@ void publishSegmentedCloud(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec, ros:
     thisKF->clearClsClouds();
 
     // create a new pointcloud2 message from the transformed and aggregated pointcloud
-    sensor_msgs::PointCloud2 cloud_msg;
+    sensor_msgs::msg::PointCloud2 cloud_msg;
     pcl::toROSMsg(*aggregatedCloud, cloud_msg);
 
     // publish the pointcloud to be seen at the plane frame
     cloud_msg.header.frame_id = cam_frame_id;
-    pubSegmentedPointcloud.publish(cloud_msg);
+    pubSegmentedPointcloud->publish(cloud_msg);
 }
 
-void publishTrackingImage(cv::Mat image, ros::Time msgTime)
+void publishTrackingImage(cv::Mat image, rclcpp::Time msgTime)
 {
-    std_msgs::Header header;
+    std_msgs::msg::Header header;
     header.stamp = msgTime;
     header.frame_id = world_frame_id;
-    const sensor_msgs::ImagePtr rendered_image_msg = cv_bridge::CvImage(header, "bgr8", image).toImageMsg();
-    pubTrackingImage.publish(rendered_image_msg);
+    const sensor_msgs::msg::Image::SharedPtr rendered_image_msg = cv_bridge::CvImage(header, "bgr8", image).toImageMsg();
+    pubTrackingImage->publish(rendered_image_msg);
 }
 
-void publishTrackedPoints(std::vector<ORB_SLAM3::MapPoint *> tracked_points, ros::Time msgTime)
+void publishTrackedPoints(std::vector<ORB_SLAM3::MapPoint *> tracked_points, rclcpp::Time msgTime)
 {
-    sensor_msgs::PointCloud2 cloud = mapPointToPointcloud(tracked_points, msgTime);
-    pubTrackedMappoints.publish(cloud);
+    sensor_msgs::msg::PointCloud2 cloud = mapPointToPointcloud(tracked_points, msgTime);
+    pubTrackedMappoints->publish(cloud);
 }
 
-void publishAllPoints(std::vector<ORB_SLAM3::MapPoint *> mapPoints, ros::Time msgTime)
+void publishAllPoints(std::vector<ORB_SLAM3::MapPoint *> mapPoints, rclcpp::Time msgTime)
 {
-    sensor_msgs::PointCloud2 cloud = mapPointToPointcloud(mapPoints, msgTime);
-    pubAllMappoints.publish(cloud);
+    sensor_msgs::msg::PointCloud2 cloud = mapPointToPointcloud(mapPoints, msgTime);
+    pubAllMappoints->publish(cloud);
 }
 
-void publishKeyFrameMarkers(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec, ros::Time msgTime)
+void publishKeyFrameMarkers(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec, rclcpp::Time msgTime)
 {
     sort(keyframe_vec.begin(), keyframe_vec.end(), ORB_SLAM3::KeyFrame::lId);
     if (keyframe_vec.size() == 0)
         return;
 
-    visualization_msgs::MarkerArray markerArray;
+    visualization_msgs::msg::MarkerArray markerArray;
 
-    visualization_msgs::Marker kf_markers;
+    visualization_msgs::msg::Marker kf_markers;
     kf_markers.header.frame_id = world_frame_id;
     kf_markers.ns = "kf_markers";
-    kf_markers.type = visualization_msgs::Marker::SPHERE_LIST;
-    kf_markers.action = visualization_msgs::Marker::ADD;
+    kf_markers.type = visualization_msgs::msg::Marker::SPHERE_LIST;
+    kf_markers.action = visualization_msgs::msg::Marker::ADD;
     kf_markers.pose.orientation.w = 1.0;
-    kf_markers.lifetime = ros::Duration();
+    kf_markers.lifetime = rclcpp::Duration::from_seconds(0);
     kf_markers.id = 0;
     kf_markers.scale.x = 0.05;
     kf_markers.scale.y = 0.05;
@@ -542,7 +766,7 @@ void publishKeyFrameMarkers(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec, ros
     kf_markers.color.g = 1.0;
     kf_markers.color.a = 1.0;
 
-    visualization_msgs::Marker kf_lines;
+    visualization_msgs::msg::Marker kf_lines;
     kf_lines.color.a = 0.15;
     kf_lines.color.r = 0.0;
     kf_lines.color.g = 0.0;
@@ -552,19 +776,20 @@ void publishKeyFrameMarkers(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec, ros
     kf_lines.scale.z = 0.003;
     kf_lines.action = kf_lines.ADD;
     kf_lines.ns = "kf_lines";
-    kf_lines.lifetime = ros::Duration();
+    kf_lines.lifetime = rclcpp::Duration::from_seconds(0);
     kf_lines.id = 1;
-    kf_lines.header.stamp = ros::Time().now();
+    kf_lines.header.stamp = rclcpp::Clock().now(); //rclcpp::Time().now();
     kf_lines.header.frame_id = world_frame_id;
-    kf_lines.type = visualization_msgs::Marker::LINE_LIST;
+    kf_lines.type = visualization_msgs::msg::Marker::LINE_LIST;
 
-    nav_msgs::Path kf_list;
+    // nav_msgs::Path kf_list;
+    nav_msgs::msg::Path kf_list;
     kf_list.header.frame_id = world_frame_id;
     kf_list.header.stamp = msgTime;
 
     for (auto &keyframe : keyframe_vec)
     {
-        geometry_msgs::Point kf_marker;
+        geometry_msgs::msg::Point kf_marker;
 
         Sophus::SE3f kf_pose = pSLAM->GetKeyFramePose(keyframe);
         kf_marker.x = kf_pose.translation().x();
@@ -573,8 +798,8 @@ void publishKeyFrameMarkers(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec, ros
         kf_markers.points.push_back(kf_marker);
 
         // populate the keyframe list
-        geometry_msgs::PoseStamped pose;
-        pose.header.stamp = ros::Time(keyframe->mTimeStamp);
+        geometry_msgs::msg::PoseStamped pose;
+        pose.header.stamp = rclcpp::Time(keyframe->mTimeStamp);
         pose.header.frame_id = world_frame_id;
         pose.pose.position.x = kf_pose.translation().x();
         pose.pose.position.y = kf_pose.translation().y();
@@ -585,6 +810,8 @@ void publishKeyFrameMarkers(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec, ros
         pose.pose.orientation.z = kf_pose.unit_quaternion().z();
         kf_list.poses.push_back(pose);
 
+        // THIS IS COMMENTED IN ROS1 VS-GRAPHS
+
         // // add lines to all keyframes in the covisibility graph
         // std::vector<ORB_SLAM3::KeyFrame *> covisibility = keyframe->GetBestCovisibilityKeyFrames(75);
         // // std::vector<ORB_SLAM3::KeyFrame *> covisibility = keyframe->GetVectorCovisibleKeyFrames();
@@ -624,7 +851,7 @@ void publishKeyFrameMarkers(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec, ros
         //     planePoint.frame_id_ = frameBC;
 
         //     tf::Stamped<tf::Point> planePointTransformed;
-        //     transform_listener->transformPoint(world_frame_id, ros::Time(0), planePoint,
+        //     transform_listener->transformPoint(world_frame_id, rclcpp::Time(0), planePoint,
         //                                        frameBC, planePointTransformed);
 
         //     geometry_msgs::Point point1;
@@ -638,22 +865,22 @@ void publishKeyFrameMarkers(std::vector<ORB_SLAM3::KeyFrame *> keyframe_vec, ros
     }
     markerArray.markers.push_back(kf_markers);
     // markerArray.markers.push_back(kf_lines);
-    pubKeyFrameMarker.publish(markerArray);
-    pubKeyFrameList.publish(kf_list);
+    pubKeyFrameMarker->publish(markerArray);
+    pubKeyFrameList->publish(kf_list);
 }
 
-void publishFiducialMarkers(std::vector<ORB_SLAM3::Marker *> markers, ros::Time msgTime)
+void publishFiducialMarkers(std::vector<ORB_SLAM3::Marker *> markers, rclcpp::Time msgTime)
 {
     int numMarkers = markers.size();
     if (numMarkers == 0)
         return;
 
-    visualization_msgs::MarkerArray markerArray;
+    visualization_msgs::msg::MarkerArray markerArray;
     markerArray.markers.resize(numMarkers);
 
     for (int idx = 0; idx < numMarkers; idx++)
     {
-        visualization_msgs::Marker fiducial_marker;
+        visualization_msgs::msg::Marker fiducial_marker;
         Sophus::SE3f markerPose = markers[idx]->getGlobalPose();
 
         fiducial_marker.color.a = 0;
@@ -661,15 +888,15 @@ void publishFiducialMarkers(std::vector<ORB_SLAM3::Marker *> markers, ros::Time
         fiducial_marker.scale.y = 0.2;
         fiducial_marker.scale.z = 0.2;
         fiducial_marker.ns = "fiducial_markers";
-        fiducial_marker.lifetime = ros::Duration();
+        fiducial_marker.lifetime = rclcpp::Duration::from_seconds(0);
         fiducial_marker.action = fiducial_marker.ADD;
         fiducial_marker.id = markerArray.markers.size();
-        fiducial_marker.header.stamp = ros::Time().now();
+        fiducial_marker.header.stamp = rclcpp::Clock().now(); //rclcpp::Time().now();
         fiducial_marker.mesh_use_embedded_materials = true;
         fiducial_marker.header.frame_id = frameBC;
-        fiducial_marker.type = visualization_msgs::Marker::MESH_RESOURCE;
+        fiducial_marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE;
         fiducial_marker.mesh_resource =
-            "package://orb_slam3_ros/config/Assets/aruco_marker.dae";
+            "package://vs_graphs/config/Assets/aruco_marker.dae";
 
         fiducial_marker.pose.position.x = markerPose.translation().x();
         fiducial_marker.pose.position.y = markerPose.translation().y();
@@ -682,10 +909,10 @@ void publishFiducialMarkers(std::vector<ORB_SLAM3::Marker *> markers, ros::Time
         markerArray.markers.push_back(fiducial_marker);
     }
 
-    pubFiducialMarker.publish(markerArray);
+    pubFiducialMarker->publish(markerArray);
 }
 
-void publishDoors(std::vector<ORB_SLAM3::Door *> doors, ros::Time msgTime)
+void publishDoors(std::vector<ORB_SLAM3::Door *> doors, rclcpp::Time msgTime)
 {
     // If there are no doors, return
     int numDoors = doors.size();
@@ -693,13 +920,13 @@ void publishDoors(std::vector<ORB_SLAM3::Door *> doors, ros::Time msgTime)
         return;
 
     // Variables
-    visualization_msgs::MarkerArray doorArray;
+    visualization_msgs::msg::MarkerArray doorArray;
     doorArray.markers.resize(numDoors);
 
     for (int idx = 0; idx < numDoors; idx++)
     {
         Sophus::SE3f doorPose = doors[idx]->getGlobalPose();
-        visualization_msgs::Marker door, doorLines, doorLabel;
+        visualization_msgs::msg::Marker door, doorLines, doorLabel;
 
         // Door values
         door.color.a = 0;
@@ -708,14 +935,14 @@ void publishDoors(std::vector<ORB_SLAM3::Door *> doors, ros::Time msgTime)
         door.scale.y = 0.5;
         door.scale.z = 0.5;
         door.action = door.ADD;
-        door.lifetime = ros::Duration();
+        door.lifetime = rclcpp::Duration::from_seconds(0);
         door.id = doorArray.markers.size();
-        door.header.stamp = ros::Time().now();
+        door.header.stamp = rclcpp::Clock().now(); //rclcpp::Time().now();
         door.mesh_use_embedded_materials = true;
         door.header.frame_id = frameBC;
-        door.type = visualization_msgs::Marker::MESH_RESOURCE;
+        door.type = visualization_msgs::msg::Marker::MESH_RESOURCE;
         door.mesh_resource =
-            "package://orb_slam3_ros/config/Assets/door.dae";
+            "package://vs_graphs/config/Assets/door.dae";
 
         // Rotation and displacement for better visualization
         Sophus::SE3f rotatedDoorPose = doorPose * Sophus::SE3f::rotX(-M_PI_2);
@@ -737,15 +964,15 @@ void publishDoors(std::vector<ORB_SLAM3::Door *> doors, ros::Time msgTime)
         doorLabel.scale.z = 0.2;
         doorLabel.ns = "doorLabel";
         doorLabel.action = doorLabel.ADD;
-        doorLabel.lifetime = ros::Duration();
+        doorLabel.lifetime = rclcpp::Duration::from_seconds(0);
         doorLabel.text = doors[idx]->getName();
         doorLabel.id = doorArray.markers.size();
-        doorLabel.header.stamp = ros::Time().now();
+        doorLabel.header.stamp = rclcpp::Clock().now(); //rclcpp::Time().now();
         doorLabel.header.frame_id = frameBC;
         doorLabel.pose.position.x = door.pose.position.x;
         doorLabel.pose.position.z = door.pose.position.z;
         doorLabel.pose.position.y = door.pose.position.y - 1.2;
-        doorLabel.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+        doorLabel.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
         doorArray.markers.push_back(doorLabel);
 
         // Door to points connection line
@@ -758,19 +985,19 @@ void publishDoors(std::vector<ORB_SLAM3::Door *> doors, ros::Time msgTime)
         doorLines.scale.z = 0.005;
         doorLines.ns = "doorLines";
         doorLines.action = doorLines.ADD;
-        doorLines.lifetime = ros::Duration();
+        doorLines.lifetime = rclcpp::Duration::from_seconds(0);
         doorLines.id = doorArray.markers.size();
-        doorLines.header.stamp = ros::Time().now();
+        doorLines.header.stamp = rclcpp::Clock().now(); //rclcpp::Time().now();
         doorLines.header.frame_id = frameBC;
-        doorLines.type = visualization_msgs::Marker::LINE_LIST;
+        doorLines.type = visualization_msgs::msg::Marker::LINE_LIST;
 
-        geometry_msgs::Point point1;
+        geometry_msgs::msg::Point point1;
         point1.x = doors[idx]->getMarker()->getGlobalPose().translation().x();
         point1.y = doors[idx]->getMarker()->getGlobalPose().translation().y();
         point1.z = doors[idx]->getMarker()->getGlobalPose().translation().z();
         doorLines.points.push_back(point1);
 
-        geometry_msgs::Point point2;
+        geometry_msgs::msg::Point point2;
         point2.x = rotatedDoorPose.translation().x();
         point2.y = rotatedDoorPose.translation().y();
         point2.z = rotatedDoorPose.translation().z();
@@ -779,26 +1006,43 @@ void publishDoors(std::vector<ORB_SLAM3::Door *> doors, ros::Time msgTime)
         doorArray.markers.push_back(doorLines);
     }
 
-    pubDoor.publish(doorArray);
+    pubDoor->publish(doorArray);
 }
 
-void publishPlanes(std::vector<ORB_SLAM3::Plane *> planes, ros::Time msgTime)
+void publishPlanes(std::vector<ORB_SLAM3::Plane *> planes, rclcpp::Time msgTime)
 {
     // Publish the planes, if any
     int numPlanes = planes.size();
     if (numPlanes == 0)
         return;
 
+    
+    std::cout << "msgTime: " << msgTime.seconds() << " seconds" << std::endl;
+    std::cout << "lastPlanePublishTime: " << lastPlanePublishTime.seconds() << " seconds" << std::endl;
+    
+    // TODO: proper inicialization of lastPlanePublishTime
+    if ( lastPlanePublishTime.seconds() == 0)
+    {
+        lastPlanePublishTime = msgTime;
+    }
+    
     // Check if sufficient time has passed since the last plane publication
-    if ((msgTime - lastPlanePublishTime).toSec() < 3)
+    // if ((msgTime - lastPlanePublishTime)rclcpp::Time::seconds() < 3)
+      
+    // double delta_time = (msgTime - lastPlanePublishTime).seconds();
+    // if (delta_time < 3)
+    // if (rclcpp::Duration(msgTime - lastPlanePublishTime).seconds() < 3.0)
+    // if ((msgTime - lastPlanePublishTime) < rclcpp::Duration::from_seconds(3.0))
+    // if ((msgTime - lastPlanePublishTime).nanoseconds() < 3000000000LL)
+    if ((msgTime - lastPlanePublishTime).seconds() < 3)
         return;
     lastPlanePublishTime = msgTime;
 
     // Variables
-    visualization_msgs::MarkerArray planeLabelArray;
+    visualization_msgs::msg::MarkerArray planeLabelArray;
     planeLabelArray.markers.resize(numPlanes);
-    visualization_msgs::Marker planeLabel, planeNormal;
-    geometry_msgs::Point normalStartPoint, normalEndPoint;
+    visualization_msgs::msg::Marker planeLabel, planeNormal;
+    geometry_msgs::msg::Point normalStartPoint, normalEndPoint;
 
     // Aggregate pointcloud XYZRGB for all planes
     pcl::PointCloud<pcl::PointXYZRGB>::Ptr aggregatedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
@@ -862,12 +1106,12 @@ void publishPlanes(std::vector<ORB_SLAM3::Plane *> planes, ros::Time msgTime)
         planeLabel.color.r = color[0] / 255.0;
         planeLabel.color.g = color[1] / 255.0;
         planeLabel.color.b = color[2] / 255.0;
-        planeLabel.lifetime = ros::Duration();
+        planeLabel.lifetime = rclcpp::Duration::from_seconds(0);
         planeLabel.pose.position.x = centroid.x();
         planeLabel.pose.position.z = centroid.z();
         planeLabel.pose.position.y = centroid.y() - 1.5;
         planeLabel.header.frame_id = frameBC;
-        planeLabel.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+        planeLabel.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
 
         // Create a marker for the plane normal (as an arrow)
         planeNormal.color.a = 1.0;
@@ -876,13 +1120,13 @@ void publishPlanes(std::vector<ORB_SLAM3::Plane *> planes, ros::Time msgTime)
         planeNormal.scale.z = 0.05; // Arrowhead length
         planeNormal.ns = "planeNormal";
         planeNormal.header.stamp = msgTime;
-        planeNormal.lifetime = ros::Duration();
+        planeNormal.lifetime = rclcpp::Duration::from_seconds(0);
         planeNormal.color.r = color[0] / 255.0;
         planeNormal.color.g = color[1] / 255.0;
         planeNormal.color.b = color[2] / 255.0;
         planeNormal.id = plane->getId() + numPlanes;
         planeNormal.header.frame_id = frameBC;
-        planeNormal.type = visualization_msgs::Marker::ARROW;
+        planeNormal.type = visualization_msgs::msg::Marker::ARROW;
 
         // Clear previous points from planeNormal
         planeNormal.points.clear();
@@ -907,7 +1151,7 @@ void publishPlanes(std::vector<ORB_SLAM3::Plane *> planes, ros::Time msgTime)
         return;
 
     // convert the aggregated pointcloud to a pointcloud2 message
-    sensor_msgs::PointCloud2 cloud_msg;
+    sensor_msgs::msg::PointCloud2 cloud_msg;
     pcl::toROSMsg(*aggregatedCloud, cloud_msg);
 
     // Set message header
@@ -915,11 +1159,11 @@ void publishPlanes(std::vector<ORB_SLAM3::Plane *> planes, ros::Time msgTime)
     cloud_msg.header.frame_id = frameBC;
 
     // Publish the point cloud
-    pubPlanePointcloud.publish(cloud_msg);
-    pubPlaneLabel.publish(planeLabelArray);
+    pubPlanePointcloud->publish(cloud_msg);
+    pubPlaneLabel->publish(planeLabelArray);
 }
 
-void publishRooms(std::vector<ORB_SLAM3::Room *> rooms, ros::Time msgTime)
+void publishRooms(std::vector<ORB_SLAM3::Room *> rooms, rclcpp::Time msgTime)
 {
     // Publish rooms, if any
     int numRooms = rooms.size();
@@ -927,15 +1171,15 @@ void publishRooms(std::vector<ORB_SLAM3::Room *> rooms, ros::Time msgTime)
         return;
 
     // Visualization markers
-    visualization_msgs::MarkerArray roomArray;
+    visualization_msgs::msg::MarkerArray roomArray;
     roomArray.markers.resize(numRooms);
 
     for (int idx = 0; idx < numRooms; idx++)
     {
         // Variables
         string roomName = rooms[idx]->getName();
-        string definedRoom = "package://orb_slam3_ros/config/Assets/room.dae";
-        string undefinedRoom = "package://orb_slam3_ros/config/Assets/qmark.dae";
+        string definedRoom = "package://vs_graphs/config/Assets/room.dae";
+        string undefinedRoom = "package://vs_graphs/config/Assets/qmark.dae";
         string roomMesh = rooms[idx]->getHasKnownLabel() ? definedRoom : undefinedRoom;
 
         // Create color for room (orange for candidate, magenta for corridor, violet for normal room)
@@ -948,7 +1192,7 @@ void publishRooms(std::vector<ORB_SLAM3::Room *> rooms, ros::Time msgTime)
                 color = {0.5, 0.1, 1.0};
 
         Eigen::Vector3d roomCenter = rooms[idx]->getRoomCenter();
-        visualization_msgs::Marker room, roomWallLine, roomDoorLine, roomMarkerLine, roomLabel;
+        visualization_msgs::msg::Marker room, roomWallLine, roomDoorLine, roomMarkerLine, roomLabel;
 
         // Room values
         room.ns = "rooms";
@@ -961,12 +1205,12 @@ void publishRooms(std::vector<ORB_SLAM3::Room *> rooms, ros::Time msgTime)
         room.color.g = color[1];
         room.color.b = color[2];
         room.mesh_resource = roomMesh;
-        room.lifetime = ros::Duration();
+        room.lifetime = rclcpp::Duration::from_seconds(0);
         room.id = roomArray.markers.size();
-        room.header.stamp = ros::Time().now();
+        room.header.stamp = rclcpp::Clock().now(); //rclcpp::Time().now();
         room.mesh_use_embedded_materials = true;
         room.header.frame_id = frameSE;
-        room.type = visualization_msgs::Marker::MESH_RESOURCE;
+        room.type = visualization_msgs::msg::Marker::MESH_RESOURCE;
 
         // Rotation and displacement of the room for better visualization
         room.pose.orientation.x = 0.0;
@@ -987,14 +1231,14 @@ void publishRooms(std::vector<ORB_SLAM3::Room *> rooms, ros::Time msgTime)
         roomLabel.text = roomName;
         roomLabel.ns = "roomLabel";
         roomLabel.action = roomLabel.ADD;
-        roomLabel.lifetime = ros::Duration();
+        roomLabel.lifetime = rclcpp::Duration::from_seconds(0);
         roomLabel.id = roomArray.markers.size();
-        roomLabel.header.stamp = ros::Time().now();
+        roomLabel.header.stamp = rclcpp::Clock().now(); //rclcpp::Time().now();
         roomLabel.pose.position.x = roomCenter.x();
         roomLabel.pose.position.z = roomCenter.z();
         roomLabel.pose.position.y = roomCenter.y() - 0.7;
         roomLabel.header.frame_id = frameSE;
-        roomLabel.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+        roomLabel.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
         roomArray.markers.push_back(roomLabel);
 
         // Room to Plane (Wall) connection line
@@ -1007,11 +1251,11 @@ void publishRooms(std::vector<ORB_SLAM3::Room *> rooms, ros::Time msgTime)
         roomWallLine.scale.z = 0.04;
         roomWallLine.ns = "room_wall_lines";
         roomWallLine.action = roomWallLine.ADD;
-        roomWallLine.lifetime = ros::Duration();
+        roomWallLine.lifetime = rclcpp::Duration::from_seconds(0);
         roomWallLine.id = roomArray.markers.size();
-        roomWallLine.header.stamp = ros::Time().now();
+        roomWallLine.header.stamp = rclcpp::Clock().now(); //rclcpp::Time().now();
         roomWallLine.header.frame_id = world_frame_id;
-        roomWallLine.type = visualization_msgs::Marker::LINE_LIST;
+        roomWallLine.type = visualization_msgs::msg::Marker::LINE_LIST;
 
         // Room to Door connection line
         roomDoorLine.color.a = 0.5;
@@ -1023,11 +1267,11 @@ void publishRooms(std::vector<ORB_SLAM3::Room *> rooms, ros::Time msgTime)
         roomDoorLine.scale.z = 0.005;
         roomDoorLine.ns = "room_door_lines";
         roomDoorLine.action = roomDoorLine.ADD;
-        roomDoorLine.lifetime = ros::Duration();
-        roomDoorLine.header.stamp = ros::Time().now();
+        roomDoorLine.lifetime = rclcpp::Duration::from_seconds(0);
+        roomDoorLine.header.stamp = rclcpp::Clock().now(); //rclcpp::Time().now();
         roomDoorLine.header.frame_id = world_frame_id;
         roomDoorLine.id = roomArray.markers.size() + 1;
-        roomDoorLine.type = visualization_msgs::Marker::LINE_LIST;
+        roomDoorLine.type = visualization_msgs::msg::Marker::LINE_LIST;
 
         // Room to Marker connection line
         roomMarkerLine.color.a = 0.5;
@@ -1038,96 +1282,209 @@ void publishRooms(std::vector<ORB_SLAM3::Room *> rooms, ros::Time msgTime)
         roomMarkerLine.scale.y = 0.005;
         roomMarkerLine.scale.z = 0.005;
         roomMarkerLine.ns = "room_marker_lines";
-        roomMarkerLine.lifetime = ros::Duration();
+        roomMarkerLine.lifetime = rclcpp::Duration::from_seconds(0);
         roomMarkerLine.action = roomMarkerLine.ADD;
-        roomMarkerLine.header.stamp = ros::Time().now();
+        roomMarkerLine.header.stamp = rclcpp::Clock().now(); //rclcpp::Time().now();
         roomMarkerLine.header.frame_id = world_frame_id;
         roomMarkerLine.id = roomArray.markers.size() + 1;
-        roomMarkerLine.type = visualization_msgs::Marker::LINE_LIST;
-
-        // Get the room center in the world frame
-        tf::Stamped<tf::Point> roomPoint, roomPointTransformed;
-
-        roomPoint.setX(roomCenter.x());
-        roomPoint.setY(roomCenter.y());
-        roomPoint.setZ(roomCenter.z());
-        roomPoint.frame_id_ = frameSE;
-        transformListener->transformPoint(world_frame_id, ros::Time(0), roomPoint,
-                                          frameSE, roomPointTransformed);
+        roomMarkerLine.type = visualization_msgs::msg::Marker::LINE_LIST;
+
+       // Get the room center in the world frame
+        // tf::Stamped<tf::Point> roomPoint, roomPointTransformed;
+
+        // roomPoint.setX(roomCenter.x());
+        // roomPoint.setY(roomCenter.y());
+        // roomPoint.setZ(roomCenter.z());
+        // roomPoint.frame_id_ = frameSE;
+        // transformListener->transformPoint(world_frame_id, ros::Time(0), roomPoint,
+        //                                   frameSE, roomPointTransformed);
+
+        geometry_msgs::msg::PointStamped roomPoint, roomPointTransformed;
+        roomPoint.header.frame_id = frameSE;
+        roomPoint.header.stamp = rclcpp::Time(0);
+        roomPoint.point.x = roomCenter.x();
+        roomPoint.point.y = roomCenter.y();
+        roomPoint.point.z = roomCenter.z();
+
+        try {
+            // Use tfBuffer_ for the transform, with timeout 100ms
+            // auto tf_stamped = tfBuffer_->lookupTransform(
+            //     world_frame_id, frameSE, tf2::TimePointZero, rclcpp::Duration::from_seconds(0.1));
+            auto tf_stamped = tfBuffer_->lookupTransform(
+                world_frame_id, frameSE, rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1));
+            tf2::doTransform(roomPoint, roomPointTransformed, tf_stamped);
+        } catch (tf2::TransformException &ex) {
+            RCLCPP_WARN(rclcpp::get_logger("visual_sgraphs"), "Could not transform room center: %s", ex.what());
+            roomPointTransformed = roomPoint; // fallback: use original point
+        }
 
         // Room to Plane (Wall) connection line
         for (const auto wall : rooms[idx]->getWalls())
         {
-            geometry_msgs::Point pointRoom, pointWall;
-            tf::Stamped<tf::Point> pointWallInit, pointWallTransform;
+            geometry_msgs::msg::Point pointRoom, pointWall;
+            geometry_msgs::msg::PointStamped wallPoint, wallPointTransformed;
 
-            pointRoom.x = roomPointTransformed.x();
-            pointRoom.y = roomPointTransformed.y();
-            pointRoom.z = roomPointTransformed.z();
+            pointRoom.x = roomPointTransformed.point.x;
+            pointRoom.y = roomPointTransformed.point.y;
+            pointRoom.z = roomPointTransformed.point.z;
             roomWallLine.points.push_back(pointRoom);
 
-            pointWallInit.frame_id_ = frameBC;
-            pointWallInit.setX(wall->getCentroid().x());
-            pointWallInit.setY(wall->getCentroid().y());
-            pointWallInit.setZ(wall->getCentroid().z());
-            transformListener->transformPoint(world_frame_id, ros::Time(0), pointWallInit,
-                                              frameBC, pointWallTransform);
+            wallPoint.header.frame_id = frameBC;
+            wallPoint.header.stamp = rclcpp::Time(0);
+            wallPoint.point.x = wall->getCentroid().x();
+            wallPoint.point.y = wall->getCentroid().y();
+            wallPoint.point.z = wall->getCentroid().z();
+
+            try {
+                auto tf_stamped = tfBuffer_->lookupTransform(
+                    world_frame_id, frameBC, tf2::TimePointZero, tf2::durationFromSec(0.1));
+                tf2::doTransform(wallPoint, wallPointTransformed, tf_stamped);
+            } catch (tf2::TransformException &ex) {
+                RCLCPP_WARN(rclcpp::get_logger("visual_sgraphs"), "Could not transform wall centroid: %s", ex.what());
+                wallPointTransformed = wallPoint;
+            }
 
-            pointWall.x = pointWallTransform.x();
-            pointWall.y = pointWallTransform.y();
-            pointWall.z = pointWallTransform.z();
+            pointWall.x = wallPointTransformed.point.x;
+            pointWall.y = wallPointTransformed.point.y;
+            pointWall.z = wallPointTransformed.point.z;
             roomWallLine.points.push_back(pointWall);
         }
+        // for (const auto wall : rooms[idx]->getWalls())
+        // {
+        //     geometry_msgs::Point pointRoom, pointWall;
+        //     tf::Stamped<tf::Point> pointWallInit, pointWallTransform;
+
+        //     pointRoom.x = roomPointTransformed.x();
+        //     pointRoom.y = roomPointTransformed.y();
+        //     pointRoom.z = roomPointTransformed.z();
+        //     roomWallLine.points.push_back(pointRoom);
+
+        //     pointWallInit.frame_id_ = frameBC;
+        //     pointWallInit.setX(wall->getCentroid().x());
+        //     pointWallInit.setY(wall->getCentroid().y());
+        //     pointWallInit.setZ(wall->getCentroid().z());
+        //     transformListener->transformPoint(world_frame_id, ros::Time(0), pointWallInit,
+        //                                       frameBC, pointWallTransform);
+
+        //     pointWall.x = pointWallTransform.x();
+        //     pointWall.y = pointWallTransform.y();
+        //     pointWall.z = pointWallTransform.z();
+        //     roomWallLine.points.push_back(pointWall);
+        // }
 
+        // Room to Door connection line
         // Room to Door connection line
         for (const auto door : rooms[idx]->getDoors())
         {
-            geometry_msgs::Point pointRoom, pointDoor;
-            tf::Stamped<tf::Point> pointDoorInit, pointDoorTransform;
+            geometry_msgs::msg::Point pointRoom, pointDoor;
+            geometry_msgs::msg::PointStamped doorPoint, doorPointTransformed;
 
-            pointRoom.x = roomPointTransformed.x();
-            pointRoom.y = roomPointTransformed.y();
-            pointRoom.z = roomPointTransformed.z();
+            pointRoom.x = roomPointTransformed.point.x;
+            pointRoom.y = roomPointTransformed.point.y;
+            pointRoom.z = roomPointTransformed.point.z;
             roomDoorLine.points.push_back(pointRoom);
 
-            pointDoorInit.frame_id_ = frameBC;
-            pointDoorInit.setX(door->getGlobalPose().translation()(0));
-            pointDoorInit.setY(door->getGlobalPose().translation()(1));
-            pointDoorInit.setZ(door->getGlobalPose().translation()(2));
-            transformListener->transformPoint(world_frame_id, ros::Time(0), pointDoorInit,
-                                              frameBC, pointDoorTransform);
+            doorPoint.header.frame_id = frameBC;
+            doorPoint.header.stamp = rclcpp::Time(0);
+            doorPoint.point.x = door->getGlobalPose().translation()(0);
+            doorPoint.point.y = door->getGlobalPose().translation()(1);
+            doorPoint.point.z = door->getGlobalPose().translation()(2);
+
+            try {
+                auto tf_stamped = tfBuffer_->lookupTransform(
+                    world_frame_id, frameBC, tf2::TimePointZero, tf2::durationFromSec(0.1));
+                tf2::doTransform(doorPoint, doorPointTransformed, tf_stamped);
+            } catch (tf2::TransformException &ex) {
+                RCLCPP_WARN(rclcpp::get_logger("visual_sgraphs"), "Could not transform door centroid: %s", ex.what());
+                doorPointTransformed = doorPoint;
+            }
 
-            pointDoor.x = pointDoorTransform.x();
-            pointDoor.z = pointDoorTransform.z();
-            pointDoor.y = pointDoorTransform.y() - 2.0;
+            pointDoor.x = doorPointTransformed.point.x;
+            pointDoor.y = doorPointTransformed.point.y - 2.0;
+            pointDoor.z = doorPointTransformed.point.z;
             roomDoorLine.points.push_back(pointDoor);
         }
 
         // Room to Marker connection line
-        geometry_msgs::Point pointRoom, pointMarker;
-        tf::Stamped<tf::Point> pointMarkerInit, pointMarkerTransform;
+        geometry_msgs::msg::Point pointRoom, pointMarker;
         ORB_SLAM3::Marker *metaMarker = rooms[idx]->getMetaMarker();
 
-        pointRoom.x = roomPointTransformed.x();
-        pointRoom.y = roomPointTransformed.y();
-        pointRoom.z = roomPointTransformed.z();
+        pointRoom.x = roomPointTransformed.point.x;
+        pointRoom.y = roomPointTransformed.point.y;
+        pointRoom.z = roomPointTransformed.point.z;
         roomMarkerLine.points.push_back(pointRoom);
+        // for (const auto door : rooms[idx]->getDoors())
+        // {
+        //     geometry_msgs::Point pointRoom, pointDoor;
+        //     tf::Stamped<tf::Point> pointDoorInit, pointDoorTransform;
+
+        //     pointRoom.x = roomPointTransformed.x();
+        //     pointRoom.y = roomPointTransformed.y();
+        //     pointRoom.z = roomPointTransformed.z();
+        //     roomDoorLine.points.push_back(pointRoom);
+
+        //     pointDoorInit.frame_id_ = frameBC;
+        //     pointDoorInit.setX(door->getGlobalPose().translation()(0));
+        //     pointDoorInit.setY(door->getGlobalPose().translation()(1));
+        //     pointDoorInit.setZ(door->getGlobalPose().translation()(2));
+        //     transformListener->transformPoint(world_frame_id, ros::Time(0), pointDoorInit,
+        //                                       frameBC, pointDoorTransform);
+
+        //     pointDoor.x = pointDoorTransform.x();
+        //     pointDoor.z = pointDoorTransform.z();
+        //     pointDoor.y = pointDoorTransform.y() - 2.0;
+        //     roomDoorLine.points.push_back(pointDoor);
+        // }
+
+        // // Room to Marker connection line
+        // geometry_msgs::Point pointRoom, pointMarker;
+        // tf::Stamped<tf::Point> pointMarkerInit, pointMarkerTransform;
+        // ORB_SLAM3::Marker *metaMarker = rooms[idx]->getMetaMarker();
+
+        // pointRoom.x = roomPointTransformed.x();
+        // pointRoom.y = roomPointTransformed.y();
+        // pointRoom.z = roomPointTransformed.z();
+        // roomMarkerLine.points.push_back(pointRoom);
 
         // In free space-based room candidate detection, the metaMarker is not available
-        if (metaMarker != nullptr)
+                if (metaMarker != nullptr)
         {
-            pointMarkerInit.frame_id_ = frameBC;
-            pointMarkerInit.setX(metaMarker->getGlobalPose().translation()(0));
-            pointMarkerInit.setY(metaMarker->getGlobalPose().translation()(1));
-            pointMarkerInit.setZ(metaMarker->getGlobalPose().translation()(2));
-            transformListener->transformPoint(world_frame_id, ros::Time(0), pointMarkerInit,
-                                              frameBC, pointMarkerTransform);
-
-            pointMarker.x = pointMarkerTransform.x();
-            pointMarker.z = pointMarkerTransform.z();
-            pointMarker.y = pointMarkerTransform.y();
+            geometry_msgs::msg::PointStamped pointMarkerInit, pointMarkerTransform;
+            pointMarkerInit.header.frame_id = frameBC;
+            pointMarkerInit.header.stamp = rclcpp::Time(0);
+            pointMarkerInit.point.x = metaMarker->getGlobalPose().translation()(0);
+            pointMarkerInit.point.y = metaMarker->getGlobalPose().translation()(1);
+            pointMarkerInit.point.z = metaMarker->getGlobalPose().translation()(2);
+
+            try {
+                auto tf_stamped = tfBuffer_->lookupTransform(
+                    world_frame_id, frameBC, tf2::TimePointZero, tf2::durationFromSec(0.1));
+                tf2::doTransform(pointMarkerInit, pointMarkerTransform, tf_stamped);
+            } catch (tf2::TransformException &ex) {
+                RCLCPP_WARN(rclcpp::get_logger("visual_sgraphs"), "Could not transform marker centroid: %s", ex.what());
+                pointMarkerTransform = pointMarkerInit;
+            }
+
+            geometry_msgs::msg::Point pointMarker;
+            pointMarker.x = pointMarkerTransform.point.x;
+            pointMarker.y = pointMarkerTransform.point.y;
+            pointMarker.z = pointMarkerTransform.point.z;
             roomMarkerLine.points.push_back(pointMarker);
         }
+        // if (metaMarker != nullptr)
+        // {
+        //     pointMarkerInit.frame_id_ = frameBC;
+        //     pointMarkerInit.setX(metaMarker->getGlobalPose().translation()(0));
+        //     pointMarkerInit.setY(metaMarker->getGlobalPose().translation()(1));
+        //     pointMarkerInit.setZ(metaMarker->getGlobalPose().translation()(2));
+        //     transformListener->transformPoint(world_frame_id, ros::Time(0), pointMarkerInit,
+        //                                       frameBC, pointMarkerTransform);
+
+        //     pointMarker.x = pointMarkerTransform.x();
+        //     pointMarker.z = pointMarkerTransform.z();
+        //     pointMarker.y = pointMarkerTransform.y();
+        //     roomMarkerLine.points.push_back(pointMarker);
+        // }
 
         // Add items to the roomArray
         roomArray.markers.push_back(roomWallLine);
@@ -1135,14 +1492,14 @@ void publishRooms(std::vector<ORB_SLAM3::Room *> rooms, ros::Time msgTime)
         roomArray.markers.push_back(roomMarkerLine);
     }
 
-    pubRoom.publish(roomArray);
+    pubRoom->publish(roomArray);
 }
 
-sensor_msgs::PointCloud2 mapPointToPointcloud(std::vector<ORB_SLAM3::MapPoint *> mapPoints, ros::Time msgTime)
+
+sensor_msgs::msg::PointCloud2 mapPointToPointcloud(std::vector<ORB_SLAM3::MapPoint *> mapPoints, rclcpp::Time msgTime)
 {
-    // Variables
     const int numChannels = 3;
-    sensor_msgs::PointCloud2 cloud;
+    sensor_msgs::msg::PointCloud2 cloud;
     std::string channelId[] = {"x", "y", "z"};
 
     // Set the attributes of the point cloud
@@ -1162,7 +1519,7 @@ sensor_msgs::PointCloud2 mapPointToPointcloud(std::vector<ORB_SLAM3::MapPoint *>
         cloud.fields[idx].count = 1;
         cloud.fields[idx].name = channelId[idx];
         cloud.fields[idx].offset = idx * sizeof(float);
-        cloud.fields[idx].datatype = sensor_msgs::PointField::FLOAT32;
+        cloud.fields[idx].datatype = sensor_msgs::msg::PointField::FLOAT32;
     }
 
     // Set the data of the point cloud
@@ -1171,24 +1528,73 @@ sensor_msgs::PointCloud2 mapPointToPointcloud(std::vector<ORB_SLAM3::MapPoint *>
 
     // Populate the point cloud with the map points
     for (unsigned int idx = 0; idx < cloud.width; idx++)
+    {
         if (mapPoints[idx] && !mapPoints[idx]->isBad())
         {
             Eigen::Vector3d P3Dw = mapPoints[idx]->GetWorldPos().cast<double>();
-
-            tf::Vector3 pointTranslation(P3Dw.x(), P3Dw.y(), P3Dw.z());
-
             float dataArray[numChannels] = {
-                pointTranslation.x(),
-                pointTranslation.y(),
-                pointTranslation.z()};
-
+                static_cast<float>(P3Dw.x()),
+                static_cast<float>(P3Dw.y()),
+                static_cast<float>(P3Dw.z())
+            };
             memcpy(cloudDataPtr + (idx * cloud.point_step), dataArray, numChannels * sizeof(float));
         }
+    }
 
-    // Return the point cloud
     return cloud;
 }
 
+// sensor_msgs::msg::mapPointToPointcloud(std::vector<ORB_SLAM3::MapPoint *> mapPoints, rclcpp::Time msgTime)
+// {
+//     // Variables
+//     const int numChannels = 3;
+//     sensor_msgs::msg::PointCloud2 cloud;
+//     std::string channelId[] = {"x", "y", "z"};
+
+//     // Set the attributes of the point cloud
+//     cloud.header.stamp = msgTime;
+//     cloud.header.frame_id = world_frame_id;
+//     cloud.height = 1;
+//     cloud.width = mapPoints.size();
+//     cloud.is_bigendian = false;
+//     cloud.is_dense = true;
+//     cloud.point_step = numChannels * sizeof(float);
+//     cloud.row_step = cloud.point_step * cloud.width;
+//     cloud.fields.resize(numChannels);
+
+//     // Set the fields of the point cloud
+//     for (int idx = 0; idx < numChannels; idx++)
+//     {
+//         cloud.fields[idx].count = 1;
+//         cloud.fields[idx].name = channelId[idx];
+//         cloud.fields[idx].offset = idx * sizeof(float);
+//         cloud.fields[idx].datatype = sensor_msgs::PointField::FLOAT32;
+//     }
+
+//     // Set the data of the point cloud
+//     cloud.data.resize(cloud.row_step * cloud.height);
+//     unsigned char *cloudDataPtr = &(cloud.data[0]);
+
+//     // Populate the point cloud with the map points
+//     for (unsigned int idx = 0; idx < cloud.width; idx++)
+//         if (mapPoints[idx] && !mapPoints[idx]->isBad())
+//         {
+//             Eigen::Vector3d P3Dw = mapPoints[idx]->GetWorldPos().cast<double>();
+
+//             tf::Vector3 pointTranslation(P3Dw.x(), P3Dw.y(), P3Dw.z());
+
+//             float dataArray[numChannels] = {
+//                 pointTranslation.x(),
+//                 pointTranslation.y(),
+//                 pointTranslation.z()};
+
+//             memcpy(cloudDataPtr + (idx * cloud.point_step), dataArray, numChannels * sizeof(float));
+//         }
+
+//     // Return the point cloud
+//     return cloud;
+// }
+
 cv::Mat SE3fToCvMat(Sophus::SE3f data)
 {
     cv::Mat cvMat;
@@ -1200,61 +1606,62 @@ cv::Mat SE3fToCvMat(Sophus::SE3f data)
     return cvMat;
 }
 
-tf::Transform SE3fToTFTransform(Sophus::SE3f data)
+// tf::Transform SE3fToTFTransform(Sophus::SE3f data)
+tf2::Transform SE3fToTFTransform(Sophus::SE3f data)
 {
     Eigen::Matrix3f rotMatrix = data.rotationMatrix();
     Eigen::Vector3f transVector = data.translation();
 
-    tf::Matrix3x3 rotationTF(
+    tf2::Matrix3x3 rotationTF(
         rotMatrix(0, 0), rotMatrix(0, 1), rotMatrix(0, 2),
         rotMatrix(1, 0), rotMatrix(1, 1), rotMatrix(1, 2),
         rotMatrix(2, 0), rotMatrix(2, 1), rotMatrix(2, 2));
 
-    tf::Vector3 translationTF(
+    tf2::Vector3 translationTF(
         transVector(0),
         transVector(1),
         transVector(2));
 
-    return tf::Transform(rotationTF, translationTF);
+    return tf2::Transform(rotationTF, translationTF);
 }
 
-void addMarkersToBuffer(const aruco_msgs::MarkerArray &markerArray)
-{
-    // The list of markers observed in the current frame
-    std::vector<ORB_SLAM3::Marker *> currentMarkers;
-
-    // Process the received marker array
-    for (const auto &marker : markerArray.markers)
-    {
-        // Access information of each passed ArUco marker
-        int markerId = marker.id;
-        double visitTime = marker.header.stamp.toSec();
-        geometry_msgs::Pose markerPose = marker.pose.pose;
-        geometry_msgs::Point markerPosition = markerPose.position;            // (x,y,z)
-        geometry_msgs::Quaternion markerOrientation = markerPose.orientation; // (x,y,z,w)
-
-        Eigen::Vector3f markerTranslation(markerPosition.x, markerPosition.y, markerPosition.z);
-        Eigen::Quaternionf markerQuaternion(markerOrientation.w, markerOrientation.x,
-                                            markerOrientation.y, markerOrientation.z);
-        Sophus::SE3f normalizedPose(markerQuaternion, markerTranslation);
-
-        // Create a marker object of the currently visited marker
-        ORB_SLAM3::Marker *currentMarker = new ORB_SLAM3::Marker();
-        currentMarker->setOpId(-1);
-        currentMarker->setId(markerId);
-        currentMarker->setTime(visitTime);
-        currentMarker->setMarkerInGMap(false);
-        currentMarker->setLocalPose(normalizedPose);
-        currentMarker->setMarkerType(ORB_SLAM3::Marker::markerVariant::UNKNOWN);
-
-        // Add it to the list of observed markers
-        currentMarkers.push_back(currentMarker);
-    }
-
-    // Add the new markers to the list of markers in buffer
-    if (currentMarkers.size() > 0)
-        markersBuffer.push_back(currentMarkers);
-}
+// void addMarkersToBuffer(const aruco_msgs::MarkerArray &markerArray)
+// {
+//     // The list of markers observed in the current frame
+//     std::vector<ORB_SLAM3::Marker *> currentMarkers;
+
+//     // Process the received marker array
+//     for (const auto &marker : markerArray.markers)
+//     {
+//         // Access information of each passed ArUco marker
+//         int markerId = marker.id;
+//         double visitTime = marker.header.stamprclcpp::Time::seconds();
+//         geometry_msgs::Pose markerPose = marker.pose.pose;
+//         geometry_msgs::msg::Point markerPosition = markerPose.position;            // (x,y,z)
+//         geometry_msgs::Quaternion markerOrientation = markerPose.orientation; // (x,y,z,w)
+
+//         Eigen::Vector3f markerTranslation(markerPosition.x, markerPosition.y, markerPosition.z);
+//         Eigen::Quaternionf markerQuaternion(markerOrientation.w, markerOrientation.x,
+//                                             markerOrientation.y, markerOrientation.z);
+//         Sophus::SE3f normalizedPose(markerQuaternion, markerTranslation);
+
+//         // Create a marker object of the currently visited marker
+//         ORB_SLAM3::Marker *currentMarker = new ORB_SLAM3::Marker();
+//         currentMarker->setOpId(-1);
+//         currentMarker->setId(markerId);
+//         currentMarker->setTime(visitTime);
+//         currentMarker->setMarkerInGMap(false);
+//         currentMarker->setLocalPose(normalizedPose);
+//         currentMarker->setMarkerType(ORB_SLAM3::Marker::markerVariant::UNKNOWN);
+
+//         // Add it to the list of observed markers
+//         currentMarkers.push_back(currentMarker);
+//     }
+
+//     // Add the new markers to the list of markers in buffer
+//     if (currentMarkers.size() > 0)
+//         markersBuffer.push_back(currentMarkers);
+// }
 
 std::pair<double, std::vector<ORB_SLAM3::Marker *>> findNearestMarker(double frameTimestamp)
 {
@@ -1275,7 +1682,7 @@ std::pair<double, std::vector<ORB_SLAM3::Marker *>> findNearestMarker(double fra
     return std::make_pair(minTimeDifference, matchedMarkers);
 }
 
-void setVoxbloxSkeletonCluster(const visualization_msgs::MarkerArray &skeletonArray)
+void setVoxbloxSkeletonCluster(const visualization_msgs::msg::MarkerArray &skeletonArray)
 {
     // Reset the buffer
     skeletonClusterPoints.clear();
@@ -1294,12 +1701,19 @@ void setVoxbloxSkeletonCluster(const visualization_msgs::MarkerArray &skeletonAr
                 for (const auto &point : skeleton.points)
                 {
                     // transform from map frame to world frame
-                    geometry_msgs::PointStamped pointIn, pointOut;
+                    geometry_msgs::msg::PointStamped pointIn, pointOut;
                     pointIn.header.frame_id = frameMap;
-                    pointIn.header.stamp = ros::Time(0);
+                    pointIn.header.stamp = rclcpp::Time(0);
                     pointIn.point = point;
-                    transformListener->transformPoint(world_frame_id, pointIn, pointOut);
-
+                    // transformListener->transformPoint(world_frame_id, pointIn, pointOut);
+                    try {
+                        auto tf_stamped = tfBuffer_->lookupTransform(
+                            world_frame_id, pointIn.header.frame_id, tf2::TimePointZero, tf2::durationFromSec(0.1));
+                        tf2::doTransform(pointIn, pointOut, tf_stamped);
+                    } catch (tf2::TransformException &ex) {
+                        RCLCPP_WARN(rclcpp::get_logger("visual_sgraphs"), "Could not transform skeleton cluster point: %s", ex.what());
+                        pointOut = pointIn; // fallback: use original point
+                    }
                     // Add the point to the cluster
                     Eigen::Vector3d newPoint(pointOut.point.x, pointOut.point.y, pointOut.point.z);
 
@@ -1316,7 +1730,7 @@ void setVoxbloxSkeletonCluster(const visualization_msgs::MarkerArray &skeletonAr
     pSLAM->setSkeletonCluster(skeletonClusterPoints);
 }
 
-void setGNNBasedRoomCandidates(const orb_slam3_ros::vSGraphs_AllDetectdetRooms &msgGNNRooms) {
+void setGNNBasedRoomCandidates(const orb_slam3_ros::msg::VSGraphsAllDetectdetRooms &msgGNNRooms) {
     // Reset the buffer
     gnnRoomCandidates.clear();
 
@@ -1331,7 +1745,7 @@ void setGNNBasedRoomCandidates(const orb_slam3_ros::vSGraphs_AllDetectdetRooms &
         newRoom->setHasKnownLabel(false);
 
         // Check if corridor (if the length of room.wallIds is 2)
-        bool isCorridor = (room.wallIds.size() == 2);
+        bool isCorridor = (room.wall_ids.size() == 2);
         newRoom->setIsCorridor(isCorridor);
 
         // [TODO] use room.wallIds to fill newRoom->setWalls
diff --git a/src/ros_mono.cc b/src/ros_mono.cc
index 43f43e87..5a3de5c5 100644
--- a/src/ros_mono.cc
+++ b/src/ros_mono.cc
@@ -30,7 +30,7 @@ class ImageGrabber
     ImageGrabber() {};
 
     void GrabImage(const sensor_msgs::ImageConstPtr &msg);
-    void GrabArUcoMarker(const aruco_msgs::MarkerArray &msg);
+    // void GrabArUcoMarker(const aruco_msgs::MarkerArray &msg);
     void GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSegImage);
     void GrabVoxbloxSkeletonGraph(const visualization_msgs::MarkerArray &msgSkeletonGraph);
 };
@@ -91,8 +91,8 @@ int main(int argc, char **argv)
     ros::Subscriber sub_img = nodeHandler.subscribe("/camera/image_raw", 500, &ImageGrabber::GrabImage, &igb);
 
     // Subscribe to the markers detected by `aruco_ros` library
-    ros::Subscriber sub_aruco = nodeHandler.subscribe("/aruco_marker_publisher/markers",
-                                                      1, &ImageGrabber::GrabArUcoMarker, &igb);
+    // ros::Subscriber sub_aruco = nodeHandler.subscribe("/aruco_marker_publisher/markers",
+    //                                                   1, &ImageGrabber::GrabArUcoMarker, &igb);
 
     // Subscriber for images obtained from the Semantic Segmentater
     ros::Subscriber sub_segmented_img = nodeHandler.subscribe("/camera/color/image_segment", 50,
@@ -143,15 +143,15 @@ void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr &msg)
     else
         Sophus::SE3f Tcw = pSLAM->TrackMonocular(cv_ptr->image, cv_ptr->header.stamp.toSec());
 
-    ros::Time msg_time = msg->header.stamp;
+    rclcpp::Time msg_time = msg->header.stamp;
     publishTopics(msg_time);
 }
 
-void ImageGrabber::GrabArUcoMarker(const aruco_msgs::MarkerArray &markerArray)
-{
-    // Pass the visited markers to a buffer to be processed later
-    addMarkersToBuffer(markerArray);
-}
+// void ImageGrabber::GrabArUcoMarker(const aruco_msgs::MarkerArray &markerArray)
+// {
+//     // Pass the visited markers to a buffer to be processed later
+//     // addMarkersToBuffer(markerArray);
+// }
 
 void ImageGrabber::GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSegImage)
 {
diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc
index be5e59ea..66bc89f8 100644
--- a/src/ros_mono_inertial.cc
+++ b/src/ros_mono_inertial.cc
@@ -43,7 +43,7 @@ class ImageGrabber
     void SyncWithImu();
 
     void GrabImage(const sensor_msgs::ImageConstPtr &msg);
-    void GrabArUcoMarker(const aruco_msgs::MarkerArray &msg);
+    // void GrabArUcoMarker(const aruco_msgs::MarkerArray &msg);
     cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg);
     void GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSegImage);
     void GrabVoxbloxSkeletonGraph(const visualization_msgs::MarkerArray &msgSkeletonGraph);
@@ -117,8 +117,8 @@ int main(int argc, char **argv)
     ros::Subscriber sub_img = nodeHandler.subscribe("/camera/image_raw", 500, &ImageGrabber::GrabImage, &igb);
 
     // Subscribe to the markers detected by `aruco_ros` library
-    ros::Subscriber sub_aruco = nodeHandler.subscribe("/aruco_marker_publisher/markers", 1,
-                                                      &ImageGrabber::GrabArUcoMarker, &igb);
+    // ros::Subscriber sub_aruco = nodeHandler.subscribe("/aruco_marker_publisher/markers", 1,
+    //                                                   &ImageGrabber::GrabArUcoMarker, &igb);
 
     // Subscriber for images obtained from the Semantic Segmentater
     ros::Subscriber sub_segmented_img = nodeHandler.subscribe("/camera/color/image_segment", 50,
@@ -197,7 +197,7 @@ void ImageGrabber::SyncWithImu()
 
             this->mBufMutex.lock();
             im = GetImage(img0Buf.front());
-            ros::Time msg_time = img0Buf.front()->header.stamp;
+            rclcpp::Time msg_time = img0Buf.front()->header.stamp;
             img0Buf.pop();
             this->mBufMutex.unlock();
 
@@ -247,11 +247,11 @@ void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg)
     return;
 }
 
-void ImageGrabber::GrabArUcoMarker(const aruco_msgs::MarkerArray &markerArray)
-{
-    // Pass the visited markers to a buffer to be processed later
-    addMarkersToBuffer(markerArray);
-}
+// void ImageGrabber::GrabArUcoMarker(const aruco_msgs::MarkerArray &markerArray)
+// {
+//     // Pass the visited markers to a buffer to be processed later
+//     // addMarkersToBuffer(markerArray);
+// }
 
 void ImageGrabber::GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSegImage)
 {
diff --git a/src/ros_rgbd.cc b/src/ros_rgbd.cc
index 020d9dac..048599fe 100644
--- a/src/ros_rgbd.cc
+++ b/src/ros_rgbd.cc
@@ -24,117 +24,148 @@
 
 using namespace std;
 
-class ImageGrabber
+class ImageGrabber : public rclcpp::Node
 {
 public:
-    ImageGrabber() {};
-
-    void GrabArUcoMarker(const aruco_msgs::MarkerArray &msg);
-    void GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSegImage);
-    void GrabVoxbloxSkeletonGraph(const visualization_msgs::MarkerArray &msgSkeletonGraph);
-    void GrabGNNRoomCandidates(const orb_slam3_ros::vSGraphs_AllDetectdetRooms &msgGNNRooms);
-    void GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sensor_msgs::ImageConstPtr &msgD,
-                  const sensor_msgs::PointCloud2ConstPtr &msgPC);
+    ImageGrabber() : rclcpp::Node("image_grabber") {}
+
+    // void GrabArUcoMarker(const aruco_msgs::MarkerArray &msg);
+    // void GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsgg &msgSegImage);
+    // void GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msgSkeletonGraph);
+    // void GrabGNNRoomCandidates(const orb_slam3_ros::msg::VSGraphsAllDetectdetRooms &msgGNNRooms);
+    // void GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sensor_msgs::ImageConstPtr &msgD,
+    //               const sensor_msgs::PointCloud2ConstPtr &msgPC);
+    void GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg &msgSegImage);
+    void GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msgSkeletonGraph);
+    void GrabGNNRoomCandidates(const orb_slam3_ros::msg::VSGraphsAllDetectdetRooms &msgGNNRooms);
+    void GrabRGBD(const sensor_msgs::msg::Image::ConstSharedPtr &msgRGB, const sensor_msgs::msg::Image::ConstSharedPtr &msgD,
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msgPC);
 };
 
-int main(int argc, char **argv)
+int main(int argc, char **argv) // Changed with Copilot's help
 {
-    ros::init(argc, argv, "RGBD");
-    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
+    // ros::init(argc, argv, "RGBD");
+    // ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<rclcpp::Node>("RGBD");
 
     if (argc > 1)
-        ROS_WARN("Arguments supplied via command line are ignored.");
-
-    std::string nodeName = ros::this_node::getName();
-
-    ros::NodeHandle nodeHandler;
-    image_transport::ImageTransport imgTransport(nodeHandler);
-
-    std::string vocFile, settingsFile, sysParamsFile;
-    nodeHandler.param<std::string>(nodeName + "/voc_file", vocFile, "file_not_set");
-    nodeHandler.param<std::string>(nodeName + "/settings_file", settingsFile, "file_not_set");
-    nodeHandler.param<std::string>(nodeName + "/sys_params_file", sysParamsFile, "file_not_set");
+        RCLCPP_WARN(node->get_logger(), "Arguments supplied via command line are ignored.");
+
+    std::string nodeName = node->get_name();
+
+    // Parameter handling in ROS2
+    node->declare_parameter<std::string>("voc_file", "file_not_set");
+    node->declare_parameter<std::string>("settings_file", "file_not_set");
+    node->declare_parameter<std::string>("sys_params_file", "file_not_set");
+    node->declare_parameter<bool>("enable_pangolin", true);
+    node->declare_parameter<double>("yaw", 0.0);
+    node->declare_parameter<double>("roll", 0.0);
+    node->declare_parameter<double>("pitch", 0.0);
+    node->declare_parameter<std::string>("frame_map", "map");
+    node->declare_parameter<bool>("colored_pointcloud", true);
+    node->declare_parameter<bool>("publish_pointclouds", true);
+    node->declare_parameter<std::string>("cam_frame_id", "camera");
+    node->declare_parameter<std::string>("world_frame_id", "world");
+    node->declare_parameter<std::string>("frame_building_component", "plane");
+    node->declare_parameter<std::string>("frame_structural_element", "room");
+    node->declare_parameter<bool>("static_transform", false);
+
+    std::string vocFile = node->get_parameter("voc_file").as_string();
+    std::string settingsFile = node->get_parameter("settings_file").as_string();
+    std::string sysParamsFile = node->get_parameter("sys_params_file").as_string();
 
     if (vocFile == "file_not_set" || settingsFile == "file_not_set")
     {
-        ROS_ERROR("[Error] 'vocabulary' and 'settings' are not provided in the launch file! Exiting...");
-        ros::shutdown();
+        RCLCPP_ERROR(node->get_logger(), "[Error] 'vocabulary' and 'settings' are not provided in the launch file! Exiting...");
+        rclcpp::shutdown();
         return 1;
     }
 
     if (sysParamsFile == "file_not_set")
     {
-        ROS_ERROR("[Error] The `YAML` file containing system parameters is not provided in the launch file! Exiting...");
-        ros::shutdown();
+        RCLCPP_ERROR(node->get_logger(), "[Error] The `YAML` file containing system parameters is not provided in the launch file! Exiting...");
+        rclcpp::shutdown();
         return 1;
     }
 
-    bool enablePangolin;
-    nodeHandler.param<bool>(nodeName + "/enable_pangolin", enablePangolin, true);
-
-    nodeHandler.param<double>(nodeName + "/yaw", yaw, 0.0);
-    nodeHandler.param<double>(nodeName + "/roll", roll, 0.0);
-    nodeHandler.param<double>(nodeName + "/pitch", pitch, 0.0);
-
-    nodeHandler.param<std::string>(nodeName + "/frame_map", frameMap, "map");
-    nodeHandler.param<bool>(nodeName + "/colored_pointcloud", colorPointcloud, true);
-    nodeHandler.param<bool>(nodeName + "/publish_pointclouds", pubPointClouds, true);
-    nodeHandler.param<std::string>(nodeName + "/cam_frame_id", cam_frame_id, "camera");
-    nodeHandler.param<std::string>(nodeName + "/world_frame_id", world_frame_id, "world");
-    nodeHandler.param<std::string>(nodeName + "/frame_building_component", frameBC, "plane");
-    nodeHandler.param<std::string>(nodeName + "/frame_structural_element", frameSE, "room");
-    nodeHandler.param<bool>(nodeName + "/static_transform", pubStaticTransform, false);
+    bool enablePangolin = node->get_parameter("enable_pangolin").as_bool();
+    yaw = node->get_parameter("yaw").as_double();
+    roll = node->get_parameter("roll").as_double();
+    pitch = node->get_parameter("pitch").as_double();
+    frameMap = node->get_parameter("frame_map").as_string();
+    colorPointcloud = node->get_parameter("colored_pointcloud").as_bool();
+    pubPointClouds = node->get_parameter("publish_pointclouds").as_bool();
+    cam_frame_id = node->get_parameter("cam_frame_id").as_string();
+    world_frame_id = node->get_parameter("world_frame_id").as_string();
+    frameBC = node->get_parameter("frame_building_component").as_string();
+    frameSE = node->get_parameter("frame_structural_element").as_string();
+    pubStaticTransform = node->get_parameter("static_transform").as_bool();
 
     // Initializing system threads and getting ready to process frames
-    ImageGrabber igb;
-    sensorType = ORB_SLAM3::System::RGBD;
+    auto igb = std::make_shared<ImageGrabber>();
 
+    sensorType = ORB_SLAM3::System::RGBD;
     pSLAM = new ORB_SLAM3::System(vocFile, settingsFile, sysParamsFile, sensorType, enablePangolin);
 
-    // Subscribe to get raw images
-    message_filters::Subscriber<sensor_msgs::Image> subImgRGB(nodeHandler, "/camera/rgb/image_raw", 500);
-    message_filters::Subscriber<sensor_msgs::Image> subImgDepth(nodeHandler, "/camera/depth_registered/image_raw", 500);
-
-    // Subscribe to get pointcloud from the depth sensor
-    message_filters::Subscriber<sensor_msgs::PointCloud2> subPointcloud(nodeHandler, "/camera/depth/points", 500);
-
-    // Synchronization of raw and depth images
-    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::PointCloud2>
-        syncPolicy;
-    message_filters::Synchronizer<syncPolicy> sync(syncPolicy(500), subImgRGB, subImgDepth, subPointcloud);
-    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD, &igb, _1, _2, _3));
-
-    // Subscribe to the markers detected by `aruco_ros` library
-    ros::Subscriber subAruco = nodeHandler.subscribe("/aruco_marker_publisher/markers", 1,
-                                                     &ImageGrabber::GrabArUcoMarker, &igb);
-
-    // Subscriber for images obtained from the Semantic Segmentater
-    ros::Subscriber subSegmentedImage = nodeHandler.subscribe("/camera/color/image_segment", 50,
-                                                              &ImageGrabber::GrabSegmentation, &igb);
-
-    // Subscriber to get the mesh from voxblox
-    ros::Subscriber subVoxbloxSkeletonMesh = nodeHandler.subscribe("/voxblox_skeletonizer/sparse_graph", 1,
-                                                                   &ImageGrabber::GrabVoxbloxSkeletonGraph, &igb);
-    
-    // Subscriber to get the room candidates detected by the GNN module
-    ros::Subscriber subGNNRooms = nodeHandler.subscribe("/gnn_room_detector", 1,
-                                                        &ImageGrabber::GrabGNNRoomCandidates, &igb);
-
-    setupPublishers(nodeHandler, imgTransport, nodeName);
-    setupServices(nodeHandler, nodeName);
-
-    ros::spin();
+    // Subscribe to get raw images (message_filters in ROS2)
+    using sensor_msgs::msg::Image;
+    using sensor_msgs::msg::PointCloud2;
+    using message_filters::Subscriber;
+    using message_filters::sync_policies::ApproximateTime;
+    using message_filters::Synchronizer;
+
+    auto subImgRGB = std::make_shared<Subscriber<Image>>(node.get(), "/camera/rgb/image_raw");
+    auto subImgDepth = std::make_shared<Subscriber<Image>>(node.get(), "/camera/depth_registered/image_raw");
+    auto subPointcloud = std::make_shared<Subscriber<PointCloud2>>(node.get(), "/camera/depth/points");
+
+    typedef ApproximateTime<Image, Image, PointCloud2> syncPolicy;
+    auto sync = std::make_shared<Synchronizer<syncPolicy>>(syncPolicy(10), *subImgRGB, *subImgDepth, *subPointcloud);
+    // sync->registerCallback(
+    //     [&igb](Image::ConstSharedPtr msgRGB, Image::ConstSharedPtr msgD, PointCloud2::ConstSharedPtr msgPC)
+    //     {
+    //         igb->GrabRGBD(msgRGB, msgD, msgPC);
+    //     }
+    // );
+    sync->registerCallback(
+        std::bind(&ImageGrabber::GrabRGBD, igb.get(),
+                std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)
+    );
+
+    // Subscribers for other topics
+    auto subSegmentedImage = node->create_subscription<segmenter_ros::msg::SegmenterDataMsg>(
+        "/camera/color/image_segment", 50,
+        [igb](const segmenter_ros::msg::SegmenterDataMsg::SharedPtr msg) { igb->GrabSegmentation(*msg); });
+
+    auto subVoxbloxSkeletonMesh = node->create_subscription<visualization_msgs::msg::MarkerArray>(
+        "/voxblox_skeletonizer/sparse_graph", 1,
+        [igb](const visualization_msgs::msg::MarkerArray::SharedPtr msg) { igb->GrabVoxbloxSkeletonGraph(*msg); });
+
+    auto subGNNRooms = node->create_subscription<orb_slam3_ros::msg::VSGraphsAllDetectdetRooms>(
+        "/gnn_room_detector", 1,
+        [igb](const orb_slam3_ros::msg::VSGraphsAllDetectdetRooms::SharedPtr msg) { igb->GrabGNNRoomCandidates(*msg); });
+
+    // TODO: Port setupPublishers and setupServices to ROS2 if needed
+    // setupPublishers(node, image_transport::ImageTransport(node), nodeName);
+    static std::shared_ptr<image_transport::ImageTransport> image_transport = std::make_shared<image_transport::ImageTransport>(node);
+    setupPublishers(node, image_transport, nodeName);
+
+    rclcpp::spin(node);
 
     // Stop all threads
     pSLAM->Shutdown();
-    ros::shutdown();
+    rclcpp::shutdown();
 
     return 0;
 }
-
-void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sensor_msgs::ImageConstPtr &msgD,
-                            const sensor_msgs::PointCloud2ConstPtr &msgPC)
+// void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sensor_msgs::ImageConstPtr &msgD,
+//                             const sensor_msgs::PointCloud2ConstPtr &msgPC)
+void ImageGrabber::GrabRGBD(const sensor_msgs::msg::Image::ConstSharedPtr &msgRGB,
+              const sensor_msgs::msg::Image::ConstSharedPtr &msgD,
+              const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msgPC)
 {
+    RCLCPP_WARN(this->get_logger(), "GrabRGBD callback triggered.");
+    
     // Variables
     cv_bridge::CvImageConstPtr cv_ptrD, cv_ptrRGB;
 
@@ -145,7 +176,8 @@ void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sens
     }
     catch (cv_bridge::Exception &e)
     {
-        ROS_ERROR("[Error] Problem occured while running `cv_bridge`: %s", e.what());
+        // ROS_ERROR("[Error] Problem occured while running `cv_bridge`: %s", e.what());
+        RCLCPP_ERROR(this->get_logger(), "[Error] Problem occured while running `cv_bridge`: %s", e.what());
         return;
     }
 
@@ -155,7 +187,7 @@ void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sens
 
     // Find the marker with the minimum time difference compared to the current frame
     std::pair<double, std::vector<ORB_SLAM3::Marker *>>
-        foundMarkerRes = findNearestMarker(cv_ptrRGB->header.stamp.toSec());
+        foundMarkerRes = findNearestMarker(cv_ptrRGB->header.stamp.sec + cv_ptrRGB->header.stamp.nanosec * 1e-9);
     double minMarkerTimeDiff = foundMarkerRes.first;
     std::vector<ORB_SLAM3::Marker *> matchedMarkers = foundMarkerRes.second;
 
@@ -163,17 +195,17 @@ void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sens
     if (minMarkerTimeDiff < 0.05)
     {
         pSLAM->TrackRGBD(cv_ptrRGB->image, cv_ptrD->image, cloud,
-                         cv_ptrRGB->header.stamp.toSec(),
+                         cv_ptrRGB->header.stamp.sec + cv_ptrRGB->header.stamp.nanosec * 1e-9,
                          {}, "", matchedMarkers);
         markersBuffer.clear();
     }
     else
     {
         pSLAM->TrackRGBD(cv_ptrRGB->image, cv_ptrD->image, cloud,
-                         cv_ptrRGB->header.stamp.toSec());
+                         cv_ptrRGB->header.stamp.sec + cv_ptrRGB->header.stamp.nanosec * 1e-9);
     }
 
-    ros::Time msgTime = cv_ptrRGB->header.stamp;
+    rclcpp::Time msgTime = cv_ptrRGB->header.stamp;
     publishTopics(msgTime);
 }
 
@@ -182,39 +214,43 @@ void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sens
  *
  * @param msgMarkerArray The markers detected by the `aruco_ros` library
  */
-void ImageGrabber::GrabArUcoMarker(const aruco_msgs::MarkerArray &msgMarkerArray)
-{
-    // Pass the visited markers to a buffer to be processed later
-    addMarkersToBuffer(msgMarkerArray);
-}
+// void ImageGrabber::GrabArUcoMarker(const aruco_msgs::MarkerArray &msgMarkerArray)
+// {
+//     // Pass the visited markers to a buffer to be processed later
+//     addMarkersToBuffer(msgMarkerArray);
+// }
 
 /**
  * @brief Callback function to get scene segmentation results from the SemanticSegmenter module
  *
  * @param msgSegImage The segmentation results from the SemanticSegmenter
  */
-void ImageGrabber::GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSegImage)
+void ImageGrabber::GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg &msgSegImage)
 {
     // Fetch the segmentation results
     cv_bridge::CvImageConstPtr cv_imgSeg;
-    uint64_t keyFrameId = msgSegImage.keyFrameId.data;
+    uint64_t key_frame_id = msgSegImage.key_frame_id.data;
 
     try
     {
-        cv_imgSeg = cv_bridge::toCvCopy(msgSegImage.segmentedImageUncertainty, sensor_msgs::image_encodings::BGR8);
+        // cv_imgSeg = cv_bridge::toCvCopy(msgSegImage.segmented_image_probability, sensor_msgs::image_encodings::BGR8);
+        // cv_imgSeg = cv_bridge::toCvCopy(std::make_shared<sensor_msgs::msg::Image>(msgSegImage.segmented_image_probability),  
+        cv_imgSeg = cv_bridge::toCvCopy(std::make_shared<sensor_msgs::msg::Image>(msgSegImage.segmented_image),  // <-- must be an Image
+                    sensor_msgs::image_encodings::BGR8);
     }
     catch (cv_bridge::Exception &e)
     {
-        ROS_ERROR("cv_bridge exception: %s", e.what());
+        // ROS_ERROR("cv_bridge exception: %s", e.what());
+        RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
         return;
     }
 
     // Convert to PCL PointCloud2 from `sensor_msgs` PointCloud2
     pcl::PCLPointCloud2::Ptr pclPc2SegPrb(new pcl::PCLPointCloud2);
-    pcl_conversions::toPCL(msgSegImage.segmentedImageProbability, *pclPc2SegPrb);
+    pcl_conversions::toPCL(msgSegImage.segmented_image_probability, *pclPc2SegPrb);
 
     // Create the tuple to be appended to the segmentedImageBuffer
-    std::tuple<uint64_t, cv::Mat, pcl::PCLPointCloud2::Ptr> tuple(keyFrameId, cv_imgSeg->image, pclPc2SegPrb);
+    std::tuple<uint64_t, cv::Mat, pcl::PCLPointCloud2::Ptr> tuple(key_frame_id, cv_imgSeg->image, pclPc2SegPrb);
 
     // Add the segmented image to a buffer to be processed in the SemanticSegmentation thread
     pSLAM->addSegmentedImage(&tuple);
@@ -225,7 +261,7 @@ void ImageGrabber::GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSe
  *
  * @param msgSkeletonGraphs The skeleton graph from the `voxblox` module
  */
-void ImageGrabber::GrabVoxbloxSkeletonGraph(const visualization_msgs::MarkerArray &msgSkeletonGraphs)
+void ImageGrabber::GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msgSkeletonGraphs)
 {
     // Pass the skeleton graph to a buffer to be processed by the SemanticSegmentation thread
     setVoxbloxSkeletonCluster(msgSkeletonGraphs);
@@ -236,7 +272,7 @@ void ImageGrabber::GrabVoxbloxSkeletonGraph(const visualization_msgs::MarkerArra
  *
  * @param msgGNNRooms The room candidates detected by the GNN module
  */
-void ImageGrabber::GrabGNNRoomCandidates(const orb_slam3_ros::vSGraphs_AllDetectdetRooms &msgGNNRooms)
+void ImageGrabber::GrabGNNRoomCandidates(const orb_slam3_ros::msg::VSGraphsAllDetectdetRooms &msgGNNRooms)
 {
     // Set the GNN room candidates in the SLAM system
     setGNNBasedRoomCandidates(msgGNNRooms);
diff --git a/src/ros_rgbd_inertial.cc b/src/ros_rgbd_inertial.cc
index aba934bd..1d5c71b0 100644
--- a/src/ros_rgbd_inertial.cc
+++ b/src/ros_rgbd_inertial.cc
@@ -24,15 +24,17 @@
 
 using namespace std;
 
-class ImuGrabber
+class ImageGrabber : public rclcpp::Node
 {
 public:
-    ImuGrabber() {};
+    ImageGrabber() : rclcpp::Node("image_grabber") {}
 
-    void GrabImu(const sensor_msgs::ImuConstPtr &imu_msg);
+    // void GrabImu(const sensor_msgs::ImuConstPtr &imu_msg);
+    void GrabImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
 
     std::mutex mBufMutex;
-    queue<sensor_msgs::ImuConstPtr> imuBuf;
+    // queue<sensor_msgs::ImuConstPtr> imuBuf;
+    queue<sensor_msgs::msg::Imu::ConstSharedPtr> imuBuf;
 };
 
 class ImageGrabber
@@ -42,67 +44,178 @@ class ImageGrabber
 
     void SyncWithImu();
 
-    void GrabArUcoMarker(const aruco_msgs::MarkerArray &msg);
-    cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg);
-    void GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSegImage);
-    void GrabVoxbloxSkeletonGraph(const visualization_msgs::MarkerArray &msgSkeletonGraph);
-    void GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sensor_msgs::ImageConstPtr &msgD,
-                  const sensor_msgs::PointCloud2ConstPtr &msgPC);
+    // void GrabArUcoMarker(const aruco_msgs::MarkerArray &msg);
+    // cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg);
+    // void GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsgg &msgSegImage);
+    // void GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msgSkeletonGraph);
+    // void GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sensor_msgs::ImageConstPtr &msgD,
+    //               const sensor_msgs::PointCloud2ConstPtr &msgPC);
+    cv::Mat GetImage(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg);
+    void GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg &msgSegImage);
+    void GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msgSkeletonGraph);
+    void GrabRGBD(const sensor_msgs::msg::Image::ConstSharedPtr &msgRGB,
+                  const sensor_msgs::msg::Image::ConstSharedPtr &msgD,
+                  const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msgPC);
 
     ImuGrabber *mpImuGb;
     std::mutex mBufMutex;
-    queue<sensor_msgs::ImageConstPtr> imgRGBBuf, imgDBuf;
-    queue<sensor_msgs::PointCloud2ConstPtr> imgPCBuf;
+    // queue<sensor_msgs::ImageConstPtr> imgRGBBuf, imgDBuf;
+    // queue<sensor_msgs::PointCloud2ConstPtr> imgPCBuf;
+    queue<sensor_msgs::msg::Image::ConstSharedPtr> imgRGBBuf, imgDBuf;
+    queue<sensor_msgs::msg::PointCloud2::ConstSharedPtr> imgPCBuf;
 };
 
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "RGBD_Inertial");
-    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
+// int main(int argc, char **argv)
+// {
+//     ros::init(argc, argv, "RGBD_Inertial");
+//     ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
 
-    if (argc > 1)
-    {
-        ROS_WARN("Arguments supplied via command line are ignored.");
-    }
+//     if (argc > 1)
+//     {
+//         ROS_WARN("Arguments supplied via command line are ignored.");
+//     }
+
+//     std::string node_name = ros::this_node::getName();
+
+//     ros::NodeHandle nodeHandler;
+//     image_transport::ImageTransport image_transport(nodeHandler);
+
+//     std::string voc_file, settings_file, sys_params_file;
+//     nodeHandler.param<std::string>(node_name + "/sys_params_file", sys_params_file, "file_not_set");
+//     nodeHandler.param<std::string>(node_name + "/voc_file", voc_file, "file_not_set");
+//     nodeHandler.param<std::string>(node_name + "/settings_file", settings_file, "file_not_set");
+
+//     if (voc_file == "file_not_set" || settings_file == "file_not_set")
+//     {
+//         ROS_ERROR("Please provide voc_file and settings_file in the launch file");
+//         ros::shutdown();
+//         return 1;
+//     }
+
+//     if (sys_params_file == "file_not_set")
+//     {
+//         ROS_ERROR("Please provide the YAML file containing system parameters in the launch file!");
+//         ros::shutdown();
+//         return 1;
+//     }
+
+//     bool enable_pangolin;
+//     nodeHandler.param<bool>(node_name + "/enable_pangolin", enable_pangolin, true);
+
+//     nodeHandler.param<double>(node_name + "/yaw", yaw, 0.0);
+//     nodeHandler.param<double>(node_name + "/roll", roll, 0.0);
+//     nodeHandler.param<double>(node_name + "/pitch", pitch, 0.0);
+
+//     nodeHandler.param<std::string>(node_name + "/frame_map", frameMap, "map");
+//     nodeHandler.param<std::string>(node_name + "/imu_frame_id", imu_frame_id, "imu");
+//     nodeHandler.param<std::string>(node_name + "/cam_frame_id", cam_frame_id, "camera");
+//     nodeHandler.param<std::string>(node_name + "/world_frame_id", world_frame_id, "world");
+//     nodeHandler.param<bool>(node_name + "/static_transform", pubStaticTransform, false);
+//     nodeHandler.param<std::string>(node_name + "/frame_building_component", frameBC, "plane");
+//     nodeHandler.param<std::string>(node_name + "/frame_structural_element", frameSE, "room");
+
+//     // Create SLAM system. It initializes all system threads and gets ready to process frames.
+//     ImuGrabber imugb;
+//     ImageGrabber igb(&imugb);
+//     sensorType = ORB_SLAM3::System::IMU_RGBD;
+
+//     pSLAM = new ORB_SLAM3::System(voc_file, settings_file, sys_params_file, sensorType, enable_pangolin);
+
+//     // Subscribe to get raw images and IMU data
+//     ros::Subscriber sub_imu = nodeHandler.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb);
+//     message_filters::Subscriber<sensor_msgs::Image> subImgRGB(nodeHandler, "/camera/rgb/image_raw", 500);
+//     message_filters::Subscriber<sensor_msgs::Image> subImgDepth(nodeHandler, "/camera/depth_registered/image_raw", 500);
+
+//     // Subscribe to get pointcloud from depth sensor
+//     message_filters::Subscriber<sensor_msgs::PointCloud2> subPointcloud(nodeHandler, "/camera/pointcloud", 500);
+
+//     // Synchronization of raw and depth images
+//     typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::PointCloud2>
+//         syncPolicy;
+//     message_filters::Synchronizer<syncPolicy> sync(syncPolicy(500), subImgRGB, subImgDepth, subPointcloud);
+//     sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD, &igb, _1, _2, _3));
+
+//     // Subscribe to the markers detected by `aruco_ros` library
+//     // ros::Subscriber sub_aruco = nodeHandler.subscribe("/aruco_marker_publisher/markers", 1,
+//     //                                                   &ImageGrabber::GrabArUcoMarker, &igb);
 
-    std::string node_name = ros::this_node::getName();
+//     // Subscriber for images obtained from the Semantic Segmentater
+//     ros::Subscriber sub_segmented_img = nodeHandler.subscribe("/camera/color/image_segment", 50,
+//                                                               &ImageGrabber::GrabSegmentation, &igb);
 
-    ros::NodeHandle nodeHandler;
-    image_transport::ImageTransport image_transport(nodeHandler);
+//     // Subscriber to get the mesh from voxblox
+//     ros::Subscriber voxblox_skeleton_mesh = nodeHandler.subscribe("/voxblox_skeletonizer/sparse_graph", 1,
+//                                                                   &ImageGrabber::GrabVoxbloxSkeletonGraph, &igb);
 
-    std::string voc_file, settings_file, sys_params_file;
-    nodeHandler.param<std::string>(node_name + "/sys_params_file", sys_params_file, "file_not_set");
-    nodeHandler.param<std::string>(node_name + "/voc_file", voc_file, "file_not_set");
-    nodeHandler.param<std::string>(node_name + "/settings_file", settings_file, "file_not_set");
+//     setupPublishers(nodeHandler, image_transport, node_name);
+//     setupServices(nodeHandler, node_name);
+
+//     // Syncing images with IMU
+//     std::thread sync_thread(&ImageGrabber::SyncWithImu, &igb);
+
+//     ros::spin();
+
+//     // Stop all threads
+//     pSLAM->Shutdown();
+//     ros::shutdown();
+
+//     return 0;
+// }
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<rclcpp::Node>("RGBD_Inertial");
+
+    if (argc > 1)
+        RCLCPP_WARN(node->get_logger(), "Arguments supplied via command line are ignored.");
+
+    std::string node_name = node->get_name();
+
+    // Declare and get parameters
+    node->declare_parameter<std::string>("sys_params_file", "file_not_set");
+    node->declare_parameter<std::string>("voc_file", "file_not_set");
+    node->declare_parameter<std::string>("settings_file", "file_not_set");
+    node->declare_parameter<bool>("enable_pangolin", true);
+    node->declare_parameter<double>("yaw", 0.0);
+    node->declare_parameter<double>("roll", 0.0);
+    node->declare_parameter<double>("pitch", 0.0);
+    node->declare_parameter<std::string>("frame_map", "map");
+    node->declare_parameter<std::string>("imu_frame_id", "imu");
+    node->declare_parameter<std::string>("cam_frame_id", "camera");
+    node->declare_parameter<std::string>("world_frame_id", "world");
+    node->declare_parameter<bool>("static_transform", false);
+    node->declare_parameter<std::string>("frame_building_component", "plane");
+    node->declare_parameter<std::string>("frame_structural_element", "room");
+
+    std::string sys_params_file = node->get_parameter("sys_params_file").as_string();
+    std::string voc_file = node->get_parameter("voc_file").as_string();
+    std::string settings_file = node->get_parameter("settings_file").as_string();
 
     if (voc_file == "file_not_set" || settings_file == "file_not_set")
     {
-        ROS_ERROR("Please provide voc_file and settings_file in the launch file");
-        ros::shutdown();
+        RCLCPP_ERROR(node->get_logger(), "Please provide voc_file and settings_file in the launch file");
+        rclcpp::shutdown();
         return 1;
     }
-
     if (sys_params_file == "file_not_set")
     {
-        ROS_ERROR("Please provide the YAML file containing system parameters in the launch file!");
-        ros::shutdown();
+        RCLCPP_ERROR(node->get_logger(), "Please provide the YAML file containing system parameters in the launch file!");
+        rclcpp::shutdown();
         return 1;
     }
 
-    bool enable_pangolin;
-    nodeHandler.param<bool>(node_name + "/enable_pangolin", enable_pangolin, true);
-
-    nodeHandler.param<double>(node_name + "/yaw", yaw, 0.0);
-    nodeHandler.param<double>(node_name + "/roll", roll, 0.0);
-    nodeHandler.param<double>(node_name + "/pitch", pitch, 0.0);
-
-    nodeHandler.param<std::string>(node_name + "/frame_map", frameMap, "map");
-    nodeHandler.param<std::string>(node_name + "/imu_frame_id", imu_frame_id, "imu");
-    nodeHandler.param<std::string>(node_name + "/cam_frame_id", cam_frame_id, "camera");
-    nodeHandler.param<std::string>(node_name + "/world_frame_id", world_frame_id, "world");
-    nodeHandler.param<bool>(node_name + "/static_transform", pubStaticTransform, false);
-    nodeHandler.param<std::string>(node_name + "/frame_building_component", frameBC, "plane");
-    nodeHandler.param<std::string>(node_name + "/frame_structural_element", frameSE, "room");
+    bool enable_pangolin = node->get_parameter("enable_pangolin").as_bool();
+    yaw = node->get_parameter("yaw").as_double();
+    roll = node->get_parameter("roll").as_double();
+    pitch = node->get_parameter("pitch").as_double();
+    frameMap = node->get_parameter("frame_map").as_string();
+    imu_frame_id = node->get_parameter("imu_frame_id").as_string();
+    cam_frame_id = node->get_parameter("cam_frame_id").as_string();
+    world_frame_id = node->get_parameter("world_frame_id").as_string();
+    pubStaticTransform = node->get_parameter("static_transform").as_bool();
+    frameBC = node->get_parameter("frame_building_component").as_string();
+    frameSE = node->get_parameter("frame_structural_element").as_string();
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
     ImuGrabber imugb;
@@ -111,49 +224,61 @@ int main(int argc, char **argv)
 
     pSLAM = new ORB_SLAM3::System(voc_file, settings_file, sys_params_file, sensorType, enable_pangolin);
 
-    // Subscribe to get raw images and IMU data
-    ros::Subscriber sub_imu = nodeHandler.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb);
-    message_filters::Subscriber<sensor_msgs::Image> subImgRGB(nodeHandler, "/camera/rgb/image_raw", 500);
-    message_filters::Subscriber<sensor_msgs::Image> subImgDepth(nodeHandler, "/camera/depth_registered/image_raw", 500);
-
-    // Subscribe to get pointcloud from depth sensor
-    message_filters::Subscriber<sensor_msgs::PointCloud2> subPointcloud(nodeHandler, "/camera/pointcloud", 500);
-
-    // Synchronization of raw and depth images
-    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::PointCloud2>
-        syncPolicy;
-    message_filters::Synchronizer<syncPolicy> sync(syncPolicy(500), subImgRGB, subImgDepth, subPointcloud);
-    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD, &igb, _1, _2, _3));
-
-    // Subscribe to the markers detected by `aruco_ros` library
-    ros::Subscriber sub_aruco = nodeHandler.subscribe("/aruco_marker_publisher/markers", 1,
-                                                      &ImageGrabber::GrabArUcoMarker, &igb);
+    // --- ROS2 Subscribers ---
+    using sensor_msgs::msg::Image;
+    using sensor_msgs::msg::Imu;
+    using sensor_msgs::msg::PointCloud2;
+    using message_filters::Subscriber;
+    using message_filters::sync_policies::ApproximateTime;
+    using message_filters::Synchronizer;
+
+    // IMU
+    auto sub_imu = node->create_subscription<Imu>(
+        "/imu", 1000,
+        [&imugb](const Imu::SharedPtr msg) { imugb.GrabImu(msg); });
+
+    // Images and pointcloud (message_filters)
+    auto subImgRGB = std::make_shared<Subscriber<Image>>(node.get(), "/camera/rgb/image_raw");
+    auto subImgDepth = std::make_shared<Subscriber<Image>>(node.get(), "/camera/depth_registered/image_raw");
+    auto subPointcloud = std::make_shared<Subscriber<PointCloud2>>(node.get(), "/camera/pointcloud");
+
+    typedef ApproximateTime<Image, Image, PointCloud2> syncPolicy;
+    auto sync = std::make_shared<Synchronizer<syncPolicy>>(syncPolicy(10), *subImgRGB, *subImgDepth, *subPointcloud);
+    sync->registerCallback(
+        [&igb](const Image::ConstSharedPtr msgRGB, const Image::ConstSharedPtr msgD, const PointCloud2::ConstSharedPtr msgPC)
+        {
+            igb.GrabRGBD(msgRGB, msgD, msgPC);
+        }
+    );
 
-    // Subscriber for images obtained from the Semantic Segmentater
-    ros::Subscriber sub_segmented_img = nodeHandler.subscribe("/camera/color/image_segment", 50,
-                                                              &ImageGrabber::GrabSegmentation, &igb);
+    // Segmentation and Voxblox
+    auto sub_segmented_img = node->create_subscription<segmenter_ros::msg::SegmenterDataMsg>(
+        "/camera/color/image_segment", 50,
+        [&igb](const segmenter_ros::msg::SegmenterDataMsg::SharedPtr msg) { igb.GrabSegmentation(*msg); });
 
-    // Subscriber to get the mesh from voxblox
-    ros::Subscriber voxblox_skeleton_mesh = nodeHandler.subscribe("/voxblox_skeletonizer/sparse_graph", 1,
-                                                                  &ImageGrabber::GrabVoxbloxSkeletonGraph, &igb);
+    auto voxblox_skeleton_mesh = node->create_subscription<visualization_msgs::msg::MarkerArray>(
+        "/voxblox_skeletonizer/sparse_graph", 1,
+        [&igb](const visualization_msgs::msg::MarkerArray::SharedPtr msg) { igb.GrabVoxbloxSkeletonGraph(*msg); });
 
-    setupPublishers(nodeHandler, image_transport, node_name);
-    setupServices(nodeHandler, node_name);
+    // TODO: setupPublishers and setupServices for ROS2 if needed
 
     // Syncing images with IMU
     std::thread sync_thread(&ImageGrabber::SyncWithImu, &igb);
 
-    ros::spin();
+    rclcpp::spin(node);
 
     // Stop all threads
     pSLAM->Shutdown();
-    ros::shutdown();
+    rclcpp::shutdown();
 
     return 0;
 }
 
-void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sensor_msgs::ImageConstPtr &msgD,
-                            const sensor_msgs::PointCloud2ConstPtr &msgPC)
+// void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sensor_msgs::ImageConstPtr &msgD,
+//                             const sensor_msgs::PointCloud2ConstPtr &msgPC)
+void GrabRGBD(const sensor_msgs::msg::Image::ConstSharedPtr &msgRGB,
+              const sensor_msgs::msg::Image::ConstSharedPtr &msgD,
+              const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msgPC)
 {
     mBufMutex.lock();
 
@@ -172,7 +297,7 @@ void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sens
     mBufMutex.unlock();
 }
 
-cv::Mat ImageGrabber::GetImage(const sensor_msgs::ImageConstPtr &img_msg)
+cv::Mat ImageGrabber::GetImage(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg)
 {
     // Copy the ros image message to cv::Mat.
     cv_bridge::CvImageConstPtr cv_ptr;
@@ -195,15 +320,15 @@ void ImageGrabber::SyncWithImu()
         if (!imgRGBBuf.empty() && !mpImuGb->imuBuf.empty())
         {
             cv::Mat im, depth;
-            sensor_msgs::PointCloud2ConstPtr msgPC;
+            sensor_msgs::msg::PointCloud2::ConstSharedPtr msgPC;
             double tIm = 0;
 
-            tIm = imgRGBBuf.front()->header.stamp.toSec();
-            if (tIm > mpImuGb->imuBuf.back()->header.stamp.toSec())
+            tIm = imgRGBBuf.front()->header.stamprclcpp::Time::seconds();
+            if (tIm > mpImuGb->imuBuf.back()->header.stamprclcpp::Time::seconds())
                 continue;
 
             this->mBufMutex.lock();
-            ros::Time msg_time = imgRGBBuf.front()->header.stamp;
+            rclcpp::Time msg_time = imgRGBBuf.front()->header.stamp;
             im = GetImage(imgRGBBuf.front());
             imgRGBBuf.pop();
             depth = GetImage(imgDBuf.front());
@@ -220,9 +345,9 @@ void ImageGrabber::SyncWithImu()
             if (!mpImuGb->imuBuf.empty())
             {
                 // Load imu measurements from buffer
-                while (!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.toSec() <= tIm)
+                while (!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamprclcpp::Time::seconds() <= tIm)
                 {
-                    double t = mpImuGb->imuBuf.front()->header.stamp.toSec();
+                    double t = mpImuGb->imuBuf.front()->header.stamprclcpp::Time::seconds();
                     cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z);
                     cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z);
                     vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc, gyr, t));
@@ -269,17 +394,17 @@ void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg)
     return;
 }
 
-void ImageGrabber::GrabArUcoMarker(const aruco_msgs::MarkerArray &markerArray)
-{
-    // Pass the visited markers to a buffer to be processed later
-    addMarkersToBuffer(markerArray);
-}
+// void ImageGrabber::GrabArUcoMarker(const aruco_msgs::MarkerArray &markerArray)
+// {
+//     // Pass the visited markers to a buffer to be processed later
+//     // addMarkersToBuffer(markerArray);
+// }
 
-void ImageGrabber::GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSegImage)
+void ImageGrabber::GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsgg &msgSegImage)
 {
     // Fetch the segmentation results
     cv_bridge::CvImageConstPtr cv_imgSeg;
-    uint64_t keyFrameId = msgSegImage.keyFrameId.data;
+    uint64_t key_frame_id = msgSegImage.key_frame_id.data;
 
     try
     {
@@ -296,13 +421,13 @@ void ImageGrabber::GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSe
     pcl_conversions::toPCL(msgSegImage.segmentedImageProbability, *pclPc2SegPrb);
 
     // Create the tuple to be appended to the segmentedImageBuffer
-    std::tuple<uint64_t, cv::Mat, pcl::PCLPointCloud2::Ptr> tuple(keyFrameId, cv_imgSeg->image, pclPc2SegPrb);
+    std::tuple<uint64_t, cv::Mat, pcl::PCLPointCloud2::Ptr> tuple(key_frame_id, cv_imgSeg->image, pclPc2SegPrb);
 
     // Add the segmented image to a buffer to be processed in the SemanticSegmentation thread
     pSLAM->addSegmentedImage(&tuple);
 }
 
-void ImageGrabber::GrabVoxbloxSkeletonGraph(const visualization_msgs::MarkerArray &msgSkeletonGraphs)
+void ImageGrabber::GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msgSkeletonGraphs)
 {
     // Pass the skeleton graph to a buffer to be processed by the SemanticSegmentation thread
     setVoxbloxSkeletonCluster(msgSkeletonGraphs);
diff --git a/src/ros_stereo.cc b/src/ros_stereo.cc
index f6a9814b..645fdbac 100644
--- a/src/ros_stereo.cc
+++ b/src/ros_stereo.cc
@@ -29,7 +29,7 @@ class ImageGrabber
 public:
     ImageGrabber() {};
 
-    void GrabArUcoMarker(const aruco_msgs::MarkerArray &msg);
+    // void GrabArUcoMarker(const aruco_msgs::MarkerArray &msg);
     void GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSegImage);
     void GrabVoxbloxSkeletonGraph(const visualization_msgs::MarkerArray &msgSkeletonGraph);
     void GrabStereo(const sensor_msgs::ImageConstPtr &msgLeft, const sensor_msgs::ImageConstPtr &msgRight);
@@ -99,8 +99,8 @@ int main(int argc, char **argv)
     sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo, &igb, _1, _2));
 
     // Subscribe to the markers detected by `aruco_ros` library
-    ros::Subscriber sub_aruco = nodeHandler.subscribe("/aruco_marker_publisher/markers", 1,
-                                                      &ImageGrabber::GrabArUcoMarker, &igb);
+    // ros::Subscriber sub_aruco = nodeHandler.subscribe("/aruco_marker_publisher/markers", 1,
+    //                                                   &ImageGrabber::GrabArUcoMarker, &igb);
 
     // Subscriber for images obtained from the Semantic Segmentater
     ros::Subscriber sub_segmented_img = nodeHandler.subscribe("/camera/color/image_segment", 50,
@@ -124,7 +124,7 @@ int main(int argc, char **argv)
 
 void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr &msgLeft, const sensor_msgs::ImageConstPtr &msgRight)
 {
-    ros::Time msg_time = msgLeft->header.stamp;
+    rclcpp::Time msg_time = msgLeft->header.stamp;
 
     // Copy the ros image message to cv::Mat.
     cv_bridge::CvImageConstPtr cv_ptrLeft, cv_ptrRight;
@@ -160,11 +160,11 @@ void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr &msgLeft, const s
     publishTopics(msg_time);
 }
 
-void ImageGrabber::GrabArUcoMarker(const aruco_msgs::MarkerArray &markerArray)
-{
-    // Pass the visited markers to a buffer to be processed later
-    addMarkersToBuffer(markerArray);
-}
+// void ImageGrabber::GrabArUcoMarker(const aruco_msgs::MarkerArray &markerArray)
+// {
+//     // Pass the visited markers to a buffer to be processed later
+//     // addMarkersToBuffer(markerArray);
+// }
 
 void ImageGrabber::GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSegImage)
 {
diff --git a/src/ros_stereo_inertial.cc b/src/ros_stereo_inertial.cc
index 7a926cd4..1a5ac0bb 100644
--- a/src/ros_stereo_inertial.cc
+++ b/src/ros_stereo_inertial.cc
@@ -42,7 +42,7 @@ class ImageGrabber
 
     void SyncWithImu();
 
-    void GrabArUcoMarker(const aruco_msgs::MarkerArray &msg);
+    // void GrabArUcoMarker(const aruco_msgs::MarkerArray &msg);
     void GrabImageLeft(const sensor_msgs::ImageConstPtr &msg);
     void GrabImageRight(const sensor_msgs::ImageConstPtr &msg);
     cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg);
@@ -117,8 +117,8 @@ int main(int argc, char **argv)
     ros::Subscriber sub_img_right = nodeHandler.subscribe("/camera/right/image_raw", 500, &ImageGrabber::GrabImageRight, &igb);
 
     // Subscribe to the markers detected by `aruco_ros` library
-    ros::Subscriber sub_aruco = nodeHandler.subscribe("/aruco_marker_publisher/markers", 1,
-                                                      &ImageGrabber::GrabArUcoMarker, &igb);
+    // ros::Subscriber sub_aruco = nodeHandler.subscribe("/aruco_marker_publisher/markers", 1,
+    //                                                   &ImageGrabber::GrabArUcoMarker, &igb);
 
     // Subscriber for images obtained from the Semantic Segmentater
     ros::Subscriber sub_segmented_img = nodeHandler.subscribe("/camera/color/image_segment", 50,
@@ -223,7 +223,7 @@ void ImageGrabber::SyncWithImu()
 
             this->mBufMutexLeft.lock();
             imLeft = GetImage(imgLeftBuf.front());
-            ros::Time msg_time = imgLeftBuf.front()->header.stamp;
+            rclcpp::Time msg_time = imgLeftBuf.front()->header.stamp;
             imgLeftBuf.pop();
             this->mBufMutexLeft.unlock();
 
@@ -290,11 +290,11 @@ void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg)
     return;
 }
 
-void ImageGrabber::GrabArUcoMarker(const aruco_msgs::MarkerArray &markerArray)
-{
-    // Pass the visited markers to a buffer to be processed later
-    addMarkersToBuffer(markerArray);
-}
+// void ImageGrabber::GrabArUcoMarker(const aruco_msgs::MarkerArray &markerArray)
+// {
+//     // Pass the visited markers to a buffer to be processed later
+//     // addMarkersToBuffer(markerArray);
+// }
 
 void ImageGrabber::GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSegImage)
 {