Allow to move to model from Angle view plugin

src/gui/plugins/view_angle/ViewAngle.cc

@@ -66,6 +66,15 @@ namespace ignition::gazebo
  /// \brief Move gui camera to pose service name
  public: std::string moveToPoseService;
 
+ /// \brief Move gui camera to model service name
+ public: std::string moveToModelService;
+
+ /// \brief New move to model message
+ public: bool newMoveToModel = false;
+
+ /// \brief Distance of the camera to the model
+ public: double distanceMoveToModel = 0.0;
+
  /// \brief gui camera pose
  public: math::Pose3d camPose;
 
@@ -154,6 +163,13 @@ void ViewAngle::LoadConfig(const tinyxml2::XMLElement *_pluginElem)
  // Move to pose service
  this->dataPtr->moveToPoseService = "/gui/move_to/pose";
 
+ // Move to model service
+ this->dataPtr->moveToModelService = "/gui/move_to/model";
+ this->dataPtr->node.Advertise(this->dataPtr->moveToModelService,
+ &ViewAngle::OnMoveToModelService, this);
+ ignmsg << "Move to model service on ["
+ << this->dataPtr->moveToModelService << "]" << std::endl;
+
  ignition::gui::App()->findChild<
  ignition::gui::MainWindow *>()->installEventFilter(this);
 }
@@ -311,6 +327,84 @@ void ViewAngle::SetCamPose(double _x, double _y, double _z,
  }
 }
 
+/////////////////////////////////////////////////
+bool ViewAngle::OnMoveToModelService(const ignition::msgs::GUICamera &_msg,
+ ignition::msgs::Boolean &_res)
+{
+ std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+ auto scene = this->dataPtr->camera->Scene();
+
+ auto visualToMove = scene->VisualByName(_msg.name());
+ if (nullptr == visualToMove)
+ {
+ ignerr << "Failed to get visual with ID ["
+ << _msg.name() << "]" << std::endl;
+ _res.set_data(false);
+ return false;
+ }
+ Entity entityId = kNullEntity;
+ try
+ {
+ // TODO(ahcorde): When forward porting this to Garder change var type to
+ // unsigned int
+ entityId = std::get<int>(visualToMove->UserData("gazebo-entity"));
+ }
+ catch(std::bad_variant_access &_e)
+ {
+ ignerr << "Failed to get gazebo-entity user data ["
+ << visualToMove->Name() << "]" << std::endl;
+ _res.set_data(false);
+ return false;
+ }
+
+ math::Quaterniond q(
+ _msg.pose().orientation().w(),
+ _msg.pose().orientation().x(),
+ _msg.pose().orientation().y(),
+ _msg.pose().orientation().z());
+
+ ignition::math::Vector3d axis;
+ double angle;
+ q.ToAxis(axis, angle);
+
+ std::function<void(const msgs::Boolean &, const bool)> cb =
+ [](const msgs::Boolean &/*_rep*/, const bool _result)
+ {
+ if (!_result)
+ ignerr << "Error setting view controller" << std::endl;
+ };
+
+ msgs::StringMsg req;
+ std::string str = _msg.projection_type();
+ if (str.find("Orbit") != std::string::npos ||
+ str.find("orbit") != std::string::npos)
+ {
+ req.set_data("orbit");
+ }
+ else if (str.find("Ortho") != std::string::npos ||
+ str.find("ortho") != std::string::npos )
+ {
+ req.set_data("ortho");
+ }
+ else
+ {
+ ignerr << "Unknown view controller selected: " << str << std::endl;
+ _res.set_data(false);
+ return false;
+ }
+
+ this->dataPtr->node.Request(this->dataPtr->viewControlService, req, cb);
+
+ this->dataPtr->viewingAngle = true;
+ this->dataPtr->newMoveToModel = true;
+ this->dataPtr->viewAngleDirection = axis;
+ this->dataPtr->distanceMoveToModel = _msg.pose().position().z();
+ this->dataPtr->selectedEntities.push_back(entityId);
+
+ _res.set_data(true);
+ return true;
+}
+
 /////////////////////////////////////////////////
 void ViewAngle::CamPoseCb(const msgs::Pose &_msg)
 {
@@ -465,6 +559,35 @@ void ViewAnglePrivate::OnComplete()
 {
  this->viewingAngle = false;
  this->moveToPoseValue.reset();
+ if (this->newMoveToModel)
+ {
+ this->selectedEntities.pop_back();
+ this->newMoveToModel = false;
+
+ auto cameraPose = this->camera->WorldPose();
+ auto distance = -(this->viewAngleDirection * this->distanceMoveToModel);
+
+ if (!math::equal(this->viewAngleDirection.X(), 0.0))
+ {
+ cameraPose.Pos().X(distance.X());
+ }
+ if (!math::equal(this->viewAngleDirection.Y(), 0.0))
+ {
+ cameraPose.Pos().Y(distance.Y());
+ }
+ if (!math::equal(this->viewAngleDirection.Z(), 0.0))
+ {
+ cameraPose.Pos().Z(distance.Z());
+ }
+
+ this->moveToPoseValue = {
+ cameraPose.Pos().X(),
+ cameraPose.Pos().Y(),
+ cameraPose.Pos().Z(),
+ cameraPose.Rot().Roll(),
+ cameraPose.Rot().Pitch(),
+ cameraPose.Rot().Yaw()};
+ }
 }
 
 /////////////////////////////////////////////////

src/gui/plugins/view_angle/ViewAngle.hh

@@ -19,6 +19,8 @@
 #define IGNITION_GAZEBO_GUI_VIEWANGLE_HH_
 
 #include <ignition/msgs/pose.pb.h>
+#include <ignition/msgs/boolean.pb.h>
+#include <ignition/msgs/gui_camera.pb.h>
 
 #include <memory>
 
@@ -101,6 +103,12 @@ namespace gazebo
  /// \param[in] _msg Pose message
  public: void CamPoseCb(const msgs::Pose &_msg);
 
+ /// \brief Move to model service received
+ /// \param[in] _msg GUI camera message
+ /// \param[in] _res Response
+ public: bool OnMoveToModelService(const ignition::msgs::GUICamera &_msg,
+ ignition::msgs::Boolean &_res);
+
  /// \brief Get the current gui camera's near and far clipping distances
  public: Q_INVOKABLE QList<double> CamClipDist() const;