Add nodelet

CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   std_msgs
   polled_camera
+  nodelet
 )
 
 #Get architecture
@@ -113,12 +114,20 @@ add_executable(sync_node
 
 add_dependencies_and_linkings(sync_node)
 
+add_library(avt_camera_nodelets
+  src/nodes/mono_camera_nodelet.cpp
+  src/nodes/stereo_camera_nodelet.cpp
+  src/stereo_camera.cpp
+  src/avt_vimba_camera.cpp
+  src/frame_observer.cpp)
+add_dependencies_and_linkings(avt_camera_nodelets)
+
 #############
 ## Install ##
 #############
 
 ## Mark executables and/or libraries for installation
-install(TARGETS mono_camera_node stereo_camera_node
+install(TARGETS mono_camera_node stereo_camera_node avt_camera_nodelets
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -133,7 +142,9 @@ install(DIRECTORY include
 
 ## Mark other files for installation (e.g. launch and bag files, etc.)
 install(FILES
+  plugins.xml
   launch/mono_camera.launch
+  launch/mono_camera_nodelet.launch
   launch/stereo_camera_one_node.launch
   launch/stereo_camera_two_nodes.launch
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
@@ -164,4 +175,3 @@ elseif("${ARCH}" STREQUAL armv8)
     DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   )
 endif()
-

include/avt_vimba_camera/mono_camera.h

@@ -49,7 +49,7 @@
 namespace avt_vimba_camera {
 class MonoCamera {
  public:
-  MonoCamera(ros::NodeHandle nh, ros::NodeHandle nhp);
+  MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp);
   ~MonoCamera(void);
 
  private:

include/avt_vimba_camera/mono_camera_nodelet.h

@@ -0,0 +1,16 @@
+#include <nodelet/nodelet.h>
+#include "avt_vimba_camera/mono_camera.h"
+
+namespace avt_vimba_camera
+{
+
+    class MonoCameraNodelet : public nodelet::Nodelet
+    {
+        public:
+            virtual void onInit();
+            virtual ~MonoCameraNodelet();
+        private:
+            MonoCamera* camera_;
+    };
+
+}

include/avt_vimba_camera/stereo_camera_nodelet.h

@@ -0,0 +1,16 @@
+#include <nodelet/nodelet.h>
+#include "avt_vimba_camera/stereo_camera.h"
+
+namespace avt_vimba_camera
+{
+
+    class StereoCameraNodelet : public nodelet::Nodelet
+    {
+        public:
+            virtual void onInit();
+            virtual ~StereoCameraNodelet();
+        private:
+            StereoCamera* camera_;
+    };
+
+}

launch/mono_camera_nodelet.launch

@@ -0,0 +1,5 @@
+<launch>
+  <node pkg="nodelet" type="nodelet" name="avt_vimba_nodelet_manager"  args="manager" output="screen"/>
+
+  <node pkg="nodelet" type="nodelet" name="mono_camera" args="load avt_vimba_camera/MonoCameraNodelet avt_vimba_nodelet_manager" output="screen"/>
+</launch>

package.xml

@@ -24,6 +24,7 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>polled_camera</build_depend>
+  <build_depend>nodelet</build_depend>
   <!--build_depend>libvimba</build_depend-->
   <run_depend>camera_info_manager</run_depend>
   <run_depend>diagnostic_updater</run_depend>
@@ -35,6 +36,11 @@
   <run_depend>sensor_msgs</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>polled_camera</run_depend>
+  <run_depend>nodelet</run_depend>
+
   <!--run_depend>libvimba</run_depend-->
+  <export>
+    <nodelet plugin="${prefix}/plugins.xml" />
+  </export>
 
 </package>

plugins.xml

@@ -0,0 +1,12 @@
+<library path="lib/libavt_camera_nodelets">
+  <class name="avt_vimba_camera/MonoCameraNodelet" type="avt_vimba_camera::MonoCameraNodelet" base_class_type="nodelet::Nodelet">
+    <description>
+      MonoCamera Nodelet
+    </description>
+  </class>
+  <class name="avt_vimba_camera/StereoCameraNodelet" type="avt_vimba_camera::StereoCameraNodelet" base_class_type="nodelet::Nodelet">
+    <description>
+      StereoCamera Nodelet
+    </description>
+  </class>
+</library>

src/mono_camera.cpp

@@ -36,7 +36,7 @@
 
 namespace avt_vimba_camera {
 
-MonoCamera::MonoCamera(ros::NodeHandle nh, ros::NodeHandle nhp) : nh_(nh), nhp_(nhp), it_(nhp), cam_(ros::this_node::getName()) {
+MonoCamera::MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp) : nh_(nh), nhp_(nhp), it_(nhp), cam_(ros::this_node::getName()) {
   // Prepare node handle for the camera
   // TODO use nodelets with getMTNodeHandle()
 
@@ -127,7 +127,7 @@ void MonoCamera::updateCameraInfo(const avt_vimba_camera::AvtVimbaCameraConfig&
   int binning_or_decimation_y = std::max(config.binning_y, config.decimation_y);
 
   // Set the operational parameters in CameraInfo (binning, ROI)
-  ci.height    = config.height; 
+  ci.height    = config.height;
   ci.width     = config.width;
   ci.binning_x = binning_or_decimation_x;
   ci.binning_y = binning_or_decimation_y;

src/nodes/mono_camera_nodelet.cpp

@@ -0,0 +1,18 @@
+#include <pluginlib/class_list_macros.h>
+#include "avt_vimba_camera/mono_camera_nodelet.h"
+
+namespace avt_vimba_camera
+{
+    void MonoCameraNodelet::onInit()
+    {
+        NODELET_DEBUG("Initializing nodelet...");
+				camera_ = new MonoCamera(getMTNodeHandle(), getMTPrivateNodeHandle());
+    }
+
+    MonoCameraNodelet::~MonoCameraNodelet()
+    {
+      	delete camera_;
+    }
+}
+
+PLUGINLIB_EXPORT_CLASS(avt_vimba_camera::MonoCameraNodelet, nodelet::Nodelet)

src/nodes/stereo_camera_nodelet.cpp

@@ -0,0 +1,20 @@
+#include <pluginlib/class_list_macros.h>
+#include "avt_vimba_camera/stereo_camera_nodelet.h"
+#include <boost/thread.hpp>
+
+namespace avt_vimba_camera
+{
+    void StereoCameraNodelet::onInit()
+    {
+        NODELET_DEBUG("Initializing nodelet...");
+				camera_ = new StereoCamera(getMTNodeHandle(), getMTPrivateNodeHandle());
+        boost::thread stereoThread(&avt_vimba_camera::StereoCamera::run, camera_);
+    }
+
+    StereoCameraNodelet::~StereoCameraNodelet()
+    {
+        delete camera_;
+    }
+}
+
+PLUGINLIB_EXPORT_CLASS(avt_vimba_camera::StereoCameraNodelet, nodelet::Nodelet)