Include MoveTo Helper class to ign-rendering (#311)

* Include MoveTo Helper class to ign-rendering Signed-off-by: ahcorde <[email protected]> * make linters happy Signed-off-by: ahcorde <[email protected]> * Added feedback Signed-off-by: ahcorde <[email protected]> * Added MoveToHelper test Signed-off-by: ahcorde <[email protected]> * Added feedback Signed-off-by: ahcorde <[email protected]> * timeout as arg Signed-off-by: ahcorde <[email protected]>
gazebosim · May 25, 2021 · e14d708 · e14d708
1 parent d3eb5ca
commit e14d708
Show file tree

Hide file tree

Showing 3 changed files with 461 additions and 0 deletions.
diff --git a/include/ignition/rendering/MoveToHelper.hh b/include/ignition/rendering/MoveToHelper.hh
@@ -0,0 +1,99 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef IGNITION_RENDERING_MOVETOHELPER_HH_
+#define IGNITION_RENDERING_MOVETOHELPER_HH_
+
+#include <memory>
+
+#include <ignition/common/Animation.hh>
+#include <ignition/common/KeyFrame.hh>
+
+#include <ignition/math/Box.hh>
+#include <ignition/math/Pose3.hh>
+
+#include "ignition/rendering/Camera.hh"
+
+namespace ignition
+{
+  namespace rendering
+  {
+    // Inline bracket to help doxygen filtering.
+    inline namespace IGNITION_RENDERING_VERSION_NAMESPACE {
+      // forward declaration
+      class MoveToHelperPrivate;
+
+      /// \brief Helper class for animating a user camera to move to a target
+      /// entity
+      class IGNITION_RENDERING_VISIBLE MoveToHelper
+      {
+        public: MoveToHelper();
+
+        public: ~MoveToHelper();
+
+        /// \brief Move the camera to look at the specified target
+        /// param[in] _camera Camera to be moved
+        /// param[in] _target Target to look at
+        /// param[in] _duration Duration of the move to animation, in seconds.
+        /// param[in] _onAnimationComplete Callback function when animation is
+        /// complete
+        public: void MoveTo(const rendering::CameraPtr &_camera,
+            const rendering::NodePtr &_target, double _duration,
+            std::function<void()> _onAnimationComplete);
+
+        /// \brief Move the camera to the specified pose.
+        /// param[in] _camera Camera to be moved
+        /// param[in] _target Pose to move to
+        /// param[in] _duration Duration of the move to animation, in seconds.
+        /// param[in] _onAnimationComplete Callback function when animation is
+        /// complete
+        public: void MoveTo(const rendering::CameraPtr &_camera,
+            const math::Pose3d &_target, double _duration,
+            std::function<void()> _onAnimationComplete);
+
+        /// \brief Move the camera to look at the specified target
+        /// param[in] _camera Camera to be moved
+        /// param[in] _direction The pose to assume relative to the
+        /// entit(y/ies), (0, 0, 0) indicates to return the camera back to the
+        /// home pose originally loaded in from the sdf.
+        /// param[in] _duration Duration of the move to animation, in seconds.
+        /// param[in] _onAnimationComplete Callback function when animation is
+        /// complete
+        public: void LookDirection(const rendering::CameraPtr &_camera,
+            const math::Vector3d &_direction, const math::Vector3d &_lookAt,
+            double _duration, std::function<void()> _onAnimationComplete);
+
+        /// \brief Add time to the animation.
+        /// \param[in] _time Time to add in seconds
+        public: void AddTime(double _time);
+
+        /// \brief Get whether the move to helper is idle, i.e. no animation
+        /// is being executed.
+        /// \return True if idle, false otherwise
+        public: bool Idle() const;
+
+        /// \brief Set the initial camera pose
+        /// param[in] _pose The init pose of the camera
+        public: void SetInitCameraPose(const math::Pose3d &_pose);
+
+        IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING
+        private: std::unique_ptr<MoveToHelperPrivate> dataPtr;
+        IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING
+      };
+    }
+  }
+}
+#endif
diff --git a/src/MoveToHelper.cc b/src/MoveToHelper.cc
@@ -0,0 +1,205 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include "ignition/rendering/MoveToHelper.hh"
+
+#include <memory>
+
+#include <ignition/common/Animation.hh>
+#include <ignition/common/KeyFrame.hh>
+#include <ignition/math/Pose3.hh>
+
+#include "ignition/rendering/Camera.hh"
+
+class ignition::rendering::MoveToHelperPrivate
+{
+  /// \brief Pose animation object
+  public: std::unique_ptr<common::PoseAnimation> poseAnim;
+
+  /// \brief Pointer to the camera being moved
+  public: rendering::CameraPtr camera;
+
+  /// \brief Callback function when animation is complete.
+  public: std::function<void()> onAnimationComplete;
+
+  /// \brief Initial pose of the camera used for view angles
+  public: math::Pose3d initCameraPose;
+};
+
+using namespace ignition;
+using namespace rendering;
+
+////////////////////////////////////////////////
+MoveToHelper::MoveToHelper() :
+  dataPtr(new MoveToHelperPrivate)
+{
+}
+
+////////////////////////////////////////////////
+MoveToHelper::~MoveToHelper() = default;
+
+////////////////////////////////////////////////
+void MoveToHelper::MoveTo(const rendering::CameraPtr &_camera,
+    const ignition::math::Pose3d &_target,
+    double _duration, std::function<void()> _onAnimationComplete)
+{
+  this->dataPtr->camera = _camera;
+  this->dataPtr->poseAnim = std::make_unique<common::PoseAnimation>(
+      "move_to", _duration, false);
+  this->dataPtr->onAnimationComplete = std::move(_onAnimationComplete);
+
+  math::Pose3d start = _camera->WorldPose();
+
+  common::PoseKeyFrame *key = this->dataPtr->poseAnim->CreateKeyFrame(0);
+  key->Translation(start.Pos());
+  key->Rotation(start.Rot());
+
+  key = this->dataPtr->poseAnim->CreateKeyFrame(_duration);
+  if (_target.Pos().IsFinite())
+    key->Translation(_target.Pos());
+  else
+    key->Translation(start.Pos());
+
+  if (_target.Rot().IsFinite())
+    key->Rotation(_target.Rot());
+  else
+    key->Rotation(start.Rot());
+}
+
+////////////////////////////////////////////////
+void MoveToHelper::MoveTo(const rendering::CameraPtr &_camera,
+    const rendering::NodePtr &_target,
+    double _duration, std::function<void()> _onAnimationComplete)
+{
+  this->dataPtr->camera = _camera;
+  this->dataPtr->poseAnim = std::make_unique<common::PoseAnimation>(
+      "move_to", _duration, false);
+  this->dataPtr->onAnimationComplete = std::move(_onAnimationComplete);
+
+  math::Pose3d start = _camera->WorldPose();
+
+  // todo(anyone) implement bounding box function in rendering to get
+  // target size and center.
+  // Assume fixed size and target world position is its center
+  math::Box targetBBox(1.0, 1.0, 1.0);
+  math::Vector3d targetCenter = _target->WorldPosition();
+  math::Vector3d dir = targetCenter - start.Pos();
+  dir.Correct();
+  dir.Normalize();
+
+  // distance to move
+  double maxSize = targetBBox.Size().Max();
+  double dist = start.Pos().Distance(targetCenter) - maxSize;
+
+  // Scale to fit in view
+  double hfov = this->dataPtr->camera->HFOV().Radian();
+  double offset = maxSize*0.5 / std::tan(hfov/2.0);
+
+  // End position and rotation
+  math::Vector3d endPos = start.Pos() + dir*(dist - offset);
+  math::Quaterniond endRot =
+      math::Matrix4d::LookAt(endPos, targetCenter).Rotation();
+  math::Pose3d end(endPos, endRot);
+
+  common::PoseKeyFrame *key = this->dataPtr->poseAnim->CreateKeyFrame(0);
+  key->Translation(start.Pos());
+  key->Rotation(start.Rot());
+
+  key = this->dataPtr->poseAnim->CreateKeyFrame(_duration);
+  key->Translation(end.Pos());
+  key->Rotation(end.Rot());
+}
+
+////////////////////////////////////////////////
+void MoveToHelper::LookDirection(const rendering::CameraPtr &_camera,
+    const math::Vector3d &_direction, const math::Vector3d &_lookAt,
+    double _duration, std::function<void()> _onAnimationComplete)
+{
+  this->dataPtr->camera = _camera;
+  this->dataPtr->poseAnim = std::make_unique<common::PoseAnimation>(
+      "view_angle", _duration, false);
+  this->dataPtr->onAnimationComplete = std::move(_onAnimationComplete);
+
+  math::Pose3d start = _camera->WorldPose();
+
+  // Look at world origin unless there are visuals selected
+  // Keep current distance to look at target
+  math::Vector3d camPos = _camera->WorldPose().Pos();
+  double distance = std::fabs((camPos - _lookAt).Length());
+
+  // Calculate camera position
+  math::Vector3d endPos = _lookAt - _direction * distance;
+
+  // Calculate camera orientation
+  math::Quaterniond endRot =
+    ignition::math::Matrix4d::LookAt(endPos, _lookAt).Rotation();
+
+  // Move camera to that pose
+  common::PoseKeyFrame *key = this->dataPtr->poseAnim->CreateKeyFrame(0);
+  key->Translation(start.Pos());
+  key->Rotation(start.Rot());
+
+  // Move camera back to initial pose
+  if (_direction == math::Vector3d::Zero)
+  {
+    endPos = this->dataPtr->initCameraPose.Pos();
+    endRot = this->dataPtr->initCameraPose.Rot();
+  }
+
+  key = this->dataPtr->poseAnim->CreateKeyFrame(_duration);
+  key->Translation(endPos);
+  key->Rotation(endRot);
+}
+
+////////////////////////////////////////////////
+void MoveToHelper::AddTime(double _time)
+{
+  if (!this->dataPtr->camera || !this->dataPtr->poseAnim)
+    return;
+
+  common::PoseKeyFrame kf(0);
+
+  this->dataPtr->poseAnim->AddTime(_time);
+  this->dataPtr->poseAnim->InterpolatedKeyFrame(kf);
+
+  math::Pose3d offset(kf.Translation(), kf.Rotation());
+
+  this->dataPtr->camera->SetWorldPose(offset);
+
+  if (this->dataPtr->poseAnim->Length() <= this->dataPtr->poseAnim->Time())
+  {
+    if (this->dataPtr->onAnimationComplete)
+    {
+      this->dataPtr->onAnimationComplete();
+    }
+    this->dataPtr->camera.reset();
+    this->dataPtr->poseAnim.reset();
+    this->dataPtr->onAnimationComplete = nullptr;
+  }
+}
+
+////////////////////////////////////////////////
+bool MoveToHelper::Idle() const
+{
+  return this->dataPtr->poseAnim == nullptr;
+}
+
+////////////////////////////////////////////////
+void MoveToHelper::SetInitCameraPose(const math::Pose3d &_pose)
+{
+  this->dataPtr->initCameraPose = _pose;
+}