mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 16:57:46 +08:00
ported rtabmap_ros_pkg_split to ros2
This commit is contained in:
matlabbe
@@ -0,0 +1,127 @@
|
||||
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()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(cv_bridge REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(nav2_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)
|
||||
|
||||
IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0)
|
||||
ADD_DEFINITIONS("-DNAV_MSGS_FOXY")
|
||||
ENDIF()
|
||||
|
||||
#optional
|
||||
find_package(octomap_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
|
||||
nav2_msgs
|
||||
rclcpp
|
||||
rclcpp_components
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
std_srvs
|
||||
tf2
|
||||
tf2_ros
|
||||
visualization_msgs
|
||||
rtabmap_msgs
|
||||
rtabmap_util
|
||||
rtabmap_sync
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
SET(rtabmap_slam_plugins_lib_src
|
||||
src/CoreWrapper.cpp
|
||||
)
|
||||
|
||||
# If octomap is found, add definition
|
||||
IF(octomap_msgs_FOUND)
|
||||
MESSAGE(STATUS "WITH octomap_msgs")
|
||||
ADD_DEFINITIONS("-DWITH_OCTOMAP_MSGS")
|
||||
ENDIF(octomap_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()
|
||||
@@ -0,0 +1,418 @@
|
||||
/*
|
||||
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/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
|
||||
|
||||
#include <nav2_msgs/action/navigate_to_pose.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
//#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();
|
||||
|
||||
using NavigateToPose = nav2_msgs::action::NavigateToPose;
|
||||
using GoalHandleNav2 = rclcpp_action::ClientGoalHandle<NavigateToPose>;
|
||||
|
||||
private:
|
||||
bool odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Time stamp);
|
||||
bool odomTFUpdate(const rclcpp::Time & stamp); // TF odom
|
||||
|
||||
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>());
|
||||
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);
|
||||
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());
|
||||
virtual void commonOdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
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);
|
||||
#ifdef WITH_APRILTAG_MSGS
|
||||
void tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr tagDetections);
|
||||
#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 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 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);
|
||||
|
||||
void publishLocalPath(const rclcpp::Time & stamp);
|
||||
void publishGlobalPath(const rclcpp::Time & stamp);
|
||||
void republishMaps();
|
||||
|
||||
private:
|
||||
rtabmap::Rtabmap rtabmap_;
|
||||
bool paused_;
|
||||
rtabmap::Transform lastPose_;
|
||||
rclcpp::Time lastPoseStamp_;
|
||||
std::vector<float> lastPoseVelocity_;
|
||||
bool lastPoseIntermediate_;
|
||||
cv::Mat covariance_;
|
||||
rtabmap::Transform currentMetricGoal_;
|
||||
rtabmap::Transform lastPublishedMetricGoal_;
|
||||
bool latestNodeWasReached_;
|
||||
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::SyncParametersClient::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
|
||||
rclcpp_action::Client<NavigateToPose>::SharedPtr nav2Client_;
|
||||
|
||||
std::thread* transformThread_;
|
||||
bool tfThreadRunning_;
|
||||
|
||||
// for loop closure detection only
|
||||
image_transport::Subscriber defaultSub_;
|
||||
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::UserData>::SharedPtr userDataAsyncSub_;
|
||||
cv::Mat userData_;
|
||||
UMutex userDataMutex_;
|
||||
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr globalPoseAsyncSub_;
|
||||
geometry_msgs::msg::PoseWithCovarianceStamped globalPose_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gpsFixAsyncSub_;
|
||||
rtabmap::GPS gps_;
|
||||
#ifdef WITH_APRILTAG_MSGS
|
||||
rclcpp::Subscription<apriltag_msgs::msg::AprilTagDetectionArray>::SharedPtr tagDetectionsSub_;
|
||||
#endif
|
||||
#ifdef WITH_FIDUCIAL_MSGS
|
||||
rclcpp::Subscription<fiducial_msgs::msg::FiducialTransformArray>::SharedPtr fiducialTransfromsSub_;
|
||||
#endif
|
||||
std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > tags_; // id, <pose, size>
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imuSub_;
|
||||
|
||||
std::map<double, rtabmap::Transform> imus_;
|
||||
std::string imuFrameId_;
|
||||
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_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* COREWRAPPER_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_
|
||||
|
||||
@@ -0,0 +1,36 @@
|
||||
<?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.21.0</version>
|
||||
<description>RTAB-Map's SLAM package.</description>
|
||||
<maintainer email="[email protected]">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>
|
||||
|
||||
<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>rtabmap_msgs</depend>
|
||||
<depend>rtabmap_util</depend>
|
||||
<depend>rtabmap_sync</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
|
||||
</package>
|
||||
@@ -0,0 +1,91 @@
|
||||
/*
|
||||
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);
|
||||
UINFO("rtabmap %s started...", RTABMAP_VERSION);
|
||||
rclcpp::spin(std::make_shared<rtabmap_slam::CoreWrapper>(options));
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user