Merge branch 'feature/docstrings' into 'master'

docs: Add docstrings for most classes

See merge request ruvu/mcl!79

ruvu_mcl/README.md

@@ -51,8 +51,28 @@ See also [BENCHMARK.md](./BENCHMARK.md) for a guide on how to compare different
 
 ## Design
 
-The design of the MCL is split into a core library (mcl) and a ROS wrapper. This makes it easy in the future to migrate to ROS2 or other platforms.
+The design of the MCL is split into a core library (ruvu_mcl::Mcl) and a ROS wrapper (ruvu_mcl::MclRos). This makes it easy in the future to migrate to ROS2 or other platforms.
 
+### Implemented motion models
+- ruvu_mcl::DifferentialMotionModel
+
+### Implemented laser sensor models
+- ruvu_mcl::BeamModel
+- ruvu_mcl::LikelihoodFieldModel (default)
+
+### Implemented landmark sensor models
+- ruvu_mcl::GaussianLandmarkModel (default)
+- ruvu_mcl::LandmarkLikelihoodFieldModel
+
+### Implemented adaptive methods
+- ruvu_mcl::Fixed (default)
+- ruvu_mcl::KLDSampling
+- ruvu_mcl::SplitAndMerge
+
+### Implemented resamplers
+- ruvu_mcl::LowVariance
+
+### Class diagram
 ```plantuml
 class Node {
     tf2_ros::MessageFilter

ruvu_mcl/src/adaptive/adaptive_method.hpp

@@ -7,16 +7,28 @@ namespace ruvu_mcl
 // forward declare
 class ParticleFilter;
 
+/**
+ * @brief Adaptive methods changes the efficiency of filtering by adapting the number of particles.
+ */
 class AdaptiveMethod
 {
 public:
   virtual ~AdaptiveMethod() = default;
 
+  /**
+   * @brief Hook that runs before the odometry update
+   */
   virtual void after_odometry_update(ParticleFilter * pf) const {}
+
+  /**
+   * @brief Hook that runs after the sensor update
+   */
   virtual void after_sensor_update(ParticleFilter * pf) const {}
 
   /**
    * @brief Calculate how many particles are wanted
+   *
+   * This number will be used as a target when resampling
    * @return Number of particles
    */
   virtual int calc_needed_particles(const ParticleFilter & pf) const = 0;

ruvu_mcl/src/adaptive/fixed.hpp

@@ -11,6 +11,9 @@ namespace ruvu_mcl
 class ParticleFilter;
 struct Config;
 
+/**
+ * @brief Use a fixed amount of particles (not adaptive)
+ */
 class Fixed : public AdaptiveMethod
 {
 public:

ruvu_mcl/src/adaptive/kld_sampling.hpp

@@ -10,6 +10,11 @@ namespace ruvu_mcl
 struct KLDSamplingConfig;
 class ParticleFilter;
 
+/**
+ * @brief Use KLD sampling to adapt the number of particles
+ *
+ * From the paper: KLD-Sampling: Adaptive Particle Filters
+ */
 class KLDSampling : public AdaptiveMethod
 {
 public:

ruvu_mcl/src/adaptive/split_and_merge.hpp

@@ -9,6 +9,12 @@ namespace ruvu_mcl
 // forward declare
 class ParticleFilter;
 
+/**
+ * @brief Use split & merge to adapt the number of particles
+ *
+ * From the paper: Monte Carlo localization for mobile robot using adaptive particle merging and
+ * splitting technique
+ */
 class SplitAndMerge : public AdaptiveMethod
 {
 public:
@@ -18,7 +24,14 @@ class SplitAndMerge : public AdaptiveMethod
   void after_sensor_update(ParticleFilter * pf) const override { split_particles(pf); }
   int calc_needed_particles(const ParticleFilter & pf) const override;
 
+  /**
+   * @brief Merge particles that are close together
+   */
   void merge_particles(ParticleFilter * pf) const;
+
+  /**
+   * @brief Split particles that have a high weight
+   */
   void split_particles(ParticleFilter * pf) const;
 
 private:

ruvu_mcl/src/adaptive/utils.hpp

@@ -7,6 +7,11 @@ namespace ruvu_mcl
 {
 using Key = std::tuple<int, int, int>;
 
+/**
+ * @brief Hash function for Keys
+ *
+ * This class can be used as hash function in unordered_map<Key> or unordered_set<Key>.
+ */
 struct KeyHasher
 {
   std::size_t operator()(const Key & key) const noexcept;

ruvu_mcl/src/cloud_publisher.hpp

@@ -20,6 +20,9 @@ namespace ruvu_mcl
 {
 class Particle;
 
+/**
+ * @brief Publishes a vector of particles as a visualization_msgs::Marker
+ */
 class CloudPublisher
 {
 public:

ruvu_mcl/src/main.cpp

@@ -9,6 +9,9 @@ using ruvu_mcl::Node;
 
 constexpr auto name = "main";
 
+/**
+* @brief Main ros node of the ruvu_mcl package
+*/
 int main(int argc, char ** argv)
 {
   ros::init(argc, argv, "mcl");

ruvu_mcl/src/map.hpp

@@ -15,51 +15,109 @@ ROS_DECLARE_MESSAGE(OccupancyGrid)
 
 namespace ruvu_mcl
 {
+/**
+ * @brief Base class for 2-D grid maps
+ */
 struct Map
 {
+  /**
+   * @brief The origin of the map
+   */
   tf2::Transform origin;
 
-  // Map scale (m/cell)
+  /**
+   * @brief Map scale (m/cell)
+   */
   double scale;
 
+  /**
+   * @brief Construct a Map from a nav_msgs::OccupancyGrid
+   */
   explicit Map(const nav_msgs::OccupancyGrid & msg);
+
+  /**
+   * @brief Convert a Map to a nav_msgs::OccupancyGrid
+   */
   explicit operator nav_msgs::OccupancyGrid() const;
 
+  /**
+   * @brief Convert world coordinates to map indices
+   */
   std::pair<Eigen::Index, Eigen::Index> world2map(const tf2::Vector3 & v) const;
 };
 
+/**
+ * @brief A 2-D grid map, in which each cell has a occupance state
+ */
 struct OccupancyMap : Map
 {
-  // Occupancy state (-1 = free, 0 = unknown, +1 = occ)
+  /**
+   * @brief A cell contains the occupancy state (-1 = free, 0 = unknown, +1 = occ)
+   */
   using CellType = int8_t;
 
-  // rows = x, cols = y
   using CellsType = Eigen::Matrix<CellType, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
 
   CellsType cells;
 
+  /**
+   * @brief Construct a Map from a nav_msgs::OccupancyGrid
+   */
   explicit OccupancyMap(const nav_msgs::OccupancyGrid & msg);
 
+  /**
+   * @brief Check if the cells in the line between v1 and v2 are occupied
+   * @param v1 from
+   * @param v2 to
+   * @return if the range is occupied
+   */
   double calc_range(const tf2::Vector3 & v1, const tf2::Vector3 & v2) const;
+
+  /**
+   * @brief Check if the cells in the line between x0,y0 and x1,y1 are occupied
+   * @return if the range is occupied
+   */
   double calc_range(Eigen::Index x0, Eigen::Index y0, Eigen::Index x1, Eigen::Index y1) const;
+
+  /**
+   * @brief Check if the indices are in bounds of the map
+   */
   bool is_valid(Eigen::Index i, Eigen::Index j) const;
 };
 
+/**
+ * @brief A 2-D grid map, in which each cell has the distance to the nearest obstacle
+ */
 struct DistanceMap : Map
 {
-  // distance to the nearest obstacle
+  /**
+   * @brief A cell contains the distance to the nearest obstacle
+   */
   using CellType = double;
 
-  // rows = x, cols = y
   using CellsType = Eigen::Matrix<CellType, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
 
   CellsType cells;
   ros::Publisher debug_pub_;
 
+  /**
+   * @brief Construct a DistanceMap from a nav_msgs::OccupancyGrid
+   */
   explicit DistanceMap(const nav_msgs::OccupancyGrid & msg);
+
+  /**
+   * @brief Convert a DistanceMap to a nav_msgs::OccupancyGrid
+   */
   explicit operator nav_msgs::OccupancyGrid() const;
 
+  /**
+   * @brief Get the distance to the closest obstacle from a given point
+   */
   double closest_obstacle(const tf2::Vector3 & v) const;
+
+  /**
+   * @brief Check if the indices are in bounds of the map
+   */
   bool is_valid(Eigen::Index i, Eigen::Index j) const;  // TODO(Ramon): move to baseclass
 };
 }  // namespace ruvu_mcl

ruvu_mcl/src/mcl.hpp

@@ -43,11 +43,17 @@ class ParticleFilter;
 class Resampler;
 class Rng;
 
+/**
+ * @brief Main class for monte-carlo localization.
+ *
+ * This class takes in measurments and outputs the position estimate of the robot. It can be
+ * configured with different sensor and motion models.
+ */
 class Mcl
 {
 public:
   Mcl();
-  Mcl(std::uint_fast32_t seed);
+  Mcl(uint_fast32_t seed);
   ~Mcl();  // to handle forward declares
 
   void configure(const Config & config);

ruvu_mcl/src/mcl_ros.hpp

@@ -38,6 +38,12 @@ namespace ruvu_mcl
 {
 class AMCLConfig;
 
+/**
+ * @brief ROS wrapper for the Mcl class
+ *
+ * This class is a ROS wrapper for the Mcl class. It receives ROS messages, converts then and sends
+ * then trough. It also publishes the output of Mcl to /tf and ~pose.
+ */
 class MclRos
 {
 public:
@@ -46,7 +52,7 @@ class MclRos
     const std::shared_ptr<const tf2_ros::Buffer> & buffer);
   MclRos(
     ros::NodeHandle nh, ros::NodeHandle private_nh,
-    const std::shared_ptr<const tf2_ros::Buffer> & buffer, std::uint_fast32_t seed);
+    const std::shared_ptr<const tf2_ros::Buffer> & buffer, uint_fast32_t seed);
   ~MclRos();  // to handle forward declares
 
   void configure(const ruvu_mcl::AMCLConfig & config);

ruvu_mcl/src/motion_models/differential_motion_model.hpp

@@ -13,11 +13,14 @@ namespace ruvu_mcl
 // forward declare
 class Rng;
 
+/**
+ * @brief Motion model for a differential drive
+ */
 class DifferentialMotionModel : public MotionModel
 {
 public:
   /**
-   * @brief A diff drive constructor
+   * @brief Construct a differental drive motion model
    * @param alpha1 error parameters, see documentation
    * @param alpha2 error parameters, see documentation
    * @param alpha3 error parameters, see documentation
@@ -26,16 +29,22 @@ class DifferentialMotionModel : public MotionModel
    */
   DifferentialMotionModel(
     double alpha1, double alpha2, double alpha3, double alpha4, const std::shared_ptr<Rng> & rng);
-  DifferentialMotionModel(
-    const DifferentialMotionModelConfig & config, const std::shared_ptr<Rng> & rng);
 
   /**
-   * @brief Update on new odometry data
-   * @param pf The particle filter to update
-   * @param delta change in pose in odometry update
+   * @brief Construct a differental drive motion model
+   * @param config parameters
+   * @param rng random number generator
    */
+  DifferentialMotionModel(
+    const DifferentialMotionModelConfig & config, const std::shared_ptr<Rng> & rng);
+
   void odometry_update(ParticleFilter * pf, const tf2::Transform & delta) override;
 
+  /**
+   * @brief Compute {delta_rot1, delta_trans, delta_rot2} from a displacement
+   * @param delta displacement
+   * @return {delta_rot1, delta_trans, delta_rot2}
+   */
   static std::array<double, 3> calculate_deltas(const tf2::Transform & delta);
 
 private:

ruvu_mcl/src/motion_models/motion_model.hpp

@@ -12,13 +12,22 @@ namespace ruvu_mcl
 {
 class ParticleFilter;
 
+/**
+ * @brief Base class for all motion models
+ *
+ * Base class for all motion models. A motion model tries to accurately model the specific types of
+ * uncertainty that exist in robot actuation.
+ *
+ * Motion models take odometry measurements as input and move the particles according to a specific
+ * probabalistic distribution. See also chapter 5 of Probabilistc Robotics.
+ */
 class MotionModel
 {
 public:
   virtual ~MotionModel();
 
   /**
-   * @brief Update on new odometry data
+   * @brief Update particles based on new odometry data
    * @param pf The particle filter to update
    * @param delta change in pose in odometry update
    */

ruvu_mcl/src/node.hpp

@@ -26,6 +26,9 @@ ROS_DECLARE_MESSAGE(LaserScan)
 
 namespace ruvu_mcl
 {
+/**
+ * @brief Main ros node of the ruvu_mcl package
+ */
 class Node
 {
 public:

ruvu_mcl/src/offline.cpp

@@ -25,6 +25,9 @@ using ruvu_mcl::MclRos;
 
 constexpr auto name = "offline";
 
+/**
+ * @brief Executable for offline playback of the particle filter on ros bagfiles
+ */
 int main(int argc, char ** argv)
 {
   if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) {

ruvu_mcl/src/offline/bag_buffer.hpp

@@ -11,6 +11,9 @@ class Bag;
 
 namespace ruvu_mcl
 {
+/**
+ * @brief Load all tf messages from a rosbag into a tf2_ros::Buffer
+ */
 class BagBuffer : public tf2_ros::Buffer
 {
 public: