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

icp_odometry: Fixed Icp/PointToPlaneK and Icp/PointPlaneRadius not updated when using directly scan_normal_k and scan_normal_radius.

Browse Source
This commit is contained in:
matlabbe
2021-12-30 14:58:25 -05:00
parent 9a3e47e054
commit 0503f1a1f6
1 changed files with 39 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/icp_odometry.cpp
+39 -4
View File
@@ -181,8 +181,8 @@ private:
iter = parameters.find(Parameters::kIcpRangeMin());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value > 1)
float value = uStr2Float(iter->second);
if(value != 0.0f)
{
if(!pnh.hasParam("scan_range_min"))
{
@@ -199,8 +199,8 @@ private:
iter = parameters.find(Parameters::kIcpRangeMax());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value > 1)
float value = uStr2Float(iter->second);
if(value != 0.0f)
{
if(!pnh.hasParam("scan_range_max"))
{
@@ -232,6 +232,11 @@ private:
}
}
}
else if(pnh.hasParam("scan_voxel_size"))
{
NODELET_INFO("IcpOdometry: scan_voxel_size is set (%f), setting %s to 0", scanVoxelSize_, Parameters::kIcpVoxelSize().c_str());
parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0"));
}
iter = parameters.find(Parameters::kIcpPointToPlaneK());
if(iter != parameters.end())
{
@@ -243,8 +248,18 @@ private:
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_k\" for convenience.", iter->second.c_str(), iter->first.c_str());
scanNormalK_ = value;
}
else
{
NODELET_INFO("IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
iter->second = uNumber2Str(scanNormalK_);
}
}
}
else if(pnh.hasParam("scan_normal_k"))
{
NODELET_INFO("IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), uNumber2Str(scanNormalK_)));
}
iter = parameters.find(Parameters::kIcpPointToPlaneRadius());
if(iter != parameters.end())
{
@@ -256,8 +271,18 @@ private:
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_radius\" for convenience.", iter->second.c_str(), iter->first.c_str());
scanNormalRadius_ = value;
}
else
{
NODELET_INFO("IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
iter->second = uNumber2Str(scanNormalK_);
}
}
}
else if(pnh.hasParam("scan_normal_radius"))
{
NODELET_INFO("IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneRadius(), uNumber2Str(scanNormalRadius_)));
}
iter = parameters.find(Parameters::kIcpPointToPlaneGroundNormalsUp());
if(iter != parameters.end())
{
@@ -269,8 +294,18 @@ private:
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_ground_up\" for convenience.", iter->second.c_str(), iter->first.c_str());
scanNormalGroundUp_ = value;
}
else
{
NODELET_INFO("IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
iter->second = uNumber2Str(scanNormalK_);
}
}
}
else if(pnh.hasParam("scan_normal_ground_up"))
{
NODELET_INFO("IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(scanNormalGroundUp_)));
}
}
void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg)