i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'ros2' of github.com:introlab/rtabmap_ros into rolling-devel

Browse Source
This commit is contained in:
matlabbe
2026-06-24 18:04:57 -07:00
parent 5510b54055 2eef2b3231
commit 3464b545b9
161 changed files with 9713 additions and 2149 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_slam/CMakeLists.txt
+51 -4
View File
@@ -54,6 +54,22 @@ include_directories(
# libraries
SET(Libraries
cv_bridge::cv_bridge
geometry_msgs::geometry_msgs
nav_msgs::nav_msgs
rclcpp::rclcpp
rclcpp_components::component
sensor_msgs::sensor_msgs
std_msgs::std_msgs
std_srvs::std_srvs
tf2::tf2
tf2_ros::tf2_ros
visualization_msgs::visualization_msgs
rtabmap_msgs::rtabmap_msgs
rtabmap_util::rtabmap_util
rtabmap_sync::rtabmap_sync
)
SET(AmentLibraries
cv_bridge
geometry_msgs
nav_msgs
@@ -74,6 +90,10 @@ if("$ENV{ROS_DISTRO}" STRLESS "jazzy")
add_definitions(-DPRE_ROS_JAZZY)
endif()
IF("$ENV{ROS_DISTRO}" STRLESS "lyrical")
add_definitions(-DPRE_ROS_LYRICAL)
ENDIF()
###########
## Build ##
###########
@@ -88,6 +108,10 @@ MESSAGE(STATUS "WITH apriltag_msgs")
ADD_DEFINITIONS("-DWITH_APRILTAG_MSGS")
SET(Libraries
${Libraries}
apriltag_msgs::apriltag_msgs
)
SET(AmentLibraries
${AmentLibraries}
apriltag_msgs
)
ENDIF(apriltag_msgs_FOUND)
@@ -98,6 +122,10 @@ MESSAGE(STATUS "WITH aruco_msgs")
ADD_DEFINITIONS("-DWITH_ARUCO_MSGS")
SET(Libraries
${Libraries}
aruco_msgs::aruco_msgs
)
SET(AmentLibraries
${AmentLibraries}
aruco_msgs
)
ENDIF(aruco_msgs_FOUND)
@@ -108,6 +136,10 @@ MESSAGE(STATUS "WITH aruco_opencv_msgs")
ADD_DEFINITIONS("-DWITH_ARUCO_OPENCV_MSGS")
SET(Libraries
${Libraries}
aruco_opencv_msgs::aruco_opencv_msgs
)
SET(AmentLibraries
${AmentLibraries}
aruco_opencv_msgs
)
ENDIF(aruco_opencv_msgs_FOUND)
@@ -118,6 +150,10 @@ MESSAGE(STATUS "WITH aruco_markers_msgs")
ADD_DEFINITIONS("-DWITH_ARUCO_MARKERS_MSGS")
SET(Libraries
${Libraries}
aruco_markers_msgs::aruco_markers_msgs
)
SET(AmentLibraries
${AmentLibraries}
aruco_markers_msgs
)
ENDIF(aruco_markers_msgs_FOUND)
@@ -128,6 +164,10 @@ MESSAGE(STATUS "WITH ros2_aruco_interfaces")
ADD_DEFINITIONS("-DWITH_ROS2_ARUCO_INTERFACES")
SET(Libraries
${Libraries}
ros2_aruco_interfaces::ros2_aruco_interfaces
)
SET(AmentLibraries
${AmentLibraries}
ros2_aruco_interfaces
)
ENDIF(ros2_aruco_interfaces_FOUND)
@@ -138,6 +178,10 @@ MESSAGE(STATUS "WITH nav2_msgs")
ADD_DEFINITIONS("-DWITH_NAV2_MSGS")
SET(Libraries
${Libraries}
nav2_msgs::nav2_msgs
)
SET(AmentLibraries
${AmentLibraries}
nav2_msgs
)
IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0)
@@ -157,20 +201,23 @@ target_include_directories(rtabmap_slam_plugins
$<INSTALL_INTERFACE:include>
)
ament_target_dependencies(rtabmap_slam_plugins ${Libraries})
if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
ament_target_dependencies(rtabmap_slam_plugins ${AmentLibraries})
else()
target_link_libraries(rtabmap_slam_plugins PUBLIC ${Libraries})
endif()
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)
target_link_libraries(rtabmap_node PRIVATE rtabmap_slam_plugins)
set_target_properties(rtabmap_node PROPERTIES OUTPUT_NAME "rtabmap")
#############
## Install ##
#############
ament_export_dependencies(${Libraries})
ament_export_dependencies(${AmentLibraries})
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
rtabmap_slam/include/rtabmap_slam/CoreWrapper.h
+13 -3
View File
@@ -33,9 +33,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <std_srvs/srv/empty.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/buffer.hpp>
#include <tf2_ros/transform_listener.hpp>
#include <tf2_ros/transform_broadcaster.hpp>
#include <std_msgs/msg/empty.hpp>
#include <std_msgs/msg/int32.hpp>
@@ -69,6 +69,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_msgs/msg/info.hpp"
#include "rtabmap_msgs/msg/landmark_detection.hpp"
#include "rtabmap_msgs/msg/landmark_detections.hpp"
#include "rtabmap_msgs/msg/env_sensor.h"
#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"
@@ -191,6 +192,7 @@ private:
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 envSensorAsyncCallback(const rtabmap_msgs::msg::EnvSensor::SharedPtr envSensorMsg);
void landmarkDetectionAsyncCallback(const rtabmap_msgs::msg::LandmarkDetection::SharedPtr landmarkDetection);
void landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::LandmarkDetections::SharedPtr landmarkDetections);
#ifdef WITH_APRILTAG_MSGS
@@ -243,6 +245,8 @@ private:
std::map<int, rtabmap::Transform> filterNodesToAssemble(
const std::map<int, rtabmap::Transform> & nodes,
const rtabmap::Transform & currentPose);
void applyParameters();
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>);
@@ -334,6 +338,7 @@ private:
double landmarkDefaultAngVariance_;
double landmarkDefaultLinVariance_;
double waitForTransform_;
double stalenessFactor_;
bool useActionForGoal_;
bool useSavedMap_;
bool genScan_;
@@ -444,6 +449,11 @@ private:
std::map<double, rtabmap::GPS> gps_;
UMutex gpsMutex_;
rclcpp::CallbackGroup::SharedPtr envSensorAsyncCallbackGroup_;
rclcpp::Subscription<rtabmap_msgs::msg::EnvSensor>::SharedPtr envSensorAsyncSub_;
rtabmap::EnvSensors envSensors_;
UMutex envSensorMutex_;
rclcpp::CallbackGroup::SharedPtr landmarkCallbackGroup_;
rclcpp::Subscription<rtabmap_msgs::msg::LandmarkDetection>::SharedPtr landmarkDetectionSub_;
rclcpp::Subscription<rtabmap_msgs::msg::LandmarkDetections>::SharedPtr landmarkDetectionsSub_;
rtabmap_slam/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?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.1</version>
<version>0.23.7</version>
<description>RTAB-Map's SLAM package.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_slam/src/CoreWrapper.cpp
+186 -54
View File
@@ -121,6 +121,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
landmarkDefaultAngVariance_(0.001),
landmarkDefaultLinVariance_(0.001),
waitForTransform_(0.2),// 200 ms
stalenessFactor_(0.0),
useActionForGoal_(false),
useSavedMap_(true),
genScan_(false),
@@ -207,6 +208,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
pubLocPoseOnlyWhenLocalizing_ = this->declare_parameter("pub_loc_pose_only_when_localizing", pubLocPoseOnlyWhenLocalizing_);
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
stalenessFactor_ = this->declare_parameter("staleness_factor", stalenessFactor_);
initialPoseStr = this->declare_parameter("initial_pose", initialPoseStr);
useActionForGoal_ = this->declare_parameter("use_action_for_goal", useActionForGoal_);
#ifndef WITH_NAV2_MSGS
@@ -231,26 +233,26 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
stereoToDepth_ = this->declare_parameter("stereo_to_depth", stereoToDepth_);
odomSensorSync_ = this->declare_parameter("odom_sensor_sync", odomSensorSync_);
RCLCPP_INFO(this->get_logger(), "rtabmap: frame_id = %s", frameId_.c_str());
if(!odomFrameId_.empty())
{
RCLCPP_INFO(this->get_logger(), "rtabmap: odom_frame_id = %s", odomFrameId_.c_str());
}
if(!groundTruthFrameId_.empty())
{
RCLCPP_INFO(this->get_logger(), "rtabmap: ground_truth_frame_id = %s -> ground_truth_base_frame_id = %s",
groundTruthFrameId_.c_str(),
groundTruthBaseFrameId_.c_str());
}
RCLCPP_INFO(this->get_logger(), "rtabmap: map_frame_id = %s", mapFrameId_.c_str());
RCLCPP_INFO(this->get_logger(), "rtabmap: frame_id = \"%s\"", frameId_.c_str());
RCLCPP_INFO(this->get_logger(), "rtabmap: odom_frame_id = \"%s\"", odomFrameId_.c_str());
RCLCPP_INFO(this->get_logger(), "rtabmap: ground_truth_frame_id = \"%s\" -> ground_truth_base_frame_id = \"%s\"",
groundTruthFrameId_.c_str(),
groundTruthBaseFrameId_.c_str());
RCLCPP_INFO(this->get_logger(), "rtabmap: map_frame_id = \"%s\"", mapFrameId_.c_str());
RCLCPP_INFO(this->get_logger(), "rtabmap: log_to_rosout_level = %d", eventLevel);
RCLCPP_INFO(this->get_logger(), "rtabmap: initial_pose = %s", initialPoseStr.c_str());
RCLCPP_INFO(this->get_logger(), "rtabmap: initial_pose = \"%s\"", initialPoseStr.c_str());
RCLCPP_INFO(this->get_logger(), "rtabmap: use_action_for_goal = %s", useActionForGoal_?"true":"false");
RCLCPP_INFO(this->get_logger(), "rtabmap: tf_delay = %f", tfDelay);
RCLCPP_INFO(this->get_logger(), "rtabmap: tf_tolerance = %f", tfTolerance);
RCLCPP_INFO(this->get_logger(), "rtabmap: odom_sensor_sync = %s", odomSensorSync_?"true":"false");
RCLCPP_INFO(this->get_logger(), "rtabmap: pub_loc_pose_only_when_localizing = %s", pubLocPoseOnlyWhenLocalizing_?"true":"false");
RCLCPP_INFO(this->get_logger(), "rtabmap: wait_for_transform = %f", waitForTransform_);
if(stalenessFactor_!=0.0 && stalenessFactor_ < 1.0) {
RCLCPP_ERROR(this->get_logger(), "rtabmap: staleness_factor should be 0 (disabled) or >= 1 (value that multiplies the detection update period). Current value is %f, setting it to 0...",
stalenessFactor_);
stalenessFactor_ = 0.0;
}
RCLCPP_INFO(this->get_logger(), "rtabmap: staleness_factor = %f", stalenessFactor_);
if(this->isSubscribedToStereo())
{
RCLCPP_INFO(this->get_logger(), "rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false");
@@ -768,9 +770,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
Parameters::kRGBDEnabled().c_str(),
Parameters::kRGBDEnabled().c_str());
}
image_transport::TransportHints hints(this);
defaultSub_ = image_transport::create_subscription(this, "image", std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(this->getTopicQueueSize()).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile(), subOptions);
std::string imageTopic = this->get_node_topics_interface()->resolve_topic_name("image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a
#ifdef PRE_ROS_LYRICAL
image_transport::TransportHints hints(this); // using "image_transport" parameter
defaultSub_ = image_transport::create_subscription(this, imageTopic, std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(this->getTopicQueueSize()).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile(), subOptions);
#else
image_transport::TransportHints hints(*this); // using "image_transport" parameter
defaultSub_ = image_transport::create_subscription(*this, imageTopic, std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(this->getTopicQueueSize()).reliability((rmw_qos_reliability_policy_t)qosImage_), subOptions);
#endif
RCLCPP_INFO(this->get_logger(), "\n%s subscribed to:\n %s", get_name(), defaultSub_.getTopic().c_str());
}
@@ -831,6 +838,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
rtabmap_.parseParameters(parameters_);
}
}
if(!this->isSubscribedToOdom() && odomFrameId_.empty())
{
bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str());
if(isRGBD) {
RCLCPP_ERROR(this->get_logger(), "\"subscribe_odom\" or \"odom_frame_id\" should be used when \"%s\" is enabled!", Parameters::kRGBDEnabled().c_str());
}
}
// Set initial pose if set
if(!initialPoseStr.empty())
@@ -861,24 +876,30 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
gpsAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
landmarkCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
imuCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
envSensorAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions userDataAsyncSubOptions;
rclcpp::SubscriptionOptions globalPoseAsyncSubOptions;
rclcpp::SubscriptionOptions gpsAsyncSubOptions;
rclcpp::SubscriptionOptions landmarkSubOptions;
rclcpp::SubscriptionOptions imuSubOptions;
rclcpp::SubscriptionOptions envSensorAsyncSubOptions;
userDataAsyncSubOptions.callback_group = userDataAsyncCallbackGroup_;
globalPoseAsyncSubOptions.callback_group = globalPoseAsyncCallbackGroup_;
gpsAsyncSubOptions.callback_group = gpsAsyncCallbackGroup_;
landmarkSubOptions.callback_group = imuCallbackGroup_;
imuSubOptions.callback_group = imuCallbackGroup_;
envSensorAsyncSubOptions.callback_group = envSensorAsyncCallbackGroup_;
int qosGPS = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
int qosIMU = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
int qosEnvSensor = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
qosGPS = this->declare_parameter("qos_gps", qosGPS);
qosIMU = this->declare_parameter("qos_imu", qosIMU);
qosEnvSensor = this->declare_parameter("qos_env_sensor", qosEnvSensor);
userDataAsyncSub_ = this->create_subscription<rtabmap_msgs::msg::UserData>("user_data_async", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosUserData_), std::bind(&CoreWrapper::userDataAsyncCallback, this, std::placeholders::_1), userDataAsyncSubOptions);
globalPoseAsyncSub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", 1, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1), globalPoseAsyncSubOptions);
gpsFixAsyncSub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>("gps/fix", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1), gpsAsyncSubOptions);
envSensorAsyncSub_ = this->create_subscription<rtabmap_msgs::msg::EnvSensor>("env_sensor", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosEnvSensor), std::bind(&CoreWrapper::envSensorAsyncCallback, this, std::placeholders::_1), envSensorAsyncSubOptions);
landmarkDetectionSub_ = this->create_subscription<rtabmap_msgs::msg::LandmarkDetection>("landmark_detection", 1, std::bind(&CoreWrapper::landmarkDetectionAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
landmarkDetectionsSub_ = this->create_subscription<rtabmap_msgs::msg::LandmarkDetections>("landmark_detections", 1, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
#ifdef WITH_APRILTAG_MSGS
@@ -947,17 +968,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
parameters_.at(key) = vStr;
}
}
RCLCPP_INFO(this->get_logger(), "rtabmap: Updating parameters");
if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end())
{
rate_ = uStr2Float(parameters_.at(Parameters::kRtabmapDetectionRate()));
RCLCPP_INFO(this->get_logger(), "RTAB-Map rate detection = %f Hz", rate_);
}
rtabmap_.parseParameters(parameters_);
// Don't reset map in localization mode
if(rtabmap_.getMemory()->isIncremental()) {
mapsManager_.setParameters(parameters_);
}
applyParameters();
}
};
@@ -982,19 +993,28 @@ CoreWrapper::~CoreWrapper()
this->saveParameters(configPath_);
printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str());
bool saveDatabase = true;
if(rtabmap_.getMemory())
{
// save the grid map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
if(!pixels.empty())
if(!rtabmap_.getMemory()->isReadOnly())
{
printf("rtabmap: 2D occupancy grid map saved.\n");
rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize);
// save the grid map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
if(!pixels.empty())
{
printf("rtabmap: 2D occupancy grid map saved.\n");
rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize);
}
}
else
{
printf("rtabmap: Database is read-only, the current state of the memory is not saved.\n");
saveDatabase = false;
}
}
rtabmap_.close();
rtabmap_.close(saveDatabase);
printf("rtabmap: Saving database/long-term memory...done! (located at %s, %ld MB)\n", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024));
delete interOdomSync_;
@@ -1137,6 +1157,26 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti
UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg.pose.covariance[0], odomMsg.twist.covariance[0]));
triggerNewMapBeforeNextUpdate_ = true;
lastPoseCovariance_ = cv::Mat();
}
else if(stalenessFactor_>0.0 &&
previousStamp_.seconds() > 0.0 &&
rate_>0.0f &&
(stamp - previousStamp_).seconds() > stalenessFactor_/rate_)
{
UWARN("The time difference (%f s) between the new timestamp received (%f) and "
"the previous one (%f) is way over than the expected update period (%s=%f Hz) "
"%f x staleness_factor (%f) = %f s. Triggering a new map! Set staleness_factor to 0 "
"to avoid triggering a new map when this happens.",
(stamp - previousStamp_).seconds(),
stamp.seconds(),
previousStamp_.seconds(),
Parameters::kRtabmapDetectionRate().c_str(),
rate_,
1.0f/rate_,
stalenessFactor_,
stalenessFactor_/rate_);
triggerNewMapBeforeNextUpdate_ = true;
lastPoseCovariance_ = cv::Mat();
}
lastPoseIntermediate_ = false;
@@ -1228,6 +1268,26 @@ bool CoreWrapper::odomTFUpdate(const std::string & odomFrameId, const rclcpp::Ti
triggerNewMapBeforeNextUpdate_ = true;
lastPoseCovariance_ = cv::Mat();
}
else if(stalenessFactor_>0.0 &&
previousStamp_.seconds() > 0.0 &&
rate_>0.0f &&
(stamp - previousStamp_).seconds() > stalenessFactor_/rate_)
{
UWARN("The time difference (%f s) between the new timestamp received (%f) and "
"the previous one (%f) is way over than the expected update period (%s=%f Hz) "
"%f x staleness_factor (%f) = %f s. Triggering a new map! Set staleness_factor to 0 "
"to avoid triggering a new map when this happens.",
(stamp - previousStamp_).seconds(),
stamp.seconds(),
previousStamp_.seconds(),
Parameters::kRtabmapDetectionRate().c_str(),
rate_,
1.0f/rate_,
stalenessFactor_,
stalenessFactor_/rate_);
triggerNewMapBeforeNextUpdate_ = true;
lastPoseCovariance_ = cv::Mat();
}
lastPoseIntermediate_ = false;
lastPose_ = odom;
@@ -1307,6 +1367,11 @@ void CoreWrapper::commonMultiCameraCallback(
mapToOdomMutex_.lock();
odomFrameId = odomFrameId_;
mapToOdomMutex_.unlock();
if(odomFrameId.empty())
{
RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set.");
return;
}
if(!scan2dMsg.ranges.empty())
{
if(!odomTFUpdate(odomFrameId, scan2dMsg.header.stamp))
@@ -1693,6 +1758,11 @@ void CoreWrapper::commonLaserScanCallback(
mapToOdomMutex_.lock();
odomFrameId = odomFrameId_;
mapToOdomMutex_.unlock();
if(odomFrameId.empty())
{
RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set.");
return;
}
if(!scan2dMsg.ranges.empty())
{
if(!odomTFUpdate(odomFrameId, scan2dMsg.header.stamp))
@@ -1902,6 +1972,11 @@ void CoreWrapper::commonSensorDataCallback(
mapToOdomMutex_.lock();
odomFrameId = odomFrameId_;
mapToOdomMutex_.unlock();
if(odomFrameId.empty())
{
RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set.");
return;
}
if(!odomTFUpdate(odomFrameId, sensorDataMsg->header.stamp))
{
return;
@@ -2190,6 +2265,16 @@ void CoreWrapper::process(
data.setLandmarks(landmarks);
}
// Env sensors
{
UScopeMutex lock(envSensorMutex_);
if(!envSensors_.empty())
{
data.setEnvSensors(envSensors_);
envSensors_.clear();
}
}
// IMU
imuMutex_.lock();
if(!imus_.empty())
@@ -2350,8 +2435,8 @@ void CoreWrapper::process(
if(rtabmap_.getMemory() == 0 ||
filteredPoses.size() == 0 ||
rtabmap_.getMemory()->getLastSignatureId() != filteredPoses.rbegin()->first ||
rtabmap_.getMemory()->getLastWorkingSignature() == 0 ||
rtabmap_.getMemory()->getLastWorkingSignature()->sensorData().gridCellSize() == 0 ||
rtabmap_.getMemory()->getLastWorkingSignature(false) == 0 ||
rtabmap_.getMemory()->getLastWorkingSignature(false)->sensorData().gridCellSize() == 0 ||
(!mapsManager_.getLocalMapMaker()->isGridFromDepth() && data.laserScanRaw().is2d())) // 2d laser scan would fill empty space for latest data
{
SensorData tmpData = data;
@@ -2629,6 +2714,17 @@ void CoreWrapper::gpsFixAsyncCallback(const sensor_msgs::msg::NavSatFix::SharedP
}
}
void CoreWrapper::envSensorAsyncCallback(const rtabmap_msgs::msg::EnvSensor::SharedPtr envSensorMsg)
{
if(!paused_)
{
// Can only insert one value for each type per node, keep the most recent
EnvSensor value = rtabmap_conversions::envSensorFromROS(*envSensorMsg);
UScopeMutex lock(envSensorMutex_);
uInsert(envSensors_, std::make_pair(value.type(), value));
}
}
void CoreWrapper::landmarkDetectionAsyncCallback(const rtabmap_msgs::msg::LandmarkDetection::SharedPtr landmarkDetection)
{
if(!paused_)
@@ -3117,6 +3213,11 @@ void CoreWrapper::updateRtabmapCallback(
}
}
}
applyParameters();
}
void CoreWrapper::applyParameters()
{
RCLCPP_INFO(get_logger(), "rtabmap: Updating parameters");
if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end())
{
@@ -3186,6 +3287,9 @@ void CoreWrapper::resetRtabmapCallback(
userDataMutex_.lock();
userData_ = cv::Mat();
userDataMutex_.unlock();
envSensorMutex_.lock();
envSensors_.clear();
envSensorMutex_.unlock();
imuMutex_.lock();
imus_.clear();
imuFrameId_.clear();
@@ -3251,18 +3355,27 @@ void CoreWrapper::loadDatabaseCallback(
// Close old database
RCLCPP_INFO(get_logger(), "LoadDatabase: Saving current map (%s)...", databasePath_.c_str());
bool saveDatabase = true;
if(rtabmap_.getMemory())
{
// save the grid map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
if(!pixels.empty())
if(!rtabmap_.getMemory()->isReadOnly())
{
printf("rtabmap: 2D occupancy grid map saved.\n");
rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize);
// save the grid map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
if(!pixels.empty())
{
printf("rtabmap: 2D occupancy grid map saved.\n");
rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize);
}
}
else
{
printf("rtabmap: Database is read-only, the current state of the memory is not saved.\n");
saveDatabase = false;
}
}
rtabmap_.close();
rtabmap_.close(saveDatabase);
RCLCPP_INFO(get_logger(), "LoadDatabase: Saving current map (%s, %ld MB)... done!", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024));
lastPoseMutex_.lock();
@@ -3288,6 +3401,9 @@ void CoreWrapper::loadDatabaseCallback(
userDataMutex_.lock();
userData_ = cv::Mat();
userDataMutex_.unlock();
envSensorMutex_.lock();
envSensors_.clear();
envSensorMutex_.unlock();
imuMutex_.lock();
imus_.clear();
imuFrameId_.clear();
@@ -3404,18 +3520,27 @@ void CoreWrapper::backupDatabaseCallback(
std::shared_ptr<std_srvs::srv::Empty::Response>)
{
RCLCPP_INFO(this->get_logger(), "Backup: Saving memory...");
bool saveDatabase = true;
if(rtabmap_.getMemory())
{
// save the grid map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
if(!pixels.empty())
if(!rtabmap_.getMemory()->isReadOnly())
{
printf("rtabmap: 2D occupancy grid map saved.\n");
rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize);
// save the grid map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
if(!pixels.empty())
{
printf("rtabmap: 2D occupancy grid map saved.\n");
rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize);
}
}
else
{
printf("rtabmap: Database is read-only, the current state of the memory is not saved.\n");
saveDatabase = false;
}
}
rtabmap_.close();
rtabmap_.close(saveDatabase);
RCLCPP_INFO(this->get_logger(), "Backup: Saving memory... done!");
lastPoseMutex_.lock();
@@ -3436,6 +3561,9 @@ void CoreWrapper::backupDatabaseCallback(
userDataMutex_.unlock();
globalPoses_.clear();
gps_.clear();
envSensorMutex_.lock();
envSensors_.clear();
envSensorMutex_.unlock();
landmarksMutex_.lock();
landmarks_.clear();
landmarksMutex_.unlock();
@@ -3721,9 +3849,9 @@ void CoreWrapper::getNodeDataCallback(
req->grid?"true":"false",
req->user_data?"true":"false");
if(req->ids.empty() && rtabmap_.getMemory() && rtabmap_.getMemory()->getLastWorkingSignature())
if(req->ids.empty() && rtabmap_.getMemory() && rtabmap_.getMemory()->getLastWorkingSignature(true))
{
req->ids.push_back(rtabmap_.getMemory()->getLastWorkingSignature()->id());
req->ids.push_back(rtabmap_.getMemory()->getLastWorkingSignature(true)->id());
}
for(size_t i=0; i<req->ids.size(); ++i)
{
@@ -3761,6 +3889,8 @@ void CoreWrapper::getMapDataCallback(
!req->graph_only,
!req->graph_only,
!req->graph_only,
!req->graph_only,
!req->graph_only,
!req->graph_only);
mapToOdomMutex_.lock();
@@ -3787,13 +3917,15 @@ void CoreWrapper::getMapData2Callback(
const std::shared_ptr<rtabmap_msgs::srv::GetMap2::Request> req,
std::shared_ptr<rtabmap_msgs::srv::GetMap2::Response> res)
{
RCLCPP_INFO(get_logger(), "rtabmap: Getting map (global=%s optimized=%s with_images=%s with_scans=%s with_user_data=%s with_grids=%s)...",
RCLCPP_INFO(get_logger(), "rtabmap: Getting map (global=%s optimized=%s with_images=%s with_scans=%s with_user_data=%s with_grids=%s with_words=%s with_global_descriptors=%s)...",
req->global_map?"true":"false",
req->optimized?"true":"false",
req->with_images?"true":"false",
req->with_scans?"true":"false",
req->with_user_data?"true":"false",
req->with_grids?"true":"false");
req->with_grids?"true":"false",
req->with_words?"true":"false",
req->with_global_descriptors?"true":"false");
std::map<int, Signature> signatures;
std::map<int, Transform> poses;
std::multimap<int, rtabmap::Link> constraints;