Use std::ceil to handle non-aligned sampling_rate

Signed-off-by: Paul Gesel <[email protected]>
PickNikRobotics · Nov 5, 2024 · e94d125 · e94d125
1 parent 826bf4d
commit e94d125
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/moveit_core/trajectory_processing/src/trajectory_tools.cpp b/moveit_core/trajectory_processing/src/trajectory_tools.cpp
@@ -87,9 +87,9 @@ trajectory_msgs::msg::JointTrajectory createTrajectoryMessage(const std::vector<
   }
   trajectory_msg.joint_names = joint_names;
   const double time_step = 1.0 / static_cast<double>(sampling_rate);
-  const int n_samples = static_cast<int>(trajectory.getDuration() / time_step) + 1;
+  const int n_samples = static_cast<int>(std::ceil(trajectory.getDuration() / time_step)) + 1;
   trajectory_msg.points.reserve(n_samples);
-  for (int sample = 0; sample <= n_samples; ++sample)
+  for (int sample = 0; sample < n_samples; ++sample)
   {
     const double t = sample * time_step;
     trajectory_msgs::msg::JointTrajectoryPoint point;