diff --git a/examples/worlds/follow_actor.sdf b/examples/worlds/follow_actor.sdf new file mode 100644 index 0000000000..50423034c8 --- /dev/null +++ b/examples/worlds/follow_actor.sdf @@ -0,0 +1,364 @@ + + + + + + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/actors/control + /world/actors/stats + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/actors/stats + + + + + + + 0 + 0 + 263 + 50 + floating + false + #03a9f4 + + + + + + + false + docked + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0.0 0.0 1 + + + + + + + 0.0 0.0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 1 -2 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 1 0 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 1 2 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + 1 4 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 1 0 1 + 1 1 0 1 + 1 1 0 1 + + + + + + + walker_slow_red + + 0 -2 1.0 0 0 0 + https://fuel.ignitionrobotics.org/1.0/chapulina/models/Walking actor + + red_box + 1.0 + 8.0 + 1 + 4.58837 + + + + + walker_fast_green + 0 0 1.0 0 0 0 + https://fuel.ignitionrobotics.org/1.0/chapulina/models/Walking actor + + green_box + 1.0 + 8.0 + 4 + 4.58837 + + + + + runner_slow_blue + 0 2 1.0 0 0 0 + https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor + + blue_box + 1.0 + 8.0 + 1 + run + 1.5 + + + + + runner_fast_yellow + 0 4 1.0 0 0 0 + https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor + + yellow_box + 1.0 + 8.0 + 4 + run + 1.5 + + + + diff --git a/include/ignition/gazebo/components/Actor.hh b/include/ignition/gazebo/components/Actor.hh index aeca817612..c4db743488 100644 --- a/include/ignition/gazebo/components/Actor.hh +++ b/include/ignition/gazebo/components/Actor.hh @@ -17,6 +17,9 @@ #ifndef IGNITION_GAZEBO_COMPONENTS_ACTOR_HH_ #define IGNITION_GAZEBO_COMPONENTS_ACTOR_HH_ +#include +#include + #include #include @@ -37,6 +40,34 @@ namespace serializers { using ActorSerializer = serializers::ComponentToMsgSerializer; + + class AnimationTimeSerializer + { + /// \brief Serialization for `std::chrono::steady_clock::duration`. + /// \param[in] _out Output stream. + /// \param[in] _time Time to stream + /// \return The stream. + public: static std::ostream &Serialize(std::ostream &_out, + const std::chrono::steady_clock::duration &_time) + { + _out << std::chrono::duration_cast( + _time).count(); + return _out; + } + + /// \brief Deserialization for `std::chrono::steady_clock::duration`. + /// \param[in] _in Input stream. + /// \param[out] _time Time to populate + /// \return The stream. + public: static std::istream &Deserialize(std::istream &_in, + std::chrono::steady_clock::duration &_time) + { + int64_t time; + _in >> time; + _time = std::chrono::duration(time); + return _in; + } + }; } namespace components @@ -47,6 +78,17 @@ namespace components using Actor = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Actor", Actor) + + /// \brief Time in seconds within animation being currently played. + using AnimationTime = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.AnimationTime", + AnimationTime) + + /// \brief Name of animation being currently played. + using AnimationName = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.AnimationName", + AnimationName) } } } diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index d28a74a15f..1857dca09e 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -411,6 +411,13 @@ void ComponentInspector::Update(const UpdateInfo &, setUnit(item, "rad/s"); } } + else if (typeId == components::AnimationName::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } else if (typeId == components::CastShadows::typeId) { auto comp = _ecm.Component( @@ -506,6 +513,13 @@ void ComponentInspector::Update(const UpdateInfo &, if (comp) setData(item, comp->Data()); } + else if (typeId == components::TrajectoryPose::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } else if (typeId == components::WindMode::typeId) { auto comp = _ecm.Component(this->dataPtr->entity); diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index eadb608ef8..379ece6b57 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1029,13 +1029,34 @@ void RenderUtilPrivate::UpdateRenderingEntities( // Trajectory origin this->entityPoses[_entity] = _pose->Data(); - if (this->actorManualSkeletonUpdate) + auto animTimeComp = _ecm.Component(_entity); + auto animNameComp = _ecm.Component(_entity); + + // Animation time set through ECM so ign-rendering can calculate bone + // transforms + if (animTimeComp && animNameComp) + { + auto skel = this->sceneManager.ActorSkeletonById(_entity); + if (nullptr != skel) + { + AnimationUpdateData animData; + animData.loop = true; + animData.followTrajectory = true; + animData.animationName = animNameComp->Data(); + animData.time = animTimeComp->Data(); + animData.rootTransform = skel->RootNode()->Transform(); + animData.valid = true; + this->actorAnimationData[_entity] = animData; + } + } + // Bone poses calculated by ign-common + else if (this->actorManualSkeletonUpdate) { - // Bone poses calculated by ign-common this->actorTransforms[_entity] = this->sceneManager.ActorSkeletonTransformsAt( _entity, this->simTime); } + // Trajectory info from SDF so ign-rendering can calculate bone poses else { this->actorAnimationData[_entity] = diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 5f7a9cc111..413e0ec7cd 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -78,6 +78,7 @@ add_subdirectory(breadcrumbs) add_subdirectory(contact) add_subdirectory(detachable_joint) add_subdirectory(diff_drive) +add_subdirectory(follow_actor) add_subdirectory(imu) add_subdirectory(joint_controller) add_subdirectory(joint_state_publisher) diff --git a/src/systems/follow_actor/CMakeLists.txt b/src/systems/follow_actor/CMakeLists.txt new file mode 100644 index 0000000000..864371e1d6 --- /dev/null +++ b/src/systems/follow_actor/CMakeLists.txt @@ -0,0 +1,5 @@ +gz_add_system(follow-actor + SOURCES + FollowActor.cc +) + diff --git a/src/systems/follow_actor/FollowActor.cc b/src/systems/follow_actor/FollowActor.cc new file mode 100644 index 0000000000..32d51ef60a --- /dev/null +++ b/src/systems/follow_actor/FollowActor.cc @@ -0,0 +1,292 @@ +/* + * Copyright (C) 2020 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 +#include + +#include "FollowActor.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Private FollowActor data class. +class ignition::gazebo::systems::FollowActorPrivate +{ + /// \brief Entity for the actor. + public: Entity actorEntity{kNullEntity}; + + /// \brief Velocity of the actor + public: double velocity{0.8}; + + /// \brief Current target to follow + public: Entity targetEntity{kNullEntity}; + + /// \brief Minimum distance in meters to keep away from target. + public: double minDistance{1.2}; + + /// \brief Maximum distance in meters to keep away from target. + public: double maxDistance{4}; + + /// \brief Velocity of the animation dislocation on the X axis, in m/s. + /// Used to coordinate translational motion with the actor's feet. + /// TODO(louise) Automatically calculate it from the root node's first and + /// last keyframes + public: double animationXVel{2.0}; + + /// \brief Time of the last update. + public: std::chrono::steady_clock::duration lastUpdate{0}; + + /// \brief True if currently following + public: bool following{true}; +}; + +////////////////////////////////////////////////// +FollowActor::FollowActor() : + System(), dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +FollowActor::~FollowActor() = default; + +////////////////////////////////////////////////// +void FollowActor::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->actorEntity = _entity; + + auto actorComp = _ecm.Component(_entity); + if (!actorComp) + { + ignerr << "Entity [" << _entity << "] is not an actor." << std::endl; + return; + } + + if (!_sdf->HasElement("target")) + { + ignerr << "Missing , can't follow." << std::endl; + return; + } + + auto targetName = _sdf->Get("target"); + this->dataPtr->targetEntity = _ecm.EntityByComponents(components::Name( + targetName)); + if (kNullEntity == this->dataPtr->targetEntity) + { + ignerr << "Failed to find target entity [" << targetName << "]" + << std::endl; + return; + } + + if (_sdf->HasElement("velocity")) + this->dataPtr->velocity = _sdf->Get("velocity"); + + if (_sdf->HasElement("min_distance")) + this->dataPtr->minDistance = _sdf->Get("min_distance"); + + if (_sdf->HasElement("max_distance")) + this->dataPtr->maxDistance = _sdf->Get("max_distance"); + + if (_sdf->HasElement("animation_x_vel")) + this->dataPtr->animationXVel = _sdf->Get("animation_x_vel"); + + std::string animationName; + + // If animation not provided, use first one from SDF + if (!_sdf->HasElement("animation")) + { + if (actorComp->Data().AnimationCount() < 1) + { + ignerr << "Actor SDF doesn't have any animations." << std::endl; + return; + } + + animationName = actorComp->Data().AnimationByIndex(0)->Name(); + } + else + { + animationName = _sdf->Get("animation"); + } + + if (animationName.empty()) + { + ignerr << "Can't find actor's animation name." << std::endl; + return; + } + + auto animationNameComp = _ecm.Component(_entity); + if (nullptr == animationNameComp) + { + _ecm.CreateComponent(_entity, components::AnimationName(animationName)); + } + else + { + *animationNameComp = components::AnimationName(animationName); + } + // Mark as a one-time-change so that the change is propagated to the GUI + _ecm.SetChanged(_entity, + components::AnimationName::typeId, ComponentState::OneTimeChange); + + // Set custom animation time from this plugin + auto animTimeComp = _ecm.Component(_entity); + if (nullptr == animTimeComp) + { + _ecm.CreateComponent(_entity, components::AnimationTime()); + } + + math::Pose3d initialPose; + auto poseComp = _ecm.Component(_entity); + if (nullptr == poseComp) + { + _ecm.CreateComponent(_entity, components::Pose( + math::Pose3d::Zero)); + } + else + { + initialPose = poseComp->Data(); + + // We'll be setting the actor's X/Y pose with respect to the world. So we + // zero the current values. + auto newPose = initialPose; + newPose.Pos().X(0); + newPose.Pos().Y(0); + *poseComp = components::Pose(newPose); + } + + // Having a trajectory pose prevents the actor from moving with the + // SDF script + auto trajPoseComp = _ecm.Component(_entity); + if (nullptr == trajPoseComp) + { + // Leave Z to the pose component, control only 2D with Trajectory + initialPose.Pos().Z(0); + _ecm.CreateComponent(_entity, components::TrajectoryPose(initialPose)); + } +} + +////////////////////////////////////////////////// +void FollowActor::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("FollowActor::PreUpdate"); + + if (_info.paused) + return; + + // TODO(louise) Throttle this system + + // Time delta + std::chrono::duration dtDuration = _info.simTime - + this->dataPtr->lastUpdate; + double dt = dtDuration.count(); + + this->dataPtr->lastUpdate = _info.simTime; + + // Is there a follow target? + if (this->dataPtr->targetEntity == kNullEntity) + return; + + // Current world pose + auto trajPoseComp = _ecm.Component( + this->dataPtr->actorEntity); + auto actorPose = trajPoseComp->Data(); + auto initialPose = actorPose; + + // Current target + auto targetPose = _ecm.Component( + this->dataPtr->targetEntity)->Data(); + + // Direction to target + auto dir = targetPose.Pos() - actorPose.Pos(); + dir.Z(0); + + // Stop if too close to target + if (dir.Length() <= this->dataPtr->minDistance) + { + return; + } + + // Stop following if too far from target + if (dir.Length() > this->dataPtr->maxDistance) + { + if (this->dataPtr->following) + { + ignmsg << "Target [" << this->dataPtr->targetEntity + << "] too far, actor [" << this->dataPtr->actorEntity + <<"] stopped following" << std::endl; + this->dataPtr->following = false; + } + return; + } + if (!this->dataPtr->following) + { + ignmsg << "Target [" << this->dataPtr->targetEntity + << "] within range, actor [" << this->dataPtr->actorEntity + <<"] started following" << std::endl; + this->dataPtr->following = true; + } + + dir.Normalize(); + + // Towards target + math::Angle yaw = atan2(dir.Y(), dir.X()); + yaw.Normalize(); + + actorPose.Pos() += dir * this->dataPtr->velocity * dt; + actorPose.Pos().Z(0); + actorPose.Rot() = math::Quaterniond(0, 0, yaw.Radian()); + + // Distance traveled is used to coordinate motion with the walking + // animation + double distanceTraveled = (actorPose.Pos() - initialPose.Pos()).Length(); + + // Update actor root pose + *trajPoseComp = components::TrajectoryPose(actorPose); + // Mark as a one-time-change so that the change is propagated to the GUI + _ecm.SetChanged(this->dataPtr->actorEntity, + components::TrajectoryPose::typeId, ComponentState::OneTimeChange); + + // Update actor bone trajectories based on animation time + auto animTimeComp = _ecm.Component( + this->dataPtr->actorEntity); + + auto animTime = animTimeComp->Data() + + std::chrono::duration_cast( + std::chrono::duration(distanceTraveled * + this->dataPtr->animationXVel)); + *animTimeComp = components::AnimationTime(animTime); + // Mark as a one-time-change so that the change is propagated to the GUI + _ecm.SetChanged(this->dataPtr->actorEntity, + components::AnimationTime::typeId, ComponentState::OneTimeChange); +} + +IGNITION_ADD_PLUGIN(FollowActor, System, + FollowActor::ISystemConfigure, + FollowActor::ISystemPreUpdate +) + +IGNITION_ADD_PLUGIN_ALIAS(FollowActor, "ignition::gazebo::systems::FollowActor") diff --git a/src/systems/follow_actor/FollowActor.hh b/src/systems/follow_actor/FollowActor.hh new file mode 100644 index 0000000000..85743aa161 --- /dev/null +++ b/src/systems/follow_actor/FollowActor.hh @@ -0,0 +1,85 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_SYSTEMS_FOLLOWACTOR_HH_ +#define IGNITION_GAZEBO_SYSTEMS_FOLLOWACTOR_HH_ + +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declarations. + class FollowActorPrivate; + + /// \class FollowActor FollowActor.hh ignition/gazebo/systems/FollowActor.hh + /// \brief Make an actor follow a target entity in the world. + /// + /// ## SDF parameters + /// + /// : Name of entity to follow. + /// + /// : Distance in meters to keep from target's origin. + /// + /// : Distance in meters from target's origin when to stop + /// following. When the actor is back within range it starts + /// following again. + /// + /// : Actor's velocity in m/s + /// + /// : Actor's animation to play. If empty, the first animation in + /// the model will be used. + /// + /// : Velocity of the animation on the X axis. Used to + /// coordinate translational motion with the actor's + /// animation. + class IGNITION_GAZEBO_VISIBLE FollowActor: + public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: explicit FollowActor(); + + /// \brief Destructor + public: ~FollowActor() override; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) final; + + /// Documentation inherited + public: void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + }; + } +} +} +} +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index c07d6e1383..8c93e19f43 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -14,6 +14,7 @@ set(tests entity_erase.cc events.cc examples_build.cc + follow_actor_system.cc imu_system.cc joint_controller_system.cc lift_drag_system.cc diff --git a/test/integration/follow_actor_system.cc b/test/integration/follow_actor_system.cc new file mode 100644 index 0000000000..562bcd905f --- /dev/null +++ b/test/integration/follow_actor_system.cc @@ -0,0 +1,189 @@ +/* + * Copyright (C) 2020 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 "ignition/gazebo/components/Actor.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "plugins/MockSystem.hh" + +#define tol 10e-4 + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test FollowActor system +class FollowActorTest : public ::testing::TestWithParam +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); + } +}; + +class Relay +{ + public: Relay() + { + auto plugin = loader.LoadPlugin("libMockSystem.so", + "ignition::gazebo::MockSystem", + nullptr); + EXPECT_TRUE(plugin.has_value()); + + this->systemPtr = plugin.value(); + + this->mockSystem = static_cast( + systemPtr->QueryInterface()); + EXPECT_NE(nullptr, this->mockSystem); + } + + public: Relay &OnPreUpdate(MockSystem::CallbackType _cb) + { + this->mockSystem->preUpdateCallback = std::move(_cb); + return *this; + } + + public: Relay &OnUpdate(MockSystem::CallbackType _cb) + { + this->mockSystem->updateCallback = std::move(_cb); + return *this; + } + + public: Relay &OnPostUpdate(MockSystem::CallbackTypeConst _cb) + { + this->mockSystem->postUpdateCallback = std::move(_cb); + return *this; + } + + public: SystemPluginPtr systemPtr; + + private: SystemLoader loader; + private: MockSystem *mockSystem; +}; + + +///////////////////////////////////////////////// +TEST_P(FollowActorTest, PublishCmd) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/follow_actor.sdf"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + unsigned int preUpdateCount{0}; + unsigned int postUpdateCount{0}; + unsigned int boxMoveCount{0}; + + // Create a system that records the actor poses + Relay testSystem; + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &_info, + gazebo::EntityComponentManager &_ecm) + { + auto entity = _ecm.EntityByComponents( + components::Name("box")); + EXPECT_NE(kNullEntity, entity); + + auto poseComp = _ecm.Component(entity); + ASSERT_NE(nullptr, poseComp); + + // Move box every 500 ms + if (_info.iterations % 500 == 0) + { + auto pose = poseComp->Data(); + pose.Pos().X() += 0.5; + poseComp->SetData(pose, + [](const math::Pose3d &, const math::Pose3d &){return true;}); + boxMoveCount++; + } + + preUpdateCount++; + EXPECT_EQ(_info.iterations, preUpdateCount); + }); + + testSystem.OnPostUpdate( + [&](const gazebo::UpdateInfo &_info, + const gazebo::EntityComponentManager &_ecm) + { + auto actorEntity = _ecm.EntityByComponents( + components::Name("walker")); + EXPECT_NE(kNullEntity, actorEntity); + + auto targetEntity = _ecm.EntityByComponents( + components::Name("box")); + EXPECT_NE(kNullEntity, targetEntity); + + auto animNameComp = _ecm.Component(actorEntity); + ASSERT_NE(nullptr, animNameComp); + EXPECT_EQ("walking", animNameComp->Data()); + + // Animation time is always increasing + auto animTimeComp = _ecm.Component(actorEntity); + ASSERT_NE(nullptr, animTimeComp); + + // Actor pose is fixed to zero X/Y and initial Z + auto actorPoseComp = _ecm.Component(actorEntity); + ASSERT_NE(nullptr, actorPoseComp); + EXPECT_EQ(math::Pose3d(0, 0, 1, 0, 0, 0), actorPoseComp->Data()); + + // Target is moving + auto targetPoseComp = _ecm.Component(targetEntity); + ASSERT_NE(nullptr, targetPoseComp); + EXPECT_DOUBLE_EQ(1.0 + static_cast(_info.iterations / 500) * 0.5, + targetPoseComp->Data().Pos().X()); + + // Actor trajectory pose X increases to follow target + auto actorTrajPoseComp = _ecm.Component( + actorEntity); + ASSERT_NE(nullptr, actorTrajPoseComp); + + // Actor is behind box, respecting min_distance and never staying too far + auto diff = targetPoseComp->Data().Pos() - + actorTrajPoseComp->Data().Pos(); + EXPECT_GE(diff.X(), 1.0) << _info.iterations; + EXPECT_LE(diff.X(), 1.8) << _info.iterations; + + postUpdateCount++; + EXPECT_EQ(_info.iterations, postUpdateCount); + }); + server.AddSystem(testSystem.systemPtr); + + unsigned int iterations{400}; + server.Run(true /* blocking */, iterations, false /* paused */); + EXPECT_EQ(iterations, preUpdateCount); + EXPECT_EQ(iterations, postUpdateCount); + EXPECT_EQ(iterations / 500, boxMoveCount); +} + +// Run multiple times +INSTANTIATE_TEST_CASE_P(ServerRepeat, FollowActorTest, + ::testing::Range(1, 2)); diff --git a/test/worlds/follow_actor.sdf b/test/worlds/follow_actor.sdf new file mode 100644 index 0000000000..1bc7015a70 --- /dev/null +++ b/test/worlds/follow_actor.sdf @@ -0,0 +1,57 @@ + + + + + true + 1 -2 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + walker + + 0 -2 1.0 0 0 0 + https://fuel.ignitionrobotics.org/1.0/chapulina/models/Walking actor + + box + 1.0 + 8.0 + 1 + 4.58837 + + + +