Enable selection of sensor resolution

Author: Woojin-Crive

README.md

@@ -123,20 +123,32 @@ The camera stream is configured once when the node starts via the following stat
 | `role`            | `string`              | configures the camera with a `StreamRole` (possible choices: `raw`, `still`, `video`, `viewfinder`) [default: `viewfinder`] |
 | `format`          | `string`              | a `PixelFormat` that is supported by the camera [default: auto]                                                             |
 | `width`, `height` | `integer`             | desired image resolution [default: auto]                                                                                    |
+| `sensor`          | `string` in format `width:height`   | desired raw sensor format resolution [default: auto]                                                            |
 
 
 The configuration is done in the following order:
 1. select camera via `camera`
 2. configure camera stream via `role`
 3. set the pixel format for the stream via `format`
 4. set the image resolution for the stream via `width` and `height`
+5. set the sensor mode resolution for the raw feed from camera to GPU
 
 Each stream role only supports a discrete set of data stream configurations as a combination of the image resolution and pixel format. The selected stream configuration is validated at the end of this sequence and adjusted to the closest valid stream configuration.
 
 By default, the node will select the first available camera and configures it with the default pixel format and image resolution. If a parameter has not been set, the node will print the available options and inform the user about the default choice.
 
 The node avoids memory copies of the image data by directly mapping from a camera pixel format to a ROS image format, with the exception of converting between "raw" and "compressed" image formats when requested by the user. As an effect, not all pixel formats that are supported by the camera may be supported by the ROS image message. Hence, the options for `format` are limited to pixel formats that are supported by the camera and the raw ROS image message.
 
+#### Sensor Modes and Cropping
+
+Some camera modules (e.g. Raspberry Pi Camera Modules) provides different sensor modes, some of which have a limited field-of-view, e.g. by allowing for the selection of a 3:2 image from a 16:9 sensor, a more native 'zoom' function etc. Unless we specify the sensor mode we want to use libcamera will automatically select one, and it is not guaranteed that this will be a full sensor mode, leading to cropping (which provides a zooming effect) of the picture.
+
+As an example: Picking the commonly used 640x480 resolution with the Raspberry Pi Camera Module v2 gives a zooming effect of ~2.5x, greatly reducing the FoV. With the v1 Module there is no such zooming effect as the v1 camera only offers full FoV modes in the 4:3 aspect ratio.
+
+With the Camera Module v2 it is suggested to use a sensor resolution of 1640x1232 when one needs lower resolution 4:3 images (e.g. 640x480, 480x320 etc.), and 1640x922 for lower resolution 16:9 images.
+
+See the [PiCamera Documentation](https://picamera.readthedocs.io/en/release-1.13/fov.html#sensor-modes) for a visualization of the issue, and [section 4.2.2.3 in the PiCamera2 Documentation](https://datasheets.raspberrypi.com/camera/picamera2-manual.pdf) for further explanations.
+
 ### Dynamic Frame Configuration (controls)
 
 The dynamic parameters are created at runtime by inspecting the [controls](https://libcamera.org/api-html/namespacelibcamera_1_1controls.html) that are exposed by the camera. The set of exposed controls is camera-dependent. The ROS parameters are directly formatted from the exposed controls, hence the parameter names match the control names.

src/CameraNode.cpp

@@ -150,6 +150,43 @@ get_role(const std::string &role)
   }
 }
 
