feat(slam): add rtabmap_ros

rtabmap_ros/rtabmap_slam/CMakeLists.txt

@@ -0,0 +1,196 @@
+cmake_minimum_required(VERSION 3.5)
+project(rtabmap_slam)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# To suppress PCL_ROOT warning
+if(POLICY CMP0074)
+    cmake_policy(SET CMP0074 NEW)
+endif()
+
+if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64")
+  # issues #1285 #1288
+  find_library(
+    builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c
+    PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
+  )
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(rtabmap_msgs REQUIRED)
+find_package(rtabmap_util REQUIRED)
+find_package(rtabmap_sync REQUIRED)
+
+#optional
+find_package(apriltag_msgs)
+find_package(aruco_msgs)
+find_package(aruco_markers_msgs)
+find_package(aruco_opencv_msgs)
+find_package(ros2_aruco_interfaces)
+find_package(nav2_msgs)
+
+IF(WIN32)
+add_compile_options(-bigobj)
+ENDIF(WIN32)
+
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+
+# libraries
+SET(Libraries
+   cv_bridge
+   geometry_msgs
+   nav_msgs
+   rclcpp
+   rclcpp_components
+   sensor_msgs
+   std_msgs
+   std_srvs
+   tf2
+   tf2_ros
+   visualization_msgs
+   rtabmap_msgs
+   rtabmap_util
+   rtabmap_sync
+)
+
+if("$ENV{ROS_DISTRO}" STRLESS "jazzy")
+  add_definitions(-DPRE_ROS_JAZZY)
+endif()
+
+###########
+## Build ##
+###########
+
+SET(rtabmap_slam_plugins_lib_src
+   src/CoreWrapper.cpp
+)
+
+# If apriltag_msgs is found, add definition
+IF(apriltag_msgs_FOUND)
+MESSAGE(STATUS "WITH apriltag_msgs")
+ADD_DEFINITIONS("-DWITH_APRILTAG_MSGS")
+SET(Libraries
+   ${Libraries}
+   apriltag_msgs
+)
+ENDIF(apriltag_msgs_FOUND)
+
+# If aruco_msgs is found, add definition
+IF(aruco_msgs_FOUND)
+MESSAGE(STATUS "WITH aruco_msgs")
+ADD_DEFINITIONS("-DWITH_ARUCO_MSGS")
+SET(Libraries
+   ${Libraries}
+   aruco_msgs
+)
+ENDIF(aruco_msgs_FOUND)
+
+# If aruco_opencv_msgs is found, add definition
+IF(aruco_opencv_msgs_FOUND)
+MESSAGE(STATUS "WITH aruco_opencv_msgs")
+ADD_DEFINITIONS("-DWITH_ARUCO_OPENCV_MSGS")
+SET(Libraries
+   ${Libraries}
+   aruco_opencv_msgs
+)
+ENDIF(aruco_opencv_msgs_FOUND)
+
+# If aruco_markers_msgs is found, add definition
+IF(aruco_markers_msgs_FOUND)
+MESSAGE(STATUS "WITH aruco_markers_msgs")
+ADD_DEFINITIONS("-DWITH_ARUCO_MARKERS_MSGS")
+SET(Libraries
+   ${Libraries}
+   aruco_markers_msgs
+)
+ENDIF(aruco_markers_msgs_FOUND)
+
+# If ros2_aruco_interfaces is found, add definition
+IF(ros2_aruco_interfaces_FOUND)
+MESSAGE(STATUS "WITH ros2_aruco_interfaces")
+ADD_DEFINITIONS("-DWITH_ROS2_ARUCO_INTERFACES")
+SET(Libraries
+   ${Libraries}
+   ros2_aruco_interfaces
+)
+ENDIF(ros2_aruco_interfaces_FOUND)
+
+# If nav2_msgs is found, add definition
+IF(nav2_msgs_FOUND)
+MESSAGE(STATUS "WITH nav2_msgs")
+ADD_DEFINITIONS("-DWITH_NAV2_MSGS")
+SET(Libraries
+   ${Libraries}
+   nav2_msgs
+)
+IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0)
+   ADD_DEFINITIONS("-DNAV_MSGS_FOXY")
+ENDIF()
+ENDIF(nav2_msgs_FOUND)
+
+############################
+## Declare a cpp library
+############################
+add_library(rtabmap_slam_plugins SHARED
+   ${rtabmap_slam_plugins_lib_src}
+)
+target_include_directories(rtabmap_slam_plugins
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(rtabmap_slam_plugins ${Libraries})
+
+rclcpp_components_register_nodes(rtabmap_slam_plugins "rtabmap_slam::CoreWrapper")
+
+add_executable(rtabmap_node src/CoreNode.cpp)
+ament_target_dependencies(rtabmap_node ${Libraries})
+target_link_libraries(rtabmap_node rtabmap_slam_plugins)
+set_target_properties(rtabmap_node PROPERTIES OUTPUT_NAME "rtabmap")
+
+#############
+## Install ##
+#############
+
+ament_export_dependencies(${Libraries})
+ament_export_include_directories(include)
+ament_export_targets(${PROJECT_NAME})          # To include downstream with targets
+ament_export_libraries(rtabmap_slam_plugins) # To include downstream without targets
+
+install(TARGETS 
+   rtabmap_slam_plugins
+   EXPORT ${PROJECT_NAME}
+   ARCHIVE DESTINATION lib
+   LIBRARY DESTINATION lib
+   RUNTIME DESTINATION bin
+)
+
+install(TARGETS 
+   rtabmap_node 
+   DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+   DESTINATION include
+   FILES_MATCHING PATTERN "*.h"
+)
+
+ament_package()

rtabmap_ros/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h

@@ -0,0 +1,533 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 the Universite de Sherbrooke 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 HOLDER 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.
+*/
+
+#ifndef COREWRAPPER_H_
+#define COREWRAPPER_H_
+
+#include <rtabmap_slam/visibility.h>
+#include <rclcpp/rclcpp.hpp>
+
+#include <std_srvs/srv/empty.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+
+#include <std_msgs/msg/empty.hpp>
+#include <std_msgs/msg/int32.hpp>
+#include <std_msgs/msg/int32_multi_array.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <nav_msgs/srv/get_map.hpp>
+#include <nav_msgs/srv/get_plan.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/OdometryInfo.h>
+
+#include "rtabmap_msgs/srv/get_node_data.hpp"
+#include "rtabmap_msgs/srv/get_map.hpp"
+#include "rtabmap_msgs/srv/get_map2.hpp"
+#include "rtabmap_msgs/srv/list_labels.hpp"
+#include "rtabmap_msgs/srv/publish_map.hpp"
+#include "rtabmap_msgs/srv/set_goal.hpp"
+#include "rtabmap_msgs/srv/set_label.hpp"
+#include "rtabmap_msgs/srv/remove_label.hpp"
+#include "rtabmap_msgs/msg/goal.hpp"
+#include "rtabmap_msgs/srv/get_plan.hpp"
+#include "rtabmap_sync/CommonDataSubscriber.h"
+
+#include "rtabmap_msgs/msg/odom_info.hpp"
+#include "rtabmap_msgs/msg/info.hpp"
+#include "rtabmap_msgs/msg/landmark_detection.hpp"
+#include "rtabmap_msgs/msg/landmark_detections.hpp"
+#include "rtabmap_msgs/srv/get_nodes_in_radius.hpp"
+#include "rtabmap_msgs/srv/load_database.hpp"
+#include "rtabmap_msgs/srv/detect_more_loop_closures.hpp"
+#include "rtabmap_msgs/srv/global_bundle_adjustment.hpp"
+#include "rtabmap_msgs/srv/cleanup_local_grids.hpp"
+#include "rtabmap_msgs/srv/add_link.hpp"
+
+#include "rtabmap_util/MapsManager.h"
+
+#include "rtabmap_util/ULogToRosout.h"
+
+#ifdef WITH_OCTOMAP_MSGS
+#include <octomap_msgs/srv/get_octomap.hpp>
+#endif
+
+#ifdef WITH_APRILTAG_MSGS
+#include <apriltag_msgs/msg/april_tag_detection_array.hpp>
+#endif
+
+#ifdef WITH_ARUCO_MSGS
+#include <aruco_msgs/msg/marker_array.hpp>
+#endif
+
+#ifdef WITH_ARUCO_OPENCV_MSGS
+#include <aruco_opencv_msgs/msg/aruco_detection.hpp>
+#endif
+
+#ifdef WITH_ARUCO_MARKERS_MSGS
+#include <aruco_markers_msgs/msg/marker_array.hpp>
+#endif
+
+#ifdef WITH_ROS2_ARUCO_INTERFACES
+#include <ros2_aruco_interfaces/msg/aruco_markers.hpp>
+#endif
+
+#ifdef WITH_NAV2_MSGS
+#include <nav2_msgs/action/navigate_to_pose.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#endif
+
+//#define WITH_FIDUCIAL_MSGS
+#ifdef WITH_FIDUCIAL_MSGS
+#include <fiducial_msgs/FiducialTransformArray.h>
+#endif
+
+namespace rtabmap {
+class StereoDense;
+}
+
+namespace rtabmap_slam {
+
+class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscriber
+{
+public:
+	RTABMAP_SLAM_PUBLIC
+	explicit CoreWrapper(const rclcpp::NodeOptions & options);
+	virtual ~CoreWrapper();
+
+#ifdef WITH_NAV2_MSGS
+	using NavigateToPose = nav2_msgs::action::NavigateToPose;
+	using GoalHandleNav2 = rclcpp_action::ClientGoalHandle<NavigateToPose>;
+#endif
+
+private:
+	bool odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Time stamp);
+	bool odomTFUpdate(const std::string & odomFrameId, const rclcpp::Time & stamp); // TF odom
+
+	// Callback called from sync thread
+	virtual void commonMultiCameraCallback(
+				const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
+				const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
+				const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+				const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+				const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
+				const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
+				const sensor_msgs::msg::LaserScan & scanMsg,
+				const sensor_msgs::msg::PointCloud2 & scan3dMsg,
+				const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
+				const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_msgs::msg::GlobalDescriptor>(),
+				const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> >(),
+				const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_msgs::msg::Point3f> >(),
+				const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>());
+	// Callback called from sync thread
+	void commonMultiCameraCallbackImpl(
+				const std::string & odomFrameId,
+				const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
+				const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+				const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+				const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
+				const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
+				const sensor_msgs::msg::LaserScan & scan2dMsg,
+				const sensor_msgs::msg::PointCloud2 & scan3dMsg,
+				const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
+				const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs,
+				const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints,
+				const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d,
+				const std::vector<cv::Mat> & localDescriptors);
+	// Callback called from sync thread
+	virtual void commonLaserScanCallback(
+				const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
+				const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
+				const sensor_msgs::msg::LaserScan & scanMsg,
+				const sensor_msgs::msg::PointCloud2 & scan3dMsg,
+				const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
+				const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor = rtabmap_msgs::msg::GlobalDescriptor());
+	// Callback called from sync thread
+	virtual void commonOdomCallback(
+			const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
+			const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
+			const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg);
+
+	// Callback called from sync thread
+	virtual void commonSensorDataCallback(
+			const rtabmap_msgs::msg::SensorData::ConstSharedPtr & sensorDataMsg,
+			const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
+			const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr & odomInfoMsg);
+
+	void defaultCallback(const sensor_msgs::msg::Image::ConstSharedPtr imageMsg); // no odom
+
+	void userDataAsyncCallback(const rtabmap_msgs::msg::UserData::SharedPtr dataMsg);
+	void globalPoseAsyncCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr globalPoseMsg);
+	void gpsFixAsyncCallback(const sensor_msgs::msg::NavSatFix::SharedPtr gpsFixMsg);
+	void landmarkDetectionAsyncCallback(const rtabmap_msgs::msg::LandmarkDetection::SharedPtr landmarkDetection);
+	void landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::LandmarkDetections::SharedPtr landmarkDetections);
+#ifdef WITH_APRILTAG_MSGS
+	void tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg);
+	void apriltagAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg);
+#endif
+#ifdef WITH_ARUCO_MSGS
+	void arucoAsyncCallback(const aruco_msgs::msg::MarkerArray::SharedPtr msg);
+#endif
+#ifdef WITH_ARUCO_OPENCV_MSGS
+	void arucoOpencvAsyncCallback(const aruco_opencv_msgs::msg::ArucoDetection::SharedPtr msg);
+#endif
+#ifdef WITH_ARUCO_MARKERS_MSGS
+	void arucoMarkersAsyncCallback(const aruco_markers_msgs::msg::MarkerArray::SharedPtr msg);
+#endif
+#ifdef WITH_ROS2_ARUCO_INTERFACES
+	void arucoInterfacesAsyncCallback(const ros2_aruco_interfaces::msg::ArucoMarkers::SharedPtr msg);
+#endif
+#ifdef WITH_FIDUCIAL_MSGS
+	void fiducialDetectionsAsyncCallback(const fiducial_msgs::msgs::FiducialTransformArray::SharedPtr fiducialDetections);
+#endif
+	void imuAsyncCallback(const sensor_msgs::msg::Imu::SharedPtr msg);
+	void republishNodeDataCallback(const std_msgs::msg::Int32MultiArray::ConstSharedPtr msg);
+	void interOdomCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
+	void interOdomInfoCallback(const nav_msgs::msg::Odometry::ConstSharedPtr & msg1, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr & msg2);
+
+	void initialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+
+	void goalCommonCallback(int id,
+			const std::string & label,
+			const std::string & frameId,
+			const rtabmap::Transform & pose,
+			const rclcpp::Time & stamp,
+			double * planningTime = 0);
+	void goalCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
+	void goalNodeCallback(const rtabmap_msgs::msg::Goal::SharedPtr msg);
+	void updateGoal(const rclcpp::Time & stamp);
+
+	void processAsync();
+
+	void process(
+			const rclcpp::Time & stamp,
+			rtabmap::SensorData & data,
+			const rtabmap::Transform & odom = rtabmap::Transform(),
+			const std::vector<float> & odomVelocity = std::vector<float>(),
+			const std::string & odomFrameId = "",
+			const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
+			const rtabmap::OdometryInfo & odomInfo = rtabmap::OdometryInfo(),
+			double timeMsgConversion = 0.0);
+	std::map<int, rtabmap::Transform> filterNodesToAssemble(
+			const std::map<int, rtabmap::Transform> & nodes,
+			const rtabmap::Transform & currentPose);
+
+	void updateRtabmapCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void resetRtabmapCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void pauseRtabmapCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void resumeRtabmapCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void loadDatabaseCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::LoadDatabase::Request>, std::shared_ptr<rtabmap_msgs::srv::LoadDatabase::Response>);
+	void triggerNewMapCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void backupDatabaseCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void detectMoreLoopClosuresCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::DetectMoreLoopClosures::Request>, std::shared_ptr<rtabmap_msgs::srv::DetectMoreLoopClosures::Response>);
+	void globalBundleAdjustmentCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::GlobalBundleAdjustment::Request>, std::shared_ptr<rtabmap_msgs::srv::GlobalBundleAdjustment::Response>);
+	void cleanupLocalGridsCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::CleanupLocalGrids::Request>, std::shared_ptr<rtabmap_msgs::srv::CleanupLocalGrids::Response>);
+	void setModeLocalizationCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void setModeMappingCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void setLogDebug(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void setLogInfo(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void setLogWarn(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void setLogError(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void getNodeDataCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::GetNodeData::Request>, std::shared_ptr<rtabmap_msgs::srv::GetNodeData::Response>);
+	void getMapDataCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::GetMap::Request>, std::shared_ptr<rtabmap_msgs::srv::GetMap::Response>);
+	void getMapData2Callback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::GetMap2::Request>, std::shared_ptr<rtabmap_msgs::srv::GetMap2::Response>);
+	void getMapCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<nav_msgs::srv::GetMap::Request>, std::shared_ptr<nav_msgs::srv::GetMap::Response>);
+	void getProbMapCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<nav_msgs::srv::GetMap::Request>, std::shared_ptr<nav_msgs::srv::GetMap::Response>);
+	void publishMapCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::PublishMap::Request>, std::shared_ptr<rtabmap_msgs::srv::PublishMap::Response>);
+	void getPlanCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<nav_msgs::srv::GetPlan::Request>, std::shared_ptr<nav_msgs::srv::GetPlan::Response>);
+	void getPlanNodesCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::GetPlan::Request>, std::shared_ptr<rtabmap_msgs::srv::GetPlan::Response>);
+	void setGoalCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::SetGoal::Request>, std::shared_ptr<rtabmap_msgs::srv::SetGoal::Response>);
+	void cancelGoalCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void setLabelCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::SetLabel::Request>, std::shared_ptr<rtabmap_msgs::srv::SetLabel::Response>);
+	void listLabelsCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::ListLabels::Request>, std::shared_ptr<rtabmap_msgs::srv::ListLabels::Response> res);
+	void removeLabelCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::RemoveLabel::Request>, std::shared_ptr<rtabmap_msgs::srv::RemoveLabel::Response> res);
+	void addLinkCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::AddLink::Request>, std::shared_ptr<rtabmap_msgs::srv::AddLink::Response> res);
+	void getNodesInRadiusCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::GetNodesInRadius::Request>, std::shared_ptr<rtabmap_msgs::srv::GetNodesInRadius::Response> res);
+
+#ifdef WITH_OCTOMAP_MSGS
+	void octomapBinaryCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<octomap_msgs::srv::GetOctomap::Request>, std::shared_ptr<octomap_msgs::srv::GetOctomap::Response>);
+	void octomapFullCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<octomap_msgs::srv::GetOctomap::Request>, std::shared_ptr<octomap_msgs::srv::GetOctomap::Response>);
+#endif
+
+	void loadParameters(const std::string & configFile, rtabmap::ParametersMap & parameters);
+	void saveParameters(const std::string & configFile);
+
+	void publishStats(const rclcpp::Time & stamp);
+	void publishCurrentGoal(const rclcpp::Time & stamp);
+#ifdef WITH_NAV2_MSGS
+#ifdef NAV_MSGS_FOXY
+	void goalResponseCallback(std::shared_future<GoalHandleNav2::SharedPtr> future);
+#else
+    void goalResponseCallback(const GoalHandleNav2::SharedPtr & goal_handle);
+#endif
+	void resultCallback(const GoalHandleNav2::WrappedResult & result);
+#endif
+
+	void publishLocalPath(const rclcpp::Time & stamp);
+	void publishGlobalPath(const rclcpp::Time & stamp);
+	void republishMaps();
+
+private:
+	rtabmap::Rtabmap rtabmap_;
+	bool paused_;
+
+	UMutex lastPoseMutex_;
+	rtabmap::Transform lastPose_;
+	rclcpp::Time lastPoseStamp_;
+	std::vector<float> lastPoseVelocity_;
+	cv::Mat lastPoseCovariance_;
+	bool lastPoseIntermediate_;
+
+	rtabmap::Transform currentMetricGoal_;
+	rtabmap::Transform lastPublishedMetricGoal_;
+	bool latestNodeWasReached_;
+	bool pubLocPoseOnlyWhenLocalizing_;
+	bool graphLatched_;
+	rtabmap::ParametersMap parameters_;
+	std::map<std::string, float> rtabmapROSStats_;
+
+	std::string frameId_;
+	std::string odomFrameId_;
+	std::string mapFrameId_;
+	std::string groundTruthFrameId_;
+	std::string groundTruthBaseFrameId_;
+	std::string configPath_;
+	std::string databasePath_;
+
+	double tfDelay;
+	double tfTolerance;
+
+	double odomDefaultAngVariance_;
+	double odomDefaultLinVariance_;
+	double landmarkDefaultAngVariance_;
+	double landmarkDefaultLinVariance_;
+	double waitForTransform_;
+	bool useActionForGoal_;
+	bool useSavedMap_;
+	bool genScan_;
+	double genScanMaxDepth_;
+	double genScanMinDepth_;
+	bool genDepth_;
+	int genDepthDecimation_;
+	int genDepthFillHolesSize_;
+	int genDepthFillIterations_;
+	double genDepthFillHolesError_;
+	int scanCloudMaxPoints_;
+	bool scanCloudIs2d_;
+
+	rtabmap::Transform mapToOdom_;
+	std::mutex mapToOdomMutex_;
+
+	rtabmap_util::MapsManager mapsManager_;
+
+	rclcpp::Publisher<rtabmap_msgs::msg::Info>::SharedPtr infoPub_;
+	rclcpp::Publisher<rtabmap_msgs::msg::MapData>::SharedPtr mapDataPub_;
+	rclcpp::Publisher<rtabmap_msgs::msg::MapGraph>::SharedPtr mapGraphPub_;
+	rclcpp::Publisher<rtabmap_msgs::msg::MapGraph>::SharedPtr odomCachePub_;
+	rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr landmarksPub_;
+	rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr labelsPub_;
+	rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr mapPathPub_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr localGridObstacle_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr localGridEmpty_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr localGridGround_;
+	rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr localizationPosePub_;
+	rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initialPoseSub_;
+
+	//Planning stuff
+	rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goalSub_;
+	rclcpp::Subscription<rtabmap_msgs::msg::Goal>::SharedPtr goalNodeSub_;
+	rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr nextMetricGoalPub_;
+	rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr goalReachedPub_;
+	rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr globalPathPub_;
+	rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr localPathPub_;
+	rclcpp::Publisher<rtabmap_msgs::msg::Path>::SharedPtr globalPathNodesPub_;
+	rclcpp::Publisher<rtabmap_msgs::msg::Path>::SharedPtr localPathNodesPub_;
+	std::string goalFrameId_;
+
+	std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
+	std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
+	std::shared_ptr<tf2_ros::TransformListener> tfListener_;
+
+	rclcpp::AsyncParametersClient::SharedPtr parametersClient_;
+	rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameterEventSub_;
+
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr updateSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr resetSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr pauseSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr resumeSrv_;
+	rclcpp::Service<rtabmap_msgs::srv::LoadDatabase>::SharedPtr loadDatabaseSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr triggerNewMapSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr backupDatabase_;
+	rclcpp::Service<rtabmap_msgs::srv::DetectMoreLoopClosures>::SharedPtr detectMoreLoopClosuresSrv_;
+	rclcpp::Service<rtabmap_msgs::srv::GlobalBundleAdjustment>::SharedPtr globalBundleAdjustmentSrv_;
+	rclcpp::Service<rtabmap_msgs::srv::CleanupLocalGrids>::SharedPtr cleanupLocalGridsSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setModeLocalizationSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setModeMappingSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogDebugSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogInfoSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogWarnSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogErrorSrv_;
+	rclcpp::Service<rtabmap_msgs::srv::GetNodeData>::SharedPtr getNodeDataSrv_;
+	rclcpp::Service<rtabmap_msgs::srv::GetMap>::SharedPtr getMapDataSrv_;
+	rclcpp::Service<rtabmap_msgs::srv::GetMap2>::SharedPtr getMapData2Srv_;
+	rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr getMapSrv_;
+	rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr getProbMapSrv_;
+	rclcpp::Service<rtabmap_msgs::srv::PublishMap>::SharedPtr publishMapDataSrv_;
+	rclcpp::Service<nav_msgs::srv::GetPlan>::SharedPtr getPlanSrv_;
+	rclcpp::Service<rtabmap_msgs::srv::GetPlan>::SharedPtr getPlanNodesSrv_;
+	rclcpp::Service<rtabmap_msgs::srv::SetGoal>::SharedPtr setGoalSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr cancelGoalSrv_;
+	rclcpp::Service<rtabmap_msgs::srv::SetLabel>::SharedPtr setLabelSrv_;
+	rclcpp::Service<rtabmap_msgs::srv::ListLabels>::SharedPtr listLabelsSrv_;
+	rclcpp::Service<rtabmap_msgs::srv::RemoveLabel>::SharedPtr removeLabelSrv_;
+	rclcpp::Service<rtabmap_msgs::srv::AddLink>::SharedPtr addLinkSrv_;
+	rclcpp::Service<rtabmap_msgs::srv::GetNodesInRadius>::SharedPtr getNodesInRadiusSrv_;
+#ifdef WITH_OCTOMAP_MSGS
+	rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapBinarySrv_;
+	rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapFullSrv_;
+#endif
+#ifdef WITH_NAV2_MSGS
+	rclcpp_action::Client<NavigateToPose>::SharedPtr nav2Client_;
+	rclcpp_action::GoalUUID lastGoalSent_;
+#endif
+
+	std::thread* transformThread_;
+	bool tfThreadRunning_;
+
+	// for loop closure detection only
+	image_transport::Subscriber defaultSub_;
+
+	rclcpp::CallbackGroup::SharedPtr userDataAsyncCallbackGroup_;
+	rclcpp::Subscription<rtabmap_msgs::msg::UserData>::SharedPtr userDataAsyncSub_;
+	cv::Mat userData_;
+	UMutex userDataMutex_;
+
+	rclcpp::CallbackGroup::SharedPtr globalPoseAsyncCallbackGroup_;
+	rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr globalPoseAsyncSub_;
+	std::map<double, geometry_msgs::msg::PoseWithCovarianceStamped> globalPoses_;
+	UMutex globalPoseMutex_;
+
+	rclcpp::CallbackGroup::SharedPtr gpsAsyncCallbackGroup_;
+	rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gpsFixAsyncSub_;
+	std::map<double, rtabmap::GPS> gps_;
+	UMutex gpsMutex_;
+
+	rclcpp::CallbackGroup::SharedPtr landmarkCallbackGroup_;
+	rclcpp::Subscription<rtabmap_msgs::msg::LandmarkDetection>::SharedPtr landmarkDetectionSub_;
+	rclcpp::Subscription<rtabmap_msgs::msg::LandmarkDetections>::SharedPtr landmarkDetectionsSub_;
+#ifdef WITH_APRILTAG_MSGS
+	rclcpp::Subscription<apriltag_msgs::msg::AprilTagDetectionArray>::SharedPtr tagDetectionsSub_;
+	rclcpp::Subscription<apriltag_msgs::msg::AprilTagDetectionArray>::SharedPtr apriltagSub_;
+#endif
+#ifdef WITH_ARUCO_MSGS
+	rclcpp::Subscription<aruco_msgs::msg::MarkerArray>::SharedPtr arucoSub_;
+#endif
+#ifdef WITH_ARUCO_OPENCV_MSGS
+	rclcpp::Subscription<aruco_opencv_msgs::msg::ArucoDetection>::SharedPtr arucoOpencvSub_;
+#endif
+#ifdef WITH_ARUCO_MARKERS_MSGS
+	rclcpp::Subscription<aruco_markers_msgs::msg::MarkerArray>::SharedPtr arucoMarkersSub_;
+#endif
+#ifdef WITH_ROS2_ARUCO_INTERFACES
+	rclcpp::Subscription<ros2_aruco_interfaces::msg::ArucoMarkers>::SharedPtr arucoInterfacesSub_;
+#endif
+#ifdef WITH_FIDUCIAL_MSGS
+	rclcpp::Subscription<fiducial_msgs::msg::FiducialTransformArray>::SharedPtr fiducialTransfromsSub_;
+#endif
+	std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > landmarks_; // id, <pose, size>
+	UMutex landmarksMutex_;
+
+	rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_;
+	rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imuSub_;
+	std::map<double, rtabmap::Transform> imus_;
+	std::string imuFrameId_;
+	UMutex imuMutex_;
+
+	rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr republishNodeDataSub_;
+
+	rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr interOdomSub_;
+	std::list<std::pair<nav_msgs::msg::Odometry, rtabmap_msgs::msg::OdomInfo> > interOdoms_;
+	message_filters::Subscriber<nav_msgs::msg::Odometry> interOdomSyncSub_;
+	message_filters::Subscriber<rtabmap_msgs::msg::OdomInfo> interOdomInfoSyncSub_;
+	typedef message_filters::sync_policies::ExactTime<nav_msgs::msg::Odometry, rtabmap_msgs::msg::OdomInfo> MyExactInterOdomSyncPolicy;
+	message_filters::Synchronizer<MyExactInterOdomSyncPolicy> * interOdomSync_;
+
+	bool stereoToDepth_;
+	bool odomSensorSync_;
+	float rate_;
+	bool createIntermediateNodes_;
+	int mappingMaxNodes_;
+	double mappingAltitudeDelta_;
+	bool alreadyRectifiedImages_;
+	bool twoDMapping_;
+	rclcpp::Time previousStamp_;
+
+	rtabmap_util::ULogToRosout ulogToRosout_;
+
+	class LocalizationStatusTask : public diagnostic_updater::DiagnosticTask
+	{
+	public:
+		LocalizationStatusTask();
+		void setLocalizationThreshold(double value);
+		void updateStatus(const cv::Mat & covariance, bool twoDMapping);
+		void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
+	private:
+		double localizationThreshold_;
+		double localizationError_;
+	};
+	LocalizationStatusTask localizationDiagnostic_;
+
+	rclcpp::CallbackGroup::SharedPtr processingCallbackGroup_;
+	struct SyncData {
+		bool valid;
+		rclcpp::Time stamp;
+		rtabmap::SensorData data;
+		rtabmap::Transform odom;
+		std::vector<float> odomVelocity;
+		std::string odomFrameId;
+		cv::Mat odomCovariance;
+		rtabmap::OdometryInfo odomInfo;
+		double timeMsgConversion;
+	};
+	rclcpp::TimerBase::SharedPtr syncTimer_;
+	SyncData syncData_;
+	UMutex syncDataMutex_;
+	bool triggerNewMapBeforeNextUpdate_;
+};
+
+}
+
+#endif /* COREWRAPPER_H_ */
+

rtabmap_ros/rtabmap_slam/include/rtabmap_slam/visibility.h

@@ -0,0 +1,59 @@
+// 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_SLAM__VISIBILITY_CONTROL_H_
+#define RTABMAP_SLAM__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_SLAM_EXPORT __attribute__ ((dllexport))
+    #define RTABMAP_SLAM_IMPORT __attribute__ ((dllimport))
+  #else
+    #define RTABMAP_SLAM_EXPORT __declspec(dllexport)
+    #define RTABMAP_SLAM_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef RTABMAP_SLAM_BUILDING_DLL
+    #define RTABMAP_SLAM_PUBLIC RTABMAP_SLAM_EXPORT
+  #else
+    #define RTABMAP_SLAM_PUBLIC RTABMAP_SLAM_IMPORT
+  #endif
+  #define RTABMAP_SLAM_PUBLIC_TYPE RTABMAP_SLAM_PUBLIC
+  #define RTABMAP_SLAM_LOCAL
+#else
+  #define RTABMAP_SLAM_EXPORT __attribute__ ((visibility("default")))
+  #define RTABMAP_SLAM_IMPORT
+  #if __GNUC__ >= 4
+    #define RTABMAP_SLAM_PUBLIC __attribute__ ((visibility("default")))
+    #define RTABMAP_SLAM_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define RTABMAP_SLAM_PUBLIC
+    #define RTABMAP_SLAM_LOCAL
+  #endif
+  #define RTABMAP_SLAM_PUBLIC_TYPE
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // RTABMAP_SLAM__VISIBILITY_CONTROL_H_
+

