Camera Pipeline Optimization

avt_vimba_camera/CMakeLists.txt

@@ -11,6 +11,7 @@ endif()
 
 find_package(ament_cmake_auto REQUIRED)
 find_package(OpenCV REQUIRED)
+find_package(camera_info_manager REQUIRED)
 
 ament_auto_find_build_dependencies()
 
@@ -57,6 +58,10 @@ ament_auto_add_library(mono_camera_node SHARED
 )
 add_dependencies_and_linkings(mono_camera_node)
 
+ament_auto_add_library(image_subscriber_node SHARED
+  src/image_subscriber_node.cpp
+)
+
 ament_auto_add_library(trigger_node SHARED
   src/trigger_node.cpp
 )
@@ -66,10 +71,33 @@ add_dependencies_and_linkings(trigger_node)
 ament_auto_add_executable(mono_camera_exec
   src/exec/mono_camera_exec.cpp
 )
+target_link_libraries(mono_camera_exec
+  mono_camera_node
+)
 
 ament_auto_add_executable(trigger_exec
   src/exec/trigger_exec.cpp
 )
+target_link_libraries(trigger_exec
+  trigger_node
+)
+
+# Add the intra-process executable
+ament_auto_add_executable(intra_process_exec
+  src/exec/intra_process_exec.cpp
+)
+target_link_libraries(intra_process_exec
+  mono_camera_node
+  image_subscriber_node
+)
+
+# Add the image subscriber executable
+ament_auto_add_executable(image_subscriber_exec
+  src/exec/image_subscriber_exec.cpp
+)
+target_link_libraries(image_subscriber_exec
+  image_subscriber_node
+)
 
 #############
 ## Install ##

avt_vimba_camera/config/params.yaml

