From 40d69fc114abf5bdb871771c4eca668c975945bf Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 11 Feb 2020 18:48:50 +0100
Subject: [PATCH] Added a nodelet version.
---
CMakeLists.txt | 10 ++--
.../iri_point_cloud_hole_detection_alg_node.h | 28 ++++++++++-
launch/nodelet.launch | 2 +-
package.xml | 4 +-
point_cloud_hole_detector_nodelet_plugin.xml | 7 +++
...ri_point_cloud_hole_detection_alg_node.cpp | 49 ++++++++++++++++++-
6 files changed, 92 insertions(+), 8 deletions(-)
create mode 100755 point_cloud_hole_detector_nodelet_plugin.xml
diff --git a/CMakeLists.txt b/CMakeLists.txt
index fd27ab0..461cd0f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,7 @@ project(iri_point_cloud_hole_detection)
# Add catkin additional components here
# ********************************************************************
# find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs pcl_ros roscpp iri_utils Eigen)
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs pcl_ros)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs pcl_ros nodelet)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -65,7 +65,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS iri_base_algorithm sensor_msgs pcl_ros
+ CATKIN_DEPENDS iri_base_algorithm sensor_msgs pcl_ros nodelet
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
@@ -88,12 +88,15 @@ include_directories(${iriutils_INCLUDE_DIR} ./include)
## Declare a cpp executable
add_executable(${PROJECT_NAME} src/iri_point_cloud_hole_detection_alg.cpp src/iri_point_cloud_hole_detection_alg_node.cpp)
+add_library(${PROJECT_NAME}_nodelet src/iri_point_cloud_hole_detection_alg.cpp src/iri_point_cloud_hole_detection_alg_node.cpp)
# ********************************************************************
# Add the libraries
# ********************************************************************
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
+target_link_libraries(${PROJECT_NAME}_nodelet ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}_nodelet ${iriutils_LIBRARY})
# ********************************************************************
# Add message headers dependencies
@@ -102,4 +105,5 @@ target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
# ********************************************************************
# Add dynamic reconfigure dependencies
# ********************************************************************
- add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS})
diff --git a/include/iri_point_cloud_hole_detection_alg_node.h b/include/iri_point_cloud_hole_detection_alg_node.h
index 3b0ac9f..8a6d57f 100644
--- a/include/iri_point_cloud_hole_detection_alg_node.h
+++ b/include/iri_point_cloud_hole_detection_alg_node.h
@@ -81,7 +81,7 @@ class PointCloudHoleDetectionAlgNode : public algorithm_base::IriBaseAlgorithm<P
* This constructor initializes specific class attributes and all ROS
* communications variables to enable message exchange.
*/
- PointCloudHoleDetectionAlgNode(void);
+ PointCloudHoleDetectionAlgNode(const ros::NodeHandle &nh=ros::NodeHandle("~"));
/**
* \brief Destructor
@@ -133,4 +133,30 @@ class PointCloudHoleDetectionAlgNode : public algorithm_base::IriBaseAlgorithm<P
// [test functions]
};
+#include "nodelet/nodelet.h"
+#include "threadserver.h"
+
+class PointCloudHoleDetectionAlgNodelet : public nodelet::Nodelet
+{
+ private:
+ PointCloudHoleDetectionAlgNode *node;
+ virtual void onInit();// initialization function
+ // thread attributes
+ CThreadServer *thread_server;
+ std::string spin_thread_id;
+ protected:
+ static void *spin_thread(void *param);
+ public:
+ PointCloudHoleDetectionAlgNodelet();
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~PointCloudHoleDetectionAlgNodelet();
+};
+
+
#endif
diff --git a/launch/nodelet.launch b/launch/nodelet.launch
index 8bc3fd6..5636ced 100644
--- a/launch/nodelet.launch
+++ b/launch/nodelet.launch
@@ -14,7 +14,7 @@
<node name="$(arg node_name)"
pkg="nodelet"
type="nodelet"
- args="load iri_point_cloud_hole_detection/AveragePointCloudAlgNodelet $(arg camera_nodelet_manager)"
+ args="load iri_point_cloud_hole_detection/PointCloudHoleDetectionAlgNodelet $(arg camera_nodelet_manager)"
output="$(arg output)">
<remap from="~input" to="$(arg cloud_in)"/>
<rosparam file="$(arg config_file)" command="load"/>
diff --git a/package.xml b/package.xml
index 7d1923b..b7bcb62 100644
--- a/package.xml
+++ b/package.xml
@@ -41,14 +41,16 @@
<build_depend>iri_base_algorithm</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>pcl_ros</build_depend>
+ <build_depend>nodelet</build_depend>
<run_depend>iri_base_algorithm</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>pcl_ros</run_depend>
-
+ <run_depend>nodelet</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
+ <nodelet plugin="${prefix}/point_cloud_hole_detector_nodelet_plugin.xml" />
</export>
</package>
diff --git a/point_cloud_hole_detector_nodelet_plugin.xml b/point_cloud_hole_detector_nodelet_plugin.xml
new file mode 100755
index 0000000..f71920c
--- /dev/null
+++ b/point_cloud_hole_detector_nodelet_plugin.xml
@@ -0,0 +1,7 @@
+<library path="lib/libiri_point_cloud_hole_detection_nodelet">
+ <class name="iri_point_cloud_hole_detection/PointCloudHoleDetectionAlgNodelet" type="PointCloudHoleDetectionAlgNodelet" base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelete for the IRI navigation Velodyne angle filter
+ </description>
+ </class>
+</library>
diff --git a/src/iri_point_cloud_hole_detection_alg_node.cpp b/src/iri_point_cloud_hole_detection_alg_node.cpp
index 0b5494a..9e06906 100644
--- a/src/iri_point_cloud_hole_detection_alg_node.cpp
+++ b/src/iri_point_cloud_hole_detection_alg_node.cpp
@@ -1,7 +1,7 @@
#include "iri_point_cloud_hole_detection_alg_node.h"
-PointCloudHoleDetectionAlgNode::PointCloudHoleDetectionAlgNode(void) :
- algorithm_base::IriBaseAlgorithm<PointCloudHoleDetectionAlgorithm>()
+PointCloudHoleDetectionAlgNode::PointCloudHoleDetectionAlgNode(const ros::NodeHandle &nh) :
+ algorithm_base::IriBaseAlgorithm<PointCloudHoleDetectionAlgorithm>(nh)
{
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
@@ -97,3 +97,48 @@ int main(int argc,char *argv[])
{
return algorithm_base::main<PointCloudHoleDetectionAlgNode>(argc, argv, "iri_point_cloud_hole_detection_alg_node");
}
+
+#include <pluginlib/class_list_macros.h>
+
+PointCloudHoleDetectionAlgNodelet::PointCloudHoleDetectionAlgNodelet()
+{
+ this->node=NULL;
+}
+
+PointCloudHoleDetectionAlgNodelet::~PointCloudHoleDetectionAlgNodelet(void)
+{
+ // kill the thread
+ this->thread_server->kill_thread(this->spin_thread_id);
+ this->thread_server->detach_thread(this->spin_thread_id);
+ this->thread_server->delete_thread(this->spin_thread_id);
+ this->spin_thread_id="";
+ // [free dynamic memory]
+ if(this->node!=NULL)
+ delete this->node;
+}
+
+void PointCloudHoleDetectionAlgNodelet::onInit()
+{
+ // initialize the driver node
+ this->node=new PointCloudHoleDetectionAlgNode(getPrivateNodeHandle());
+ // initialize the thread
+ this->thread_server=CThreadServer::instance();
+ this->spin_thread_id=getName() + "_point_cloud_hole_detector_spin";
+ this->thread_server->create_thread(this->spin_thread_id);
+ this->thread_server->attach_thread(this->spin_thread_id,this->spin_thread,this);
+ // start the spin thread
+ this->thread_server->start_thread(this->spin_thread_id);
+}
+
+void *PointCloudHoleDetectionAlgNodelet::spin_thread(void *param)
+{
+ PointCloudHoleDetectionAlgNodelet *nodelet=(PointCloudHoleDetectionAlgNodelet *)param;
+
+ nodelet->node->spin();
+
+ pthread_exit(NULL);
+}
+
+// parameters are: package, class name, class type, base class type
+PLUGINLIB_DECLARE_CLASS(iri_point_cloud_hole_detection, PointCloudHoleDetectionAlgNodelet, PointCloudHoleDetectionAlgNodelet, nodelet::Nodelet);
+
--
GitLab