Porting rtabmap_costmap_plugins (Voxel Layer) to ROS2 (#1373)

* Porting rtabmap_costmap_plugins (Voxel Layer) to ROS2

* fixed voxel grid

* Ported voxel_marker

* ported patrol.py

rtabmap_costmap_plugins/CMakeLists.txt

@@ -0,0 +1,85 @@
+cmake_minimum_required(VERSION 3.5)
+project(rtabmap_costmap_plugins)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_ros REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(visualization_msgs REQUIRED)
+
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+
+SET(Libraries
+  pluginlib
+  rclcpp
+  nav2_costmap_2d
+  visualization_msgs
+)
+
+###########
+## Build ##
+###########
+
+add_library(rtabmap_costmap_plugins SHARED
+       src/voxel_layer.cpp
+)
+target_include_directories(rtabmap_costmap_plugins
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+IF("$ENV{ROS_DISTRO}" STRLESS "jazzy")
+  target_compile_definitions(rtabmap_costmap_plugins PRIVATE -DPRE_ROS_JAZZY)
+ENDIF()
+
+ament_target_dependencies(rtabmap_costmap_plugins ${Libraries})
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(rtabmap_costmap_plugins PRIVATE "RTABMAP_ROS_BUILDING_LIBRARY")
+
+# prevent pluginlib from using boost
+target_compile_definitions(rtabmap_costmap_plugins PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml)
+
+add_executable(rtabmap_costmap_voxel_marker src/voxel_marker.cpp)
+ament_target_dependencies(rtabmap_costmap_voxel_marker ${Libraries})
+set_target_properties(rtabmap_costmap_voxel_marker PROPERTIES OUTPUT_NAME "voxel_marker")
+
+#############
+## Install ##
+#############
+
+ament_export_dependencies(${Libraries})
+ament_export_include_directories(include)
+ament_export_targets(${PROJECT_NAME})           # To include downstream with targets
+ament_export_libraries(rtabmap_costmap_plugins) # To include downstream without targets
+
+install(TARGETS 
+   rtabmap_costmap_plugins
+   EXPORT ${PROJECT_NAME}
+   ARCHIVE DESTINATION lib
+   LIBRARY DESTINATION lib
+   RUNTIME DESTINATION bin
+   INCLUDES DESTINATION include
+)
+
+install(TARGETS 
+   rtabmap_costmap_voxel_marker 
+   DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+   DESTINATION include
+   FILES_MATCHING PATTERN "*.h"
+)
+
+ament_package()

rtabmap_costmap_plugins/costmap_plugins.xml

@@ -0,0 +1,5 @@
+<library path="rtabmap_costmap_plugins">
+  <class type="rtabmap_costmap_plugins::VoxelLayer" base_class_type="nav2_costmap_2d::Layer">
+    <description>Similar to nav2_costmap_2d::VoxelLayer, but can also move along z-axis.</description>
+  </class>
+</library>

rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/visibility.h

@@ -0,0 +1,58 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RTABMAP_COSTMAP_PLUGINS__VISIBILITY_CONTROL_H_
+#define RTABMAP_COSTMAP_PLUGINS__VISIBILITY_CONTROL_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define RTABMAP_COSTMAP_PLUGINS_EXPORT __attribute__ ((dllexport))
+    #define RTABMAP_COSTMAP_PLUGINS_IMPORT __attribute__ ((dllimport))
+  #else
+    #define RTABMAP_COSTMAP_PLUGINS_EXPORT __declspec(dllexport)
+    #define RTABMAP_COSTMAP_PLUGINS_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef RTABMAP_COSTMAP_PLUGINS_BUILDING_DLL
+    #define RTABMAP_COSTMAP_PLUGINS_PUBLIC RTABMAP_COSTMAP_PLUGINS_EXPORT
+  #else
+    #define RTABMAP_COSTMAP_PLUGINS_PUBLIC RTABMAP_COSTMAP_PLUGINS_IMPORT
+  #endif
+  #define RTABMAP_COSTMAP_PLUGINS_PUBLIC_TYPE RTABMAP_COSTMAP_PLUGINS_PUBLIC
+  #define RTABMAP_COSTMAP_PLUGINS_LOCAL
+#else
+  #define RTABMAP_COSTMAP_PLUGINS_EXPORT __attribute__ ((visibility("default")))
+  #define RTABMAP_COSTMAP_PLUGINS_IMPORT
+  #if __GNUC__ >= 4
+    #define RTABMAP_COSTMAP_PLUGINS_PUBLIC __attribute__ ((visibility("default")))
+    #define RTABMAP_COSTMAP_PLUGINS_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define RTABMAP_COSTMAP_PLUGINS_PUBLIC
+    #define RTABMAP_COSTMAP_PLUGINS_LOCAL
+  #endif
+  #define RTABMAP_COSTMAP_PLUGINS_PUBLIC_TYPE
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // RTABMAP_COSTMAP_PLUGINS__VISIBILITY_CONTROL_H_

rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp

@@ -0,0 +1,293 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *********************************************************************/
+#ifndef RTABMAP_COSTMAP_PLUGINS__VOXEL_LAYER_HPP_
+#define RTABMAP_COSTMAP_PLUGINS__VOXEL_LAYER_HPP_
+
+#include <vector>
+
+#include <rtabmap_costmap_plugins/visibility.h>
+#include <rclcpp/rclcpp.hpp>
+#include <nav2_costmap_2d/layer.hpp>
+#include <nav2_costmap_2d/layered_costmap.hpp>
+#include <nav2_costmap_2d/observation_buffer.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav2_msgs/msg/voxel_grid.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <laser_geometry/laser_geometry.hpp>
+#include <sensor_msgs/msg/point_cloud.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <nav2_costmap_2d/obstacle_layer.hpp>
+#include <nav2_voxel_grid/voxel_grid.hpp>
+
+namespace rtabmap_costmap_plugins
+{
+
+/**
+ * @class VoxelLayer
+ * @brief Takes laser and pointcloud data to populate a 3D voxel representation of the environment
+ */
+class VoxelLayer : public nav2_costmap_2d::ObstacleLayer
+{
+public:
+  RTABMAP_COSTMAP_PLUGINS_PUBLIC
+  VoxelLayer()
+  : voxel_grid_(0, 0, 0)
+  {
+    costmap_ = NULL;  // this is the unsigned char* member of parent class's parent class Costmap2D
+  }
+
+  /**
+   * @brief Voxel Layer destructor
+   */
+  virtual ~VoxelLayer();
+
+  /**
+   * @brief Initialization process of layer on startup
+   */
+  virtual void onInitialize();
+
+  /**
+   * @brief Update the bounds of the master costmap by this layer's update dimensions
+   * @param robot_x X pose of robot
+   * @param robot_y Y pose of robot
+   * @param robot_yaw Robot orientation
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
+  virtual void updateBounds(
+    double robot_x, double robot_y, double robot_yaw, double * min_x,
+    double * min_y,
+    double * max_x,
+    double * max_y);
+
+  /**
+   * @brief Update the layer's origin to a new pose, often when in a rolling costmap
+   */
+  void updateOrigin(double new_origin_x, double new_origin_y);
+
+  /**
+   * @brief If layer is discretely populated
+   */
+  bool isDiscretized()
+  {
+    return true;
+  }
+
+  /**
+   * @brief Match the size of the master costmap
+   */
+  virtual void matchSize();
+
+  /**
+   * @brief Reset this costmap
+   */
+  virtual void reset();
+
+  /**
+   * @brief If clearing operations should be processed on this layer or not
+   */
+  virtual bool isClearable() {return true;}
+
+protected:
+  /**
+   * @brief Reset internal maps
+   */
+  virtual void resetMaps();
+
+  /**
+   * @brief Use raycasting between 2 points to clear freespace
+   */
+  virtual void raytraceFreespace(
+    const nav2_costmap_2d::Observation & clearing_observation,
+    double * min_x, double * min_y,
+    double * max_x,
+    double * max_y);
+
+  bool publish_voxel_;
+  std::string robot_base_frame_;
+  rclcpp::Publisher<nav2_msgs::msg::VoxelGrid>::SharedPtr voxel_pub_;
+  nav2_voxel_grid::VoxelGrid voxel_grid_;
+  double z_resolution_, origin_z_;
+  int unknown_threshold_, mark_threshold_, size_z_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr
+    clearing_endpoints_pub_;
+
+  /**
+   * @brief Convert world coordinates into map coordinates
+   */
+  inline bool worldToMap3DFloat(
+    double wx, double wy, double wz, double & mx, double & my,
+    double & mz)
+  {
+    if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_) {
+      return false;
+    }
+    mx = ((wx - origin_x_) / resolution_);
+    my = ((wy - origin_y_) / resolution_);
+    mz = ((wz - origin_z_) / z_resolution_);
+    if (mx < size_x_ && my < size_y_ && mz < size_z_) {
+      return true;
+    }
+
+    return false;
+  }
+
+  /**
+   * @brief Convert world coordinates into map coordinates
+   */
+  inline bool worldToMap3D(
+    double wx, double wy, double wz, unsigned int & mx, unsigned int & my,
+    unsigned int & mz)
+  {
+    if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_) {
+      return false;
+    }
+
+    mx = static_cast<unsigned int>((wx - origin_x_) / resolution_);
+    my = static_cast<unsigned int>((wy - origin_y_) / resolution_);
+    mz = static_cast<unsigned int>((wz - origin_z_) / z_resolution_);
+
+    if (mx < size_x_ && my < size_y_ && mz < (unsigned int)size_z_) {
+      return true;
+    }
+
+    return false;
+  }
+
+  /**
+   * @brief Convert map coordinates into world coordinates
+   */
+  inline void mapToWorld3D(
+    unsigned int mx, unsigned int my, unsigned int mz, double & wx,
+    double & wy,
+    double & wz)
+  {
+    // returns the center point of the cell
+    wx = origin_x_ + (mx + 0.5) * resolution_;
+    wy = origin_y_ + (my + 0.5) * resolution_;
+    wz = origin_z_ + (mz + 0.5) * z_resolution_;
+  }
+
+  /**
+   * @brief Find L2 norm distance in 3D
+   */
+  inline double dist(double x0, double y0, double z0, double x1, double y1, double z1)
+  {
+    return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
+  }
+
+  /**
+   * @brief Get the height of the voxel sizes in meters
+   */
+  double getSizeInMetersZ() const
+  {
+    return (size_z_ - 1 + 0.5) * z_resolution_;
+  }
+
+  /**
+   * @brief  Copy a region of a source map into a destination map
+   * @param  source_map The source map
+   * @param sm_lower_left_x The lower left x point of the source map to start the copy
+   * @param sm_lower_left_y The lower left y point of the source map to start the copy
+   * @param sm_size_x The x size of the source map
+   * @param  dest_map The destination map
+   * @param dm_lower_left_x The lower left x point of the destination map to start the copy
+   * @param dm_lower_left_y The lower left y point of the destination map to start the copy
+   * @param dm_size_x The x size of the destination map
+   * @param region_size_x The x size of the region to copy
+   * @param region_size_y The y size of the region to copy
+   */
+  template<typename data_type>
+  void copyMapRegion3D(
+    data_type * source_map, unsigned int sm_lower_left_x,
+    unsigned int sm_lower_left_y,
+    unsigned int sm_size_x, data_type * dest_map, unsigned int dm_lower_left_x,
+    unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
+    unsigned int region_size_y, int z_shift)
+  {
+    // we'll first need to compute the starting points for each map
+    // this is like getting voxel column. We are not taking into account the z position of the voxel
+    data_type * sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
+    data_type * dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);
+
+    uint32_t marked_bits_mask = (data_type) 0xFFFF0000;
+    uint32_t unknown_bits_mask = (data_type) 0x0000FFFF;
+
+    // now, we'll copy the source map into the destination map
+    for (unsigned int i = 0; i < region_size_y; ++i) {
+      memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
+
+      for (unsigned int j = 0; j < region_size_x; j++) {
+        // known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits
+				if (z_shift > 0) {
+					dm_index[j] =
+							// Shift marked cells, insert zeros for new unknowns
+							((dm_index[j] & marked_bits_mask) >> z_shift & marked_bits_mask) |
+							// Shift empty/unknown cells, insert ones for new unknowns
+              (((dm_index[j] & unknown_bits_mask) >> z_shift | (~((data_type) 0) << (sizeof(data_type) * 4 - z_shift))) & unknown_bits_mask);
+
+				} else if (z_shift < 0) {
+					dm_index[j] =
+							// Shift marked cells, insert zeros for new unknowns
+							(dm_index[j] & marked_bits_mask) << z_shift * -1 |
+							// Shift empty/unknown cells, insert ones for new unknowns
+              ((dm_index[j] << z_shift * -1 & unknown_bits_mask) | ~(~((data_type) 0) << z_shift * -1));
+				}
+			}
+
+      sm_index += sm_size_x;
+      dm_index += dm_size_x;
+    }
+  }
+
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+};
+
+}  // namespace rtabmap_costmap_plugins
+
+#endif  // RTABMAP_COSTMAP_PLUGINS__VOXEL_LAYER_HPP_

