From 0cacbd5f1618b2fe64a559c7e93e2fbf9495fba8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 26 May 2021 22:45:49 +0200 Subject: [PATCH 01/11] Use moveToHelper from ign-rendering (#825) Signed-off-by: ahcorde --- src/gui/plugins/scene3d/Scene3D.cc | 221 +---------------------------- 1 file changed, 3 insertions(+), 218 deletions(-) diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 289cf9de0b..78853d824a 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -48,6 +48,7 @@ #include #include +#include #include #include #include @@ -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 _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); - - /// \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; - }; - /// \brief Private data class for IgnRenderer class IgnRendererPrivate { @@ -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; @@ -449,7 +386,7 @@ QList 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"; @@ -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 _onAnimationComplete) -{ - this->camera = _camera; - this->poseAnim = std::make_unique( - "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 _onAnimationComplete) -{ - this->camera = _camera; - this->poseAnim = std::make_unique( - "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 _onAnimationComplete) -{ - this->camera = _camera; - this->poseAnim = std::make_unique( - "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) From 34803028478a832e631ecc4b6f8264039472d8ab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 27 May 2021 14:41:44 +0200 Subject: [PATCH 02/11] Remove unused headers in video_recoder plugin (#834) Signed-off-by: ahcorde --- src/gui/plugins/video_recorder/VideoRecorder.cc | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/gui/plugins/video_recorder/VideoRecorder.cc b/src/gui/plugins/video_recorder/VideoRecorder.cc index 3e9df0964c..84406d718f 100644 --- a/src/gui/plugins/video_recorder/VideoRecorder.cc +++ b/src/gui/plugins/video_recorder/VideoRecorder.cc @@ -30,10 +30,6 @@ #include #include -#include "ignition/gazebo/components/Name.hh" -#include "ignition/gazebo/components/ParentEntity.hh" -#include "ignition/gazebo/EntityComponentManager.hh" - namespace ignition::gazebo { class VideoRecorderPrivate From 65e8b04bc1041294993a46c91100aa21b555e9c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 27 May 2021 19:37:46 +0200 Subject: [PATCH 03/11] Removed duplicated code with rendering::sceneFromFirstRenderEngine (#819) Signed-off-by: ahcorde Co-authored-by: Michael Carroll --- src/gui/plugins/align_tool/AlignTool.cc | 58 ++++--------------- src/gui/plugins/grid_config/GridConfig.cc | 37 +----------- .../transform_control/TransformControl.cc | 37 +----------- .../CameraVideoRecorder.cc | 33 +---------- 4 files changed, 13 insertions(+), 152 deletions(-) diff --git a/src/gui/plugins/align_tool/AlignTool.cc b/src/gui/plugins/align_tool/AlignTool.cc index c5b6094882..ca3520fde5 100644 --- a/src/gui/plugins/align_tool/AlignTool.cc +++ b/src/gui/plugins/align_tool/AlignTool.cc @@ -82,6 +82,9 @@ namespace ignition::gazebo /// \brief The map of the original transparency values for the nodes. public: std::map originalTransparency; + + /// \brief Pointer to the scene. + public: rendering::ScenePtr scene{nullptr}; }; } @@ -316,52 +319,9 @@ void AlignTool::Align() if (this->dataPtr->currentState == AlignState::NONE) return; - auto loadedEngNames = rendering::loadedEngines(); - if (loadedEngNames.empty()) - { - ignerr << "Internal error: engine should be loaded at this point." - << std::endl; - return; - } - - // Assume there is only one engine loaded - auto engineName = loadedEngNames[0]; - if (loadedEngNames.size() > 1) - { - ignwarn << "Found more than one loaded engine " - "- using " << engineName << "." << std::endl; - } - auto engine = rendering::engine(engineName); - - if (!engine) - { - ignerr << "Internal error: failed to load engine [" << engineName - << "]. Align tool plugin won't work." << std::endl; - return; - } - - if (engine->SceneCount() == 0) - { - ignerr<< "Internal error: no scenes are available with the loaded engine." - << std::endl; - return; - } - // assume there is only one scene // load scene - auto scene = engine->SceneByIndex(0); - - if (!scene) - { - ignerr << "Internal error: scene is null." << std::endl; - return; - } - - if (!scene->IsInitialized() || scene->VisualCount() == 0) - { - ignerr << "Internal error: scene is either not initialized " - "or there are no visuals within it." << std::endl; - return; - } + if (!this->dataPtr->scene) + this->dataPtr->scene = rendering::sceneFromFirstRenderEngine(); // Get current list of selected entities std::vector selectedList; @@ -369,9 +329,10 @@ void AlignTool::Align() for (const auto &entityId : this->dataPtr->selectedEntities) { - for (auto i = 0u; i < scene->VisualCount(); ++i) + for (auto i = 0u; i < this->dataPtr->scene->VisualCount(); ++i) { - ignition::rendering::VisualPtr vis = scene->VisualByIndex(i); + ignition::rendering::VisualPtr vis = + this->dataPtr->scene->VisualByIndex(i); if (!vis) continue; if (std::get(vis->UserData("gazebo-entity")) == @@ -417,7 +378,8 @@ void AlignTool::Align() rendering::VisualPtr vis = selectedList[i]; // Check here to see if visual is top level or not, continue if not - rendering::VisualPtr topLevelVis = this->TopLevelVisual(scene, vis); + rendering::VisualPtr topLevelVis = this->TopLevelVisual( + this->dataPtr->scene, vis); if (topLevelVis != vis) continue; diff --git a/src/gui/plugins/grid_config/GridConfig.cc b/src/gui/plugins/grid_config/GridConfig.cc index df449039c0..aa7fac9280 100644 --- a/src/gui/plugins/grid_config/GridConfig.cc +++ b/src/gui/plugins/grid_config/GridConfig.cc @@ -149,42 +149,7 @@ void GridConfig::UpdateGrid() ///////////////////////////////////////////////// void GridConfig::LoadGrid() { - auto loadedEngNames = rendering::loadedEngines(); - if (loadedEngNames.empty()) - return; - - // assume there is only one engine loaded - auto engineName = loadedEngNames[0]; - if (loadedEngNames.size() > 1) - { - igndbg << "More than one engine is available. " - << "Grid config plugin will use engine [" - << engineName << "]" << std::endl; - } - auto engine = rendering::engine(engineName); - if (!engine) - { - ignerr << "Internal error: failed to load engine [" << engineName - << "]. Grid plugin won't work." << std::endl; - return; - } - - if (engine->SceneCount() == 0) - return; - - // assume there is only one scene - // load scene - auto scene = engine->SceneByIndex(0); - if (!scene) - { - ignerr << "Internal error: scene is null." << std::endl; - return; - } - - if (!scene->IsInitialized() || nullptr == scene->RootVisual()) - { - return; - } + auto scene = rendering::sceneFromFirstRenderEngine(); // load grid // if gridPtr found, load the existing gridPtr to class diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index d4b1d8c94e..cf2a39738e 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -164,42 +164,7 @@ void TransformControl::SnapToGrid() ///////////////////////////////////////////////// void TransformControl::LoadGrid() { - auto loadedEngNames = rendering::loadedEngines(); - if (loadedEngNames.empty()) - return; - - // assume there is only one engine loaded - auto engineName = loadedEngNames[0]; - if (loadedEngNames.size() > 1) - { - igndbg << "More than one engine is available. " - << "Grid config plugin will use engine [" - << engineName << "]" << std::endl; - } - auto engine = rendering::engine(engineName); - if (!engine) - { - ignerr << "Internal error: failed to load engine [" << engineName - << "]. Grid plugin won't work." << std::endl; - return; - } - - if (engine->SceneCount() == 0) - return; - - // assume there is only one scene - // load scene - auto scene = engine->SceneByIndex(0); - if (!scene) - { - ignerr << "Internal error: scene is null." << std::endl; - return; - } - - if (!scene->IsInitialized() || scene->VisualCount() == 0) - { - return; - } + auto scene = rendering::sceneFromFirstRenderEngine(); // load grid // if gridPtr found, load the existing gridPtr to class diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.cc b/src/systems/camera_video_recorder/CameraVideoRecorder.cc index 52c24ad0f9..22fadcacee 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.cc +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.cc @@ -218,38 +218,7 @@ void CameraVideoRecorderPrivate::OnPostRender() // get scene if (!this->scene) { - auto loadedEngNames = rendering::loadedEngines(); - if (loadedEngNames.empty()) - return; - - // assume there is only one engine loaded - auto engineName = loadedEngNames[0]; - if (loadedEngNames.size() > 1) - { - igndbg << "More than one engine is available. " - << "Camera video recorder system will use engine [" - << engineName << "]" << std::endl; - } - auto engine = rendering::engine(engineName); - if (!engine) - { - ignerr << "Internal error: failed to load engine [" << engineName - << "]. Camera video recorder system won't work." << std::endl; - return; - } - - if (engine->SceneCount() == 0) - return; - - // assume there is only one scene - // load scene - auto s = engine->SceneByIndex(0); - if (!s) - { - ignerr << "Internal error: scene is null." << std::endl; - return; - } - this->scene = s; + this->scene = rendering::sceneFromFirstRenderEngine(); } // return if scene not ready or no sensors available. From ee895b82525a4ee8943b317bc2e44799260f5446 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 28 May 2021 08:06:02 +0200 Subject: [PATCH 04/11] Cleanup and alphabetize plugin headers (#838) Signed-off-by: ahcorde --- src/gui/plugins/align_tool/AlignTool.cc | 16 +++++++++------- src/gui/plugins/align_tool/AlignTool.hh | 2 +- src/gui/plugins/grid_config/GridConfig.cc | 2 +- .../playback_scrubber/PlaybackScrubber.cc | 8 +++----- .../playback_scrubber/PlaybackScrubber.hh | 2 +- .../plugins/resource_spawner/ResourceSpawner.cc | 7 +++---- src/gui/plugins/shapes/Shapes.cc | 1 - .../transform_control/TransformControl.cc | 11 ++++------- 8 files changed, 22 insertions(+), 27 deletions(-) diff --git a/src/gui/plugins/align_tool/AlignTool.cc b/src/gui/plugins/align_tool/AlignTool.cc index ca3520fde5..8a720e44bc 100644 --- a/src/gui/plugins/align_tool/AlignTool.cc +++ b/src/gui/plugins/align_tool/AlignTool.cc @@ -24,24 +24,26 @@ #include #include #include + +#include #include #include -#include -#include #include -#include -#include #include #include +#include #include #include -#include #include -#include "ignition/gazebo/components/World.hh" -#include "ignition/gazebo/components/Name.hh" +#include +#include #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/gui/GuiEvents.hh" +#include "ignition/gazebo/rendering/RenderUtil.hh" + #include "AlignTool.hh" namespace ignition::gazebo diff --git a/src/gui/plugins/align_tool/AlignTool.hh b/src/gui/plugins/align_tool/AlignTool.hh index 0f3c525d36..41a0d4d726 100644 --- a/src/gui/plugins/align_tool/AlignTool.hh +++ b/src/gui/plugins/align_tool/AlignTool.hh @@ -20,8 +20,8 @@ #include -#include #include +#include #include namespace ignition diff --git a/src/gui/plugins/grid_config/GridConfig.cc b/src/gui/plugins/grid_config/GridConfig.cc index aa7fac9280..740ee7b299 100644 --- a/src/gui/plugins/grid_config/GridConfig.cc +++ b/src/gui/plugins/grid_config/GridConfig.cc @@ -18,9 +18,9 @@ #include #include #include -#include #include #include +#include #include #include "ignition/gazebo/gui/GuiEvents.hh" diff --git a/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc b/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc index 5edadd3145..9f4606091b 100644 --- a/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc +++ b/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc @@ -27,19 +27,17 @@ #include #include -#include - #include #include #include #include +#include #include #include #include -#include "ignition/gazebo/components/LogPlaybackStatistics.hh" -#include "ignition/gazebo/components/World.hh" -#include "ignition/gazebo/components/Name.hh" + #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/components/LogPlaybackStatistics.hh" #include "ignition/gazebo/gui/GuiEvents.hh" namespace ignition::gazebo diff --git a/src/gui/plugins/playback_scrubber/PlaybackScrubber.hh b/src/gui/plugins/playback_scrubber/PlaybackScrubber.hh index f770a735b3..f722669e92 100644 --- a/src/gui/plugins/playback_scrubber/PlaybackScrubber.hh +++ b/src/gui/plugins/playback_scrubber/PlaybackScrubber.hh @@ -21,8 +21,8 @@ #include #include -#include #include +#include namespace ignition { diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.cc b/src/gui/plugins/resource_spawner/ResourceSpawner.cc index a567b3cb8f..8d7a2b1b61 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.cc +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.cc @@ -28,17 +28,16 @@ #include #include -#include #include +#include +#include +#include #include #include #include #include #include -#include -#include -#include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/gui/GuiEvents.hh" namespace ignition::gazebo diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index 489ec3d988..3b9165e5e3 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -31,7 +31,6 @@ #include #include -#include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/gui/GuiEvents.hh" namespace ignition::gazebo diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index cf2a39738e..8c5835469d 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -27,19 +27,16 @@ #include #include #include -#include -#include -#include #include #include +#include #include #include -#include #include +#include +#include +#include -#include "ignition/gazebo/components/Name.hh" -#include "ignition/gazebo/components/ParentEntity.hh" -#include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/gui/GuiEvents.hh" namespace ignition::gazebo From 5a3b1b8de31bb1ab459a6ef582a7bf752aabb471 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Mon, 14 Jun 2021 21:31:18 -0400 Subject: [PATCH 05/11] Fix potentially flaky integration component test case (#848) Signed-off-by: Ashton Larkin --- test/integration/components.cc | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/test/integration/components.cc b/test/integration/components.cc index e4bc94071a..20ae6a89e2 100644 --- a/test/integration/components.cc +++ b/test/integration/components.cc @@ -882,31 +882,33 @@ TEST_F(ComponentsTest, LogicalAudioSourcePlayInfo) TEST_F(ComponentsTest, LogicalMicrophone) { logical_audio::Microphone mic1; - mic1.id = 0; + mic1.id = 4; mic1.volumeDetectionThreshold = 0.5; logical_audio::Microphone mic2; - mic2.id = 1; - mic2.volumeDetectionThreshold = mic1.volumeDetectionThreshold; + mic2.id = 8; + mic2.volumeDetectionThreshold = 0.8; // create components auto comp1 = components::LogicalMicrophone(mic1); auto comp2 = components::LogicalMicrophone(mic2); // equality operators - EXPECT_NE(mic1, mic2); - EXPECT_FALSE(mic1 == mic2); - EXPECT_TRUE(mic1 != mic2); + EXPECT_NE(comp1, comp2); + EXPECT_FALSE(comp1 == comp2); + EXPECT_TRUE(comp1 != comp2); // stream operators std::ostringstream ostr; comp1.Serialize(ostr); - EXPECT_EQ("0 0.5", ostr.str()); + EXPECT_EQ("4 0.5", ostr.str()); - std::istringstream istr; + std::istringstream istr(ostr.str()); components::LogicalMicrophone comp3; comp3.Deserialize(istr); EXPECT_EQ(comp1, comp3); + EXPECT_DOUBLE_EQ(comp1.Data().volumeDetectionThreshold, + comp3.Data().volumeDetectionThreshold); } ///////////////////////////////////////////////// From 44f2e921246ee654a7515ec63570d599e0c11569 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Thu, 17 Jun 2021 13:55:44 -0300 Subject: [PATCH 06/11] Fix WindEffects Plugin bug, not configuring new links (#844) * Fix issue not configuring links added after start Signed-off-by: Jorge Perez * Add a test checking the fix Signed-off-by: Jorge Perez --- src/systems/wind_effects/WindEffects.cc | 66 +++++++++++-------------- test/integration/wind_effects.cc | 52 +++++++++++++++++++ test/worlds/wind_effects.sdf | 4 ++ 3 files changed, 84 insertions(+), 38 deletions(-) diff --git a/src/systems/wind_effects/WindEffects.cc b/src/systems/wind_effects/WindEffects.cc index 11ba62836d..72c1f25b93 100644 --- a/src/systems/wind_effects/WindEffects.cc +++ b/src/systems/wind_effects/WindEffects.cc @@ -152,10 +152,6 @@ class ignition::gazebo::systems::WindEffectsPrivate /// valid. public: bool validConfig{false}; - /// \brief Whether links have been initialized. Initialization involves - /// creating components this system needs on links that are affected by wind. - public: bool linksInitialized{false}; - /// \brief Mutex to protect currWindVelSeed and windEnabled public: std::mutex windInfoMutex; @@ -542,45 +538,39 @@ void WindEffects::PreUpdate(const UpdateInfo &_info, // Process commands this->dataPtr->ProcessCommandQueue(_ecm); - if (this->dataPtr->validConfig) + if (!this->dataPtr->validConfig) + return; + + _ecm.EachNew( + [&](const Entity &_entity, components::Link *, + components::WindMode *_windMode) -> bool { - if (!this->dataPtr->linksInitialized) + if (_windMode->Data()) { - _ecm.Each( - [&](const Entity &_entity, components::Link *, - components::WindMode *_windMode) -> bool - { - if (_windMode->Data()) - { - // Create a WorldLinearVelocity component on the link so that - // physics can populate it - if (!_ecm.Component(_entity)) - { - _ecm.CreateComponent(_entity, - components::WorldLinearVelocity()); - } - if (!_ecm.Component(_entity)) - { - _ecm.CreateComponent(_entity, components::WorldPose()); - } - } - return true; - }); - - this->dataPtr->linksInitialized = true; + // Create a WorldLinearVelocity component on the link so that + // physics can populate it + if (!_ecm.Component(_entity)) + { + _ecm.CreateComponent(_entity, + components::WorldLinearVelocity()); + } + if (!_ecm.Component(_entity)) + { + _ecm.CreateComponent(_entity, components::WorldPose()); + } } - else - { - if (_info.paused) - return; + return true; + }); - if (!this->dataPtr->currentWindInfo.enable_wind()) - return; + if (_info.paused) + return; + + if (!this->dataPtr->currentWindInfo.enable_wind()) + return; + + this->dataPtr->UpdateWindVelocity(_info, _ecm); + this->dataPtr->ApplyWindForce(_info, _ecm); - this->dataPtr->UpdateWindVelocity(_info, _ecm); - this->dataPtr->ApplyWindForce(_info, _ecm); - } - } } IGNITION_ADD_PLUGIN(WindEffects, System, diff --git a/test/integration/wind_effects.cc b/test/integration/wind_effects.cc index a441740542..f91d0ce1c3 100644 --- a/test/integration/wind_effects.cc +++ b/test/integration/wind_effects.cc @@ -326,3 +326,55 @@ TEST_F(WindEffectsTest , TopicsAndServices) lastVelMagnitude = velMagnitude; } } + +/// Test if adding a link with wind after first iteration adds +/// WorldLinearVelocity component properly +TEST_F(WindEffectsTest, WindEntityAddedAfterStart) +{ + const std::string windBox = R"EOF( + + + + 5 5 5 0 0 0 + true + + + + + 1 1 1 + + + + + + )EOF"; + + this->StartServer("/test/worlds/wind_effects.sdf"); + + LinkComponentRecorder + linkVelocityComponent("test_link_wind"); + this->server->AddSystem(linkVelocityComponent.systemPtr); + EXPECT_TRUE(linkVelocityComponent.values.empty()); + + // Run the logger for a time, check it is still empty + this->server->Run(true, 10, false); + EXPECT_TRUE(linkVelocityComponent.values.empty()); + + // Add the box to be logged via the command system + // and check that is not empty + transport::Node node; + msgs::EntityFactory req; + unsigned int timeout = 5000; + std::string service{"/world/wind_demo/create"}; + msgs::Boolean res; + bool result; + + req.set_sdf(windBox); + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Now box_wind WorldLinearVelocity component should be added + this->server->Run(true, 10, false); + ASSERT_FALSE(linkVelocityComponent.values.empty()); +} diff --git a/test/worlds/wind_effects.sdf b/test/worlds/wind_effects.sdf index 1d8b251640..cc90a3cb42 100644 --- a/test/worlds/wind_effects.sdf +++ b/test/worlds/wind_effects.sdf @@ -7,6 +7,10 @@ filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics"> + + Date: Thu, 17 Jun 2021 14:50:56 -0700 Subject: [PATCH 07/11] New example: get an ECM snapshot from an external program (#859) Signed-off-by: Louise Poubel --- .../standalone/external_ecm/CMakeLists.txt | 8 ++ examples/standalone/external_ecm/README.md | 93 ++++++++++++++++++ .../standalone/external_ecm/external_ecm.cc | 98 +++++++++++++++++++ 3 files changed, 199 insertions(+) create mode 100644 examples/standalone/external_ecm/CMakeLists.txt create mode 100644 examples/standalone/external_ecm/README.md create mode 100644 examples/standalone/external_ecm/external_ecm.cc diff --git a/examples/standalone/external_ecm/CMakeLists.txt b/examples/standalone/external_ecm/CMakeLists.txt new file mode 100644 index 0000000000..7ebe46521a --- /dev/null +++ b/examples/standalone/external_ecm/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +find_package(ignition-gazebo3 REQUIRED) + +add_executable(external_ecm external_ecm.cc) +target_link_libraries(external_ecm + ignition-gazebo3::core) + diff --git a/examples/standalone/external_ecm/README.md b/examples/standalone/external_ecm/README.md new file mode 100644 index 0000000000..d65789813e --- /dev/null +++ b/examples/standalone/external_ecm/README.md @@ -0,0 +1,93 @@ +# External ECM + +Example showing how to get a snapshot of all entities and components in a +running simulation from an external program using the state message. + +## Build + +From the root of the `ign-gazebo` repository, do the following to build the example: + +~~~ +cd ign-gazebo/examples/standalone/external_ecm +mkdir build +cd build +cmake .. +make +~~~ + +This will generate the `external_ecm` executable under `build`. + +## Run + +Start a simulation, for example: + + ign gazebo shapes.sdf + +On another terminal, run the `external_ecm` executable, passing the name of the +running world you want to inspect: + + cd ign-gazebo/examples/standalone/external_ecm + ./external_ecm shapes + +You should see something like this: + +``` +$ ./external_ecm shapes + +Requesting state for world [shapes] on service [/world/shapes/state]... + +Entity [1] + - Name: shapes + - Parent: +Entity [4] + - Name: ground_plane + - Parent: shapes [1] +Entity [5] + - Name: link + - Parent: ground_plane [4] +Entity [6] + - Name: visual + - Parent: link [5] +Entity [7] + - Name: collision + - Parent: link [5] +Entity [8] + - Name: box + - Parent: shapes [1] +Entity [9] + - Name: box_link + - Parent: box [8] +Entity [10] + - Name: box_visual + - Parent: box_link [9] +Entity [11] + - Name: box_collision + - Parent: box_link [9] +Entity [12] + - Name: cylinder + - Parent: shapes [1] +Entity [13] + - Name: cylinder_link + - Parent: cylinder [12] +Entity [14] + - Name: cylinder_visual + - Parent: cylinder_link [13] +Entity [15] + - Name: cylinder_collision + - Parent: cylinder_link [13] +Entity [16] + - Name: sphere + - Parent: shapes [1] +Entity [17] + - Name: sphere_link + - Parent: sphere [16] +Entity [18] + - Name: sphere_visual + - Parent: sphere_link [17] +Entity [19] + - Name: sphere_collision + - Parent: sphere_link [17] +Entity [20] + - Name: sun + - Parent: shapes [1] +``` diff --git a/examples/standalone/external_ecm/external_ecm.cc b/examples/standalone/external_ecm/external_ecm.cc new file mode 100644 index 0000000000..6c9fa3f354 --- /dev/null +++ b/examples/standalone/external_ecm/external_ecm.cc @@ -0,0 +1,98 @@ +/* + * 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 +#include + +////////////////////////////////////////////////// +int main(int argc, char **argv) +{ + if (argc < 2) + { + std::cout << "Usage: `./external_ecm `" << std::endl; + return -1; + } + + // Get arguments + std::string world = argv[1]; + + // Create a transport node. + ignition::transport::Node node; + + bool executed{false}; + bool result{false}; + unsigned int timeout{5000}; + std::string service{"/world/" + world + "/state"}; + + std::cout << std::endl << "Requesting state for world [" << world + << "] on service [" << service << "]..." << std::endl << std::endl; + + // Request and block + ignition::msgs::SerializedStepMap res; + executed = node.Request(service, timeout, res, result); + + if (!executed) + { + std::cerr << std::endl << "Service call to [" << service << "] timed out" + << std::endl; + return -1; + } + + if (!result) + { + std::cerr << std::endl << "Service call to [" << service << "] failed" + << std::endl; + return -1; + } + + // Instantiate an ECM and populate with data from message + ignition::gazebo::EntityComponentManager ecm; + ecm.SetState(res.state()); + + // Print some information + ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::Name *_name) -> bool + { + auto parentComp = + ecm.Component(_entity); + + std::string parentInfo; + if (parentComp) + { + auto parentNameComp = + ecm.Component( + parentComp->Data()); + + if (parentNameComp) + { + parentInfo += parentNameComp->Data() + " "; + } + parentInfo += "[" + std::to_string(parentComp->Data()) + "]"; + } + + std::cout << "Entity [" << _entity << "]" << std::endl + << " - Name: " << _name->Data() << std::endl + << " - Parent: " << parentInfo << std::endl; + + return true; + }); +} From 7876ca3d1c1bbcf3399b1f7bd089539ae04ed781 Mon Sep 17 00:00:00 2001 From: LolaSegura <48759425+LolaSegura@users.noreply.github.com> Date: Fri, 18 Jun 2021 01:24:07 -0300 Subject: [PATCH 08/11] Using math::SpeedLimiter on the diff_drive controller. (#833) * Change the diff_drive_system test to cover min_acceleration and min_velocity limits. Added those limits to the .sdf file. Signed-off-by: LolaSegura Co-authored-by: Louise Poubel Co-authored-by: Franco Cipollone <53065142+francocipollone@users.noreply.github.com> Co-authored-by: Steve Peters --- src/systems/diff_drive/CMakeLists.txt | 1 - src/systems/diff_drive/DiffDrive.cc | 67 +++--- src/systems/diff_drive/SpeedLimiter.cc | 200 ---------------- src/systems/diff_drive/SpeedLimiter.hh | 130 ----------- test/integration/diff_drive_system.cc | 280 ++++++++++++----------- test/worlds/diff_drive.sdf | 2 + test/worlds/diff_drive_custom_topics.sdf | 2 + 7 files changed, 183 insertions(+), 499 deletions(-) delete mode 100644 src/systems/diff_drive/SpeedLimiter.cc delete mode 100644 src/systems/diff_drive/SpeedLimiter.hh diff --git a/src/systems/diff_drive/CMakeLists.txt b/src/systems/diff_drive/CMakeLists.txt index 0a415da6d4..7c7da3ec05 100644 --- a/src/systems/diff_drive/CMakeLists.txt +++ b/src/systems/diff_drive/CMakeLists.txt @@ -1,7 +1,6 @@ gz_add_system(diff-drive SOURCES DiffDrive.cc - SpeedLimiter.cc PUBLIC_LINK_LIBS ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index e8a49f4356..eb356bd491 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -27,6 +27,7 @@ #include #include #include +#include #include #include @@ -36,8 +37,6 @@ #include "ignition/gazebo/Link.hh" #include "ignition/gazebo/Model.hh" -#include "SpeedLimiter.hh" - using namespace ignition; using namespace gazebo; using namespace systems; @@ -123,10 +122,10 @@ class ignition::gazebo::systems::DiffDrivePrivate public: transport::Node::Publisher tfPub; /// \brief Linear velocity limiter. - public: std::unique_ptr limiterLin; + public: std::unique_ptr limiterLin; /// \brief Angular velocity limiter. - public: std::unique_ptr limiterAng; + public: std::unique_ptr limiterAng; /// \brief Previous control command. public: Commands last0Cmd; @@ -197,56 +196,48 @@ void DiffDrive::Configure(const Entity &_entity, this->dataPtr->wheelRadius = _sdf->Get("wheel_radius", this->dataPtr->wheelRadius).first; - // Parse speed limiter parameters. - bool hasVelocityLimits = false; - bool hasAccelerationLimits = false; - bool hasJerkLimits = false; - double minVel = std::numeric_limits::lowest(); - double maxVel = std::numeric_limits::max(); - double minAccel = std::numeric_limits::lowest(); - double maxAccel = std::numeric_limits::max(); - double minJerk = std::numeric_limits::lowest(); - double maxJerk = std::numeric_limits::max(); + // Instantiate the speed limiters. + this->dataPtr->limiterLin = std::make_unique(); + this->dataPtr->limiterAng = std::make_unique(); + // Parse speed limiter parameters. if (_sdf->HasElement("min_velocity")) { - minVel = _sdf->Get("min_velocity"); - hasVelocityLimits = true; + const double minVel = _sdf->Get("min_velocity"); + this->dataPtr->limiterLin->SetMinVelocity(minVel); + this->dataPtr->limiterAng->SetMinVelocity(minVel); } if (_sdf->HasElement("max_velocity")) { - maxVel = _sdf->Get("max_velocity"); - hasVelocityLimits = true; + const double maxVel = _sdf->Get("max_velocity"); + this->dataPtr->limiterLin->SetMaxVelocity(maxVel); + this->dataPtr->limiterAng->SetMaxVelocity(maxVel); } if (_sdf->HasElement("min_acceleration")) { - minAccel = _sdf->Get("min_acceleration"); - hasAccelerationLimits = true; + const double minAccel = _sdf->Get("min_acceleration"); + this->dataPtr->limiterLin->SetMinAcceleration(minAccel); + this->dataPtr->limiterAng->SetMinAcceleration(minAccel); } if (_sdf->HasElement("max_acceleration")) { - maxAccel = _sdf->Get("max_acceleration"); - hasAccelerationLimits = true; + const double maxAccel = _sdf->Get("max_acceleration"); + this->dataPtr->limiterLin->SetMaxAcceleration(maxAccel); + this->dataPtr->limiterAng->SetMaxAcceleration(maxAccel); } if (_sdf->HasElement("min_jerk")) { - minJerk = _sdf->Get("min_jerk"); - hasJerkLimits = true; + const double minJerk = _sdf->Get("min_jerk"); + this->dataPtr->limiterLin->SetMinJerk(minJerk); + this->dataPtr->limiterAng->SetMinJerk(minJerk); } if (_sdf->HasElement("max_jerk")) { - maxJerk = _sdf->Get("max_jerk"); - hasJerkLimits = true; + const double maxJerk = _sdf->Get("max_jerk"); + this->dataPtr->limiterLin->SetMaxJerk(maxJerk); + this->dataPtr->limiterAng->SetMaxJerk(maxJerk); } - // Instantiate the speed limiters. - this->dataPtr->limiterLin = std::make_unique( - hasVelocityLimits, hasAccelerationLimits, hasJerkLimits, - minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk); - - this->dataPtr->limiterAng = std::make_unique( - hasVelocityLimits, hasAccelerationLimits, hasJerkLimits, - minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk); double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; if (odomFreq > 0) @@ -502,11 +493,11 @@ void DiffDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, angVel = this->targetVel.angular().z(); } - const double dt = std::chrono::duration(_info.dt).count(); - // Limit the target velocity if needed. - this->limiterLin->Limit(linVel, this->last0Cmd.lin, this->last1Cmd.lin, dt); - this->limiterAng->Limit(angVel, this->last0Cmd.ang, this->last1Cmd.ang, dt); + this->limiterLin->Limit( + linVel, this->last0Cmd.lin, this->last1Cmd.lin, _info.dt); + this->limiterAng->Limit( + angVel, this->last0Cmd.ang, this->last1Cmd.ang, _info.dt); // Update history of commands. this->last1Cmd = last0Cmd; diff --git a/src/systems/diff_drive/SpeedLimiter.cc b/src/systems/diff_drive/SpeedLimiter.cc deleted file mode 100644 index 59b50f54b9..0000000000 --- a/src/systems/diff_drive/SpeedLimiter.cc +++ /dev/null @@ -1,200 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, PAL Robotics, S.L. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the PAL Robotics nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author: Enrique Fernández - * Modified: Carlos Agüero - */ - -#include - -#include "SpeedLimiter.hh" - -using namespace ignition; -using namespace gazebo; -using namespace systems; - -/// \brief Private SpeedLimiter data class. -class ignition::gazebo::systems::SpeedLimiterPrivate -{ - /// \brief Class constructor. - public: SpeedLimiterPrivate(bool _hasVelocityLimits, - bool _hasAccelerationLimits, - bool _hasJerkLimits, - double _minVelocity, - double _maxVelocity, - double _minAcceleration, - double _maxAcceleration, - double _minJerk, - double _maxJerk) - : hasVelocityLimits(_hasVelocityLimits), - hasAccelerationLimits(_hasAccelerationLimits), - hasJerkLimits(_hasJerkLimits), - minVelocity(_minVelocity), - maxVelocity(_maxVelocity), - minAcceleration(_minAcceleration), - maxAcceleration(_maxAcceleration), - minJerk(_minJerk), - maxJerk(_maxJerk) - { - } - - /// \brief Enable/Disable velocity. - public: bool hasVelocityLimits; - - /// \brief Enable/Disable acceleration. - public: bool hasAccelerationLimits; - - /// \brief Enable/Disable jerk. - public: bool hasJerkLimits; - - /// \brief Minimum velocity limit. - public: double minVelocity; - - /// \brief Maximum velocity limit. - public: double maxVelocity; - - /// \brief Minimum acceleration limit. - public: double minAcceleration; - - /// \brief Maximum acceleration limit. - public: double maxAcceleration; - - /// \brief Minimum jerk limit. - public: double minJerk; - - /// \brief Maximum jerk limit. - public: double maxJerk; -}; - -////////////////////////////////////////////////// -SpeedLimiter::SpeedLimiter(bool _hasVelocityLimits, - bool _hasAccelerationLimits, - bool _hasJerkLimits, - double _minVelocity, - double _maxVelocity, - double _minAcceleration, - double _maxAcceleration, - double _minJerk, - double _maxJerk) - : dataPtr(std::make_unique(_hasVelocityLimits, - _hasAccelerationLimits, _hasJerkLimits, _minVelocity, _maxVelocity, - _minAcceleration, _maxAcceleration, _minJerk, _maxJerk)) -{ -} - -////////////////////////////////////////////////// -SpeedLimiter::~SpeedLimiter() -{ -} - -////////////////////////////////////////////////// -double SpeedLimiter::Limit(double &_v, double _v0, double _v1, double _dt) const -{ - const double tmp = _v; - - this->LimitJerk(_v, _v0, _v1, _dt); - this->LimitAcceleration(_v, _v0, _dt); - this->LimitVelocity(_v); - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} - -////////////////////////////////////////////////// -double SpeedLimiter::LimitVelocity(double &_v) const -{ - const double tmp = _v; - - if (this->dataPtr->hasVelocityLimits) - { - _v = ignition::math::clamp( - _v, this->dataPtr->minVelocity, this->dataPtr->maxVelocity); - } - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} - -////////////////////////////////////////////////// -double SpeedLimiter::LimitAcceleration(double &_v, double _v0, double _dt) const -{ - const double tmp = _v; - - if (this->dataPtr->hasAccelerationLimits) - { - const double dvMin = this->dataPtr->minAcceleration * _dt; - const double dvMax = this->dataPtr->maxAcceleration * _dt; - - const double dv = ignition::math::clamp(_v - _v0, dvMin, dvMax); - - _v = _v0 + dv; - } - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} - -////////////////////////////////////////////////// -double SpeedLimiter::LimitJerk(double &_v, double _v0, double _v1, double _dt) - const -{ - const double tmp = _v; - - if (this->dataPtr->hasJerkLimits) - { - const double dv = _v - _v0; - const double dv0 = _v0 - _v1; - - const double dt2 = 2. * _dt * _dt; - - const double daMin = this->dataPtr->minJerk * dt2; - const double daMax = this->dataPtr->maxJerk * dt2; - - const double da = ignition::math::clamp(dv - dv0, daMin, daMax); - - _v = _v0 + dv0 + da; - } - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} diff --git a/src/systems/diff_drive/SpeedLimiter.hh b/src/systems/diff_drive/SpeedLimiter.hh deleted file mode 100644 index 34286bc3ae..0000000000 --- a/src/systems/diff_drive/SpeedLimiter.hh +++ /dev/null @@ -1,130 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, PAL Robotics, S.L. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the PAL Robotics nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author: Enrique Fernández - * Modified: Carlos Agüero - */ - -#ifndef IGNITION_GAZEBO_SYSTEMS_SPEEDLIMITER_HH_ -#define IGNITION_GAZEBO_SYSTEMS_SPEEDLIMITER_HH_ - -#include - -#include - -namespace ignition -{ -namespace gazebo -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { -namespace systems -{ - // Forward declaration. - class SpeedLimiterPrivate; - - /// \brief Class to limit velocity, acceleration and jerk. - /// \ref https://github.com/ros-controls/ros_controllers/tree/melodic-devel/diff_drive_controller - class SpeedLimiter - { - /// \brief Constructor. - /// \param [in] _hasVelocityLimits if true, applies velocity limits. - /// \param [in] _hasAccelerationLimits if true, applies acceleration limits. - /// \param [in] _hasJerkLimits if true, applies jerk limits. - /// \param [in] _minVelocity Minimum velocity [m/s], usually <= 0. - /// \param [in] _maxVelocity Maximum velocity [m/s], usually >= 0. - /// \param [in] _minAcceleration Minimum acceleration [m/s^2], usually <= 0. - /// \param [in] _maxAcceleration Maximum acceleration [m/s^2], usually >= 0. - /// \param [in] _minJerk Minimum jerk [m/s^3], usually <= 0. - /// \param [in] _maxJerk Maximum jerk [m/s^3], usually >= 0. - public: SpeedLimiter(bool _hasVelocityLimits = false, - bool _hasAccelerationLimits = false, - bool _hasJerkLimits = false, - double _minVelocity = 0.0, - double _maxVelocity = 0.0, - double _minAcceleration = 0.0, - double _maxAcceleration = 0.0, - double _minJerk = 0.0, - double _maxJerk = 0.0); - - /// \brief Destructor. - public: ~SpeedLimiter(); - - /// \brief Limit the velocity and acceleration. - /// \param [in, out] _v Velocity [m/s]. - /// \param [in] _v0 Previous velocity to v [m/s]. - /// \param [in] _v1 Previous velocity to v0 [m/s]. - /// \param [in] _dt Time step [s]. - /// \return Limiting factor (1.0 if none). - public: double Limit(double &_v, - double _v0, - double _v1, - double _dt) const; - - /// \brief Limit the velocity. - /// \param [in, out] _v Velocity [m/s]. - /// \return Limiting factor (1.0 if none). - public: double LimitVelocity(double &_v) const; - - /// \brief Limit the acceleration. - /// \param [in, out] _v Velocity [m/s]. - /// \param [in] _v0 Previous velocity [m/s]. - /// \param [in] _dt Time step [s]. - /// \return Limiting factor (1.0 if none). - public: double LimitAcceleration(double &_v, - double _v0, - double _dt) const; - - /// \brief Limit the jerk. - /// \param [in, out] _v Velocity [m/s]. - /// \param [in] _v0 Previous velocity to v [m/s]. - /// \param [in] _v1 Previous velocity to v0 [m/s]. - /// \param [in] _dt Time step [s]. - /// \return Limiting factor (1.0 if none). - /// \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control. - public: double LimitJerk(double &_v, - double _v0, - double _v1, - double _dt) const; - - /// \brief Private data pointer. - private: std::unique_ptr dataPtr; - }; - } -} -} -} - -#endif diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index df756fcd52..067476bfb0 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -54,145 +54,165 @@ class DiffDriveTest : public ::testing::TestWithParam const std::string &_cmdVelTopic, const std::string &_odomTopic) { - // Start server - ServerConfig serverConfig; - serverConfig.SetSdfFile(_sdfFile); - - Server server(serverConfig); - EXPECT_FALSE(server.Running()); - EXPECT_FALSE(*server.Running(0)); - - // Create a system that records the vehicle poses - test::Relay testSystem; - - std::vector poses; - testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) - { - auto id = _ecm.EntityByComponents( - components::Model(), - components::Name("vehicle")); - EXPECT_NE(kNullEntity, id); + /// \param[in] forward If forward is true, the 'max_acceleration' + // and 'max_velocity' properties are tested, as the movement + // is forward, otherwise 'min_acceleration' and 'min_velocity' + // properties are tested. + auto testCmdVel = [&](bool forward){ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle poses + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle")); + EXPECT_NE(kNullEntity, id); - auto poseComp = _ecm.Component(id); - ASSERT_NE(nullptr, poseComp); + auto poseComp = _ecm.Component(id); + ASSERT_NE(nullptr, poseComp); - poses.push_back(poseComp->Data()); - }); - server.AddSystem(testSystem.systemPtr); + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); - // Run server and check that vehicle didn't move - server.Run(true, 1000, false); + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); - EXPECT_EQ(1000u, poses.size()); + EXPECT_EQ(1000u, poses.size()); - for (const auto &pose : poses) - { - EXPECT_EQ(poses[0], pose); - } - - // Get odometry messages - double period{1.0 / 50.0}; - double lastMsgTime{1.0}; - std::vector odomPoses; - std::function odomCb = - [&](const msgs::Odometry &_msg) + for (const auto &pose : poses) { - ASSERT_TRUE(_msg.has_header()); - ASSERT_TRUE(_msg.header().has_stamp()); - - double msgTime = - static_cast(_msg.header().stamp().sec()) + - static_cast(_msg.header().stamp().nsec()) * 1e-9; - - EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period); - lastMsgTime = msgTime; - - odomPoses.push_back(msgs::Convert(_msg.pose())); - }; - - // Publish command and check that vehicle moved - transport::Node node; - auto pub = node.Advertise(_cmdVelTopic); - node.Subscribe(_odomTopic, odomCb); - - msgs::Twist msg; - - // Avoid wheel slip by limiting acceleration - // Avoid wheel slip by limiting acceleration (1 m/s^2) - // and max velocity (0.5 m/s). - // See parameters - // in "/test/worlds/diff_drive.sdf". - test::Relay velocityRamp; - const double desiredLinVel = 10.5; - const double desiredAngVel = 0.2; - velocityRamp.OnPreUpdate( - [&](const gazebo::UpdateInfo &/*_info*/, - const gazebo::EntityComponentManager &) + EXPECT_EQ(poses[0], pose); + } + + // Get odometry messages + double period{1.0 / 50.0}; + double lastMsgTime{1.0}; + std::vector odomPoses; + std::function odomCb = + [&](const msgs::Odometry &_msg) { - msgs::Set(msg.mutable_linear(), - math::Vector3d(desiredLinVel, 0, 0)); - msgs::Set(msg.mutable_angular(), - math::Vector3d(0.0, 0, desiredAngVel)); - pub.Publish(msg); - }); - - server.AddSystem(velocityRamp.systemPtr); - - server.Run(true, 3000, false); + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + double msgTime = + static_cast(_msg.header().stamp().sec()) + + static_cast(_msg.header().stamp().nsec()) * 1e-9; + + EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period); + lastMsgTime = msgTime; + + odomPoses.push_back(msgs::Convert(_msg.pose())); + }; + + // Publish command and check that vehicle moved + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + node.Subscribe(_odomTopic, odomCb); + + msgs::Twist msg; + + // Avoid wheel slip by limiting acceleration (1 m/s^2) + // and max velocity (0.5 m/s). + // See and parameters + // in "/test/worlds/diff_drive.sdf". + // See , , and + // parameters in "/test/worlds/diff_drive.sdf". + test::Relay velocityRamp; + const int movementDirection = (forward ? 1 : -1); + double desiredLinVel = movementDirection * 10.5; + double desiredAngVel = 0.2; + velocityRamp.OnPreUpdate( + [&](const gazebo::UpdateInfo &/*_info*/, + const gazebo::EntityComponentManager &) + { + msgs::Set(msg.mutable_linear(), + math::Vector3d(desiredLinVel, 0, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, desiredAngVel)); + pub.Publish(msg); + }); + + server.AddSystem(velocityRamp.systemPtr); + + server.Run(true, 3000, false); + + // Poses for 4s + ASSERT_EQ(4000u, poses.size()); + + int sleep = 0; + int maxSleep = 30; + for (; odomPoses.size() < 150 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); + + // Odometry calculates the pose of a point that is located half way + // between the two wheels, not the origin of the model. For example, + // if the vehicle is commanded to rotate in place, the vehicle will + // rotate about the point half way between the two wheels, thus, + // the odometry position will remain zero. + // However, since the model origin is offset, the model position will + // change. To find the final pose of the model, we have to do the + // following similarity transformation + math::Pose3d tOdomModel(0.554283, 0, -0.325, 0, 0, 0); + auto finalModelFramePose = + tOdomModel * odomPoses.back() * tOdomModel.Inverse(); + + // Odom for 3s + ASSERT_FALSE(odomPoses.empty()); + EXPECT_EQ(150u, odomPoses.size()); + + auto expectedLowerPosition = + (forward ? poses[0].Pos() : poses[3999].Pos()); + auto expectedGreaterPosition = + (forward ? poses[3999].Pos() : poses[0].Pos()); + + EXPECT_LT(expectedLowerPosition.X(), expectedGreaterPosition.X()); + EXPECT_LT(expectedLowerPosition.Y(), expectedGreaterPosition.Y()); + EXPECT_NEAR(expectedLowerPosition.Z(), expectedGreaterPosition.Z(), tol); + EXPECT_NEAR(poses[0].Rot().X(), poses[3999].Rot().X(), tol); + EXPECT_NEAR(poses[0].Rot().Y(), poses[3999].Rot().Y(), tol); + EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z()); + + // The value from odometry will be close, but not exactly the ground truth + // pose of the robot model. This is partially due to throttling the + // odometry publisher, partially due to a frame difference between the + // odom frame and the model frame, and partially due to wheel slip. + EXPECT_NEAR(poses[1020].Pos().X(), odomPoses[0].Pos().X(), 1e-2); + EXPECT_NEAR(poses[1020].Pos().Y(), odomPoses[0].Pos().Y(), 1e-2); + EXPECT_NEAR(poses.back().Pos().X(), finalModelFramePose.Pos().X(), 1e-2); + EXPECT_NEAR(poses.back().Pos().Y(), finalModelFramePose.Pos().Y(), 1e-2); + + // Verify velocity and acceleration boundaries. + // Moving time. + double t = 3.0; + double d = poses[3999].Pos().Distance(poses[0].Pos()); + double v0 = 0; + double v = d / t; + double a = (v - v0) / t; + + EXPECT_LT(v, 0.5); + EXPECT_LT(a, 1); + EXPECT_GT(v, -0.5); + EXPECT_GT(a, -1); - // Poses for 4s - ASSERT_EQ(4000u, poses.size()); + }; - int sleep = 0; - int maxSleep = 30; - for (; odomPoses.size() < 150 && sleep < maxSleep; ++sleep) - { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - ASSERT_NE(maxSleep, sleep); - - // Odometry calculates the pose of a point that is located half way between - // the two wheels, not the origin of the model. For example, if the - // vehicle is commanded to rotate in place, the vehicle will rotate about - // the point half way between the two wheels, thus, the odometry position - // will remain zero. - // However, since the model origin is offset, the model position will - // change. To find the final pose of the model, we have to do the - // following similarity transformation - math::Pose3d tOdomModel(0.554283, 0, -0.325, 0, 0, 0); - auto finalModelFramePose = - tOdomModel * odomPoses.back() * tOdomModel.Inverse(); - - // Odom for 3s - ASSERT_FALSE(odomPoses.empty()); - EXPECT_EQ(150u, odomPoses.size()); - - EXPECT_LT(poses[0].Pos().X(), poses[3999].Pos().X()); - EXPECT_LT(poses[0].Pos().Y(), poses[3999].Pos().Y()); - EXPECT_NEAR(poses[0].Pos().Z(), poses[3999].Pos().Z(), tol); - EXPECT_NEAR(poses[0].Rot().X(), poses[3999].Rot().X(), tol); - EXPECT_NEAR(poses[0].Rot().Y(), poses[3999].Rot().Y(), tol); - EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z()); - - // The value from odometry will be close, but not exactly the ground truth - // pose of the robot model. This is partially due to throttling the - // odometry publisher, partially due to a frame difference between the - // odom frame and the model frame, and partially due to wheel slip. - EXPECT_NEAR(poses[1020].Pos().X(), odomPoses[0].Pos().X(), 1e-2); - EXPECT_NEAR(poses[1020].Pos().Y(), odomPoses[0].Pos().Y(), 1e-2); - EXPECT_NEAR(poses.back().Pos().X(), finalModelFramePose.Pos().X(), 1e-2); - EXPECT_NEAR(poses.back().Pos().Y(), finalModelFramePose.Pos().Y(), 1e-2); - - // Max velocities/accelerations expectations. - // Moving time. - double t = 3.0; - double d = poses[3999].Pos().Distance(poses[1000].Pos()); - const double v0 = 0; - double v = d / t; - double a = (v - v0) / t; - EXPECT_LT(v, 0.5); - EXPECT_LT(a, 1); + testCmdVel(true /*test forward movement*/); + testCmdVel(false /*test backward movement*/); } }; diff --git a/test/worlds/diff_drive.sdf b/test/worlds/diff_drive.sdf index b2b00bb0c4..dcb6a1fc2e 100644 --- a/test/worlds/diff_drive.sdf +++ b/test/worlds/diff_drive.sdf @@ -228,7 +228,9 @@ 1.25 0.3 1 + -1 0.5 + -0.5 diff --git a/test/worlds/diff_drive_custom_topics.sdf b/test/worlds/diff_drive_custom_topics.sdf index 075c1c6744..15a414d1e3 100644 --- a/test/worlds/diff_drive_custom_topics.sdf +++ b/test/worlds/diff_drive_custom_topics.sdf @@ -231,6 +231,8 @@ /model/bar/odom 1 0.5 + -1 + -0.5 From 8abfc3b2330493b27103f2e4ca8efb17abcc8c00 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 18 Jun 2021 12:20:51 -0700 Subject: [PATCH 09/11] Depend on ign-rendering 3.5 (#867) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel Co-authored-by: Alejandro Hernández Cordero --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8d0ac04519..f9295b628a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -114,7 +114,7 @@ set(IGN_SENSORS_VER ${ignition-sensors3_VERSION_MAJOR}) #-------------------------------------- # Find ignition-rendering -ign_find_package(ignition-rendering3 REQUIRED VERSION 3.2) +ign_find_package(ignition-rendering3 REQUIRED VERSION 3.5) set(IGN_RENDERING_VER ${ignition-rendering3_VERSION_MAJOR}) #-------------------------------------- From fada73c6d2bffdde8b019575e5e60640ac3e20d4 Mon Sep 17 00:00:00 2001 From: Franco Cipollone <53065142+francocipollone@users.noreply.github.com> Date: Fri, 18 Jun 2021 16:23:17 -0300 Subject: [PATCH 10/11] Enables confirmation dialog when closing Gazebo. (#850) Signed-off-by: Franco Cipollone --- CMakeLists.txt | 2 +- src/gui/gui.config | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f9295b628a..028af75f64 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,7 +75,7 @@ set(IGN_FUEL_TOOLS_VER ${ignition-fuel_tools4_VERSION_MAJOR}) #-------------------------------------- # Find ignition-gui -ign_find_package(ignition-gui3 REQUIRED VERSION 3.5) +ign_find_package(ignition-gui3 REQUIRED VERSION 3.6) set(IGN_GUI_VER ${ignition-gui3_VERSION_MAJOR}) ign_find_package (Qt5 COMPONENTS diff --git a/src/gui/gui.config b/src/gui/gui.config index bac2bcba8f..9553b1d34e 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -21,6 +21,7 @@ + true From 483684f885bb8975063a22a510bb114033a9faaa Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Fri, 18 Jun 2021 16:38:21 -0700 Subject: [PATCH 11/11] Set gui camera pose (#863) Signed-off-by: Jenn Nguyen Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- src/gui/plugins/view_angle/ViewAngle.cc | 66 +++++++++- src/gui/plugins/view_angle/ViewAngle.hh | 25 ++++ src/gui/plugins/view_angle/ViewAngle.qml | 149 ++++++++++++++++++++++- 3 files changed, 234 insertions(+), 6 deletions(-) diff --git a/src/gui/plugins/view_angle/ViewAngle.cc b/src/gui/plugins/view_angle/ViewAngle.cc index eae23f7d4f..9de7fddf4f 100644 --- a/src/gui/plugins/view_angle/ViewAngle.cc +++ b/src/gui/plugins/view_angle/ViewAngle.cc @@ -18,6 +18,7 @@ #include "ViewAngle.hh" #include +#include #include #include @@ -38,7 +39,13 @@ namespace ignition::gazebo public: std::mutex mutex; /// \brief View Angle service name - public: std::string service; + public: std::string viewAngleService; + + /// \brief Move gui camera to pose service name + public: std::string moveToPoseService; + + /// \brief gui camera pose + public: math::Pose3d camPose; }; } @@ -61,7 +68,15 @@ void ViewAngle::LoadConfig(const tinyxml2::XMLElement *) this->title = "View Angle"; // For view angle requests - this->dataPtr->service = "/gui/view_angle"; + this->dataPtr->viewAngleService = "/gui/view_angle"; + + // Subscribe to camera pose + std::string topic = "/gui/camera/pose"; + this->dataPtr->node.Subscribe( + topic, &ViewAngle::CamPoseCb, this); + + // Move to pose service + this->dataPtr->moveToPoseService = "/gui/move_to/pose"; } ///////////////////////////////////////////////// @@ -79,7 +94,52 @@ void ViewAngle::OnAngleMode(int _x, int _y, int _z) req.set_y(_y); req.set_z(_z); - this->dataPtr->node.Request(this->dataPtr->service, req, cb); + this->dataPtr->node.Request(this->dataPtr->viewAngleService, req, cb); +} + +///////////////////////////////////////////////// +QList ViewAngle::CamPose() const +{ + return QList({ + this->dataPtr->camPose.Pos().X(), + this->dataPtr->camPose.Pos().Y(), + this->dataPtr->camPose.Pos().Z(), + this->dataPtr->camPose.Rot().Roll(), + this->dataPtr->camPose.Rot().Pitch(), + this->dataPtr->camPose.Rot().Yaw() + }); +} + +///////////////////////////////////////////////// +void ViewAngle::SetCamPose(double _x, double _y, double _z, + double _roll, double _pitch, double _yaw) +{ + this->dataPtr->camPose.Set(_x, _y, _z, _roll, _pitch, _yaw); + + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error sending move camera to pose request" << std::endl; + }; + + ignition::msgs::GUICamera req; + msgs::Set(req.mutable_pose(), this->dataPtr->camPose); + + this->dataPtr->node.Request(this->dataPtr->moveToPoseService, req, cb); +} + +///////////////////////////////////////////////// +void ViewAngle::CamPoseCb(const msgs::Pose &_msg) +{ + std::lock_guard lock(this->dataPtr->mutex); + math::Pose3d pose = msgs::Convert(_msg); + + if (pose != this->dataPtr->camPose) + { + this->dataPtr->camPose = pose; + this->CamPoseChanged(); + } } // Register this plugin diff --git a/src/gui/plugins/view_angle/ViewAngle.hh b/src/gui/plugins/view_angle/ViewAngle.hh index 02cf34dae9..d58b064aed 100644 --- a/src/gui/plugins/view_angle/ViewAngle.hh +++ b/src/gui/plugins/view_angle/ViewAngle.hh @@ -18,6 +18,8 @@ #ifndef IGNITION_GAZEBO_GUI_VIEWANGLE_HH_ #define IGNITION_GAZEBO_GUI_VIEWANGLE_HH_ +#include + #include #include @@ -36,6 +38,13 @@ namespace gazebo { Q_OBJECT + /// \brief gui camera pose (QList order is x, y, z, roll, pitch, yaw) + Q_PROPERTY( + QList camPose + READ CamPose + NOTIFY CamPoseChanged + ) + /// \brief Constructor public: ViewAngle(); @@ -54,6 +63,22 @@ namespace gazebo /// to assume. All 0s for x, y, and z indicate the initial camera pose. public slots: void OnAngleMode(int _x, int _y, int _z); + /// \brief Get the current gui camera pose. + public: Q_INVOKABLE QList CamPose() const; + + /// \brief Notify that the gui camera pose has changed. + signals: void CamPoseChanged(); + + /// \brief Callback to update gui camera pose + /// \param[in] _x, _y, _z cartesion coordinates + /// \param[in] _roll, _pitch, _yaw principal coordinates + public slots: void SetCamPose(double _x, double _y, double _z, + double _roll, double _pitch, double _yaw); + + /// \brief Callback for retrieving gui camera pose + /// \param[in] _msg Pose message + public: void CamPoseCb(const msgs::Pose &_msg); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/src/gui/plugins/view_angle/ViewAngle.qml b/src/gui/plugins/view_angle/ViewAngle.qml index a0ea12e017..311ca12b3e 100644 --- a/src/gui/plugins/view_angle/ViewAngle.qml +++ b/src/gui/plugins/view_angle/ViewAngle.qml @@ -20,10 +20,12 @@ import QtQuick.Controls.Material 2.2 import QtQuick.Controls.Material.impl 2.2 import QtQuick.Layouts 1.3 import QtQuick.Controls.Styles 1.4 +import "qrc:/qml" ToolBar { - Layout.minimumWidth: 200 - Layout.minimumHeight: 200 + Layout.minimumWidth: 320 + Layout.minimumHeight: 380 + anchors.fill: parent background: Rectangle { color: "transparent" @@ -34,7 +36,9 @@ ToolBar { } GridLayout { - columns: 4 + id: views + anchors.horizontalCenter: parent.horizontalCenter + columns: 8 ToolButton { id: top checkable: true @@ -183,4 +187,143 @@ ToolBar { } } } + + // set camera pose + Rectangle { + y: views.height + 10 + width: parent.width + color: "transparent" + + ColumnLayout { + width: parent.width + Text { + text: "Camera Pose" + color: Material.Grey + Layout.row: 4 + Layout.column: 1 + leftPadding: 5 + } + + GridLayout { + width: parent.width + columns: 6 + + Text { + text: "X (m)" + color: "dimgrey" + Layout.row: 4 + Layout.column: 1 + leftPadding: 5 + } + IgnSpinBox { + id: x + Layout.fillWidth: true + Layout.row: 4 + Layout.column: 2 + value: ViewAngle.camPose[0] + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) + } + Text { + text: "Y (m)" + color: "dimgrey" + Layout.row: 5 + Layout.column: 1 + leftPadding: 5 + } + IgnSpinBox { + id: y + Layout.fillWidth: true + Layout.row: 5 + Layout.column: 2 + value: ViewAngle.camPose[1] + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) + } + Text { + text: "Z (m)" + color: "dimgrey" + Layout.row: 6 + Layout.column: 1 + leftPadding: 5 + } + IgnSpinBox { + id: z + Layout.fillWidth: true + Layout.row: 6 + Layout.column: 2 + value: ViewAngle.camPose[2] + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) + } + + Text { + text: "Roll (rad)" + color: "dimgrey" + Layout.row: 4 + Layout.column: 3 + leftPadding: 5 + } + IgnSpinBox { + id: roll + Layout.fillWidth: true + Layout.row: 4 + Layout.column: 4 + value: ViewAngle.camPose[3] + maximumValue: 6.28 + minimumValue: -6.28 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) + } + Text { + text: "Pitch (rad)" + color: "dimgrey" + Layout.row: 5 + Layout.column: 3 + leftPadding: 5 + } + IgnSpinBox { + id: pitch + Layout.fillWidth: true + Layout.row: 5 + Layout.column: 4 + value: ViewAngle.camPose[4] + maximumValue: 6.28 + minimumValue: -6.28 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) + } + Text { + text: "Yaw (rad)" + color: "dimgrey" + Layout.row: 6 + Layout.column: 3 + leftPadding: 5 + } + IgnSpinBox { + id: yaw + Layout.fillWidth: true + Layout.row: 6 + Layout.column: 4 + value: ViewAngle.camPose[5] + maximumValue: 6.28 + minimumValue: -6.28 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) + } + } + } + } }