Off by one in getAverageSegmentDuration (#1079) (#1136)

* Off by one in getAverageSegmentDuration * Case for one waypoint Co-authored-by: AndyZe <[email protected]> * Warn if too few waypoints to get duration Co-authored-by: AndyZe <[email protected]> * 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 <[email protected]> Co-authored-by: Henning Kayser <[email protected]> Co-authored-by: AndyZe <[email protected]> Co-authored-by: Stephanie Eng <[email protected]> Co-authored-by: Henning Kayser <[email protected]>
moveit · Mar 24, 2022 · a860a95 · a860a95
1 parent 08cf78b
commit a860a95
Showing 1 changed file with 15 additions and 0 deletions.
diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/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());
 }