Fix correct use of if constexpr()

Closes #1270
MRPT · Oct 25, 2023 · 4b6e108 · 4b6e108
1 parent 814650b
commit 4b6e108
Showing 1 changed file with 22 additions and 9 deletions.
diff --git 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