Nodelet imgtransport feature (#30)

* uas arm modifications

* image_transport added in - seg faults

* image_transport added in - seg faults

* Removing shared pointer for image transport and removing gdb

* A couple little tweaks

* removing gdb

* image_transport added in - seg faults

* image_transport added in - seg faults

* Removing shared pointer for image transport and removing gdb

* A couple little tweaks

* support to change target grey value and dynamic reconfigure

* fixed imagetransport, changed from imagetransport:;publisher to imagetransport::camerapublisher and made caminfo to caminfoptr

* image_transport added in - seg faults

* image_transport added in - seg faults

* Removing shared pointer for image transport and removing gdb

* A couple little tweaks

* image_transport added in - seg faults

* image_transport added in - seg faults

* Removing shared pointer for image transport and removing gdb

* support to change target grey value and dynamic reconfigure

* changed dynamic reconfig levels

* added support for nodelets

* debugging nodelets and added a tester node to measure performance

* addind params

* added respawn

*  fix CMakeList for aarch64 flag

* cleaned acquisition_nodelet launch file and added nodelet_test laucnh file for the same

* changed Readme on instructions to use nodelets and changed launch files for nodelets and added example for multiple nodelets

* update Readme for typos

* will make changes as suggested after review in pl #30, changing names of launch files, changing dynamic recfg server namespace to fix ns issue for nodelets, nodelet manager starts by default in acquisition.launch

Author: mithundiddi

CMakeLists.txt

@@ -22,6 +22,8 @@ find_package(catkin REQUIRED COMPONENTS
   cv_bridge
   image_transport
   sensor_msgs
+  dynamic_reconfigure
+  nodelet
 )
 
 #find_package(PCL REQUIRED)
@@ -31,7 +33,15 @@ find_package(catkin REQUIRED COMPONENTS
 ###
 # Find Packages
 find_package(OpenCV REQUIRED)
-find_package(LibUnwind REQUIRED)
+# use LibUnwind only for x86_64 or x86_32 architecture
+# do not use LibUnwind for arm architecture
+if(${CMAKE_SYSTEM_PROCESSOR} MATCHES x86_64 OR x86_32)
+  message("uses LibUnwind for x86_64 or x86_32 architecture")
+  find_package(LibUnwind REQUIRED)
+endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES x86_64 OR x86_32)
+if(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm)
+  message("Detected ARM architecture")
+endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm)
 
 find_package(Boost REQUIRED)
 if(Boost_FOUND)
@@ -46,43 +56,83 @@ add_message_files(
   SpinnakerImageNames.msg
 )
 
-generate_messages(
-  DEPENDENCIES
-  std_msgs
-)
+generate_dynamic_reconfigure_options(
+  cfg/spinnaker_cam.cfg
 
-catkin_package(
-  INCLUDE_DIRS include
-  CATKIN_DEPENDS roscpp std_msgs message_runtime
-  DEPENDS OpenCV LibUnwind
 )
 
-include_directories(
-  ${PROJECT_INCLUDE_DIR}
-  ${catkin_INCLUDE_DIRS}
-  ${SPINNAKER_INCLUDE_DIR}
-  ${OpenCV_INCLUDE_DIRS}
-  ${Boost_INCLUDE_DIR}
-  ${LibUnwind_INCLUDE_DIRS}
+generate_messages(
+  DEPENDENCIES
+  std_msgs
 )
-
-link_directories( ${SPINNAKER_LIB_DIR} )
-
-set (LIBS ${LibUnwind_LIBRARIES} Spinnaker ${OpenCV_LIBS} ${Boost_GENERAL})
+## setting catkin_package, include_directories and libs based on architecture
+if(${CMAKE_SYSTEM_PROCESSOR} MATCHES x86_64 OR x86_32)
+    catkin_package(
+    INCLUDE_DIRS include
+    CATKIN_DEPENDS roscpp std_msgs message_runtime nodelet
+    DEPENDS OpenCV LibUnwind
+    )
+
+    include_directories(
+      ${PROJECT_INCLUDE_DIR}
+      ${catkin_INCLUDE_DIRS}
+      ${SPINNAKER_INCLUDE_DIR}
+      ${OpenCV_INCLUDE_DIRS}
+      ${Boost_INCLUDE_DIR}
+      ${LibUnwind_INCLUDE_DIRS}
+    )
+
+    link_directories( ${SPINNAKER_LIB_DIR} )
+
+    set (LIBS ${LibUnwind_LIBRARIES} Spinnaker ${OpenCV_LIBS} ${Boost_GENERAL})
+    
+endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES x86_64 OR x86_32)
+if(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm)
+    catkin_package(
+    INCLUDE_DIRS include
+    CATKIN_DEPENDS roscpp std_msgs message_runtime nodelet
+    DEPENDS OpenCV
+    )
+
+    include_directories(
+      ${PROJECT_INCLUDE_DIR}
+      ${catkin_INCLUDE_DIRS}
+      ${SPINNAKER_INCLUDE_DIR}
+      ${OpenCV_INCLUDE_DIRS}
+      ${Boost_INCLUDE_DIR}
+    )
+
+    link_directories( ${SPINNAKER_LIB_DIR} )
+
+    set (LIBS Spinnaker ${OpenCV_LIBS} ${Boost_GENERAL})
+    
+endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm)
 
 add_library (acquilib SHARED
   src/capture.cpp
   src/camera.cpp
 )
 
-add_dependencies(acquilib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+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})
+add_dependencies(acquisition_node acquilib ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
 target_link_libraries (acquisition_node acquilib ${LIBS} ${catkin_LIBRARIES})
 
-install(TARGETS acquilib acquisition_node
+
+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
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

README.md

@@ -1,4 +1,4 @@
-# spinnaker_camera_driver
+# spinnaker_sdk_camera_driver
 
 These are the ros drivers for running the Pt Grey (FLIR) cameras that use the Spinnaker SDK.  This code has been tested with various Point Grey Blackfly S (BFS) cameras. 
 
@@ -13,6 +13,8 @@ The pre-requisites for this repo include:
 * spinnaker (download from [Pt Grey's website](https://www.ptgrey.com/support/downloads))
 * ros-kinetic-cv-bridge
 * ros-kinetic-image-transport
+
+# Incase of x86_64 or x86_32 architecture, install the following:
 * libunwind-dev
 
 ```bash
@@ -21,6 +23,11 @@ The pre-requisites for this repo include:
 # after installing ros, install other pre-requisites with: 
 
 sudo apt install libunwind-dev ros-kinetic-cv-bridge ros-kinetic-image-transport
+
+# if you use arm64 (aarch64), install pre-requisites with: 
+
+sudo apt install ros-kinetic-cv-bridge ros-kinetic-image-transport
+
 ```
 
 ### Installing
@@ -39,7 +46,13 @@ source ~/spinnaker_ws/devel/setup.bash
 
 Modify the `params/test_params.yaml` file replacing the cam-ids and master cam serial number to match your camera's serial number. Then run the code as:
 ```bash
-roslaunch spinnaker_sdk_camera_driver acquisition.launch
+# To launch nodelet verison of driver, use #
+
+roslaunch spinnaker_camera_driver acquisition_nodelet.launch
+
+# To launch node version of driver, use 
+
+roslaunch spinnaker_camera_driver acquisition.launch
 # Test that the images are being published by running
 rqt_image_view
 ```
@@ -51,8 +64,8 @@ All the parameters can be set via the launch file or via the yaml config_file.
   Binning for cameras, when changing from 2 to 1 cameras need to be unplugged and replugged
 * ~color (bool, default: false)  
   Should color images be used (only works on models that support color images)
-* ~exp (int, default: 0)  
-  Exposure setting for cameras
+* ~exposure_time (int, default: 0, 0:auto)  
+  Exposure setting for cameras, also available as dynamic reconfiguarble parameter.
 * ~frames (int, default: 50)  
   Number of frames to save/view 0=ON
 * ~live (bool, default: false)  
@@ -74,7 +87,7 @@ All the parameters can be set via the launch file or via the yaml config_file.
 * ~utstamps (bool, default:false)  
   Flag whether each image should have Unique timestamps vs the master cams time stamp for all
 * ~max_rate_save (bool, default: false)  
-  Flag for max rate mode which is when the master triggers the slaves and saves images at maximum rate possible.  This is the multithreaded mode"
+  Flag for max rate mode which is when the master triggers the slaves and saves images at maximum rate possible.  This is the multithreaded mode
 
 ### System configuration parameters
 * ~cam_ids (yaml sequence or array)  
@@ -86,7 +99,27 @@ This is the names that would be given to the cameras for filenames and rostopics
 * ~skip (int)  
   Number of frames to be skipped initially to flush the buffer
 * ~delay (float)  
-  Secs to wait in the deinit/init sequence
+  Secs to wait in the deinit/init sequence.
+
+
+### Dynamic Reconfigure parameters
+* ~target_grey_val (double)
+  Target Grey Value is a parameter that helps to compensate for various lighting conditions by adjusting brightness to achieve optimal imaging results. The value is linear and is a percentage of the maximum pixel value.
+  Explained in detail at [FLIR webpage](https://www.flir.com/support-center/iis/machine-vision/application-note/using-auto-exposure-in-blackfly-s/).
+
+  Setting target_grey_val invokes setting AutoExposureTargetGreyValueAuto to 'off' and AutoExposureTargetGreyValue is set to target_grey_val.
+* ~exposure_time (int, default= 0, 0:auto)
+  Exposure time for the sensor.
+  When Exposure time is set within minimum and maximum exposure time limits(varies with camera model), ExposureAuto is set to 'off' and ExposureTime is set to exposure_time param value. 
+
+  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)
+  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).
 
 ### Camera info message details
 * ~image_width (int)
@@ -100,9 +133,11 @@ This is the names that would be given to the cameras for filenames and rostopics
   Specified as [fx  0 cx 0 fy cy 0  0  1]
 * ~projection_coeffs (array of arrays)  
   Projection coefficients of all the cameras in the array.  Must match the number of cam_ids provided.
+  For case of Monocular camera, if projection_coeffs is not set, intrinsic_coeff will be used to set, P[1:3,1:3]=K
 * ~rectification_coeffs (array of arrays)  
   Rectification coefficients of all the cameras in the array.  Must match the number of cam_ids provided.
 
+
 ## Multicamera Master-Slave Setup
 When using multiple cameras, we have found that the only way to keep images between different cameras synched is by using a master-slave setup using the GPIO connector. So this is the only way we support multicamera operation with this code. A general guide for multi camera setup is available at https://www.ptgrey.com/tan/11052, however note that we use a slightly different setup with our package.
 Refer to the `params/multi-cam_example.yaml` for an example on how to setup the configuration. You must specify a master_cam which must be one of the cameras in the cam_ids list. This master camera is the camera that is either explicitly software triggered by the code or triggered internally via a counter at a given frame rate. All the other cameras are triggered externally when the master camera triggers. In order to make this work, the wiring must be such that the external signal from the master camera **Line2** is connected to **Line3** on all slave cameras. To connect cameras in this way:

cfg/spinnaker_cam.cfg

@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+PACKAGE = "spinnaker_sdk_camera_driver"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+
+gen.add("target_grey_val", double_t, 1, "Set Target Grey Value", 50, 4, 90)
+gen.add("exposure_time", int_t, 2, "Set Exposure time (0:auto)", 0, 0, 15000)
+
+
+exit(gen.generate(PACKAGE, "spinnaker_sdk_camera_driver", "spinnaker_cam"))

include/spinnaker_sdk_camera_driver/camera.h

@@ -45,6 +45,7 @@ namespace acquisition {
         void setResolutionPixels(int width, int height);
         void setBufferSize(int numBuf);
         void adcBitDepth(gcstring bitDep);
+        void targetGreyValueTest();
 
         // void set_acquisition_mode_continuous();
         // void set_frame_rate(float);

include/spinnaker_sdk_camera_driver/capture.h

@@ -7,11 +7,17 @@
 
 #include <boost/archive/binary_oarchive.hpp>
 #include <boost/filesystem.hpp>
-
+//ROS
+#include "std_msgs/Float64.h"
 #include "std_msgs/String.h"
+//Dynamic reconfigure
+#include <dynamic_reconfigure/server.h>
+#include <spinnaker_sdk_camera_driver/spinnaker_camConfig.h>
+
 #include "spinnaker_sdk_camera_driver/SpinnakerImageNames.h"
 
 #include <sstream>
+#include <image_transport/image_transport.h>
 
 using namespace Spinnaker;
 using namespace Spinnaker::GenApi;
@@ -27,6 +33,7 @@ namespace acquisition {
 
         ~Capture();
         Capture();
+        Capture( ros::NodeHandle node,ros::NodeHandle private_nh);
 
         void load_cameras();
 
@@ -58,6 +65,7 @@ namespace acquisition {
         void get_mat_images();
         void update_grid();
         void export_to_ROS();
+        void dynamicReconfigureCallback(spinnaker_sdk_camera_driver::spinnaker_camConfig &config, uint32_t level);
 
         float mem_usage();
 
@@ -123,13 +131,18 @@ namespace acquisition {
         // ros variables
         ros::NodeHandle nh_;
         ros::NodeHandle nh_pvt_;
+        //image_transport::ImageTransport it_;
+        image_transport::ImageTransport it_;
+        dynamic_reconfigure::Server<spinnaker_sdk_camera_driver::spinnaker_camConfig>* dynamicReCfgServer_;
 
         ros::Publisher acquisition_pub;
-        vector<ros::Publisher> camera_image_pubs;
-        vector<ros::Publisher> camera_info_pubs;
+        //vector<ros::Publisher> camera_image_pubs;
+        vector<image_transport::CameraPublisher> camera_image_pubs;
+        //vector<ros::Publisher> camera_info_pubs;
+
 
         vector<sensor_msgs::ImagePtr> img_msgs;
-        vector<sensor_msgs::CameraInfo> cam_info_msgs;
+        vector<sensor_msgs::CameraInfoPtr> cam_info_msgs;
         spinnaker_sdk_camera_driver::SpinnakerImageNames mesg;
         boost::mutex queue_mutex_;  
     };

include/spinnaker_sdk_camera_driver/capture_nodelet.h

@@ -0,0 +1,32 @@
+//
+// Created by pushyami on 1/10/19.
+//
+
+#ifndef SPINNAKER_SDK_CAMERA_DRIVER_CAPTURE_NODELET_H
+#define SPINNAKER_SDK_CAMERA_DRIVER_CAPTURE_NODELET_H
+
+#endif //SPINNAKER_SDK_CAMERA_DRIVER_CAPTURE_NODELET_H
+
+#include <nodelet/nodelet.h>
+#include "capture.h"
+namespace acquisition {
+    class capture_nodelet: public nodelet::Nodelet
+    {
+
+    public:
+        capture_nodelet(){}
+        ~capture_nodelet(){
+            if (pubThread_) {
+                pubThread_->interrupt();
+                pubThread_->join();
+            }
+        }
+        virtual void onInit();
+
+        boost::shared_ptr<Capture> inst_;
+        std::shared_ptr<boost::thread> pubThread_;
+
+    };
+
+}
+

include/spinnaker_sdk_camera_driver/std_include.h

@@ -15,6 +15,9 @@
 #include <cv_bridge/cv_bridge.h>
 #include "sensor_msgs/Image.h"
 #include "sensor_msgs/CameraInfo.h"
+#include "std_msgs/String.h"
+#include <image_transport/image_transport.h>
+#include "sensor_msgs/Image.h"
 
 
 // Standard Libs

include/spinnaker_sdk_camera_driver/subscriber_nodelet.h

@@ -0,0 +1,26 @@
+//
+// Created by pushyami on 1/15/19.
+//
+
+#ifndef SPINNAKER_SDK_CAMERA_DRIVER_SUBSCRIBER_NODELET_H
+#define SPINNAKER_SDK_CAMERA_DRIVER_SUBSCRIBER_NODELET_H
+
+#endif //SPINNAKER_SDK_CAMERA_DRIVER_SUBSCRIBER_NODELET_H
+#include "spinnaker_sdk_camera_driver/std_include.h"
+#include <nodelet/nodelet.h>
+#include "capture.h"
+namespace acquisition {
+    class subscriber_nodelet: public nodelet::Nodelet
+    {
+
+    public:
+        subscriber_nodelet(){}
+        ~subscriber_nodelet(){}
+        virtual void onInit();
+        void chatterCallback(const sensor_msgs::Image::ConstPtr& msg);
+        std::shared_ptr<image_transport::ImageTransport> it_;
+        image_transport::Subscriber image_sub_;
+
+    };
+
+}