Fix findWayPointIndicesForDurationAfterStart method when out of bounds (#3626)

* Fix findWayPointIndicesForDurationAfterStart method when out of bounds and add unit tests

* Use .at() to index deque; Add 2 more edge test cases

* Revise function comments

---------

Co-authored-by: Nathan Brooks <[email protected]>
Co-authored-by: Ezra Brooks <[email protected]>

Author: 3 people

moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.hpp

@@ -310,6 +310,16 @@ class RobotTrajectory
    *  @param The waypoint index before the supplied duration.
    *  @param The waypoint index after (or equal to) the supplied duration.
    *  @param The progress (0 to 1) between the two waypoints, based on time (not based on joint distances).
+   *
+   *  General Case:
+   *  - Usually after == before + 1, except in the edge cases listed below.
+   *  - blend 0.0 implies being exactly at the before waypoint.
+   *  - blend 1.0 implies being exactly at the after waypoint.
+   *  Edge Cases:
+   *  - Empty Trajectory: before == 0, after == 0, blend == 0.0
+   *  - Negative Duration: before == 0, after == 0, blend == 0.0
+   *  - Duration > Total Duration: before == size() - 1, after == size() - 1, blend == 1.0
+   *  - Single Waypoint Trajectory: before == 0, after == 0, blend == 1.0
    */
   void findWayPointIndicesForDurationAfterStart(double duration, int& before, int& after, double& blend) const;
 

moveit_core/robot_trajectory/src/robot_trajectory.cpp

@@ -568,7 +568,7 @@ RobotTrajectory& RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::Robo
 void RobotTrajectory::findWayPointIndicesForDurationAfterStart(double duration, int& before, int& after,
                                                                double& blend) const
 {
-  if (duration < 0.0)
+  if (duration < 0.0 || waypoints_.empty())
   {
     before = 0;
     after = 0;
@@ -581,22 +581,22 @@ void RobotTrajectory::findWayPointIndicesForDurationAfterStart(double duration,
   double running_duration = 0.0;
   for (; index < num_points; ++index)
   {
-    running_duration += duration_from_previous_[index];
+    running_duration += duration_from_previous_.at(index);
     if (running_duration >= duration)
       break;
   }
   before = std::max<int>(index - 1, 0);
   after = std::min<int>(index, num_points - 1);
 
   // Compute duration blend
-  double before_time = running_duration - duration_from_previous_[index];
   if (after == before)
   {
     blend = 1.0;
   }
   else
   {
-    blend = (duration - before_time) / duration_from_previous_[index];
+    double before_time = running_duration - duration_from_previous_.at(index);
+    blend = (duration - before_time) / duration_from_previous_.at(index);
   }
 }
 

moveit_core/robot_trajectory/test/test_robot_trajectory.cpp

@@ -599,6 +599,111 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDensity)
   EXPECT_FALSE(density.has_value());
 }
 
+TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryFindWayPointIndicesBetweenWaypoints)
+{
+  robot_trajectory::RobotTrajectoryPtr trajectory;
+  initTestTrajectory(trajectory);
+  EXPECT_EQ(trajectory->size(), 5);
+  EXPECT_EQ(trajectory->getDuration(), 0.5);
+
+  int before = -1;
+  int after = -1;
+  double blend = -1.0;
+
+  EXPECT_NO_THROW(trajectory->findWayPointIndicesForDurationAfterStart(0.15, before, after, blend));
+  EXPECT_EQ(before, 0);
+  EXPECT_EQ(after, 1);
+  EXPECT_NEAR(blend, /*between 0 and 1*/ 0.5, 1e-6);
+
+  EXPECT_NO_THROW(trajectory->findWayPointIndicesForDurationAfterStart(0.3, before, after, blend));
+  EXPECT_EQ(before, 1);
+  EXPECT_EQ(after, 2);
+  EXPECT_NEAR(blend, /*exactly at 2*/ 1.0, 1e-6);
+}
+
+TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryFindWayPointIndicesAtLastOfManyWaypoints)
+{
+  robot_trajectory::RobotTrajectoryPtr trajectory;
+  initTestTrajectory(trajectory);
+
+  int before = -1;
+  int after = -1;
+  double blend = -1.0;
+
+  const double total_duration = trajectory->getDuration();
+  EXPECT_NO_THROW(trajectory->findWayPointIndicesForDurationAfterStart(total_duration, before, after, blend));
+  EXPECT_EQ(before, 3);
+  EXPECT_EQ(after, 4);
+  EXPECT_DOUBLE_EQ(blend, 1.0);
+}
+
+TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryFindWayPointIndicesAfterLastWaypoint)
+{
+  robot_trajectory::RobotTrajectoryPtr trajectory;
+  initTestTrajectory(trajectory);
+
+  const double total_duration = trajectory->getDuration();
+  const double outbound_duration = total_duration + 100.0;
+  EXPECT_GT(outbound_duration, total_duration);
+
+  int before = -1;
+  int after = -1;
+  double blend = -1.0;
+  EXPECT_NO_THROW(trajectory->findWayPointIndicesForDurationAfterStart(outbound_duration, before, after, blend));
+  EXPECT_EQ(before, 4);
+  EXPECT_EQ(after, 4);
+  EXPECT_DOUBLE_EQ(blend, 1.0);
+}
+
+TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryFindWayPointIndicesEmptyWaypoints)
+{
+  robot_trajectory::RobotTrajectory empty_traj(robot_model_, arm_jmg_name_);
+  const double total_duration = empty_traj.getDuration();
+  EXPECT_DOUBLE_EQ(total_duration, 0.0);
+
+  const double outbound_duration = 1.0;
+  EXPECT_GT(outbound_duration, total_duration);
+
+  int before = -1;
+  int after = -1;
+  double blend = -1.0;
+  EXPECT_NO_THROW(empty_traj.findWayPointIndicesForDurationAfterStart(outbound_duration, before, after, blend));
+  EXPECT_EQ(before, 0);
+  EXPECT_EQ(after, 0);
+  EXPECT_DOUBLE_EQ(blend, 0.0);
+}
+
+TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryFindWayPointIndicesBeforeFirstWaypoint)
+{
+  robot_trajectory::RobotTrajectoryPtr trajectory;
+  initTestTrajectory(trajectory);
+
+  int before = -1;
+  int after = -1;
+  double blend = -1.0;
+
+  EXPECT_NO_THROW(trajectory->findWayPointIndicesForDurationAfterStart(-0.1, before, after, blend));
+  EXPECT_EQ(before, 0);
+  EXPECT_EQ(after, 0);
+  EXPECT_DOUBLE_EQ(blend, 0.0);
+}
+
+TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryFindWayPointIndicesAtLastOfSingleWaypoint)
+{
+  robot_trajectory::RobotTrajectory trajectory(robot_model_, arm_jmg_name_);
+  trajectory.addSuffixWayPoint(robot_state_, 0.0);
+
+  int before = -1;
+  int after = -1;
+  double blend = -1.0;
+
+  const double total_duration = trajectory.getDuration();
+  EXPECT_NO_THROW(trajectory.findWayPointIndicesForDurationAfterStart(total_duration, before, after, blend));
+  EXPECT_EQ(before, 0);
+  EXPECT_EQ(after, 0);
+  EXPECT_DOUBLE_EQ(blend, 1.0);
+}
+
 TEST_F(OneRobot, Unwind)
 {
   const double epsilon = 1e-4;