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

Fixed gen_scan not working since this commit https://github.com/introlab/rtabmap_ros/commit/7e283f9f1d20a9628ed29f10a39f82b7c90c9002

Browse Source
This commit is contained in:
matlabbe
2020-12-23 17:05:41 -05:00
parent 7bae01ff39
commit 656b049369
1 changed files with 27 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+27 -5
View File
@@ -222,6 +222,28 @@ void CoreWrapper::onInit()
NODELET_INFO("rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false");
}
NODELET_INFO("rtabmap: gen_scan = %s", genScan_?"true":"false");
if(genScan_)
{
NODELET_INFO("rtabmap: gen_scan_max_depth = %f", genScanMaxDepth_);
NODELET_INFO("rtabmap: gen_scan_min_depth = %f", genScanMinDepth_);
}
NODELET_INFO("rtabmap: gen_depth = %s", genDepth_?"true":"false");
if(genDepth_)
{
NODELET_INFO("rtabmap: gen_depth_decimation = %d", genDepthDecimation_);
NODELET_INFO("rtabmap: gen_depth_fill_holes_size = %d", genDepthFillHolesSize_);
NODELET_INFO("rtabmap: gen_depth_fill_iterations = %d", genDepthFillIterations_);
NODELET_INFO("rtabmap: gen_depth_fill_holes_error = %f", genDepthFillHolesError_);
}
bool subscribeScanCloud = false;
pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud);
if(subscribeScanCloud)
{
NODELET_INFO("rtabmap: scan_cloud_max_points = %d", scanCloudMaxPoints_);
}
infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
mapGraphPub_ = nh.advertise<rtabmap_ros::MapGraph>("mapGraph", 1);
@@ -410,9 +432,9 @@ void CoreWrapper::onInit()
pnh.param("subscribe_scan", subscribeScan2d, subscribeScan2d);
pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
bool gridFromDepth = Parameters::defaultGridFromDepth();
if((subscribeScan2d || subscribeScan3d) && parameters_.find(Parameters::kGridFromDepth()) == parameters_.end())
if((subscribeScan2d || subscribeScan3d || genScan_) && parameters_.find(Parameters::kGridFromDepth()) == parameters_.end())
{
NODELET_WARN("Setting \"%s\" parameter to false (default true) as \"subscribe_scan\" or \"subscribe_scan_cloud\" is "
NODELET_WARN("Setting \"%s\" parameter to false (default true) as \"subscribe_scan\", \"subscribe_scan_cloud\" or \"gen_scan\" is "
"true. The occupancy grid map will be constructed from "
"laser scans. To get occupancy grid map from cloud projection, set \"%s\" "
"to true. To suppress this warning, "
@@ -423,9 +445,9 @@ void CoreWrapper::onInit()
parameters_.insert(ParametersPair(Parameters::kGridFromDepth(), "false"));
}
Parameters::parse(parameters_, Parameters::kGridFromDepth(), gridFromDepth);
if((subscribeScan2d || subscribeScan3d) && parameters_.find(Parameters::kGridRangeMax()) == parameters_.end() && !gridFromDepth)
if((subscribeScan2d || subscribeScan3d || genScan_) && parameters_.find(Parameters::kGridRangeMax()) == parameters_.end() && !gridFromDepth)
{
NODELET_INFO("Setting \"%s\" parameter to 0 (default %f) as \"subscribe_scan\" or \"subscribe_scan_cloud\" is true and %s is false.",
NODELET_INFO("Setting \"%s\" parameter to 0 (default %f) as \"subscribe_scan\", \"subscribe_scan_cloud\" or \"gen_scan\" is true and %s is false.",
Parameters::kGridRangeMax().c_str(),
Parameters::defaultGridRangeMax(),
Parameters::kGridFromDepth().c_str());
@@ -1196,7 +1218,7 @@ void CoreWrapper::commonDepthCallbackImpl(
LaserScan scan;
bool genMaxScanPts = 0;
if(!scan2dMsg.ranges.empty() && !scan3dMsg.data.empty() && !depth.empty() && genScan_)
if(scan2dMsg.ranges.empty() && scan3dMsg.data.empty() && !depth.empty() && genScan_)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud2d(new pcl::PointCloud<pcl::PointXYZ>);
*scanCloud2d = util3d::laserScanFromDepthImages(