LVMS 2025 onsite modifications

avt_vimba_camera/CMakeLists.txt

@@ -12,7 +12,8 @@ endif()
 find_package(ament_cmake_auto REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(camera_info_manager REQUIRED)
-
+find_package(cv_bridge REQUIRED)
+find_package(OpenCV REQUIRED)
 ament_auto_find_build_dependencies()
 
 # Get architecture
@@ -104,6 +105,10 @@ target_link_libraries(intra_process_exec
 ament_auto_add_executable(image_subscriber_exec
   src/exec/image_subscriber_exec.cpp
 )
+target_link_libraries(image_subscriber_exec
+  ${OpenCV_LIBS}
+)
+
 target_link_libraries(image_subscriber_exec
   image_subscriber_node
 )
@@ -140,4 +145,4 @@ ament_auto_package(INSTALL_TO_SHARE
   calibrations
   config
   launch
-)
+)

avt_vimba_camera/config/params.yaml

@@ -2,7 +2,7 @@
   ros__parameters:
     name: camera
     frame_id: "vimba_front"
-    ip: '10.42.7.51'
+    ip: '10.42.17.51'
     guid: ''
     use_measurement_time: false
     ptp_offset: 0

avt_vimba_camera/include/avt_vimba_camera/avt_vimba_api.hpp

@@ -39,7 +39,6 @@
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/image_encodings.hpp>
 #include <sensor_msgs/fill_image.hpp>
-#include <sensor_msgs/msg/compressed_image.hpp>
 
 #include <opencv2/opencv.hpp>
 #include <opencv2/imgproc.hpp>
@@ -147,9 +146,7 @@ class AvtVimbaApi
   }
 
   bool frameToImage(const FramePtr vimba_frame_ptr,
-                    sensor_msgs::msg::Image& image,
-                    sensor_msgs::msg::CompressedImage& compressed_image,
-                    bool publish_compressed)
+                    sensor_msgs::msg::Image& image)
   {
     VmbPixelFormatType pixel_format;
     VmbUint32_t width, height, nSize;
@@ -259,23 +256,6 @@ class AvtVimbaApi
         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";
-            cv::imencode(".jpg", output_mat, compressed_image.data,
-              std::vector<int>{
-                cv::IMWRITE_JPEG_QUALITY, 90
-                // 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);
-        }
       } else {
         res = sensor_msgs::fillImage(image, encoding, height, width, step, buffer_ptr);
       }

avt_vimba_camera/include/avt_vimba_camera/image_subscriber_node.hpp

@@ -18,15 +18,11 @@ class ImageSubscriberNode : public rclcpp::Node
 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);
+
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_;
 };
 
 }  // namespace avt_vimba_camera

avt_vimba_camera/include/avt_vimba_camera/mono_camera_node.hpp

@@ -67,17 +67,15 @@ class MonoCameraNode : public rclcpp::Node
   std::string camera_info_url_;
   std::string frame_id_;
   bool use_measurement_time_;
-  bool publish_compressed_;
   int32_t ptp_offset_;
 
-  image_transport::CameraPublisher camera_info_pub_;
-  rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr compressed_pub;
+  // image_transport::CameraPublisher camera_info_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_;
+  // 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_;
@@ -86,7 +84,6 @@ class MonoCameraNode : public rclcpp::Node
   rclcpp::Service<avt_vimba_camera_msgs::srv::LoadSettings>::SharedPtr load_srv_;
   rclcpp::Service<avt_vimba_camera_msgs::srv::SaveSettings>::SharedPtr save_srv_;
 
-
   void loadParams();
   void frameCallback(const FramePtr& vimba_frame_ptr);
   void publishImagePtr(sensor_msgs::msg::Image & image);

avt_vimba_camera/package.xml

@@ -19,6 +19,7 @@
   <depend>rclcpp_components</depend>
   <depend>backward_ros</depend>
 
+<depend>cv_bridge</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
   <depend>sensor_msgs</depend>

