From ac9f4b6281b81b3a90fc3901f0bc43284a4362a8 Mon Sep 17 00:00:00 2001 From: Stephanie Eng Date: Mon, 14 Mar 2022 11:28:24 -0400 Subject: [PATCH] Off by one in getAverageSegmentDuration (#1079) * Off by one in getAverageSegmentDuration * Case for one waypoint Co-authored-by: AndyZe * Warn if too few waypoints to get duration Co-authored-by: AndyZe * 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 Co-authored-by: Henning Kayser Co-authored-by: AndyZe --- .../robot_trajectory/src/robot_trajectory.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index bfe2dae702..794ea9d78a 100644 --- 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(duration_from_previous_.size() - 1); + } else return getDuration() / static_cast(duration_from_previous_.size()); }