RobotTrajectory as standard container (#720)

* Based on initial size/iterator implementations from moveit/moveit#1162

moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h

@@ -102,6 +102,12 @@ class RobotTrajectory
     return waypoints_.size();
   }
 
+  std::size_t size() const
+  {
+    assert(waypoints_.size() == duration_from_previous_.size());
+    return waypoints_.size();
+  }
+
   const moveit::core::RobotState& getWayPoint(std::size_t index) const
   {
     return *waypoints_[index];
@@ -295,6 +301,60 @@ class RobotTrajectory
    */
   bool getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr& output_state) const;
 
+  class Iterator
+  {
+    std::deque<moveit::core::RobotStatePtr>::iterator waypoint_iterator;
+    std::deque<double>::iterator duration_iterator;
+
+  public:
+    explicit Iterator(std::deque<moveit::core::RobotStatePtr>::iterator _waypoint_iterator,
+                      std::deque<double>::iterator _duration_iterator)
+      : waypoint_iterator(_waypoint_iterator), duration_iterator(_duration_iterator)
+    {
+    }
+    Iterator& operator++()
+    {
+      waypoint_iterator++;
+      duration_iterator++;
+      return *this;
+    }
+    Iterator operator++(int)
+    {
+      Iterator retval = *this;
+      ++(*this);
+      return retval;
+    }
+    bool operator==(Iterator other) const
+    {
+      return ((waypoint_iterator == other.waypoint_iterator) && (duration_iterator == other.duration_iterator));
+    }
+    bool operator!=(Iterator other) const
+    {
+      return !(*this == other);
+    }
+    std::pair<moveit::core::RobotStatePtr, double> operator*() const
+    {
+      return std::pair{ *waypoint_iterator, *duration_iterator };
+    }
+
+    // iterator traits
+    using difference_type = long;
+    using value_type = std::pair<moveit::core::RobotStatePtr, double>;
+    using pointer = const std::pair<moveit::core::RobotStatePtr, double>*;
+    using reference = std::pair<moveit::core::RobotStatePtr, double>;
+    using iterator_category = std::input_iterator_tag;
+  };
+
+  RobotTrajectory::Iterator begin()
+  {
+    assert(waypoints_.size() == duration_from_previous_.size());
+    return Iterator(waypoints_.begin(), duration_from_previous_.begin());
+  }
+  RobotTrajectory::Iterator end()
+  {
+    assert(waypoints_.size() == duration_from_previous_.size());
+    return Iterator(waypoints_.end(), duration_from_previous_.end());
+  }
   /** @brief Print information about the trajectory
    * @param out              Stream to print to
    * @param variable_indexes The indexes of the variables to print.

moveit_core/robot_trajectory/test/test_robot_trajectory.cpp

@@ -83,6 +83,7 @@ class RobotTrajectoryTestFixture : public testing::Test
         << "Generated trajectory duration incorrect";
     EXPECT_EQ(waypoint_count, trajectory->getWayPointDurations().size())
         << "Generated trajectory has the wrong number of waypoints";
+    EXPECT_EQ(waypoint_count, trajectory->size());
   }
 
   void copyTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory,
@@ -242,6 +243,50 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDeepCopy)
   EXPECT_NE(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]);
 }
 
+TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryIterator)
+{
+  robot_trajectory::RobotTrajectoryPtr trajectory;
+  initTestTrajectory(trajectory);
+
+  ASSERT_EQ(5u, trajectory->size());
+  std::vector<double> positions;
+
+  double start_pos = 0.0;
+
+  for (size_t i = 0; i < trajectory->size(); i++)
+  {
+    auto waypoint = trajectory->getWayPointPtr(i);
+    // modify joint values
+    waypoint->copyJointGroupPositions(arm_jmg_name_, positions);
+    start_pos = positions[0];
+    positions[0] += 0.01 * i;
+    waypoint->setJointGroupPositions(arm_jmg_name_, positions);
+  }
+
+  unsigned int count = 0;
+  for (const auto& waypoint_and_duration : *trajectory)
+  {
+    const auto& waypoint = waypoint_and_duration.first;
+    waypoint->copyJointGroupPositions(arm_jmg_name_, positions);
+    EXPECT_EQ(start_pos + count * 0.01, positions[0]);
+    count++;
+  }
+
+  EXPECT_EQ(count, trajectory->size());
+
+  // Consistency checks
+  EXPECT_EQ(trajectory->begin(), trajectory->begin());
+  EXPECT_EQ(trajectory->end(), trajectory->end());
+
+  // trajectory has length 5; incrementing begin 5 times should reach the end
+  EXPECT_NE(trajectory->begin(), trajectory->end());
+  EXPECT_NE(++trajectory->begin(), trajectory->end());
+  EXPECT_NE(++(++trajectory->begin()), trajectory->end());
+  EXPECT_NE(++(++(++trajectory->begin())), trajectory->end());
+  EXPECT_NE(++(++(++(++trajectory->begin()))), trajectory->end());
+  EXPECT_EQ(++(++(++(++(++trajectory->begin())))), trajectory->end());
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);