From 5256a140945bce7628507f805db6c41bf2c3bf44 Mon Sep 17 00:00:00 2001 From: Maganty Rushyendra Date: Mon, 11 Jan 2021 16:38:06 +0800 Subject: [PATCH] Add documentation for variables and functions Includes minor stylistic changes. Signed-off-by: Maganty Rushyendra --- .../odometry_publisher/OdometryPublisher.cc | 35 ++++++++++--------- .../odometry_publisher/OdometryPublisher.hh | 22 +++++++----- 2 files changed, 31 insertions(+), 26 deletions(-) diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index 424980f248c..dfc0c0e27d0 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -41,7 +41,7 @@ using namespace systems; class ignition::gazebo::systems::OdometryPublisherPrivate { - /// \brief Update odometry and publish an odometry message. + /// \brief Calculates odometry and publishes an odometry message. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. @@ -54,8 +54,10 @@ class ignition::gazebo::systems::OdometryPublisherPrivate /// \brief Model interface public: Model model{kNullEntity}; + /// \brief Name of the world-fixed coordinate frame for the odometry message. public: std::string odomFrame; + /// \brief Name of the coordinate frame rigidly attached to the mobile robot base. public: std::string robotBaseFrame; /// \brief Update period calculated from . @@ -73,8 +75,10 @@ class ignition::gazebo::systems::OdometryPublisherPrivate /// \brief Rolling mean accumulators for the angular velocity public: math::RollingMean angularMean; + /// \brief Initialized flag. public: bool initialized{false}; + /// \brief Current pose of the model in the odom frame. public: math::Pose3d lastUpdatePose{0, 0, 0, 0, 0, 0}; /// \brief Current timestamp. @@ -111,7 +115,7 @@ void OdometryPublisher::Configure(const Entity &_entity, this->dataPtr->odomFrame = "odom"; if (!_sdf->HasElement("odom_frame")) { - ignwarn << "PlanarMovePlugin missing , " + ignwarn << "OdometryPublisher system plugin missing , " << "defaults to \"" << this->dataPtr->odomFrame << "\"" << std::endl; } else @@ -122,7 +126,7 @@ void OdometryPublisher::Configure(const Entity &_entity, this->dataPtr->robotBaseFrame = "base_footprint"; if (!_sdf->HasElement("robot_base_frame")) { - ignwarn << "PlanarMovePlugin missing , " + ignwarn << "OdometryPublisher system plugin missing , " << "defaults to \"" << this->dataPtr->robotBaseFrame << "\"" << std::endl; } else @@ -133,14 +137,14 @@ void OdometryPublisher::Configure(const Entity &_entity, double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; if (odomFreq > 0) { - std::chrono::duration odomPer{1 / odomFreq}; + std::chrono::duration period{1 / odomFreq}; this->dataPtr->odomPubPeriod = - std::chrono::duration_cast(odomPer); + std::chrono::duration_cast(period); } // Setup odometry std::string odomTopic{"/model/" + this->dataPtr->model.Name(_ecm) + - "/odometryFoo"}; + "/odometry"}; if (_sdf->HasElement("odom_topic")) odomTopic = _sdf->Get("odom_topic"); this->dataPtr->odomPub = this->dataPtr->node.Advertise( @@ -188,18 +192,17 @@ void OdometryPublisherPrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo const ignition::gazebo::EntityComponentManager &_ecm) { IGN_PROFILE("DiffDrive::UpdateOdometry"); - // Initialize, if not already initialized. + // Record start time. if (!this->initialized) { this->lastUpdateTime = std::chrono::steady_clock::time_point(_info.simTime); this->initialized = true; - return; //to check why + return; } // Construct the odometry message and publish it. msgs::Odometry msg; - // Compute x, y and heading using velocity const std::chrono::duration dt = std::chrono::steady_clock::time_point(_info.simTime) - lastUpdateTime; // We cannot estimate the speed if the time interval is zero (or near @@ -207,13 +210,13 @@ void OdometryPublisherPrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo if (math::equal(0.0, dt.count())) return; - // World to odom transformation + // Get and set robotBaseFrame to odom transformation. const math::Pose3d pose = worldPose(this->model.Entity(), _ecm); msg.mutable_pose()->mutable_position()->set_x(pose.Pos().X()); msg.mutable_pose()->mutable_position()->set_y(pose.Pos().Y()); msgs::Set(msg.mutable_pose()->mutable_orientation(), pose.Rot()); - // Get linear and angular displacements from last updated pose + // Get linear and angular displacements from last updated pose. double linearDisplacementX = pose.Pos().X() - this->lastUpdatePose.Pos().X(); double linearDisplacementY = pose.Pos().Y() - this->lastUpdatePose.Pos().Y(); @@ -223,8 +226,7 @@ void OdometryPublisherPrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo while (currentYaw > lastYaw + M_PI) currentYaw -= 2 * M_PI; const float angularDiff = currentYaw - lastYaw; - // auto odom_lin_vel_comp = linearVelocity->Data(); - // Get velocities in child frame (i.e. base_link frame) and add to message + // Get velocities in robotBaseFrame and add to message. double linearVelocityX = (cosf(currentYaw) * linearDisplacementX + sinf(currentYaw) * linearDisplacementX) / dt.count(); double linearVelocityY = (cosf(currentYaw) * linearDisplacementY @@ -236,14 +238,14 @@ void OdometryPublisherPrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo msg.mutable_twist()->mutable_linear()->set_y(this->linearMean.second.Mean()); msg.mutable_twist()->mutable_angular()->set_z(this->angularMean.Mean()); - // Set the time stamp in the header + // Set the time stamp in the header. msg.mutable_header()->mutable_stamp()->CopyFrom( convert(_info.simTime)); // Set the frame ids. auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->model.Name(_ecm) + "/" +odomFrame); + frame->add_value(this->model.Name(_ecm) + "/" + odomFrame); auto childFrame = msg.mutable_header()->add_data(); childFrame->set_key("child_frame_id"); childFrame->add_value(this->model.Name(_ecm) + "/" + robotBaseFrame); @@ -251,7 +253,7 @@ void OdometryPublisherPrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo this->lastUpdatePose = pose; this->lastUpdateTime = std::chrono::steady_clock::time_point(_info.simTime); - // Throttle publishing + // Throttle publishing. auto diff = _info.simTime - this->lastOdomPubTime; if (diff > std::chrono::steady_clock::duration::zero() && diff < this->odomPubPeriod) @@ -259,7 +261,6 @@ void OdometryPublisherPrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo return; } this->lastOdomPubTime = _info.simTime; - // Publish the message this->odomPub.Publish(msg); } diff --git a/src/systems/odometry_publisher/OdometryPublisher.hh b/src/systems/odometry_publisher/OdometryPublisher.hh index 1c1d90e6293..3d23f21f79e 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.hh +++ b/src/systems/odometry_publisher/OdometryPublisher.hh @@ -17,8 +17,6 @@ #ifndef IGNITION_GAZEBO_SYSTEMS_ODOMETRYPUBLISHER_HH_ #define IGNITION_GAZEBO_SYSTEMS_ODOMETRYPUBLISHER_HH_ -#include - #include namespace ignition @@ -32,20 +30,26 @@ namespace systems // Forward declaration class OdometryPublisherPrivate; - /// \brief Odometry Publisher + /// \brief Odometry Publisher which can be attached to any entity in + /// order to periodically publish its odometry in the form of + /// ignition::msgs::Odometry messages. /// /// # System Parameters - /// ``: Custom topic on which this system will publish odometry - /// messages. This element if optional, and the default value is - /// `/model/{name_of_model}/odometry`. /// - /// ``: + /// ``: Name of the world-fixed coordinate frame for the + // odometry message. This element is optional, and the default value + /// is `odom`. + /// + /// ``: Name of the coordinate frame rigidly attached + /// to the mobile robot base. This element is optional, and the default + /// value is `robot_base_frame`. /// /// ``: Odometry publication frequency. This /// element is optional, and the default value is 50Hz. /// - /// ``: - /// + /// ``: Custom topic on which this system will publish odometry + /// messages. This element is optional, and the default value is + /// `/model/{name_of_model}/odometry`. class IGNITION_GAZEBO_VISIBLE OdometryPublisher : public System, public ISystemConfigure,