rtabmap_ros/rtabmap_slam/package.xml

@@ -0,0 +1,42 @@
+<?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_slam</name>
+  <version>0.22.0</version>
+  <description>RTAB-Map's SLAM package.</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>cv_bridge</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>nav2_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>visualization_msgs</depend>
+  <depend>apriltag_msgs</depend>
+  <depend>aruco_msgs</depend>
+  <depend>aruco_opencv_msgs</depend>
+  <!-- depend>aruco_markers_msgs</depend --> <!-- binaries only available on humble -->
+  
+  <depend>rtabmap_msgs</depend>
+  <depend>rtabmap_util</depend>
+  <depend>rtabmap_sync</depend>
+  
+  <export>
+	<build_type>ament_cmake</build_type>
+  </export>
+
+</package>

rtabmap_ros/rtabmap_slam/src/CoreNode.cpp

@@ -0,0 +1,94 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 the Universite de Sherbrooke 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 HOLDER 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.
+*/
+
+#include "rtabmap_slam/CoreWrapper.h"
+#include "rclcpp/rclcpp.hpp"
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UDirectory.h>
+
+int main(int argc, char** argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	// process "--params" argument
+	std::vector<std::string> arguments;
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0)
+		{
+			rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+			uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDCreateOccupancyGrid(), "true")); // default true in ROS
+			char * rosHomePath = getenv("ROS_HOME");
+			std::string workingDir = rosHomePath?rosHomePath:UDirectory::homeDir()+"/.ros";
+			uInsert(parameters,	rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapWorkingDirectory(), workingDir)); // change default to ~/.ros
+
+			if(strcmp(argv[i], "--params") == 0)
+			{
+				// hide specific parameters
+				for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
+				{
+					if(iter->first.find("Odom") == 0)
+					{
+						parameters.erase(iter++);
+					}
+					else
+					{
+						++iter;
+					}
+				}
+			}
+
+			for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+			{
+				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
+				std::cout <<
+						str <<
+						std::setw(60 - str.size()) <<
+						" [" <<
+						rtabmap::Parameters::getDescription(iter->first).c_str() <<
+						"]" <<
+						std::endl;
+			}
+			UWARN("Node will now exit after showing default RTAB-Map parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+		arguments.push_back(argv[i]);
+	}
+
+	rclcpp::init(argc, argv);
+	rclcpp::NodeOptions options;
+	options.arguments(arguments);
+	auto node = std::make_shared<rtabmap_slam::CoreWrapper>(options);
+	rclcpp::executors::MultiThreadedExecutor executor;
+	executor.add_node(node);
+	UINFO("rtabmap %s started...", RTABMAP_VERSION);
+	executor.spin();
+	rclcpp::shutdown();
+	return 0;
+}