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

Updated with rtabmap-0.19.5, added tange_min and range_max parameters to point_cloud_assembler nodelet

Browse Source
This commit is contained in:
matlabbe
2019-08-01 16:31:10 -04:00
parent ca42a35709
commit 18ee9909fc
9 changed files with 611 additions and 234 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/icp_odometry.cpp
+54 -4
View File
@@ -59,6 +59,8 @@ public:
OdometryROS(false, false, true),
scanCloudMaxPoints_(0),
scanDownsamplingStep_(1),
scanRangeMin_(0),
scanRangeMax_(0),
scanVoxelSize_(0.0),
scanNormalK_(0),
scanNormalRadius_(0.0)
@@ -78,8 +80,10 @@ private:
pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_);
pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
pnh.param("scan_range_min", scanRangeMin_, scanRangeMin_);
pnh.param("scan_range_max", scanRangeMax_, scanRangeMax_);
pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
{
ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
@@ -90,6 +94,8 @@ private:
NODELET_INFO("IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
NODELET_INFO("IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
NODELET_INFO("IcpOdometry: scan_range_min = %f m", scanRangeMin_);
NODELET_INFO("IcpOdometry: scan_range_max = %f m", scanRangeMax_);
NODELET_INFO("IcpOdometry: scan_voxel_size = %f m", scanVoxelSize_);
NODELET_INFO("IcpOdometry: scan_normal_k = %d", scanNormalK_);
NODELET_INFO("IcpOdometry: scan_normal_radius = %f m", scanNormalRadius_);
@@ -119,7 +125,7 @@ private:
{
if(!pnh.hasParam("scan_downsampling_step"))
{
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_downsampling_step\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_downsampling_step\" for convenience. \"%s\" is set to 1.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
scanDownsamplingStep_ = value;
iter->second = "1";
}
@@ -129,6 +135,42 @@ private:
}
}
}
iter = parameters.find(Parameters::kIcpRangeMin());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value > 1)
{
if(!pnh.hasParam("scan_range_min"))
{
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_range_min\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
scanRangeMin_ = value;
iter->second = "0";
}
else
{
ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_min\" are set.", iter->first.c_str());
}
}
}
iter = parameters.find(Parameters::kIcpRangeMax());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value > 1)
{
if(!pnh.hasParam("scan_range_max"))
{
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_range_max\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
scanRangeMax_ = value;
iter->second = "0";
}
else
{
ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_max\" are set.", iter->first.c_str());
}
}
}
iter = parameters.find(Parameters::kIcpVoxelSize());
if(iter != parameters.end())
{
@@ -371,8 +413,14 @@ private:
}
}
LaserScan laserScan = LaserScan::backwardCompatibility(scan, maxLaserScans, 0, localScanTransform);
if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
{
laserScan = util3d::rangeFiltering(laserScan, scanRangeMin_, scanRangeMax_);
}
rtabmap::SensorData data(
LaserScan::backwardCompatibility(scan, maxLaserScans, 0, localScanTransform),
laserScan,
cv::Mat(),
cv::Mat(),
CameraModel(),
@@ -394,6 +442,8 @@ private:
ros::Publisher filtered_scan_pub_;
int scanCloudMaxPoints_;
int scanDownsamplingStep_;
double scanRangeMin_;
double scanRangeMax_;
double scanVoxelSize_;
int scanNormalK_;
double scanNormalRadius_;
src/nodelets/point_cloud_assembler.cpp
+25 -5
View File
@@ -45,6 +45,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <message_filters/sync_policies/exact_time.h>
#include <rtabmap_ros/MsgConversion.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h>
namespace rtabmap_ros
{
@@ -68,6 +70,8 @@ public:
skipClouds_(0),
cloudsSkipped_(0),
waitForTransformDuration_(0.1),
rangeMin_(0),
rangeMax_(0),
fixedFrameId_("odom")
{}
@@ -96,6 +100,8 @@ private:
pnh.param("max_clouds", maxClouds_, maxClouds_);
pnh.param("skip_clouds", skipClouds_, skipClouds_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("range_min", rangeMin_, rangeMin_);
pnh.param("range_max", rangeMax_, rangeMax_);
ROS_ASSERT(maxClouds_>0);
cloudsSkipped_ = skipClouds_;
@@ -179,12 +185,24 @@ private:
return;
}
sensor_msgs::PointCloud2 output;
pcl_ros::transformPointCloud(t.toEigen4f(), *clouds_[i], output);
pcl::PCLPointCloud2 output2;
pcl_conversions::toPCL(output, output2);
pcl::PCLPointCloud2Ptr assembledTmp(new pcl::PCLPointCloud2);
pcl::concatenatePointCloud(*assembled, output2, *assembledTmp);
if(rangeMin_ > 0.0 || rangeMax_ > 0.0)
{
pcl::PCLPointCloud2 output2;
pcl_conversions::toPCL(*clouds_[i], output2);
rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(output2);
scan = rtabmap::util3d::rangeFiltering(scan, rangeMin_, rangeMax_);
pcl::concatenatePointCloud(*assembled, *rtabmap::util3d::laserScanToPointCloud2(scan, t), *assembledTmp);
}
else
{
sensor_msgs::PointCloud2 output;
pcl_ros::transformPointCloud(t.toEigen4f(), *clouds_[i], output);
pcl::PCLPointCloud2 output2;
pcl_conversions::toPCL(output, output2);
pcl::concatenatePointCloud(*assembled, output2, *assembledTmp);
}
assembled = assembledTmp;
}
@@ -235,6 +253,8 @@ private:
int skipClouds_;
int cloudsSkipped_;
double waitForTransformDuration_;
double rangeMin_;
double rangeMax_;
std::string fixedFrameId_;
tf::TransformListener tfListener_;