From 279e4ffebfaa7aa165f4e02962db92e669577f4d Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 24 Jun 2021 14:51:22 -0700 Subject: [PATCH 1/6] Fix mouse view control target position (#879) * fix mouse view control race condition Signed-off-by: Ian Chen * add dragging Signed-off-by: Ian Chen --- src/gui/plugins/scene3d/Scene3D.cc | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 78853d824a..66c31073c3 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -251,7 +251,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::RayQueryPtr rayQuery; /// \brief View control focus target - public: math::Vector3d target; + public: math::Vector3d target = math::Vector3d( + math::INF_D, math::INF_D, math::INF_D); /// \brief Rendering utility public: RenderUtil renderUtil; @@ -1581,12 +1582,24 @@ void IgnRenderer::HandleMouseViewControl() } else { - if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::PRESS) + if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::PRESS || + // the rendering thread may miss the press event due to + // race condition when doing a drag operation (press and move, where + // the move event overrides the press event before it is processed) + // so we double check to see if target is set or not + (this->dataPtr->mouseEvent.Type() == common::MouseEvent::MOVE && + this->dataPtr->mouseEvent.Dragging() && + std::isinf(this->dataPtr->target.X()))) { this->dataPtr->target = this->ScreenToScene( this->dataPtr->mouseEvent.PressPos()); this->dataPtr->viewControl.SetTarget(this->dataPtr->target); } + // unset the target on release (by setting to inf) + else if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE) + { + this->dataPtr->target = ignition::math::INF_D; + } // Pan with left button if (this->dataPtr->mouseEvent.Buttons() & common::MouseEvent::LEFT) From 5d2c076dfd16886146d62df2ac0bc8e8dea5fdc8 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 25 Jun 2021 10:03:09 -0700 Subject: [PATCH 2/6] Fix codecheck (#887) Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- src/rendering/RenderUtil.cc | 3 ++- src/systems/diff_drive/DiffDrive.cc | 3 +-- test/integration/diff_drive_system.cc | 3 +++ test/integration/odometry_publisher.cc | 1 + test/integration/optical_tactile_plugin.cc | 1 + 5 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 600ccc84f2..eff16149ea 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1875,7 +1875,8 @@ void RenderUtilPrivate::RemoveSensor(const Entity _entity) auto sensorEntityIt = this->sensorEntities.find(_entity); if (sensorEntityIt != this->sensorEntities.end()) { - this->removeSensorCb(_entity); + if (this->removeSensorCb) + this->removeSensorCb(_entity); this->sensorEntities.erase(sensorEntityIt); } } diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 25aa51e381..27226d01af 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -509,8 +509,7 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, // Construct the Pose_V/tf message and publish it. msgs::Pose_V tfMsg; - ignition::msgs::Pose *tfMsgPose = nullptr; - tfMsgPose = tfMsg.add_pose(); + ignition::msgs::Pose *tfMsgPose = tfMsg.add_pose(); tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header()); tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position()); tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation()); diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index 20df077115..2a91cba74a 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -440,6 +440,7 @@ TEST_P(DiffDriveTest, OdomCustomFrameId) int sleep = 0; int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -499,6 +500,7 @@ TEST_P(DiffDriveTest, Pose_VFrameId) int sleep = 0; int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -558,6 +560,7 @@ TEST_P(DiffDriveTest, Pose_VCustomFrameId) int sleep = 0; int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc index 248f33e18d..1488dbd14e 100644 --- a/test/integration/odometry_publisher.cc +++ b/test/integration/odometry_publisher.cc @@ -260,6 +260,7 @@ class OdometryPublisherTest : public ::testing::TestWithParam int sleep = 0; int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); diff --git a/test/integration/optical_tactile_plugin.cc b/test/integration/optical_tactile_plugin.cc index ea6ec02e85..45ac53609c 100644 --- a/test/integration/optical_tactile_plugin.cc +++ b/test/integration/optical_tactile_plugin.cc @@ -161,6 +161,7 @@ TEST_F(OpticalTactilePluginTest, // Give some time for messages to propagate sleep = 0; + // cppcheck-suppress knownConditionTrueFalse while (!receivedMsg && sleep < maxSleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); From dd9f7b5d586e996684e8f9360047099700551a96 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Sat, 26 Jun 2021 00:17:19 -0400 Subject: [PATCH 3/6] Use UINT64_MAX for kComponentTpyeIDInvalid instead of relying on underflow (#889) Signed-off-by: Ashton Larkin --- include/ignition/gazebo/Types.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/gazebo/Types.hh b/include/ignition/gazebo/Types.hh index 8cc847809c..b475d1fdeb 100644 --- a/include/ignition/gazebo/Types.hh +++ b/include/ignition/gazebo/Types.hh @@ -98,7 +98,7 @@ namespace ignition static const ComponentId kComponentIdInvalid = -1; /// \brief Id that indicates an invalid component type. - static const ComponentTypeId kComponentTypeIdInvalid = -1; + static const ComponentTypeId kComponentTypeIdInvalid = UINT64_MAX; } } } From 15b456bc79a040279d645f1b630d3a6955768775 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Thu, 1 Jul 2021 19:52:30 -0400 Subject: [PATCH 4/6] Fix documentation for the Sensor component (#898) Signed-off-by: Louise Poubel --- include/ignition/gazebo/components/Sensor.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/gazebo/components/Sensor.hh b/include/ignition/gazebo/components/Sensor.hh index 97ca8d74a0..2d7ddb39a3 100644 --- a/include/ignition/gazebo/components/Sensor.hh +++ b/include/ignition/gazebo/components/Sensor.hh @@ -31,7 +31,7 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { - /// \brief A component that identifies an entity as being a link. + /// \brief A component that identifies an entity as being a sensor. using Sensor = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Sensor", Sensor) From 12f12375e6a0b06ad943334ef9d3cfe0bf330802 Mon Sep 17 00:00:00 2001 From: addy1997 Date: Fri, 2 Jul 2021 05:27:17 +0530 Subject: [PATCH 5/6] Hello world plugin added (#699) Signed-off-by: addy1997 Signed-off-by: Louise Poubel Co-authored-by: Jose Luis Rivero Co-authored-by: Louise Poubel --- examples/plugin/hello_world/CMakeLists.txt | 17 ++++++ examples/plugin/hello_world/HelloWorld.cc | 56 +++++++++++++++++++ examples/plugin/hello_world/HelloWorld.hh | 46 +++++++++++++++ examples/plugin/hello_world/README.md | 42 ++++++++++++++ .../plugin/hello_world/hello_world_plugin.sdf | 11 ++++ 5 files changed, 172 insertions(+) create mode 100644 examples/plugin/hello_world/CMakeLists.txt create mode 100644 examples/plugin/hello_world/HelloWorld.cc create mode 100644 examples/plugin/hello_world/HelloWorld.hh create mode 100644 examples/plugin/hello_world/README.md create mode 100644 examples/plugin/hello_world/hello_world_plugin.sdf diff --git a/examples/plugin/hello_world/CMakeLists.txt b/examples/plugin/hello_world/CMakeLists.txt new file mode 100644 index 0000000000..e02e8adedb --- /dev/null +++ b/examples/plugin/hello_world/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +find_package(ignition-cmake2 REQUIRED) + +project(Hello_world) + +ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register) +set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) + +ign_find_package(ignition-gazebo4 REQUIRED) +set(IGN_GAZEBO_VER ${ignition-gazebo4_VERSION_MAJOR}) + +add_library(HelloWorld SHARED HelloWorld) +set_property(TARGET HelloWorld PROPERTY CXX_STANDARD 17) +target_link_libraries(HelloWorld + PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} + PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER}) diff --git a/examples/plugin/hello_world/HelloWorld.cc b/examples/plugin/hello_world/HelloWorld.cc new file mode 100644 index 0000000000..d73ed28b6c --- /dev/null +++ b/examples/plugin/hello_world/HelloWorld.cc @@ -0,0 +1,56 @@ +/* + * 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. + * +*/ + +// We'll use a string and the ignmsg command below for a brief example. +// Remove these includes if your plugin doesn't need them. +#include +#include + +// This header is required to register plugins. It's good practice to place it +// in the cc file, like it's done here. +#include + +// Don't forget to include the plugin's header. +#include "HelloWorld.hh" + +// This is required to register the plugin. Make sure the interfaces match +// what's in the header. +IGNITION_ADD_PLUGIN( + hello_world::HelloWorld, + ignition::gazebo::System, + hello_world::HelloWorld::ISystemPostUpdate) + +using namespace hello_world; + +// Here we implement the PostUpdate function, which is called at every +// iteration. +void HelloWorld::PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &/*_ecm*/) +{ + // This is a simple example of how to get information from UpdateInfo. + std::string msg = "Hello, world! Simulation is "; + if (!_info.paused) + msg += "not "; + msg += "paused."; + + // Messages printed with ignmsg only show when running with verbosity 3 or + // higher (i.e. ign gazebo -v 3) + ignmsg << msg << std::endl; +} + + + diff --git a/examples/plugin/hello_world/HelloWorld.hh b/examples/plugin/hello_world/HelloWorld.hh new file mode 100644 index 0000000000..ff60e09392 --- /dev/null +++ b/examples/plugin/hello_world/HelloWorld.hh @@ -0,0 +1,46 @@ +/* + * 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 SYSTEM_PLUGIN_HELLOWORLD_HH_ +#define SYSTEM_PLUGIN_HELLOWORLD_HH_ + +// The only required include in the header is this one. +// All others will depend on what your plugin does. +#include + +// It's good practice to use a custom namespace for your project. +namespace hello_world +{ + // This is the main plugin's class. It must inherit from System and at least + // one other interface. + // Here we use `ISystemPostUpdate`, which is used to get results after + // physics runs. The opposite of that, `ISystemPreUpdate`, would be used by + // plugins that want to send commands. + class HelloWorld: + public ignition::gazebo::System, + public ignition::gazebo::ISystemPostUpdate + { + // Plugins inheriting ISystemPostUpdate must implement the PostUpdate + // callback. This is called at every simulation iteration after the physics + // updates the world. The _info variable provides information such as time, + // while the _ecm provides an interface to all entities and components in + // simulation. + public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) override; + }; +} +#endif diff --git a/examples/plugin/hello_world/README.md b/examples/plugin/hello_world/README.md new file mode 100644 index 0000000000..75422b8f93 --- /dev/null +++ b/examples/plugin/hello_world/README.md @@ -0,0 +1,42 @@ +# Hello world + +This example contains the bare minimum that's necessary to create a Gazebo +system plugin. + +## Build + +From the root of the `ign-gazebo` repository, do the following to build the example: + +~~~ +cd ign-gazebo/examples/plugins/hello_world +mkdir build +cd build +cmake .. +make +~~~ + +This will generate the `HelloWorld` library under `build`. + +## Run + +The plugin must be attached to an entity to be loaded. This is demonstrated in +the `hello_world_plugin.sdf` file that's going to be loaded. + +Before starting Gazebo, we must make sure it can find the plugin by doing: + +~~~ +cd ign-gazebo/examples/plugins/hello_world +export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=`pwd`/build +~~~ + +Then load the example world: + + ign gazebo -v 3 hello_world_plugin.sdf + +You should see green messages on the terminal like: + +``` +[Msg] Hello, world! Simulation is paused. +``` + +Toggle the play / pause buttons to see the message change. diff --git a/examples/plugin/hello_world/hello_world_plugin.sdf b/examples/plugin/hello_world/hello_world_plugin.sdf new file mode 100644 index 0000000000..b304e03302 --- /dev/null +++ b/examples/plugin/hello_world/hello_world_plugin.sdf @@ -0,0 +1,11 @@ + + + + + + + + From e3fbbcd4944d179c1b663fca6027648a4c938a44 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Mon, 12 Jul 2021 15:43:25 -0600 Subject: [PATCH 6/6] Physics system: update link poses if the canonical link pose has been updated (#876) Signed-off-by: Ashton Larkin --- src/systems/physics/Physics.cc | 182 +++++++++++++++------- test/integration/physics_system.cc | 98 ++++++++++++ test/worlds/only_canonical_link_moves.sdf | 139 +++++++++++++++++ 3 files changed, 360 insertions(+), 59 deletions(-) create mode 100644 test/worlds/only_canonical_link_moves.sdf diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 5b5ed22293..aba20dec1b 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -1868,15 +1868,30 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) // Link poses, velocities... IGN_PROFILE_BEGIN("Links"); - _ecm.Each( - [&](const Entity &_entity, components::Link * /*_link*/, - components::Pose *_pose, + // Process pose updates of canonical links that moved first, so that the + // corresponding model pose is up-to-date. This is necessary because the poses + // of non-canonical links are saved w.r.t. the top level model they're + // attached to. If a canonical link's pose changed, then all other links that + // belong to this model will need to have their pose updated. We keep track of + // which models had a canonical link update so that we update all link poses + // in this model later. + // + // We also keep track of each canonical link's FrameData because calling + // FrameDataRelativeToWorld can be expensive, so we want to make sure that we + // avoid calling this again on canonical links when doing the remaining link + // update work later. + std::unordered_set modelPoseChanged; + std::unordered_map canonicalLinkWorldFrameData; + _ecm.Each( + [&](const Entity &_entity, components::CanonicalLink * /*_link*/, const components::ParentEntity *_parent)->bool { // If parent is static, don't process pose changes as periodic if (this->staticEntities.find(_entity) != this->staticEntities.end()) return true; + IGN_PROFILE_BEGIN("Local pose"); + auto linkPhys = this->entityLinkMap.Get(_entity); if (nullptr == linkPhys) { @@ -1884,21 +1899,12 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) << "] not in entity map" << std::endl; return true; } - - IGN_PROFILE_BEGIN("Local pose"); - // get top level model of this link - auto topLevelModelEnt = this->topLevelModelMap[_parent->Data()]; - - auto canonicalLink = - _ecm.Component(_entity); - auto frameData = linkPhys->FrameDataRelativeToWorld(); - const auto &worldPose = frameData.pose; + canonicalLinkWorldFrameData[_entity] = frameData; + const auto worldPoseMath3d = math::eigen3::convert(frameData.pose); - // update the link or top level model pose if this is the first update, - // or if the link pose has changed since the last update - // (if the link pose hasn't changed, there's no need for a pose update) - const auto worldPoseMath3d = ignition::math::eigen3::convert(worldPose); + // update the top level model pose if this is the first update, + // or if the canonical link pose has changed since the last update if ((this->linkWorldPoses.find(_entity) == this->linkWorldPoses.end()) || !this->pose3Eql(this->linkWorldPoses[_entity], worldPoseMath3d)) { @@ -1906,52 +1912,110 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) // during the next iteration this->linkWorldPoses[_entity] = worldPoseMath3d; - if (canonicalLink) + auto topLevelModelEnt = this->topLevelModelMap[_parent->Data()]; + modelPoseChanged.insert(topLevelModelEnt); + + // Since this is the canonical link, update the top level model. + // The pose of this link w.r.t its top level model never changes + // because it's "fixed" to the model. Instead, we change + // the top level model's pose here. The physics engine gives us the + // pose of this link relative to world so to set the top level + // model's pose, we have to post-multiply it by the inverse of the + // transform of the link w.r.t to its top level model. + math::Pose3d linkPoseFromTopLevelModel; + linkPoseFromTopLevelModel = + this->RelativePose(topLevelModelEnt, _entity, _ecm); + + // update top level model's pose + auto mutableModelPose = + _ecm.Component(topLevelModelEnt); + *(mutableModelPose) = components::Pose( + worldPoseMath3d * linkPoseFromTopLevelModel.Inverse()); + + _ecm.SetChanged(topLevelModelEnt, components::Pose::typeId, + ComponentState::PeriodicChange); + } + + return true; + }); + + // Now that all canonical link pose updates have been processed and all model + // poses are up-to-date, we perform all other link updates. + _ecm.Each( + [&](const Entity &_entity, components::Link * /*_link*/, + components::Pose *_pose, + const components::ParentEntity *_parent)->bool + { + // If parent is static, don't process pose changes as periodic + if (this->staticEntities.find(_entity) != this->staticEntities.end()) + return true; + + IGN_PROFILE_BEGIN("Local pose"); + + // if we're processing a canonical link, we can access the cached + // FrameData instead of calling FrameDataRelativeToWorld again (this can + // be an expensive call) + physics::FrameData3d frameData; + auto frameDataIter = canonicalLinkWorldFrameData.find(_entity); + bool isCanonicalLink = + frameDataIter != canonicalLinkWorldFrameData.end(); + if (isCanonicalLink) + { + frameData = frameDataIter->second; + } + else + { + auto linkPhys = this->entityLinkMap.Get(_entity); + if (nullptr == linkPhys) { - // This is the canonical link, update the top level model. - // The pose of this link w.r.t its top level model never changes - // because it's "fixed" to the model. Instead, we change - // the top level model's pose here. The physics engine gives us the - // pose of this link relative to world so to set the top level - // model's pose, we have to post-multiply it by the inverse of the - // transform of the link w.r.t to its top level model. - math::Pose3d linkPoseFromTopLevelModel; - linkPoseFromTopLevelModel = - this->RelativePose(topLevelModelEnt, _entity, _ecm); - - // update top level model's pose - auto mutableModelPose = - _ecm.Component(topLevelModelEnt); - *(mutableModelPose) = components::Pose( - math::eigen3::convert(worldPose) * - linkPoseFromTopLevelModel.Inverse()); - - _ecm.SetChanged(topLevelModelEnt, components::Pose::typeId, - ComponentState::PeriodicChange); + ignerr << "Internal error: link [" << _entity + << "] not in entity map" << std::endl; + return true; } - else + frameData = linkPhys->FrameDataRelativeToWorld(); + } + const auto &worldPose = frameData.pose; + + auto topLevelModelEnt = this->topLevelModelMap[_parent->Data()]; + + // Update the link pose if this is the first update, or if the link pose + // has changed since the last update. If this link is a canonical link, + // we can skip it since we just updated all of the canonical link poses. + // We should also update the link pose if the link's top level model + // pose has been updated because non-canonical links are saved w.r.t. + // the top level model (if the top level model pose changes, but we + // don't update the pose of the model's non-canonical links, then it + // seems like the link(s) have changed pose, which may not be true). + const auto worldPoseMath3d = ignition::math::eigen3::convert(worldPose); + if (!isCanonicalLink && + ((this->linkWorldPoses.find(_entity) == this->linkWorldPoses.end()) + || modelPoseChanged.find(topLevelModelEnt) != modelPoseChanged.end() + || !this->pose3Eql(this->linkWorldPoses[_entity], worldPoseMath3d))) + { + // cache the updated link pose to check if the link pose has changed + // during the next iteration + this->linkWorldPoses[_entity] = worldPoseMath3d; + + // Compute the relative pose of this link from the top level model + // first get the world pose of the top level model + auto worldComp = + _ecm.Component(topLevelModelEnt); + // if the worldComp is a nullptr, something is wrong with ECS + if (!worldComp) { - // Compute the relative pose of this link from the top level model - // first get the world pose of the top level model - auto worldComp = - _ecm.Component(topLevelModelEnt); - // if the worldComp is a nullptr, something is wrong with ECS - if (!worldComp) - { - ignerr << "The parent component of " << topLevelModelEnt - << " could not be found. This should never happen!\n"; - return true; - } - math::Pose3d parentWorldPose = - this->RelativePose(worldComp->Data(), _parent->Data(), _ecm); - - // Unlike canonical links, pose of regular links can move relative - // to the parent. Same for links inside nested models. - *_pose = components::Pose(math::eigen3::convert(worldPose) + - parentWorldPose.Inverse()); - _ecm.SetChanged(_entity, components::Pose::typeId, - ComponentState::PeriodicChange); + ignerr << "The parent component of " << topLevelModelEnt + << " could not be found. This should never happen!\n"; + return true; } + math::Pose3d parentWorldPose = + this->RelativePose(worldComp->Data(), _parent->Data(), _ecm); + + // Unlike canonical links, pose of regular links can move relative + // to the parent. Same for links inside nested models. + *_pose = components::Pose(parentWorldPose.Inverse() * + worldPoseMath3d); + _ecm.SetChanged(_entity, components::Pose::typeId, + ComponentState::PeriodicChange); } IGN_PROFILE_END(); @@ -1962,7 +2026,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) if (worldPoseComp) { auto state = - worldPoseComp->SetData(math::eigen3::convert(frameData.pose), + worldPoseComp->SetData(worldPoseMath3d, this->pose3Eql) ? ComponentState::PeriodicChange : ComponentState::NoChange; diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index c0fe7c662f..028f1c8c0a 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -33,6 +34,7 @@ #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/Util.hh" #include "ignition/gazebo/test_config.hh" // NOLINT(build/include) #include "ignition/gazebo/components/AxisAlignedBox.hh" @@ -987,3 +989,99 @@ TEST_F(PhysicsSystemFixture, NestedModel) EXPECT_NE(parents.end(), parentIt); EXPECT_EQ("model_01", parentIt->second); } + +///////////////////////////////////////////////// +// This tests whether pose updates are correct for a model whose canonical link +// changes, but other links do not +TEST_F(PhysicsSystemFixture, MovingCanonicalLinkOnly) +{ + ignition::gazebo::ServerConfig serverConfig; + + const auto sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", + "only_canonical_link_moves.sdf"); + + sdf::Root root; + root.Load(sdfFile); + const sdf::World *world = root.WorldByIndex(0); + ASSERT_TRUE(nullptr != world); + + serverConfig.SetSdfFile(sdfFile); + + gazebo::Server server(serverConfig); + + server.SetUpdatePeriod(1us); + + // Create a system that records the poses of the links after physics + test::Relay testSystem; + + size_t numBaseLinkChecks = 0; + size_t numOuterLinkChecks = 0; + size_t numBaseLinkCustomChecks = 0; + size_t numOuterLinkCustomChecks = 0; + + size_t currIter = 0; + testSystem.OnPostUpdate( + [&world, &numBaseLinkChecks, &numOuterLinkChecks, &numBaseLinkCustomChecks, + &numOuterLinkCustomChecks, &currIter]( + const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + currIter++; + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Link *, const components::Name *_name)->bool + { + // ignore the link for the ground plane + if (_name->Data() != "surface") + { + const double dt = 0.001; + const double zExpected = + 0.5 * world->Gravity().Z() * pow(dt * currIter, 2); + + if (_name->Data() == "base_link") + { + // link "base_link" falls due to gravity, starting from rest + EXPECT_NEAR(zExpected, + ignition::gazebo::worldPose(_entity, _ecm).Z(), 1e-2); + numBaseLinkChecks++; + } + else if (_name->Data() == "link0_outer") + { + // link "link0_outer" is resting on the ground and does not + // move, so it should always have a pose of (1 0 0 0 0 0) + EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), + ignition::gazebo::worldPose(_entity, _ecm)); + numOuterLinkChecks++; + } + else if (_name->Data() == "base_link_custom") + { + // same as link "base_link" + EXPECT_NEAR(zExpected, + ignition::gazebo::worldPose(_entity, _ecm).Z(), 1e-2); + numBaseLinkCustomChecks++; + } + else if (_name->Data() == "link0_outer_custom") + { + // same as "link0_outer", but with an offset pose + EXPECT_EQ(ignition::math::Pose3d(1, 2, 0, 0, 0, 0), + ignition::gazebo::worldPose(_entity, _ecm)); + numOuterLinkCustomChecks++; + } + } + + return true; + }); + + return true; + }); + + server.AddSystem(testSystem.systemPtr); + const size_t iters = 500; + server.Run(true, iters, false); + + EXPECT_EQ(iters, currIter); + EXPECT_EQ(iters, numBaseLinkChecks); + EXPECT_EQ(iters, numOuterLinkChecks); + EXPECT_EQ(iters, numBaseLinkCustomChecks); + EXPECT_EQ(iters, numOuterLinkCustomChecks); +} diff --git a/test/worlds/only_canonical_link_moves.sdf b/test/worlds/only_canonical_link_moves.sdf new file mode 100644 index 0000000000..32abf7737b --- /dev/null +++ b/test/worlds/only_canonical_link_moves.sdf @@ -0,0 +1,139 @@ + + + + + + + + + 0 0 10 0 0 0 + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 2 2 + + + + + + + + + + + + + 0.2 + 0.1 + + + + 0.0 0.6 0.9 0.4 + 0.0 0.6 0.9 0.4 + + + + + + 1 0 0 0 0 0 + + + + 0.2 + 0.2 + + + + + + + 0.2 + 0.2 + + + + 0.6 0.6 0.9 1 + 0.6 0.6 0.9 1 + 1.0 1.0 1.0 1 + + + + + + + + 0 2 0 0 0 0 + + 1 0 0 0 0 0 + + + + 0.2 + 0.2 + + + + + + + 0.2 + 0.2 + + + + 0.6 0.6 0.9 1 + 0.6 0.6 0.9 1 + 1.0 1.0 1.0 1 + + + + + + + + + 0.2 + 0.1 + + + + 0.0 0.6 0.9 0.4 + 0.0 0.6 0.9 0.4 + + + + + + + +