Skip to content

Commit

Permalink
Add support for moving the GUI camera to a pose (gazebosim#352)
Browse files Browse the repository at this point in the history 
Signed-off-by: Nate Koenig <[email protected]>

Co-authored-by: Louise Poubel <[email protected]>
Co-authored-by: Ian Chen <[email protected]>
Signed-off-by: Guillaume Doisy <[email protected]>
  • Loading branch information
@nkoenig @chapulina @iche033
3 people authored and Guillaume Doisy committed Dec 13, 2020
1 parent 9c12ce8 commit e865cff
Show file tree
Hide file tree
Showing 2 changed files with 184 additions and 2 deletions.
159 changes: 157 additions & 2 deletions src/gui/plugins/scene3d/Scene3D.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,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),
Expand Down Expand Up @@ -250,6 +260,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;

Expand Down Expand Up @@ -348,11 +361,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;
};
}
}
Expand Down Expand Up @@ -502,6 +524,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");
Expand Down Expand Up @@ -1633,6 +1677,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)
{
Expand Down Expand Up @@ -1696,6 +1747,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)
{
Expand Down Expand Up @@ -1737,6 +1795,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
Expand Down Expand Up @@ -2253,6 +2319,21 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem)
ignmsg << "View angle service on ["
<< this->dataPtr->viewAngleService << "]" << std::endl;

// move to 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);
}
Expand All @@ -2265,6 +2346,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
Expand All @@ -2277,7 +2359,6 @@ void Scene3D::Update(const UpdateInfo &_info,
return true;
});

auto renderWindow = this->PluginItem()->findChild<RenderWindowItem *>();
renderWindow->SetWorldName(this->dataPtr->worldName);
auto worldEntity =
_ecm.EntityByComponents(components::Name(this->dataPtr->worldName),
Expand All @@ -2295,6 +2376,11 @@ void Scene3D::Update(const UpdateInfo &_info,
}
}

if (this->dataPtr->cameraPosePub.HasConnections())
{
msgs::Pose poseMsg = msgs::Convert(renderWindow->CameraPose());
this->dataPtr->cameraPosePub.Publish(poseMsg);
}
this->dataPtr->renderUtil->UpdateFromECM(_info, _ecm);
}

Expand Down Expand Up @@ -2358,6 +2444,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)
{
Expand Down Expand Up @@ -2569,6 +2682,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)
{
Expand All @@ -2593,6 +2712,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)
{
Expand Down Expand Up @@ -2720,6 +2847,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,
Expand Down
27 changes: 27 additions & 0 deletions src/gui/plugins/scene3d/Scene3D.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit e865cff

Please sign in to comment.