AfWindows

CMakeLists.txt

@@ -60,6 +60,12 @@ target_link_libraries(camera_component ${libcamera_LINK_LIBRARIES} utils)
 install(TARGETS camera_component
   DESTINATION lib)
 
+# install the launch directory
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
 if(BUILD_TESTING)
   set(ament_cmake_clang_format_CONFIG_FILE "${CMAKE_SOURCE_DIR}/.clang-format")
   find_package(ament_lint_auto REQUIRED)

launch/camera_launch.py

@@ -0,0 +1,34 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, LogInfo
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("width", default_value="640", description="Camera image width"),
+            DeclareLaunchArgument("height", default_value="320", description="Camera image height"),
+            DeclareLaunchArgument("log_level", default_value="debug", description="Log level (e.g., debug)"),
+            # You can add other launch arguments as needed.
+            # Run the camera_node with the specified parameters.
+            Node(
+                package="camera_ros",
+                executable="camera_node",
+                name="camera",
+                output="screen",  # Adjust the output as needed.
+                parameters=[
+                    {"width": LaunchConfiguration("width")},
+                    {"height": LaunchConfiguration("height")},
+                    {"mode_width": 2304},
+                    {"mode_height": 1296},
+                    {"qos_overrides./camera/image_raw.publisher.reliability": "best_effort"},
+                    {"qos_overrides./camera/image_raw/compressed.publisher.reliability": "best_effort"},
+                    # {"qos_overrides./camera/camera_info.publisher.reliability": "best_effort"},
+                ],
+                remappings=[],  # Add remappings if necessary.
+                arguments=["--log-level", LaunchConfiguration("log_level")],
+            ),
+            # You can add additional nodes or actions here as needed.
+        ]
+    )

src/CameraNode.cpp

@@ -21,6 +21,7 @@
 #include <libcamera/base/signal.h>
 #include <libcamera/base/span.h>
 #include <libcamera/camera.h>
+#include <libcamera/formats.h>
 #include <libcamera/camera_manager.h>
 #include <libcamera/controls.h>
 #include <libcamera/framebuffer.h>
@@ -161,14 +162,26 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti
   declare_parameter<int64_t>("width", {}, param_descr_ro);
   declare_parameter<int64_t>("height", {}, param_descr_ro);
 
+  declare_parameter<int64_t>("mode_width", {}, param_descr_ro);
+  declare_parameter<int64_t>("mode_height", {}, param_descr_ro);
+
   // camera ID
   declare_parameter("camera", rclcpp::ParameterValue {}, param_descr_ro.set__dynamic_typing(true));
 
+  auto qos_override_options = rclcpp::QosOverridingOptions({
+    rclcpp::QosPolicyKind::Depth,
+    rclcpp::QosPolicyKind::History,
+    rclcpp::QosPolicyKind::Reliability,
+    rclcpp::QosPolicyKind::Durability,
+  });
+
+  rclcpp::PublisherOptions pub_options;
+  pub_options.qos_overriding_options = qos_override_options;
   // publisher for raw and compressed image
-  pub_image = this->create_publisher<sensor_msgs::msg::Image>("~/image_raw", 1);
+  pub_image = this->create_publisher<sensor_msgs::msg::Image>("~/image_raw", 1, pub_options);
   pub_image_compressed =
-    this->create_publisher<sensor_msgs::msg::CompressedImage>("~/image_raw/compressed", 1);
-  pub_ci = this->create_publisher<sensor_msgs::msg::CameraInfo>("~/camera_info", 1);
+    this->create_publisher<sensor_msgs::msg::CompressedImage>("~/image_raw/compressed", 1, pub_options);
+  pub_ci = this->create_publisher<sensor_msgs::msg::CameraInfo>("~/camera_info", 1, pub_options);
 
   // start camera manager and check for cameras
   camera_manager.start();
@@ -216,9 +229,18 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti
   if (camera->acquire())
     throw std::runtime_error("failed to acquire camera");
 
+  std::vector<libcamera::StreamRole> roles;
+
+  roles.push_back(get_role(get_parameter("role").as_string()));
+  const libcamera::Size mode_size(get_parameter("mode_width").as_int(), get_parameter("mode_height").as_int());
+
+  if(!mode_size.isNull()) {
+    roles.push_back(libcamera::StreamRole::Raw);
+  }
+
   // configure camera stream
   std::unique_ptr<libcamera::CameraConfiguration> cfg =
-    camera->generateConfiguration({get_role(get_parameter("role").as_string())});
+    camera->generateConfiguration({roles});
 
   if (!cfg)
     throw std::runtime_error("failed to generate configuration");
@@ -266,6 +288,7 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti
   }
 
   const libcamera::Size size(get_parameter("width").as_int(), get_parameter("height").as_int());
+
   if (size.isNull()) {
     RCLCPP_INFO_STREAM(get_logger(), scfg);
     scfg.size = scfg.formats().sizes(scfg.pixelFormat).back();
@@ -276,6 +299,20 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti
     scfg.size = size;
   }
 
+  if(!mode_size.isNull()) {
+    RCLCPP_INFO_STREAM(get_logger(), mode_size);
+    libcamera::StreamConfiguration &modecfg = cfg->at(1);
+    modecfg.size = mode_size;
+
+    modecfg.pixelFormat = libcamera::formats::SBGGR12_CSI2P;
+
+    RCLCPP_INFO_STREAM(get_logger(), "modecfg ");
+    RCLCPP_INFO_STREAM(get_logger(), modecfg);
+
+    set_parameter(rclcpp::Parameter("mode_width", int64_t(modecfg.size.width)));
+    set_parameter(rclcpp::Parameter("mode_height", int64_t(modecfg.size.height)));
+  }
+
   // store selected stream configuration
   const libcamera::StreamConfiguration selected_scfg = scfg;
 
@@ -394,6 +431,8 @@ CameraNode::declareParameters()
   // dynamic camera configuration
   ParameterMap parameters_init;
   for (const auto &[id, info] : camera->controls()) {
+    if (id->name() == "AfWindows")
+      continue;
     // store control id with name
     parameter_ids[id->name()] = id;