i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 17:57:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Odom: added parameter "always_process_most_recent_frame" (default true like before) so that we can disable aggressive frame dropping in case input frames are published with flaky latency (e.g., with large rosbag having issue to replay in time topics)

Browse Source
This commit is contained in:
matlabbe
2025-09-07 19:08:58 -07:00
parent d3d3120804
commit 0ad034680b
10 changed files with 132 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/src/nodelets/lidar_deskewing.cpp
+24
View File
@@ -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);
}
}