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

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2023-11-19 17:14:24 -08:00
parent 76e83081a7 67de27b1ee
commit 399fb33310
31 changed files with 1148 additions and 295 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_slam/include/rtabmap_slam/CoreWrapper.h
+18
View File
@@ -156,6 +156,11 @@ private:
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg);
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);
@@ -417,6 +422,19 @@ private:
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_;
};
}
rtabmap_slam/src/CoreWrapper.cpp
+111 -10
View File
@@ -703,8 +703,18 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
Parameters::kOptimizerIterations().c_str(), mapFrameId_.c_str());
}
std::vector<diagnostic_updater::DiagnosticTask*> tasks;
double localizationThreshold = 0.0f;
localizationThreshold = this->declare_parameter("loc_thr", localizationThreshold);
if(rtabmap_.getMemory() && !rtabmap_.getMemory()->isIncremental() && localizationThreshold > 0.0)
{
RCLCPP_INFO(this->get_logger(), "rtabmap: loc_thr = %f", localizationThreshold);
localizationDiagnostic_.setLocalizationThreshold(localizationThreshold);
tasks.push_back(&localizationDiagnostic_);
}
RCLCPP_INFO(this->get_logger(), "Setup callbacks");
setupCallbacks(*this); // do it at the end
setupCallbacks(*this, tasks); // do it at the end
if(!this->isDataSubscribed())
{
bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str());
@@ -728,7 +738,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
!this->isSubscribedToStereo() &&
!this->isSubscribedToRGBD() &&
!this->isSubscribedToRGB() &&
(this->isSubscribedToScan2d() || this->isSubscribedToScan3d() || this->isSubscribedToOdom()))
(this->isSubscribedToScan2d() || this->isSubscribedToScan3d() || this->isSubscribedToOdom()) &&
!this->isSubscribedToSensorData())
{
RCLCPP_WARN(this->get_logger(), "There is no image subscription, bag-of-words loop closure detection will be disabled...");
int kpMaxFeatures = Parameters::defaultKpMaxFeatures();
@@ -1720,6 +1731,51 @@ void CoreWrapper::commonOdomCallback(
covariance_ = cv::Mat();
}
void CoreWrapper::commonSensorDataCallback(
const rtabmap_msgs::msg::SensorData::ConstSharedPtr & sensorDataMsg,
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg)
{
UTimer timerConversion;
UASSERT(sensorDataMsg.get());
std::string odomFrameId = odomFrameId_;
if(odomMsg.get())
{
odomFrameId = odomMsg->header.frame_id;
if(!odomUpdate(*odomMsg, sensorDataMsg->header.stamp))
{
return;
}
}
else if(!odomTFUpdate(sensorDataMsg->header.stamp))
{
return;
}
SensorData data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg);
if(lastPoseIntermediate_)
{
data.setId(-1);
}
OdometryInfo odomInfo;
if(odomInfoMsg.get())
{
odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg);
}
process(lastPoseStamp_,
data,
lastPose_,
lastPoseVelocity_,
odomFrameId,
covariance_,
odomInfo,
timerConversion.ticks());
covariance_ = cv::Mat();
}
void CoreWrapper::process(
const rclcpp::Time & stamp,
SensorData & data,
@@ -2198,6 +2254,13 @@ void CoreWrapper::process(
timePublishMaps = timer.ticks();
}
// If not intermediate node
if(data.id() > 0)
{
localizationDiagnostic_.updateStatus(rtabmap_.getStatistics().localizationCovariance(), twoDMapping_);
tick(stamp, rate_>0?rate_:1000.0/(timeMsgConversion+timeRtabmap+timeUpdateMaps+timePublishMaps));
}
}
else
{
@@ -2219,12 +2282,6 @@ void CoreWrapper::process(
rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeUpdatingMaps/ms"), timeUpdateMaps*1000.0f));
rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimePublishing/ms"), timePublishMaps*1000.0f));
rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeTotal/ms"), (timeMsgConversion+timeRtabmap+timeUpdateMaps+timePublishMaps)*1000.0f));
// If not intermediate node
if(data.id() >= 0)
{
tick(stamp, rate_>0?rate_:1000.0/(timeMsgConversion+timeRtabmap+timeUpdateMaps+timePublishMaps));
}
}
else if(!rtabmap_.isIDsGenerated())
{
@@ -3249,8 +3306,8 @@ void CoreWrapper::getNodeDataCallback(
if(s.id()>0)
{
rtabmap_msgs::msg::NodeData msg;
rtabmap_conversions::nodeDataToROS(s, msg);
rtabmap_msgs::msg::Node msg;
rtabmap_conversions::nodeToROS(s, msg);
res->data.push_back(msg);
}
}
@@ -4486,6 +4543,50 @@ void CoreWrapper::publishGlobalPath(const rclcpp::Time & stamp)
}
}
CoreWrapper::LocalizationStatusTask::LocalizationStatusTask() :
diagnostic_updater::DiagnosticTask("Localization status"),
localizationThreshold_(0.0),
localizationError_(9999)
{}
void CoreWrapper::LocalizationStatusTask::setLocalizationThreshold(double value)
{
localizationThreshold_ = value;
}
void CoreWrapper::LocalizationStatusTask::updateStatus(const cv::Mat & cov, bool twoDMapping)
{
if(localizationThreshold_ > 0.0 && !cov.empty())
{
if(cov.at<double>(0,0) >= 9999.0)
{
localizationError_ = 9999.0;
}
else
{
localizationError_ = sqrt(uMax3(cov.at<double>(0,0), cov.at<double>(1,1), twoDMapping?0.0:cov.at<double>(2,2)));
}
}
}
void CoreWrapper::LocalizationStatusTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if(localizationError_>=9999)
{
stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Not localized!");
}
else if(localizationError_ > localizationThreshold_)
{
stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Localization error is high!");
}
else
{
stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Localized.");
}
stat.add("Localization error (m)", localizationError_);
stat.add("loc_thr (m)", localizationThreshold_);
}
#ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP
void CoreWrapper::octomapBinaryCallback(