@@ -1,116 +1,119 @@
-# This file is an example of the kind of parameters you can edit with your AVT camera.
-# Note that your camera will likely have different defaults.
-# Most of them are commented out, since the driver will use the configs already in the camera by default.
-# See the README for more details.
-
 /**:
   ros__parameters:
+    name: camera
+    frame_id: "vimba_front"
+    ip: '10.42.7.51'
+    guid: ''
+    use_measurement_time: false
+    ptp_offset: 0
+    publish_compressed: false
     feature/AcquisitionFrameCount: 1
-    feature/AcquisitionFrameRateAbs: 10.0
-    # feature/AcquisitionMode: Continuous
-    # feature/ActionDeviceKey: 0
-    # feature/ActionGroupKey: 0
-    # feature/ActionGroupMask: 0
-    # feature/ActionSelector: 0
-    # feature/BalanceRatioAbs: 2.33
-    # feature/BalanceRatioSelector: Red
-    # feature/BalanceWhiteAuto: 'Off'
-    # feature/BalanceWhiteAutoAdjustTol: 5
-    # feature/BalanceWhiteAutoRate: 100
-    # feature/BandwidthControlMode: StreamBytesPerSecond
+    # feature/AcquisitionFrameRateAbs: 10.0
+    feature/AcquisitionMode: Continuous
+    feature/ActionDeviceKey: 0
+    feature/ActionGroupKey: 0
+    feature/ActionGroupMask: 0
+    feature/ActionSelector: 0
+    feature/BalanceRatioAbs: 2.0
+    feature/BalanceRatioSelector: Red
+    feature/BalanceWhiteAuto: 'Off'
+    feature/BalanceWhiteAutoAdjustTol: 5
+    feature/BalanceWhiteAutoRate: 100
+    feature/BandwidthControlMode: StreamBytesPerSecond
     # feature/BinningHorizontal: 1
     # feature/BinningVertical: 1
-    # feature/BlackLevel: 4.0
-    # feature/BlackLevelSelector: All
-    # feature/ChunkModeActive: false
-    # feature/ColorTransformationMode: 'Off'
-    # feature/ColorTransformationSelector: RGBtoRGB
-    # feature/ColorTransformationValue: 1.0
-    # feature/ColorTransformationValueSelector: Gain00
-    # feature/DSPSubregionBottom: 1544
+    feature/BlackLevel: 50.0
+    feature/BlackLevelSelector: All
+    feature/ChunkModeActive: false
+    feature/ColorTransformationMode: 'Off'
+    feature/ColorTransformationSelector: RGBtoRGB
+    feature/ColorTransformationValue: 1.0
+    feature/ColorTransformationValueSelector: Gain00
+    # feature/DSPSubregionBottom: 490
     # feature/DSPSubregionLeft: 0
-    # feature/DSPSubregionRight: 2064
-    # feature/DSPSubregionTop: 0
-    # feature/DecimationHorizontal: 1
-    # feature/DecimationVertical: 1
-    # feature/DeviceTemperatureSelector: Main
-    # feature/DeviceUserID: ''
-    # feature/EventNotification: 'Off'
-    # feature/EventSelector: AcquisitionStart
-    # feature/EventsEnable1: 0
-    # feature/ExposureAuto: 'Off'
+    # feature/DSPSubregionRight: 1032
+    # feature/DSPSubregionTop: 275
+    feature/DecimationHorizontal: 1
+    feature/DecimationVertical: 1
+    feature/DeviceTemperatureSelector: Main
+    feature/DeviceUserID: ''
+    feature/EventNotification: 'Off'
+    feature/EventSelector: AcquisitionStart
+    feature/EventsEnable1: 0
+    feature/ExposureAuto: 'Off'
     # feature/ExposureAutoAdjustTol: 5
-    # feature/ExposureAutoAlg: Mean
-    # feature/ExposureAutoMax: 499997
+    # feature/ExposureAutoAlg: FitRange
+    # feature/ExposureAutoMax: 30000
     # feature/ExposureAutoMin: 29
-    # feature/ExposureAutoOutliers: 0
-    # feature/ExposureAutoRate: 100
+    # feature/ExposureAutoOutliers: 1000
+    # feature/ExposureAutoRate: 10
     # feature/ExposureAutoTarget: 50
     # feature/ExposureMode: Timed
-    # feature/ExposureTimeAbs: 15005.0
+    feature/ExposureTimeAbs: 493.0
     # feature/GVCPCmdRetries: 5
     # feature/GVCPCmdTimeout: 250
     # feature/GVSPBurstSize: 1
     # feature/GVSPDriver: Socket
-    # feature/GVSPHostReceiveBufferSize: 67108864
+    # feature/GVSPHostReceiveBufferSize: 10240
     # feature/GVSPMaxLookBack: 30
     # feature/GVSPMaxRequests: 1
     # feature/GVSPMaxWaitSize: 100
     # feature/GVSPMissingSize: 256
     # feature/GVSPPacketSize: 9014
     # feature/GVSPTiltingSize: 100
     # feature/GVSPTimeout: 70
-    # feature/Gain: 0.0
-    # feature/GainAuto: 'Off'
-    # feature/GainAutoAdjustTol: 5
-    # feature/GainAutoMax: 40.0
-    # feature/GainAutoMin: 0.0
-    # feature/GainAutoOutliers: 0
-    # feature/GainAutoRate: 100
-    # feature/GainAutoTarget: 50
-    # feature/GainSelector: All
-    # feature/Gamma: 1.0
+    feature/Gain: 0.0
+    feature/GainAuto: 'Continuous'
+    feature/GainAutoAdjustTol: 5
+    feature/GainAutoMax: 32.0
+    feature/GainAutoMin: 0.0
+    feature/GainAutoOutliers: 100
+    feature/GainAutoRate: 50
+    feature/GainAutoTarget: 50
+    feature/GainSelector: All
+    feature/Gamma: 1.0
     # feature/GevHeartbeatInterval: 1450
     # feature/GevHeartbeatTimeout: 3000
     # feature/GevSCPSPacketSize: 9014
-    # feature/Height: 1544
-    # feature/Hue: 0.0
-    # feature/LUTEnable: false
-    # feature/LUTIndex: 0
-    # feature/LUTMode: Luminance
-    # feature/LUTSelector: LUT1
-    # feature/LUTValue: 4095
-    # feature/MulticastEnable: false
-    # feature/MulticastIPAddress: 823132143
-    # feature/OffsetX: 0
-    # feature/OffsetY: 0
-    # feature/PixelFormat: BayerRG8
-    # feature/PtpAcquisitionGateTime: 0
-    # feature/PtpMode: 'Off'
-    # feature/RecorderPreEventCount: 0
-    # feature/ReverseX: false
-    # feature/ReverseY: false
-    # feature/Saturation: 1.0
-    # feature/StreamBufferHandlingMode: Default
-    # feature/StreamBytesPerSecond: 115000000
-    # feature/StreamFrameRateConstrain: true
-    # feature/StreamHoldEnable: 'Off'
-    # feature/StrobeDelay: 0
-    # feature/StrobeDuration: 0
-    # feature/StrobeDurationMode: Source
-    # feature/StrobeSource: FrameTrigger
-    # feature/SyncInGlitchFilter: 0
-    # feature/SyncInSelector: SyncIn1
-    # feature/SyncOutLevels: 0
-    # feature/SyncOutPolarity: Normal
-    # feature/SyncOutSelector: SyncOut1
-    # feature/SyncOutSource: Exposing
-    # feature/TriggerActivation: RisingEdge
-    # feature/TriggerDelayAbs: 0.0
-    # feature/TriggerMode: 'On'
-    # feature/TriggerOverlap: 'Off'
-    # feature/TriggerSelector: FrameStart
+    feature/Height: 772
+    feature/Hue: 0.0
+    feature/LUTEnable: false
+    feature/LUTIndex: 0
+    feature/LUTMode: Luminance
+    feature/LUTSelector: LUT1
+    feature/LUTValue: 4095
+    feature/MulticastEnable: false
+    feature/MulticastIPAddress: 4026470193
+    feature/OffsetX: 0
+    feature/OffsetY: 0
+    feature/PixelFormat: RGB8Packed
+    feature/PtpAcquisitionGateTime: 0
+    feature/PtpMode: 'Auto'
+    feature/RecorderPreEventCount: 0
+    feature/ReverseX: false
+    feature/ReverseY: false
+    feature/Saturation: 1.0
+    feature/StreamBufferHandlingMode: Default
+    feature/StreamBytesPerSecond: 12400000
+    feature/StreamFrameRateConstrain: true
+    feature/StreamHoldEnable: 'Off'
+    feature/StrobeDelay: 0
+    feature/StrobeDuration: 0
+    feature/StrobeDurationMode: Source
+    feature/StrobeSource: FrameTrigger
+    feature/SyncInGlitchFilter: 0
+    feature/SyncInSelector: SyncIn1
+    feature/SyncOutLevels: 0
+    feature/SyncOutPolarity: Normal
+    feature/SyncOutSelector: SyncOut1
+    feature/SyncOutSource: Exposing
+    feature/TriggerActivation: RisingEdge
+    feature/TriggerDelayAbs: 0.0
+    feature/TriggerMode: 'On'
+    feature/TriggerOverlap: 'Off'
+    feature/TriggerSelector: FrameStart
     feature/TriggerSource: FixedRate
-    # feature/UserSetDefaultSelector: Default
-    # feature/UserSetSelector: Default
-    # feature/Width: 2064
+    feature/UserSetDefaultSelector: Default
+    feature/UserSetSelector: Default
+    feature/Width: 1032
+

avt_vimba_camera/include/VimbaCPP/Include/Feature.hpp

@@ -137,7 +137,7 @@ inline VmbErrorType Feature::GetValue( std::string &rStrValue ) const
     VmbErrorType    res;
     VmbUint32_t     nLength;
 
-    res = GetValue( (char * const)NULL, nLength );
+    res = GetValue( static_cast<char*>(nullptr), nLength );
     if ( VmbErrorSuccess == res )
     {
         if ( 0 != nLength )

avt_vimba_camera/include/avt_vimba_camera/avt_vimba_api.hpp

@@ -238,23 +238,30 @@ class AvtVimbaApi
     if (VmbErrorSuccess == err)
     {
       bool PUB_DEBAYER = true;
+      if (pixel_format == VmbPixelFormatRgb8 || pixel_format == VmbPixelFormatBgr8 || pixel_format == VmbPixelFormatRgb16 || pixel_format == VmbPixelFormatBgr16) {
+        PUB_DEBAYER = false;
+      }
 
       if (PUB_DEBAYER) {
+        // RCLCPP_INFO(logger_, "[frameToImage] PUB_DEBAYER");
         const cv::Mat m(height, width, CV_8UC1, static_cast<uint8_t*>(buffer_ptr), step);
         image.height = height;
         image.width = width;
         image.encoding = "rgb8";
         image.is_bigendian = false;
         image.step = step * 3;
         image.data = std::vector<uint8_t>(image.height * image.width * 3);
+        // RCLCPP_INFO(logger_, "[frameToImage] image.data size: %lu", image.data.size());
 
         cv::Mat output_mat(image.height, image.width, CV_8UC3,
             static_cast<uint8_t*>(image.data.data()), image.step);
+        // RCLCPP_INFO(logger_, "[frameToImage] output_mat size: %lu", output_mat.total());
         cv::ColorConversionCodes code = cv::COLOR_BayerBG2RGB;
         cv::demosaicing(m, output_mat, code);
         res = true;
 
         if (publish_compressed){
+          // RCLCPP_INFO(logger_, "[frameToImage] publish_compressed: %d", publish_compressed);
             // const auto debayer_start = std::chrono::high_resolution_clock::now();
             compressed_image.header = image.header;
             compressed_image.format = "jpeg";
@@ -264,7 +271,7 @@ class AvtVimbaApi
                 // TODO: Add more parameters here
               }
             );
-
+            // RCLCPP_INFO(logger_, "[frameToImage] compressed_image.data size: %lu", compressed_image.data.size());
             // const auto debayer_end = std::chrono::high_resolution_clock::now();
             // auto ms = std::chrono::duration_cast<std::chrono::microseconds>(debayer_end - debayer_start).count();
             // RCLCPP_WARN(logger_, "image debayer took %lu microseconds", ms);

avt_vimba_camera/include/avt_vimba_camera/image_subscriber_node.hpp

@@ -0,0 +1,34 @@
+#ifndef AVT_VIMBA_CAMERA__IMAGE_SUBSCRIBER_NODE_HPP_
+#define AVT_VIMBA_CAMERA__IMAGE_SUBSCRIBER_NODE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <std_msgs/msg/int32.hpp>
+#include <memory>
+
+namespace avt_vimba_camera
+{
+
+class ImageSubscriberNode : public rclcpp::Node
+{
+public:
+  explicit ImageSubscriberNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  virtual ~ImageSubscriberNode() = default;
+
+private:
+  // Image subscription
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
+
+  // Demo message subscription
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_demo_;
+
+  // Callback for image messages
+  void imageCallback(sensor_msgs::msg::Image::UniquePtr msg);
+
+  // Callback for demo messages
+  void demoCallback(std_msgs::msg::Int32::UniquePtr msg);
+};
+
+}  // namespace avt_vimba_camera
+
+#endif  // AVT_VIMBA_CAMERA__IMAGE_SUBSCRIBER_NODE_HPP_ 

avt_vimba_camera/include/avt_vimba_camera/mono_camera_node.hpp

@@ -46,14 +46,15 @@
 #include <std_srvs/srv/trigger.hpp>
 #include <avt_vimba_camera_msgs/srv/load_settings.hpp>
 #include <avt_vimba_camera_msgs/srv/save_settings.hpp>
+#include "std_msgs/msg/int32.hpp"
 
 
 namespace avt_vimba_camera
 {
 class MonoCameraNode : public rclcpp::Node
 {
 public:
-  MonoCameraNode();
+  explicit MonoCameraNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
   ~MonoCameraNode();
   void start();
 
@@ -72,7 +73,13 @@ class MonoCameraNode : public rclcpp::Node
   image_transport::CameraPublisher camera_info_pub_;
   rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr compressed_pub;
   std::shared_ptr<camera_info_manager::CameraInfoManager> info_man_;
-
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
+  std::weak_ptr<rclcpp::Publisher<sensor_msgs::msg::Image>> captured_pub;
+
+  std::weak_ptr<rclcpp::Publisher<std_msgs::msg::Int32>> captured_pub_demo;
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub_demo_;
+  rclcpp::TimerBase::SharedPtr timer_demo_;
+
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr start_srv_;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr stop_srv_;
 
@@ -82,6 +89,8 @@ class MonoCameraNode : public rclcpp::Node
 
   void loadParams();
   void frameCallback(const FramePtr& vimba_frame_ptr);
+  void publishImagePtr(sensor_msgs::msg::Image & image);
+
   void startSrvCallback(const std::shared_ptr<rmw_request_id_t> request_header,
                         const std_srvs::srv::Trigger::Request::SharedPtr req,
                         std_srvs::srv::Trigger::Response::SharedPtr res);