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/image_subscriber_node.hpp

@@ -20,13 +20,16 @@ class ImageSubscriberNode : public rclcpp::Node
   rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
 
   // Demo message subscription
-  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_demo_;
+  // 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);
+  // void demoCallback(std_msgs::msg::Int32::UniquePtr msg);
+
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_;
+  // rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr demo_publisher_;
 };
 
 }  // namespace avt_vimba_camera

avt_vimba_camera/include/avt_vimba_camera/mono_camera_node.hpp

@@ -71,13 +71,13 @@ class MonoCameraNode : public rclcpp::Node
   int32_t ptp_offset_;
 
   image_transport::CameraPublisher camera_info_pub_;
-  rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr compressed_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_;
+  // 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 +86,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
@@ -24,11 +27,17 @@ ImageSubscriberNode::ImageSubscriberNode(const rclcpp::NodeOptions & 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)
-  );
+  // 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);
+  // demo_publisher_ = this->create_publisher<std_msgs::msg::Int32>("demo/output_image", 10);
+
+
 
   RCLCPP_INFO(this->get_logger(), "Image subscriber node started with IPC enabled");
 }
@@ -40,29 +49,54 @@ void ImageSubscriberNode::imageCallback(sensor_msgs::msg::Image::UniquePtr msg)
         return;
     }
 
-    std::stringstream ss;
-    ss << "0x" << std::hex << reinterpret_cast<std::uintptr_t>(msg.get());
+    // std::stringstream ss;
+    // 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());
+// 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());
-}
+//   RCLCPP_INFO(this->get_logger(), 
+//     "Received demo message: %d, address: %s",
+//     msg->data, ss.str().c_str());
+
+//   std_msgs::msg::Int32 last_demo_;
+//   last_demo_ = *msg;
+//   demo_publisher_->publish(std::move(*msg));
+
+// }
 
 }  // namespace avt_vimba_camera
 

avt_vimba_camera/src/mono_camera_node.cpp

@@ -65,22 +65,22 @@ 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);
-  }
+  // 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_;
+  // pub_demo_ = this->create_publisher<std_msgs::msg::Int32>("image/demo", qos);
+  // captured_pub_demo = pub_demo_;
 
   try {
     this->start();
@@ -106,7 +106,7 @@ void MonoCameraNode::loadParams()
   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(), "publish_compressed: %d", publish_compressed_);
   RCLCPP_INFO(this->get_logger(), "Parameters loaded");
 }
 
@@ -143,14 +143,14 @@ 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);
-      }
+      // 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 +168,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));
 }