Added more conversions

ufomap/src/math/pose6.cpp

@@ -25,6 +25,11 @@ Pose6::Pose6(float x, float y, float z, float roll, float pitch, float yaw)
 {
 }
 
+Pose6::Pose6(float t_x, float t_y, float t_z, float r_w, float r_x, float r_y, float r_z)
+	: translation_(t_x, t_y, t_z), rotation_(r_w, r_x, r_y, r_z)
+{
+}
+
 Pose6& Pose6::operator=(const Pose6& rhs)
 {
 	translation_ = rhs.translation_;
@@ -145,4 +150,4 @@ float Pose6::translationLength() const
 	return sqrt((x() * x()) + (y() * y()) + (z() * z()));
 }
 
-}  // namespace ufomap
+}  // namespace ufomap_math

ufomap_msgs/include/ufomap_msgs/conversions.h

@@ -19,10 +19,10 @@ bool msgToMap(const Ufomap& msg, TreeType& tree)
 
 template <typename TreeType>
 bool mapToMsgData(const TreeType& tree, std::vector<int8_t>& map_data,
-									bool binary = false)
+									bool compress = false, bool binary = false)
 {
 	std::stringstream data_stream;
-	if (!tree.writeData(data_stream, binary))
+	if (!tree.writeData(data_stream, compress, binary))
 	{
 		return false;
 	}
@@ -33,14 +33,16 @@ bool mapToMsgData(const TreeType& tree, std::vector<int8_t>& map_data,
 }
 
 template <typename TreeType>
-bool mapToMsg(const TreeType& tree, Ufomap& msg, bool binary = false)
+bool mapToMsg(const TreeType& tree, Ufomap& msg, bool compress = false,
+							bool binary = false)
 {
 	msg.resolution = tree.getResolution();
 	msg.id = tree.getTreeType();
 	msg.depth_levels = tree.getTreeDepthLevels();
+	msg.compressed = compress;
 	msg.binary = binary;
 
-	return mapToMsgData(tree, msg.data);
+	return mapToMsgData(tree, msg.data, compress, binary);
 }
 }  // namespace ufomap_msgs
 

ufomap_ros/include/ufomap_ros/conversions.h

@@ -1,24 +1,64 @@
 #ifndef UFOMAP_ROS_CONVERSIONS_H
 #define UFOMAP_ROS_CONVERSIONS_H
 
+#include <ufomap/math/pose6.h>
+#include <ufomap/math/quaternion.h>
+#include <ufomap/math/vector3.h>
 #include <ufomap/point_cloud.h>
 
+#include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/Transform.h>
+#include <geometry_msgs/Vector3.h>
 #include <sensor_msgs/PointCloud2.h>
 
 namespace ufomap
 {
+// Point clouds
 void toUfomap(sensor_msgs::PointCloud2::ConstPtr cloud_in, PointCloud& cloud_out);
 
 void fromUfomap(const PointCloud& cloud_in, sensor_msgs::PointCloud2::Ptr cloud_out);
 
-
 void toUfomap(sensor_msgs::PointCloud2::ConstPtr cloud_in, PointCloudRGB& cloud_out);
 
 void fromUfomap(const PointCloudRGB& cloud_in, sensor_msgs::PointCloud2::Ptr cloud_out);
 
 void toUfomap(sensor_msgs::PointCloud2::ConstPtr cloud_in, PointCloudI& cloud_out);
 
 void fromUfomap(const PointCloudI& cloud_in, sensor_msgs::PointCloud2::Ptr cloud_out);
+
+// Vector3
+void toUfomap(const geometry_msgs::Vector3& vector3_in,
+							ufomap_math::Vector3& vector3_out);
+
+ufomap_math::Vector3 toUfomap(const geometry_msgs::Vector3& vector3);
+
+void fromUfomap(const ufomap_math::Vector3& vector3_in,
+								geometry_msgs::Vector3& vector3_out);
+
+geometry_msgs::Vector3 fromUfomap(const ufomap_math::Vector3& vector3);
+
+// Quaternion
+void toUfomap(const geometry_msgs::Quaternion& quaternion_in,
+							ufomap_math::Quaternion& quaternion_out);
+
+ufomap_math::Quaternion toUfomap(const geometry_msgs::Quaternion& quaternion);
+
+void fromUfomap(const ufomap_math::Quaternion& quaternion_in,
+								geometry_msgs::Quaternion& quaternion_out);
+
+geometry_msgs::Quaternion fromUfomap(const ufomap_math::Quaternion& quaternion);
+
+// Transforms
+void toUfomap(const geometry_msgs::Transform& transform_in,
+							ufomap_math::Pose6& transform_out);
+
+ufomap_math::Pose6 toUfomap(const geometry_msgs::Transform& transform);
+
+void fromUfomap(const ufomap_math::Pose6& transform_in,
+								geometry_msgs::Transform& transform_out);
+
+geometry_msgs::Transform fromUfomap(const ufomap_math::Pose6& transform);
+
 }  // namespace ufomap
 
