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

sync with upstream 0.18.2. Fixed icp_odometry pause not working, added expected_update_rate parameter to odometry to filter messages with bad stamps (gazebo issue), filter consecutive messages with same stamp

Browse Source
This commit is contained in:
matlabbe
2018-11-21 12:01:09 -05:00
parent 801bac5221
commit 7f8a221f7e
7 changed files with 83 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/icp_odometry.cpp
+11 -2
View File
@@ -90,9 +90,9 @@ private:
NODELET_INFO("IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
NODELET_INFO("IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
NODELET_INFO("IcpOdometry: scan_voxel_size = %f", scanVoxelSize_);
NODELET_INFO("IcpOdometry: scan_voxel_size = %f m", scanVoxelSize_);
NODELET_INFO("IcpOdometry: scan_normal_k = %d", scanNormalK_);
NODELET_INFO("IcpOdometry: scan_normal_radius = %f", scanNormalRadius_);
NODELET_INFO("IcpOdometry: scan_normal_radius = %f m", scanNormalRadius_);
scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
@@ -175,6 +175,11 @@ private:
void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg)
{
if(this->isPaused())
{
return;
}
// make sure the frame of the laser is updated too
Transform localScanTransform = getTransform(this->frameId(),
scanMsg->header.frame_id,
@@ -244,6 +249,10 @@ private:
void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
{
if(this->isPaused())
{
return;
}
cv::Mat scan;
bool containNormals = false;
if(scanVoxelSize_ == 0.0f)