Added PathComponentsIterator to iterate over PathComponents easily and tracks state

Author: SteveMacenski

README.md

@@ -32,7 +32,7 @@ Finally, it contains the polygon information. This can be represented either as
 
 When setting the polygon (`goal.polygons`), this is a vector of polygons. If only considering a bounding field, only populate the first field shape. If there are internal voids, use subsequent polygons to indicate them. The coordinate type has `axis1` and `axis2` instead of X and Y as the server can process both GPS and cartesian coordinates. If specifying the polygon outside of GML files, you must specify the frame of reference of the polygon using the `goal.frame_id` field. This is not used for GML files as those should contain the frame within it.
 
-The result returns a `result.nav_path` -- which is a `nav_msgs/Path` containing the coverage path requested **only if** all `generate_path` is `true`. This can be followed by a local trajectory planner or controller directly. This is what is used in the `opennav_coverage_bt` examples for basic coverage navigation. It also returns `result.coverage_path` which contains an ordered set of swaths and paths to connect them (if applicable settings enabled) which can be used for more task-specific navigation. For example, navigating with a tool down or enabled on swaths and raised in turns to connect to other swaths. A utility is provided in `opennav_coverage/utils.hpp` for iterating through this custom `coverage_path` for convenience.
+The result returns a `result.nav_path` -- which is a `nav_msgs/Path` containing the coverage path requested **only if** all `generate_path` is `true`. This can be followed by a local trajectory planner or controller directly. This is what is used in the `opennav_coverage_bt` examples for basic coverage navigation. It also returns `result.coverage_path` which contains an ordered set of swaths and paths to connect them (if applicable settings enabled) which can be used for more task-specific navigation. For example, navigating with a tool down or enabled on swaths and raised in turns to connect to other swaths. A utility is provided in `opennav_coverage/utils.hpp` for iterating through this custom `coverage_path` for convenience, `PathComponentsIterator`.
 
 It also returns an error code, if any error occurred and the total planning time for metrics analysis.
 

opennav_coverage/include/opennav_coverage/utils.hpp

@@ -19,6 +19,7 @@
 #include <string>
 #include <algorithm>
 #include <memory>
+#include <utility>
 
 #include "fields2cover.h" // NOLINT
 #include "rclcpp/rclcpp.hpp"
@@ -276,6 +277,82 @@ inline void toUpper(std::string & string)
   std::transform(string.begin(), string.end(), string.begin(), ::toupper);
 }
 
+/**
+ * @brief A Path Components iterator object to get next turn/swath from action return
+ *
+ * Example Use:
+    for (opennav_coverage::util::PathComponentsIterator it(msg); it.isValid(); it.advance()) {
+      auto curr_row_info = it.getNext();
+
+      // Always should be valid
+      (void)std::get<0>(curr_row_info)->start;
+
+      if (std::get<1>(curr_row_info)) {
+        // Always should be before last
+        (void)std::get<1>(curr_row_info)->poses;
+      }
+    }
+
+    auto last_row_info = it.getNext();
+    ASSERT(std::get<1>(last_row_info) == nullptr);
+ */
+class PathComponentsIterator
+{
+public:
+  /**
+   * @brief A Path Components iterator constructor
+   * @param PathComponents object to iterate over
+   */
+  explicit PathComponentsIterator(opennav_coverage_msgs::msg::PathComponents & msg)
+  : path_components_(msg), idx_(0)
+  {
+    if (fabs(path_components_.swaths.size() - path_components_.turns.size()) > 1) {
+      throw std::runtime_error("PathComponents size not valid for iteration!");
+    } else if (!path_components_.contains_turns) {
+      throw std::runtime_error("PathComponents cannot be iterated over without turns!");
+    } else if (!path_components_.swaths_ordered) {
+      throw std::runtime_error("PathComponents cannot be iterated over without ordered swaths!");
+    }
+
+    max_idx_ = path_components_.swaths.size();
+  }
+
+  /**
+   * @brief For condition if still valid to continue iterating
+   */
+  bool isValid()
+  {
+    return idx_ <= max_idx_;
+  }
+
+  /**
+   * @brief For loop marching
+   */
+  void advance()
+  {
+    idx_++;
+  }
+
+  /**
+   * @brief Get the data of the current iteration
+   * @return returns a pair of pointers to the current swath and its next turn
+   * If at the end, the turn is nullptr, so be sure to check it!
+   */
+  std::pair<opennav_coverage_msgs::msg::Swath *, nav_msgs::msg::Path *>
+  getNext()
+  {
+    if (idx_ < max_idx_ - 1) {
+      return std::make_pair(&path_components_.swaths[idx_], &path_components_.turns[idx_]);
+    } else {
+      return std::make_pair(&path_components_.swaths[idx_], nullptr);
+    }
+  }
+
+  opennav_coverage_msgs::msg::PathComponents & path_components_;
+  unsigned int idx_;
+  unsigned int max_idx_;
+};
+
 }  // namespace util
 
 }  // namespace opennav_coverage

opennav_coverage/test/test_utils.cpp

@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <utility>
+
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
 #include "opennav_coverage/utils.hpp"
@@ -193,4 +195,43 @@ TEST(UtilsTests, TestgetFieldFromGoal)
   EXPECT_EQ(field2.field.getGeometry(0).getGeometry(1).size(), 3u);
 }
 
+TEST(UtilsTests, TestPathComponentsIterator)
+{
+  opennav_coverage_msgs::msg::PathComponents msg;
+
+  // Sizes don't match
+  msg.swaths.resize(4);
+  EXPECT_THROW(opennav_coverage::util::PathComponentsIterator it(msg), std::runtime_error);
+
+  // Isn't properly filled in to contain turns
+  msg.turns.resize(4);
+  EXPECT_THROW(opennav_coverage::util::PathComponentsIterator it(msg), std::runtime_error);
+
+  // Isn't properly filled in to be ordered
+  msg.contains_turns = true;
+  EXPECT_THROW(opennav_coverage::util::PathComponentsIterator it(msg), std::runtime_error);
+
+  // Now should work
+  msg.swaths_ordered = true;
+  EXPECT_NO_THROW(opennav_coverage::util::PathComponentsIterator it(msg));
+
+  unsigned int i = 0;
+  opennav_coverage::util::PathComponentsIterator it(msg);
+  for (; it.isValid(); it.advance()) {
+    auto curr_row_info = it.getNext();
+
+    // Always should be valid
+    (void)std::get<0>(curr_row_info)->start;
+
+    if (i < 3) {
+      // Always should be before last
+      (void)std::get<1>(curr_row_info)->poses;
+    }
+    i++;
+  }
+
+  auto last_row_info = it.getNext();
+  EXPECT_EQ(std::get<1>(last_row_info), nullptr);
+}
+
 }  // namespace opennav_coverage