-#endif // UFOMAP_ROS_CONVERSIONS_H
+#endif  // UFOMAP_ROS_CONVERSIONS_H

ufomap_ros/src/conversions.cpp

@@ -240,4 +240,94 @@ void fromUfomap(const PointCloudI& cloud_in, sensor_msgs::PointCloud2::Ptr cloud
 		*iter_i = cloud_in[i].getIntensity();
 	}
 }
+
+// Vector3
+void toUfomap(const geometry_msgs::Vector3& vector3_in, ufomap_math::Vector3& vector3_out)
+{
+	vector3_out.x() = vector3_in.x;
+	vector3_out.y() = vector3_in.y;
+	vector3_out.z() = vector3_in.z;
+}
+
+ufomap_math::Vector3 toUfomap(const geometry_msgs::Vector3& vector3)
+{
+	return ufomap_math::Vector3(vector3.x, vector3.y, vector3.z);
+}
+
+void fromUfomap(const ufomap_math::Vector3& vector3_in,
+								geometry_msgs::Vector3& vector3_out)
+{
+	vector3_out.x = vector3_in.x();
+	vector3_out.y = vector3_in.y();
+	vector3_out.z = vector3_in.z();
+}
+
+geometry_msgs::Vector3 fromUfomap(const ufomap_math::Vector3& vector3)
+{
+	geometry_msgs::Vector3 vector3_out;
+	fromUfomap(vector3, vector3_out);
+	return vector3_out;
+}
+
+// Quaternion
+void toUfomap(const geometry_msgs::Quaternion& quaternion_in,
+							ufomap_math::Quaternion& quaternion_out)
+{
+	quaternion_out.x() = quaternion_in.x;
+	quaternion_out.y() = quaternion_in.y;
+	quaternion_out.z() = quaternion_in.z;
+	quaternion_out.w() = quaternion_in.w;
+}
+
+ufomap_math::Quaternion toUfomap(const geometry_msgs::Quaternion& quaternion)
+{
+	return ufomap_math::Quaternion(quaternion.w, quaternion.x, quaternion.y, quaternion.z);
+}
+
+void fromUfomap(const ufomap_math::Quaternion& quaternion_in,
+								geometry_msgs::Quaternion& quaternion_out)
+{
+	quaternion_out.x = quaternion_in.x();
+	quaternion_out.y = quaternion_in.y();
+	quaternion_out.z = quaternion_in.z();
+	quaternion_out.w = quaternion_in.w();
+}
+
+geometry_msgs::Quaternion fromUfomap(const ufomap_math::Quaternion& quaternion)
+{
+	geometry_msgs::Quaternion quaternion_out;
+	fromUfomap(quaternion, quaternion_out);
+	return quaternion_out;
+}
+
+// Transforms
+void toUfomap(const geometry_msgs::Transform& transform_in,
+							ufomap_math::Pose6& transform_out)
+{
+	toUfomap(transform_in.translation, transform_out.translation());
+	toUfomap(transform_in.rotation, transform_out.rotation());
+}
+
+ufomap_math::Pose6 toUfomap(const geometry_msgs::Transform& transform)
+{
+	return ufomap_math::Pose6(transform.translation.x, transform.translation.y,
+														transform.translation.z, transform.rotation.w,
+														transform.rotation.x, transform.rotation.y,
+														transform.rotation.z);
+}
+
+void fromUfomap(const ufomap_math::Pose6& transform_in,
+								geometry_msgs::Transform& transform_out)
+{
+	fromUfomap(transform_in.translation(), transform_out.translation);
+	fromUfomap(transform_in.rotation(), transform_out.rotation);
+}
+
+geometry_msgs::Transform fromUfomap(const ufomap_math::Pose6& transform)
+{
+	geometry_msgs::Transform transform_out;
+	fromUfomap(transform, transform_out);
+	return transform_out;
+}
+
 }  // namespace ufomap