Fix createTrajectoryMessage (#3064)

pac48 · web-flow · commit fe3a0907779b · 2024-11-06T09:37:23.000Z
* Fix createTrajectoryMessage

Signed-off-by: Paul Gesel &lt;paul.gesel@picknik.ai&gt;

* Use std::ceil to handle non-aligned sampling_rate

Signed-off-by: Paul Gesel &lt;paul.gesel@picknik.ai&gt;

---------

Signed-off-by: Paul Gesel &lt;paul.gesel@picknik.ai&gt;
diff --git a/moveit_core/trajectory_processing/src/trajectory_tools.cpp b/moveit_core/trajectory_processing/src/trajectory_tools.cpp
@@ -87,7 +87,7 @@ 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)
   {

Original file line number	Diff line number	Diff line change
`@@ -87,7 +87,7 @@ trajectory_msgs::msg::JointTrajectory createTrajectoryMessage(const std::vector<`
`87`	`87`	`}`
`88`	`88`	`trajectory_msg.joint_names = joint_names;`
`89`	`89`	`const double time_step = 1.0 / static_cast<double>(sampling_rate);`
`90`		`- const int n_samples = static_cast<int>(trajectory.getDuration() / time_step) + 1;`
	`90`	`+ const int n_samples = static_cast<int>(std::ceil(trajectory.getDuration() / time_step)) + 1;`
`91`	`91`	`trajectory_msg.points.reserve(n_samples);`
`92`	`92`	`for (int sample = 0; sample < n_samples; ++sample)`
`93`	`93`	`{`