avt_vimba_camera/src/image_subscriber_node.cpp

@@ -1,5 +1,8 @@
 #include "avt_vimba_camera/image_subscriber_node.hpp"
 #include <sstream>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.hpp>
+#include <opencv2/highgui.hpp>  // For imshow and waitKey
 
 
 namespace avt_vimba_camera
@@ -17,18 +20,15 @@ ImageSubscriberNode::ImageSubscriberNode(const rclcpp::NodeOptions & options)
 
   // Create the image subscription
   subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
-    "image/ptr",
+    "/image/ptr",
     qos,
     std::bind(&ImageSubscriberNode::imageCallback, this, std::placeholders::_1),
     sub_options
   );
 
-  // Create the demo subscription
-  subscription_demo_ = this->create_subscription<std_msgs::msg::Int32>(
-    "image/demo",
-    qos,
-    std::bind(&ImageSubscriberNode::demoCallback, this, std::placeholders::_1)
-  );
+
+  // Create a publisher to re-publish the image on "output_image" topic
+  // image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>("ptr/output_image", 10);
 
   RCLCPP_INFO(this->get_logger(), "Image subscriber node started with IPC enabled");
 }
@@ -44,25 +44,36 @@ void ImageSubscriberNode::imageCallback(sensor_msgs::msg::Image::UniquePtr msg)
     ss << "0x" << std::hex << reinterpret_cast<std::uintptr_t>(msg.get());
 
     // Store values locally before logging
-    const auto width = msg->width;
-    const auto height = msg->height;
-    const auto encoding = msg->encoding;
-    const auto addr = ss.str();
+    // const auto width = msg->width;
+    // const auto height = msg->height;
+    // const auto encoding = msg->encoding;
+    // const auto addr = ss.str();
 
-    RCLCPP_INFO(this->get_logger(), 
-        "Received image: %dx%d, encoding: %s, address: %s",
-        width, height, encoding.c_str(), addr.c_str());
+    // RCLCPP_INFO(this->get_logger(), 
+    //     "Received image: %dx%d, encoding: %s, address: %s",
+    //     width, height, encoding.c_str(), addr.c_str());
+    try {
+        // Convert ROS image message to OpenCV image
+        cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::BGR8);
+
+        // Create a named window first (with proper flags for ROS environment)
+        std::string window_name = msg->header.frame_id.empty() ? 
+            "Camera Image" : msg->header.frame_id;
+        cv::namedWindow(window_name, cv::WINDOW_AUTOSIZE | cv::WINDOW_KEEPRATIO);
+
+        // Display the image
+        cv::imshow(window_name, cv_ptr->image);
+        cv::pollKey(); // Non-blocking key check instead of waitKey
+
+    } catch (cv_bridge::Exception& e) {
+        RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
+        return;
+    } catch (cv::Exception& e) {
+        RCLCPP_ERROR(this->get_logger(), "OpenCV exception: %s", e.what());
+        return;
+    }
 }
 
-void ImageSubscriberNode::demoCallback(std_msgs::msg::Int32::UniquePtr msg)
-{
-  std::stringstream ss;
-  ss << "0x" << std::hex << reinterpret_cast<std::uintptr_t>(msg.get());
-
-  RCLCPP_INFO(this->get_logger(), 
-    "Received demo message: %d, address: %s",
-    msg->data, ss.str().c_str());
-}
 
 }  // namespace avt_vimba_camera
 

avt_vimba_camera/src/mono_camera_node.cpp

@@ -49,7 +49,7 @@ MonoCameraNode::MonoCameraNode(const rclcpp::NodeOptions & options)
 : Node("camera", options), api_(this->get_logger()), cam_(std::shared_ptr<rclcpp::Node>(dynamic_cast<rclcpp::Node * >(this)))
 {
   // Set the image publisher before streaming
-  camera_info_pub_ = image_transport::create_camera_publisher(this, "~/image");
+  // camera_info_pub_ = image_transport::create_camera_publisher(this, "~/image");
 
   // Set the frame callback
   cam_.setCallback(std::bind(&avt_vimba_camera::MonoCameraNode::frameCallback, this, _1));
@@ -65,22 +65,15 @@ MonoCameraNode::MonoCameraNode(const rclcpp::NodeOptions & options)
   auto qos = rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 1));
   qos.best_effort();
 
