Camera: forward port #81 to ros2

models/gimbal_small_3d/model.sdf

@@ -207,7 +207,8 @@
 
         <plugin filename="CameraZoomPlugin"
             name="CameraZoomPlugin">
-          <max_zoom>15.0</max_zoom>
+          <max_zoom>125.0</max_zoom>
+          <slew_rate>0.42514285714</slew_rate>
         </plugin>
 
       </sensor>

src/CameraZoomPlugin.cc

@@ -17,7 +17,10 @@
 
 #include "CameraZoomPlugin.hh"
 
+#include <algorithm>
 #include <atomic>
+#include <cmath>
+#include <limits>
 #include <mutex>
 #include <string>
 
@@ -99,7 +102,6 @@ class CameraZoomPlugin::Impl
     return std::optional<sim::Entity>(parent->Data());
   }
 
-
   /// \brief World occupied by the parent model.
   public: World world{kNullEntity};
 
@@ -123,15 +125,22 @@ class CameraZoomPlugin::Impl
   /// \brief Value of the most recently received zoom command.
   public: std::atomic<double> zoomCommand{1.0};
 
-  /// \brief Current horizontal field of view (radians).
-  public: double hfov{2.0};
+  /// \brief Reference horizontal field of view (radians).
+  public: double refHfov{2.0};
+
+  /// \brief Goal horizontal field of view (radians).
+  public: std::atomic<double> goalHfov{2.0};
 
-  /// \brief Zoom factor.
-  public: double zoom{1.0};
+  /// \brief Current zoom factor.
+  public: double curZoom{1.0};
 
   /// \brief Maximum zoom factor.
   public: double maxZoom{10.0};
 
+  /// \brief Slew rate (meters change in focal length per second).
+  ///        Default: infinity, which causes instant changes in focal length.
+  public: double slewRate{std::numeric_limits<double>::infinity()};
+
   /// \brief Minimum zoom factor == 1.0.
   public: static constexpr double minZoom{1.0};
 
@@ -152,6 +161,29 @@ class CameraZoomPlugin::Impl
 
   /// \brief Pointer to the rendering camera
   public: rendering::CameraPtr camera;
+
+  /// \brief Convert from focal length to FOV for a rectilinear lens
+  /// \ref https://en.wikipedia.org/wiki/Focal_length
+  /// @param sensorWidth Diagonal sensor width [meter]
+  /// @param focalLength The focal length [meter]
+  /// @return The field of view [rad]
+  public: static double FocalLengthToFov(
+      double sensorWidth, double focalLength);
+
+  /// \brief Convert from FOV to focal length for a rectilinear lens
+  /// \ref https://en.wikipedia.org/wiki/Focal_length
+  /// @param sensorWidth Diagonal sensor width [meter]
+  /// @param fov The field of view [rad]
+  /// @return The focal length [meter]
+  public: static double FovToFocalLength(
+      double sensorWidth, double fov);
+
+  /// @brief Compute diagonal sensor width given focal length and FOV
+  /// @param focalLength Focal length [meter]
+  /// @param fov Field of view [rad]
+  /// @return Sensor width [m]
+  public: static double SensorWidth(
+      double focalLength, double fov);
 };
 
 //////////////////////////////////////////////////
@@ -215,6 +247,29 @@ void CameraZoomPlugin::Impl::OnRenderTeardown()
   this->isValidConfig = false;
 }
 
+//////////////////////////////////////////////////
+double CameraZoomPlugin::Impl::FocalLengthToFov(
+    double sensorWidth, double focalLength)
+{
+  return 2 * std::atan2(sensorWidth, 2 * focalLength);
+}
+
+//////////////////////////////////////////////////
+double CameraZoomPlugin::Impl::FovToFocalLength(
+    double sensorWidth, double fov)
+{
+  // This is derived from FocalLengthToFov.
+  return sensorWidth / (2 * std::tan(fov / 2));
+}
+
+//////////////////////////////////////////////////
+double CameraZoomPlugin::Impl::SensorWidth(
+    double focalLength, double fov)
+{
+  // This is derived from FocalLengthToFov.
+  return 2 * std::tan(fov / 2) * focalLength;
+}
+
 //////////////////////////////////////////////////
 //////////////////////////////////////////////////
 CameraZoomPlugin::~CameraZoomPlugin() = default;
