Merge branch 'main' into andyz/ogre_build_error

moveit · Mar 14, 2022 · be33e82 · be33e82
2 parents 20baf33 + d5b2d26
commit be33e82
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());
 }