First complete version

ufomap_mapping/CMakeLists.txt

@@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS
   std_srvs
   tf2
   tf2_ros
+  tf2_sensor_msgs
   ufomap
   ufomap_msgs
   ufomap_ros
@@ -96,10 +97,9 @@ find_package(catkin REQUIRED COMPONENTS
 ##     and list every .cfg file to be processed
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
+generate_dynamic_reconfigure_options(
+  cfg/Server.cfg
+)
 
 ###################################
 ## catkin specific configuration ##
@@ -111,8 +111,8 @@ find_package(catkin REQUIRED COMPONENTS
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES ufomap_mapping
+  INCLUDE_DIRS include
+  LIBRARIES ufomap_mapping_server_nodelet
 #  CATKIN_DEPENDS roscpp ufomap ufomap_msgs
 #  DEPENDS system_lib
 )
@@ -124,24 +124,32 @@ catkin_package(
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(
-# include
+  include
   ${catkin_INCLUDE_DIRS}
 )
 
 ## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/ufomap_mapping.cpp
-# )
+add_library(${PROJECT_NAME}_server_nodelet
+  src/server_nodelet.cpp
+  src/server.cpp
+)
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
 ## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_server_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(${PROJECT_NAME}_server_nodelet
+  ${catkin_LIBRARIES}
+)
 
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/ufomap_mapping_node.cpp)
+add_executable(${PROJECT_NAME}_server_node 
+  src/server_node.cpp
+  src/server.cpp
+)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -151,12 +159,12 @@ include_directories(
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_server_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
+target_link_libraries(${PROJECT_NAME}_server_node
+  ${catkin_LIBRARIES}
+)
 
 #############
 ## Install ##
@@ -174,31 +182,34 @@ include_directories(
 
 ## Mark executables for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+install(TARGETS ${PROJECT_NAME}_server_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
 ## Mark libraries for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
+install(TARGETS ${PROJECT_NAME}_server_nodelet 
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
 
 ## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  # PATTERN ".svn" EXCLUDE
+)
 
 ## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
+install(FILES
+  nodelet_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)
 
 #############
 ## Testing ##

ufomap_mapping/cfg/Server.cfg

@@ -0,0 +1,34 @@
+#!/usr/bin/env python
+PACKAGE = "ufomap_mapping"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#           Name                 Type      Level Description                                            Default Min  Max
+gen.add("frame_id",              str_t,    1,    "Frame id",                                            "map")
+
+gen.add("max_range",             double_t, 2,    "Max range (m) when integrating data into map",        7.0,    0.0, 100.0)
+
+gen.add("insert_discrete",       bool_t,   3,    "Enable discrete integration",                         True)
+gen.add("insert_depth",          int_t,    3,    "Integration depth of the octree",                     0,      0,   10)
+gen.add("insert_n",              int_t,    3,    "The n in integration for UFOMap, 0 or 2 recommended", 0,      0,   10)
+gen.add("clear_robot",           bool_t,   3,    "Clear map at robot position",                         True)
+
+gen.add("robot_height",          double_t, 4,    "Robot height (m)",                                    0.2,    0.0, 100.0)
+gen.add("robot_radius",          double_t, 4,    "Robot radius (m)",                                    0.5,    0.0, 100.0)
+
+gen.add("pub_rate",              double_t, 5,    "How often to publish map (/s)",                       1.0,    0.0, 100.0)
+
+gen.add("transform_timeout",     double_t, 6,    "How long to wait for transform (s)",                  0.1,    0.0, 100.0)
+
+gen.add("cloud_in_queue_size",   int_t,    7,    "Queue size for cloud_in",                             10,     0,   10000)
+gen.add("map_queue_size",        int_t,    7,    "Queue size for map",                                  1,      0,   10000)
+gen.add("map_binary_queue_size", int_t,    7,    "Queue size for map_binary",                           1,      0,   10000)
+gen.add("map_cloud_queue_size",  int_t,    7,    "Queue size for map_cloud",                            1,      0,   10000)
+
+gen.add("map_latch",             bool_t,   8,    "Enable latched map topic",                            False)
+gen.add("map_binary_latch",      bool_t,   8,    "Enable latched map binary topic",                     False)
+gen.add("map_cloud_latch",       bool_t,   8,    "Enable latched map cloud topic",                      False)
+
+exit(gen.generate(PACKAGE, "ufomap_mapping", "Server"))

ufomap_mapping/include/server.h → ufomap_mapping/include/ufomap_mapping/server.h

@@ -11,21 +11,24 @@
 #include <tf2_ros/transform_listener.h>
 #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
 
-namespace ufomap_server
+namespace ufomap_mapping
 {
 class UFOMapServer
 {
 public:
-	UFOMapServer();
+	UFOMapServer(ros::NodeHandle& nh, ros::NodeHandle& nh_priv);
 
 private:
 	void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
 
 	void timerCallback(const ros::TimerEvent& event);
 
-	void configCallback(ufomap_server::ServerConfig& config, uint32_t level);
+	void configCallback(ufomap_mapping::ServerConfig& config, uint32_t level);
 
 private:
+	ros::NodeHandle& nh_;
+	ros::NodeHandle& nh_priv_;
+
 	ros::Subscriber cloud_sub_;
 
 	ros::Publisher map_pub_;
@@ -42,14 +45,35 @@ class UFOMapServer
 	tf2_ros::Buffer tf_buffer_;
 	tf2_ros::TransformListener tf_listener_;
 
-	float max_range_;
+	// Dynamic reconfigure
+	dynamic_reconfigure::Server<ufomap_mapping::ServerConfig> cs_;
+	dynamic_reconfigure::Server<ufomap_mapping::ServerConfig>::CallbackType f_;
+
+	ufomap::Octree map_;
+
+	// Configureable variables
 
 	std::string frame_id_;
 
-	bool discrete_insert_;
-	bool depth_insert_;
-	bool n_insert_;
+	float max_range_;
+
+	bool insert_discrete_;
+	unsigned int insert_depth_;
+	unsigned int insert_n_;
+	bool clear_robot_enabled_;
+
+	float robot_height_;
+	float robot_radius_;
+
+	float pub_rate_;
+
+	ros::Duration transform_timeout_;
+
+	unsigned int cloud_in_queue_size_;
+	unsigned int map_queue_size_;
+	unsigned int map_binary_queue_size_;
+	unsigned int map_cloud_queue_size_;
 };
-}  // namespace ufomap_server
+}  // namespace ufomap_mapping
 
 #endif  // UFOMAP_MAPPING_SERVER_H

ufomap_mapping/include/ufomap_mapping/server_nodelet.h

@@ -0,0 +1,22 @@
+#ifndef UFOMAP_MAPPING_SERVER_NODELET_H
+#define UFOMAP_MAPPING_SERVER_NODELET_H
+
+#include <nodelet/nodelet.h>
+
+#include <ufomap_mapping/server.h>
+
+#include <memory>
+
+namespace ufomap_mapping
+{
+class UFOMapServerNodelet : public nodelet::Nodelet
+{
+private:
+	std::shared_ptr<UFOMapServer> server_;
+
+public:
+	void onInit() override;
+};
+}  // namespace ufomap_mapping
+
+#endif  // UFOMAP_MAPPING_SERVER_NODELET_H

ufomap_mapping/launch/server.launch

@@ -0,0 +1,15 @@
+<?xml version="1.0" ?>
+<launch>
+	<arg name="resolution" default="0.1" />
+	<arg name="depth_levels" default="16" />
+	<arg name="multithreaded" default="false" />
+
+  <node pkg="ufomap_mapping" type="ufomap_mapping_server_node" name="ufomap_mapping_server_node" output="log" required="true">
+    <remap from="cloud_in" to="/camera/depth/points" />
+
+		<param name="multithreaded" value="$(arg multithreaded)" />
+
+    <param name="resolution" value="$(arg resolution)" />
+		<param name="depth_levels" value="$(arg depth_levels)" />
+  </node>
+</launch>

ufomap_mapping/launch/server_nodelet.launch

@@ -0,0 +1,21 @@
+<?xml version="1.0" ?>
+<launch>
+	<arg name="nodelet_manager" default="ufomap_mapping_server_nodelet_manager" />
+
+	<arg name="resolution" default="0.1" />
+	<arg name="depth_levels" default="16" />
+	<arg name="multithreaded" default="false" />
+
+	<node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen">
+		<param name="num_worker_threads" value="12" />
+	</node>
+
+	<node pkg="nodelet" type="nodelet" name="ufomap_mapping_server_nodelet" args="load ufomap_mapping/UFOMapServerNodelet $(arg nodelet_manager)" output="log" required="true">
+		<remap from="cloud_in" to="/camera/depth/points" />
+
+		<param name="multithreaded" value="$(arg multithreaded)" />
+
+    <param name="resolution" value="$(arg resolution)" />
+		<param name="depth_levels" value="$(arg depth_levels)" />
+	</node>
+</launch>

ufomap_mapping/nodelet_plugins.xml

@@ -0,0 +1,7 @@
+<library path="lib/libufomap_mapping_server_nodelet">
+  <class name="ufomap_mapping/UFOMapServerNodelet" type="ufomap_mapping::UFOMapServerNodelet" base_class_type="nodelet::Nodelet">
+    <description>
+      This is the UFOMap mapping server Nodelet.
+    </description>
+  </class>
+</library>

ufomap_mapping/package.xml

@@ -22,12 +22,13 @@
   <depend>std_srvs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
+  <depend>tf2_sensor_msgs</depend>
   <depend>ufomap</depend>
   <depend>ufomap_msgs</depend>
   <depend>ufomap_ros</depend>
 
 
   <export>
-
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
   </export>
 </package>