i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-07 18:27: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_sync/src/nodelets/rgb_sync.cpp
+12 -30
View File
@@ -43,9 +43,8 @@ namespace rtabmap_sync
RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
Node("rgbd_sync", options),
SyncDiagnostic(this),
compressedRate_(0),
warningThread_(0),
callbackCalled_(false),
approxSync_(0),
exactSync_(0)
{
@@ -88,33 +87,23 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCaminfo).get_rmw_qos_profile());
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=std::numeric_limits<double>::max()?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
imageSub_.getSubscriber().getTopic().c_str(),
cameraInfoSub_.getSubscriber()->get_topic_name());
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
warningThread_ = new std::thread([&](){
rclcpp::Rate r(1/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());
}
}
});
initDiagnostic(imageSub_.getSubscriber().getTopic(),
uFormat("%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()));
}
@@ -124,20 +113,13 @@ RGBSync::~RGBSync()
delete approxSync_;
if(exactSync_)
delete exactSync_;
if(warningThread_)
{
callbackCalled_=true;
warningThread_->join();
delete warningThread_;
}
}
void RGBSync::callback(
const sensor_msgs::msg::Image::ConstSharedPtr image,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
callbackCalled_ = true;
tick(image->header.stamp);
if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
{
double stamp = rtabmap_conversions::timestampFromROS(image->header.stamp);
rtabmap_sync/src/nodelets/rgbd_sync.cpp
+9 -23
View File
@@ -43,11 +43,10 @@ namespace rtabmap_sync
RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
Node("rgbd_sync", options),
SyncDiagnostic(this),
depthScale_(1.0),
decimation_(1),
compressedRate_(0),
warningThread_(0),
callbackCalled_(false),
approxSyncDepth_(0),
exactSyncDepth_(0)
{
@@ -100,7 +99,7 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
@@ -108,35 +107,22 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
imageDepthSub_.getSubscriber().getTopic().c_str(),
cameraInfoSub_.getSubscriber()->get_topic_name());
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
warningThread_ = new std::thread([&](){
rclcpp::Rate r(1/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 "
initDiagnostic(imageSub_.getSubscriber().getTopic(),
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(),
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());
}
}
});
subscribedTopicsMsg.c_str()));
}
RGBDSync::~RGBDSync()
{
delete approxSyncDepth_;
delete exactSyncDepth_;
callbackCalled_ = true;
warningThread_->join();
delete warningThread_;
}
void RGBDSync::callback(
@@ -144,7 +130,7 @@ void RGBDSync::callback(
const sensor_msgs::msg::Image::ConstSharedPtr depth,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
callbackCalled_ = true;
tick(image->header.stamp);
if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
{
double rgbStamp = rtabmap_conversions::timestampFromROS(image->header.stamp);
rtabmap_sync/src/nodelets/rgbdx_sync.cpp
+18 -35
View File
@@ -34,15 +34,14 @@ namespace rtabmap_sync
RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
Node("rgbd_sync", options),
SyncDiagnostic(this),
SYNC_INIT(rgbd2),
SYNC_INIT(rgbd3),
SYNC_INIT(rgbd4),
SYNC_INIT(rgbd5),
SYNC_INIT(rgbd6),
SYNC_INIT(rgbd7),
SYNC_INIT(rgbd8),
warningThread_(0),
callbackCalled_(false)
SYNC_INIT(rgbd8)
{
int queueSize = 10;
bool approxSync = true;
@@ -135,24 +134,15 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
RCLCPP_INFO(this->get_logger(), "%s%s", subscribedTopicsMsg_.c_str(),
approxSync&&approxSyncMaxInterval!=0.0?uFormat(" (approx sync max interval=%fs)", approxSyncMaxInterval).c_str():"");
warningThread_ = new std::thread([&](){
rclcpp::Rate r(1/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());
}
}
});
// Setup diagnostic
initDiagnostic("",
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",
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()));
}
RGBDXSync::~RGBDXSync()
@@ -164,20 +154,13 @@ RGBDXSync::~RGBDXSync()
SYNC_DEL(rgbd6);
SYNC_DEL(rgbd7);
SYNC_DEL(rgbd8);
if(warningThread_)
{
callbackCalled_=true;
warningThread_->join();
delete warningThread_;
}
}
void RGBDXSync::rgbd2Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image0,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1)
{
callbackCalled_ = true;
tick(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(2);
@@ -191,7 +174,7 @@ void RGBDXSync::rgbd3Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
{
callbackCalled_ = true;
tick(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(3);
@@ -207,7 +190,7 @@ void RGBDXSync::rgbd4Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
{
callbackCalled_ = true;
tick(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(4);
@@ -225,7 +208,7 @@ void RGBDXSync::rgbd5Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
{
callbackCalled_ = true;
tick(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(5);
@@ -245,7 +228,7 @@ void RGBDXSync::rgbd6Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
{
callbackCalled_ = true;
tick(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(6);
@@ -267,7 +250,7 @@ void RGBDXSync::rgbd7Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
{
callbackCalled_ = true;
tick(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(7);
@@ -291,7 +274,7 @@ void RGBDXSync::rgbd8Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image7)
{
callbackCalled_ = true;
tick(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(8);
rtabmap_sync/src/nodelets/stereo_sync.cpp
+12 -27
View File
@@ -42,9 +42,8 @@ namespace rtabmap_sync
StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
Node("stereo_sync", options),
SyncDiagnostic(this),
compressedRate_(0),
warningThread_(0),
callbackCalled_(false),
approxSync_(0),
exactSync_(0)
{
@@ -88,7 +87,7 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
cameraInfoLeftSub_.subscribe(this, "left/camera_info"), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile();
cameraInfoRightSub_.subscribe(this, "right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s",
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
@@ -97,36 +96,22 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
cameraInfoLeftSub_.getSubscriber()->get_topic_name(),
cameraInfoRightSub_.getSubscriber()->get_topic_name());
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
warningThread_ = new std::thread([&](){
rclcpp::Rate r(1/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",
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());
}
}
});
initDiagnostic(imageLeftSub_.getSubscriber().getTopic(),
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",
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()));
}
StereoSync::~StereoSync()
{
delete approxSync_;
delete exactSync_;
callbackCalled_=true;
warningThread_->join();
delete warningThread_;
}
void StereoSync::callback(
@@ -135,7 +120,7 @@ void StereoSync::callback(
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
{
callbackCalled_ = true;
tick(imageLeft->header.stamp);
if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
{
double leftStamp = rtabmap_conversions::timestampFromROS(imageLeft->header.stamp);