Off by one in getAverageSegmentDuration (#1079)

* Off by one in getAverageSegmentDuration

* Case for one waypoint

Co-authored-by: AndyZe <andyz@utexas.edu>

* Warn if too few waypoints to get duration

Co-authored-by: AndyZe <andyz@utexas.edu>

* Discount first duration_from_previous from average duration if it is 0

* Restore empty duration from previous check as per Andy's suggestion

* Changed warning message for case with 1 segment with 0 duration to be distinct from empty durations

Co-authored-by: AndyZe <andyz@utexas.edu>
Co-authored-by: Henning Kayser <henningkayser@picknik.ai>
Co-authored-by: AndyZe <zelenak@picknik.ai>

moveit_core/robot_trajectory/src/robot_trajectory.cpp

@@ -92,7 +92,22 @@ double RobotTrajectory::getDuration() const
 double RobotTrajectory::getAverageSegmentDuration() const
 {
   if (duration_from_previous_.empty())
+  {
+    RCLCPP_WARN(rclcpp::get_logger("RobotTrajectory"), "Too few waypoints to calculate a duration. Returning 0.");
     return 0.0;
+  }
+
+  // If the initial segment has a duration of 0, exclude it from the average calculation
+  if (duration_from_previous_[0] == 0)
+  {
+    if (duration_from_previous_.size() <= 1)
+    {
+      RCLCPP_WARN(rclcpp::get_logger("RobotTrajectory"), "First and only waypoint has a duration of 0.");
+      return 0.0;
+    }
+    else
+      return getDuration() / static_cast<double>(duration_from_previous_.size() - 1);
+  }
   else
     return getDuration() / static_cast<double>(duration_from_previous_.size());
 }