Add support for moving the GUI camera to a pose

src/gui/plugins/scene3d/Scene3D.cc

@@ -98,6 +98,16 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
         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),
@@ -249,6 +259,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// pose originally loaded from the sdf.
     public: math::Vector3d viewAngleDirection = math::Vector3d::Zero;
 
+    /// \brief The pose set from the move to pose service.
+    public: std::optional<math::Pose3d> moveToPoseValue;
+
     /// \brief Last move to animation time
     public: std::chrono::time_point<std::chrono::system_clock> prevMoveToTime;
 
@@ -347,11 +360,20 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \brief Follow service
     public: std::string followService;
 
-    /// \brief Follow service
+    /// \brief View angle service
     public: std::string viewAngleService;
 
+    /// \brief Move to pose service
+    public: std::string moveToPoseService;
+
     /// \brief Shapes service
     public: std::string shapesService;
+
+    /// \brief Camera pose topic
+    public: std::string cameraPoseTopic;
+
+    /// \brief Camera pose publisher
+    public: transport::Node::Publisher cameraPosePub;
   };
 }
 }
@@ -501,6 +523,28 @@ void IgnRenderer::Render()
     }
   }
 
+  // Move to pose
+  {
+    IGN_PROFILE("IgnRenderer::Render MoveToPose");
+    if (this->dataPtr->moveToPoseValue)
+    {
+      if (this->dataPtr->moveToHelper.Idle())
+      {
+        this->dataPtr->moveToHelper.MoveTo(this->dataPtr->camera,
+            *(this->dataPtr->moveToPoseValue),
+            0.5, std::bind(&IgnRenderer::OnMoveToPoseComplete, this));
+        this->dataPtr->prevMoveToTime = std::chrono::system_clock::now();
+      }
+      else
+      {
+        auto now = std::chrono::system_clock::now();
+        std::chrono::duration<double> dt = now - this->dataPtr->prevMoveToTime;
+        this->dataPtr->moveToHelper.AddTime(dt.count());
+        this->dataPtr->prevMoveToTime = now;
+      }
+    }
+  }
+
   // Follow
   {
     IGN_PROFILE("IgnRenderer::Render Follow");
@@ -1632,6 +1676,13 @@ void IgnRenderer::SetViewAngle(const math::Vector3d &_direction)
   this->dataPtr->viewAngleDirection = _direction;
 }
 
+/////////////////////////////////////////////////
+void IgnRenderer::SetMoveToPose(const math::Pose3d &_pose)
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+  this->dataPtr->moveToPoseValue = _pose;
+}
+
 /////////////////////////////////////////////////
 void IgnRenderer::SetFollowPGain(double _gain)
 {
@@ -1695,6 +1746,13 @@ void IgnRenderer::OnViewAngleComplete()
   this->dataPtr->viewAngle = false;
 }
 
+/////////////////////////////////////////////////
+void IgnRenderer::OnMoveToPoseComplete()
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+  this->dataPtr->moveToPoseValue.reset();
+}
+
 /////////////////////////////////////////////////
 void IgnRenderer::NewHoverEvent(const math::Vector2i &_hoverPos)
 {
@@ -1736,6 +1794,14 @@ math::Vector3d IgnRenderer::ScreenToPlane(
   return origin + direction * distance;
 }
 
+/////////////////////////////////////////////////
+math::Pose3d IgnRenderer::CameraPose() const
+{
+  if (this->dataPtr->camera)
+    return this->dataPtr->camera->WorldPose();
+  return math::Pose3d::Zero;
+}
+
 /////////////////////////////////////////////////
 math::Vector3d IgnRenderer::ScreenToScene(
     const math::Vector2i &_screenPos) const
@@ -2252,6 +2318,21 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem)
   ignmsg << "View angle service on ["
          << this->dataPtr->viewAngleService << "]" << std::endl;
 
