From 2ba64f732dbd01baa121056060dc0a97733894cd Mon Sep 17 00:00:00 2001 From: chcaya Date: Thu, 31 Oct 2024 09:27:32 -0400 Subject: [PATCH] Fixed callbackCloud warning --- .../src/nodelets/point_cloud_assembler.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp index 2264af399..b064833c4 100644 --- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp @@ -174,16 +174,13 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) : rclcpp::Rate r(1.0/5.0); while(!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", + get_name(), + subscribedTopicsMsg_.c_str()); 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", - get_name(), - subscribedTopicsMsg_.c_str()); - } } }); @@ -286,13 +283,14 @@ void PointCloudAssembler::callbackCloudOdomInfo( } else { - RCLCPP_WARN(this->get_logger(), "Reseting point cloud assembler as null odometry has been received."); + RCLCPP_WARN(this->get_logger(), "Resetting point cloud assembler as null odometry has been received."); clouds_.clear(); } } void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg) { + callbackCalled_ = true; if(cloudPub_->get_subscription_count()) { UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,