Merge pull request #10 from aliddell/8-add-launch-file-for-oryx-camera

Add launch file for Oryx camera

acquire_zarr/launch/blackfly_to_zarr.launch.py

@@ -35,7 +35,7 @@ def generate_launch_description():
         package='acquire_zarr', 
         executable='zarr_writer_node_exe', 
         parameters=[{
-            'zarr_path': zarr_out_path,
+            'zarr_out_path': zarr_out_path,
             'dimension_sizes': [0, image_height, image_width],
             'chunk_sizes': [1, image_height, image_width],
         }],

acquire_zarr/launch/oryx_to_zarr.launch.py

@@ -0,0 +1,62 @@
+from launch_ros.substitutions import FindPackageShare
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument as LaunchArg
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration as LaunchConfig
+
+oryx_parameters = {
+    'debug': False,
+    'compute_brightness': False,
+    'adjust_timestamp': True,
+    'dump_node_map': False,
+    'frame_rate_auto': 'Off',
+    'buffer_queue_size': 10,
+}
+
+def launch_setup(context, *args, **kwargs):
+    """Launch camera driver node."""
+    parameter_file = PathJoinSubstitution(
+            [FindPackageShare('spinnaker_camera_driver'), 'config', 'oryx.yaml']
+        )
+
+    camera_node = Node(
+        package='spinnaker_camera_driver',
+        executable='camera_driver_node',
+        output='screen',
+        name='oryx',
+        parameters=[
+            oryx_parameters,
+            {
+                'ffmpeg_image_transport.encoding': 'hevc_nvenc',
+                'parameter_file': parameter_file,
+                'serial_number': [LaunchConfig('serial')], #"'23011970'",
+            },
+        ],
+    )
+
+    zarr_writer_node = Node(
+        package='acquire_zarr', 
+        executable='zarr_writer_node_exe', 
+        parameters=[{
+            'zarr_out_path': LaunchConfig('zarr_out_path'),
+            'dimension_sizes': [0, LaunchConfig('image_height'), LaunchConfig('image_width')],
+            'chunk_sizes': [1, LaunchConfig('image_height'), LaunchConfig('image_width')],
+        }],
+        remappings=[
+            ('image_raw', '/oryx/image_raw'),
+        ],
+        output='screen'
+    )
+
+    return [camera_node, zarr_writer_node]
+
+def generate_launch_description():
+    return LaunchDescription([
+        LaunchArg('serial', default_value="'23011970'", description='Camera serial number'),
+        LaunchArg('image_width', default_value='2448', description='Image width'),
+        LaunchArg('image_height', default_value='2048', description='Image height'),
+        LaunchArg('zarr_out_path', default_value='/tmp/out.zarr', description='zarr output path'),
+        OpaqueFunction(function=launch_setup)
+    ])

acquire_zarr/src/zarr_writer_node.cpp

@@ -11,12 +11,11 @@
 
 namespace acquire_zarr
 {
-  ZarrWriterNode::ZarrWriterNode(const rclcpp::NodeOptions node_options): Node("zarr_writer_node", node_options)
+  ZarrWriterNode::ZarrWriterNode(const rclcpp::NodeOptions node_options) : Node("zarr_writer_node", node_options)
   {
 
     settings_from_params();
 
-
     // manually enable topic statistics via options
     auto options = rclcpp::SubscriptionOptions();
     options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable;
@@ -27,34 +26,37 @@ namespace acquire_zarr
     // configure the topic name (default '/statistics')
     // options.topic_stats_options.publish_topic = "/topic_statistics";
 
-    auto callback = [this](const sensor_msgs::msg::Image & msg) {
-        this->topic_callback(msg);
-      };
+    auto callback = [this](const sensor_msgs::msg::Image &msg)
+    {
+      this->topic_callback(msg);
+    };
 
     subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
-      "image_raw", 10, callback, options);
-
+        "image_raw", 10, callback, options);
   }
 
   ZarrWriterNode::~ZarrWriterNode()
   {
     ZarrStreamSettings_destroy_dimension_array(&zarr_stream_settings_);
+    if (zarr_stream_ != nullptr)
+    {
+      ZarrStream_destroy(zarr_stream_);
+    }
   }
 
   void ZarrWriterNode::settings_from_params()
   {
 
     zarr_stream_settings_.version = ZarrVersion_2;
 
-    this->declare_parameter<std::string>("store_path", "out.zarr");
-    store_path_ = this->get_parameter("store_path").as_string();
+    this->declare_parameter<std::string>("zarr_out_path", "out.zarr");
+    store_path_ = this->get_parameter("zarr_out_path").as_string();
     zarr_stream_settings_.store_path = store_path_.c_str();
 
     this->declare_parameter<int>("data_type", (int)ZarrDataType_uint8);
     auto data_type = this->get_parameter("data_type").as_int();
     this->zarr_stream_settings_.data_type = (ZarrDataType)data_type;
 
-
     this->declare_parameter("dimension_names", std::vector<std::string>{"t", "y", "x"});
     dimension_names_ = this->get_parameter("dimension_names").as_string_array();
     auto n_dimensions = dimension_names_.size();
@@ -72,28 +74,26 @@ namespace acquire_zarr
     auto dimension_shard_chunks = this->get_parameter("dimension_shard_chunks").as_integer_array();
 
     ZarrStreamSettings_create_dimension_array(&zarr_stream_settings_, n_dimensions);
-    for(size_t i = 0; i < n_dimensions; i++)
+    for (size_t i = 0; i < n_dimensions; i++)
     {
 
       zarr_stream_settings_.dimensions[i] = {
-        dimension_names_[i].c_str(),
-        (ZarrDimensionType)dimension_types[i],
-        (uint32_t)dimension_sizes[i],
-        (uint32_t)dimension_chunk_px_sizes[i],
-        (uint32_t)dimension_shard_chunks[i]
-      };
+          dimension_names_[i].c_str(),
+          (ZarrDimensionType)dimension_types[i],
+          (uint32_t)dimension_sizes[i],
+          (uint32_t)dimension_chunk_px_sizes[i],
+          (uint32_t)dimension_shard_chunks[i]};
     }
 
     zarr_stream_ = ZarrStream_create(&zarr_stream_settings_);
-
   }
 
-  void ZarrWriterNode::topic_callback(const sensor_msgs::msg::Image & img) const
+  void ZarrWriterNode::topic_callback(const sensor_msgs::msg::Image &img) const
   {
     size_t size_out = 0;
     ZarrStream_append(zarr_stream_, img.data.data(), img.data.size(), &size_out);
   }
-}  // namespace acquire_zarr
+} // namespace acquire_zarr
 
 #include "rclcpp_components/register_node_macro.hpp"