Added Nodelet Support

CMakeLists.txt

@@ -16,6 +16,7 @@ set(
      roslaunch
      sensor_msgs
      #std_srvs
+     nodelet
 )
 
 find_package(Pylon QUIET)
@@ -49,6 +50,7 @@ roslint_cpp(
     src/${PROJECT_NAME}/encoding_conversions.cpp
     src/${PROJECT_NAME}/main.cpp
     src/${PROJECT_NAME}/pylon_camera_node.cpp
+    src/${PROJECT_NAME}/pylon_camera_nodelet.cpp
     src/${PROJECT_NAME}/pylon_camera_parameter.cpp
     src/${PROJECT_NAME}/pylon_camera.cpp
     src/${PROJECT_NAME}/write_device_user_id_to_camera.cpp
@@ -79,6 +81,7 @@ add_library(
      src/${PROJECT_NAME}/encoding_conversions.cpp
      src/${PROJECT_NAME}/pylon_camera.cpp
      src/${PROJECT_NAME}/pylon_camera_node.cpp
+     src/${PROJECT_NAME}/pylon_camera_nodelet.cpp
      src/${PROJECT_NAME}/pylon_camera_parameter.cpp
 )
 
@@ -170,6 +173,13 @@ install(
     PATTERN "internal" EXCLUDE
 )
 
+install(
+    FILES
+      nodelet_plugins.xml
+    DESTINATION
+      ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
 ## Testing ##
 # All Jenkins-Tests are now in the pylon_camera_tests-pkg
 ############

README.rst

@@ -26,6 +26,8 @@ This means that the image acquisition is triggered with a certain rate and the c
 
 The package opens either a predefined camera (using a given 'device_user_id' parameter) or, if no camera id is predefined the first camera device it can find.
 
+The package supports running as nodelet.
+
 |
 
 ******
@@ -127,6 +129,14 @@ Images were only published if another node connects to the image topic. The publ
 
 ``rosrun image_view image_view image:=/pylon_camera_node/image_raw``
 
+The pylon_camera package also supports running as nodelet. To run as nodelet, use following launch file
+
+``roslaunch pylon_camera pylon_camera_nodelet.launch``
+
+The image topic can be viewed using image_view using
+
+``rosrun image_view image_view image:=/pylon_camera_nodelet/image_raw``
+
 ******
 **Questions**
 ******

include/pylon_camera/pylon_camera_node.h

@@ -64,7 +64,7 @@ typedef actionlib::SimpleActionServer<camera_control_msgs::GrabImagesAction> Gra
 class PylonCameraNode
 {
 public:
-    PylonCameraNode();
+    PylonCameraNode(ros::NodeHandle &nh_private, ros::NodeHandle &nh_image);
     virtual ~PylonCameraNode();
 
     /**
@@ -74,9 +74,9 @@ class PylonCameraNode
     void init();
 
     /**
-     * spin the node
+     * spin the node (with timer callback)
      */
-    virtual void spin();
+    virtual void spinCallback(const ros::TimerEvent& event);
 
     /**
      * Getter for the frame rate set by the launch script or from the ros parameter
@@ -317,7 +317,10 @@ class PylonCameraNode
      */
     bool waitForCamera(const ros::Duration& timeout) const;
 
-    ros::NodeHandle nh_;
+    ros::NodeHandle* nh_private_;
+    ros::NodeHandle* nh_image_;
+    ros::Timer grab_image_timer_;
+
     PylonCameraParameter pylon_camera_parameter_set_;
     ros::ServiceServer set_binning_srv_;
     ros::ServiceServer set_exposure_srv_;

launch/pylon_camera_nodelet.launch

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<launch>
+
+    <arg name="node_name" default="pylon_camera_node" />
+    <arg name="config_file" default="$(find pylon_camera)/config/default.yaml" />
+
+    <node pkg="nodelet" type="nodelet" name="pylon_camera_manager" args="manager" output="screen" />
+    <node pkg="nodelet" type="nodelet" name="pylon_camera_nodelet" args="load pylon_camera/PylonCameraNodelet pylon_camera_manager" output="screen">
+        <param name="camera_frame" value="$(arg node_name)" type="string" />
+        <rosparam command="load" file="$(arg config_file)" />
+    </node>
+
+</launch>

nodelet_plugins.xml

@@ -0,0 +1,9 @@
+<library path="lib/libpylon_camera">
+  <class name="pylon_camera/PylonCameraNodelet"
+         type="pylon_camera::PylonCameraNodelet"
+         base_class_type="nodelet::Nodelet">
+    <description>
+      description
+    </description>
+  </class>
+</library>

package.xml

@@ -35,6 +35,7 @@
   <build_depend>roslaunch</build_depend>
   <!--build_depend>std_srvs</build_depend-->
   <build_depend>roslint</build_depend>
+  <build_depend>nodelet</build_depend>
 
   <run_depend>actionlib</run_depend>
   <run_depend version_gte="0.0.14" version_lt="0.0.15">camera_control_msgs</run_depend>
@@ -47,5 +48,10 @@
   <run_depend>roslaunch</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <!--run_depend>std_srvs</run_depend-->
+  <run_depend>nodelet</run_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+  </export>
 
 </package>

src/pylon_camera/main.cpp

@@ -40,22 +40,15 @@
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "pylon_camera_node");
+    ros::NodeHandle nh_private("~");
+    ros::NodeHandle nh_image;
 
-    pylon_camera::PylonCameraNode pylon_camera_node;
-
-    ros::Rate r(pylon_camera_node.frameRate());
+    pylon_camera::PylonCameraNode pylon_camera_node(nh_private, nh_image);
 
     ROS_INFO_STREAM("Start image grabbing if node connects to topic with "
         << "a frame_rate of: " << pylon_camera_node.frameRate() << " Hz");
 
-    // Main thread and brightness-service thread
-    boost::thread th(boost::bind(&ros::spin));
-
-    while ( ros::ok() )
-    {
-        pylon_camera_node.spin();
-        r.sleep();
-    }
+    ros::spin();
 
     ROS_INFO("Terminate PylonCameraNode");
     return EXIT_SUCCESS;

src/pylon_camera/pylon_camera_node.cpp

@@ -40,34 +40,33 @@ namespace pylon_camera
 using sensor_msgs::CameraInfo;
 using sensor_msgs::CameraInfoPtr;
 
-PylonCameraNode::PylonCameraNode()
-    : nh_("~"),
-      pylon_camera_parameter_set_(),
-      set_binning_srv_(nh_.advertiseService("set_binning",
+PylonCameraNode::PylonCameraNode(ros::NodeHandle &nh_private, ros::NodeHandle &nh_image)
+    : pylon_camera_parameter_set_(),
+      set_binning_srv_(nh_private.advertiseService("set_binning",
                                             &PylonCameraNode::setBinningCallback,
                                             this)),
-      set_exposure_srv_(nh_.advertiseService("set_exposure",
+      set_exposure_srv_(nh_private.advertiseService("set_exposure",
                                              &PylonCameraNode::setExposureCallback,
                                              this)),
-      set_gain_srv_(nh_.advertiseService("set_gain",
+      set_gain_srv_(nh_private.advertiseService("set_gain",
                                          &PylonCameraNode::setGainCallback,
                                          this)),
-      set_gamma_srv_(nh_.advertiseService("set_gamma",
+      set_gamma_srv_(nh_private.advertiseService("set_gamma",
                                           &PylonCameraNode::setGammaCallback,
                                           this)),
-      set_brightness_srv_(nh_.advertiseService("set_brightness",
+      set_brightness_srv_(nh_private.advertiseService("set_brightness",
                                                &PylonCameraNode::setBrightnessCallback,
                                                this)),
-      set_sleeping_srv_(nh_.advertiseService("set_sleeping",
+      set_sleeping_srv_(nh_private.advertiseService("set_sleeping",
                                              &PylonCameraNode::setSleepingCallback,
                                              this)),
       set_user_output_srvs_(),
       pylon_camera_(nullptr),
-      it_(new image_transport::ImageTransport(nh_)),
-      img_raw_pub_(it_->advertiseCamera("image_raw", 1)),
+      it_(new image_transport::ImageTransport(nh_image)),
+      img_raw_pub_(it_->advertiseCamera(nh_private.getNamespace() + "/image_raw", 1)),
       img_rect_pub_(nullptr),
       grab_imgs_raw_as_(
-              nh_,
+              nh_image,
               "grab_images_raw",
               boost::bind(&PylonCameraNode::grabImagesRawActionExecuteCB,
                           this,
@@ -76,11 +75,14 @@ PylonCameraNode::PylonCameraNode()
       grab_imgs_rect_as_(nullptr),
       pinhole_model_(nullptr),
       cv_bridge_img_rect_(nullptr),
-      camera_info_manager_(new camera_info_manager::CameraInfoManager(nh_)),
+      camera_info_manager_(new camera_info_manager::CameraInfoManager(nh_image)),
       sampling_indices_(),
       brightness_exp_lut_(),
       is_sleeping_(false)
 {
+    // Pointer to node handler
+    nh_private_ = &nh_private;
+    nh_image_ = &nh_image;
     init();
 }
 
@@ -91,7 +93,7 @@ void PylonCameraNode::init()
     // detected, the interface will reset them to the default values.
     // These parameters furthermore contain the intrinsic calibration matrices,
     // in case they are provided
-    pylon_camera_parameter_set_.readFromRosParameterServer(nh_);
+    pylon_camera_parameter_set_.readFromRosParameterServer(*nh_private_);
 
     // creating the target PylonCamera-Object with the specified
     // device_user_id, registering the Software-Trigger-Mode, starting the
@@ -108,6 +110,8 @@ void PylonCameraNode::init()
         ros::shutdown();
         return;
     }
+    // setup timer
+    grab_image_timer_ = nh_image_->createTimer(ros::Duration(1/frameRate()), &PylonCameraNode::spinCallback, this);
 }
 
 bool PylonCameraNode::initAndRegister()
@@ -174,7 +178,7 @@ bool PylonCameraNode::startGrabbing()
     for ( int i = 0; i < set_user_output_srvs_.size(); ++i )
     {
         std::string srv_name = "set_user_output_" + std::to_string(i);
-        set_user_output_srvs_.at(i) = nh_.advertiseService< camera_control_msgs::SetBool::Request,
+        set_user_output_srvs_.at(i) = nh_private_->advertiseService< camera_control_msgs::SetBool::Request,
                                                             camera_control_msgs::SetBool::Response >(
                                             srv_name,
                                             boost::bind(&PylonCameraNode::setUserOutputCB,
@@ -224,7 +228,7 @@ bool PylonCameraNode::startGrabbing()
          !camera_info_manager_->validateURL(pylon_camera_parameter_set_.cameraInfoURL()) )
     {
         ROS_INFO_STREAM("CameraInfoURL needed for rectification! ROS-Param: "
-            << "'" << nh_.getNamespace() << "/camera_info_url' = '"
+            << "'" << nh_private_->getNamespace() << "/camera_info_url' = '"
             << pylon_camera_parameter_set_.cameraInfoURL() << "' is invalid!");
         ROS_DEBUG_STREAM("CameraInfoURL should have following style: "
             << "'file:///full/path/to/local/file.yaml' or "
@@ -340,12 +344,12 @@ bool PylonCameraNode::startGrabbing()
                  pylon_camera_parameter_set_.frameRate(),
                  pylon_camera_->maxPossibleFramerate());
         pylon_camera_parameter_set_.setFrameRate(
-                nh_,
+                *nh_private_,
                 pylon_camera_->maxPossibleFramerate());
     }
     else if ( pylon_camera_parameter_set_.frameRate() == -1 )
     {
-        pylon_camera_parameter_set_.setFrameRate(nh_,
+        pylon_camera_parameter_set_.setFrameRate(*nh_private_,
                                                  pylon_camera_->maxPossibleFramerate());
         ROS_INFO("Max possible framerate is %.2f Hz",
                  pylon_camera_->maxPossibleFramerate());
@@ -356,10 +360,10 @@ bool PylonCameraNode::startGrabbing()
 void PylonCameraNode::setupRectification()
 {
     img_rect_pub_ =
-        new ros::Publisher(nh_.advertise<sensor_msgs::Image>("image_rect", 1));
+        new ros::Publisher(nh_image_->advertise<sensor_msgs::Image>(nh_private_->getNamespace() + "/image_rect", 1));
 
     grab_imgs_rect_as_ =
-        new GrabImagesAS(nh_,
+        new GrabImagesAS(*nh_image_,
                          "grab_images_rect",
                          boost::bind(
                             &PylonCameraNode::grabImagesRectActionExecuteCB,
@@ -376,7 +380,7 @@ void PylonCameraNode::setupRectification()
     cv_bridge_img_rect_->encoding = img_raw_msg_.encoding;
 }
 
-void PylonCameraNode::spin()
+void PylonCameraNode::spinCallback(const ros::TimerEvent& event)
 {
     if ( camera_info_manager_->isCalibrated() )
     {

src/pylon_camera/pylon_camera_nodelet.cpp

@@ -0,0 +1,30 @@
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include <pylon_camera/pylon_camera_node.h>
+
+namespace pylon_camera
+{
+
+class PylonCameraNodelet : public nodelet::Nodelet
+{
+public:
+    PylonCameraNodelet()
+    {
+    }
+    ~PylonCameraNodelet()
+    {
+    }
+
+    void onInit(void)
+    {
+        node_.reset(new pylon_camera::PylonCameraNode(getPrivateNodeHandle(), getMTNodeHandle()));
+    }
+
+private:
+    boost::shared_ptr<pylon_camera::PylonCameraNode> node_;
+};
+
+} // namespace pylon_camera
+
+PLUGINLIB_DECLARE_CLASS(pylon_camera, PylonCameraNodelet, pylon_camera::PylonCameraNodelet, nodelet::Nodelet);