Initial commit

danielduberg · Mar 2, 2020 · c971c70 · c971c70
1 parent dc05eed
commit c971c70
Show file tree

Hide file tree

Showing 4 changed files with 388 additions and 0 deletions.
diff --git a/ufomap_ros/CMakeLists.txt b/ufomap_ros/CMakeLists.txt
@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ufomap_ros)
+
+set(CMAKE_CXX_STANDARD 17)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  sensor_msgs
+  ufomap
+  ufomap_msgs
+)
+
+catkin_package(
+  INCLUDE_DIRS include ${catkin_INCLUDE_DIRS}
+  LIBRARIES ${PROJECT_NAME} ${catkin_LIBRARIES}
+  CATKIN_DEPENDS roscpp sensor_msgs ufomap ufomap_msgs
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME}
+  src/conversions.cpp
+)
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+)
diff --git a/ufomap_ros/include/ufomap_ros/conversions.h b/ufomap_ros/include/ufomap_ros/conversions.h
@@ -0,0 +1,83 @@
+#ifndef UFOMAP_ROS_CONVERSIONS_H
+#define UFOMAP_ROS_CONVERSIONS_H
+
+#include <ufomap/point_cloud.h>
+
+#include <sensor_msgs/PointCloud2.h>
+
+namespace ufomap
+{
+/**
+ * @brief Converts ROS point cloud msg to UFOMap point cloud
+ *
+ * @code{.cpp}
+	void callback(const sensor_msgs::PointCloud2::ConstPtr& ros_cloud)
+	{
+		ufomap::PointCloud ufo_cloud;
+		ufomap::toUfomap(ros_cloud, ufo_cloud);
+	}
+ * @endcode
+ *
+ * @param cloud_in The ROS point cloud msg
+ * @param cloud_out The UFOMap point cloud
+ */
+void toUfomap(sensor_msgs::PointCloud2::ConstPtr cloud_in, PointCloud& cloud_out);
+
+/**
+ * @brief Converts UFOMap point cloud to ROS point cloud msg
+ *
+ * @param cloud_in The UFOMap point cloud
+ * @param cloud_out The ROS point cloud msg
+ */
+void fromUfomap(const PointCloud& cloud_in, sensor_msgs::PointCloud2::Ptr cloud_out);
+
+/**
+ * @brief Converts ROS RGB point cloud msg to UFOMap RGB point cloud
+ *
+ * @code{.cpp}
+	void callback(const sensor_msgs::PointCloud2::ConstPtr& ros_cloud)
+	{
+		ufomap::PointCloudRGB ufo_cloud;
+		ufomap::toUfomap(ros_cloud, ufo_cloud);
+	}
+ * @endcode
+ *
+ * @param cloud_in The ROS RGB point cloud msg
+ * @param cloud_out The UFOMap RGB point cloud
+ */
+void toUfomap(sensor_msgs::PointCloud2::ConstPtr cloud_in, PointCloudRGB& cloud_out);
+
+/**
+ * @brief Converts UFOMap RGB point cloud to ROS RGB point cloud msg
+ *
+ * @param cloud_in The UFOMap RGB point cloud
+ * @param cloud_out The ROS RGB point cloud msg
+ */
+void fromUfomap(const PointCloudRGB& cloud_in, sensor_msgs::PointCloud2::Ptr cloud_out);
+
+/**
+ * @brief Converts ROS intensity point cloud msg to UFOMap intensity point cloud
+ *
+ * @code{.cpp}
+	void callback(const sensor_msgs::PointCloud2::ConstPtr& ros_cloud)
+	{
+		ufomap::PointCloudI ufo_cloud;
+		ufomap::toUfomap(ros_cloud, ufo_cloud);
+	}
+ * @endcode
+ *
+ * @param cloud_in The ROS intensity point cloud msg
+ * @param cloud_out The UFOMap intensity point cloud
+ */
+void toUfomap(sensor_msgs::PointCloud2::ConstPtr cloud_in, PointCloudI& cloud_out);
+
+/**
+ * @brief Converts UFOMap intensity point cloud to ROS intensity point cloud msg
+ *
+ * @param cloud_in The UFOMap intensity point cloud
+ * @param cloud_out The ROS intensity point cloud msg
+ */
+void fromUfomap(const PointCloudI& cloud_in, sensor_msgs::PointCloud2::Ptr cloud_out);
+}  // namespace ufomap
+
+#endif  // UFOMAP_ROS_CONVERSIONS_H
diff --git a/ufomap_ros/package.xml b/ufomap_ros/package.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ufomap_ros</name>
+  <version>1.0.0</version>
+  <description>The ufomap_ros package provides conversion functions between UFOMap and ROS native types</description>
+
+  <author email="[email protected]">Daniel Duberg</author>
+  <maintainer email="[email protected]">Daniel Duberg</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">https://github.com/danielduberg/UFOMap</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>roscpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>ufomap</depend>
+  <depend>ufomap_msgs</depend>
+
+</package>
diff --git a/ufomap_ros/src/conversions.cpp b/ufomap_ros/src/conversions.cpp
@@ -0,0 +1,243 @@
+#include <ufomap/types.h>
+#include <ufomap_ros/conversions.h>
+
+#include <sensor_msgs/point_cloud2_iterator.h>
+
+// TODO: Add support for intensity?
+
+namespace ufomap
+{
+void getFields(sensor_msgs::PointCloud2::ConstPtr cloud, bool& has_x, bool& has_y,
+							 bool& has_z, bool& has_rgb, bool& has_i)
+{
+	has_x = false;
+	has_y = false;
+	has_z = false;
+	has_rgb = false;
+	has_i = false;
+
+	for (const auto& field : cloud->fields)
+	{
+		if ("x" == field.name)
+		{
+			has_x = true;
+		}
+		else if ("y" == field.name)
+		{
+			has_y = true;
+		}
+		else if ("z" == field.name)
+		{
+			has_z = true;
+		}
+		else if ("rgb" == field.name)
+		{
+			has_rgb = true;
+		}
+		else if ("r" == field.name)
+		{
+			has_rgb = true;
+		}
+		else if ("g" == field.name)
+		{
+			has_rgb = true;
+		}
+		else if ("b" == field.name)
+		{
+			has_rgb = true;
+		}
+		else if ("intensity" == field.name)
+		{
+			has_i = true;
+		}
+	}
+}
+
+void toUfomap(sensor_msgs::PointCloud2::ConstPtr cloud_in, PointCloud& cloud_out)
+{
+	cloud_out.reserve(cloud_in->data.size() / cloud_in->point_step);
+
+	bool has_x, has_y, has_z, has_rgb, has_i;
+	getFields(cloud_in, has_x, has_y, has_z, has_rgb, has_i);
+
+	if (!has_x || !has_y || !has_z)
+	{
+		throw std::runtime_error("cloud_in missing one or more of the xyz fields");
+	}
+
+	sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud_in, "x");
+	sensor_msgs::PointCloud2ConstIterator<float> iter_y(*cloud_in, "y");
+	sensor_msgs::PointCloud2ConstIterator<float> iter_z(*cloud_in, "z");
+
+	for (int i = 0; iter_x != iter_x.end(); ++i, ++iter_x, ++iter_y, ++iter_z)
+	{
+		if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z))
+		{
+			cloud_out.push_back(Point3(*iter_x, *iter_y, *iter_z));
+		}
+	}
+}
+
+void fromUfomap(const PointCloud& cloud_in, sensor_msgs::PointCloud2::Ptr cloud_out)
+{
+	bool has_x, has_y, has_z, has_rgb, has_i;
+	getFields(cloud_out, has_x, has_y, has_z, has_rgb, has_i);
+
+	// if (!has_x || !has_y || !has_z)
+	// {
+	// 	throw std::runtime_error("cloud_out missing one or more of the xyz fields");
+	// }
+
+	// TODO: Is these two needed?
+	sensor_msgs::PointCloud2Modifier cloud_out_modifier(*cloud_out);
+	cloud_out_modifier.setPointCloud2FieldsByString(1, "xyz");
+	cloud_out_modifier.resize(cloud_in.size());
+
+	sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_out, "x");
+	sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_out, "y");
+	sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_out, "z");
+
+	for (unsigned int i = 0; i < cloud_in.size(); ++i, ++iter_x, ++iter_y, ++iter_z)
+	{
+		*iter_x = cloud_in[i][0];
+		*iter_y = cloud_in[i][1];
+		*iter_z = cloud_in[i][2];
+	}
+}
+
+void toUfomap(sensor_msgs::PointCloud2::ConstPtr cloud_in, PointCloudRGB& cloud_out)
+{
+	cloud_out.reserve(cloud_in->data.size() / cloud_in->point_step);
+
+	bool has_x, has_y, has_z, has_rgb, has_i;
+	getFields(cloud_in, has_x, has_y, has_z, has_rgb, has_i);
+
+	if (!has_x || !has_y || !has_z)
+	{
+		throw std::runtime_error("cloud_in missing one or more of the xyz fields");
+	}
+
+	if (!has_rgb)
+	{
+		throw std::runtime_error("cloud_in missing one or more of the rgb fields");
+	}
+
+	sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud_in, "x");
+	sensor_msgs::PointCloud2ConstIterator<float> iter_y(*cloud_in, "y");
+	sensor_msgs::PointCloud2ConstIterator<float> iter_z(*cloud_in, "z");
+	sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_rgb(*cloud_in, "rgb");
+
+	for (int i = 0; iter_x != iter_x.end(); ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb)
+	{
+		if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z) &&
+				!std::isnan(iter_rgb[0]) && !std::isnan(iter_rgb[1]) && !std::isnan(iter_rgb[2]))
+		{
+			cloud_out.push_back(
+					Point3RGB(*iter_x, *iter_y, *iter_z, iter_rgb[2], iter_rgb[1], iter_rgb[0]));
+		}
+	}
+}
+
+void fromUfomap(const PointCloudRGB& cloud_in, sensor_msgs::PointCloud2::Ptr cloud_out)
+{
+	bool has_x, has_y, has_z, has_rgb, has_i;
+	getFields(cloud_out, has_x, has_y, has_z, has_rgb, has_i);
+
+	// if (!has_x || !has_y || !has_z)
+	// {
+	// 	throw std::runtime_error("cloud_out missing one or more of the xyz fields");
+	// }
+
+	// if (!has_r || !has_g || !has_b)
+	// {
+	// 	throw std::runtime_error("cloud_out missing one or more of the rgb fields");
+	// }
+
+	// TODO: Is these two needed?
+	sensor_msgs::PointCloud2Modifier cloud_out_modifier(*cloud_out);
+	cloud_out_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
+	cloud_out_modifier.resize(cloud_in.size());
+
+	sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_out, "x");
+	sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_out, "y");
+	sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_out, "z");
+	sensor_msgs::PointCloud2Iterator<uint8_t> iter_rgb(*cloud_out, "rgb");
+
+	for (unsigned int i = 0; i < cloud_in.size();
+			 ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb)
+	{
+		*iter_x = cloud_in[i][0];
+		*iter_y = cloud_in[i][1];
+		*iter_z = cloud_in[i][2];
+		iter_rgb[0] = cloud_in[i].getColor().r;
+		iter_rgb[1] = cloud_in[i].getColor().g;
+		iter_rgb[2] = cloud_in[i].getColor().b;
+	}
+}
+
+void toUfomap(sensor_msgs::PointCloud2::ConstPtr cloud_in, PointCloudI& cloud_out)
+{
+	cloud_out.reserve(cloud_in->data.size() / cloud_in->point_step);
+
+	bool has_x, has_y, has_z, has_rgb, has_i;
+	getFields(cloud_in, has_x, has_y, has_z, has_rgb, has_i);
+
+	if (!has_x || !has_y || !has_z)
+	{
+		throw std::runtime_error("cloud_in missing one or more of the xyz fields");
+	}
+
+	if (!has_i)
+	{
+		throw std::runtime_error("cloud_in missing the intensity field");
+	}
+
+	sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud_in, "x");
+	sensor_msgs::PointCloud2ConstIterator<float> iter_y(*cloud_in, "y");
+	sensor_msgs::PointCloud2ConstIterator<float> iter_z(*cloud_in, "z");
+	sensor_msgs::PointCloud2ConstIterator<float> iter_i(*cloud_in, "intensity");
+
+	for (int i = 0; iter_x != iter_x.end(); ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_i)
+	{
+		if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z))
+		{
+			cloud_out.push_back(Point3I(*iter_x, *iter_y, *iter_z, *iter_i));
+		}
+	}
+}
+
+void fromUfomap(const PointCloudI& cloud_in, sensor_msgs::PointCloud2::Ptr cloud_out)
+{
+	bool has_x, has_y, has_z, has_rgb, has_i;
+	getFields(cloud_out, has_x, has_y, has_z, has_rgb, has_i);
+
+	// if (!has_x || !has_y || !has_z)
+	// {
+	// 	throw std::runtime_error("cloud_out missing one or more of the xyz fields");
+	// }
+
+	// if (!has_r || !has_g || !has_b)
+	// {
+	// 	throw std::runtime_error("cloud_out missing one or more of the rgb fields");
+	// }
+
+	// TODO: Is these two needed?
+	sensor_msgs::PointCloud2Modifier cloud_out_modifier(*cloud_out);
+	cloud_out_modifier.setPointCloud2FieldsByString(2, "xyz", "intensity");
+	cloud_out_modifier.resize(cloud_in.size());
+
+	sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_out, "x");
+	sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_out, "y");
+	sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_out, "z");
+	sensor_msgs::PointCloud2Iterator<float> iter_i(*cloud_out, "intensity");
+
+	for (unsigned int i = 0; i < cloud_in.size();
+			 ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_i)
+	{
+		*iter_x = cloud_in[i][0];
+		*iter_y = cloud_in[i][1];
+		*iter_z = cloud_in[i][2];
+		*iter_i = cloud_in[i].getIntensity();
+	}
+}
+}  // namespace ufomap