diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 14436f260a..1199a2d42e 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -508,6 +508,16 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Actor *_actor) this->dataPtr->ecm->CreateComponent(actorEntity, components::Name(_actor->Name())); + // Links + for (uint64_t linkIndex = 0; linkIndex < _actor->LinkCount(); + ++linkIndex) + { + auto link = _actor->LinkByIndex(linkIndex); + auto linkEntity = this->CreateEntities(link); + + this->SetParent(linkEntity, actorEntity); + } + // Actor plugins this->dataPtr->eventManager->Emit(actorEntity, _actor->Plugins()); diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index f06e775018..8bcf59642e 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1204,6 +1204,13 @@ void RenderUtil::Update() entityId, std::get<1>(model), std::get<2>(model)); } + for (const auto &actor : newActors) + { + this->dataPtr->sceneManager.CreateActor( + std::get<0>(actor), std::get<1>(actor), std::get<2>(actor), + std::get<3>(actor)); + } + for (const auto &link : newLinks) { this->dataPtr->sceneManager.CreateLink( @@ -1216,13 +1223,6 @@ void RenderUtil::Update() std::get<0>(visual), std::get<1>(visual), std::get<2>(visual)); } - for (const auto &actor : newActors) - { - this->dataPtr->sceneManager.CreateActor( - std::get<0>(actor), std::get<1>(actor), std::get<2>(actor), - std::get<3>(actor)); - } - for (const auto &light : newLights) { auto newLightRendering = this->dataPtr->sceneManager.CreateLight( @@ -1336,7 +1336,7 @@ void RenderUtil::Update() { auto actorMesh = this->dataPtr->sceneManager.ActorMeshById(tf.first); auto actorVisual = this->dataPtr->sceneManager.NodeById(tf.first); - if (!actorMesh || !actorVisual) + if (!actorVisual) { ignerr << "Actor with Entity ID '" << tf.first << "'. not found. " << "Skipping skeleton animation update." << std::endl; @@ -1371,7 +1371,8 @@ void RenderUtil::Update() } tf.second.erase("actorPose"); - actorMesh->SetSkeletonLocalTransforms(tf.second); + if (actorMesh) + actorMesh->SetSkeletonLocalTransforms(tf.second); } } else @@ -3118,7 +3119,7 @@ void RenderUtilPrivate::UpdateAnimation(const std::unordered_mapsceneManager.NodeById(it.first); auto actorSkel = this->sceneManager.ActorSkeletonById( it.first); - if (!actorMesh || !actorVisual || !actorSkel) + if (!actorVisual) { ignerr << "Actor with Entity ID '" << it.first << "'. not found. " << "Skipping skeleton animation update." << std::endl; @@ -3131,51 +3132,55 @@ void RenderUtilPrivate::UpdateAnimation(const std::unordered_mapSkeletonAnimationEnabled(animData.animationName)) + + if (actorMesh && actorSkel) { - // disable all animations for this actor - for (unsigned int i = 0; i < actorSkel->AnimationCount(); ++i) + // Enable skeleton animation + if (!actorMesh->SkeletonAnimationEnabled(animData.animationName)) { + // disable all animations for this actor + for (unsigned int i = 0; i < actorSkel->AnimationCount(); ++i) + { + actorMesh->SetSkeletonAnimationEnabled( + actorSkel->Animation(i)->Name(), false, false, 0.0); + } + + // enable requested animation actorMesh->SetSkeletonAnimationEnabled( - actorSkel->Animation(i)->Name(), false, false, 0.0); + animData.animationName, true, animData.loop); + + // Set skeleton root node weight to zero so it is not affected by + // the animation being played. This is needed if trajectory animation + // is enabled. We need to let the trajectory animation set the + // position of the actor instead + common::SkeletonPtr skeleton = + this->sceneManager.ActorSkeletonById(it.first); + if (skeleton) + { + float rootBoneWeight = (animData.followTrajectory) ? 0.0 : 1.0; + std::unordered_map weights; + weights[skeleton->RootNode()->Name()] = rootBoneWeight; + actorMesh->SetSkeletonWeights(weights); + } } - - // enable requested animation - actorMesh->SetSkeletonAnimationEnabled( - animData.animationName, true, animData.loop); - - // Set skeleton root node weight to zero so it is not affected by - // the animation being played. This is needed if trajectory animation - // is enabled. We need to let the trajectory animation set the - // position of the actor instead - common::SkeletonPtr skeleton = - this->sceneManager.ActorSkeletonById(it.first); - if (skeleton) - { - float rootBoneWeight = (animData.followTrajectory) ? 0.0 : 1.0; - std::unordered_map weights; - weights[skeleton->RootNode()->Name()] = rootBoneWeight; - actorMesh->SetSkeletonWeights(weights); + // Update skeleton animation by setting animation time. + // Note that animation time is different from sim time. An actor can + // have multiple animations. Animation time is associated with + // current animation that is being played. It is also adjusted if + // interpotate_x is enabled. + actorMesh->UpdateSkeletonAnimation(animData.time); + + // manually update root transform in order to sync with trajectory + // animation + if (animData.followTrajectory) + { + common::SkeletonPtr skeleton = + this->sceneManager.ActorSkeletonById(it.first); + std::map rootTf; + rootTf[skeleton->RootNode()->Name()] = animData.rootTransform; + actorMesh->SetSkeletonLocalTransforms(rootTf); } } - // Update skeleton animation by setting animation time. - // Note that animation time is different from sim time. An actor can - // have multiple animations. Animation time is associated with - // current animation that is being played. It is also adjusted if - // interpotate_x is enabled. - actorMesh->UpdateSkeletonAnimation(animData.time); - - // manually update root transform in order to sync with trajectory - // animation - if (animData.followTrajectory) - { - common::SkeletonPtr skeleton = - this->sceneManager.ActorSkeletonById(it.first); - std::map rootTf; - rootTf[skeleton->RootNode()->Name()] = animData.rootTransform; - actorMesh->SetSkeletonLocalTransforms(rootTf); - } // update actor trajectory animation math::Pose3d globalPose; diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 450be7b8cb..0d6109cc57 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -115,6 +115,16 @@ class ignition::gazebo::SceneManagerPrivate /// also sets the time point in which the animation should be played public: AnimationUpdateData ActorTrajectoryAt( Entity _id, const std::chrono::steady_clock::duration &_time) const; + + /// \brief Load Actor trajectories + /// \param[in] _actor Actor + /// \param[in] _mapAnimNameId Animation name to id map + /// \param[in] _skel Mesh skeleton + /// \return Trajectory vector + public: std::vector + LoadTrajectories(const sdf::Actor &_actor, + std::unordered_map &_mapAnimNameId, + common::SkeletonPtr _skel); }; @@ -964,114 +974,39 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, descriptor.meshName = asFullPath(_actor.SkinFilename(), _actor.FilePath()); common::MeshManager *meshManager = common::MeshManager::Instance(); descriptor.mesh = meshManager->Load(descriptor.meshName); + std::unordered_map mapAnimNameId; + common::SkeletonPtr meshSkel; if (nullptr == descriptor.mesh) { - ignerr << "Actor skin mesh [" << descriptor.meshName << "] not found." - << std::endl; - return rendering::VisualPtr(); - } - - // todo(anyone) create a copy of meshSkel so we don't modify the original - // when adding animations! - common::SkeletonPtr meshSkel = descriptor.mesh->MeshSkeleton(); - if (nullptr == meshSkel) - { - ignerr << "Mesh skeleton in [" << descriptor.meshName << "] not found." - << std::endl; - return rendering::VisualPtr(); + ignwarn << "Actor skin mesh [" << descriptor.meshName << "] not found." + << std::endl; } - - // Load all animations - auto mapAnimNameId = this->LoadAnimations(_actor); - if (mapAnimNameId.size() == 0) - return nullptr; - - this->dataPtr->actorSkeletons[_id] = meshSkel; - - std::vector trajectories; - if (_actor.TrajectoryCount() != 0) + else { - // Load all trajectories specified in sdf - for (unsigned i = 0; i < _actor.TrajectoryCount(); i++) + // todo(anyone) create a copy of meshSkel so we don't modify the original + // when adding animations! + meshSkel = descriptor.mesh->MeshSkeleton(); + if (nullptr == meshSkel) { - const sdf::Trajectory *trajSdf = _actor.TrajectoryByIndex(i); - if (nullptr == trajSdf) - { - ignerr << "Null trajectory SDF for [" << _actor.Name() << "]" - << std::endl; - continue; - } - - common::TrajectoryInfo trajInfo; - trajInfo.SetId(trajSdf->Id()); - trajInfo.SetAnimIndex(mapAnimNameId[trajSdf->Type()]); - - if (trajSdf->WaypointCount() != 0) - { - std::map waypoints; - for (unsigned j = 0; j < trajSdf->WaypointCount(); j++) - { - auto point = trajSdf->WaypointByIndex(j); - TP pointTp(std::chrono::milliseconds( - static_cast(point->Time()*1000))); - waypoints[pointTp] = point->Pose(); - } - trajInfo.SetWaypoints(waypoints, trajSdf->Tension()); - // Animations are offset by 1 because index 0 is taken by the mesh name - auto animation = _actor.AnimationByIndex(trajInfo.AnimIndex()-1); - - if (animation && animation->InterpolateX()) - { - // warn if no x displacement can be interpolated - // warn only once per mesh - static std::unordered_set animInterpolateCheck; - if (animInterpolateCheck.count(animation->Filename()) == 0) - { - std::string rootNodeName = meshSkel->RootNode()->Name(); - common::SkeletonAnimation *skelAnim = - meshSkel->Animation(trajInfo.AnimIndex()); - common::NodeAnimation *rootNode = skelAnim->NodeAnimationByName( - rootNodeName); - math::Matrix4d lastPos = rootNode->KeyFrame( - rootNode->FrameCount() - 1).second; - math::Matrix4d firstPos = rootNode->KeyFrame(0).second; - if (!math::equal(firstPos.Translation().X(), - lastPos.Translation().X())) - { - trajInfo.Waypoints()->SetInterpolateX(animation->InterpolateX()); - } - else - { - ignwarn << "Animation has no x displacement. " - << "Ignoring for the animation in '" - << animation->Filename() << "'." << std::endl; - } - animInterpolateCheck.insert(animation->Filename()); - } - } - } - else - { - trajInfo.SetTranslated(false); - } - trajectories.push_back(trajInfo); + ignwarn << "Mesh skeleton in [" << descriptor.meshName << "] not found." + << std::endl; + } + else + { + this->dataPtr->actorSkeletons[_id] = meshSkel; + // Load all animations + mapAnimNameId = this->LoadAnimations(_actor); + if (mapAnimNameId.size() == 0) + return nullptr; } - } - // if there are no trajectories, but there are animations, add a trajectory - else - { - auto skel = this->dataPtr->actorSkeletons[_id]; - common::TrajectoryInfo trajInfo; - trajInfo.SetId(0); - trajInfo.SetAnimIndex(0); - trajInfo.SetStartTime(TP(0ms)); - auto timepoint = std::chrono::milliseconds( - static_cast(skel->Animation(0)->Length() * 1000)); - trajInfo.SetEndTime(TP(timepoint)); - trajInfo.SetTranslated(false); - trajectories.push_back(trajInfo); } + // load trajectories regardless of whether the mesh skeleton exists + // or not. If there is no mesh skeleon, we can still do trajectory + // animation + auto trajectories = this->dataPtr->LoadTrajectories(_actor, mapAnimNameId, + meshSkel); + // sequencing all trajectories auto delayStartTime = std::chrono::milliseconds( static_cast(_actor.ScriptDelayStart() * 1000)); @@ -1093,26 +1028,28 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, this->dataPtr->actorTrajectories[_id] = trajectories; + rendering::VisualPtr actorVisual = this->dataPtr->scene->CreateVisual(name); + rendering::MeshPtr actorMesh; // create mesh with animations - rendering::MeshPtr actorMesh = this->dataPtr->scene->CreateMesh( - descriptor); - if (nullptr == actorMesh) + if (descriptor.mesh) { - ignerr << "Actor skin file [" << descriptor.meshName << "] not found." - << std::endl; - return rendering::VisualPtr(); + actorMesh = this->dataPtr->scene->CreateMesh(descriptor); + if (nullptr == actorMesh) + { + ignerr << "Actor skin file [" << descriptor.meshName << "] not found." + << std::endl; + return rendering::VisualPtr(); + } + actorVisual->AddGeometry(actorMesh); } - rendering::VisualPtr actorVisual = this->dataPtr->scene->CreateVisual(name); actorVisual->SetLocalPose(_actor.RawPose()); - actorVisual->AddGeometry(actorMesh); actorVisual->SetUserData("gazebo-entity", static_cast(_id)); actorVisual->SetUserData("pause-update", static_cast(0)); this->dataPtr->visuals[_id] = actorVisual; this->dataPtr->actors[_id] = actorMesh; - if (parent) parent->AddChild(actorVisual); else @@ -1796,12 +1733,12 @@ AnimationUpdateData SceneManager::ActorAnimationAt( if (trajIt == this->dataPtr->actorTrajectories.end()) return animData; + animData = this->dataPtr->ActorTrajectoryAt(_id, _time); + auto skelIt = this->dataPtr->actorSkeletons.find(_id); if (skelIt == this->dataPtr->actorSkeletons.end()) return animData; - animData = this->dataPtr->ActorTrajectoryAt(_id, _time); - auto skel = skelIt->second; unsigned int animIndex = animData.trajectory.AnimIndex(); animData.animationName = skel->Animation(animIndex)->Name(); @@ -2390,3 +2327,103 @@ SceneManager::LoadAnimations(const sdf::Actor &_actor) } return mapAnimNameId; } + +///////////////////////////////////////////////// +std::vector +SceneManagerPrivate::LoadTrajectories(const sdf::Actor &_actor, + std::unordered_map &_mapAnimNameId, + common::SkeletonPtr _meshSkel) +{ + std::vector trajectories; + if (_actor.TrajectoryCount() != 0) + { + // Load all trajectories specified in sdf + for (unsigned i = 0; i < _actor.TrajectoryCount(); ++i) + { + const sdf::Trajectory *trajSdf = _actor.TrajectoryByIndex(i); + if (nullptr == trajSdf) + { + ignerr << "Null trajectory SDF for [" << _actor.Name() << "]" + << std::endl; + continue; + } + common::TrajectoryInfo trajInfo; + trajInfo.SetId(trajSdf->Id()); + + if (!_mapAnimNameId.empty()) + trajInfo.SetAnimIndex(_mapAnimNameId[trajSdf->Type()]); + + if (trajSdf->WaypointCount() != 0) + { + std::map waypoints; + for (unsigned j = 0; j < trajSdf->WaypointCount(); ++j) + { + auto point = trajSdf->WaypointByIndex(j); + TP pointTp(std::chrono::milliseconds( + static_cast(point->Time() * 1000))); + waypoints[pointTp] = point->Pose(); + } + trajInfo.SetWaypoints(waypoints, trajSdf->Tension()); + + if (_meshSkel) + { + // Animations are offset by 1 because index 0 is taken by the mesh + // name + auto animation = _actor.AnimationByIndex(trajInfo.AnimIndex() - 1); + + if (animation && animation->InterpolateX() && _meshSkel) + { + // warn if no x displacement can be interpolated + // warn only once per mesh + static std::unordered_set animInterpolateCheck; + if (animInterpolateCheck.count(animation->Filename()) == 0) + { + std::string rootNodeName = _meshSkel->RootNode()->Name(); + common::SkeletonAnimation *skelAnim = + _meshSkel->Animation(trajInfo.AnimIndex()); + common::NodeAnimation *rootNode = skelAnim->NodeAnimationByName( + rootNodeName); + math::Matrix4d lastPos = rootNode->KeyFrame( + rootNode->FrameCount() - 1).second; + math::Matrix4d firstPos = rootNode->KeyFrame(0).second; + if (!math::equal(firstPos.Translation().X(), + lastPos.Translation().X())) + { + trajInfo.Waypoints()->SetInterpolateX( + animation->InterpolateX()); + } + else + { + ignwarn << "Animation has no x displacement. " + << "Ignoring for the animation in '" + << animation->Filename() << "'." << std::endl; + } + animInterpolateCheck.insert(animation->Filename()); + } + } + } + } + else + { + trajInfo.SetTranslated(false); + } + trajectories.push_back(trajInfo); + } + } + // if there are no trajectories, but there are animations, add a trajectory + else + { + common::TrajectoryInfo trajInfo; + trajInfo.SetId(0); + trajInfo.SetAnimIndex(0); + trajInfo.SetStartTime(TP(0ms)); + + double animLength = (_meshSkel) ? _meshSkel->Animation(0)->Length() : 0.0; + auto timepoint = std::chrono::milliseconds( + static_cast(animLength * 1000)); + trajInfo.SetEndTime(TP(timepoint)); + trajInfo.SetTranslated(false); + trajectories.push_back(trajInfo); + } + return trajectories; +} diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 33eb83abdf..949e7960bb 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -90,6 +90,7 @@ #include "ignition/gazebo/Util.hh" // Components +#include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/AngularAcceleration.hh" #include "ignition/gazebo/components/AngularVelocity.hh" #include "ignition/gazebo/components/AngularVelocityCmd.hh" @@ -2526,8 +2527,14 @@ std::map PhysicsPrivate::ChangedLinks( if (this->linkAddedToModel.find(_entity) == this->linkAddedToModel.end()) { - ignerr << "Internal error: link [" << _entity - << "] not in entity map" << std::endl; + // ignore links from actors for now + auto parentId = + _ecm.Component(_entity)->Data(); + if (!_ecm.Component(parentId)) + { + ignerr << "Internal error: link [" << _entity + << "] not in entity map" << std::endl; + } } return true; } diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 1fc615ca54..683a5151d7 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -1,8 +1,8 @@ set(TEST_TYPE "INTEGRATION") set(tests - actor.cc ackermann_steering_system.cc + actor.cc air_pressure_system.cc altimeter_system.cc apply_joint_force_system.cc @@ -86,6 +86,7 @@ endif() # Tests that require a valid display set(tests_needing_display + actor_trajectory.cc camera_sensor_background.cc camera_sensor_background_from_scene.cc camera_video_record_system.cc @@ -173,4 +174,7 @@ if(VALID_DISPLAY AND VALID_DRI_DISPLAY) target_link_libraries(INTEGRATION_sensors_system ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} ) + target_link_libraries(INTEGRATION_actor_trajectory + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + ) endif() diff --git a/test/integration/actor_trajectory.cc b/test/integration/actor_trajectory.cc new file mode 100644 index 0000000000..66b25c6516 --- /dev/null +++ b/test/integration/actor_trajectory.cc @@ -0,0 +1,166 @@ +/* + * Copyright (C) 2023 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 "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/EventManager.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/Types.hh" +#include "ignition/gazebo/test_config.hh" + +#include "ignition/gazebo/rendering/Events.hh" + +#include "plugins/MockSystem.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace std::chrono_literals; + +// Pointer to scene +rendering::ScenePtr g_scene; + +// Map of model names to their poses +std::unordered_map> g_modelPoses; + +// mutex to project model poses +std::mutex g_mutex; + +///////////////////////////////////////////////// +void OnPostRender() +{ + if (!g_scene) + { + g_scene = rendering::sceneFromFirstRenderEngine(); + } + ASSERT_TRUE(g_scene); + + auto rootVis = g_scene->RootVisual(); + ASSERT_TRUE(rootVis); + + // store all the model poses + std::lock_guard lock(g_mutex); + for (unsigned int i = 0; i < rootVis->ChildCount(); ++i) + { + auto vis = rootVis->ChildByIndex(i); + ASSERT_TRUE(vis); + g_modelPoses[vis->Name()].push_back(vis->WorldPose()); + } +} + +////////////////////////////////////////////////// +class ActorFixture : public InternalFixture> +{ + protected: void SetUp() override + { + InternalFixture::SetUp(); + + sdf::Plugin sdfPlugin; + sdfPlugin.SetFilename("libMockSystem.so"); + sdfPlugin.SetName("ignition::gazebo::MockSystem"); + auto plugin = sm.LoadPlugin(sdfPlugin); + EXPECT_TRUE(plugin.has_value()); + this->systemPtr = plugin.value(); + this->mockSystem = static_cast( + systemPtr->QueryInterface()); + } + + public: gazebo::SystemPluginPtr systemPtr; + public: gazebo::MockSystem *mockSystem; + + private: gazebo::SystemLoader sm; +}; + +///////////////////////////////////////////////// +// Load the actor_trajectory.sdf world that animates a box (actor) to follow +// a trajectory. Verify that the box pose changes over time on the rendering +// side. +TEST_F(ActorFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(ActorTrajectoryNoMesh)) +{ + gazebo::ServerConfig serverConfig; + + const std::string sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/actor_trajectory.sdf"; + + serverConfig.SetSdfFile(sdfFile); + gazebo::Server server(serverConfig); + + common::ConnectionPtr postRenderConn; + + // A pointer to the ecm. This will be valid once we run the mock system + gazebo::EntityComponentManager *ecm = nullptr; + this->mockSystem->preUpdateCallback = + [&ecm](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + { + ecm = &_ecm; + }; + this->mockSystem->configureCallback = + [&](const gazebo::Entity &, + const std::shared_ptr &, + gazebo::EntityComponentManager &, + gazebo::EventManager &_eventMgr) + { + postRenderConn = _eventMgr.Connect( + std::bind(&::OnPostRender)); + }; + + server.AddSystem(this->systemPtr); + server.Run(true, 500, false); + ASSERT_NE(nullptr, ecm); + + // verify that pose of the animated box exists + bool hasBoxPose = false; + int sleep = 0; + int maxSleep = 50; + const std::string boxName = "animated_box"; + unsigned int boxPoseCount = 0u; + while (!hasBoxPose && sleep++ < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::lock_guard lock(g_mutex); + if (g_modelPoses.find(boxName) != g_modelPoses.end()) + { + hasBoxPose = true; + boxPoseCount = g_modelPoses.size(); + } + } + EXPECT_TRUE(hasBoxPose); + EXPECT_LT(1u, boxPoseCount); + + // check that box is animated, i.e. pose changes over time + { + std::lock_guard lock(g_mutex); + auto it = g_modelPoses.find(boxName); + auto &poses = it->second; + for (unsigned int i = 0; i < poses.size()-1; ++i) + { + EXPECT_NE(poses[i], poses[i+1]); + } + } + + g_scene.reset(); +} diff --git a/test/worlds/actor_trajectory.sdf b/test/worlds/actor_trajectory.sdf new file mode 100644 index 0000000000..ecb2e9f718 --- /dev/null +++ b/test/worlds/actor_trajectory.sdf @@ -0,0 +1,83 @@ + + + + + + + ogre2 + + + + + + + + + .2 .2 .2 + + + + + + + + + + true + -5 0 0 0 0.5 0 + + + + + 0.1 0.1 0.1 + + + + + 0 0 0.0 0 0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 30 + camera + + + + + +