Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Use moveToHelper from ign-rendering #825

Merged
merged 1 commit into from
May 26, 2021
Merged

Use moveToHelper from ign-rendering #825

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
221 changes: 3 additions & 218 deletions src/gui/plugins/scene3d/Scene3D.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@

#include <ignition/rendering/Image.hh>
#include <ignition/rendering/OrbitViewController.hh>
#include <ignition/rendering/MoveToHelper.hh>
#include <ignition/rendering/RayQuery.hh>
#include <ignition/rendering/RenderEngine.hh>
#include <ignition/rendering/RenderingIface.hh>
Expand Down Expand Up @@ -94,70 +95,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
bool sendEvent{false};
};

//
/// \brief Helper class for animating a user camera to move to a target entity
/// todo(anyone) Move this functionality to rendering::Camera class in
/// ign-rendering3
class 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);

/// \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;
};

/// \brief Private data class for IgnRenderer
class IgnRendererPrivate
{
Expand Down Expand Up @@ -229,7 +166,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
public: std::string moveToTarget;

/// \brief Helper object to move user camera
public: MoveToHelper moveToHelper;
public: ignition::rendering::MoveToHelper moveToHelper;

/// \brief Target to view collisions
public: std::string viewCollisionsTarget;
Expand Down Expand Up @@ -449,7 +386,7 @@ QList<QThread *> RenderWindowItemPrivate::threads;
IgnRenderer::IgnRenderer()
: dataPtr(new IgnRendererPrivate)
{
this->dataPtr->moveToHelper.initCameraPose = this->cameraPose;
this->dataPtr->moveToHelper.SetInitCameraPose(this->cameraPose);

// recorder stats topic
std::string recorderStatsTopic = "/gui/record_video/stats";
Expand Down Expand Up @@ -3273,158 +3210,6 @@ void RenderWindowItem::HandleKeyRelease(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,
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();

// 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->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->poseAnim->CreateKeyFrame(0);
key->Translation(start.Pos());
key->Rotation(start.Rot());

key = this->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->camera = _camera;
this->poseAnim = std::make_unique<common::PoseAnimation>(
"view_angle", _duration, false);
this->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->poseAnim->CreateKeyFrame(0);
key->Translation(start.Pos());
key->Rotation(start.Rot());

// Move camera back to initial pose
if (_direction == math::Vector3d::Zero)
{
endPos = this->initCameraPose.Pos();
endRot = this->initCameraPose.Rot();
}

key = this->poseAnim->CreateKeyFrame(_duration);
key->Translation(endPos);
key->Rotation(endRot);
}

////////////////////////////////////////////////
void MoveToHelper::AddTime(double _time)
{
if (!this->camera || !this->poseAnim)
return;

common::PoseKeyFrame kf(0);

this->poseAnim->AddTime(_time);
this->poseAnim->InterpolatedKeyFrame(kf);

math::Pose3d offset(kf.Translation(), kf.Rotation());

this->camera->SetWorldPose(offset);

if (this->poseAnim->Length() <= this->poseAnim->Time())
{
if (this->onAnimationComplete)
{
this->onAnimationComplete();
}
this->camera.reset();
this->poseAnim.reset();
this->onAnimationComplete = nullptr;
}
}

////////////////////////////////////////////////
bool MoveToHelper::Idle() const
{
return this->poseAnim == nullptr;
}

////////////////////////////////////////////////
void MoveToHelper::SetInitCameraPose(const math::Pose3d &_pose)
{
this->initCameraPose = _pose;
}

// Register this plugin
IGNITION_ADD_PLUGIN(ignition::gazebo::Scene3D,
ignition::gui::Plugin)