+  // view pose service
+  this->dataPtr->moveToPoseService =
+      "/gui/move_to/pose";
+  this->dataPtr->node.Advertise(this->dataPtr->moveToPoseService,
+      &Scene3D::OnMoveToPose, this);
+  ignmsg << "Move to pose service on ["
+         << this->dataPtr->moveToPoseService << "]" << std::endl;
+
+  // camera position topic
+  this->dataPtr->cameraPoseTopic = "/gui/camera/pose";
+  this->dataPtr->cameraPosePub =
+    this->dataPtr->node.Advertise<msgs::Pose>(this->dataPtr->cameraPoseTopic);
+  ignmsg << "Camera pose topic advertised on ["
+         << this->dataPtr->cameraPoseTopic << "]" << std::endl;
+
   ignition::gui::App()->findChild<
       ignition::gui::MainWindow *>()->installEventFilter(this);
 }
@@ -2264,6 +2345,7 @@ void Scene3D::Update(const UpdateInfo &_info,
     return;
 
   IGN_PROFILE("Scene3D::Update");
+  auto renderWindow = this->PluginItem()->findChild<RenderWindowItem *>();
   if (this->dataPtr->worldName.empty())
   {
     // TODO(anyone) Only one scene is supported for now
@@ -2276,10 +2358,11 @@ void Scene3D::Update(const UpdateInfo &_info,
           return true;
         });
 
-    auto renderWindow = this->PluginItem()->findChild<RenderWindowItem *>();
     renderWindow->SetWorldName(this->dataPtr->worldName);
   }
 
+  msgs::Pose poseMsg = msgs::Convert(renderWindow->CameraPose());
+  this->dataPtr->cameraPosePub.Publish(poseMsg);
   this->dataPtr->renderUtil->UpdateFromECM(_info, _ecm);
 }
 
@@ -2343,6 +2426,33 @@ bool Scene3D::OnViewAngle(const msgs::Vector3d &_msg,
   return true;
 }
 
+/////////////////////////////////////////////////
+bool Scene3D::OnMoveToPose(const msgs::GUICamera &_msg, msgs::Boolean &_res)
+{
+  auto renderWindow = this->PluginItem()->findChild<RenderWindowItem *>();
+
+  math::Pose3d pose = msgs::Convert(_msg.pose());
+
+  // If there is no orientation in the message, then set a Rot value in the
+  // math::Pose3d object to infinite. This will prevent the orientation from
+  // being used when positioning the camera.
+  // See the MoveToHelper::MoveTo function
+  if (!_msg.pose().has_orientation())
+    pose.Rot().X() = math::INF_D;
+
+  // If there is no position in the message, then set a Pos value in the
+  // math::Pose3d object to infinite. This will prevent the orientation from
+  // being used when positioning the camera.
+  // See the MoveToHelper::MoveTo function
+  if (!_msg.pose().has_position())
+    pose.Pos().X() = math::INF_D;
+
+  renderWindow->SetMoveToPose(pose);
+
+  _res.set_data(true);
+  return true;
+}
+
 /////////////////////////////////////////////////
 void Scene3D::OnHovered(int _mouseX, int _mouseY)
 {
@@ -2554,6 +2664,12 @@ void RenderWindowItem::SetViewAngle(const math::Vector3d &_direction)
   this->dataPtr->renderThread->ignRenderer.SetViewAngle(_direction);
 }
 
+/////////////////////////////////////////////////
+void RenderWindowItem::SetMoveToPose(const math::Pose3d &_pose)
+{
+  this->dataPtr->renderThread->ignRenderer.SetMoveToPose(_pose);
+}
+
 /////////////////////////////////////////////////
 void RenderWindowItem::SetFollowPGain(double _gain)
 {
@@ -2578,6 +2694,14 @@ void RenderWindowItem::SetCameraPose(const math::Pose3d &_pose)
   this->dataPtr->renderThread->ignRenderer.cameraPose = _pose;
 }
 
