diff --git a/include/ignition/rendering/MoveToHelper.hh b/include/ignition/rendering/MoveToHelper.hh new file mode 100644 index 000000000..d14f9da1a --- /dev/null +++ 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 + +#include +#include + +#include +#include + +#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 _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 _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 _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 dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} +#endif diff --git a/src/MoveToHelper.cc b/src/MoveToHelper.cc new file mode 100644 index 000000000..e0ca641fe --- /dev/null +++ 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 + +#include +#include +#include + +#include "ignition/rendering/Camera.hh" + +class ignition::rendering::MoveToHelperPrivate +{ + /// \brief Pose animation object + public: std::unique_ptr poseAnim; + + /// \brief Pointer to the camera being moved + public: rendering::CameraPtr camera; + + /// \brief Callback function when animation is complete. + public: std::function 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 _onAnimationComplete) +{ + this->dataPtr->camera = _camera; + this->dataPtr->poseAnim = std::make_unique( + "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 _onAnimationComplete) +{ + this->dataPtr->camera = _camera; + this->dataPtr->poseAnim = std::make_unique( + "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 _onAnimationComplete) +{ + this->dataPtr->camera = _camera; + this->dataPtr->poseAnim = std::make_unique( + "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; +} diff --git a/src/MoveToHelper_TEST.cc b/src/MoveToHelper_TEST.cc new file mode 100644 index 000000000..c07f3c619 --- /dev/null +++ b/src/MoveToHelper_TEST.cc @@ -0,0 +1,157 @@ +/* + * 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 + +#include + +#include +#include + +#include "test_config.h" // NOLINT(build/include) + +#include "ignition/rendering/RenderEngine.hh" +#include "ignition/rendering/RenderingIface.hh" +#include "ignition/rendering/Scene.hh" + +using namespace ignition; +using namespace rendering; + +class MoveToHelperTest : public testing::Test, + public testing::WithParamInterface +{ + public: void MoveTo(const std::string &_renderEngine); + + public: void OnMoveToComplete(); + + public: void checkIsCompleted(double timeout); + + public: MoveToHelper moveToHelper; + + public: bool isMoveCompleted = false; +}; + +void MoveToHelperTest::OnMoveToComplete() +{ + this->isMoveCompleted = true; +} + +void MoveToHelperTest::checkIsCompleted(double timeout) +{ + isMoveCompleted = false; + + std::chrono::time_point startTime; + std::chrono::time_point endTime; + + startTime = std::chrono::system_clock::now(); + + while (!isMoveCompleted) + { + moveToHelper.AddTime(0.01); + endTime = std::chrono::system_clock::now(); + + auto seconds = + std::chrono::duration_cast( + endTime - startTime).count() / 1000.0; + if (seconds > timeout) + break; + } +} + +void MoveToHelperTest::MoveTo(const std::string &_renderEngine) +{ + // create and populate scene + RenderEngine *engine = rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + ScenePtr scene = engine->CreateScene("scene"); + ASSERT_NE(nullptr, scene); + + VisualPtr root = scene->RootVisual(); + ASSERT_NE(nullptr, root); + + // create center visual + VisualPtr center = scene->CreateVisual("center"); + ASSERT_NE(nullptr, center); + center->AddGeometry(scene->CreateSphere()); + center->SetLocalPosition(30, 0, 0); + center->SetLocalScale(0.1, 0.1, 0.1); + root->AddChild(center); + + CameraPtr camera(scene->CreateCamera()); + ASSERT_NE(nullptr, camera); + + camera->SetLocalPosition(0.0, 0.0, 0.0); + camera->SetLocalRotation(0.0, 0.0, 0.0); + camera->SetImageWidth(800); + camera->SetImageHeight(600); + camera->SetAntiAliasing(2); + camera->SetAspectRatio(1.333); + camera->SetHFOV(IGN_PI / 2); + root->AddChild(camera); + + ASSERT_NE(nullptr, camera); + + rendering::NodePtr target = scene->NodeByName("center"); + + moveToHelper.MoveTo(camera, target, 0.5, + std::bind(&MoveToHelperTest::OnMoveToComplete, this)); + EXPECT_FALSE(moveToHelper.Idle()); + checkIsCompleted(0.5); + EXPECT_TRUE(moveToHelper.Idle()); + EXPECT_EQ(math::Vector3d(28.5, 0.0, 0.0), camera->LocalPosition()); + + moveToHelper.MoveTo(camera, math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), 0.5, + std::bind(&MoveToHelperTest::OnMoveToComplete, this)); + EXPECT_FALSE(moveToHelper.Idle()); + checkIsCompleted(0.5); + EXPECT_EQ(math::Vector3d(0.0, 0.0, 0.0), camera->LocalPosition()); + EXPECT_TRUE(moveToHelper.Idle()); + + math::Vector3d lookAt(0, 0, 1); + math::Vector3d viewAngleDirection(0, 1, 1); + moveToHelper.LookDirection(camera, + viewAngleDirection, lookAt, + 0.5, std::bind(&MoveToHelperTest::OnMoveToComplete, this)); + EXPECT_FALSE(moveToHelper.Idle()); + checkIsCompleted(0.5); + EXPECT_TRUE(moveToHelper.Idle()); + EXPECT_EQ(math::Vector3d(0.0, -1, 0.0), camera->LocalPosition()); + EXPECT_EQ(math::Quaterniond(0.0, -0.785398, 1.5708), camera->LocalRotation()); + + engine->DestroyScene(scene); + rendering::unloadEngine(engine->Name()); +} + +///////////////////////////////////////////////// +TEST_P(MoveToHelperTest, MoveToHelper) +{ + MoveTo(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(MoveToHelper, MoveToHelperTest, + RENDER_ENGINE_VALUES, + ignition::rendering::PrintToStringParam()); + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}