Feature/cleanup nodelets (#55)

* wip cleaned constructor, nodelet class

* updated Readme, changed names of launch files for ease of access and name of manager to standard name manager, removed nodelet exmaples and utils

* added example subscriber nodelet as requested in pr #55

Author: mithundiddi

CMakeLists.txt

@@ -112,27 +112,20 @@ add_library (acquilib SHARED
   src/capture.cpp
   src/camera.cpp
 )
-
 add_dependencies(acquilib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
 target_link_libraries(acquilib ${LIBS} ${catkin_LIBRARIES})
 
-add_library(capture_nodelet src/capture_nodelet.cpp src/subscriber_nodelet.cpp)
-target_link_libraries(capture_nodelet acquilib ${catkin_LIBRARIES})
-
 add_executable (acquisition_node src/acquisition_node.cpp)
 add_dependencies(acquisition_node acquilib ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
 target_link_libraries (acquisition_node acquilib ${LIBS} ${catkin_LIBRARIES})
 
+## subscriber_example for subscribing as nodelet
+add_library (subscriber_example examples/subscriber_nodelet.cpp)
+add_dependencies(subscriber_example ${catkin_EXPORTED_TARGETS})
+target_link_libraries(subscriber_example ${catkin_LIBRARIES})
 
-add_executable (acquisition_nodelet src/acquisition_nodelet.cpp)
-add_dependencies(acquisition_nodelet acquilib capture_nodelet ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
-target_link_libraries (acquisition_nodelet acquilib capture_nodelet ${LIBS} ${catkin_LIBRARIES})
-
-add_executable (nodelet_test src/nodelet_test.cpp)
-add_dependencies(nodelet_test ${catkin_EXPORTED_TARGETS})
-target_link_libraries (nodelet_test capture_nodelet ${LIBS} ${catkin_LIBRARIES})
 
-install(TARGETS acquilib acquisition_node capture_nodelet acquisition_nodelet nodelet_test
+install(TARGETS acquilib acquisition_node subscriber_example
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -142,3 +135,7 @@ install(DIRECTORY include
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
   FILES_MATCHING PATTERN "*.h"
 )
+
+install(FILES nodelet_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

README.md

@@ -47,11 +47,11 @@ Modify the `params/test_params.yaml` file replacing the cam-ids and master cam s
 ```bash
 # To launch nodelet verison of driver, use #
 
-roslaunch spinnaker_camera_driver acquisition_nodelet.launch
+roslaunch spinnaker_camera_driver acquisition.launch
 
 # To launch node version of driver, use 
 
-roslaunch spinnaker_camera_driver acquisition.launch
+roslaunch spinnaker_camera_driver node_acquisition.launch
 # Test that the images are being published by running
 rqt_image_view
 ```
@@ -130,11 +130,11 @@ This is the names that would be given to the cameras for filenames and rostopics
   When exposure_time is set to 0(zero), the ExposureAuto is set to 'Continuous', enabling auto exposure.
 
 ### nodelet details
-* ~nodelet_manager_name (string, default: vision_nodelet_manager)
+* ~manager (string, default: vision_nodelet_manager)
   Specify the name of the nodelet_manager under which nodelet to be launched.
-* ~start_nodelet_manager (bool, default: false)
-  If set True, nodelet_manager of name $(arg nodelet_manager_name) will be launched.
-  If set False(default), the acquisition/capture_nodelet waits for the nodelet_manager name $(arg nodelet_manager_name).
+* ~external_manager (bool, default: false)
+  If set False(default), nodelet_manager of name $(arg manager) will be launched.
+  If set True, the acquisition/Capture waits for the nodelet_manager name $(arg manager).
 
 ### Camera info message details
 * ~image_width (int)

examples/subscriber_nodelet.cpp

@@ -0,0 +1,57 @@
+//
+// Created by pushyami on 1/15/19.
+//
+//cpp
+#include <iostream>
+// ROS
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+//#include <cv_bridge/cv_bridge.h>
+// msgs
+#include "sensor_msgs/Image.h"
+//#include "sensor_msgs/CameraInfo.h"
+
+// nodelets
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+
+
+
+namespace subscriber_nodelet_ns
+{
+    class subscriber_nodelet: public nodelet::Nodelet
+    {
+    //This is a test nodelet for  measuring nodelet performance
+        public:
+            subscriber_nodelet(){}
+            ~subscriber_nodelet()
+            {
+                it_.reset();
+            }
+
+            std::shared_ptr<image_transport::ImageTransport> it_;
+            image_transport::Subscriber image_sub_;
+            
+            virtual void onInit()
+            {
+                NODELET_INFO("Initializing Subscriber nodelet");
+                ros::NodeHandle& node = getNodeHandle();
+                ros::NodeHandle& private_nh = getPrivateNodeHandle();
+
+                it_.reset(new image_transport::ImageTransport(node));
+                image_sub_ = it_->subscribe("/camera_array/cam0/image_raw",1, &subscriber_nodelet::imgCallback, this);
+                NODELET_INFO("onInit for Subscriber nodelet Initialized");
+            }
+
+            void imgCallback(const sensor_msgs::Image::ConstPtr& msg)
+            {
+                // dont modify the input msg in any way
+                sensor_msgs::Image tmp_img = *msg;
+                NODELET_INFO_STREAM("diff time is "<< ros::Time::now().toSec() - tmp_img.header.stamp.toSec());
+                // copy input img to cv::mat and do any cv stuff
+                // dont do time intense stuff in callbacks
+            }
+    };
+}
+
+PLUGINLIB_EXPORT_CLASS(subscriber_nodelet_ns::subscriber_nodelet, nodelet::Nodelet)

include/spinnaker_sdk_camera_driver/capture.h

@@ -18,6 +18,10 @@
 
 #include <sstream>
 #include <image_transport/image_transport.h>
+// nodelets
+#include <nodelet/nodelet.h>
+#include <nodelet/loader.h>
+#include "pluginlib/class_list_macros.h"
 
 using namespace Spinnaker;
 using namespace Spinnaker::GenApi;
@@ -27,16 +31,18 @@ using namespace std;
 
 namespace acquisition {
 
-    class Capture {
+    class Capture : public nodelet::Nodelet {
 
     public:
 
         ~Capture();
         Capture();
-        Capture( ros::NodeHandle node,ros::NodeHandle private_nh);
+        virtual void onInit();
+
+        std::shared_ptr<boost::thread> pubThread_;
 
         void load_cameras();
-        
+        void init_variables_register_to_ros();
         void init_array();
         void init_cameras(bool);
         void start_acquisition();
@@ -146,8 +152,8 @@ namespace acquisition {
         // ros variables
         ros::NodeHandle nh_;
         ros::NodeHandle nh_pvt_;
-        //image_transport::ImageTransport it_;
-        image_transport::ImageTransport it_;
+        std::shared_ptr<image_transport::ImageTransport> it_;
+
         dynamic_reconfigure::Server<spinnaker_sdk_camera_driver::spinnaker_camConfig>* dynamicReCfgServer_;
 
         ros::Publisher acquisition_pub;

include/spinnaker_sdk_camera_driver/capture_nodelet.h

@@ -23,20 +23,20 @@
 
 
   <!-- nodelet manager params-->
-  <arg name="nodelet_manager_name" default="vision_nodelet_manager" doc="name of the nodelet manager, comes handy when launching multiple nodelets from different launch files" />
-  <arg name="start_nodelet_manager" default="true" doc="If set to True(default), creates a nodelet manager with $(arg nodelet_manager_name).
-  If False, the acquisition/capture_nodelet waits for the nodelet_manager name $(arg nodelet_manager_name)" />
+  <arg name="manager" default="vision_nodelet_manager" doc="name of the nodelet manager, comes handy when launching multiple nodelets from different launch files" />
+  <arg name="external_manager" default="false" doc="If set to False(default), creates a nodelet manager with $(arg manager).
+  If True, the acquisition/Capture waits for the nodelet_manager name $(arg manager)" />
 
   <!-- Capture nodelet params-->
   <arg name="tf_prefix" default="" doc="will prefix (arg tf_prefix)/ to existing frame_id in Image msg and camerainfo msg" />
   <arg name="config_file"       default="$(find spinnaker_sdk_camera_driver)/params/test_params.yaml" doc="File specifying the parameters of the camera_array"/>
 
    <!-- start the nodelet manager if $(arg start_nodelet_manager) is true-->
-  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager" output="screen" if="$(arg start_nodelet_manager)" />
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen" unless="$(arg external_manager)" />
 
   <!-- load the acquisition nodelet -->
   <node pkg="nodelet" type="nodelet" name="acquisition_node"
-          args="load acquisition/capture_nodelet $(arg nodelet_manager_name)" >
+          args="load acquisition/Capture $(arg manager)" >
     <!-- load the acquisition node parameters file.  Note any parameters provided in this file will 
 	 override what is in the yaml file.  Thus use it to set parameters camer_array configuration params -->
     <rosparam command="load"        file="$(arg config_file)" />

include/spinnaker_sdk_camera_driver/subscriber_nodelet.h

@@ -1,7 +1,7 @@
 <launch>    
     <!-- args namespacing  -->
-    <arg name="robot_ns"                    default="$(env ROBOT_NAME)" />
-    <arg name="vision_nm" default="vision_nodelet_manager_uas" doc="name of the nodelet manager" />
+    <arg name="robot_ns"              default="$(optenv ROBOT_NAME uas2)" />
+    <arg name="vision_nm" default="vision_nodelet_manager_ext" doc="name of the nodelet manager" />
     <arg name="camera_tf_prefix" default="" />
 
 
@@ -10,14 +10,18 @@
         <node pkg="nodelet" type="nodelet" name="$(arg vision_nm)" args="manager" output="screen"/>
         <!-- launch camera nodelet -->
         <include file="$(find spinnaker_sdk_camera_driver)/launch/acquisition.launch" >
-            <arg name="nodelet_manager_name" value="$(arg vision_nm)" />
-            <arg name="start_nodelet_manager" value="false" />
+            <arg name="manager" value="$(arg vision_nm)" />
+            <arg name="external_manager" value="true" />
             <arg name="tf_prefix" value="$(arg camera_tf_prefix)" />
         </include>
 
         <!-- launch apriltag_ros nodelet -->
         <!-- include file="$(find apriltag_ros)/launch/continuous_detection_nodelet.launch" >
             <arg name="nodelet_manager_name" value="$(arg vision_nm)" />
         </include -->
-    </group>
+
+        <!-- launch example_subscriber_nodelet -->
+        <node pkg="nodelet" type="nodelet" name="subscriber_nodelet"
+        args="load subscriber_nodelet_ns/subscriber_nodelet $(arg vision_nm)" />
+    </group >
 </launch>