+/////////////////////////////////////////////////
+math::Pose3d RenderWindowItem::CameraPose() const
+{
+  if (this->dataPtr->renderThread)
+    return this->dataPtr->renderThread->ignRenderer.CameraPose();
+  return math::Pose3d::Zero;
+}
+
 /////////////////////////////////////////////////
 void RenderWindowItem::SetInitCameraPose(const math::Pose3d &_pose)
 {
@@ -2705,6 +2829,34 @@ void RenderWindowItem::keyReleaseEvent(QKeyEvent *_e)
 // }
 //
 
+////////////////////////////////////////////////
+void MoveToHelper::MoveTo(const rendering::CameraPtr &_camera,
+    const ignition::math::Pose3d &_target,
+    double _duration, std::function<void()> _onAnimationComplete)
+{
+  this->camera = _camera;
+  this->poseAnim = std::make_unique<common::PoseAnimation>(
+      "move_to", _duration, false);
+  this->onAnimationComplete = std::move(_onAnimationComplete);
+
+  math::Pose3d start = _camera->WorldPose();
+
+  common::PoseKeyFrame *key = this->poseAnim->CreateKeyFrame(0);
+  key->Translation(start.Pos());
+  key->Rotation(start.Rot());
+
+  key = this->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,

src/gui/plugins/scene3d/Scene3D.hh

@@ -19,6 +19,7 @@
 #define IGNITION_GAZEBO_GUI_SCENE3D_HH_
 
 #include <ignition/msgs/boolean.pb.h>
+#include <ignition/msgs/gui_camera.pb.h>
 #include <ignition/msgs/stringmsg.pb.h>
 #include <ignition/msgs/vector3d.pb.h>
 #include <ignition/msgs/video_record.pb.h>
@@ -146,6 +147,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     private: bool OnViewAngle(const msgs::Vector3d &_msg,
         msgs::Boolean &_res);
 
+    /// \brief Callback for a move to pose request.
+    /// \param[in] _msg GUICamera request message.
+    /// \param[in] _res Response data
+    /// \return True if the request is received
+    private: bool OnMoveToPose(const msgs::GUICamera &_msg,
+                 msgs::Boolean &_res);
+
     /// \internal
     /// \brief Pointer to private data.
     private: std::unique_ptr<Scene3DPrivate> dataPtr;
@@ -216,6 +224,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// originally loaded in from the sdf
     public: void SetViewAngle(const math::Vector3d &_direction);
 
+    /// \brief Set the world pose of the camera
+    /// \param[in] _pose The world pose to set the camera to.
+    public: void SetMoveToPose(const math::Pose3d &_pose);
+
     /// \brief Set the p gain for the camera follow movement
     /// \param[in] _gain Camera follow p gain.
     public: void SetFollowPGain(double _gain);
@@ -365,12 +377,19 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     public: math::Vector3d ScreenToScene(const math::Vector2i &_screenPos)
         const;
 
+    /// \brief Get the current camera pose.
+    /// \return Pose of the camera.
+    public: math::Pose3d CameraPose() const;
+
     /// \brief Callback when a move to animation is complete
     private: void OnMoveToComplete();
 
     /// \brief Callback when a view angle animation is complete
     private: void OnViewAngleComplete();
 
+    /// \brief Callback when a move to  pose animation is complete
+    private: void OnMoveToPoseComplete();
+
     /// \brief Process a node's selection
     /// \param[in] _node The node to be selected.
     /// \param[in] _sendEvent True to notify other widgets. This should be true
@@ -469,6 +488,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \param[in] _pose Pose to set the camera to
     public: void SetCameraPose(const math::Pose3d &_pose);
 
+    /// \brief Get the user camera pose.
+    /// \return Pose of the user camera.
+    public: math::Pose3d CameraPose() const;
+
     /// \brief Set the initial user camera pose
     /// \param[in] _pose Pose to set the camera to
     public: void SetInitCameraPose(const math::Pose3d &_pose);
@@ -510,6 +533,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// originally loaded in from the sdf
     public: void SetViewAngle(const math::Vector3d &_direction);
 
+    /// \brief Set the pose of the camera
+    /// \param[in] _pose The new camera pose in the world frame.
+    public: void SetMoveToPose(const math::Pose3d &_pose);
+
     /// \brief Set the p gain for the camera follow movement
     /// \param[in] _gain Camera follow p gain.
     public: void SetFollowPGain(double _gain);