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