Skip to content

Commit

Permalink
Fixed odometry latency
Browse files Browse the repository at this point in the history
  • Loading branch information
matlabbe committed Sep 2, 2024
1 parent 10ddeee commit 92e7ac4
Show file tree
Hide file tree
Showing 7 changed files with 133 additions and 76 deletions.
8 changes: 5 additions & 3 deletions rtabmap_launch/launch/rtabmap.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -373,7 +373,9 @@ def launch_setup(context, *args, **kwargs):
"decimation": 4,
"voxel_size": 0.0,
"approx_sync": LaunchConfiguration('approx_sync'),
"approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval')
"approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'),
"qos": LaunchConfiguration('qos_image'),
"qos_camera_info": LaunchConfiguration('qos_camera_info')
}],
remappings=[
('left/image', LaunchConfiguration('left_image_topic_relay')),
Expand Down Expand Up @@ -418,8 +420,8 @@ def generate_launch_description():
DeclareLaunchArgument('publish_tf_map', default_value='true', description='Publish TF between map and odomerty.'),
DeclareLaunchArgument('namespace', default_value='rtabmap', description=''),
DeclareLaunchArgument('database_path', default_value='~/.ros/rtabmap.db', description='Where is the map saved/loaded.'),
DeclareLaunchArgument('topic_queue_size', default_value='1', description='Queue size of individual topic subscribers.'),
DeclareLaunchArgument('queue_size', default_value='10', description='Backward compatibility, use "sync_queue_size" instead.'),
DeclareLaunchArgument('topic_queue_size', default_value='10', description='Queue size of individual topic subscribers.'),
DeclareLaunchArgument('queue_size', default_value='2', description='Backward compatibility, use "sync_queue_size" instead.'),
DeclareLaunchArgument('qos', default_value='1', description='General QoS used for sensor input data: 0=system default, 1=Reliable, 2=Best Effort.'),
DeclareLaunchArgument('wait_for_transform', default_value='0.2', description=''),
DeclareLaunchArgument('rtabmap_args', default_value='', description='Backward compatibility, use "args" instead.'),
Expand Down
17 changes: 15 additions & 2 deletions rtabmap_odom/include/rtabmap_odom/OdometryROS.h
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_msgs/srv/reset_pose.hpp>
#include <rtabmap/core/SensorData.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/utilite/UThread.h>

#include <boost/thread.hpp>

Expand All @@ -59,7 +60,7 @@ class Odometry;

namespace rtabmap_odom {

class OdometryROS : public rclcpp::Node
class OdometryROS : public rclcpp::Node, public UThread
{

public:
Expand Down Expand Up @@ -98,12 +99,17 @@ class OdometryROS : public rclcpp::Node

private:

virtual void mainLoop();
virtual void mainLoopKill();
virtual void updateParameters(rtabmap::ParametersMap &) {}
virtual void onOdomInit() {}

void callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg);
void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity());

protected:
rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_;

private:
rtabmap::Odometry * odometry_;

Expand Down Expand Up @@ -147,6 +153,14 @@ class OdometryROS : public rclcpp::Node
std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
std::shared_ptr<tf2_ros::TransformListener> tfListener_;
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imuSub_;
rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_;

UMutex dataMutex_;
UMutex imuMutex_;
USemaphore dataReady_;

rtabmap::SensorData dataToProcess_;
std_msgs::msg::Header dataHeaderToProcess_;

bool paused_;
int resetCountdown_;
Expand All @@ -166,7 +180,6 @@ class OdometryROS : public rclcpp::Node
bool waitIMUToinit_;
bool imuProcessed_;
std::map<double, rtabmap::IMU> imus_;
std::pair<rtabmap::SensorData, std_msgs::msg::Header > bufferedData_;
std::string configPath_;
rtabmap::Transform initialPose_;

Expand Down
120 changes: 75 additions & 45 deletions rtabmap_odom/src/OdometryROS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
initialPose_(Transform::getIdentity()),
ulogToRosout_(this)
{
dataCallbackGroup_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

int qos = this->declare_parameter("qos", (int)qos_);
qos_ = (rmw_qos_reliability_policy_t)qos;

Expand Down Expand Up @@ -204,6 +206,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o

OdometryROS::~OdometryROS()
{
this->join(true);
delete odometry_;
}

Expand Down Expand Up @@ -365,14 +368,19 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_);
if(waitIMUToinit_)
{
int queueSize = 10;
this->get_parameter_or("queue_size", queueSize, queueSize);
imuCallbackGroup_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions options;
options.callback_group = imuCallbackGroup_;
int queueSize = this->declare_parameter("imu_queue_size", 200);
int qosImu = this->declare_parameter("qos_imu", (int)qos_);
imuSub_ = create_subscription<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(queueSize*5).reliability((rmw_qos_reliability_policy_t)qosImu), std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1));
imuSub_ = create_subscription<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosImu), std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1));
RCLCPP_INFO(this->get_logger(), "odometry: Subscribing to IMU topic %s", imuSub_->get_topic_name());
RCLCPP_INFO(this->get_logger(), "odometry: qos_imu = %d", qosImu);
RCLCPP_INFO(this->get_logger(), "odometry: imu_queue_size = %d", queueSize);
}

this->start();

onOdomInit();
}

Expand Down Expand Up @@ -408,6 +416,8 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg)
if(!this->isPaused())
{
double stamp = rtabmap_conversions::timestampFromROS(msg->header.stamp);
//RCLCPP_WARN(get_logger(), "Received imu: %f delay=%f", stamp, (now() - msg->header.stamp).seconds());

rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
if(this->frameId().compare(msg->header.frame_id) != 0)
{
Expand All @@ -428,63 +438,81 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg)
cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(),
localTransform);

UScopeMutex m(imuMutex_);
imus_.insert(std::make_pair(stamp, imu));
//RCLCPP_WARN(get_logger(), "Received imu: %f", stamp);

if(bufferedData_.first.isValid() && stamp > bufferedData_.first.stamp())
{
SensorData data = bufferedData_.first;
bufferedData_.first = SensorData();
processData(data, bufferedData_.second);
}

if(imus_.size() > 1000)
{
RCLCPP_WARN(this->get_logger(), "Dropping imu data!");
imus_.erase(imus_.begin());
}
}
}

void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & header)
{
if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty())
//RCLCPP_WARN(get_logger(), "Received image: %f delay=%f", data.stamp(), (now() - header.stamp).seconds());
if(dataMutex_.lockTry() == 0)
{
RCLCPP_WARN(this->get_logger(), "odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_->get_topic_name());
return;
dataToProcess_ = data;
dataHeaderToProcess_ = header;
dataReady_.release();
dataMutex_.unlock();
}
}

if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp)))
{
//RCLCPP_WARN(get_logger(), "No imu received with higher stamp than last image (%f)! Buffering this image until we get more imu msgs...", timestampFromROS(header.stamp));
void OdometryROS::mainLoopKill()
{
RCLCPP_WARN(this->get_logger(), "OdometryROS::mainLoopKill()");
// in case we were waiting, unblock thread
dataReady_.release();
}

// keep in cache to process later when we will receive imu msgs
if(bufferedData_.first.isValid())
{
RCLCPP_ERROR(this->get_logger(), "Overwriting previous data! Make sure IMU is "
"published faster than data rate. (last image stamp "
"buffered=%f and new one is %f, last imu stamp received=%f)",
bufferedData_.first.stamp(), data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
}
bufferedData_.first = data;
bufferedData_.second = header;
return;
}
// process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp)
std::map<double, rtabmap::IMU>::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp));
if(iterEnd!= imus_.end())
void OdometryROS::mainLoop()
{
dataReady_.acquire();

if(!this->isRunning())
{
++iterEnd;
// thread killed
return;
}
for(std::map<double, rtabmap::IMU>::iterator iter=imus_.begin(); iter!=iterEnd;)

UScopeMutex lock(dataMutex_);

// aliases
SensorData & data = dataToProcess_;
std_msgs::msg::Header & header = dataHeaderToProcess_;

{
//NODELET_WARN("img callback: process imu %f", iter->first);
SensorData dataIMU(iter->second, 0, iter->first);
odometry_->process(dataIMU);
imus_.erase(iter++);
imuProcessed_ = true;
}
UScopeMutex m(imuMutex_);

if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty())
{
RCLCPP_WARN(this->get_logger(), "odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_->get_topic_name());
return;
}

//RCLCPP_WARN(get_logger(), "img callback: process image %f", timestampFromROS(header.stamp));
if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp)))
{
RCLCPP_ERROR(this->get_logger(), "Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f)",
data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
return;
}
// process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp)
std::map<double, rtabmap::IMU>::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp));
if(iterEnd!= imus_.end())
{
++iterEnd;
}
for(std::map<double, rtabmap::IMU>::iterator iter=imus_.begin(); iter!=iterEnd;)
{
SensorData dataIMU(iter->second, 0, iter->first);
odometry_->process(dataIMU);
imus_.erase(iter++);
imuProcessed_ = true;
}
} // end imu lock

Transform groundTruth;
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
Expand Down Expand Up @@ -1021,16 +1049,16 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
{
if(icpParams_)
{
RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, 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());
RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, 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(), (now()-header.stamp).seconds());
}
else
{
RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, 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());
RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, 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(), (now()-header.stamp).seconds());
}
}
else // if(icpParams_)
{
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());
RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs delay=%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(), (now()-header.stamp).seconds());
}

statusDiagnostic_.setStatus(pose.isNull());
Expand Down Expand Up @@ -1067,14 +1095,16 @@ void OdometryROS::resetToPose(

void OdometryROS::reset(const Transform & pose)
{
UScopeMutex lock(dataMutex_);
odometry_->reset(pose);
guess_.setNull();
guessPreviousPose_.setNull();
previousStamp_ = 0.0;
resetCurrentCount_ = resetCountdown_;
imuProcessed_ = false;
bufferedData_.first= SensorData();
imuMutex_.lock();
imus_.clear();
imuMutex_.unlock();
this->flushCallbacks();
}

Expand Down
5 changes: 4 additions & 1 deletion rtabmap_odom/src/RGBDOdometryNode.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,10 @@ int main(int argc, char **argv)
rclcpp::init(argc, argv);
rclcpp::NodeOptions options;
options.arguments(arguments);
rclcpp::spin(std::make_shared<rtabmap_odom::RGBDOdometry>(options));
auto node = std::make_shared<rtabmap_odom::RGBDOdometry>(options);
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(node);
executor.spin();
rclcpp::shutdown();
return 0;
}
7 changes: 5 additions & 2 deletions rtabmap_odom/src/nodelets/icp_odometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -97,8 +97,11 @@ void ICPOdometry::onOdomInit()
RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing = %s", deskewing_?"true":"false");
RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing_slerp = %s", deskewingSlerp_?"true":"false");

scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1));
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));
rclcpp::SubscriptionOptions options;
options.callback_group = dataCallbackGroup_;

scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options);
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), options);

filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()));

Expand Down
25 changes: 14 additions & 11 deletions rtabmap_odom/src/nodelets/rgbd_odometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -125,29 +125,32 @@ 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");

rclcpp::SubscriptionOptions options;
options.callback_group = dataCallbackGroup_;

std::string subscribedTopic;
std::string subscribedTopicsMsg;
if(subscribeRGBD)
{
if(rgbdCameras >= 2)
{
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
if(rgbdCameras >= 3)
{
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
}
if(rgbdCameras >= 4)
{
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
}
if(rgbdCameras >= 5)
{
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
}
if(rgbdCameras >= 6)
{
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
}

if(rgbdCameras == 2)
Expand Down Expand Up @@ -327,7 +330,7 @@ void RGBDOdometry::onOdomInit()
}
else if(rgbdCameras == 0)
{
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1));
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1), options);

subscribedTopic = rgbdxSub_->get_topic_name();
subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s",
Expand All @@ -336,7 +339,7 @@ void RGBDOdometry::onOdomInit()
}
else
{
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1), options);

subscribedTopic = rgbdSub_->get_topic_name();
subscribedTopicsMsg =
Expand All @@ -348,9 +351,9 @@ void RGBDOdometry::onOdomInit()
else
{
image_transport::TransportHints hints(this);
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(), options);

if(approxSync)
{
Expand Down
Loading

0 comments on commit 92e7ac4

Please sign in to comment.