i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-07 10:17:45 +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_sync/src/nodelets/rgb_sync.cpp
+3 -3
View File
@@ -43,7 +43,6 @@ namespace rtabmap_sync
RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
Node("rgbd_sync", options),
SyncDiagnostic(this),
compressedRate_(0),
approxSync_(0),
exactSync_(0)
@@ -96,7 +95,8 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
initDiagnostic(imageSub_.getSubscriber().getTopic(),
syncDiagnostic_.reset(new SyncDiagnostic(this));
syncDiagnostic_->init(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",
@@ -119,7 +119,7 @@ void RGBSync::callback(
const sensor_msgs::msg::Image::ConstSharedPtr image,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
tick(image->header.stamp);
syncDiagnostic_->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
+10 -10
View File
@@ -43,7 +43,6 @@ namespace rtabmap_sync
RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
Node("rgbd_sync", options),
SyncDiagnostic(this),
depthScale_(1.0),
decimation_(1),
compressedRate_(0),
@@ -109,14 +108,15 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
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",
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()));
syncDiagnostic_.reset(new SyncDiagnostic(this));
syncDiagnostic_->init(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",
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()));
}
RGBDSync::~RGBDSync()
@@ -130,7 +130,7 @@ void RGBDSync::callback(
const sensor_msgs::msg::Image::ConstSharedPtr depth,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
tick(image->header.stamp);
syncDiagnostic_->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
+13 -12
View File
@@ -34,7 +34,6 @@ namespace rtabmap_sync
RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
Node("rgbd_sync", options),
SyncDiagnostic(this),
SYNC_INIT(rgbd2),
SYNC_INIT(rgbd3),
SYNC_INIT(rgbd4),
@@ -131,18 +130,20 @@ 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():"");
std::string subscribedTopicsMsg = uFormat("%s%s", subscribedTopicsMsg_.c_str(),
approxSync&&approxSyncMaxInterval!=0.0?uFormat(" (approx sync max interval=%fs)", approxSyncMaxInterval).c_str():"");
RCLCPP_INFO(this->get_logger(), subscribedTopicsMsg.c_str());
// Setup diagnostic
initDiagnostic("",
syncDiagnostic_.reset(new SyncDiagnostic(this));
syncDiagnostic_->init("",
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()));
subscribedTopicsMsg.c_str()));
}
RGBDXSync::~RGBDXSync()
@@ -160,7 +161,7 @@ void RGBDXSync::rgbd2Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image0,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(2);
@@ -174,7 +175,7 @@ void RGBDXSync::rgbd3Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(3);
@@ -190,7 +191,7 @@ void RGBDXSync::rgbd4Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(4);
@@ -208,7 +209,7 @@ void RGBDXSync::rgbd5Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(5);
@@ -228,7 +229,7 @@ void RGBDXSync::rgbd6Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(6);
@@ -250,7 +251,7 @@ void RGBDXSync::rgbd7Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(7);
@@ -274,7 +275,7 @@ void RGBDXSync::rgbd8Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image7)
{
tick(image0->header.stamp);
syncDiagnostic_->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
+3 -3
View File
@@ -42,7 +42,6 @@ namespace rtabmap_sync
StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
Node("stereo_sync", options),
SyncDiagnostic(this),
compressedRate_(0),
approxSync_(0),
exactSync_(0)
@@ -98,7 +97,8 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
initDiagnostic(imageLeftSub_.getSubscriber().getTopic(),
syncDiagnostic_.reset(new SyncDiagnostic(this));
syncDiagnostic_->init(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",
@@ -120,7 +120,7 @@ void StereoSync::callback(
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
{
tick(imageLeft->header.stamp);
syncDiagnostic_->tick(imageLeft->header.stamp);
if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
{
double leftStamp = rtabmap_conversions::timestampFromROS(imageLeft->header.stamp);