From 4b6e10895848a1c66e12d26e63dad4e3ebb52bc8 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco-Claraco <joseluisblancoc@gmail.com>
Date: Wed, 25 Oct 2023 12:55:03 +0200
Subject: [PATCH] Fix correct use of if constexpr()

Closes #1270
---
 libs/ros2bridge/src/range.cpp | 31 ++++++++++++++++++++++---------
 1 file changed, 22 insertions(+), 9 deletions(-)

diff --git a/libs/ros2bridge/src/range.cpp b/libs/ros2bridge/src/range.cpp
index d33dd55fa6..62655ca104 100644
--- a/libs/ros2bridge/src/range.cpp
+++ b/libs/ros2bridge/src/range.cpp
@@ -27,6 +27,26 @@ struct has_variance<T, std::void_t<decltype(T::variance)>> : std::true_type
 {
 };
 
+// for "if constexpr" to work (avoid build errors if field does not exist)
+// it must be inside a template:
+template <class MSG_T>
+void fromROS_variance(const MSG_T& msg, mrpt::obs::CObservationRange& obj)
+{
+	if constexpr (has_variance<MSG_T>::value)
+	{
+		obj.sensedData.at(0).sensorNoiseStdDeviation = std::sqrt(msg.variance);
+	}
+}
+template <class MSG_T>
+void toROS_variance(
+	MSG_T& msg, const mrpt::obs::CObservationRange::TMeasurement& m)
+{
+	if constexpr (has_variance<MSG_T>::value)
+	{
+		msg.variance = mrpt::square(m.sensorNoiseStdDeviation);
+	}
+}
+
 bool mrpt::ros2bridge::fromROS(
 	const sensor_msgs::msg::Range& msg, mrpt::obs::CObservationRange& obj)
 {
@@ -38,10 +58,7 @@ bool mrpt::ros2bridge::fromROS(
 	obj.sensedData.at(0).sensedDistance = msg.range;
 
 	// See: https://github.com/MRPT/mrpt/issues/1270
-	if constexpr (has_variance<sensor_msgs::msg::Range>::value)
-	{
-		obj.sensedData.at(0).sensorNoiseStdDeviation = std::sqrt(msg.variance);
-	}
+	fromROS_variance(msg, obj);
 
 	return true;
 }
@@ -64,11 +81,7 @@ bool mrpt::ros2bridge::toROS(
 		msg[i].field_of_view = obj.sensorConeAperture;
 
 		// See: https://github.com/MRPT/mrpt/issues/1270
-		if constexpr (has_variance<sensor_msgs::msg::Range>::value)
-		{
-			msg[i].variance =
-				mrpt::square(obj.sensedData[i].sensorNoiseStdDeviation);
-		}
+		toROS_variance(msg[i], obj.sensedData[i]);
 	}
 
 	/// following part needs to be double checked, it looks incorrect