diff --git a/CMakeLists.txt b/CMakeLists.txt index 17e844b..a290db0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,7 +61,7 @@ target_link_libraries(collision_plugin PRIVATE gz-sim${GAZEBO_VERSION}::gz-sim${GAZEBO_VERSION} ) -add_library(groundtruth_plugin SHARED src/groundtruth_plugin.cc) +add_library(groundtruth_plugin SHARED src/groundtruth_plugin.cpp) target_link_libraries(groundtruth_plugin PRIVATE gz-sim${GAZEBO_VERSION}::gz-sim${GAZEBO_VERSION} PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} diff --git a/include/groundtruth_plugin.h b/include/groundtruth_plugin.h new file mode 100644 index 0000000..e9dfac1 --- /dev/null +++ b/include/groundtruth_plugin.h @@ -0,0 +1,96 @@ +#pragma once + +#include +#include +#include +#include +#include "gz/sim/Model.hh" +#include "common.h" +#include + +namespace gz { + namespace sim { +// Inline bracket to help doxygen filtering. + inline namespace GZ_SIM_VERSION_NAMESPACE { + namespace systems { + + /// \brief Pose publisher system. Attach to an entity to publish the + /// transform of its child entities in the form of gz::msgs::Pose + /// messages + /// + /// ## System Parameters + /// + /// - ``: Frequency of pose publications in Hz. A negative + /// frequency publishes as fast as possible (i.e, at the rate of the + /// simulation step) + class GroundtruthPlugin + : public System, + public ISystemConfigure, + public ISystemPostUpdate { + public: + /// \brief Constructor + GroundtruthPlugin(); + + /// \brief Destructor + ~GroundtruthPlugin() override = default; + + // Documentation inherited + void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + void PostUpdate( + const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; + + /// \brief Publishes pose with the provided time + /// stamp. + /// \param[in] _stampMsg Time stamp associated with published poses + void PublishPose(const EntityComponentManager &_ecm, + const msgs::Time &_stampMsg); + + private: + /// \brief Gazebo communication node. + transport::Node node; + + /// \brief publisher for pose data + transport::Node::Publisher posePub; + + /// \brief Model interface + Model model{kNullEntity}; + + /// \brief Frequency of pose publications in Hz. A negative frequency + /// publishes as fast as possible (i.e, at the rate of the simulation step) + double updateFrequency = -1; + + /// \brief Last time poses were published. + std::chrono::steady_clock::duration lastPosePubTime{0}; + + /// \brief Update period in nanoseconds calculated from the update_frequency + /// parameter + std::chrono::steady_clock::duration updatePeriod{0}; + + /// \brief A variable that gets populated with poses. This also here as a + /// member variable to avoid repeated memory allocations and improve + /// performance. + msgs::Pose poseMsg; + + /// \brief Model name + std::string model_name_{}; + + // Home defaults to Zurich Irchel Park + // @note The home position can be specified using the environment variables: + // PX4_HOME_LAT, PX4_HOME_LON, and PX4_HOME_ALT + double lat_home_ = kDefaultHomeLatitude; + double lon_home_ = kDefaultHomeLongitude; + double alt_home_ = kDefaultHomeAltitude; + double world_latitude_ = 0.0; + double world_longitude_ = 0.0; + double world_altitude_ = 0.0; + }; + } + } + } +} diff --git a/include/groundtruth_plugin.hh b/include/groundtruth_plugin.hh deleted file mode 100644 index 5f8d789..0000000 --- a/include/groundtruth_plugin.hh +++ /dev/null @@ -1,203 +0,0 @@ -/* - * Copyright (C) 2019 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 GZ_SIM_SYSTEMS_POSEPUBLISHER_HH_ -#define GZ_SIM_SYSTEMS_POSEPUBLISHER_HH_ - -#include -#include -#include -#include -#include "gz/sim/Model.hh" -#include - -namespace gz { - namespace sim { -// Inline bracket to help doxygen filtering. - inline namespace GZ_SIM_VERSION_NAMESPACE { - namespace systems { - - /// \brief Pose publisher system. Attach to an entity to publish the - /// transform of its child entities in the form of gz::msgs::Pose - /// messages, or a single gz::msgs::Pose_V message if - /// "use_pose_vector_msg" is true. - /// - /// ## System Parameters - /// - /// - ``: Set to true to publish link pose - /// - ``: Set to true to publish visual pose - /// - ``: Set to true to publish collision pose - /// - ``: Set to true to publish sensor pose - /// - ``: Set to true to publish model pose. - /// - ``: Set to true to publish nested model - /// pose. The pose of the model that contains this system is also published - /// unless publish_model_pose is set to false - /// - ``: Set to true to publish a gz::msgs::Pose_V - /// message instead of mulitple gz::msgs::Pose messages. - /// - ``: Frequency of pose publications in Hz. A negative - /// frequency publishes as fast as possible (i.e, at the rate of the - /// simulation step) - /// - ``: Set to true to publish static poses on a - /// "/pose_static" topic. This will cause only dynamic - /// poses to be published on the "/pose" topic. - /// - ``: Frequency of static pose publications in - /// Hz. A negative frequency publishes as fast as possible (i.e, at the - /// rate of the simulation step). - class PosePublisher - : public System, - public ISystemConfigure, - public ISystemPostUpdate { - public: - /// \brief Constructor - PosePublisher(); - - /// \brief Destructor - ~PosePublisher() override = default; - - // Documentation inherited - void Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &_eventMgr) override; - - // Documentation inherited - void PostUpdate( - const UpdateInfo &_info, - const EntityComponentManager &_ecm) override; - - /// \brief Private data pointer -// private: std::unique_ptr dataPtr; - - /// \brief Initializes internal caches for entities whose poses are to be - /// published and their names - /// \param[in] _ecm Immutable reference to the entity component manager - void InitializeEntitiesToPublish(const EntityComponentManager &_ecm); - - /// \brief Helper function to collect entity pose data - /// \param[in] _ecm Immutable reference to the entity component manager - /// \param[out] _poses Pose vector to be filled - /// \param[in] _static True to fill only static transforms, - /// false to fill only dynamic transforms - void FillPoses(const EntityComponentManager &_ecm, - std::vector> &_poses, - bool _static); - - /// \brief Publishes poses collected by FillPoses with the provided time - /// stamp. - /// \param[in] _poses Pose to publish - /// \param[in] _stampMsg Time stamp associated with published poses - /// \param[in] _publisher Publisher to publish the message - void PublishPoses( - std::vector> &_poses, - const msgs::Time &_stampMsg, - transport::Node::Publisher &_publisher); - - private: - /// \brief Gazebo communication node. - transport::Node node; - - /// \brief publisher for pose data - transport::Node::Publisher posePub; - - /// \brief True to publish static transforms to a separate topic - bool staticPosePublisher = false; - - /// \brief publisher for pose data - transport::Node::Publisher poseStaticPub; - - /// \brief Model interface - Model model{kNullEntity}; - - /// \brief True to publish link pose - bool publishLinkPose = true; - - /// \brief True to publish visual pose - bool publishVisualPose = false; - - /// \brief True to publish collision pose - bool publishCollisionPose = false; - - /// \brief True to publish sensor pose - bool publishSensorPose = false; - - /// \brief True to publish nested model pose - bool publishNestedModelPose = false; - - /// \brief True to publish model pose - bool publishModelPose = false; - - /// \brief Frequency of pose publications in Hz. A negative frequency - /// publishes as fast as possible (i.e, at the rate of the simulation step) - double updateFrequency = -1; - - /// \brief Last time poses were published. - std::chrono::steady_clock::duration lastPosePubTime{0}; - - /// \brief Last time static poses were published. - std::chrono::steady_clock::duration lastStaticPosePubTime{0}; - - /// \brief Update period in nanoseconds calculated from the update_frequency - /// parameter - std::chrono::steady_clock::duration updatePeriod{0}; - - /// \brief Update period in nanoseconds calculated from the - /// static_update_frequency parameter - std::chrono::steady_clock::duration staticUpdatePeriod{0}; - - /// \brief Cache of entities, their frame names and their child frame names. - /// The key is the entity whose pose is to be published. - /// The frame name is the scoped name of the parent entity. - /// The child frame name is the scoped name of the entity (the key) - std::unordered_map> - entitiesToPublish; - - /// \brief Entities with pose that can change over time, i.e. links connected - /// by joints - std::unordered_set dynamicEntities; - - /// \brief A vector that contains the entities and their poses. This could - /// easily be a temporary, but having it as a member variable improves - /// performance by avoiding memory allocation - std::vector> poses; - - /// \brief A vector that contains the entities and poses that are static. - /// This could easily be a temporary, but having it as a member variable - /// improves performance by avoiding memory allocation - std::vector> staticPoses; - - /// \brief A variable that gets populated with poses. This also here as a - /// member variable to avoid repeated memory allocations and improve - /// performance. - msgs::Pose poseMsg; - - /// \brief A variable that gets populated with poses. This also here as a - /// member variable to avoid repeated memory allocations and improve - /// performance. - msgs::Pose_V poseVMsg; - - /// \brief True to publish a vector of poses. False to publish individual pose - /// msgs. - bool usePoseV = false; - - /// \brief Whether cache variables have been initialized - bool initialized{false}; - }; - } - } - } -} - -#endif diff --git a/src/groundtruth_plugin.cc b/src/groundtruth_plugin.cc deleted file mode 100644 index c6e0d29..0000000 --- a/src/groundtruth_plugin.cc +++ /dev/null @@ -1,472 +0,0 @@ -/* - * Copyright (C) 2019 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 "groundtruth_plugin.hh" - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include "gz/sim/Util.hh" -#include "gz/sim/components/CanonicalLink.hh" -#include "gz/sim/components/ChildLinkName.hh" -#include "gz/sim/components/Collision.hh" -#include "gz/sim/components/Joint.hh" -#include "gz/sim/components/JointType.hh" -#include "gz/sim/components/Link.hh" -#include "gz/sim/components/Model.hh" -#include "gz/sim/components/Name.hh" -#include "gz/sim/components/ParentEntity.hh" -#include "gz/sim/components/ParentLinkName.hh" -#include "gz/sim/components/Pose.hh" -#include "gz/sim/components/Sensor.hh" -#include "gz/sim/components/Visual.hh" -#include "gz/sim/Conversions.hh" - -using namespace gz; -using namespace sim; -using namespace systems; - -////////////////////////////////////////////////// -PosePublisher::PosePublisher() -{ -} - -////////////////////////////////////////////////// -void PosePublisher::Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &/*_eventMgr*/) -{ - model = Model(_entity); - - if (!model.Valid(_ecm)) - { - gzerr << "PosePublisher plugin should be attached to a model entity. " - << "Failed to initialize." << std::endl; - return; - } - - // parse optional params - publishLinkPose = _sdf->Get("publish_link_pose", - publishLinkPose).first; - - publishNestedModelPose = - _sdf->Get("publish_nested_model_pose", - publishNestedModelPose).first; - - // for backward compatibility, publish_model_pose will be set to the - // same value as publish_nested_model_pose if it is not specified. - publishModelPose = - _sdf->Get("publish_model_pose", - publishNestedModelPose).first; - - publishVisualPose = - _sdf->Get("publish_visual_pose", - publishVisualPose).first; - - publishCollisionPose = - _sdf->Get("publish_collision_pose", - publishCollisionPose).first; - - publishSensorPose = - _sdf->Get("publish_sensor_pose", - publishSensorPose).first; - - double updateFrequency = _sdf->Get("update_frequency", -1).first; - - if (updateFrequency > 0) - { - std::chrono::duration period{1 / updateFrequency}; - updatePeriod = - std::chrono::duration_cast(period); - } - - staticPosePublisher = - _sdf->Get("static_publisher", - staticPosePublisher).first; - - if (staticPosePublisher) - { - // update rate for static transforms. Default to same as - double staticPoseUpdateFrequency = - _sdf->Get("static_update_frequency", updateFrequency).first; - - if (staticPoseUpdateFrequency > 0) - { - std::chrono::duration period{1 / staticPoseUpdateFrequency}; - staticUpdatePeriod = - std::chrono::duration_cast( - period); - } - } - - // create publishers - usePoseV = - _sdf->Get("use_pose_vector_msg", usePoseV).first; - - std::string poseTopic = scopedName(_entity, _ecm) + "/pose_custom"; - poseTopic = transport::TopicUtils::AsValidTopic(poseTopic); - if (poseTopic.empty()) - { - poseTopic = "/pose_custom"; - gzerr << "Empty pose topic generated for pose_publisher system. " - << "Setting to " << poseTopic << std::endl; - } - std::string staticPoseTopic = poseTopic + "_static"; - - if (usePoseV) - { - posePub = - node.Advertise(poseTopic); - - if (staticPosePublisher) - { - poseStaticPub = - node.Advertise( - staticPoseTopic); - } - } - else - { - posePub = - node.Advertise(poseTopic); - if (staticPosePublisher) - { - poseStaticPub = - node.Advertise( - staticPoseTopic); - } - } -} - -////////////////////////////////////////////////// -void PosePublisher::PostUpdate(const UpdateInfo &_info, - const EntityComponentManager &_ecm) -{ - GZ_PROFILE("PosePublisher::PostUpdate"); - - // \TODO(anyone) Support rewind - if (_info.dt < std::chrono::steady_clock::duration::zero()) - { - gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; - } - - // Nothing left to do if paused. - if (_info.paused) - return; - - bool publish = true; - auto diff = _info.simTime - lastPosePubTime; - // If the diff is positive and it's less than the update period, we skip - // publication. If the diff is negative, then time has gone backward, we go - // ahead publish and allow the time to be reset - if ((diff > std::chrono::steady_clock::duration::zero()) && - (diff < updatePeriod)) - { - publish = false; - } - - bool publishStatic = true; - auto staticDiff = _info.simTime - lastStaticPosePubTime; - if (!staticPosePublisher || - ((staticDiff > std::chrono::steady_clock::duration::zero()) && - (staticDiff < staticUpdatePeriod))) - { - publishStatic = false; - } - - if (!publish && !publishStatic) - return; - - if (!initialized) - { - InitializeEntitiesToPublish(_ecm); - initialized = true; - } - - - // if static transforms are published through a different topic - if (staticPosePublisher) - { - if (publishStatic) - { - staticPoses.clear(); - FillPoses(_ecm, staticPoses, true); - PublishPoses(staticPoses, - convert(_info.simTime), poseStaticPub); - lastStaticPosePubTime = _info.simTime; - } - - if (publish) - { - poses.clear(); - FillPoses(_ecm, poses, false); - PublishPoses(poses, - convert(_info.simTime), posePub); - lastPosePubTime = _info.simTime; - } - } - // publish all transforms to the same topic - else if (publish) - { - poses.clear(); - FillPoses(_ecm, poses, true); - FillPoses(_ecm, poses, false); - PublishPoses(poses, - convert(_info.simTime), posePub); - lastPosePubTime = _info.simTime; - } -} - -////////////////////////////////////////////////// -void PosePublisher::InitializeEntitiesToPublish( - const EntityComponentManager &_ecm) -{ - std::stack toCheck; - toCheck.push(this->model.Entity()); - std::vector visited; - while (!toCheck.empty()) - { - Entity entity = toCheck.top(); - toCheck.pop(); - visited.push_back(entity); - - auto link = _ecm.Component(entity); - auto visual = _ecm.Component(entity); - auto collision = _ecm.Component(entity); - auto sensor = _ecm.Component(entity); - auto joint = _ecm.Component(entity); - - auto isModel = _ecm.Component(entity); - auto parent = _ecm.Component(entity); - - bool fillPose = (link && this->publishLinkPose) || - (visual && this->publishVisualPose) || - (collision && this->publishCollisionPose) || - (sensor && this->publishSensorPose); - - // for backward compatibility, top level model pose will be published - // if publishNestedModelPose is set to true unless the user explicity - // disables this by setting publishModelPose to false - if (isModel) - { - if (parent) - { - auto nestedModel = _ecm.Component(parent->Data()); - if (nestedModel) - fillPose = this->publishNestedModelPose; - } - if (!fillPose) - { - fillPose = this->publishNestedModelPose && this->publishModelPose; - } - } - - if (fillPose) - { - std::string frame; - std::string childFrame; - auto entityName = _ecm.Component(entity); - if (!entityName) - continue; - childFrame = - removeParentScope(scopedName(entity, _ecm, "::", false), "::"); - - if (parent) - { - auto parentName = _ecm.Component(parent->Data()); - if (parentName) - { - frame = removeParentScope( - scopedName(parent->Data(), _ecm, "::", false), "::"); - } - } - this->entitiesToPublish[entity] = std::make_pair(frame, childFrame); - } - - // get dynamic entities - if (this->staticPosePublisher && joint) - { - sdf::JointType jointType = - _ecm.Component(entity)->Data(); - if (jointType != sdf::JointType::INVALID && - jointType != sdf::JointType::FIXED) - { - std::string parentLinkName = - _ecm.Component(entity)->Data(); - std::string childLinkName = - _ecm.Component(entity)->Data(); - - auto parentLinkEntity = _ecm.EntityByComponents( - components::Name(parentLinkName), components::Link(), - components::ParentEntity(this->model.Entity())); - auto childLinkEntity = _ecm.EntityByComponents( - components::Name(childLinkName), components::Link(), - components::ParentEntity(this->model.Entity())); - - // add to list if not a canonical link - if (!_ecm.Component(parentLinkEntity)) - this->dynamicEntities.insert(parentLinkEntity); - if (!_ecm.Component(childLinkEntity)) - this->dynamicEntities.insert(childLinkEntity); - } - } - - // Recursively check if child entities need to be published - auto childEntities = - _ecm.ChildrenByComponents(entity, components::ParentEntity(entity)); - - // Use reverse iterators to match the order of entities found so as to match - // the expected order in the pose_publisher integration test. - for (auto childIt = childEntities.rbegin(); childIt != childEntities.rend(); - ++childIt) - { - auto it = std::find(visited.begin(), visited.end(), *childIt); - if (it == visited.end()) - { - // Only add to stack if the entity hasn't been already been visited. - // This also ensures there are no cycles. - toCheck.push(*childIt); - } - } - } - - // sanity check to make sure dynamicEntities are a subset of entitiesToPublish - for (auto const &ent : this->dynamicEntities) - { - if (this->entitiesToPublish.find(ent) == this->entitiesToPublish.end()) - { - gzwarn << "Entity id: '" << ent << "' not found when creating a list " - << "of dynamic entities in pose publisher." << std::endl; - } - } - - if (this->staticPosePublisher) - { - this->poses.reserve(this->dynamicEntities.size()); - this->staticPoses.reserve( - this->entitiesToPublish.size() - this->dynamicEntities.size()); - } - else - { - this->poses.reserve(this->entitiesToPublish.size()); - } -} - -////////////////////////////////////////////////// -void PosePublisher::FillPoses(const EntityComponentManager &_ecm, - std::vector> &_poses, bool _static) -{ - GZ_PROFILE("PosePublisher::FillPose"); - - for (const auto &entity : this->entitiesToPublish) - { - auto pose = _ecm.Component(entity.first); - if (!pose) - continue; - - bool isStatic = this->dynamicEntities.find(entity.first) == - this->dynamicEntities.end(); - - if (_static == isStatic) - _poses.emplace_back(entity.first, pose->Data()); - } -} - -////////////////////////////////////////////////// -void PosePublisher::PublishPoses( - std::vector> &_poses, - const msgs::Time &_stampMsg, - transport::Node::Publisher &_publisher) -{ - GZ_PROFILE("PosePublisher::PublishPoses"); - - // publish poses - msgs::Pose *msg = nullptr; - if (this->usePoseV) - this->poseVMsg.Clear(); - - for (const auto &[entity, pose] : _poses) - { - auto entityIt = this->entitiesToPublish.find(entity); - if (entityIt == this->entitiesToPublish.end()) - continue; - - if (this->usePoseV) - { - msg = this->poseVMsg.add_pose(); - } - else - { - this->poseMsg.Clear(); - msg = &this->poseMsg; - } - - // fill pose msg - // frame_id: parent entity name - // child_frame_id = entity name - // pose is the transform from frame_id to child_frame_id - GZ_ASSERT(msg != nullptr, "Pose msg is null"); - auto header = msg->mutable_header(); - - header->mutable_stamp()->CopyFrom(_stampMsg); - const std::string &frameId = entityIt->second.first; - const std::string &childFrameId = entityIt->second.second; - const math::Pose3d &transform = pose; - auto frame = header->add_data(); - frame->set_key("frame_id"); - frame->add_value(frameId); - auto childFrame = header->add_data(); - childFrame->set_key("child_frame_id"); - childFrame->add_value(childFrameId); - - // set pose - msg->set_name(childFrameId); - msgs::Set(msg, transform); - - // publish individual pose msgs - if (!this->usePoseV) - _publisher.Publish(this->poseMsg); - } - - // publish pose vector msg - if (this->usePoseV) - _publisher.Publish(this->poseVMsg); -} - -GZ_ADD_PLUGIN(PosePublisher, - System, - PosePublisher::ISystemConfigure, - PosePublisher::ISystemPostUpdate) - -GZ_ADD_PLUGIN_ALIAS(PosePublisher, - "gz::sim::systems::PosePublisher") diff --git a/src/groundtruth_plugin.cpp b/src/groundtruth_plugin.cpp new file mode 100644 index 0000000..c69932e --- /dev/null +++ b/src/groundtruth_plugin.cpp @@ -0,0 +1,130 @@ +#include "groundtruth_plugin.h" + +#include +#include + +#include + +#include + +#include +#include +#include + +#include "gz/sim/Util.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/Conversions.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +////////////////////////////////////////////////// +GroundtruthPlugin::GroundtruthPlugin() { +} + +////////////////////////////////////////////////// +void GroundtruthPlugin::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) { + model = Model(_entity); + + if (!model.Valid(_ecm)) { + gzerr << "PosePublisher plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + model_name_ = removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + + double updateFrequency = _sdf->Get("update_frequency", -1).first; + + if (updateFrequency > 0) { + std::chrono::duration period{1 / updateFrequency}; + updatePeriod = std::chrono::duration_cast(period); + } + + // create publisher + std::string poseTopic = scopedName(_entity, _ecm) + "/groundtruth"; + poseTopic = transport::TopicUtils::AsValidTopic(poseTopic); + if (poseTopic.empty()) { + poseTopic = "/groundtruth"; + gzerr << "Empty pose topic generated for pose_publisher system. " + << "Setting to " << poseTopic << std::endl; + } + + posePub = node.Advertise(poseTopic); +} + +////////////////////////////////////////////////// +void GroundtruthPlugin::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) { + GZ_PROFILE("PosePublisher::PostUpdate"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) { + gzwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + // Nothing left to do if paused. + if (_info.paused) + return; + + + auto diff = _info.simTime - lastPosePubTime; + // If the diff is positive and it's less than the update period, we skip + // publication. If the diff is negative, then time has gone backward, we go + // ahead publish and allow the time to be reset + if ((diff > std::chrono::steady_clock::duration::zero()) && + (diff < updatePeriod)) { + return; + } + + PublishPose(_ecm, convert(_info.simTime)); + lastPosePubTime = _info.simTime; +} + +////////////////////////////////////////////////// +void GroundtruthPlugin::PublishPose(const EntityComponentManager &_ecm, + const msgs::Time &_stampMsg) { + GZ_PROFILE("PosePublisher::PublishPoses"); + + // publish poses + msgs::Pose *msg{nullptr}; + auto pose = _ecm.Component(model.Entity()); + if (!pose) + return; + + this->poseMsg.Clear(); + msg = &this->poseMsg; + + // fill pose msg + // pose is the transform from parent_name_ to model_name_ + GZ_ASSERT(msg != nullptr, "Pose msg is null"); + auto header = msg->mutable_header(); + + header->mutable_stamp()->CopyFrom(_stampMsg); + const math::Pose3d &transform = pose->Data(); + auto childFrame = header->add_data(); + childFrame->set_key("model_name"); + childFrame->add_value(model_name_); + + // set pose + msg->set_name(model_name_); + msgs::Set(msg, transform); + + // publish individual pose msgs + posePub.Publish(this->poseMsg); +} + +GZ_ADD_PLUGIN(GroundtruthPlugin, + System, + GroundtruthPlugin::ISystemConfigure, + GroundtruthPlugin::ISystemPostUpdate) + +GZ_ADD_PLUGIN_ALIAS(GroundtruthPlugin, + "gz::sim::systems::GroundtruthPlugin")