+libcamera::Size
+get_sensor_format(const std::string &format_str)
+{
+  if (format_str.empty()) {
+    return libcamera::Size();
+  }
+
+  size_t delimiter_pos = format_str.find(':');
+  if (delimiter_pos == std::string::npos) {
+    throw std::runtime_error("Invalid sensor parameter: '" + format_str +
+                             "'. Expected format 'width:height'");
+  }
+
+  std::string width_str = format_str.substr(0, delimiter_pos);
+  std::string height_str = format_str.substr(delimiter_pos + 1);
+
+  // Check that width contains only digits
+  if (!std::all_of(width_str.begin(), width_str.end(), [](unsigned char c) { return std::isdigit(c); })) {
+    throw std::runtime_error("Width in sensor parameter '" + format_str +
+                             "' must contain only digits");
+  }
+
+  // Check that height contains only digits
+  if (!std::all_of(height_str.begin(), height_str.end(), [](unsigned char c) { return std::isdigit(c); })) {
+    throw std::runtime_error("Height in sensor parameter '" + format_str +
+                             "' must contain only digits");
+  }
+
+  try {
+    const uint32_t width = std::stoul(width_str);
+    const uint32_t height = std::stoul(height_str);
+    return libcamera::Size {width, height};
+  }
+  catch (const std::exception &e) {
+    throw std::runtime_error("Failed to parse sensor parameter '" + format_str + "': " + e.what());
+  }
+}
 
 // The following function "compressImageMsg" is adapted from "CvImage::toCompressedImageMsg"
 // (https://github.com/ros-perception/vision_opencv/blob/066793a23e5d06d76c78ca3d69824a501c3554fd/cv_bridge/src/cv_bridge.cpp#L512-L535)
@@ -221,6 +258,9 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options)
   const uint32_t h = declare_parameter<int64_t>("height", {}, param_descr_ro);
   const libcamera::Size size {w, h};
 
+  // Raw format dimensions
+  libcamera::Size sensor_size = get_sensor_format(declare_parameter<std::string>("sensor", "", param_descr_ro));
+
   // camera info file url
   rcl_interfaces::msg::ParameterDescriptor param_descr_camera_info_url;
   param_descr_camera_info_url.description = "camera calibration info file url";
@@ -299,18 +339,26 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options)
   if (camera->acquire())
     throw std::runtime_error("failed to acquire camera");
 
+  std::vector<libcamera::StreamRole> roles;
+  roles.push_back(role);
+
+  // Add the RAW role if the sensor_size is defined
+  if (!sensor_size.isNull() && roles.front() != libcamera::StreamRole::Raw) {
+    roles.push_back(libcamera::StreamRole::Raw);
+  }
+
   // configure camera stream
   std::unique_ptr<libcamera::CameraConfiguration> cfg =
-    camera->generateConfiguration({role});
+    camera->generateConfiguration(roles);
 
   if (!cfg)
     throw std::runtime_error("failed to generate configuration");
 
-  assert(cfg->size() == 1);
+  assert(cfg->size() >= 1);
   libcamera::StreamConfiguration &scfg = cfg->at(0);
 
   // list all camera formats, including those not supported by the ROS message
-  RCLCPP_DEBUG_STREAM(get_logger(), "default " << role << " stream configuration: \"" << scfg.toString() << "\"");
+  RCLCPP_DEBUG_STREAM(get_logger(), "default " << roles.front() << " stream configuration: \"" << scfg.toString() << "\"");
   RCLCPP_DEBUG_STREAM(get_logger(), scfg.formats());
 
   // get common pixel formats that are supported by the camera and the node
@@ -357,6 +405,12 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options)
     scfg.size = size;
   }
 
+  if (!sensor_size.isNull() && roles.front() != libcamera::StreamRole::Raw) {
+    libcamera::StreamConfiguration &modecfg = cfg->at(1);
+    modecfg.size = sensor_size;
+    RCLCPP_INFO_STREAM(get_logger(), "Sensor mode configuration: " << modecfg.toString());
+  }
+
   // store selected stream configuration
   const libcamera::StreamConfiguration selected_scfg = scfg;
 
@@ -392,6 +446,8 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options)
   const std::optional<std::string> model = props.get(libcamera::properties::Model);
   if (model)
     cname = model.value() + '_' + cname;
+  if (!sensor_size.isNull())
+    cname = cname + '_' + cfg->at(1).toString();
 
   // clean camera name of non-alphanumeric characters
   cname.erase(