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

rtabmap.launch: added rgbd sync option. Updated icp_odometry node.

Browse Source
This commit is contained in:
matlabbe
2017-11-30 15:46:11 -05:00
parent 46a9e91884
commit 5115b9a96b
2 changed files with 137 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/icp_odometry.cpp
+47 -1
View File
@@ -99,11 +99,57 @@ private:
{
//make sure we are using Reg/Strategy=0
ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
if(iter != parameters.end() && iter->second.compare("0") != 0)
if(iter != parameters.end() && iter->second.compare("1") != 0)
{
ROS_WARN("ICP odometry works only with \"Reg/Strategy\"=1. Ignoring value %s.", iter->second.c_str());
}
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1"));
ros::NodeHandle & pnh = getPrivateNodeHandle();
iter = parameters.find(Parameters::kIcpVoxelSize());
if(iter != parameters.end())
{
float value = uStr2Float(iter->second);
if(value != 0.0f)
{
if(!pnh.hasParam("scan_voxel_size"))
{
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_voxel_size\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
scanVoxelSize_ = value;
iter->second = "0";
}
else
{
ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_voxel_size\" are set.", iter->first.c_str());
}
}
}
iter = parameters.find(Parameters::kIcpPointToPlaneK());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value != 0)
{
if(!pnh.hasParam("scan_normal_k"))
{
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;
}
}
}
iter = parameters.find(Parameters::kIcpPointToPlaneRadius());
if(iter != parameters.end())
{
float value = uStr2Float(iter->second);
if(value != 0.0f)
{
if(!pnh.hasParam("scan_normal_radius"))
{
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;
}
}
}
}
void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg)