diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index c2fff130f8..fe5270ebe2 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -59,7 +59,9 @@ class gz::sim::systems::OdometryPublisherPrivate public: transport::Node node; /// \brief Model interface + //! [modelDeclaration] public: Model model{kNullEntity}; + //! [modelDeclaration] /// \brief Name of the world-fixed coordinate frame for the odometry message. public: std::string odomFrame; @@ -133,12 +135,14 @@ OdometryPublisher::OdometryPublisher() } ////////////////////////////////////////////////// +//! [Configure] void OdometryPublisher::Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, EventManager &/*_eventMgr*/) { this->dataPtr->model = Model(_entity); + //! [Configure] if (!this->dataPtr->model.Valid(_ecm)) { @@ -233,8 +237,10 @@ void OdometryPublisher::Configure(const Entity &_entity, } else { + //! [definePub] this->dataPtr->odomPub = this->dataPtr->node.Advertise( odomTopicValid); + //! [definePub] gzmsg << "OdometryPublisher publishing odometry on [" << odomTopicValid << "]" << std::endl; } @@ -300,15 +306,6 @@ void OdometryPublisher::PreUpdate(const gz::sim::UpdateInfo &_info, << std::chrono::duration(_info.dt).count() << "s]. System may not work properly." << std::endl; } - - // Create the pose component if it does not exist. - auto pos = _ecm.Component( - this->dataPtr->model.Entity()); - if (!pos) - { - _ecm.CreateComponent(this->dataPtr->model.Entity(), - components::Pose()); - } } ////////////////////////////////////////////////// @@ -347,7 +344,9 @@ void OdometryPublisherPrivate::UpdateOdometry( } // Construct the odometry message and publish it. + //! [declarePoseMsg] msgs::Odometry msg; + //! [declarePoseMsg] const std::chrono::duration dt = std::chrono::steady_clock::time_point(_info.simTime) - lastUpdateTime; @@ -357,7 +356,10 @@ void OdometryPublisherPrivate::UpdateOdometry( return; // Get and set robotBaseFrame to odom transformation. + //! [worldPose] const math::Pose3d rawPose = worldPose(this->model.Entity(), _ecm); + //! [worldPose] + //! [setPoseMsg] math::Pose3d pose = rawPose * this->offset; msg.mutable_pose()->mutable_position()->set_x(pose.Pos().X()); msg.mutable_pose()->mutable_position()->set_y(pose.Pos().Y()); @@ -366,6 +368,7 @@ void OdometryPublisherPrivate::UpdateOdometry( { msg.mutable_pose()->mutable_position()->set_z(pose.Pos().Z()); } + //! [setPoseMsg] // Get linear and angular displacements from last updated pose. double linearDisplacementX = pose.Pos().X() - this->lastUpdatePose.Pos().X(); @@ -476,7 +479,9 @@ void OdometryPublisherPrivate::UpdateOdometry( this->lastOdomPubTime = _info.simTime; if (this->odomPub.Valid()) { + //! [publishMsg] this->odomPub.Publish(msg); + //! [publishMsg] } // Generate odometry with covariance message and publish it. diff --git a/src/systems/odometry_publisher/OdometryPublisher.hh b/src/systems/odometry_publisher/OdometryPublisher.hh index cab9bd6d14..33c8fd23af 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.hh +++ b/src/systems/odometry_publisher/OdometryPublisher.hh @@ -76,6 +76,13 @@ namespace systems /// - ``: Standard deviation of the Gaussian noise to be added /// to pose and twist messages. This element is optional, and the default /// value is 0. + /// + /// ## Components + /// + /// This system uses the following components: + /// + /// - gz::sim::components::Pose: Pose represented by gz::math::Pose3d. Used + /// to calculate the odometry to publish. class OdometryPublisher : public System, public ISystemConfigure, diff --git a/tutorials/component_pose.md b/tutorials/component_pose.md new file mode 100644 index 0000000000..34d440e12e --- /dev/null +++ b/tutorials/component_pose.md @@ -0,0 +1,80 @@ +\page posecomponent Case Study: Using the Pose Component + +We will show how to use the gz::sim::components::Pose component in a system. + +An example usage of the component can be found in the +gz::sim::systems::OdometryPublisher system +([source code](https://github.com/gazebosim/gz-sim/tree/gz-sim8/src/systems/odometry_publisher)), +which reads the pose component of a model through the Model entity, uses the +pose for some calculations, and then publishes the result as a message. + +More usage can be found in the +[integration test](https://github.com/gazebosim/gz-sim/blob/gz-sim8/test/integration/odometry_publisher.cc) +for the system, with test worlds `odometry*.sdf` +[here](https://github.com/gazebosim/gz-sim/tree/main/test/worlds). + +### Objects of interest + +- gz::sim::components::Pose: A component containing pose information +- gz::math::Pose3d: The actual data underlying a pose component +- gz::sim::systems::OdometryPublisher: A system that reads the pose component + of a model +- gz::sim::Model: The type underlying a model entity (gz::sim::Entity) + +### Find the model entity + +First, we will need access to an entity, the \ref gz::sim::Model entity in this +case. +`OdometryPublisher` happens to be a system meant to be specified under `` +in the SDF, so at the time `Configure()` is called, it has access to a model +entity from which we can extract a \ref gz::sim::Model: + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc modelDeclaration +\snippet src/systems/odometry_publisher/OdometryPublisher.cc Configure + +### Read the pose component + +Once we have the handle to an entity, we can access components associated with +it. +A component may have been created at the time the world is loaded, or you may +create a component at runtime if it does not exist yet. + +In this case, we use the model entity found above to access its pose component, +which is created by default on every model entity. + +In `PostUpdate()`, which happens after physics has updated, we can get the +world pose of a model through gz::sim::worldPose, by passing in the model +entity and the entity component manager. + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc worldPose + +It returns the raw data to us in the gz::math::Pose3d type, which is also the +data type underlying a pose component. +We can perform calculations on the gz::math::Pose3d type, not the +gz::sim::components::Pose type, which is just a wrapper. + +### Use the pose component + +Now we can use the pose data as we like. + +Here, we manipulate the pose and package the result into a gz::msgs::Odometry +message to be published: + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc declarePoseMsg + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc setPoseMsg + +See the source code for setting other fields of the message, such as twist and +the header. + +The message is then published: + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc publishMsg + +where `odomPub` is defined in `Configure()`: + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc definePub + +Outside the scope of this tutorial, the odometry publisher system also +calculates the covariance and publishes a pose vector on a TF topic. +See the source code to learn more. diff --git a/tutorials/using_components.md b/tutorials/using_components.md index 717beea566..1c91d47e46 100644 --- a/tutorials/using_components.md +++ b/tutorials/using_components.md @@ -72,3 +72,4 @@ The rest of the tutorial is case studies that walk through the usage of specific components. - \subpage jointforcecmdcomponent "JointForceCmd" +- \subpage posecomponent "Pose"