From 0503f1a1f6edb1e269499f8e1f18187a1b2dd62d Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Thu, 30 Dec 2021 14:58:25 -0500
Subject: [PATCH] icp_odometry: Fixed Icp/PointToPlaneK and
 Icp/PointPlaneRadius not updated when using directly scan_normal_k and
 scan_normal_radius.

---
 src/nodelets/icp_odometry.cpp | 43 +++++++++++++++++++++++++++++++----
 1 file changed, 39 insertions(+), 4 deletions(-)

diff --git a/src/nodelets/icp_odometry.cpp b/src/nodelets/icp_odometry.cpp
index e7893200..efb205cc 100644
--- a/src/nodelets/icp_odometry.cpp
+++ b/src/nodelets/icp_odometry.cpp
@@ -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)