diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index 4a1b7a81..582d0bc5 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -98,7 +98,7 @@ protected: virtual void postProcessData(const rtabmap::SensorData & /*data*/, const std_msgs::msg::Header & /*header*/) const {} private: - + void processData(); virtual void mainLoop(); virtual void mainLoopKill(); virtual void updateParameters(rtabmap::ParametersMap &) {} @@ -174,9 +174,12 @@ private: rtabmap::Transform guessPreviousPose_; double previousStamp_; double previousClockTime_; + double lastReceivedTopicClock_; + double lastReceivedTopicStamp_; double expectedUpdateRate_; double maxUpdateRate_; double minUpdateRate_; + bool alwaysProcessMostRecentFrame_; std::string compressionImgFormat_; bool compressionParallelized_; int odomStrategy_; diff --git a/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp index 28ab0e55..dc50b450 100644 --- a/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp +++ b/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp @@ -78,6 +78,7 @@ private: double scanNormalGroundUp_; bool deskewing_; bool deskewingSlerp_; + int topicQueueSize_; //std::vector > plugins_; //pluginlib::ClassLoader plugin_loader_; bool scanReceived_ = false; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index c9955d99..d7624e62 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -89,9 +89,12 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o icpParams_(false), previousStamp_(0.0), previousClockTime_(0.0), + lastReceivedTopicClock_(0.0), + lastReceivedTopicStamp_(0.0), expectedUpdateRate_(0.0), maxUpdateRate_(0.0), minUpdateRate_(0.0), + alwaysProcessMostRecentFrame_(true), compressionImgFormat_(".jpg"), compressionParallelized_(true), odomStrategy_(Parameters::defaultOdomStrategy()), @@ -144,6 +147,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o expectedUpdateRate_ = this->declare_parameter("expected_update_rate", expectedUpdateRate_); maxUpdateRate_ = this->declare_parameter("max_update_rate", maxUpdateRate_); minUpdateRate_ = this->declare_parameter("min_update_rate", minUpdateRate_); + alwaysProcessMostRecentFrame_ = this->declare_parameter("always_process_most_recent_frame", alwaysProcessMostRecentFrame_); compressionImgFormat_ = this->declare_parameter("sensor_data_compression_format", compressionImgFormat_); compressionParallelized_ = this->declare_parameter("sensor_data_parallel_compression", compressionParallelized_); @@ -461,25 +465,47 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg) void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & header) { //RCLCPP_WARN(get_logger(), "Received image: %f delay=%f", data.stamp(), (now() - header.stamp).seconds()); + double clockNow = rtabmap_conversions::timestampFromROS(now()); if(dataMutex_.lockTry() == 0) { if(bufferedDataToProcess_) { - RCLCPP_ERROR(this->get_logger(), "We didn't receive IMU newer than previous image (%f) and we just received a new image (%f). The previous image is dropped!", + RCLCPP_ERROR(this->get_logger(), "We didn't receive IMU newer than previous image/scan (%f) and we just received a new image/scan (%f). The previous image/scan is dropped!", rtabmap_conversions::timestampFromROS(dataHeaderToProcess_.stamp), rtabmap_conversions::timestampFromROS(header.stamp)); ++droppedMsgs_; } dataToProcess_ = data; dataHeaderToProcess_ = header; bufferedDataToProcess_ = false; - dataReady_.release(); + if(alwaysProcessMostRecentFrame_) { + dataReady_.release(); + } dataMutex_.unlock(); ++processedMsgs_; + if(!alwaysProcessMostRecentFrame_) { + processData(); + } } else { - //RCLCPP_WARN(get_logger(), "Dropping image/scan data"); + double estimatedPeriod = clockNow - lastReceivedTopicClock_; + double topicPeriod = rtabmap_conversions::timestampFromROS(header.stamp) - lastReceivedTopicStamp_; + if(estimatedPeriod>0.0 && topicPeriod>0.0 && estimatedPeriod < topicPeriod*0.9) { + RCLCPP_WARN(get_logger(), + "Dropping image/scan data with stamp %f (delay=%f). Something is wrong " + "because the clock difference with the previous topic received (%fs) is much lower than the " + "expected one (%fs) estimated from the topic stamps (previous stamp=%f). If you are processing " + "a large bag with flaky replaying delay, consider setting parameter \"always_process_most_recent_frame:=false\" " + "to avoid aggressively dropping data.", + rtabmap_conversions::timestampFromROS(header.stamp), + clockNow - rtabmap_conversions::timestampFromROS(header.stamp), + estimatedPeriod, + topicPeriod, + lastReceivedTopicStamp_); + } ++droppedMsgs_; } + lastReceivedTopicStamp_ = rtabmap_conversions::timestampFromROS(header.stamp); + lastReceivedTopicClock_ = clockNow; } void OdometryROS::mainLoopKill() @@ -497,7 +523,10 @@ void OdometryROS::mainLoop() // thread killed return; } - + processData(); +} +void OdometryROS::processData() +{ UScopeMutex lock(dataMutex_); // aliases @@ -516,21 +545,37 @@ void OdometryROS::mainLoop() if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp))) { - RCLCPP_WARN(this->get_logger(), "Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.", - data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + if(!imus_.empty()) { + RCLCPP_WARN(this->get_logger(), "Make sure IMU is published faster than data rate! (last image/scan stamp=%f and last imu stamp received=%f). Buffering the image/scan until an imu with same or greater stamp is received.", + data.stamp(), imus_.rbegin()->first); + } + else { + // If empty, it is an error! + RCLCPP_ERROR(this->get_logger(), "Make sure IMU is published faster than data rate! (last image/scan stamp=%f and imu buffer is empty). Buffering the image/scan until an imu with same or greater stamp is received.", + data.stamp()); + } bufferedDataToProcess_ = true; 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::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp)); + std::map::iterator iterLast = iterEnd; if(iterEnd!= imus_.end()) { ++iterEnd; } for(std::map::iterator iter=imus_.begin(); iter!=iterEnd;) { - imus.push_back(*iter); - imus_.erase(iter++); + // Because we always keep the last processed imu in the buffer, skip the first one when processing again the buffer. + if(iter!=imus_.begin()) { + imus.push_back(*iter); + } + if(iter!=iterLast) { + imus_.erase(iter++); + } + else { + ++iter; + } } } // end imu lock @@ -1241,6 +1286,8 @@ void OdometryROS::reset(const Transform & pose) guessPreviousPose_.setNull(); previousStamp_ = 0.0; previousClockTime_ = 0.0; + lastReceivedTopicClock_ = 0.0; + lastReceivedTopicStamp_ = 0.0; resetCurrentCount_ = resetCountdown_; imuProcessed_ = false; dataToProcess_ = SensorData(); diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index d30b68aa..990976ec 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -60,6 +60,7 @@ ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) : scanNormalGroundUp_(0.0), deskewing_(false), deskewingSlerp_(false), + topicQueueSize_(1), scanReceived_(false), cloudReceived_(false) { @@ -83,6 +84,7 @@ void ICPOdometry::onOdomInit() scanNormalGroundUp_ = this->declare_parameter("scan_normal_ground_up", scanNormalGroundUp_); deskewing_ = this->declare_parameter("deskewing", deskewing_); deskewingSlerp_ = this->declare_parameter("deskewing_slerp", deskewingSlerp_); + topicQueueSize_ = this->declare_parameter("topic_queue_size", topicQueueSize_); RCLCPP_INFO(this->get_logger(), "IcpOdometry: qos = %d", (int)qos()); RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_); @@ -96,12 +98,13 @@ void ICPOdometry::onOdomInit() RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up = %f", scanNormalGroundUp_); RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing = %s", deskewing_?"true":"false"); RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing_slerp = %s", deskewingSlerp_?"true":"false"); + RCLCPP_INFO(this->get_logger(), "IcpOdometry: topic_queue_size = %d", topicQueueSize_); rclcpp::SubscriptionOptions options; options.callback_group = dataCallbackGroup_; - scan_sub_ = create_subscription("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options); - cloud_sub_ = create_subscription("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1), options); + scan_sub_ = create_subscription("scan", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options); + cloud_sub_ = create_subscription("scan_cloud", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1), options); filtered_scan_pub_ = create_publisher("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos())); diff --git a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h index 03b49348..f9411519 100644 --- a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h +++ b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h @@ -62,7 +62,7 @@ class SyncDiagnostic { diagnosticTimer_ = node_->create_wall_timer(5s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr); } - void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0) + void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0.0) { updateFrequency( stamp, @@ -74,9 +74,12 @@ class SyncDiagnostic { lastTickInputStamp_); } - void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0) + void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0.0) { - double lastTickOutputStamp; + if(expectedFrequency == 0.0) { + outTargetFrequency_ = inTargetFrequency_; + } + double lastTickOutputStamp = 0.0; updateFrequency( stamp, expectedFrequency, @@ -112,31 +115,33 @@ private: timeStatus.tick(stamp); double stampSec = rtabmap_conversions::timestampFromROS(stamp); - double singlePeriod = stampSec - lastTickStamp; - window.push_back(singlePeriod); - if(window.size() > windowSize_) + if(expectedFrequency>0) { - window.pop_front(); + targetFrequency = expectedFrequency; + } + else if(lastTickStamp > 0.0) { + double singlePeriod = stampSec - lastTickStamp; - double period = 0.0; - if(window.size() == windowSize_) + window.push_back(singlePeriod); + if(window.size() > windowSize_) { - for(size_t i=0; i0.0 && expectedFrequency == 0 && (targetFrequency == 0.0 || period < 1.0/targetFrequency)) - { - targetFrequency = 1.0/period; - } - else if(expectedFrequency>0) - { - targetFrequency = expectedFrequency; - + if(period>0.0 && (targetFrequency == 0.0 || period < 1.0/targetFrequency)) + { + targetFrequency = 1.0/period; + } } } diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index 525d239e..15fc4274 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -26,6 +26,7 @@ find_package(pcl_ros REQUIRED) find_package(message_filters REQUIRED) find_package(rtabmap_msgs REQUIRED) find_package(rtabmap_conversions REQUIRED) +find_package(rtabmap_sync REQUIRED) # Optional components find_package(octomap_msgs) @@ -54,6 +55,7 @@ SET(Libraries message_filters rtabmap_msgs rtabmap_conversions + rtabmap_sync ) if("$ENV{ROS_DISTRO}" STRLESS "jazzy") diff --git a/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp b/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp index 3f19b09c..a1def1c0 100644 --- a/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp +++ b/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp @@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rclcpp/rclcpp.hpp" +#include + #include #include @@ -58,6 +60,8 @@ private: bool slerp_; std::shared_ptr tfBuffer_; std::shared_ptr tfListener_; + std::unique_ptr scanSyncDiagnostic_; + std::unique_ptr cloudSyncDiagnostic_; }; } diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index a802b4f0..21d5df95 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -32,6 +32,7 @@ message_filters rtabmap_msgs rtabmap_conversions + rtabmap_sync grid_map_ros diff --git a/rtabmap_util/src/nodelets/lidar_deskewing.cpp b/rtabmap_util/src/nodelets/lidar_deskewing.cpp index 2ebd00a7..d15e189b 100644 --- a/rtabmap_util/src/nodelets/lidar_deskewing.cpp +++ b/rtabmap_util/src/nodelets/lidar_deskewing.cpp @@ -46,6 +46,16 @@ LidarDeskewing::~LidarDeskewing() void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr msg) { + if(scanSyncDiagnostic_.get() == 0) { + scanSyncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5)); + scanSyncDiagnostic_->init(subScan_->get_topic_name(), + uFormat("%s: Did not receive data since 5 seconds! Make sure the input topic \"%s\" is " + "published (\"$ rostopic hz my_topic\") and the timestamps in their " + "header are set.", + this->get_name(), + subScan_->get_topic_name())); + } + scanSyncDiagnostic_->tickInput(msg->header.stamp); // make sure the frame of the laser is updated during the whole scan time rtabmap::Transform tmpT = rtabmap_conversions::getMovingTransform( msg->header.frame_id, @@ -75,10 +85,23 @@ void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstShared rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed); scanOutDeskewed.header.frame_id = msg->header.frame_id; pubScan_->publish(scanOutDeskewed); + + scanSyncDiagnostic_->tickOutput(msg->header.stamp); } void LidarDeskewing::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { + if(cloudSyncDiagnostic_.get() == 0) { + cloudSyncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5)); + cloudSyncDiagnostic_->init(subCloud_->get_topic_name(), + uFormat("%s: Did not receive data since 5 seconds! Make sure the input topic \"%s\" is " + "published (\"$ rostopic hz my_topic\") and the timestamps in their " + "header are set.", + this->get_name(), + subCloud_->get_topic_name())); + } + cloudSyncDiagnostic_->tickInput(msg->header.stamp); + sensor_msgs::msg::PointCloud2 msgDeskewed; if(rtabmap_conversions::deskew(*msg, msgDeskewed, fixedFrameId_, *tfBuffer_, waitForTransformDuration_, slerp_)) { @@ -91,6 +114,7 @@ void LidarDeskewing::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSha RCLCPP_WARN(this->get_logger(), "deskewing failed! returning possible skewed cloud!"); pubCloud_->publish(*msg); } + cloudSyncDiagnostic_->tickOutput(msg->header.stamp); } } diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index 44ac1c97..e0283d7c 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -203,7 +203,11 @@ void GuiWrapper::infoMapCallback( this->post(new RtabmapEvent(stat)); - tick(infoMsg->header.stamp); + ParametersMap allParameters = prefDialog_->getAllParameters(); + float detectionRate = Parameters::defaultRtabmapDetectionRate(); + Parameters::parse(allParameters, Parameters::kRtabmapDetectionRate(), detectionRate); + + tick(infoMsg->header.stamp, detectionRate); } @@ -236,7 +240,11 @@ void GuiWrapper::infoCallback( this->post(new RtabmapEvent(stat)); - tick(infoMsg->header.stamp); + ParametersMap allParameters = prefDialog_->getAllParameters(); + float detectionRate = Parameters::defaultRtabmapDetectionRate(); + Parameters::parse(allParameters, Parameters::kRtabmapDetectionRate(), detectionRate); + + tick(infoMsg->header.stamp, detectionRate); } void GuiWrapper::goalPathCallback(