Consider region of interest in CameraDisplay

This feature was already implemented in Rviz1

rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp

@@ -500,8 +500,9 @@ bool CameraDisplay::updateCamera()
   setStatus(StatusLevel::Ok, CAM_INFO_STATUS, "OK");
 
   // adjust the image rectangles to fit the zoom & aspect ratio
-  background_screen_rect_->setCorners(-1.0f * zoom.x, 1.0f * zoom.y, 1.0f * zoom.x, -1.0f * zoom.y);
-  overlay_screen_rect_->setCorners(-1.0f * zoom.x, 1.0f * zoom.y, 1.0f * zoom.x, -1.0f * zoom.y);
+  Ogre::Vector4 corners = calculateScreenCorners(info, zoom);
+  background_screen_rect_->setCorners(corners.x, corners.y, corners.z, corners.w);
+  overlay_screen_rect_->setCorners(corners.x, corners.y, corners.z, corners.w);
 
   Ogre::AxisAlignedBox aabInf;
   aabInf.setInfinite();
@@ -620,6 +621,28 @@ Ogre::Matrix4 CameraDisplay::calculateProjectionMatrix(
   return proj_matrix;
 }
 
+static Ogre::Vector4 calculateScreenCorners(
+  const sensor_msgs::msg::CameraInfo::ConstSharedPtr info,
+  const Ogre::Vector2 & zoom)
+{
+  float x_corner_start, y_corner_start, x_corner_end, y_corner_end;
+
+  if (info->roi.height != 0 || info->roi.width != 0) {
+    // corners are computed according to roi
+    x_corner_start = (2.0 * info->roi.x_offset / info->width - 1.0) * zoom.x;
+    y_corner_start = (-2.0 * info->roi.y_offset / info->height + 1.0) * zoom.y;
+    x_corner_end = x_corner_start + (2.0 * info->roi.width / info->width) * zoom.x;
+    y_corner_end = y_corner_start - (2.0 * info->roi.height / info->height) * zoom.y;
+  } else {
+    x_corner_start = -1.0f * zoom.x;
+    y_corner_start = 1.0f * zoom.y;
+    x_corner_end = 1.0f * zoom.x;
+    y_corner_end = -1.0f * zoom.y;
+  }
+
+  return {x_corner_start, y_corner_start, x_corner_end, y_corner_end};
+}
+
 void CameraDisplay::processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg)
 {
   texture_->addMessage(msg);