@@ -294,6 +349,10 @@ void CameraZoomPlugin::Configure(
   {
     this->impl->maxZoom = _sdf->Get<double>("max_zoom");
   }
+  if (_sdf->HasElement("slew_rate"))
+  {
+    this->impl->slewRate = _sdf->Get<double>("slew_rate");
+  }
 
   // Configure zoom command topic.
   {
@@ -330,7 +389,7 @@ void CameraZoomPlugin::Configure(
 
 //////////////////////////////////////////////////
 void CameraZoomPlugin::PreUpdate(
-    const UpdateInfo &/*_info*/,
+    const UpdateInfo &_info,
     EntityComponentManager &_ecm)
 {
   GZ_PROFILE("CameraZoomPlugin::PreUpdate");
@@ -352,31 +411,76 @@ void CameraZoomPlugin::PreUpdate(
   if (!comp)
     return;
 
-  if (!this->impl->zoomChanged)
-    return;
+  if (this->impl->zoomChanged)
+  {
+    // Only calculate goal once each time zoom is changed.
+    const auto requestedZoomCmd = this->impl->zoomCommand.load();
+    const auto clampedZoomCmd = std::clamp(requestedZoomCmd,
+      this->impl->minZoom, this->impl->maxZoom);
+    if (std::abs(requestedZoomCmd - clampedZoomCmd) >
+      std::numeric_limits<double>::epsilon())
+    {
+      gzwarn << "Requested zoom command of " << requestedZoomCmd
+        << " has been clamped to " << clampedZoomCmd << ".\n";
+    }
+    this->impl->goalHfov = this->impl->refHfov / clampedZoomCmd;
+    this->impl->zoomChanged = false;
+  }
 
   // Update component.
   sdf::Sensor &sensor = comp->Data();
   sdf::Camera *cameraSdf = sensor.CameraSensor();
+  if (!cameraSdf)
+    return;
+
+  const auto oldHfov = cameraSdf->HorizontalFov().Radian();
+
+  // Goal is achieved, nothing to update.
+  if (std::abs(this->impl->goalHfov - oldHfov) <
+    std::numeric_limits<double>::epsilon())
+    return;
+
+  const auto curFocalLength = cameraSdf->LensFocalLength();
+
+  // This value should be static every iteration.
+  const auto sensorWidth = CameraZoomPlugin::Impl::SensorWidth(
+    curFocalLength, oldHfov);
+  const auto goalFocalLength = CameraZoomPlugin::Impl::FovToFocalLength(
+    sensorWidth, this->impl->goalHfov);
 
-  this->impl->zoom = std::max(std::min(this->impl->zoomCommand.load(),
-      this->impl->maxZoom), this->impl->minZoom);
-  math::Angle oldHfov = cameraSdf->HorizontalFov();
-  math::Angle newHfov = this->impl->hfov / this->impl->zoom;
+  double newFocalLength;
+  if (std::isfinite(this->impl->slewRate)) {
+    // This is the amount of time passed since the last update.
+    const auto dt = _info.dt;
+    // How many meters the focal length could change per update loop
+    const auto maxFocalLengthChange = this->impl->slewRate *
+      std::chrono::duration<double>(dt).count();
 
+    // How many meters the focal length should change this iteration
+    const auto deltaFL = std::min(maxFocalLengthChange,
+      std::abs(curFocalLength - goalFocalLength));
+
+    if (goalFocalLength > curFocalLength)
+    {
+      newFocalLength = curFocalLength + deltaFL;
+    }
+    else
+    {
+      newFocalLength = curFocalLength - deltaFL;
+    }
+  } else {
+    newFocalLength = goalFocalLength;
+  }
+
+  const auto newHfov = CameraZoomPlugin::Impl::FocalLengthToFov(
+      sensorWidth, newFocalLength);
+  // Update rendering camera with the latest focal length.
   cameraSdf->SetHorizontalFov(newHfov);
   _ecm.SetChanged(cameraEntity, components::Camera::typeId,
-        ComponentState::OneTimeChange);
+    ComponentState::OneTimeChange);
 
   // Update rendering camera.
   this->impl->camera->SetHFOV(newHfov);
-
-  gzdbg << "CameraZoomPlugin:\n"
-        << "Zoom:     " << this->impl->zoom << "\n"
-        << "Old HFOV: " << oldHfov << " rad\n"
-        << "New HFOV: " << newHfov << " rad\n";
-
-  this->impl->zoomChanged = false;
 }
 
 //////////////////////////////////////////////////