-  if (publish_compressed_) {
-    compressed_pub = this->create_publisher<sensor_msgs::msg::CompressedImage>("image/compressed", qos);
-  }
-
   rclcpp::PublisherOptions pub_options;
   pub_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
 
   image_pub_ = this->create_publisher<sensor_msgs::msg::Image>(
-      "image/ptr", 
+      frame_id_ + "/image/ptr",
       qos,
       pub_options
   );
   captured_pub = image_pub_;
-
-  pub_demo_ = this->create_publisher<std_msgs::msg::Int32>("image/demo", qos);
-  captured_pub_demo = pub_demo_;
 
   try {
     this->start();
@@ -93,7 +86,7 @@ MonoCameraNode::MonoCameraNode(const rclcpp::NodeOptions & options)
 MonoCameraNode::~MonoCameraNode()
 {
   cam_.stop();
-  camera_info_pub_.shutdown();
+  // camera_info_pub_.shutdown();
 }
 
 void MonoCameraNode::loadParams()
@@ -104,9 +97,7 @@ void MonoCameraNode::loadParams()
   frame_id_ = this->declare_parameter("frame_id", "");
   use_measurement_time_ = this->declare_parameter("use_measurement_time", false);
   ptp_offset_ = this->declare_parameter("ptp_offset", 0);
-  publish_compressed_ = this->declare_parameter("publish_compressed", true);
 
-  RCLCPP_INFO(this->get_logger(), "publish_compressed: %d", publish_compressed_);
   RCLCPP_INFO(this->get_logger(), "Parameters loaded");
 }
 
@@ -132,8 +123,7 @@ void MonoCameraNode::frameCallback(const FramePtr& vimba_frame_ptr)
   rclcpp::Time ros_time = this->get_clock()->now();
   {
     sensor_msgs::msg::Image img;
-    sensor_msgs::msg::CompressedImage compressed_image;
-    if (api_.frameToImage(vimba_frame_ptr, img, compressed_image, publish_compressed_))
+    if (api_.frameToImage(vimba_frame_ptr, img))
     {
       sensor_msgs::msg::CameraInfo ci = cam_.getCameraInfo();
       ci.header.frame_id = frame_id_;
@@ -143,14 +133,8 @@ void MonoCameraNode::frameCallback(const FramePtr& vimba_frame_ptr)
 	    ci.header.stamp = ros_time;
       img.header.frame_id = ci.header.frame_id;
       img.header.stamp = ci.header.stamp;
-      camera_info_pub_.publish(img, ci);
+      // camera_info_pub_.publish(img, ci);
       publishImagePtr(img);
-
-      if (publish_compressed_) {
-        compressed_image.header.frame_id = ci.header.frame_id;
-        compressed_image.header.stamp = ci.header.stamp;
-        compressed_pub->publish(compressed_image);
-      }
     }
     else
     {
@@ -168,10 +152,10 @@ void MonoCameraNode::publishImagePtr(sensor_msgs::msg::Image & image) {
     }
     sensor_msgs::msg::Image::UniquePtr msg(new sensor_msgs::msg::Image(image));
 
-    std::stringstream ss;
-    ss << "0x" << std::hex << reinterpret_cast<std::uintptr_t>(msg.get());
-    RCLCPP_INFO(this->get_logger(), "Published message with address: %s", 
-                ss.str().c_str());
+    // std::stringstream ss;
+    // ss << "0x" << std::hex << reinterpret_cast<std::uintptr_t>(msg.get());
+    // RCLCPP_INFO(this->get_logger(), "Published message with address: %s", 
+    //             ss.str().c_str());
     pub_ptr->publish(std::move(msg));
 }