From 61a13aceffe7d2ef3a90b51f532d55da9f511b11 Mon Sep 17 00:00:00 2001 From: Michael Wu Date: Thu, 21 Nov 2024 00:03:08 -0800 Subject: [PATCH 1/8] feat: avt camera modified --- avt_vimba_camera/CMakeLists.txt | 1 + .../include/avt_vimba_camera/avt_vimba_api.hpp | 9 ++++++++- avt_vimba_camera/src/mono_camera_node.cpp | 5 ++++- 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/avt_vimba_camera/CMakeLists.txt b/avt_vimba_camera/CMakeLists.txt index 28857d7e..4360cf56 100644 --- a/avt_vimba_camera/CMakeLists.txt +++ b/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() diff --git a/avt_vimba_camera/include/avt_vimba_camera/avt_vimba_api.hpp b/avt_vimba_camera/include/avt_vimba_camera/avt_vimba_api.hpp index a12aa3ca..7a475df6 100644 --- a/avt_vimba_camera/include/avt_vimba_camera/avt_vimba_api.hpp +++ b/avt_vimba_camera/include/avt_vimba_camera/avt_vimba_api.hpp @@ -238,8 +238,12 @@ 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(buffer_ptr), step); image.height = height; image.width = width; @@ -247,14 +251,17 @@ class AvtVimbaApi image.is_bigendian = false; image.step = step * 3; image.data = std::vector(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(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(debayer_end - debayer_start).count(); // RCLCPP_WARN(logger_, "image debayer took %lu microseconds", ms); diff --git a/avt_vimba_camera/src/mono_camera_node.cpp b/avt_vimba_camera/src/mono_camera_node.cpp index 735f6e8d..8e046367 100644 --- a/avt_vimba_camera/src/mono_camera_node.cpp +++ b/avt_vimba_camera/src/mono_camera_node.cpp @@ -96,14 +96,16 @@ void MonoCameraNode::start() void MonoCameraNode::frameCallback(const FramePtr& vimba_frame_ptr) { rclcpp::Time ros_time = this->get_clock()->now(); - + // RCLCPP_INFO(this->get_logger(), "frameCallback start"); // getNumSubscribers() is not yet supported in Foxy, will be supported in later versions // if (camera_info_pub_.getNumSubscribers() > 0) { sensor_msgs::msg::Image img; sensor_msgs::msg::CompressedImage compressed_image; + // RCLCPP_INFO(this->get_logger(), "frameToImage start"); if (api_.frameToImage(vimba_frame_ptr, img, compressed_image, publish_compressed_)) { + // RCLCPP_INFO(this->get_logger(), "frameToImage success"); sensor_msgs::msg::CameraInfo ci = cam_.getCameraInfo(); // Note: getCameraInfo() doesn't fill in header frame_id or stamp ci.header.frame_id = frame_id_; @@ -127,6 +129,7 @@ void MonoCameraNode::frameCallback(const FramePtr& vimba_frame_ptr) RCLCPP_WARN_STREAM(this->get_logger(), "Function frameToImage returned 0. No image published."); } } + // RCLCPP_INFO(this->get_logger(), "frameCallback end"); } void MonoCameraNode::startSrvCallback(const std::shared_ptr request_header, From aa2b8ea12f9425cda25d1cc54220003218b53cac Mon Sep 17 00:00:00 2001 From: Michael Wu Date: Thu, 21 Nov 2024 21:44:03 -0800 Subject: [PATCH 2/8] feat: sample shared ptr implemented --- avt_vimba_camera/CMakeLists.txt | 27 +++ avt_vimba_camera/config/params.yaml | 185 +++++++++--------- .../include/VimbaCPP/Include/Feature.hpp | 2 +- .../image_subscriber_node.hpp | 34 ++++ .../avt_vimba_camera/mono_camera_node.hpp | 13 +- .../launch/intra_process_test.launch.py | 19 ++ avt_vimba_camera/src/avt_vimba_camera.cpp | 6 +- .../src/exec/image_subscriber_exec.cpp | 11 ++ .../src/exec/intra_process_exec.cpp | 37 ++++ .../src/image_subscriber_node.cpp | 68 +++++++ avt_vimba_camera/src/mono_camera_node.cpp | 82 ++++++-- 11 files changed, 376 insertions(+), 108 deletions(-) create mode 100644 avt_vimba_camera/include/avt_vimba_camera/image_subscriber_node.hpp create mode 100644 avt_vimba_camera/launch/intra_process_test.launch.py create mode 100644 avt_vimba_camera/src/exec/image_subscriber_exec.cpp create mode 100644 avt_vimba_camera/src/exec/intra_process_exec.cpp create mode 100644 avt_vimba_camera/src/image_subscriber_node.cpp diff --git a/avt_vimba_camera/CMakeLists.txt b/avt_vimba_camera/CMakeLists.txt index 4360cf56..012874ea 100644 --- a/avt_vimba_camera/CMakeLists.txt +++ b/avt_vimba_camera/CMakeLists.txt @@ -58,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 ) @@ -67,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 ## diff --git a/avt_vimba_camera/config/params.yaml b/avt_vimba_camera/config/params.yaml index 149d93eb..0080d599 100644 --- a/avt_vimba_camera/config/params.yaml +++ b/avt_vimba_camera/config/params.yaml @@ -1,58 +1,60 @@ -# 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 @@ -60,57 +62,58 @@ # 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 + diff --git a/avt_vimba_camera/include/VimbaCPP/Include/Feature.hpp b/avt_vimba_camera/include/VimbaCPP/Include/Feature.hpp index 023b5f7b..2d5604c9 100644 --- a/avt_vimba_camera/include/VimbaCPP/Include/Feature.hpp +++ b/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(nullptr), nLength ); if ( VmbErrorSuccess == res ) { if ( 0 != nLength ) diff --git a/avt_vimba_camera/include/avt_vimba_camera/image_subscriber_node.hpp b/avt_vimba_camera/include/avt_vimba_camera/image_subscriber_node.hpp new file mode 100644 index 00000000..b284f925 --- /dev/null +++ b/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 +#include +#include +#include + +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::SharedPtr subscription_; + + // Demo message subscription + rclcpp::Subscription::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_ \ No newline at end of file diff --git a/avt_vimba_camera/include/avt_vimba_camera/mono_camera_node.hpp b/avt_vimba_camera/include/avt_vimba_camera/mono_camera_node.hpp index 1d53ac25..999aaf9b 100644 --- a/avt_vimba_camera/include/avt_vimba_camera/mono_camera_node.hpp +++ b/avt_vimba_camera/include/avt_vimba_camera/mono_camera_node.hpp @@ -46,6 +46,7 @@ #include #include #include +#include "std_msgs/msg/int32.hpp" namespace avt_vimba_camera @@ -53,7 +54,7 @@ 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::SharedPtr compressed_pub; std::shared_ptr info_man_; - + rclcpp::Publisher::SharedPtr image_pub_; + std::weak_ptr> captured_pub; + + std::weak_ptr> captured_pub_demo; + rclcpp::Publisher::SharedPtr pub_demo_; + rclcpp::TimerBase::SharedPtr timer_demo_; + rclcpp::Service::SharedPtr start_srv_; rclcpp::Service::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::SharedPtr image); + void startSrvCallback(const std::shared_ptr request_header, const std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr res); diff --git a/avt_vimba_camera/launch/intra_process_test.launch.py b/avt_vimba_camera/launch/intra_process_test.launch.py new file mode 100644 index 00000000..c8941864 --- /dev/null +++ b/avt_vimba_camera/launch/intra_process_test.launch.py @@ -0,0 +1,19 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + package_share_dir = get_package_share_directory('avt_vimba_camera') + params_file = os.path.join(package_share_dir, 'config', 'params.yaml') + + return LaunchDescription([ + Node( + package='avt_vimba_camera', + executable='intra_process_exec', + name='camera_with_subscriber', + parameters=[params_file], + output='screen', + emulate_tty=True + ) + ]) \ No newline at end of file diff --git a/avt_vimba_camera/src/avt_vimba_camera.cpp b/avt_vimba_camera/src/avt_vimba_camera.cpp index 7a50ead9..28ff11ce 100644 --- a/avt_vimba_camera/src/avt_vimba_camera.cpp +++ b/avt_vimba_camera/src/avt_vimba_camera.cpp @@ -576,7 +576,7 @@ bool AvtVimbaCamera::getFeatureValue(const std::string& feature_str, std::string template void AvtVimbaCamera::configureFeature(const std::string& feature_str, const Vimba_Type& val_in, Std_Type& val_out) { - Vimba_Type actual_value; + Vimba_Type actual_value = val_in; // Initialize with input value VmbErrorType return_value = setFeatureValue(feature_str, val_in); if (return_value == VmbErrorSuccess || return_value == VmbErrorInvalidValue) @@ -690,8 +690,8 @@ void AvtVimbaCamera::initConfig() { on_init_config_ = true; RCLCPP_INFO(nh_->get_logger(), "Configuring camera:"); - // Query all camera features to translate into ROS params - for (const FeaturePtr feature : features) + // Use const reference in the loop + for (const FeaturePtr& feature : features) { std::string feature_name = ""; bool is_writable = false; diff --git a/avt_vimba_camera/src/exec/image_subscriber_exec.cpp b/avt_vimba_camera/src/exec/image_subscriber_exec.cpp new file mode 100644 index 00000000..da0961af --- /dev/null +++ b/avt_vimba_camera/src/exec/image_subscriber_exec.cpp @@ -0,0 +1,11 @@ +#include +#include "avt_vimba_camera/image_subscriber_node.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/avt_vimba_camera/src/exec/intra_process_exec.cpp b/avt_vimba_camera/src/exec/intra_process_exec.cpp new file mode 100644 index 00000000..f9b5271b --- /dev/null +++ b/avt_vimba_camera/src/exec/intra_process_exec.cpp @@ -0,0 +1,37 @@ +#include +#include +#include "avt_vimba_camera/mono_camera_node.hpp" +#include "avt_vimba_camera/image_subscriber_node.hpp" + +int main(int argc, char * argv[]) +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); + + // Create node options with intra-process communication enabled + rclcpp::NodeOptions options; + options.use_intra_process_comms(true); + + // Create both nodes with the same options + auto camera_node = std::make_shared(options); + auto subscriber_node = std::make_shared(options); + + // Create a single-threaded executor + rclcpp::executors::SingleThreadedExecutor executor; + + // Add both nodes to the executor + executor.add_node(camera_node); + executor.add_node(subscriber_node); + + // Start the camera after adding to executor + camera_node->start(); + + // Spin the executor + executor.spin(); + + // Cleanup + camera_node.reset(); + subscriber_node.reset(); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/avt_vimba_camera/src/image_subscriber_node.cpp b/avt_vimba_camera/src/image_subscriber_node.cpp new file mode 100644 index 00000000..3cd26f8a --- /dev/null +++ b/avt_vimba_camera/src/image_subscriber_node.cpp @@ -0,0 +1,68 @@ +#include "avt_vimba_camera/image_subscriber_node.hpp" +#include + + +namespace avt_vimba_camera +{ + +ImageSubscriberNode::ImageSubscriberNode(const rclcpp::NodeOptions & options) +: Node("image_subscriber", options) +{ + // Create QoS profile matching the publisher + auto qos = rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 1)); + qos.reliable(); + + rclcpp::SubscriptionOptions sub_options; + sub_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + + // Create the image subscription + subscription_ = this->create_subscription( + "image/ptr", + qos, + std::bind(&ImageSubscriberNode::imageCallback, this, std::placeholders::_1), + sub_options + ); + + // Create the demo subscription + subscription_demo_ = this->create_subscription( + "image/demo", + qos, + std::bind(&ImageSubscriberNode::demoCallback, this, std::placeholders::_1) + ); + + RCLCPP_INFO(this->get_logger(), "Image subscriber node started with IPC enabled"); +} + +void ImageSubscriberNode::imageCallback(sensor_msgs::msg::Image::UniquePtr msg) +{ + if (!msg) { + RCLCPP_WARN(this->get_logger(), "Received null image message"); + return; + } + + std::stringstream ss; + ss << "0x" << std::hex << reinterpret_cast(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(); + + RCLCPP_INFO(this->get_logger(), + "Received image: %dx%d, encoding: %s, address: %s", + width, height, encoding.c_str(), addr.c_str()); +} + +void ImageSubscriberNode::demoCallback(std_msgs::msg::Int32::UniquePtr msg) +{ + std::stringstream ss; + ss << "0x" << std::hex << reinterpret_cast(msg.get()); + + RCLCPP_INFO(this->get_logger(), + "Received demo message: %d, address: %s", + msg->data, ss.str().c_str()); +} + +} // namespace avt_vimba_camera + diff --git a/avt_vimba_camera/src/mono_camera_node.cpp b/avt_vimba_camera/src/mono_camera_node.cpp index 8e046367..d45dcb8f 100644 --- a/avt_vimba_camera/src/mono_camera_node.cpp +++ b/avt_vimba_camera/src/mono_camera_node.cpp @@ -31,16 +31,20 @@ /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include #include #include #include +#include +using namespace std::chrono_literals; // Add this for time literals using namespace std::placeholders; namespace avt_vimba_camera { -MonoCameraNode::MonoCameraNode() : Node("camera"), api_(this->get_logger()), cam_(std::shared_ptr(dynamic_cast(this))) +MonoCameraNode::MonoCameraNode(const rclcpp::NodeOptions & options) +: Node("camera", options), api_(this->get_logger()), cam_(std::shared_ptr(dynamic_cast(this))) { // Set the image publisher before streaming camera_info_pub_ = image_transport::create_camera_publisher(this, "~/image"); @@ -60,8 +64,38 @@ MonoCameraNode::MonoCameraNode() : Node("camera"), api_(this->get_logger()), cam qos.reliable(); if (publish_compressed_) { - compressed_pub = this->create_publisher("~/image/compressed", qos); + compressed_pub = this->create_publisher("image/compressed", qos); } + + rclcpp::PublisherOptions pub_options; + pub_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + + image_pub_ = this->create_publisher( + "image/ptr", + qos, + pub_options + ); + captured_pub = image_pub_; + + pub_demo_ = this->create_publisher("image/demo", qos); + captured_pub_demo = pub_demo_; + + auto callback_demo = [this]() -> void { + auto pub_ptr = captured_pub_demo.lock(); + if (!pub_ptr) { + return; + } + static int32_t count = 0; + std_msgs::msg::Int32::UniquePtr msg(new std_msgs::msg::Int32()); + msg->data = count++; + std::stringstream ss; + ss << "0x" << std::hex << reinterpret_cast(msg.get()); + RCLCPP_INFO(this->get_logger(), "Published message with value: %d, and address: %s", + msg->data, ss.str().c_str()); + pub_ptr->publish(std::move(msg)); + }; + + timer_demo_ = this->create_wall_timer(1s, callback_demo); } MonoCameraNode::~MonoCameraNode() @@ -96,27 +130,21 @@ void MonoCameraNode::start() void MonoCameraNode::frameCallback(const FramePtr& vimba_frame_ptr) { rclcpp::Time ros_time = this->get_clock()->now(); - // RCLCPP_INFO(this->get_logger(), "frameCallback start"); - // getNumSubscribers() is not yet supported in Foxy, will be supported in later versions - // if (camera_info_pub_.getNumSubscribers() > 0) { sensor_msgs::msg::Image img; sensor_msgs::msg::CompressedImage compressed_image; - // RCLCPP_INFO(this->get_logger(), "frameToImage start"); if (api_.frameToImage(vimba_frame_ptr, img, compressed_image, publish_compressed_)) { - // RCLCPP_INFO(this->get_logger(), "frameToImage success"); sensor_msgs::msg::CameraInfo ci = cam_.getCameraInfo(); - // Note: getCameraInfo() doesn't fill in header frame_id or stamp ci.header.frame_id = frame_id_; VmbUint64_t frame_timestamp; vimba_frame_ptr->GetTimestamp(frame_timestamp); ci.header.stamp = rclcpp::Time(cam_.getTimestampRealTime(frame_timestamp)) + rclcpp::Duration(ptp_offset_, 0); -//std::cout << cam_.getTimestampRealTime(frame_timestamp) << std::endl; - ci.header.stamp = ros_time; + 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); + // publishImagePtr(std::make_shared(img)); if (publish_compressed_) { compressed_image.header.frame_id = ci.header.frame_id; @@ -129,7 +157,39 @@ void MonoCameraNode::frameCallback(const FramePtr& vimba_frame_ptr) RCLCPP_WARN_STREAM(this->get_logger(), "Function frameToImage returned 0. No image published."); } } - // RCLCPP_INFO(this->get_logger(), "frameCallback end"); +} + + +void MonoCameraNode::publishImagePtr(sensor_msgs::msg::Image::SharedPtr image) { + auto pub_ptr = captured_pub.lock(); + if (!pub_ptr) { + RCLCPP_ERROR(this->get_logger(), "Failed to lock publisher"); + return; + } + + // Borrow a loaned message from the publisher + auto loaned_msg = pub_ptr->borrow_loaned_message(); + auto& msg = loaned_msg.get(); + + // Move the image data instead of copying + msg.header = std::move(image->header); + msg.height = image->height; + msg.width = image->width; + msg.encoding = std::move(image->encoding); + msg.is_bigendian = image->is_bigendian; + msg.step = image->step; + msg.data = std::move(image->data); + + std::stringstream ss; + ss << "0x" << std::hex << reinterpret_cast(&msg); + + RCLCPP_INFO(this->get_logger(), + "Publishing image: %dx%d, encoding: %s, address: %s", + msg.width, msg.height, + msg.encoding.c_str(), ss.str().c_str()); + + // Publish using move semantics + pub_ptr->publish(std::move(loaned_msg)); } void MonoCameraNode::startSrvCallback(const std::shared_ptr request_header, From eadbbfd5ab09c9ef862d223fbd5e525f04dd0db4 Mon Sep 17 00:00:00 2001 From: Michael Wu Date: Thu, 21 Nov 2024 22:03:50 -0800 Subject: [PATCH 3/8] feat: image streaming example done --- .../avt_vimba_camera/mono_camera_node.hpp | 2 +- avt_vimba_camera/src/mono_camera_node.cpp | 33 +++++-------------- 2 files changed, 9 insertions(+), 26 deletions(-) diff --git a/avt_vimba_camera/include/avt_vimba_camera/mono_camera_node.hpp b/avt_vimba_camera/include/avt_vimba_camera/mono_camera_node.hpp index 999aaf9b..4b3e6334 100644 --- a/avt_vimba_camera/include/avt_vimba_camera/mono_camera_node.hpp +++ b/avt_vimba_camera/include/avt_vimba_camera/mono_camera_node.hpp @@ -89,7 +89,7 @@ class MonoCameraNode : public rclcpp::Node void loadParams(); void frameCallback(const FramePtr& vimba_frame_ptr); - void publishImagePtr(sensor_msgs::msg::Image::SharedPtr image); + void publishImagePtr(sensor_msgs::msg::Image & image); void startSrvCallback(const std::shared_ptr request_header, const std_srvs::srv::Trigger::Request::SharedPtr req, diff --git a/avt_vimba_camera/src/mono_camera_node.cpp b/avt_vimba_camera/src/mono_camera_node.cpp index d45dcb8f..9bee6aa8 100644 --- a/avt_vimba_camera/src/mono_camera_node.cpp +++ b/avt_vimba_camera/src/mono_camera_node.cpp @@ -95,7 +95,7 @@ MonoCameraNode::MonoCameraNode(const rclcpp::NodeOptions & options) pub_ptr->publish(std::move(msg)); }; - timer_demo_ = this->create_wall_timer(1s, callback_demo); + // timer_demo_ = this->create_wall_timer(1s, callback_demo); } MonoCameraNode::~MonoCameraNode() @@ -144,7 +144,7 @@ void MonoCameraNode::frameCallback(const FramePtr& vimba_frame_ptr) img.header.frame_id = ci.header.frame_id; img.header.stamp = ci.header.stamp; camera_info_pub_.publish(img, ci); - // publishImagePtr(std::make_shared(img)); + publishImagePtr(img); if (publish_compressed_) { compressed_image.header.frame_id = ci.header.frame_id; @@ -160,36 +160,19 @@ void MonoCameraNode::frameCallback(const FramePtr& vimba_frame_ptr) } -void MonoCameraNode::publishImagePtr(sensor_msgs::msg::Image::SharedPtr image) { +void MonoCameraNode::publishImagePtr(sensor_msgs::msg::Image & image) { auto pub_ptr = captured_pub.lock(); if (!pub_ptr) { RCLCPP_ERROR(this->get_logger(), "Failed to lock publisher"); return; } - - // Borrow a loaned message from the publisher - auto loaned_msg = pub_ptr->borrow_loaned_message(); - auto& msg = loaned_msg.get(); - - // Move the image data instead of copying - msg.header = std::move(image->header); - msg.height = image->height; - msg.width = image->width; - msg.encoding = std::move(image->encoding); - msg.is_bigendian = image->is_bigendian; - msg.step = image->step; - msg.data = std::move(image->data); + sensor_msgs::msg::Image::UniquePtr msg(new sensor_msgs::msg::Image(image)); std::stringstream ss; - ss << "0x" << std::hex << reinterpret_cast(&msg); - - RCLCPP_INFO(this->get_logger(), - "Publishing image: %dx%d, encoding: %s, address: %s", - msg.width, msg.height, - msg.encoding.c_str(), ss.str().c_str()); - - // Publish using move semantics - pub_ptr->publish(std::move(loaned_msg)); + ss << "0x" << std::hex << reinterpret_cast(msg.get()); + RCLCPP_INFO(this->get_logger(), "Published message with address: %s", + ss.str().c_str()); + pub_ptr->publish(std::move(msg)); } void MonoCameraNode::startSrvCallback(const std::shared_ptr request_header, From 50450673ebc8f838035ef5f8cdedca35033314ad Mon Sep 17 00:00:00 2001 From: Michael Wu Date: Thu, 5 Dec 2024 18:33:16 -0800 Subject: [PATCH 4/8] fix: remove useless code --- avt_vimba_camera/src/mono_camera_node.cpp | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/avt_vimba_camera/src/mono_camera_node.cpp b/avt_vimba_camera/src/mono_camera_node.cpp index 9bee6aa8..41641b55 100644 --- a/avt_vimba_camera/src/mono_camera_node.cpp +++ b/avt_vimba_camera/src/mono_camera_node.cpp @@ -80,22 +80,6 @@ MonoCameraNode::MonoCameraNode(const rclcpp::NodeOptions & options) pub_demo_ = this->create_publisher("image/demo", qos); captured_pub_demo = pub_demo_; - auto callback_demo = [this]() -> void { - auto pub_ptr = captured_pub_demo.lock(); - if (!pub_ptr) { - return; - } - static int32_t count = 0; - std_msgs::msg::Int32::UniquePtr msg(new std_msgs::msg::Int32()); - msg->data = count++; - std::stringstream ss; - ss << "0x" << std::hex << reinterpret_cast(msg.get()); - RCLCPP_INFO(this->get_logger(), "Published message with value: %d, and address: %s", - msg->data, ss.str().c_str()); - pub_ptr->publish(std::move(msg)); - }; - - // timer_demo_ = this->create_wall_timer(1s, callback_demo); } MonoCameraNode::~MonoCameraNode() From d9091c236de75f27ccd50c5bc391811f79c4b0e5 Mon Sep 17 00:00:00 2001 From: Michael Wu Date: Thu, 5 Dec 2024 18:39:36 -0800 Subject: [PATCH 5/8] feat: composable done --- avt_vimba_camera/CMakeLists.txt | 9 +++ .../launch/composable_test.launch.py | 55 +++++++++++++++++++ .../src/exec/intra_process_exec.cpp | 2 +- .../src/image_subscriber_node.cpp | 4 ++ avt_vimba_camera/src/mono_camera_node.cpp | 4 ++ 5 files changed, 73 insertions(+), 1 deletion(-) create mode 100644 avt_vimba_camera/launch/composable_test.launch.py diff --git a/avt_vimba_camera/CMakeLists.txt b/avt_vimba_camera/CMakeLists.txt index 012874ea..e2380093 100644 --- a/avt_vimba_camera/CMakeLists.txt +++ b/avt_vimba_camera/CMakeLists.txt @@ -58,10 +58,19 @@ ament_auto_add_library(mono_camera_node SHARED ) add_dependencies_and_linkings(mono_camera_node) +# Register the component +rclcpp_components_register_nodes(mono_camera_node + "avt_vimba_camera::MonoCameraNode") + + ament_auto_add_library(image_subscriber_node SHARED src/image_subscriber_node.cpp ) +# Add this line after the library definition +rclcpp_components_register_nodes(image_subscriber_node + "avt_vimba_camera::ImageSubscriberNode") + ament_auto_add_library(trigger_node SHARED src/trigger_node.cpp ) diff --git a/avt_vimba_camera/launch/composable_test.launch.py b/avt_vimba_camera/launch/composable_test.launch.py new file mode 100644 index 00000000..09c5961d --- /dev/null +++ b/avt_vimba_camera/launch/composable_test.launch.py @@ -0,0 +1,55 @@ +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import RegisterEventHandler, EmitEvent, TimerAction +from launch.event_handlers import OnProcessStart +from launch.substitutions import FindExecutable +from launch.actions import ExecuteProcess +import os +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + package_share_dir = get_package_share_directory('avt_vimba_camera') + params_file = os.path.join(package_share_dir, 'config', 'params.yaml') + + container = ComposableNodeContainer( + name='camera_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + # ComposableNode( + # package='avt_vimba_camera', + # plugin='avt_vimba_camera::MonoCameraNode', + # name='camera', + # parameters=[params_file], + # extra_arguments=[{'use_intra_process_comms': True}] + # ), + ComposableNode( + package='avt_vimba_camera', + plugin='avt_vimba_camera::ImageSubscriberNode', + name='image_subscriber', + extra_arguments=[{'use_intra_process_comms': True}] + ) + ], + output='screen', + ) + + # # Add a delay before starting the camera + # start_camera_cmd = ExecuteProcess( + # cmd=[FindExecutable(name='ros2'), 'service', 'call', + # '/camera/start_stream', 'std_srvs/srv/Trigger', '{}'], + # output='screen' + # ) + + # # Wait for container to be ready before starting camera + # delay_camera_start = TimerAction( + # period=2.0, # 2 second delay + # actions=[start_camera_cmd] + # ) + + return LaunchDescription([ + container, + # delay_camera_start + ]) \ No newline at end of file diff --git a/avt_vimba_camera/src/exec/intra_process_exec.cpp b/avt_vimba_camera/src/exec/intra_process_exec.cpp index f9b5271b..456d98bb 100644 --- a/avt_vimba_camera/src/exec/intra_process_exec.cpp +++ b/avt_vimba_camera/src/exec/intra_process_exec.cpp @@ -17,7 +17,7 @@ int main(int argc, char * argv[]) auto subscriber_node = std::make_shared(options); // Create a single-threaded executor - rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::executors::MultiThreadedExecutor executor; // Add both nodes to the executor executor.add_node(camera_node); diff --git a/avt_vimba_camera/src/image_subscriber_node.cpp b/avt_vimba_camera/src/image_subscriber_node.cpp index 3cd26f8a..ea7fc0d9 100644 --- a/avt_vimba_camera/src/image_subscriber_node.cpp +++ b/avt_vimba_camera/src/image_subscriber_node.cpp @@ -66,3 +66,7 @@ void ImageSubscriberNode::demoCallback(std_msgs::msg::Int32::UniquePtr msg) } // namespace avt_vimba_camera + +// Add these lines at the end of the file +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(avt_vimba_camera::ImageSubscriberNode) diff --git a/avt_vimba_camera/src/mono_camera_node.cpp b/avt_vimba_camera/src/mono_camera_node.cpp index 41641b55..ad1f4abc 100644 --- a/avt_vimba_camera/src/mono_camera_node.cpp +++ b/avt_vimba_camera/src/mono_camera_node.cpp @@ -41,6 +41,8 @@ using namespace std::chrono_literals; // Add this for time literals using namespace std::placeholders; + + namespace avt_vimba_camera { MonoCameraNode::MonoCameraNode(const rclcpp::NodeOptions & options) @@ -218,3 +220,5 @@ void MonoCameraNode::saveSrvCallback(const std::shared_ptr req } } } // namespace avt_vimba_camera +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(avt_vimba_camera::MonoCameraNode) \ No newline at end of file From 6297468290d6bbd253c00121903e7c8b50b9a0e1 Mon Sep 17 00:00:00 2001 From: Michael Wu Date: Thu, 5 Dec 2024 18:47:55 -0800 Subject: [PATCH 6/8] feat: container solution done --- .../launch/composable_test.launch.py | 28 +++++-------------- avt_vimba_camera/src/mono_camera_node.cpp | 25 +++++++++++++---- 2 files changed, 26 insertions(+), 27 deletions(-) diff --git a/avt_vimba_camera/launch/composable_test.launch.py b/avt_vimba_camera/launch/composable_test.launch.py index 09c5961d..eb8b8310 100644 --- a/avt_vimba_camera/launch/composable_test.launch.py +++ b/avt_vimba_camera/launch/composable_test.launch.py @@ -19,13 +19,13 @@ def generate_launch_description(): package='rclcpp_components', executable='component_container', composable_node_descriptions=[ - # ComposableNode( - # package='avt_vimba_camera', - # plugin='avt_vimba_camera::MonoCameraNode', - # name='camera', - # parameters=[params_file], - # extra_arguments=[{'use_intra_process_comms': True}] - # ), + ComposableNode( + package='avt_vimba_camera', + plugin='avt_vimba_camera::MonoCameraNode', + name='camera', + parameters=[params_file], + extra_arguments=[{'use_intra_process_comms': True}] + ), ComposableNode( package='avt_vimba_camera', plugin='avt_vimba_camera::ImageSubscriberNode', @@ -36,20 +36,6 @@ def generate_launch_description(): output='screen', ) - # # Add a delay before starting the camera - # start_camera_cmd = ExecuteProcess( - # cmd=[FindExecutable(name='ros2'), 'service', 'call', - # '/camera/start_stream', 'std_srvs/srv/Trigger', '{}'], - # output='screen' - # ) - - # # Wait for container to be ready before starting camera - # delay_camera_start = TimerAction( - # period=2.0, # 2 second delay - # actions=[start_camera_cmd] - # ) - return LaunchDescription([ container, - # delay_camera_start ]) \ No newline at end of file diff --git a/avt_vimba_camera/src/mono_camera_node.cpp b/avt_vimba_camera/src/mono_camera_node.cpp index ad1f4abc..1b8439f7 100644 --- a/avt_vimba_camera/src/mono_camera_node.cpp +++ b/avt_vimba_camera/src/mono_camera_node.cpp @@ -82,6 +82,12 @@ MonoCameraNode::MonoCameraNode(const rclcpp::NodeOptions & options) pub_demo_ = this->create_publisher("image/demo", qos); captured_pub_demo = pub_demo_; + try { + this->start(); + } catch (const std::exception& e) { + RCLCPP_ERROR(this->get_logger(), "Failed to start camera: %s", e.what()); + throw; + } } MonoCameraNode::~MonoCameraNode() @@ -105,12 +111,19 @@ void MonoCameraNode::loadParams() void MonoCameraNode::start() { - // Start Vimba & list all available cameras - api_.start(); - - // Start camera - cam_.start(ip_, guid_, frame_id_, camera_info_url_); - cam_.startImaging(); + RCLCPP_INFO(this->get_logger(), "Starting camera"); + try { + // Start Vimba & list all available cameras + api_.start(); + + // Start camera + cam_.start(ip_, guid_, frame_id_, camera_info_url_); + cam_.startImaging(); + } catch (const std::exception& e) { + RCLCPP_ERROR(this->get_logger(), "Error starting camera: %s", e.what()); + throw; + } + RCLCPP_INFO(this->get_logger(), "Camera started"); } void MonoCameraNode::frameCallback(const FramePtr& vimba_frame_ptr) From be6fa13dd7b9ffd4df5bea523b5274d0a2d210b8 Mon Sep 17 00:00:00 2001 From: Michael Wu Date: Thu, 5 Dec 2024 18:50:15 -0800 Subject: [PATCH 7/8] feat: composable node and intra process solution cleaned up --- avt_vimba_camera/src/exec/intra_process_exec.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/avt_vimba_camera/src/exec/intra_process_exec.cpp b/avt_vimba_camera/src/exec/intra_process_exec.cpp index 456d98bb..95ae1526 100644 --- a/avt_vimba_camera/src/exec/intra_process_exec.cpp +++ b/avt_vimba_camera/src/exec/intra_process_exec.cpp @@ -23,9 +23,6 @@ int main(int argc, char * argv[]) executor.add_node(camera_node); executor.add_node(subscriber_node); - // Start the camera after adding to executor - camera_node->start(); - // Spin the executor executor.spin(); From 24141ab8f44af690c5ff8a08f4d5f98bb4ea53cf Mon Sep 17 00:00:00 2001 From: Michael Wu Date: Sat, 7 Dec 2024 14:32:36 -0800 Subject: [PATCH 8/8] fix: updated QoS --- .vscode/c_cpp_properties.json | 18 ++++++ .vscode/launch.json | 13 ++++ .vscode/settings.json | 59 +++++++++++++++++++ .../src/image_subscriber_node.cpp | 2 +- avt_vimba_camera/src/mono_camera_node.cpp | 3 +- 5 files changed, 93 insertions(+), 2 deletions(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/launch.json create mode 100644 .vscode/settings.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 00000000..980fd575 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,18 @@ +{ + "configurations": [ + { + "name": "macos-clang-arm64", + "includePath": [ + "${workspaceFolder}/**" + ], + "compilerPath": "/usr/bin/clang", + "cStandard": "${default}", + "cppStandard": "${default}", + "intelliSenseMode": "macos-clang-arm64", + "compilerArgs": [ + "" + ] + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 00000000..08f1be44 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,13 @@ +{ + "version": "0.2.0", + "configurations": [ + { + "name": "C/C++ Runner: Debug Session", + "type": "lldb", + "request": "launch", + "args": [], + "cwd": "/Users/michaelwu/Desktop/projects/avt_vimba_camera/avt_vimba_camera/src", + "program": "/Users/michaelwu/Desktop/projects/avt_vimba_camera/avt_vimba_camera/src/build/Debug/outDebug" + } + ] +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..b9c6ac87 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,59 @@ +{ + "C_Cpp_Runner.cCompilerPath": "clang", + "C_Cpp_Runner.cppCompilerPath": "clang++", + "C_Cpp_Runner.debuggerPath": "lldb", + "C_Cpp_Runner.cStandard": "", + "C_Cpp_Runner.cppStandard": "", + "C_Cpp_Runner.msvcBatchPath": "", + "C_Cpp_Runner.useMsvc": false, + "C_Cpp_Runner.warnings": [ + "-Wall", + "-Wextra", + "-Wpedantic", + "-Wshadow", + "-Wformat=2", + "-Wcast-align", + "-Wconversion", + "-Wsign-conversion", + "-Wnull-dereference" + ], + "C_Cpp_Runner.msvcWarnings": [ + "/W4", + "/permissive-", + "/w14242", + "/w14287", + "/w14296", + "/w14311", + "/w14826", + "/w44062", + "/w44242", + "/w14905", + "/w14906", + "/w14263", + "/w44265", + "/w14928" + ], + "C_Cpp_Runner.enableWarnings": true, + "C_Cpp_Runner.warningsAsError": false, + "C_Cpp_Runner.compilerArgs": [], + "C_Cpp_Runner.linkerArgs": [], + "C_Cpp_Runner.includePaths": [], + "C_Cpp_Runner.includeSearch": [ + "*", + "**/*" + ], + "C_Cpp_Runner.excludeSearch": [ + "**/build", + "**/build/**", + "**/.*", + "**/.*/**", + "**/.vscode", + "**/.vscode/**" + ], + "C_Cpp_Runner.useAddressSanitizer": false, + "C_Cpp_Runner.useUndefinedSanitizer": false, + "C_Cpp_Runner.useLeakSanitizer": false, + "C_Cpp_Runner.showCompilationTime": false, + "C_Cpp_Runner.useLinkTimeOptimization": false, + "C_Cpp_Runner.msvcSecureNoWarnings": false +} \ No newline at end of file diff --git a/avt_vimba_camera/src/image_subscriber_node.cpp b/avt_vimba_camera/src/image_subscriber_node.cpp index ea7fc0d9..03e9dc03 100644 --- a/avt_vimba_camera/src/image_subscriber_node.cpp +++ b/avt_vimba_camera/src/image_subscriber_node.cpp @@ -10,7 +10,7 @@ ImageSubscriberNode::ImageSubscriberNode(const rclcpp::NodeOptions & options) { // Create QoS profile matching the publisher auto qos = rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 1)); - qos.reliable(); + qos.best_effort(); rclcpp::SubscriptionOptions sub_options; sub_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; diff --git a/avt_vimba_camera/src/mono_camera_node.cpp b/avt_vimba_camera/src/mono_camera_node.cpp index 1b8439f7..fb67db00 100644 --- a/avt_vimba_camera/src/mono_camera_node.cpp +++ b/avt_vimba_camera/src/mono_camera_node.cpp @@ -63,7 +63,7 @@ MonoCameraNode::MonoCameraNode(const rclcpp::NodeOptions & options) loadParams(); auto qos = rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 1)); - qos.reliable(); + qos.best_effort(); if (publish_compressed_) { compressed_pub = this->create_publisher("image/compressed", qos); @@ -106,6 +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(), "Parameters loaded"); }