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 (diagnostics, #1046)

Browse Source
This commit is contained in:
matlabbe
2023-10-14 15:22:56 -07:00
parent 0473206ade 61bd84bcd6
commit a3b6f0a88f
34 changed files with 393 additions and 288 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_odom/src/OdometryROS.cpp
+45 -28
View File
@@ -62,9 +62,8 @@ OdometryROS::OdometryROS(const rclcpp::NodeOptions & options) :
OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & options) :
Node(name, options),
rtabmap_sync::SyncDiagnostic(this, 0.5),
odometry_(0),
warningThread_(0),
callbackCalled_(false),
frameId_("base_link"),
odomFrameId_("odom"),
groundTruthFrameId_(""),
@@ -190,13 +189,6 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
OdometryROS::~OdometryROS()
{
if(warningThread_)
{
callbackCalled();
warningThread_->join();
delete warningThread_;
}
delete odometry_;
}
@@ -369,28 +361,21 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
onOdomInit();
}
void OdometryROS::startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync)
void OdometryROS::initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic)
{
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
subscribedTopicsMsg_ = subscribedTopicsMsg;
warningThread_ = new std::thread([&](){
rclcpp::Rate r(1.0/5.0);
while(!callbackCalled_)
{
r.sleep();
if(!callbackCalled_)
{
RCLCPP_WARN(this->get_logger(), "%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
this->get_name(),
approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg_.c_str());
}
}
});
std::vector<diagnostic_updater::DiagnosticTask*> tasks;
tasks.push_back(&statusDiagnostic_);
initDiagnostic(subscribedTopic,
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
this->get_name(),
approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg.c_str()),
tasks);
}
rtabmap::Transform OdometryROS::velocityGuess() const
@@ -945,6 +930,17 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
{
RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (now()-timeStart).seconds());
}
statusDiagnostic_.setStatus(pose.isNull());
if(!pose.isNull())
{
double curentRate = 1.0/(this->now()-timeStart).seconds();
tick(header.stamp,
maxUpdateRate_>0 && maxUpdateRate_ < curentRate ? maxUpdateRate_:
expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
previousStamp_ == 0.0 || rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_ > 1.0/curentRate?0:curentRate);
}
previousStamp_ = rtabmap_conversions::timestampFromROS(header.stamp);
}
}
@@ -1046,6 +1042,27 @@ void OdometryROS::setLogError(
ULogger::setLevel(ULogger::kError);
}
OdometryROS::OdomStatusTask::OdomStatusTask() :
diagnostic_updater::DiagnosticTask("Odom status"),
lost_(false)
{}
void OdometryROS::OdomStatusTask::setStatus(bool isLost)
{
lost_ = isLost;
}
void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if(lost_)
{
stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Lost!");
}
else
{
stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Tracking.");
}
}
}
rtabmap_odom/src/nodelets/icp_odometry.cpp
+5
View File
@@ -101,6 +101,11 @@ void ICPOdometry::onOdomInit()
cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1));
filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()));
initDiagnosticMsg(uFormat("\n%s subscribed to %s and %s (make sure only one of this topic is published, otherwise remap one to a dummy topic name).",
get_name(),
scan_sub_->get_topic_name(),
cloud_sub_->get_topic_name()), true);
}
void ICPOdometry::updateParameters(ParametersMap & parameters)
rtabmap_odom/src/nodelets/rgbd_odometry.cpp
+5 -8
View File
@@ -109,6 +109,7 @@ void RGBDOdometry::onOdomInit()
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: keep_color = %s", keepColor_?"true":"false");
std::string subscribedTopic;
std::string subscribedTopicsMsg;
if(subscribeRGBD)
{
@@ -260,6 +261,7 @@ void RGBDOdometry::onOdomInit()
{
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1));
subscribedTopic = rgbdxSub_->get_topic_name();
subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s",
get_name(),
rgbdxSub_->get_topic_name());
@@ -268,6 +270,7 @@ void RGBDOdometry::onOdomInit()
{
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
subscribedTopic = rgbdSub_->get_topic_name();
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
get_name(),
@@ -294,6 +297,7 @@ void RGBDOdometry::onOdomInit()
exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
subscribedTopic = image_mono_sub_.getSubscriber().getTopic();
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
get_name(),
approxSync?"approx":"exact",
@@ -302,7 +306,7 @@ void RGBDOdometry::onOdomInit()
image_depth_sub_.getSubscriber().getTopic().c_str(),
info_sub_.getSubscriber()->get_topic_name());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic);
}
void RGBDOdometry::updateParameters(ParametersMap & parameters)
@@ -492,7 +496,6 @@ void RGBDOdometry::callback(
const sensor_msgs::msg::Image::ConstSharedPtr depth,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
@@ -521,7 +524,6 @@ void RGBDOdometry::callback(
void RGBDOdometry::callbackRGBDX(
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
{
callbackCalled();
if(!this->isPaused())
{
if(images->rgbd_images.empty())
@@ -545,7 +547,6 @@ void RGBDOdometry::callbackRGBDX(
void RGBDOdometry::callbackRGBD(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
@@ -562,7 +563,6 @@ void RGBDOdometry::callbackRGBD2(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
@@ -582,7 +582,6 @@ void RGBDOdometry::callbackRGBD3(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3);
@@ -605,7 +604,6 @@ void RGBDOdometry::callbackRGBD4(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4);
@@ -631,7 +629,6 @@ void RGBDOdometry::callbackRGBD5(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);
rtabmap_odom/src/nodelets/stereo_odometry.cpp
+5 -7
View File
@@ -90,6 +90,7 @@ void StereoOdometry::onOdomInit()
RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
RCLCPP_INFO(this->get_logger(), "StereoOdometry: keep_color = %s", keepColor_?"true":"false");
std::string subscribedTopic;
std::string subscribedTopicsMsg;
if(subscribeRGBD)
{
@@ -206,6 +207,7 @@ void StereoOdometry::onOdomInit()
{
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1));
subscribedTopic = rgbdxSub_->get_topic_name();
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
get_name(),
@@ -215,6 +217,7 @@ void StereoOdometry::onOdomInit()
{
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
subscribedTopic = rgbdSub_->get_topic_name();
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
get_name(),
@@ -242,6 +245,7 @@ void StereoOdometry::onOdomInit()
exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
subscribedTopic = imageRectLeft_.getTopic();
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
get_name(),
approxSync?"approx":"exact",
@@ -252,7 +256,7 @@ void StereoOdometry::onOdomInit()
cameraInfoRight_.getSubscriber()->get_topic_name());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic);
}
void StereoOdometry::updateParameters(ParametersMap & parameters)
@@ -587,7 +591,6 @@ void StereoOdometry::callback(
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
@@ -618,7 +621,6 @@ void StereoOdometry::callback(
void StereoOdometry::callbackRGBD(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
@@ -636,7 +638,6 @@ void StereoOdometry::callbackRGBD(
void StereoOdometry::callbackRGBDX(
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
{
callbackCalled();
if(!this->isPaused())
{
if(images->rgbd_images.empty())
@@ -663,7 +664,6 @@ void StereoOdometry::callbackRGBD2(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
@@ -686,7 +686,6 @@ void StereoOdometry::callbackRGBD3(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
@@ -713,7 +712,6 @@ void StereoOdometry::callbackRGBD4(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);