moveit2
The MoveIt Motion Planning Framework for ROS 2.
test_trajectory_cache.cpp
Go to the documentation of this file.
1 // Copyright 2024 Intrinsic Innovation LLC.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
19 #include <rclcpp/rclcpp.hpp>
20 
21 #include <moveit/robot_state/conversions.h>
22 #include <moveit/robot_state/robot_state.h>
23 #include <moveit/trajectory_cache/trajectory_cache.hpp>
24 
25 #include <atomic>
26 #include <thread>
27 
28 using moveit::planning_interface::MoveGroupInterface;
29 using moveit_ros::trajectory_cache::TrajectoryCache;
30 
31 const std::string ROBOT_NAME = "panda";
32 const std::string ROBOT_FRAME = "world";
33 
34 // UTILS =======================================================================
35 // Utility function to emit a pass or fail statement.
36 void checkAndEmit(bool predicate, const std::string& test_case, const std::string& label)
37 {
38  if (predicate)
39  {
40  std::cout << "[PASS] " << test_case << ": " << label << "\n";
41  }
42  else
43  {
44  std::cout << "[FAIL] " << test_case << ": " << label << "\n";
45  }
46 }
47 
48 moveit_msgs::msg::RobotTrajectory getDummyPandaTraj()
49 {
50  static moveit_msgs::msg::RobotTrajectory out = []() {
51  moveit_msgs::msg::RobotTrajectory traj;
52 
53  auto trajectory = &traj.joint_trajectory;
54  trajectory->header.frame_id = ROBOT_FRAME;
55 
56  trajectory->joint_names.push_back(ROBOT_NAME + "_joint1");
57  trajectory->joint_names.push_back(ROBOT_NAME + "_joint2");
58  trajectory->joint_names.push_back(ROBOT_NAME + "_joint3");
59  trajectory->joint_names.push_back(ROBOT_NAME + "_joint4");
60  trajectory->joint_names.push_back(ROBOT_NAME + "_joint5");
61  trajectory->joint_names.push_back(ROBOT_NAME + "_joint6");
62  trajectory->joint_names.push_back(ROBOT_NAME + "_joint7");
63 
64  trajectory->points.emplace_back();
65  trajectory->points.at(0).positions = { 0, 0, 0, 0, 0, 0 };
66  trajectory->points.at(0).velocities = { 0, 0, 0, 0, 0, 0 };
67  trajectory->points.at(0).accelerations = { 0, 0, 0, 0, 0, 0 };
68  trajectory->points.at(0).time_from_start.sec = 999999;
69 
70  return traj;
71  }();
72  return out;
73 }
74 
75 std::vector<geometry_msgs::msg::Pose> getDummyWaypoints()
76 {
77  static std::vector<geometry_msgs::msg::Pose> out = []() {
78  std::vector<geometry_msgs::msg::Pose> waypoints;
79  for (size_t i = 0; i < 3; i++)
80  {
81  waypoints.emplace_back();
82  waypoints.at(i).position.x = i;
83  waypoints.at(i).position.y = i;
84  waypoints.at(i).position.z = i;
85  waypoints.at(i).orientation.w = i;
86  waypoints.at(i).orientation.x = i + 0.1;
87  waypoints.at(i).orientation.y = i + 0.1;
88  waypoints.at(i).orientation.z = i + 0.1;
89  }
90  return waypoints;
91  }();
92  return out;
93 }
94 
95 void testGettersAndSetters(const std::shared_ptr<TrajectoryCache>& cache)
96 {
97  std::string test_case = "getters_and_setters";
98 
99  checkAndEmit(cache->getDbPath() == ":memory:", test_case, "getDbPath");
100  checkAndEmit(cache->getDbPort() == 0, test_case, "getDbPort");
101 
102  checkAndEmit(cache->getExactMatchPrecision() == 10, test_case, "getExactMatchPrecision");
103  cache->setExactMatchPrecision(1);
104  checkAndEmit(cache->getExactMatchPrecision() == 1, test_case, "getExactMatchPrecisionAfterSet");
105 
106  checkAndEmit(cache->getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() == 10, test_case,
107  "getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse");
108  cache->setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse(1);
109  checkAndEmit(cache->getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() == 1, test_case,
110  "getNumAdditionalTrajectoriesToPreserveWhenDeletingWorseAfterSet");
111 }
112 
113 void testMotionTrajectories(const std::shared_ptr<MoveGroupInterface>& move_group,
114  const std::shared_ptr<TrajectoryCache>& cache)
115 {
116  // Setup =====================================================================
117  // All variants are modified copies of `plan_req`.
118 
120 
121  // Plain start
122  moveit_msgs::msg::MotionPlanRequest plan_req;
123  move_group->constructMotionPlanRequest(plan_req);
124  plan_req.workspace_parameters.header.frame_id = ROBOT_FRAME;
125  plan_req.workspace_parameters.max_corner.x = 10;
126  plan_req.workspace_parameters.max_corner.y = 10;
127  plan_req.workspace_parameters.max_corner.z = 10;
128  plan_req.workspace_parameters.min_corner.x = -10;
129  plan_req.workspace_parameters.min_corner.y = -10;
130  plan_req.workspace_parameters.min_corner.z = -10;
131  plan_req.start_state.multi_dof_joint_state.joint_names.clear();
132  plan_req.start_state.multi_dof_joint_state.transforms.clear();
133  plan_req.start_state.multi_dof_joint_state.twist.clear();
134  plan_req.start_state.multi_dof_joint_state.wrench.clear();
135 
136  // Empty frame start
137  moveit_msgs::msg::MotionPlanRequest empty_frame_plan_req = plan_req;
138  empty_frame_plan_req.workspace_parameters.header.frame_id = "";
139 
140  // is_diff = true
141  auto is_diff_plan_req = plan_req;
142  is_diff_plan_req.start_state.is_diff = true;
143  is_diff_plan_req.start_state.joint_state.header.frame_id = "";
144  is_diff_plan_req.start_state.joint_state.name.clear();
145  is_diff_plan_req.start_state.joint_state.position.clear();
146  is_diff_plan_req.start_state.joint_state.velocity.clear();
147  is_diff_plan_req.start_state.joint_state.effort.clear();
148 
149  // Something close enough (mod 0.1 away)
150  auto close_matching_plan_req = plan_req;
151  close_matching_plan_req.workspace_parameters.max_corner.x += 0.05;
152  close_matching_plan_req.workspace_parameters.min_corner.x -= 0.05;
153  close_matching_plan_req.start_state.joint_state.position.at(0) -= 0.05;
154  close_matching_plan_req.start_state.joint_state.position.at(1) += 0.05;
155  close_matching_plan_req.start_state.joint_state.position.at(2) -= 0.05;
156  close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(0).position -= 0.05;
157  close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(1).position += 0.05;
158  close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(2).position -= 0.05;
159 
160  // Close with swapped constraints (mod 0.1 away)
161  auto swapped_close_matching_plan_req = close_matching_plan_req;
162  std::swap(swapped_close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(0),
163  swapped_close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(1));
164 
165  // Smaller workspace start
166  auto smaller_workspace_plan_req = plan_req;
167  smaller_workspace_plan_req.workspace_parameters.max_corner.x = 1;
168  smaller_workspace_plan_req.workspace_parameters.max_corner.y = 1;
169  smaller_workspace_plan_req.workspace_parameters.max_corner.z = 1;
170  smaller_workspace_plan_req.workspace_parameters.min_corner.x = -1;
171  smaller_workspace_plan_req.workspace_parameters.min_corner.y = -1;
172  smaller_workspace_plan_req.workspace_parameters.min_corner.z = -1;
173 
174  // Larger workspace start
175  auto larger_workspace_plan_req = plan_req;
176  larger_workspace_plan_req.workspace_parameters.max_corner.x = 50;
177  larger_workspace_plan_req.workspace_parameters.max_corner.y = 50;
178  larger_workspace_plan_req.workspace_parameters.max_corner.z = 50;
179  larger_workspace_plan_req.workspace_parameters.min_corner.x = -50;
180  larger_workspace_plan_req.workspace_parameters.min_corner.y = -50;
181  larger_workspace_plan_req.workspace_parameters.min_corner.z = -50;
182 
183  // Different
184  auto different_plan_req = plan_req;
185  different_plan_req.workspace_parameters.max_corner.x += 1.05;
186  different_plan_req.workspace_parameters.min_corner.x -= 2.05;
187  different_plan_req.start_state.joint_state.position.at(0) -= 3.05;
188  different_plan_req.start_state.joint_state.position.at(1) += 4.05;
189  different_plan_req.start_state.joint_state.position.at(2) -= 5.05;
190  different_plan_req.goal_constraints.at(0).joint_constraints.at(0).position -= 6.05;
191  different_plan_req.goal_constraints.at(0).joint_constraints.at(1).position += 7.05;
192  different_plan_req.goal_constraints.at(0).joint_constraints.at(2).position -= 8.05;
193 
195 
196  // Trajectory
197  auto traj = getDummyPandaTraj();
198 
199  // Trajectory with no frame_id in its trajectory header
200  auto empty_frame_traj = traj;
201  empty_frame_traj.joint_trajectory.header.frame_id = "";
202 
203  auto different_traj = traj;
204  different_traj.joint_trajectory.points.at(0).positions.at(0) = 999;
205  different_traj.joint_trajectory.points.at(0).positions.at(1) = 999;
206  different_traj.joint_trajectory.points.at(0).positions.at(2) = 999;
207 
208  // Test Utils
209 
210  std::string test_case;
211 
212  // Checks ====================================================================
213 
214  // Initially empty
215  test_case = "testMotionTrajectories.empty";
216 
217  checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 0, test_case, "Trajectory cache initially empty");
218 
219  checkAndEmit(cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 999, 999).empty(), test_case,
220  "Fetch all trajectories on empty cache returns empty");
221 
222  checkAndEmit(cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 999, 999) == nullptr, test_case,
223  "Fetch best trajectory on empty cache returns nullptr");
224 
225  // Put trajectory empty frame
226  //
227  // Trajectory must have frame in joint trajectory, expect put fail
228  test_case = "testMotionTrajectories.insertTrajectory_empty_frame";
229  double execution_time = 999;
230  double planning_time = 999;
231 
232  checkAndEmit(!cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, empty_frame_traj, execution_time,
233  planning_time, false),
234  test_case, "Put empty frame trajectory, no prune_worse_trajectories, not ok");
235 
236  checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 0, test_case, "No trajectories in cache");
237 
238  // Put trajectory req empty frame
239  //
240  // Trajectory request having no frame in workspace should default to robot frame
241  test_case = "testMotionTrajectories.insertTrajectory_req_empty_frame";
242  execution_time = 1000;
243  planning_time = 1000;
244 
245  checkAndEmit(cache->insertTrajectory(*move_group, ROBOT_NAME, empty_frame_plan_req, traj, execution_time,
246  planning_time, false),
247  test_case, "Put empty frame req trajectory, no prune_worse_trajectories, ok");
248 
249  checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache");
250 
251  // Put second, no prune_worse_trajectories
252  test_case = "testMotionTrajectories.put_second";
253  execution_time = 999;
254  planning_time = 999;
255 
256  checkAndEmit(cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, traj, execution_time, planning_time, false),
257  test_case, "Put second valid trajectory, no prune_worse_trajectories, ok");
258 
259  checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache");
260 
261  // Fetch matching, no tolerance
262  //
263  // Exact key match should have cache hit
264  test_case = "testMotionTrajectories.put_second.fetch_matching_no_tolerance";
265 
266  auto fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0);
267 
268  auto fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0);
269 
270  checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two");
271  checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr");
272 
273  checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match");
274 
275  checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original");
276 
277  checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case,
278  "Fetched trajectory has correct execution time");
279 
280  checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case,
281  "Fetched trajectory has correct planning time");
282 
283  // Fetch with is_diff
284  //
285  // is_diff key should be equivalent to exact match if robot state did not
286  // change, hence should have cache hit
287  test_case = "testMotionTrajectories.put_second.fetch_is_diff_no_tolerance";
288 
289  fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, is_diff_plan_req, 0, 0);
290 
291  fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, is_diff_plan_req, 0, 0);
292 
293  checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two");
294  checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr");
295 
296  checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match");
297 
298  checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original");
299 
300  checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case,
301  "Fetched trajectory has correct execution time");
302 
303  checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case,
304  "Fetched trajectory has correct planning time");
305 
306  // Fetch non-matching, out of tolerance
307  //
308  // Non-matching key should not have cache hit
309  test_case = "testMotionTrajectories.put_second.fetch_non_matching_out_of_tolerance";
310 
311  fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 0);
312 
313  fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 0);
314 
315  checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty");
316  checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr");
317 
318  // Fetch non-matching, only start in tolerance (but not goal)
319  //
320  // Non-matching key should not have cache hit
321  test_case = "testMotionTrajectories.put_second.fetch_non_matching_only_start_in_tolerance";
322 
323  fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 999, 0);
324 
325  fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 999, 0);
326 
327  checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty");
328  checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr");
329 
330  // Fetch non-matching, only goal in tolerance (but not start)
331  //
332  // Non-matching key should not have cache hit
333  test_case = "testMotionTrajectories.put_second.fetch_non_matching_only_goal_in_tolerance";
334 
335  fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 999);
336 
337  fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 999);
338 
339  checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty");
340  checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr");
341 
342  // Fetch non-matching, in tolerance
343  //
344  // Close key within tolerance limit should have cache hit
345  test_case = "testMotionTrajectories.put_second.fetch_non_matching_in_tolerance";
346 
347  fetched_trajectories =
348  cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0.1, 0.1);
349 
350  fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0.1, 0.1);
351 
352  checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two");
353  checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr");
354 
355  checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match");
356 
357  checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original");
358 
359  checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case,
360  "Fetched trajectory has correct execution time");
361 
362  checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case,
363  "Fetched trajectory has correct planning time");
364 
365  // Fetch swapped
366  //
367  // Matches should be mostly invariant to constraint ordering
368  test_case = "testMotionTrajectories.put_second.fetch_swapped";
369 
370  fetched_trajectories =
371  cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, swapped_close_matching_plan_req, 0.1, 0.1);
372 
373  fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, swapped_close_matching_plan_req, 0.1, 0.1);
374 
375  checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two");
376  checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr");
377 
378  checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match");
379 
380  checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original");
381 
382  checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case,
383  "Fetched trajectory has correct execution time");
384 
385  checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case,
386  "Fetched trajectory has correct planning time");
387 
388  // Fetch with smaller workspace
389  //
390  // Matching trajectories with more restrictive workspace requirements should not
391  // pull up trajectories cached for less restrictive workspace requirements
392  test_case = "testMotionTrajectories.put_second.fetch_smaller_workspace";
393 
394  fetched_trajectories =
395  cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, smaller_workspace_plan_req, 999, 999);
396 
397  fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, smaller_workspace_plan_req, 999, 999);
398 
399  checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty");
400  checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr");
401 
402  // Fetch with larger workspace
403  //
404  // Matching trajectories with less restrictive workspace requirements should pull up
405  // trajectories cached for more restrictive workspace requirements
406  test_case = "testMotionTrajectories.put_second.fetch_larger_workspace";
407 
408  fetched_trajectories =
409  cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, larger_workspace_plan_req, 999, 999);
410 
411  fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, larger_workspace_plan_req, 999, 999);
412 
413  checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two");
414  checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr");
415 
416  checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match");
417 
418  checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original");
419 
420  checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case,
421  "Fetched trajectory has correct execution time");
422 
423  checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case,
424  "Fetched trajectory has correct planning time");
425 
426  checkAndEmit(fetched_traj->lookupDouble("workspace_parameters.max_corner.x") <=
427  larger_workspace_plan_req.workspace_parameters.max_corner.x,
428  test_case, "Fetched trajectory has more restrictive workspace max_corner");
429 
430  checkAndEmit(fetched_traj->lookupDouble("workspace_parameters.max_corner.x") >=
431  larger_workspace_plan_req.workspace_parameters.min_corner.x,
432  test_case, "Fetched trajectory has more restrictive workspace min_corner");
433 
434  // Put worse, no prune_worse_trajectories
435  //
436  // Worse trajectories should not be inserted
437  test_case = "testMotionTrajectories.put_worse";
438  double worse_execution_time = execution_time + 100;
439 
440  checkAndEmit(!cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, traj, worse_execution_time, planning_time,
441  false),
442  test_case, "Put worse trajectory, no prune_worse_trajectories, not ok");
443 
444  checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache");
445 
446  // Put better, no prune_worse_trajectories
447  //
448  // Better trajectories should be inserted
449  test_case = "testMotionTrajectories.put_better";
450  double better_execution_time = execution_time - 100;
451 
452  checkAndEmit(cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, traj, better_execution_time, planning_time,
453  false),
454  test_case, "Put better trajectory, no prune_worse_trajectories, ok");
455 
456  checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 3, test_case, "Three trajectories in cache");
457 
458  // Fetch sorted
459  //
460  // With multiple trajectories in cache, fetches should be sorted accordingly
461  test_case = "testMotionTrajectories.put_better.fetch_sorted";
462 
463  fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0);
464 
465  fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0);
466 
467  checkAndEmit(fetched_trajectories.size() == 3, test_case, "Fetch all returns three");
468  checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr");
469 
470  checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match");
471 
472  checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original");
473 
474  checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case,
475  "Fetched trajectory has correct execution time");
476 
477  checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case,
478  "Fetched trajectory has correct planning time");
479 
480  checkAndEmit(fetched_trajectories.at(0)->lookupDouble("execution_time_s") == better_execution_time &&
481  fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time,
482  test_case, "Fetched trajectories are sorted correctly");
483 
484  // Put better, prune_worse_trajectories
485  //
486  // Better, different, trajectories should be inserted
487  test_case = "testMotionTrajectories.put_better_prune_worse_trajectories";
488  double even_better_execution_time = better_execution_time - 100;
489 
490  checkAndEmit(cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, different_traj, even_better_execution_time,
491  planning_time, true),
492  test_case, "Put better trajectory, prune_worse_trajectories, ok");
493 
494  checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache");
495 
496  // Fetch better plan
497  test_case = "testMotionTrajectories.put_better_prune_worse_trajectories.fetch";
498 
499  fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0);
500 
501  fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0);
502 
503  checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one");
504  checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr");
505 
506  checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match");
507 
508  checkAndEmit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original");
509 
510  checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, test_case,
511  "Fetched trajectory has correct execution time");
512 
513  checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case,
514  "Fetched trajectory has correct planning time");
515 
516  // Put different req, prune_worse_trajectories
517  //
518  // Unrelated trajectory requests should live alongside pre-existing plans
519  test_case = "testMotionTrajectories.put_different_req";
520 
521  checkAndEmit(cache->insertTrajectory(*move_group, ROBOT_NAME, different_plan_req, different_traj,
522  better_execution_time, planning_time, true),
523  test_case, "Put different trajectory req, prune_worse_trajectories, ok");
524 
525  checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache");
526 
527  // Fetch with different trajectory req
528  //
529  // With multiple trajectories in cache, fetches should be sorted accordingly
530  test_case = "testMotionTrajectories.put_different_req.fetch";
531 
532  fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, different_plan_req, 0, 0);
533 
534  fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, different_plan_req, 0, 0);
535 
536  checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one");
537  checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr");
538 
539  checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match");
540 
541  checkAndEmit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original");
542 
543  checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case,
544  "Fetched trajectory has correct execution time");
545 
546  checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case,
547  "Fetched trajectory has correct planning time");
548 
549  // Fetch different robot
550  //
551  // Since we didn't populate anything, we should expect empty
552  test_case = "testMotionTrajectories.different_robot.empty";
553  std::string different_robot_name = "different_robot";
554 
555  checkAndEmit(cache->countCartesianTrajectories(different_robot_name) == 0, test_case, "No trajectories in cache");
556 
557  // Put first for different robot, prune_worse_trajectories
558  //
559  // A different robot's cache should not interact with the original cache
560  test_case = "testMotionTrajectories.different_robot.put_first";
561  checkAndEmit(cache->insertTrajectory(*move_group, different_robot_name, different_plan_req, different_traj,
562  better_execution_time, planning_time, true),
563  test_case, "Put different trajectory req, prune_worse_trajectories, ok");
564 
565  checkAndEmit(cache->countTrajectories(different_robot_name) == 1, test_case, "One trajectory in cache");
566 
567  checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in original robot's cache");
568 
569  fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, different_plan_req, 0, 0);
570 
571  checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all on original still returns one");
572 }
573 
574 void testCartesianTrajectories(const std::shared_ptr<MoveGroupInterface>& move_group,
575  const std::shared_ptr<TrajectoryCache>& cache)
576 {
577  std::string test_case;
578 
580 
581  // Construct get cartesian trajectory request
582  test_case = "testCartesianTrajectories.constructGetCartesianPathRequest";
583 
584  int test_step = 1;
585  int test_jump = 2;
586  auto test_waypoints = getDummyWaypoints();
587  auto cartesian_plan_req_under_test =
588  cache->constructGetCartesianPathRequest(*move_group, test_waypoints, test_step, test_jump, false);
589 
590  checkAndEmit(cartesian_plan_req_under_test.waypoints == test_waypoints &&
591  static_cast<int>(cartesian_plan_req_under_test.max_step) == test_step &&
592  static_cast<int>(cartesian_plan_req_under_test.jump_threshold) == test_jump &&
593  !cartesian_plan_req_under_test.avoid_collisions,
594  test_case, "Ok");
595 
596  // Setup =====================================================================
597  // All variants are modified copies of `cartesian_plan_req`.
598 
600 
601  // Plain start
602  auto waypoints = getDummyWaypoints();
603  auto cartesian_plan_req = cache->constructGetCartesianPathRequest(*move_group, waypoints, 1, 1, false);
604  cartesian_plan_req.start_state.multi_dof_joint_state.joint_names.clear();
605  cartesian_plan_req.start_state.multi_dof_joint_state.transforms.clear();
606  cartesian_plan_req.start_state.multi_dof_joint_state.twist.clear();
607  cartesian_plan_req.start_state.multi_dof_joint_state.wrench.clear();
608  cartesian_plan_req.path_constraints.joint_constraints.clear();
609  cartesian_plan_req.path_constraints.position_constraints.clear();
610  cartesian_plan_req.path_constraints.orientation_constraints.clear();
611  cartesian_plan_req.path_constraints.visibility_constraints.clear();
612 
613  // Empty frame start
614  auto empty_frame_cartesian_plan_req = cartesian_plan_req;
615  empty_frame_cartesian_plan_req.header.frame_id = "";
616 
617  // is_diff = true
618  auto is_diff_cartesian_plan_req = cartesian_plan_req;
619  is_diff_cartesian_plan_req.start_state.is_diff = true;
620  is_diff_cartesian_plan_req.start_state.joint_state.header.frame_id = "";
621  is_diff_cartesian_plan_req.start_state.joint_state.name.clear();
622  is_diff_cartesian_plan_req.start_state.joint_state.position.clear();
623  is_diff_cartesian_plan_req.start_state.joint_state.velocity.clear();
624  is_diff_cartesian_plan_req.start_state.joint_state.effort.clear();
625 
626  // Something close enough (mod 0.1 away)
627  auto close_matching_cartesian_plan_req = cartesian_plan_req;
628  close_matching_cartesian_plan_req.start_state.joint_state.position.at(0) -= 0.05;
629  close_matching_cartesian_plan_req.start_state.joint_state.position.at(1) += 0.05;
630  close_matching_cartesian_plan_req.start_state.joint_state.position.at(2) -= 0.05;
631  close_matching_cartesian_plan_req.waypoints.at(0).position.x -= 0.05;
632  close_matching_cartesian_plan_req.waypoints.at(1).position.x += 0.05;
633  close_matching_cartesian_plan_req.waypoints.at(2).position.x -= 0.05;
634 
635  // Different
636  auto different_cartesian_plan_req = cartesian_plan_req;
637  different_cartesian_plan_req.start_state.joint_state.position.at(0) -= 1.05;
638  different_cartesian_plan_req.start_state.joint_state.position.at(1) += 2.05;
639  different_cartesian_plan_req.start_state.joint_state.position.at(2) -= 3.05;
640  different_cartesian_plan_req.waypoints.at(0).position.x -= 1.05;
641  different_cartesian_plan_req.waypoints.at(1).position.x += 2.05;
642  different_cartesian_plan_req.waypoints.at(2).position.x -= 3.05;
643 
645 
646  // Trajectory
647  auto traj = getDummyPandaTraj();
648 
649  // Trajectory with no frame_id in its trajectory header
650  auto empty_frame_traj = traj;
651  empty_frame_traj.joint_trajectory.header.frame_id = "";
652 
653  auto different_traj = traj;
654  different_traj.joint_trajectory.points.at(0).positions.at(0) = 999;
655  different_traj.joint_trajectory.points.at(0).positions.at(1) = 999;
656  different_traj.joint_trajectory.points.at(0).positions.at(2) = 999;
657 
658  // Checks ====================================================================
659 
660  // Initially empty
661  test_case = "testCartesianTrajectories.empty";
662 
663  checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, test_case, "Trajectory cache initially empty");
664 
665  checkAndEmit(
666  cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999).empty(),
667  test_case, "Fetch all trajectories on empty cache returns empty");
668 
669  checkAndEmit(cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999) ==
670  nullptr,
671  test_case, "Fetch best trajectory on empty cache returns nullptr");
672 
673  // Put trajectory empty frame
674  //
675  // Trajectory must have frame in joint trajectory, expect put fail
676  test_case = "testCartesianTrajectories.insertTrajectory_empty_frame";
677  double execution_time = 999;
678  double planning_time = 999;
679  double fraction = 0.5;
680 
681  checkAndEmit(!cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, empty_frame_traj,
682  execution_time, planning_time, fraction, false),
683  test_case, "Put empty frame trajectory, no prune_worse_trajectories, not ok");
684 
685  checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, test_case, "No trajectories in cache");
686 
687  // Put trajectory req empty frame
688  //
689  // Trajectory request having no frame in workspace should default to robot frame
690  test_case = "testCartesianTrajectories.insertTrajectory_req_empty_frame";
691  execution_time = 1000;
692  planning_time = 1000;
693 
694  checkAndEmit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, empty_frame_cartesian_plan_req, traj,
695  execution_time, planning_time, fraction, false),
696  test_case, "Put empty frame req trajectory, no prune_worse_trajectories, ok");
697 
698  checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache");
699 
700  // Put second, no prune_worse_trajectories
701  test_case = "testCartesianTrajectories.put_second";
702  execution_time = 999;
703  planning_time = 999;
704 
705  checkAndEmit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, execution_time,
706  planning_time, fraction, false),
707  test_case, "Put second valid trajectory, no prune_worse_trajectories, ok");
708 
709  checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache");
710 
711  // Fetch matching, no tolerance
712  //
713  // Exact key match should have cache hit
714  test_case = "testCartesianTrajectories.put_second.fetch_matching_no_tolerance";
715 
716  auto fetched_trajectories =
717  cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0);
718 
719  auto fetched_traj =
720  cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0);
721 
722  checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two");
723  checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr");
724 
725  checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match");
726 
727  checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original");
728 
729  checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case,
730  "Fetched trajectory has correct execution time");
731 
732  checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case,
733  "Fetched trajectory has correct planning time");
734 
735  checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction");
736 
737  // Fetch with is_diff
738  //
739  // is_diff key should be equivalent to exact match if robot state did not
740  // change, hence should have cache hit
741  test_case = "testCartesianTrajectories.put_second.fetch_is_diff_no_tolerance";
742 
743  fetched_trajectories =
744  cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, is_diff_cartesian_plan_req, fraction, 0, 0);
745 
746  fetched_traj =
747  cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, is_diff_cartesian_plan_req, fraction, 0, 0);
748 
749  checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two");
750  checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr");
751 
752  checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match");
753 
754  checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original");
755 
756  checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case,
757  "Fetched trajectory has correct execution time");
758 
759  checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case,
760  "Fetched trajectory has correct planning time");
761 
762  checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction");
763 
764  // Fetch non-matching, out of tolerance
765  //
766  // Non-matching key should not have cache hit
767  test_case = "testCartesianTrajectories.put_second.fetch_non_matching_out_of_tolerance";
768 
769  fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(
770  *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 0);
771 
772  fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req,
773  fraction, 0, 0);
774 
775  checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty");
776  checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr");
777 
778  // Fetch non-matching, only start in tolerance (but not goal)
779  //
780  // Non-matching key should not have cache hit
781  test_case = "testMotionTrajectories.put_second.fetch_non_matching_only_start_in_tolerance";
782 
783  fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(
784  *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 999, 0);
785 
786  fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req,
787  fraction, 999, 0);
788 
789  checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty");
790  checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr");
791 
792  // Fetch non-matching, only goal in tolerance (but not start)
793  //
794  // Non-matching key should not have cache hit
795  test_case = "testMotionTrajectories.put_second.fetch_non_matching_only_goal_in_tolerance";
796 
797  fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(
798  *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 999);
799 
800  fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req,
801  fraction, 0, 999);
802 
803  checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty");
804  checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr");
805 
806  // Fetch non-matching, in tolerance
807  //
808  // Close key within tolerance limit should have cache hit
809  test_case = "testCartesianTrajectories.put_second.fetch_non_matching_in_tolerance";
810 
811  fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(
812  *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0.1, 0.1);
813 
814  fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req,
815  fraction, 0.1, 0.1);
816 
817  checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two");
818  checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr");
819 
820  checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match");
821 
822  checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original");
823 
824  checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case,
825  "Fetched trajectory has correct execution time");
826 
827  checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case,
828  "Fetched trajectory has correct planning time");
829 
830  checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction");
831 
832  // Fetch with higher fraction
833  //
834  // Matching trajectories with more restrictive fraction requirements should not
835  // pull up trajectories cached for less restrictive fraction requirements
836  test_case = "testCartesianTrajectories.put_second.fetch_higher_fraction";
837 
838  fetched_trajectories =
839  cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 1, 999, 999);
840 
841  fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 1, 999, 999);
842 
843  checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty");
844  checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr");
845 
846  // Fetch with lower fraction
847  //
848  // Matching trajectories with less restrictive fraction requirements should pull up
849  // trajectories cached for more restrictive fraction requirements
850  test_case = "testCartesianTrajectories.put_second.fetch_lower_fraction";
851 
852  fetched_trajectories =
853  cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999);
854 
855  fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999);
856 
857  checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two");
858  checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr");
859 
860  checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match");
861 
862  checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original");
863 
864  checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case,
865  "Fetched trajectory has correct execution time");
866 
867  checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case,
868  "Fetched trajectory has correct planning time");
869 
870  checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction");
871 
872  // Put worse, no prune_worse_trajectories
873  //
874  // Worse trajectories should not be inserted
875  test_case = "testCartesianTrajectories.put_worse";
876  double worse_execution_time = execution_time + 100;
877 
878  checkAndEmit(!cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj,
879  worse_execution_time, planning_time, fraction, false),
880  test_case, "Put worse trajectory, no prune_worse_trajectories, not ok");
881 
882  checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache");
883 
884  // Put better, no prune_worse_trajectories
885  //
886  // Better trajectories should be inserted
887  test_case = "testCartesianTrajectories.put_better";
888  double better_execution_time = execution_time - 100;
889 
890  checkAndEmit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj,
891  better_execution_time, planning_time, fraction, false),
892  test_case, "Put better trajectory, no prune_worse_trajectories, ok");
893 
894  checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 3, test_case, "Three trajectories in cache");
895 
896  // Fetch sorted
897  //
898  // With multiple trajectories in cache, fetches should be sorted accordingly
899  test_case = "testCartesianTrajectories.put_better.fetch_sorted";
900 
901  fetched_trajectories =
902  cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0);
903 
904  fetched_traj =
905  cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0);
906 
907  checkAndEmit(fetched_trajectories.size() == 3, test_case, "Fetch all returns three");
908  checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr");
909 
910  checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match");
911 
912  checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original");
913 
914  checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case,
915  "Fetched trajectory has correct execution time");
916 
917  checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case,
918  "Fetched trajectory has correct planning time");
919 
920  checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction");
921 
922  checkAndEmit(fetched_trajectories.at(0)->lookupDouble("execution_time_s") == better_execution_time &&
923  fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time,
924  test_case, "Fetched trajectories are sorted correctly");
925 
926  // Put better, prune_worse_trajectories
927  //
928  // Better, different, trajectories should be inserted
929  test_case = "testCartesianTrajectories.put_better_prune_worse_trajectories";
930  double even_better_execution_time = better_execution_time - 100;
931 
932  checkAndEmit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, different_traj,
933  even_better_execution_time, planning_time, fraction, true),
934  test_case, "Put better trajectory, prune_worse_trajectories, ok");
935 
936  checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache");
937 
938  // Fetch better plan
939  test_case = "testCartesianTrajectories.put_better_prune_worse_trajectories.fetch";
940 
941  fetched_trajectories =
942  cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0);
943 
944  fetched_traj =
945  cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0);
946 
947  checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one");
948  checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr");
949 
950  checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match");
951 
952  checkAndEmit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original");
953 
954  checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, test_case,
955  "Fetched trajectory has correct execution time");
956 
957  checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case,
958  "Fetched trajectory has correct planning time");
959 
960  checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction");
961 
962  // Put different req, prune_worse_trajectories
963  //
964  // Unrelated trajectory requests should live alongside pre-existing plans
965  test_case = "testCartesianTrajectories.put_different_req";
966 
967  checkAndEmit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, different_cartesian_plan_req, different_traj,
968  better_execution_time, planning_time, fraction, true),
969  test_case, "Put different trajectory req, prune_worse_trajectories, ok");
970 
971  checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache");
972 
973  // Fetch with different trajectory req
974  //
975  // With multiple trajectories in cache, fetches should be sorted accordingly
976  test_case = "testCartesianTrajectories.put_different_req.fetch";
977 
978  fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME,
979  different_cartesian_plan_req, fraction, 0, 0);
980 
981  fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, different_cartesian_plan_req,
982  fraction, 0, 0);
983 
984  checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one");
985  checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr");
986 
987  checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match");
988 
989  checkAndEmit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original");
990 
991  checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case,
992  "Fetched trajectory has correct execution time");
993 
994  checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case,
995  "Fetched trajectory has correct planning time");
996 
997  checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction");
998 
999  // Fetch different robot
1000  //
1001  // Since we didn't populate anything, we should expect empty
1002  test_case = "testCartesianTrajectories.different_robot.empty";
1003  std::string different_robot_name = "different_robot";
1004 
1005  checkAndEmit(cache->countCartesianTrajectories(different_robot_name) == 0, test_case, "No trajectories in cache");
1006 
1007  // Put first for different robot, prune_worse_trajectories
1008  //
1009  // A different robot's cache should not interact with the original cache
1010  test_case = "testCartesianTrajectories.different_robot.put_first";
1011  checkAndEmit(cache->insertCartesianTrajectory(*move_group, different_robot_name, different_cartesian_plan_req,
1012  different_traj, better_execution_time, planning_time, fraction, true),
1013  test_case, "Put different trajectory req, prune_worse_trajectories, ok");
1014 
1015  checkAndEmit(cache->countCartesianTrajectories(different_robot_name) == 1, test_case, "One trajectory in cache");
1016 
1017  checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case,
1018  "Two trajectories in original robot's cache");
1019 
1020  fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME,
1021  different_cartesian_plan_req, fraction, 0, 0);
1022 
1023  checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all on original still returns one");
1024 }
1025 
1026 int main(int argc, char** argv)
1027 {
1028  // Setup
1029  rclcpp::init(argc, argv);
1030 
1031  rclcpp::NodeOptions test_node_options;
1032  test_node_options.automatically_declare_parameters_from_overrides(true);
1033  test_node_options.arguments({ "--ros-args", "-r", "__node:=test_node" });
1034 
1035  rclcpp::NodeOptions move_group_node_options;
1036  move_group_node_options.automatically_declare_parameters_from_overrides(true);
1037  move_group_node_options.arguments({ "--ros-args", "-r", "__node:=move_group_node" });
1038 
1039  auto test_node = rclcpp::Node::make_shared("test_node", test_node_options);
1040  auto move_group_node = rclcpp::Node::make_shared("move_group_node", move_group_node_options);
1041 
1042  std::atomic<bool> running = true;
1043 
1044  std::thread spin_thread([&]() {
1045  while (rclcpp::ok() && running)
1046  {
1047  rclcpp::spin_some(test_node);
1048  rclcpp::spin_some(move_group_node);
1049  }
1050  });
1051 
1052  // This is necessary
1053  test_node->declare_parameter<std::string>("warehouse_plugin", "warehouse_ros_sqlite::DatabaseConnection");
1054 
1055  {
1056  // Init.
1057 
1058  auto move_group = std::make_shared<MoveGroupInterface>(move_group_node, "panda_arm");
1059  auto curr_state = move_group->getCurrentState(60);
1060  move_group->setStartState(*curr_state);
1061 
1062  auto cache = std::make_shared<TrajectoryCache>(test_node);
1063 
1064  TrajectoryCache::Options options;
1065  options.db_path = ":memory:";
1066  options.db_port = 0;
1067  options.exact_match_precision = 10;
1068  options.num_additional_trajectories_to_preserve_when_deleting_worse = 10;
1069 
1070  // Tests.
1071 
1072  checkAndEmit(cache->init(options), "init", "Cache init");
1073 
1074  testGettersAndSetters(cache);
1075 
1076  cache->setExactMatchPrecision(1e-4);
1077  cache->setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse(0);
1078 
1079  testMotionTrajectories(move_group, cache);
1080  testCartesianTrajectories(move_group, cache);
1081  }
1082 
1083  running = false;
1084  spin_thread.join();
1085 
1086  test_node.reset();
1087  move_group_node.reset();
1088 
1089  rclcpp::shutdown();
1090  return 0;
1091 }
moveit::planning_interface::MoveGroupInterface
Client class to conveniently use the ROS interfaces provided by the move_group node.
Definition: move_group_interface.h:76
moveit_ros::trajectory_cache::TrajectoryCache
Trajectory Cache manager for MoveIt.
Definition: trajectory_cache.hpp:118
robot_state.h
move_group
Definition: capability_names.h:42
moveit_benchmark_statistics.options
options
Definition: moveit_benchmark_statistics.py:774
planning_interface::MotionPlanRequest
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
Definition: planning_request.h:45
ROBOT_NAME
const std::string ROBOT_NAME
Definition: test_trajectory_cache.cpp:31
main
int main(int argc, char **argv)
Definition: test_trajectory_cache.cpp:1026
testGettersAndSetters
void testGettersAndSetters(const std::shared_ptr< TrajectoryCache > &cache)
Definition: test_trajectory_cache.cpp:95
testMotionTrajectories
void testMotionTrajectories(const std::shared_ptr< MoveGroupInterface > &move_group, const std::shared_ptr< TrajectoryCache > &cache)
Definition: test_trajectory_cache.cpp:113
ROBOT_FRAME
const std::string ROBOT_FRAME
Definition: test_trajectory_cache.cpp:32
getDummyWaypoints
std::vector< geometry_msgs::msg::Pose > getDummyWaypoints()
Definition: test_trajectory_cache.cpp:75
testCartesianTrajectories
void testCartesianTrajectories(const std::shared_ptr< MoveGroupInterface > &move_group, const std::shared_ptr< TrajectoryCache > &cache)
Definition: test_trajectory_cache.cpp:574
checkAndEmit
void checkAndEmit(bool predicate, const std::string &test_case, const std::string &label)
Definition: test_trajectory_cache.cpp:36
getDummyPandaTraj
moveit_msgs::msg::RobotTrajectory getDummyPandaTraj()
Definition: test_trajectory_cache.cpp:48
trajectory_cache.hpp
Fuzzy-Matching Trajectory Cache.
Generated by doxygen 1.9.1