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

Added error messages when subscribe_odom and odom_frame_id are both not set. rgb_sync: added option "fill_empty_depth" to add fake empty depth for monocular cameras. See also #1363

Browse Source
This commit is contained in:
matlabbe
2025-10-18 13:59:55 -07:00
parent bc5f0a7146
commit 3b5a4ed675
8 changed files with 82 additions and 220 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_slam/src/CoreWrapper.cpp
+30 -13
View File
@@ -233,20 +233,14 @@ 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);
@@ -840,6 +834,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())
@@ -1356,6 +1358,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))
@@ -1742,6 +1749,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))
@@ -1951,6 +1963,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;