Add tutorial for using the Pose component (#2219)

Adds a tutorial on how to use the Pose component, using OdometryPublisher system as an example, because that system depends on only 1 component, good as a standalone example.

---------

Signed-off-by: Addisu Z. Taddese <[email protected]>
Signed-off-by: Mabel Zhang <[email protected]>
Signed-off-by: Ian Chen <[email protected]>
Signed-off-by: Mabel Zhang <[email protected]>
Co-authored-by: Addisu Z. Taddese <[email protected]>
Co-authored-by: Ian Chen <[email protected]>
Co-authored-by: Alejandro Hernández Cordero <[email protected]>

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<const sdf::Element> &_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<msgs::Odometry>(
         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<double>(_info.dt).count()
            << "s]. System may not work properly." << std::endl;
   }
-
-  // Create the pose component if it does not exist.
-  auto pos = _ecm.Component<components::Pose>(
-      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<double> 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.

src/systems/odometry_publisher/OdometryPublisher.hh

@@ -76,6 +76,13 @@ namespace systems
   /// - `<gaussian_noise>`: 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,

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 `<model>`
+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.

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"