Fix precision issue for floating-point types

moveit · Sep 10, 2021 · 45935c0 · 45935c0
1 parent 4b8957c
commit 45935c0
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 1 deletion.
diff --git a/core/include/moveit/task_constructor/properties/serialize.hpp b/core/include/moveit/task_constructor/properties/serialize.hpp
@@ -128,6 +128,17 @@ class PropertySerializerROS : public PropertySerializerBase
 	}
 };
 
+template <typename T, typename SFINAE = void>
+struct oss_configure
+{
+	static void setprecision(std::ostringstream& oss) {}
+};
+template <typename T>
+struct oss_configure<T, std::enable_if_t<std::is_floating_point<T>::value>>
+{
+	static void setprecision(std::ostringstream& oss) { oss << std::setprecision(std::numeric_limits<T>::digits10 + 1); }
+};
+
 /** Serialization based on std::[io]stringstream */
 template <typename T>
 class PropertySerializerStream : public PropertySerializerBase
@@ -138,6 +149,7 @@ class PropertySerializerStream : public PropertySerializerBase
 	template <typename Q = T>
 	static std::enable_if_t<hasInsertionOperator<Q>::value, std::string> serialize(const boost::any& value) {
 		std::ostringstream oss;
+		oss_configure<Q>::setprecision(oss);
 		oss << boost::any_cast<T>(value);
 		return oss.str();
 	}

diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp
@@ -133,7 +133,7 @@ TEST(Property, storeInMsg) {
 	STORABLE_PROPERTY(int, 42);
 	STORABLE_PROPERTY(double, 42);
 	STORABLE_PROPERTY(stages::Connect::MergeMode, stages::Connect::WAYPOINTS);
-	//	STORABLE_PROPERTY(double, M_PI);
+	STORABLE_PROPERTY(double, M_PI);
 
 	STORABLE_PROPERTY(geometry_msgs::PoseStamped, []() {
 		geometry_msgs::PoseStamped msg;