rtabmap_costmap_plugins/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rtabmap_costmap_plugins</name>
+  <version>0.22.1</version>
+  <description>RTAB-Map's costmap plugins.</description>
+  <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
+  <author>Mathieu Labbe</author>
+  <license>BSD</license>
+  <url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
+  <url type="repository">https://github.com/introlab/rtabmap_ros</url>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <build_depend>ros_environment</build_depend>
+
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>visualization_msgs</depend>
+  
+  <export>
+	<build_type>ament_cmake</build_type>
+  </export>
+
+</package>

rtabmap_costmap_plugins/src/voxel_layer.cpp

@@ -0,0 +1,600 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *********************************************************************/
+
+#include "rtabmap_costmap_plugins/voxel_layer.hpp"
+
+#include <algorithm>
+#include <cassert>
+#include <vector>
+#include <memory>
+#include <utility>
+
+#include "pluginlib/class_list_macros.hpp"
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+
+#define VOXEL_BITS 16
+PLUGINLIB_EXPORT_CLASS(rtabmap_costmap_plugins::VoxelLayer, nav2_costmap_2d::Layer)
+
+using nav2_costmap_2d::NO_INFORMATION;
+using nav2_costmap_2d::LETHAL_OBSTACLE;
+using nav2_costmap_2d::FREE_SPACE;
+using rcl_interfaces::msg::ParameterType;
+
+namespace rtabmap_costmap_plugins
+{
+
+void VoxelLayer::onInitialize()
+{
+  nav2_costmap_2d::ObstacleLayer::onInitialize();
+
+  declareParameter("enabled", rclcpp::ParameterValue(true));
+  declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true));
+  declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0));
+  declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
+  declareParameter("z_voxels", rclcpp::ParameterValue(10));
+  declareParameter("origin_z", rclcpp::ParameterValue(0.0));
+  declareParameter("z_resolution", rclcpp::ParameterValue(0.2));
+  declareParameter("unknown_threshold", rclcpp::ParameterValue(15));
+  declareParameter("mark_threshold", rclcpp::ParameterValue(0));
+  declareParameter("combination_method", rclcpp::ParameterValue(1));
+  declareParameter("publish_voxel_map", rclcpp::ParameterValue(false));
+  declareParameter("robot_base_frame", rclcpp::ParameterValue("base_link"));
+
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  node->get_parameter(name_ + "." + "enabled", enabled_);
+  node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
+  node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_);
+  node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_);
+  node->get_parameter(name_ + "." + "z_voxels", size_z_);
+  node->get_parameter(name_ + "." + "origin_z", origin_z_);
+  node->get_parameter(name_ + "." + "z_resolution", z_resolution_);
+  node->get_parameter(name_ + "." + "unknown_threshold", unknown_threshold_);
+  node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_);
+  node->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_);
+  node->get_parameter(name_ + "." + "robot_base_frame", robot_base_frame_);
+
+  int combination_method_param{};
+  node->get_parameter(name_ + "." + "combination_method", combination_method_param);
+#ifdef PRE_ROS_JAZZY
+  combination_method_ = combination_method_param;
+#else
+  combination_method_ = combination_method_from_int(combination_method_param);
+#endif
+
+  if (publish_voxel_) {
+    voxel_pub_ = node->create_publisher<nav2_msgs::msg::VoxelGrid>(
+      "voxel_grid", rclcpp::QoS(1).transient_local());
+    //voxel_pub_->on_activate();
+  }
+
+  clearing_endpoints_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>(
+    "clearing_endpoints", rclcpp::QoS(1).transient_local());
+  //clearing_endpoints_pub_->on_activate();
+
+  unknown_threshold_ += (VOXEL_BITS - size_z_);
+  matchSize();
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &VoxelLayer::dynamicParametersCallback,
+      this, std::placeholders::_1));
+}
+
+VoxelLayer::~VoxelLayer()
+{
+  auto node = node_.lock();
+  if (dyn_params_handler_ && node) {
+    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  }
+  dyn_params_handler_.reset();
+}
+
+void VoxelLayer::matchSize()
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  ObstacleLayer::matchSize();
+  voxel_grid_.resize(size_x_, size_y_, size_z_);
+  assert(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);
+}
+
+void VoxelLayer::reset()
+{
+  // Call the base class method before adding our own functionality
+  ObstacleLayer::reset();
+  resetMaps();
+}
+
+void VoxelLayer::resetMaps()
+{
+  // Call the base class method before adding our own functionality
+  // Note: at the time this was written, ObstacleLayer doesn't implement
+  // resetMaps so this goes to the next layer down Costmap2DLayer which also
+  // doesn't implement this, so it actually goes all the way to Costmap2D
+  ObstacleLayer::resetMaps();
+  voxel_grid_.reset();
+}
+
+void VoxelLayer::updateBounds(
+  double robot_x, double robot_y, double robot_yaw, double * min_x,
+  double * min_y, double * max_x, double * max_y)
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+
+  if (rolling_window_) {
+    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
+  }
+  if (!enabled_) {
+    return;
+  }
+  useExtraBounds(min_x, min_y, max_x, max_y);
+
+  bool current = true;
+  std::vector<nav2_costmap_2d::Observation> observations, clearing_observations;
+
+  // get the marking observations
+  current = getMarkingObservations(observations) && current;
+
+  // get the clearing observations
+  current = getClearingObservations(clearing_observations) && current;
+
+  // update the global current status
+  current_ = current;
+
+  // raytrace freespace
+  for (unsigned int i = 0; i < clearing_observations.size(); ++i) {
+    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
+  }
+
+  // place the new obstacles into a priority queue... each with a priority of zero to begin with
+  for (std::vector<nav2_costmap_2d::Observation>::const_iterator it = observations.begin(); it != observations.end();
+    ++it)
+  {
+    const nav2_costmap_2d::Observation & obs = *it;
+
+    const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_);
+
+    double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_;
+    double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_;
+
+    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
+
+    for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+      // if the obstacle is too low, we won't add it
+      if (*iter_z < min_obstacle_height_) {
+        continue;
+      }
+
+      // if the obstacle is too high or too far away from the robot we won't add it
+      if (*iter_z > max_obstacle_height_) {
+        continue;
+      }
+
+      // compute the squared distance from the hitpoint to the pointcloud's origin
+      double sq_dist = (*iter_x - obs.origin_.x) * (*iter_x - obs.origin_.x) +
+        (*iter_y - obs.origin_.y) * (*iter_y - obs.origin_.y) +
+        (*iter_z - obs.origin_.z) * (*iter_z - obs.origin_.z);
+
+      // if the point is far enough away... we won't consider it
+      if (sq_dist >= sq_obstacle_max_range) {
+        continue;
+      }
+
+      // If the point is too close, do not consider it
+      if (sq_dist < sq_obstacle_min_range) {
+        continue;
+      }
+
+      // now we need to compute the map coordinates for the observation
+      unsigned int mx, my, mz;
+      if (!worldToMap3D(*iter_x, *iter_y, *iter_z, mx, my, mz)) {
+        continue;
+      }
+
+      // mark the cell in the voxel grid and check if we should also mark it in the costmap
+      if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_)) {
+        unsigned int index = getIndex(mx, my);
+
+        costmap_[index] = LETHAL_OBSTACLE;
+        touch(
+          static_cast<double>(*iter_x), static_cast<double>(*iter_y),
+          min_x, min_y, max_x, max_y);
+      }
+    }
+  }
+
+  if (publish_voxel_) {
+    auto grid_msg = std::make_unique<nav2_msgs::msg::VoxelGrid>();
+    unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
+    grid_msg->size_x = voxel_grid_.sizeX();
+    grid_msg->size_y = voxel_grid_.sizeY();
+    grid_msg->size_z = voxel_grid_.sizeZ();
+    grid_msg->data.resize(size);
+    memcpy(&grid_msg->data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
+
+    grid_msg->origin.x = origin_x_;
+    grid_msg->origin.y = origin_y_;
+    grid_msg->origin.z = origin_z_;
+
+    grid_msg->resolutions.x = resolution_;
+    grid_msg->resolutions.y = resolution_;
+    grid_msg->resolutions.z = z_resolution_;
+    grid_msg->header.frame_id = global_frame_;
+    grid_msg->header.stamp = clock_->now();
+
+    voxel_pub_->publish(std::move(grid_msg));
+  }
+
+  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
+}
+
+void VoxelLayer::raytraceFreespace(
+  const nav2_costmap_2d::Observation & clearing_observation, double * min_x,
+  double * min_y,
+  double * max_x,
+  double * max_y)
+{
+  auto clearing_endpoints_ = std::make_unique<sensor_msgs::msg::PointCloud2>();
+
+  if (clearing_observation.cloud_->height == 0 || clearing_observation.cloud_->width == 0) {
+    return;
+  }
+
+  double sensor_x, sensor_y, sensor_z;
+  double ox = clearing_observation.origin_.x;
+  double oy = clearing_observation.origin_.y;
+  double oz = clearing_observation.origin_.z;
+
+  if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) {
+    RCLCPP_WARN(
+      logger_,
+      "Sensor origin at (%.2f, %.2f %.2f) is out of map bounds "
+      "(%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f). "
+      "The costmap cannot raytrace for it.",
+      ox, oy, oz,
+      origin_x_, origin_y_, origin_z_,
+      origin_x_ + getSizeInMetersX(), origin_y_ + getSizeInMetersY(),
+      origin_z_ + getSizeInMetersZ());
+
+    return;
+  }
+
+  bool publish_clearing_points;
+
+  {
+    auto node = node_.lock();
+    if (!node) {
+      throw std::runtime_error{"Failed to lock node"};
+    }
+    publish_clearing_points = (node->count_subscribers("clearing_endpoints") > 0);
+  }
+
+  clearing_endpoints_->data.clear();
+  clearing_endpoints_->width = clearing_observation.cloud_->width;
+  clearing_endpoints_->height = clearing_observation.cloud_->height;
+  clearing_endpoints_->is_dense = true;
+  clearing_endpoints_->is_bigendian = false;
+
+  sensor_msgs::PointCloud2Modifier modifier(*clearing_endpoints_);
+  modifier.setPointCloud2Fields(
+    3, "x", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "y", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "z", 1, sensor_msgs::msg::PointField::FLOAT32);
+
+  sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_x(*clearing_endpoints_, "x");
+  sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_y(*clearing_endpoints_, "y");
+  sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_z(*clearing_endpoints_, "z");
+
+  // we can pre-compute the endpoints of the map outside of the inner loop... we'll need these later
+  double map_end_x = origin_x_ + getSizeInMetersX();
+  double map_end_y = origin_y_ + getSizeInMetersY();
+  double map_end_z = origin_z_ + getSizeInMetersZ();
+
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
+
+  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+    double wpx = *iter_x;
+    double wpy = *iter_y;
+    double wpz = *iter_z;
+
+    double distance = dist(ox, oy, oz, wpx, wpy, wpz);
+    double scaling_fact = 1.0;
+    scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
+    wpx = scaling_fact * (wpx - ox) + ox;
+    wpy = scaling_fact * (wpy - oy) + oy;
+    wpz = scaling_fact * (wpz - oz) + oz;
+
+    double a = wpx - ox;
+    double b = wpy - oy;
+    double c = wpz - oz;
+    double t = 1.0;
+    bool wp_outside = false;
+
+    // we can only raytrace to a maximum z height
+    if (wpz > map_end_z) {
+      // we know we want the vector's z value to be max_z
+      t = std::max(0.0, std::min(t, (map_end_z - 0.01 - oz) / c));
+      wp_outside = true;
+    } else if (wpz < origin_z_) {
+      // and we can only raytrace down to the floor
+      // we know we want the vector's z value to be 0.0
+      t = std::min(t, (origin_z_ - oz) / c);
+      wp_outside = true;
+    }
+
+    // the minimum value to raytrace from is the origin
+    if (wpx < origin_x_) {
+      t = std::min(t, (origin_x_ - ox) / a);
+      wp_outside = true;
+    }
+    if (wpy < origin_y_) {
+      t = std::min(t, (origin_y_ - oy) / b);
+      wp_outside = true;
+    }
+
+    // the maximum value to raytrace to is the end of the map
+    if (wpx > map_end_x) {
+      t = std::min(t, (map_end_x - ox) / a);
+      wp_outside = true;
+    }
+    if (wpy > map_end_y) {
+      t = std::min(t, (map_end_y - oy) / b);
+      wp_outside = true;
+    }
+
+    constexpr double wp_epsilon = 1e-5;
+    if (wp_outside) {
+      if (t > 0.0) {
+        t -= wp_epsilon;
+      } else if (t < 0.0) {
+        t += wp_epsilon;
+      }
+    }
+
+    wpx = ox + a * t;
+    wpy = oy + b * t;
+    wpz = oz + c * t;
+
+    double point_x, point_y, point_z;
+    if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z)) {
+      unsigned int cell_raytrace_max_range = cellDistance(clearing_observation.raytrace_max_range_);
+      unsigned int cell_raytrace_min_range = cellDistance(clearing_observation.raytrace_min_range_);
+
+
+      // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
+      voxel_grid_.clearVoxelLineInMap(
+        sensor_x, sensor_y, sensor_z, point_x, point_y, point_z,
+        costmap_,
+        unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
+        cell_raytrace_max_range, cell_raytrace_min_range);
+
+      updateRaytraceBounds(
+        ox, oy, wpx, wpy, clearing_observation.raytrace_max_range_,
+        clearing_observation.raytrace_min_range_, min_x, min_y,
+        max_x,
+        max_y);
+
+      if (publish_clearing_points) {
+        *clearing_endpoints_iter_x = wpx;
+        *clearing_endpoints_iter_y = wpy;
+        *clearing_endpoints_iter_z = wpz;
+
+        ++clearing_endpoints_iter_x;
+        ++clearing_endpoints_iter_y;
+        ++clearing_endpoints_iter_z;
+      }
+    }
+  }
+
+  if (publish_clearing_points) {
+    clearing_endpoints_->header.frame_id = global_frame_;
+    clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp;
+
+    clearing_endpoints_pub_->publish(std::move(clearing_endpoints_));
+  }
+}
+
+void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
+{
+  int cell_oz;
+  // get the global pose of the robot
+  try
+  {
+      geometry_msgs::msg::TransformStamped transformStamped;
+
+      transformStamped = tf_->lookupTransform(global_frame_, robot_base_frame_, rclcpp::Time(0));
+
+      const double robot_z = transformStamped.transform.translation.z;
+      const double z_grid_height = z_resolution_ * size_z_;
+      const double new_origin_z = robot_z - z_grid_height / 2;
+      cell_oz = int((new_origin_z - origin_z_) / z_resolution_);
+  }
+  catch (tf2::TransformException& ex)
+  {
+      RCLCPP_ERROR(logger_, "%s", ex.what());
+      // If the robot pose is not detected, the origin_z_ will remain the same.
+      cell_oz = 0;
+  }
+
+  // project the new origin into the grid
+  int cell_ox, cell_oy;
+  cell_ox = static_cast<int>((new_origin_x - origin_x_) / resolution_);
+  cell_oy = static_cast<int>((new_origin_y - origin_y_) / resolution_);
+
+  // compute the associated world coordinates for the origin cell
+  // because we want to keep things grid-aligned
+  double new_grid_ox, new_grid_oy, new_grid_oz;
+  new_grid_ox = origin_x_ + cell_ox * resolution_;
+  new_grid_oy = origin_y_ + cell_oy * resolution_;
+  new_grid_oz = origin_z_ + cell_oz * z_resolution_;
+
+  // To save casting from unsigned int to int a bunch of times
+  int size_x = size_x_;
+  int size_y = size_y_;
+
+  // we need to compute the overlap of the new and existing windows
+  int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
+  lower_left_x = std::min(std::max(cell_ox, 0), size_x);
+  lower_left_y = std::min(std::max(cell_oy, 0), size_y);
+  upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
+  upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
+
+  unsigned int cell_size_x = upper_right_x - lower_left_x;
+  unsigned int cell_size_y = upper_right_y - lower_left_y;
+
+  // we need a map to store the obstacles in the window temporarily
+  unsigned char * local_map = new unsigned char[cell_size_x * cell_size_y];
+  unsigned int * local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
+  unsigned int * voxel_map = voxel_grid_.getData();
+
+  // copy the local window in the costmap to the local map
+  copyMapRegion(
+    costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x,
+    cell_size_x,
+    cell_size_y);
+  copyMapRegion(
+    voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x,
+    cell_size_x,
+    cell_size_y);
+
+  // we'll reset our maps to unknown space if appropriate
+  resetMaps();
+
+  // update the origin with the appropriate world coordinates
+  origin_x_ = new_grid_ox;
+  origin_y_ = new_grid_oy;
+  origin_z_ = new_grid_oz;
+
+  // compute the starting cell location for copying data back in
+  int start_x = lower_left_x - cell_ox;
+  int start_y = lower_left_y - cell_oy;
+
+  // now we want to copy the overlapping information back into the map, but in its new location
+  copyMapRegion(
+    local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x,
+    cell_size_y);
+  copyMapRegion3D(
+    local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_,
+    cell_size_x,
+    cell_size_y,
+    cell_oz);
+
+  // make sure to clean up
+  delete[] local_map;
+  delete[] local_voxel_map;
+}
+
+/**
+  * @brief Callback executed when a parameter change is detected
+  * @param event ParameterEvent message
+  */
+rcl_interfaces::msg::SetParametersResult
+VoxelLayer::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  rcl_interfaces::msg::SetParametersResult result;
+  bool resize_map_needed = false;
+
+  for (auto parameter : parameters) {
+    const auto & param_type = parameter.get_type();
+    const auto & param_name = parameter.get_name();
+    if (param_name.find(name_ + ".") != 0) {
+      continue;
+    }
+
+    if (param_type == ParameterType::PARAMETER_DOUBLE) {
+      if (param_name == name_ + "." + "min_obstacle_height") {
+        min_obstacle_height_ = parameter.as_double();
+      } else if (param_name == name_ + "." + "max_obstacle_height") {
+        max_obstacle_height_ = parameter.as_double();
+      } else if (param_name == name_ + "." + "origin_z") {
+        origin_z_ = parameter.as_double();
+        resize_map_needed = true;
+      } else if (param_name == name_ + "." + "z_resolution") {
+        z_resolution_ = parameter.as_double();
+        resize_map_needed = true;
+      }
+    } else if (param_type == ParameterType::PARAMETER_BOOL) {
+      if (param_name == name_ + "." + "enabled") {
+        enabled_ = parameter.as_bool();
+        current_ = false;
+      } else if (param_name == name_ + "." + "footprint_clearing_enabled") {
+        footprint_clearing_enabled_ = parameter.as_bool();
+      } else if (param_name == name_ + "." + "publish_voxel_map") {
+        RCLCPP_WARN(
+          logger_, "publish voxel map is not a dynamic parameter "
+          "cannot be changed while running. Rejecting parameter update.");
+        continue;
+      }
+
+    } else if (param_type == ParameterType::PARAMETER_INTEGER) {
+      if (param_name == name_ + "." + "z_voxels") {
+        size_z_ = parameter.as_int();
+        resize_map_needed = true;
+      } else if (param_name == name_ + "." + "unknown_threshold") {
+        unknown_threshold_ = parameter.as_int() + (VOXEL_BITS - size_z_);
+      } else if (param_name == name_ + "." + "mark_threshold") {
+        mark_threshold_ = parameter.as_int();
+      } else if (param_name == name_ + "." + "combination_method") {
+#ifdef PRE_ROS_JAZZY
+        combination_method_ = parameter.as_int();
+#else
+        combination_method_ = combination_method_from_int(parameter.as_int());
+#endif
+      }
+    }
+  }
+
+  if (resize_map_needed) {
+    matchSize();
+  }
+
+  result.successful = true;
+  return result;
+}
+
+}  // namespace rtabmap_costmap_plugins

rtabmap_costmap_plugins/src/voxel_marker.cpp

@@ -0,0 +1,152 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *********************************************************************/
+
+/**
+ * Modified matlabbe:
+ * Added option to choose between unknown, free and marked cells
+ */
+
+#include <rclcpp/rclcpp.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+#include <nav2_voxel_grid/voxel_grid.hpp>
+#include <nav2_msgs/msg/voxel_grid.hpp>
+
+namespace rtabmap_costmap_plugins 
+{
+
+// FREE, UNKNOWN, MARKED
+double g_voxel_colors_r[] = {0.0, 1.0, 1.0};
+double g_voxel_colors_g[] = {1.0, 1.0, 0.0};
+double g_voxel_colors_b[] = {1.0, 1.0, 0.0};
+double g_voxel_colors_a[] = {0.5, 0.1, 0.5};
+
+class VoxelMarker: public rclcpp::Node
+{
+public:
+	explicit VoxelMarker(const rclcpp::NodeOptions & options) :
+    rclcpp::Node("voxel_marker", options)
+  {
+    cell_type_ = this->declare_parameter("cell_type", (int)nav2_voxel_grid::VoxelStatus::MARKED);
+    color_r_ = this->declare_parameter("r", g_voxel_colors_r[cell_type_]);
+    color_g_ = this->declare_parameter("g", g_voxel_colors_g[cell_type_]);
+    color_b_ = this->declare_parameter("b", g_voxel_colors_b[cell_type_]);
+    color_a_ = this->declare_parameter("a", g_voxel_colors_a[cell_type_]);  
+    
+    voxel_sub_ = this->create_subscription<nav2_msgs::msg::VoxelGrid>("voxel_grid", rclcpp::QoS(1), std::bind(&VoxelMarker::voxelCallback, this, std::placeholders::_1));
+    marker_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("visualization_marker", rclcpp::QoS(1));
+
+  }
+	virtual ~VoxelMarker() {}
+
+  void voxelCallback(const nav2_msgs::msg::VoxelGrid::SharedPtr grid)
+  {
+    if (grid->data.empty())
+    {
+      RCLCPP_ERROR(get_logger(), "Received empty voxel grid");
+      return;
+    }
+
+    visualization_msgs::msg::Marker m;
+    m.header.frame_id = grid->header.frame_id;
+    m.header.stamp = grid->header.stamp;
+    m.ns = "voxel_grid";
+    m.id = 0;
+    m.type = visualization_msgs::msg::Marker::CUBE_LIST;
+    m.action = visualization_msgs::msg::Marker::ADD;
+    m.pose.orientation.w = 1.0;
+    m.color.r = color_r_;
+    m.color.g = color_g_;
+    m.color.b = color_b_;
+    m.color.a = color_a_;
+
+    const uint32_t* data = &grid->data.front();
+    const double x_origin = grid->origin.x;
+    const double y_origin = grid->origin.y;
+    const double z_origin = grid->origin.z;
+    const double x_res = grid->resolutions.x;
+    const double y_res = grid->resolutions.y;
+    const double z_res = grid->resolutions.z;
+    const uint32_t x_size = grid->size_x;
+    const uint32_t y_size = grid->size_y;
+    const uint32_t z_size = grid->size_z;
+    for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid)
+    {
+      for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid)
+      {
+        for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid)
+        {
+          nav2_voxel_grid::VoxelStatus status = nav2_voxel_grid::VoxelGrid::getVoxel(x_grid, y_grid, z_grid, x_size, y_size, z_size,
+                                                                          data);
+
+          if (status == (nav2_voxel_grid::VoxelStatus)cell_type_)
+          {
+            geometry_msgs::msg::Point p;
+            p.x = x_origin + (x_grid + 0.5) * x_res;
+            p.y = y_origin + (y_grid + 0.5) * y_res;
+            p.z = z_origin + (z_grid + 0.5) * z_res;
+            m.points.push_back(p);
+          }
+        }
+      }
+    }
+    m.scale.x = x_res;
+    m.scale.y = y_res;
+    m.scale.z = z_res;
+
+    marker_pub_->publish(m);
+  }
+
+private:
+  int cell_type_;
+  double color_r_;
+  double color_g_;
+  double color_b_;
+  double color_a_;
+
+  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_pub_;
+	rclcpp::Subscription<nav2_msgs::msg::VoxelGrid>::SharedPtr voxel_sub_;
+};
+
+} // rtabmap_costmap_plugins
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_costmap_plugins::VoxelMarker>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
+}

rtabmap_util/CMakeLists.txt

@@ -232,7 +232,7 @@ ament_export_libraries(rtabmap_util_plugins) # To include downstream without tar
 
 # Install Python executables
 install(PROGRAMS
-#  scripts/patrol.py
+  scripts/patrol.py
 #  scripts/objects_to_tags.py
 #  scripts/point_to_tf.py
 #  scripts/netvlad_tf_ros.py

rtabmap_util/scripts/patrol.py

@@ -1,87 +1,104 @@
-#!/usr/bin/env python
-import rospy
+#!/usr/bin/env python3
 import sys
+import time
+import rclpy
+from rclpy.node import Node
 from std_msgs.msg import Bool
-from rtabmap_ros.msg import Goal
+from rtabmap_msgs.msg import Goal
 
-pub = rospy.Publisher('rtabmap/goal_node', Goal, queue_size=1)
-waypoints = []
-currentIndex = 0
-waitingTime = 1.0
-frameId = ""
 
-def callback(data):
-    global currentIndex
-    global waitingTime
-    global frameId
-    if data.data:
-        rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' reached! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime)
-    else:
-        rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' failed! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime)
+class PatrolNode(Node):
+    def __init__(self, waypoints):
+        super().__init__('patrol')
 
-    currentIndex = (currentIndex+1) % len(waypoints)
+        # --- Parameters ---
+        self.declare_parameter('time', 1.0)
+        self.declare_parameter('frame_id', '')
+        self.waiting_time = self.get_parameter('time').value
+        self.frame_id = self.get_parameter('frame_id').value
 
-    # Waiting time before sending next goal
-    rospy.sleep(waitingTime)
+        # --- Variables ---
+        self.waypoints = waypoints
+        self.current_index = 0
+
+        # --- Publisher & Subscriber ---
+        self.pub = self.create_publisher(Goal, 'rtabmap/goal_node', 10)
+        self.sub = self.create_subscription(Bool, 'rtabmap/goal_reached', self.callback, 10)
+
+        self.get_logger().info(f"Waypoints: {self.waypoints}")
+        self.get_logger().info(f"Waiting time: {self.waiting_time:.1f} sec")
+        self.get_logger().info(f"Publishing goals on: {self.pub.topic_name}")
+        self.get_logger().info(f"Receiving goal status on: {self.sub.topic_name}")
+
+        # Delay before sending first goal (ensure discovery)
+        time.sleep(1.0)
+
+        # Send first goal
+        self.send_goal()
+
+    def callback(self, msg: Bool):
+        """Called when goal_reached is received."""
+        if msg.data:
+            self.get_logger().info(
+                f"Goal '{self.waypoints[self.current_index]}' reached! "
+                f"Publishing next goal in {self.waiting_time:.1f} sec..."
+            )
+        else:
+            self.get_logger().info(
+                f"Goal '{self.waypoints[self.current_index]}' failed! "
+                f"Publishing next goal in {self.waiting_time:.1f} sec..."
+            )
+
+        # Move to next waypoint
+        self.current_index = (self.current_index + 1) % len(self.waypoints)
+
+        # Wait before sending next goal
+        time.sleep(self.waiting_time)
+        self.send_goal()
+
+    def send_goal(self):
+        """Send current goal to RTAB-Map."""
+        waypoint = self.waypoints[self.current_index]
+        msg = Goal()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.frame_id = self.frame_id
+
+        # Check if waypoint is a node id (int) or a label (string)
+        try:
+            msg.node_id = int(waypoint)
+            msg.node_label = ""
+        except ValueError:
+            msg.node_id = 0
+            msg.node_label = waypoint
+
+        self.get_logger().info(
+            f"Publishing goal '{waypoint}' ({self.current_index + 1}/{len(self.waypoints)})"
+        )
+        self.pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    # Extract waypoints from command-line args
+    if len(sys.argv) < 3:
+        print(
+            "Usage: patrol.py waypointA waypointB waypointC ... "
+            "[--ros-args -p time:=1.0 -p frame_id:=base_footprint]"
+        )
+        return
+
+    waypoints = [x for x in sys.argv[1:] if not x.startswith('--') and not x.startswith('_')]
+    node = PatrolNode(waypoints)
 
-    msg = Goal()
-    msg.frame_id = frameId
     try:
-        int(waypoints[currentIndex])
-        is_dig = True
-    except ValueError:
-        is_dig = False
-    if is_dig:
-        msg.node_id = int(waypoints[currentIndex])
-        msg.node_label = ""
-    else:
-        msg.node_id = 0
-        msg.node_label = waypoints[currentIndex]
-    
-    rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
-    msg.header.stamp = rospy.get_rostime()
-    pub.publish(msg)
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
 
-def main():
-    rospy.init_node('patrol', anonymous=False)
-    sub = rospy.Subscriber("rtabmap/goal_reached", Bool, callback)
-    global waitingTime
-    global frameId
-    waitingTime = rospy.get_param('~time', waitingTime)
-    frameId = rospy.get_param('~frame_id', frameId)
-    rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal
-
-    rospy.loginfo(rospy.get_caller_id() + ": Waypoints: [%s]", str(waypoints).strip('[]'))
-    rospy.loginfo(rospy.get_caller_id() + ": time: %f", waitingTime)
-    rospy.loginfo(rospy.get_caller_id() + ": publish goal on %s", pub.resolved_name)
-    rospy.loginfo(rospy.get_caller_id() + ": receive goal status on %s", sub.resolved_name)
-
-    # send the first goal
-    msg = Goal()
-    msg.frame_id = frameId
-    try:
-        int(waypoints[currentIndex])
-        is_dig = True
-    except ValueError:
-        is_dig = False
-    if is_dig:
-        msg.node_id = int(waypoints[currentIndex])
-        msg.node_label = ""
-    else:
-        msg.node_id = 0
-        msg.node_label = waypoints[currentIndex]
-    while rospy.Time.now().secs == 0:
-         rospy.loginfo(rospy.get_caller_id() + ": Waiting clock...")
-         rospy.sleep(.1)
-    msg.header.stamp = rospy.Time.now()
-    rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
-    pub.publish(msg)
-    rospy.spin()    
 
 if __name__ == '__main__':
-    if len(sys.argv) < 3:
-        print("usage: patrol.py waypointA waypointB waypointC ... [_time:=1 frame_id:=base_footprint] [topic remaps] (at least 2 waypoints, can be node id, landmark or label)")
-    else:
-        waypoints = sys.argv[1:] 
-        waypoints = [x for x in waypoints if not x.startswith('/') and not x.startswith('_')]
-        main()
+    main()