From ba08301c5be668e00d3d9eb799ce49f07c548edf Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Jun 2021 15:23:52 -0700 Subject: [PATCH 1/6] Support locked entities, and headless video recording using sim time Signed-off-by: Nate Koenig --- .../ignition/gazebo/EntityComponentManager.hh | 29 +++++ src/EntityComponentManager.cc | 107 +++++++++++++++++- src/EntityComponentManager_TEST.cc | 60 ++++++++++ src/gui/plugins/scene3d/Scene3D.cc | 4 +- .../CameraVideoRecorder.cc | 62 +++++++++- .../CameraVideoRecorder.hh | 6 +- 6 files changed, 259 insertions(+), 9 deletions(-) diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index ad28252e65..204464201c 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -91,6 +91,35 @@ namespace ignition public: void RequestRemoveEntity(const Entity _entity, bool _recursive = true); + /// \brief Prevent an entity and optionally its children from + /// being removed. + /// + /// This function can be useful when seek operations during log + /// playback are used in conjunciton with spawned entities. For + /// example, you may want to record a video based on a log file + /// using a headless simulation instance. This requires a + /// camera sensor which would be spawned during log playback. If + /// a seek backward in time is performed during log playback, then the + /// spawned camera would be removed. Use this function to prevent the + /// camera from automatic removal. + /// + /// \param[in] _entity Entity to be locked. + /// \param[in] _recursive Whether to recursively lock all child + /// entities. True by default. + public: void LockEntity(const Entity _entity, bool _recursive = true); + + /// \brief Allow a previously lock entity and optionally its children + /// to be removed. + /// \param[in] _entity Entity to be unlocked. + /// \param[in] _recursive Whether to recursively unlock all child + /// entities. True by default. + /// \sa void LockEntity(const Entity, bool) + public: void UnlockEntity(const Entity _entity, bool _recursive = true); + + /// \brief Allow all previously locked entitied to be removed. + /// \sa void LockEntity(const Entity, bool) + public: void UnlockAllEntities(); + /// \brief Request to remove all entities. This will insert the request /// into a queue. The queue is processed toward the end of a simulation /// update step. diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 825f6227bd..11d356c69a 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -45,6 +45,13 @@ class ignition::gazebo::EntityComponentManagerPrivate public: void InsertEntityRecursive(Entity _entity, std::unordered_set &_set); + /// \brief Recursively erase an entity and all its descendants from a given + /// set. + /// \param[in] _entity Entity to be erased. + /// \param[in, out] _set Set to erase from. + public: void EraseEntityRecursive(Entity _entity, + std::unordered_set &_set); + /// \brief Register a new component type. /// \param[in] _typeId Type if of the new component. /// \return True if created successfully. @@ -156,6 +163,9 @@ class ignition::gazebo::EntityComponentManagerPrivate /// which belongs the component, and the value is the component being /// removed. std::unordered_multimap removedComponents; + + /// \brief Set of entities that are prevented from removal. + public: std::unordered_set lockedEntities; }; ////////////////////////////////////////////////// @@ -236,6 +246,17 @@ void EntityComponentManagerPrivate::InsertEntityRecursive(Entity _entity, _set.insert(_entity); } +///////////////////////////////////////////////// +void EntityComponentManagerPrivate::EraseEntityRecursive(Entity _entity, + std::unordered_set &_set) +{ + for (const auto &vertex : this->entities.AdjacentsFrom(_entity)) + { + this->EraseEntityRecursive(vertex.first, _set); + } + _set.erase(_entity); +} + ///////////////////////////////////////////////// void EntityComponentManager::RequestRemoveEntity(Entity _entity, bool _recursive) @@ -252,6 +273,22 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity, this->dataPtr->InsertEntityRecursive(_entity, tmpToRemoveEntities); } + // Remove entities from tmpToRemoveEntities that are locked. + for (auto iter = tmpToRemoveEntities.begin(); + iter != tmpToRemoveEntities.end();) + { + if (std::find(this->dataPtr->lockedEntities.begin(), + this->dataPtr->lockedEntities.end(), *iter) != + this->dataPtr->lockedEntities.end()) + { + iter = tmpToRemoveEntities.erase(iter); + } + else + { + ++iter; + } + } + { std::lock_guard lock(this->dataPtr->entityRemoveMutex); this->dataPtr->toRemoveEntities.insert(tmpToRemoveEntities.begin(), @@ -267,11 +304,41 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity, ///////////////////////////////////////////////// void EntityComponentManager::RequestRemoveEntities() { + if (this->dataPtr->lockedEntities.empty()) { - std::lock_guard lock(this->dataPtr->entityRemoveMutex); - this->dataPtr->removeAllEntities = true; + { + std::lock_guard lock(this->dataPtr->entityRemoveMutex); + this->dataPtr->removeAllEntities = true; + } + this->RebuildViews(); + } + else + { + std::unordered_set tmpToRemoveEntities; + + // Store the to-be-removed entities in a temporary set so we can call + // UpdateViews on each of them + for (const auto &vertex : this->dataPtr->entities.Vertices()) + { + if (std::find(this->dataPtr->lockedEntities.begin(), + this->dataPtr->lockedEntities.end(), vertex.first) == + this->dataPtr->lockedEntities.end()) + { + tmpToRemoveEntities.insert(vertex.first); + } + } + + { + std::lock_guard lock(this->dataPtr->entityRemoveMutex); + this->dataPtr->toRemoveEntities.insert(tmpToRemoveEntities.begin(), + tmpToRemoveEntities.end()); + } + + for (const auto &removedEntity : tmpToRemoveEntities) + { + this->UpdateViews(removedEntity); + } } - this->RebuildViews(); } ///////////////////////////////////////////////// @@ -1505,3 +1572,37 @@ void EntityComponentManagerPrivate::AddModifiedComponent(const Entity &_entity) this->modifiedComponents.insert(_entity); } + +///////////////////////////////////////////////// +void EntityComponentManager::LockEntity(const Entity _entity, bool _recursive) +{ + if (_recursive) + { + this->dataPtr->InsertEntityRecursive(_entity, + this->dataPtr->lockedEntities); + } + else + { + this->dataPtr->lockedEntities.insert(_entity); + } +} + +///////////////////////////////////////////////// +void EntityComponentManager::UnlockEntity(const Entity _entity, bool _recursive) +{ + if (_recursive) + { + this->dataPtr->EraseEntityRecursive(_entity, + this->dataPtr->lockedEntities); + } + else + { + this->dataPtr->lockedEntities.erase(_entity); + } +} + +///////////////////////////////////////////////// +void EntityComponentManager::UnlockAllEntities() +{ + this->dataPtr->lockedEntities.clear(); +} diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index e22a4ce5c2..e532ff649d 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -2563,6 +2563,66 @@ TEST_P(EntityComponentManagerFixture, RemovedComponentsSyncBetweenServerAndGUI) } } +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, LockEntity) +{ + // Create some entities + auto e1 = manager.CreateEntity(); + EXPECT_EQ(1u, e1); + EXPECT_TRUE(manager.HasEntity(e1)); + + auto e2 = manager.CreateEntity(); + EXPECT_TRUE(manager.SetParentEntity(e2, e1)); + EXPECT_EQ(2u, e2); + EXPECT_TRUE(manager.HasEntity(e2)); + + auto e3 = manager.CreateEntity(); + EXPECT_EQ(3u, e3); + EXPECT_TRUE(manager.HasEntity(e3)); + + EXPECT_EQ(3u, manager.EntityCount()); + + // Lock e1, which should also lock its child entity e2 + manager.LockEntity(e1); + + // Try to remove e1, which is locked entity + manager.RequestRemoveEntity(e1); + EXPECT_EQ(3u, manager.EntityCount()); + EXPECT_FALSE(manager.HasEntitiesMarkedForRemoval()); + manager.ProcessEntityRemovals(); + EXPECT_EQ(3u, manager.EntityCount()); + + // Try to remove e2, which has been locked recursively + manager.RequestRemoveEntity(e2); + EXPECT_EQ(3u, manager.EntityCount()); + EXPECT_FALSE(manager.HasEntitiesMarkedForRemoval()); + manager.ProcessEntityRemovals(); + EXPECT_EQ(3u, manager.EntityCount()); + + std::cout << "RequestRemoveEntities\n"; + // Try to remove all entities, which should leave just e1 and e2 + manager.RequestRemoveEntities(); + std::cout << "RequestRemoveEntities Done\n"; + EXPECT_TRUE(manager.HasEntitiesMarkedForRemoval()); + manager.ProcessEntityRemovals(); + EXPECT_EQ(2u, manager.EntityCount()); + + // Unlock e2, and now it should be removable. + manager.UnlockEntity(e2); + manager.RequestRemoveEntity(e2); + EXPECT_EQ(2u, manager.EntityCount()); + EXPECT_TRUE(manager.HasEntitiesMarkedForRemoval()); + manager.ProcessEntityRemovals(); + EXPECT_EQ(1u, manager.EntityCount()); + + // Unlock all entities, and now it should be removable. + manager.UnlockAllEntities(); + manager.RequestRemoveEntities(); + EXPECT_TRUE(manager.HasEntitiesMarkedForRemoval()); + manager.ProcessEntityRemovals(); + EXPECT_EQ(0u, manager.EntityCount()); +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(EntityComponentManagerRepeat, diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 0e517455be..cb987e3c8a 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -219,10 +219,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// only used in lockstep mode and recording in sim time. public: std::chrono::steady_clock::time_point recordVideoUpdateTime; - /// \brief Start tiem of video recording + /// \brief Start time of video recording public: std::chrono::steady_clock::time_point recordStartTime; - /// \brief Camera pose publisher + /// \brief Video recording statistics publisher public: transport::Node::Publisher recorderStatsPub; /// \brief Target to move the user camera to diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.cc b/src/systems/camera_video_recorder/CameraVideoRecorder.cc index cadc7b4b3d..49fc969405 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.cc +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.cc @@ -108,6 +108,19 @@ class ignition::gazebo::systems::CameraVideoRecorderPrivate /// \brief Topic that the sensor publishes to public: std::string sensorTopic; + + /// \brief Video recording statistics publisher + public: transport::Node::Publisher recorderStatsPub; + + /// \brief Start time of video recording. + public: std::chrono::steady_clock::time_point recordStartTime; + + /// \brief Current simulation time. + public: std::chrono::steady_clock::duration simTime{0}; + + /// \brief Use sim time as timestamp during video recording + /// By default (false), video encoding is done using real time. + public: bool recordVideoUseSimTime = false; }; ////////////////////////////////////////////////// @@ -224,6 +237,17 @@ void CameraVideoRecorder::Configure( } } this->dataPtr->sensorTopic = topic; + + // Get whether sim time should be used for recording. + this->dataPtr->recordVideoUseSimTime = _sdf->Get("use_sim_time", + this->dataPtr->recordVideoUseSimTime).first; + + // recorder stats topic + std::string recorderStatsTopic = this->dataPtr->sensorTopic + "/stats"; + this->dataPtr->recorderStatsPub = + this->dataPtr->node.Advertise(recorderStatsTopic); + ignmsg << "Camera Video recorder stats topic advertised on [" + << recorderStatsTopic << "]" << std::endl; } ////////////////////////////////////////////////// @@ -312,8 +336,36 @@ void CameraVideoRecorderPrivate::OnPostRender() if (this->videoEncoder.IsEncoding()) { this->camera->Copy(this->cameraImage); - this->videoEncoder.AddFrame( - this->cameraImage.Data(), width, height); + std::chrono::steady_clock::time_point t; + std::chrono::steady_clock::now(); + if (this->recordVideoUseSimTime) + t = std::chrono::steady_clock::time_point(this->simTime); + else + t = std::chrono::steady_clock::now(); + + bool frameAdded = this->videoEncoder.AddFrame( + this->cameraImage.Data(), width, height, t); + + if (frameAdded) + { + // publish recorder stats + if (this->recordStartTime == + std::chrono::steady_clock::time_point( + std::chrono::duration(std::chrono::seconds(0)))) + { + // start time, i.e. time when first frame is added + this->recordStartTime = t; + } + + std::chrono::steady_clock::duration dt; + dt = t - this->recordStartTime; + int64_t sec, nsec; + std::tie(sec, nsec) = ignition::math::durationToSecNsec(dt); + msgs::Time msg; + msg.set_sec(sec); + msg.set_nsec(nsec); + this->recorderStatsPub.Publish(msg); + } } // Video recorder is idle. Start recording. else @@ -329,6 +381,9 @@ void CameraVideoRecorderPrivate::OnPostRender() this->videoEncoder.Start(this->recordVideoFormat, this->tmpVideoFilename, width, height); + this->recordStartTime = std::chrono::steady_clock::time_point( + std::chrono::duration(std::chrono::seconds(0))); + ignmsg << "Start video recording on [" << this->service << "]. " << "Encoding to tmp file: [" << this->tmpVideoFilename << "]" << std::endl; @@ -362,9 +417,10 @@ void CameraVideoRecorderPrivate::OnPostRender() } ////////////////////////////////////////////////// -void CameraVideoRecorder::PostUpdate(const UpdateInfo &, +void CameraVideoRecorder::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) { + this->dataPtr->simTime = _info.simTime; if (!this->dataPtr->cameraName.empty()) return; diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.hh b/src/systems/camera_video_recorder/CameraVideoRecorder.hh index b8627f7355..be7771719f 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.hh +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.hh @@ -37,10 +37,14 @@ namespace systems **/ /// \brief Record video from a camera sensor /// The system takes in the following parameter: - /// Name of topic for the video recorder service. If this is + /// Name of topic for the video recorder service. If this is /// not specified, the topic defaults to: /// /world//link// /// sensor//record_video + /// True/false value that specified if the video should + // be recorded using simulation time or real time. The + // default is false, which indicates the use of real + // time. class CameraVideoRecorder: public System, public ISystemConfigure, From 7d65ef4d7183fd3a2fa4940c4833fbf891896f99 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Jun 2021 15:40:01 -0700 Subject: [PATCH 2/6] Spelling Signed-off-by: Nate Koenig --- src/systems/camera_video_recorder/CameraVideoRecorder.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.hh b/src/systems/camera_video_recorder/CameraVideoRecorder.hh index be7771719f..14a4e95bef 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.hh +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.hh @@ -41,7 +41,7 @@ namespace systems /// not specified, the topic defaults to: /// /world//link// /// sensor//record_video - /// True/false value that specified if the video should + /// True/false value that specifies if the video should // be recorded using simulation time or real time. The // default is false, which indicates the use of real // time. From 5b5b3dab6f8728edb7d16795dc194f2dbb9a4dfc Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 18 Jun 2021 08:52:59 -0700 Subject: [PATCH 3/6] Add bitrate (#864) Signed-off-by: Ian Chen --- .../camera_video_recorder/CameraVideoRecorder.cc | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.cc b/src/systems/camera_video_recorder/CameraVideoRecorder.cc index 49fc969405..8375be8c7b 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.cc +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.cc @@ -121,6 +121,9 @@ class ignition::gazebo::systems::CameraVideoRecorderPrivate /// \brief Use sim time as timestamp during video recording /// By default (false), video encoding is done using real time. public: bool recordVideoUseSimTime = false; + + /// \brief Video recorder bitrate (bps) + public: unsigned int recordVideoBitrate = 2070000; }; ////////////////////////////////////////////////// @@ -242,6 +245,10 @@ void CameraVideoRecorder::Configure( this->dataPtr->recordVideoUseSimTime = _sdf->Get("use_sim_time", this->dataPtr->recordVideoUseSimTime).first; + // Get video recoder bitrate param + this->dataPtr->recordVideoBitrate = _sdf->Get("bitrate", + this->dataPtr->recordVideoBitrate).first; + // recorder stats topic std::string recorderStatsTopic = this->dataPtr->sensorTopic + "/stats"; this->dataPtr->recorderStatsPub = @@ -379,7 +386,8 @@ void CameraVideoRecorderPrivate::OnPostRender() &CameraVideoRecorderPrivate::OnImage, this); this->videoEncoder.Start(this->recordVideoFormat, - this->tmpVideoFilename, width, height); + this->tmpVideoFilename, width, height, 25, + this->recordVideoBitrate); this->recordStartTime = std::chrono::steady_clock::time_point( std::chrono::duration(std::chrono::seconds(0))); From 1e908d310e46dfd94c1529c1a658ac8ec09397df Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 18 Jun 2021 08:53:36 -0700 Subject: [PATCH 4/6] Changing Locked name and adding fps Signed-off-by: Nate Koenig --- .../ignition/gazebo/EntityComponentManager.hh | 7 ++-- src/EntityComponentManager.cc | 34 ++++++++++--------- src/EntityComponentManager_TEST.cc | 16 ++++----- .../CameraVideoRecorder.cc | 7 +++- 4 files changed, 35 insertions(+), 29 deletions(-) diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 204464201c..ac7e798d0f 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -106,7 +106,8 @@ namespace ignition /// \param[in] _entity Entity to be locked. /// \param[in] _recursive Whether to recursively lock all child /// entities. True by default. - public: void LockEntity(const Entity _entity, bool _recursive = true); + public: void MarkEntityUnremovable(const Entity _entity, + bool _recursive = true); /// \brief Allow a previously lock entity and optionally its children /// to be removed. @@ -114,11 +115,11 @@ namespace ignition /// \param[in] _recursive Whether to recursively unlock all child /// entities. True by default. /// \sa void LockEntity(const Entity, bool) - public: void UnlockEntity(const Entity _entity, bool _recursive = true); + public: void MarkEntityRemovable(const Entity _entity, bool _recursive = true); /// \brief Allow all previously locked entitied to be removed. /// \sa void LockEntity(const Entity, bool) - public: void UnlockAllEntities(); + public: void MarkAllEntitiesRemovable(); /// \brief Request to remove all entities. This will insert the request /// into a queue. The queue is processed toward the end of a simulation diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 11d356c69a..40b81e011a 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -165,7 +165,7 @@ class ignition::gazebo::EntityComponentManagerPrivate std::unordered_multimap removedComponents; /// \brief Set of entities that are prevented from removal. - public: std::unordered_set lockedEntities; + public: std::unordered_set unremovableEntities; }; ////////////////////////////////////////////////// @@ -277,9 +277,9 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity, for (auto iter = tmpToRemoveEntities.begin(); iter != tmpToRemoveEntities.end();) { - if (std::find(this->dataPtr->lockedEntities.begin(), - this->dataPtr->lockedEntities.end(), *iter) != - this->dataPtr->lockedEntities.end()) + if (std::find(this->dataPtr->unremovableEntities.begin(), + this->dataPtr->unremovableEntities.end(), *iter) != + this->dataPtr->unremovableEntities.end()) { iter = tmpToRemoveEntities.erase(iter); } @@ -304,7 +304,7 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity, ///////////////////////////////////////////////// void EntityComponentManager::RequestRemoveEntities() { - if (this->dataPtr->lockedEntities.empty()) + if (this->dataPtr->unremovableEntities.empty()) { { std::lock_guard lock(this->dataPtr->entityRemoveMutex); @@ -320,9 +320,9 @@ void EntityComponentManager::RequestRemoveEntities() // UpdateViews on each of them for (const auto &vertex : this->dataPtr->entities.Vertices()) { - if (std::find(this->dataPtr->lockedEntities.begin(), - this->dataPtr->lockedEntities.end(), vertex.first) == - this->dataPtr->lockedEntities.end()) + if (std::find(this->dataPtr->unremovableEntities.begin(), + this->dataPtr->unremovableEntities.end(), vertex.first) == + this->dataPtr->unremovableEntities.end()) { tmpToRemoveEntities.insert(vertex.first); } @@ -1574,35 +1574,37 @@ void EntityComponentManagerPrivate::AddModifiedComponent(const Entity &_entity) } ///////////////////////////////////////////////// -void EntityComponentManager::LockEntity(const Entity _entity, bool _recursive) +void EntityComponentManager::MarkEntityUnremovable(const Entity _entity, + bool _recursive) { if (_recursive) { this->dataPtr->InsertEntityRecursive(_entity, - this->dataPtr->lockedEntities); + this->dataPtr->unremovableEntities); } else { - this->dataPtr->lockedEntities.insert(_entity); + this->dataPtr->unremovableEntities.insert(_entity); } } ///////////////////////////////////////////////// -void EntityComponentManager::UnlockEntity(const Entity _entity, bool _recursive) +void EntityComponentManager::MarkEntityRemovable(const Entity _entity, + bool _recursive) { if (_recursive) { this->dataPtr->EraseEntityRecursive(_entity, - this->dataPtr->lockedEntities); + this->dataPtr->unremovableEntities); } else { - this->dataPtr->lockedEntities.erase(_entity); + this->dataPtr->unremovableEntities.erase(_entity); } } ///////////////////////////////////////////////// -void EntityComponentManager::UnlockAllEntities() +void EntityComponentManager::MarkAllEntitiesRemovable() { - this->dataPtr->lockedEntities.clear(); + this->dataPtr->unremovableEntities.clear(); } diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index e532ff649d..0cd7b5642c 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -2564,7 +2564,7 @@ TEST_P(EntityComponentManagerFixture, RemovedComponentsSyncBetweenServerAndGUI) } ////////////////////////////////////////////////// -TEST_P(EntityComponentManagerFixture, LockEntity) +TEST_P(EntityComponentManagerFixture, UnremovableEntity) { // Create some entities auto e1 = manager.CreateEntity(); @@ -2582,8 +2582,8 @@ TEST_P(EntityComponentManagerFixture, LockEntity) EXPECT_EQ(3u, manager.EntityCount()); - // Lock e1, which should also lock its child entity e2 - manager.LockEntity(e1); + // Mark e1 as unremovable, which should also lock its child entity e2 + manager.MarkEntityUnremovable(e1); // Try to remove e1, which is locked entity manager.RequestRemoveEntity(e1); @@ -2599,24 +2599,22 @@ TEST_P(EntityComponentManagerFixture, LockEntity) manager.ProcessEntityRemovals(); EXPECT_EQ(3u, manager.EntityCount()); - std::cout << "RequestRemoveEntities\n"; // Try to remove all entities, which should leave just e1 and e2 manager.RequestRemoveEntities(); - std::cout << "RequestRemoveEntities Done\n"; EXPECT_TRUE(manager.HasEntitiesMarkedForRemoval()); manager.ProcessEntityRemovals(); EXPECT_EQ(2u, manager.EntityCount()); - // Unlock e2, and now it should be removable. - manager.UnlockEntity(e2); + // Unmark e2, and now it should be removable. + manager.MarkEntityRemovable(e2); manager.RequestRemoveEntity(e2); EXPECT_EQ(2u, manager.EntityCount()); EXPECT_TRUE(manager.HasEntitiesMarkedForRemoval()); manager.ProcessEntityRemovals(); EXPECT_EQ(1u, manager.EntityCount()); - // Unlock all entities, and now it should be removable. - manager.UnlockAllEntities(); + // Unmark all entities, and now it should be removable. + manager.MarkAllEntitiesRemovable(); manager.RequestRemoveEntities(); EXPECT_TRUE(manager.HasEntitiesMarkedForRemoval()); manager.ProcessEntityRemovals(); diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.cc b/src/systems/camera_video_recorder/CameraVideoRecorder.cc index 49fc969405..2a4dbde512 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.cc +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.cc @@ -121,6 +121,9 @@ class ignition::gazebo::systems::CameraVideoRecorderPrivate /// \brief Use sim time as timestamp during video recording /// By default (false), video encoding is done using real time. public: bool recordVideoUseSimTime = false; + + /// \brief Recording frames per second. + public: int fps = 25; }; ////////////////////////////////////////////////// @@ -242,6 +245,8 @@ void CameraVideoRecorder::Configure( this->dataPtr->recordVideoUseSimTime = _sdf->Get("use_sim_time", this->dataPtr->recordVideoUseSimTime).first; + this->dataPtr->fps = _sdf->Get("fps", this->dataPtr->fps).first; + // recorder stats topic std::string recorderStatsTopic = this->dataPtr->sensorTopic + "/stats"; this->dataPtr->recorderStatsPub = @@ -379,7 +384,7 @@ void CameraVideoRecorderPrivate::OnPostRender() &CameraVideoRecorderPrivate::OnImage, this); this->videoEncoder.Start(this->recordVideoFormat, - this->tmpVideoFilename, width, height); + this->tmpVideoFilename, width, height, this->fps); this->recordStartTime = std::chrono::steady_clock::time_point( std::chrono::duration(std::chrono::seconds(0))); From 61cddf6d71a8761c46149fd25caab580f2d4cea9 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 24 Jun 2021 12:57:22 -0700 Subject: [PATCH 5/6] Rename to unpin Signed-off-by: Nate Koenig --- .../ignition/gazebo/EntityComponentManager.hh | 24 ++++++------- src/EntityComponentManager.cc | 34 +++++++++---------- src/EntityComponentManager_TEST.cc | 8 ++--- 3 files changed, 31 insertions(+), 35 deletions(-) diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 17aaa6a0c4..1cb4e72787 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -103,24 +103,22 @@ namespace ignition /// spawned camera would be removed. Use this function to prevent the /// camera from automatic removal. /// - /// \param[in] _entity Entity to be marked. - /// \param[in] _recursive Whether to recursively mark all child + /// \param[in] _entity Entity to be pinned. + /// \param[in] _recursive Whether to recursively pin all child /// entities. True by default. - public: void MarkEntityUnremovable(const Entity _entity, - bool _recursive = true); + public: void PinEntity(const Entity _entity, bool _recursive = true); /// \brief Allow an entity, and optionally its children, previously - /// marked as unremoable to be removed. - /// \param[in] _entity Entity to be unmarked. - /// \param[in] _recursive Whether to recursively unmark all child + /// marked as pinned to be removed. + /// \param[in] _entity Entity to be unpinned. + /// \param[in] _recursive Whether to recursively unpin all child /// entities. True by default. - /// \sa void MarkEntityUnremovable(const Entity, bool) - public: void MarkEntityRemovable(const Entity _entity, - bool _recursive = true); + /// \sa void PinEntity(const Entity, bool) + public: void UnpinEntity(const Entity _entity, bool _recursive = true); - /// \brief Allow all previously marked entities to be removed. - /// \sa void MarkEntityUnremovable(const Entity, bool) - public: void MarkAllEntitiesRemovable(); + /// \brief Allow all previously pinned entities to be removed. + /// \sa void PinEntity(const Entity, bool) + public: void UnpinAllEntities(); /// \brief Request to remove all entities. This will insert the request /// into a queue. The queue is processed toward the end of a simulation diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 484a07d45d..5433bd7cd5 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -165,7 +165,7 @@ class ignition::gazebo::EntityComponentManagerPrivate std::unordered_multimap removedComponents; /// \brief Set of entities that are prevented from removal. - public: std::unordered_set unremovableEntities; + public: std::unordered_set pinnedEntities; }; ////////////////////////////////////////////////// @@ -278,9 +278,9 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity, for (auto iter = tmpToRemoveEntities.begin(); iter != tmpToRemoveEntities.end();) { - if (std::find(this->dataPtr->unremovableEntities.begin(), - this->dataPtr->unremovableEntities.end(), *iter) != - this->dataPtr->unremovableEntities.end()) + if (std::find(this->dataPtr->pinnedEntities.begin(), + this->dataPtr->pinnedEntities.end(), *iter) != + this->dataPtr->pinnedEntities.end()) { iter = tmpToRemoveEntities.erase(iter); } @@ -305,7 +305,7 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity, ///////////////////////////////////////////////// void EntityComponentManager::RequestRemoveEntities() { - if (this->dataPtr->unremovableEntities.empty()) + if (this->dataPtr->pinnedEntities.empty()) { { std::lock_guard lock(this->dataPtr->entityRemoveMutex); @@ -321,9 +321,9 @@ void EntityComponentManager::RequestRemoveEntities() // UpdateViews on each of them for (const auto &vertex : this->dataPtr->entities.Vertices()) { - if (std::find(this->dataPtr->unremovableEntities.begin(), - this->dataPtr->unremovableEntities.end(), vertex.first) == - this->dataPtr->unremovableEntities.end()) + if (std::find(this->dataPtr->pinnedEntities.begin(), + this->dataPtr->pinnedEntities.end(), vertex.first) == + this->dataPtr->pinnedEntities.end()) { tmpToRemoveEntities.insert(vertex.first); } @@ -1575,37 +1575,35 @@ void EntityComponentManagerPrivate::AddModifiedComponent(const Entity &_entity) } ///////////////////////////////////////////////// -void EntityComponentManager::MarkEntityUnremovable(const Entity _entity, - bool _recursive) +void EntityComponentManager::PinEntity(const Entity _entity, bool _recursive) { if (_recursive) { this->dataPtr->InsertEntityRecursive(_entity, - this->dataPtr->unremovableEntities); + this->dataPtr->pinnedEntities); } else { - this->dataPtr->unremovableEntities.insert(_entity); + this->dataPtr->pinnedEntities.insert(_entity); } } ///////////////////////////////////////////////// -void EntityComponentManager::MarkEntityRemovable(const Entity _entity, - bool _recursive) +void EntityComponentManager::UnpinEntity(const Entity _entity, bool _recursive) { if (_recursive) { this->dataPtr->EraseEntityRecursive(_entity, - this->dataPtr->unremovableEntities); + this->dataPtr->pinnedEntities); } else { - this->dataPtr->unremovableEntities.erase(_entity); + this->dataPtr->pinnedEntities.erase(_entity); } } ///////////////////////////////////////////////// -void EntityComponentManager::MarkAllEntitiesRemovable() +void EntityComponentManager::UnpinAllEntities() { - this->dataPtr->unremovableEntities.clear(); + this->dataPtr->pinnedEntities.clear(); } diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 0cd7b5642c..948ab31650 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -2564,7 +2564,7 @@ TEST_P(EntityComponentManagerFixture, RemovedComponentsSyncBetweenServerAndGUI) } ////////////////////////////////////////////////// -TEST_P(EntityComponentManagerFixture, UnremovableEntity) +TEST_P(EntityComponentManagerFixture, PinnedEntity) { // Create some entities auto e1 = manager.CreateEntity(); @@ -2583,7 +2583,7 @@ TEST_P(EntityComponentManagerFixture, UnremovableEntity) EXPECT_EQ(3u, manager.EntityCount()); // Mark e1 as unremovable, which should also lock its child entity e2 - manager.MarkEntityUnremovable(e1); + manager.PinEntity(e1); // Try to remove e1, which is locked entity manager.RequestRemoveEntity(e1); @@ -2606,7 +2606,7 @@ TEST_P(EntityComponentManagerFixture, UnremovableEntity) EXPECT_EQ(2u, manager.EntityCount()); // Unmark e2, and now it should be removable. - manager.MarkEntityRemovable(e2); + manager.UnpinEntity(e2); manager.RequestRemoveEntity(e2); EXPECT_EQ(2u, manager.EntityCount()); EXPECT_TRUE(manager.HasEntitiesMarkedForRemoval()); @@ -2614,7 +2614,7 @@ TEST_P(EntityComponentManagerFixture, UnremovableEntity) EXPECT_EQ(1u, manager.EntityCount()); // Unmark all entities, and now it should be removable. - manager.MarkAllEntitiesRemovable(); + manager.UnpinAllEntities(); manager.RequestRemoveEntities(); EXPECT_TRUE(manager.HasEntitiesMarkedForRemoval()); manager.ProcessEntityRemovals(); From fd9ca26c0f1799b2dd97dd72c43186358f710027 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 25 Jun 2021 08:26:57 -0700 Subject: [PATCH 6/6] Fix codecheck Signed-off-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));