From 99d50cdaa96b3c689bee0c4981027d54ea2eb866 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Fri, 17 May 2024 23:08:51 -0700 Subject: [PATCH 01/26] Implement trajectory cache Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/CHANGELOG.rst | 7 + moveit_ros/trajectory_cache/CMakeLists.txt | 63 + moveit_ros/trajectory_cache/README.md | 1 + moveit_ros/trajectory_cache/package.xml | 40 + .../src/test_trajectory_cache.cpp | 1066 +++++++++++++ .../trajectory_cache/src/trajectory_cache.cpp | 1340 +++++++++++++++++ .../trajectory_cache/src/trajectory_cache.hpp | 261 ++++ .../test/test_trajectory_cache.py | 154 ++ 8 files changed, 2932 insertions(+) create mode 100644 moveit_ros/trajectory_cache/CHANGELOG.rst create mode 100644 moveit_ros/trajectory_cache/CMakeLists.txt create mode 100644 moveit_ros/trajectory_cache/README.md create mode 100644 moveit_ros/trajectory_cache/package.xml create mode 100644 moveit_ros/trajectory_cache/src/test_trajectory_cache.cpp create mode 100644 moveit_ros/trajectory_cache/src/trajectory_cache.cpp create mode 100644 moveit_ros/trajectory_cache/src/trajectory_cache.hpp create mode 100755 moveit_ros/trajectory_cache/test/test_trajectory_cache.py diff --git a/moveit_ros/trajectory_cache/CHANGELOG.rst b/moveit_ros/trajectory_cache/CHANGELOG.rst new file mode 100644 index 0000000000..82ad0f2be9 --- /dev/null +++ b/moveit_ros/trajectory_cache/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package nexus_motion_planner +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2024-05-17) +------------------ +* Add ``moveit_ros_trajectory_cache`` package for trajectory caching. \ No newline at end of file diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt new file mode 100644 index 0000000000..4fabec17d7 --- /dev/null +++ b/moveit_ros/trajectory_cache/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.8) +project(moveit_ros_trajectory_cache) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(moveit_ros_warehouse REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(trajectory_msgs REQUIRED) +find_package(warehouse_ros REQUIRED) +find_package(warehouse_ros_sqlite REQUIRED) + +include_directories(include) + +set (trajectory_cache_dependencies + geometry_msgs + moveit_ros_planning_interface + moveit_ros_warehouse + rclcpp + tf2 + tf2_ros + trajectory_msgs + warehouse_ros + warehouse_ros_sqlite +) + +#=============================================================================== +set(TRAJECTORY_CACHE_LIBRARY_NAME trajectory_cache) + +# Motion plan cache library +add_library(${TRAJECTORY_CACHE_LIBRARY_NAME} src/trajectory_cache.cpp) +ament_target_dependencies(${TRAJECTORY_CACHE_LIBRARY_NAME} ${trajectory_cache_dependencies}) + +#=============================================================================== + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + find_package(rmf_utils REQUIRED) + + add_executable(test_trajectory_cache src/test_trajectory_cache.cpp) + target_link_libraries(test_trajectory_cache ${TRAJECTORY_CACHE_LIBRARY_NAME}) + + install(TARGETS + test_trajectory_cache + RUNTIME DESTINATION lib/${PROJECT_NAME} + ) + + ament_add_pytest_test(test_trajectory_cache_py "test/test_trajectory_cache.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ) + +endif() + +ament_export_dependencies(${trajectory_cache_dependencies}) +ament_package() diff --git a/moveit_ros/trajectory_cache/README.md b/moveit_ros/trajectory_cache/README.md new file mode 100644 index 0000000000..d0ff499365 --- /dev/null +++ b/moveit_ros/trajectory_cache/README.md @@ -0,0 +1 @@ +# Trajectory Cache diff --git a/moveit_ros/trajectory_cache/package.xml b/moveit_ros/trajectory_cache/package.xml new file mode 100644 index 0000000000..9a1edf74b3 --- /dev/null +++ b/moveit_ros/trajectory_cache/package.xml @@ -0,0 +1,40 @@ + + + + moveit_ros_trajectory_cache + 0.1.0 + A trajectory cache for MoveIt 2 motion plans and cartesian plans. + Brandon Ong + Apache License 2.0 + + ament_cmake + + geometry_msgs + moveit_ros_planning_interface + rclcpp + rclcpp_action + tf2_ros + trajectory_msgs + + moveit_ros + python3-yaml + warehouse_ros_sqlite + xacro + + ament_cmake_pytest + ament_cmake_uncrustify + launch_pytest + launch_testing_ament_cmake + + moveit_configs_utils + moveit_planners_ompl + moveit_resources + python3-pytest + rmf_utils + robot_state_publisher + ros2_control + + + ament_cmake + + diff --git a/moveit_ros/trajectory_cache/src/test_trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/test_trajectory_cache.cpp new file mode 100644 index 0000000000..799b2d60c8 --- /dev/null +++ b/moveit_ros/trajectory_cache/src/test_trajectory_cache.cpp @@ -0,0 +1,1066 @@ +// Copyright 2023 Johnson & Johnson +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "moveit/robot_state/conversions.h" +#include "moveit/robot_state/robot_state.h" +#include "trajectory_cache.hpp" + +#include +#include + +using moveit::planning_interface::MoveGroupInterface; +using moveit_ros::trajectory_cache::TrajectoryCache; + +const std::string g_robot_name = "panda"; +const std::string g_robot_frame = "world"; + +// UTILS ======================================================================= +// Utility function to emit a pass or fail statement. +void check_and_emit(bool predicate, const std::string& prefix, const std::string& label) +{ + if (predicate) + { + std::cout << "[PASS] " << prefix << ": " << label << std::endl; + } + else + { + std::cout << "[FAIL] " << prefix << ": " << label << std::endl; + exit(1); + } +} + +moveit_msgs::msg::RobotTrajectory get_dummy_panda_traj() +{ + static moveit_msgs::msg::RobotTrajectory out = []() { + moveit_msgs::msg::RobotTrajectory traj; + + auto trajectory = &traj.joint_trajectory; + trajectory->header.frame_id = g_robot_frame; + + trajectory->joint_names.push_back(g_robot_name + "_joint1"); + trajectory->joint_names.push_back(g_robot_name + "_joint2"); + trajectory->joint_names.push_back(g_robot_name + "_joint3"); + trajectory->joint_names.push_back(g_robot_name + "_joint4"); + trajectory->joint_names.push_back(g_robot_name + "_joint5"); + trajectory->joint_names.push_back(g_robot_name + "_joint6"); + trajectory->joint_names.push_back(g_robot_name + "_joint7"); + + trajectory->points.emplace_back(); + trajectory->points.at(0).positions = { 0, 0, 0, 0, 0, 0 }; + trajectory->points.at(0).velocities = { 0, 0, 0, 0, 0, 0 }; + trajectory->points.at(0).accelerations = { 0, 0, 0, 0, 0, 0 }; + trajectory->points.at(0).time_from_start.sec = 999999; + + return traj; + }(); + return out; +} + +std::vector get_dummy_waypoints() +{ + static std::vector out = []() { + std::vector waypoints; + for (size_t i = 0; i < 3; i++) + { + waypoints.emplace_back(); + waypoints.at(i).position.x = i; + waypoints.at(i).position.y = i; + waypoints.at(i).position.z = i; + waypoints.at(i).orientation.w = i; + waypoints.at(i).orientation.x = i + 0.1; + waypoints.at(i).orientation.y = i + 0.1; + waypoints.at(i).orientation.z = i + 0.1; + } + return waypoints; + }(); + return out; +} + +void test_motion_trajectories(std::shared_ptr move_group, std::shared_ptr cache) +{ + // Setup ===================================================================== + // All variants are copies. + + /// MotionPlanRequest + + // Plain start + moveit_msgs::msg::MotionPlanRequest plan_req; + move_group->constructMotionPlanRequest(plan_req); + plan_req.workspace_parameters.header.frame_id = g_robot_frame; + plan_req.workspace_parameters.max_corner.x = 10; + plan_req.workspace_parameters.max_corner.y = 10; + plan_req.workspace_parameters.max_corner.z = 10; + plan_req.workspace_parameters.min_corner.x = -10; + plan_req.workspace_parameters.min_corner.y = -10; + plan_req.workspace_parameters.min_corner.z = -10; + plan_req.start_state.multi_dof_joint_state.joint_names.clear(); + plan_req.start_state.multi_dof_joint_state.transforms.clear(); + plan_req.start_state.multi_dof_joint_state.twist.clear(); + plan_req.start_state.multi_dof_joint_state.wrench.clear(); + + // Empty frame start + moveit_msgs::msg::MotionPlanRequest empty_frame_plan_req = plan_req; + empty_frame_plan_req.workspace_parameters.header.frame_id = ""; + + // is_diff = true + auto is_diff_plan_req = plan_req; + is_diff_plan_req.start_state.is_diff = true; + is_diff_plan_req.start_state.joint_state.header.frame_id = ""; + is_diff_plan_req.start_state.joint_state.name.clear(); + is_diff_plan_req.start_state.joint_state.position.clear(); + is_diff_plan_req.start_state.joint_state.velocity.clear(); + is_diff_plan_req.start_state.joint_state.effort.clear(); + + // Something close enough (mod 0.1 away) + auto close_matching_plan_req = plan_req; + close_matching_plan_req.workspace_parameters.max_corner.x += 0.05; + close_matching_plan_req.workspace_parameters.min_corner.x -= 0.05; + close_matching_plan_req.start_state.joint_state.position.at(0) -= 0.05; + close_matching_plan_req.start_state.joint_state.position.at(1) += 0.05; + close_matching_plan_req.start_state.joint_state.position.at(2) -= 0.05; + close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(0).position -= 0.05; + close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(1).position += 0.05; + close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(2).position -= 0.05; + + // Close with swapped constraints (mod 0.1 away) + auto swapped_close_matching_plan_req = close_matching_plan_req; + std::swap(swapped_close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(0), + swapped_close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(1)); + + // Smaller workspace start + auto smaller_workspace_plan_req = plan_req; + smaller_workspace_plan_req.workspace_parameters.max_corner.x = 1; + smaller_workspace_plan_req.workspace_parameters.max_corner.y = 1; + smaller_workspace_plan_req.workspace_parameters.max_corner.z = 1; + smaller_workspace_plan_req.workspace_parameters.min_corner.x = -1; + smaller_workspace_plan_req.workspace_parameters.min_corner.y = -1; + smaller_workspace_plan_req.workspace_parameters.min_corner.z = -1; + + // Larger workspace start + auto larger_workspace_plan_req = plan_req; + larger_workspace_plan_req.workspace_parameters.max_corner.x = 50; + larger_workspace_plan_req.workspace_parameters.max_corner.y = 50; + larger_workspace_plan_req.workspace_parameters.max_corner.z = 50; + larger_workspace_plan_req.workspace_parameters.min_corner.x = -50; + larger_workspace_plan_req.workspace_parameters.min_corner.y = -50; + larger_workspace_plan_req.workspace_parameters.min_corner.z = -50; + + // Different + auto different_plan_req = plan_req; + different_plan_req.workspace_parameters.max_corner.x += 1.05; + different_plan_req.workspace_parameters.min_corner.x -= 2.05; + different_plan_req.start_state.joint_state.position.at(0) -= 3.05; + different_plan_req.start_state.joint_state.position.at(1) += 4.05; + different_plan_req.start_state.joint_state.position.at(2) -= 5.05; + different_plan_req.goal_constraints.at(0).joint_constraints.at(0).position -= 6.05; + different_plan_req.goal_constraints.at(0).joint_constraints.at(1).position += 7.05; + different_plan_req.goal_constraints.at(0).joint_constraints.at(2).position -= 8.05; + + /// RobotTrajectory + + // Trajectory + auto traj = get_dummy_panda_traj(); + + // Trajectory with no frame_id in its trajectory header + auto empty_frame_traj = traj; + empty_frame_traj.joint_trajectory.header.frame_id = ""; + + auto different_traj = traj; + different_traj.joint_trajectory.points.at(0).positions.at(0) = 999; + different_traj.joint_trajectory.points.at(0).positions.at(1) = 999; + different_traj.joint_trajectory.points.at(0).positions.at(2) = 999; + + // Test Utils + + std::string prefix; + + // Checks ==================================================================== + + // Initially empty + prefix = "test_motion_trajectories.empty"; + + check_and_emit(cache->count_trajectories(g_robot_name) == 0, prefix, "Trajectory cache initially empty"); + + check_and_emit(cache->fetch_all_matching_trajectories(*move_group, g_robot_name, plan_req, 999, 999).empty(), prefix, + "Fetch all trajectories on empty cache returns empty"); + + check_and_emit(cache->fetch_best_matching_trajectory(*move_group, g_robot_name, plan_req, 999, 999) == nullptr, + prefix, "Fetch best trajectory on empty cache returns nullptr"); + + // Put trajectory empty frame + // + // Trajectory must have frame in joint trajectory, expect put fail + prefix = "test_motion_trajectories.put_trajectory_empty_frame"; + double execution_time = 999; + double planning_time = 999; + + check_and_emit(!cache->put_trajectory(*move_group, g_robot_name, plan_req, empty_frame_traj, execution_time, + planning_time, false), + prefix, "Put empty frame trajectory, no delete_worse_trajectories, not ok"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 0, prefix, "No trajectories in cache"); + + // Put trajectory req empty frame + // + // Trajectory request must have frame in workspace, expect put fail + prefix = "test_motion_trajectories.put_trajectory_req_empty_frame"; + execution_time = 999; + planning_time = 999; + + check_and_emit(!cache->put_trajectory(*move_group, g_robot_name, empty_frame_plan_req, traj, execution_time, + planning_time, false), + prefix, "Put empty frame req trajectory, no delete_worse_trajectories, not ok"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 0, prefix, "No trajectories in cache"); + + // Put first, no delete_worse_trajectories + prefix = "test_motion_trajectories.put_first"; + execution_time = 999; + planning_time = 999; + + check_and_emit(cache->put_trajectory(*move_group, g_robot_name, plan_req, traj, execution_time, planning_time, false), + prefix, "Put first valid trajectory, no delete_worse_trajectories, ok"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + + // Fetch matching, no tolerance + // + // Exact key match should have cache hit + prefix = "test_motion_trajectories.put_first.fetch_matching_no_tolerance"; + + auto fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, plan_req, 0, 0); + + auto fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, plan_req, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + // Fetch with is_diff + // + // is_diff key should be equivalent to exact match if robot state did not + // change, hence should have cache hit + prefix = "test_motion_trajectories.put_first.fetch_is_diff_no_tolerance"; + + fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, is_diff_plan_req, 0, 0); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, is_diff_plan_req, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + // Fetch non-matching, out of tolerance + // + // Non-matching key should not have cache hit + prefix = "test_motion_trajectories.put_first.fetch_non_matching_out_of_tolerance"; + + fetched_trajectories = + cache->fetch_all_matching_trajectories(*move_group, g_robot_name, close_matching_plan_req, 0, 0); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, close_matching_plan_req, 0, 0); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, only start in tolerance (but not goal) + // + // Non-matching key should not have cache hit + prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_start_in_tolerance"; + + fetched_trajectories = + cache->fetch_all_matching_trajectories(*move_group, g_robot_name, close_matching_plan_req, 999, 0); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, close_matching_plan_req, 999, 0); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, only goal in tolerance (but not start) + // + // Non-matching key should not have cache hit + prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_goal_in_tolerance"; + + fetched_trajectories = + cache->fetch_all_matching_trajectories(*move_group, g_robot_name, close_matching_plan_req, 0, 999); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, close_matching_plan_req, 0, 999); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, in tolerance + // + // Close key within tolerance limit should have cache hit + prefix = "test_motion_trajectories.put_first.fetch_non_matching_in_tolerance"; + + fetched_trajectories = + cache->fetch_all_matching_trajectories(*move_group, g_robot_name, close_matching_plan_req, 0.1, 0.1); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, close_matching_plan_req, 0.1, 0.1); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + // Fetch swapped + // + // Matches should be mostly invariant to constraint ordering + prefix = "test_motion_trajectories.put_first.fetch_swapped"; + + fetched_trajectories = + cache->fetch_all_matching_trajectories(*move_group, g_robot_name, swapped_close_matching_plan_req, 0.1, 0.1); + + fetched_traj = + cache->fetch_best_matching_trajectory(*move_group, g_robot_name, swapped_close_matching_plan_req, 0.1, 0.1); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + // Fetch with smaller workspace + // + // Matching trajectories with more restrictive workspace requirements should not + // pull up trajectories cached for less restrictive workspace requirements + prefix = "test_motion_trajectories.put_first.fetch_smaller_workspace"; + + fetched_trajectories = + cache->fetch_all_matching_trajectories(*move_group, g_robot_name, smaller_workspace_plan_req, 999, 999); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, smaller_workspace_plan_req, 999, 999); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + + // Fetch with larger workspace + // + // Matching trajectories with less restrictive workspace requirements should pull up + // trajectories cached for more restrictive workspace requirements + prefix = "test_motion_trajectories.put_first.fetch_larger_workspace"; + + fetched_trajectories = + cache->fetch_all_matching_trajectories(*move_group, g_robot_name, larger_workspace_plan_req, 999, 999); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, larger_workspace_plan_req, 999, 999); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("workspace_parameters.max_corner.x") <= + larger_workspace_plan_req.workspace_parameters.max_corner.x, + prefix, "Fetched trajectory has more restrictive workspace max_corner"); + + check_and_emit(fetched_traj->lookupDouble("workspace_parameters.max_corner.x") >= + larger_workspace_plan_req.workspace_parameters.min_corner.x, + prefix, "Fetched trajectory has more restrictive workspace min_corner"); + + // Put worse, no delete_worse_trajectories + // + // Worse trajectories should not be inserted + prefix = "test_motion_trajectories.put_worse"; + double worse_execution_time = execution_time + 100; + + check_and_emit(!cache->put_trajectory(*move_group, g_robot_name, plan_req, traj, worse_execution_time, planning_time, + false), + prefix, "Put worse trajectory, no delete_worse_trajectories, not ok"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + + // Put better, no delete_worse_trajectories + // + // Better trajectories should be inserted + prefix = "test_motion_trajectories.put_better"; + double better_execution_time = execution_time - 100; + + check_and_emit(cache->put_trajectory(*move_group, g_robot_name, plan_req, traj, better_execution_time, planning_time, + false), + prefix, "Put better trajectory, no delete_worse_trajectories, ok"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 2, prefix, "Two trajectories in cache"); + + // Fetch sorted + // + // With multiple trajectories in cache, fetches should be sorted accordingly + prefix = "test_motion_trajectories.put_better.fetch_sorted"; + + fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, plan_req, 0, 0); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, plan_req, 0, 0); + + check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_trajectories.at(0)->lookupDouble("execution_time_s") == better_execution_time && + fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched trajectories are sorted correctly"); + + // Put better, delete_worse_trajectories + // + // Better, different, trajectories should be inserted + prefix = "test_motion_trajectories.put_better_delete_worse_trajectories"; + double even_better_execution_time = better_execution_time - 100; + + check_and_emit(cache->put_trajectory(*move_group, g_robot_name, plan_req, different_traj, even_better_execution_time, + planning_time, true), + prefix, "Put better trajectory, delete_worse_trajectories, ok"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + + // Fetch better plan + prefix = "test_motion_trajectories.put_better_delete_worse_trajectories.fetch"; + + fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, plan_req, 0, 0); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, plan_req, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == different_traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + // Put different req, delete_worse_trajectories + // + // Unrelated trajectory requests should live alongside pre-existing plans + prefix = "test_motion_trajectories.put_different_req"; + + check_and_emit(cache->put_trajectory(*move_group, g_robot_name, different_plan_req, different_traj, + better_execution_time, planning_time, true), + prefix, "Put different trajectory req, delete_worse_trajectories, ok"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 2, prefix, "Two trajectories in cache"); + + // Fetch with different trajectory req + // + // With multiple trajectories in cache, fetches should be sorted accordingly + prefix = "test_motion_trajectories.put_different_req.fetch"; + + fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, different_plan_req, 0, 0); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, different_plan_req, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == different_traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + // Fetch different robot + // + // Since we didn't populate anything, we should expect empty + prefix = "test_motion_trajectories.different_robot.empty"; + std::string different_robot_name = "different_robot"; + + check_and_emit(cache->count_cartesian_trajectories(different_robot_name) == 0, prefix, "No trajectories in cache"); + + // Put first for different robot, delete_worse_trajectories + // + // A different robot's cache should not interact with the original cache + prefix = "test_motion_trajectories.different_robot.put_first"; + check_and_emit(cache->put_trajectory(*move_group, different_robot_name, different_plan_req, different_traj, + better_execution_time, planning_time, true), + prefix, "Put different trajectory req, delete_worse_trajectories, ok"); + + check_and_emit(cache->count_trajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); + + check_and_emit(cache->count_trajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 2, prefix, "Two trajectories in original robot's cache"); + + fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, different_plan_req, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all on original still returns one"); +} + +void test_cartesian_trajectories(std::shared_ptr move_group, std::shared_ptr cache) +{ + std::string prefix; + + /// First, test if construction even works... + + // Construct get cartesian trajectory request + prefix = "test_cartesian_trajectories.construct_get_cartesian_path_request"; + + int test_step = 1; + int test_jump = 2; + auto test_waypoints = get_dummy_waypoints(); + auto cartesian_plan_req_under_test = + cache->construct_get_cartesian_path_request(*move_group, test_waypoints, test_step, test_jump, false); + + check_and_emit(cartesian_plan_req_under_test.waypoints == test_waypoints && + static_cast(cartesian_plan_req_under_test.max_step) == test_step && + static_cast(cartesian_plan_req_under_test.jump_threshold) == test_jump && + !cartesian_plan_req_under_test.avoid_collisions, + prefix, "Ok"); + + // Setup ===================================================================== + // All variants are copies. + + /// GetCartesianPath::Request + + // Plain start + auto waypoints = get_dummy_waypoints(); + auto cartesian_plan_req = cache->construct_get_cartesian_path_request(*move_group, waypoints, 1, 1, false); + cartesian_plan_req.start_state.multi_dof_joint_state.joint_names.clear(); + cartesian_plan_req.start_state.multi_dof_joint_state.transforms.clear(); + cartesian_plan_req.start_state.multi_dof_joint_state.twist.clear(); + cartesian_plan_req.start_state.multi_dof_joint_state.wrench.clear(); + cartesian_plan_req.path_constraints.joint_constraints.clear(); + cartesian_plan_req.path_constraints.position_constraints.clear(); + cartesian_plan_req.path_constraints.orientation_constraints.clear(); + cartesian_plan_req.path_constraints.visibility_constraints.clear(); + + // Empty frame start + auto empty_frame_cartesian_plan_req = cartesian_plan_req; + empty_frame_cartesian_plan_req.header.frame_id = ""; + + // is_diff = true + auto is_diff_cartesian_plan_req = cartesian_plan_req; + is_diff_cartesian_plan_req.start_state.is_diff = true; + is_diff_cartesian_plan_req.start_state.joint_state.header.frame_id = ""; + is_diff_cartesian_plan_req.start_state.joint_state.name.clear(); + is_diff_cartesian_plan_req.start_state.joint_state.position.clear(); + is_diff_cartesian_plan_req.start_state.joint_state.velocity.clear(); + is_diff_cartesian_plan_req.start_state.joint_state.effort.clear(); + + // Something close enough (mod 0.1 away) + auto close_matching_cartesian_plan_req = cartesian_plan_req; + close_matching_cartesian_plan_req.start_state.joint_state.position.at(0) -= 0.05; + close_matching_cartesian_plan_req.start_state.joint_state.position.at(1) += 0.05; + close_matching_cartesian_plan_req.start_state.joint_state.position.at(2) -= 0.05; + close_matching_cartesian_plan_req.waypoints.at(0).position.x -= 0.05; + close_matching_cartesian_plan_req.waypoints.at(1).position.x += 0.05; + close_matching_cartesian_plan_req.waypoints.at(2).position.x -= 0.05; + + // Different + auto different_cartesian_plan_req = cartesian_plan_req; + different_cartesian_plan_req.start_state.joint_state.position.at(0) -= 1.05; + different_cartesian_plan_req.start_state.joint_state.position.at(1) += 2.05; + different_cartesian_plan_req.start_state.joint_state.position.at(2) -= 3.05; + different_cartesian_plan_req.waypoints.at(0).position.x -= 1.05; + different_cartesian_plan_req.waypoints.at(1).position.x += 2.05; + different_cartesian_plan_req.waypoints.at(2).position.x -= 3.05; + + /// RobotTrajectory + + // Trajectory + auto traj = get_dummy_panda_traj(); + + // Trajectory with no frame_id in its trajectory header + auto empty_frame_traj = traj; + empty_frame_traj.joint_trajectory.header.frame_id = ""; + + auto different_traj = traj; + different_traj.joint_trajectory.points.at(0).positions.at(0) = 999; + different_traj.joint_trajectory.points.at(0).positions.at(1) = 999; + different_traj.joint_trajectory.points.at(0).positions.at(2) = 999; + + // Checks ==================================================================== + + // Initially empty + prefix = "test_cartesian_trajectories.empty"; + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 0, prefix, "Trajectory cache initially empty"); + + check_and_emit( + cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, 0, 999, 999) + .empty(), + prefix, "Fetch all trajectories on empty cache returns empty"); + + check_and_emit(cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, 0, 999, + 999) == nullptr, + prefix, "Fetch best trajectory on empty cache returns nullptr"); + + // Put trajectory empty frame + // + // Trajectory must have frame in joint trajectory, expect put fail + prefix = "test_cartesian_trajectories.put_trajectory_empty_frame"; + double execution_time = 999; + double planning_time = 999; + double fraction = 0.5; + + check_and_emit(!cache->put_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, empty_frame_traj, + execution_time, planning_time, fraction, false), + prefix, "Put empty frame trajectory, no delete_worse_trajectories, not ok"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 0, prefix, "No trajectories in cache"); + + // Put trajectory req empty frame + // + // Trajectory request must have frame in workspace, expect put fail + prefix = "test_cartesian_trajectories.put_trajectory_req_empty_frame"; + execution_time = 999; + planning_time = 999; + + check_and_emit(!cache->put_cartesian_trajectory(*move_group, g_robot_name, empty_frame_cartesian_plan_req, traj, + execution_time, planning_time, fraction, false), + prefix, "Put empty frame req trajectory, no delete_worse_trajectories, not ok"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 0, prefix, "No trajectories in cache"); + + // Put first, no delete_worse_trajectories + prefix = "test_cartesian_trajectories.put_first"; + execution_time = 999; + planning_time = 999; + + check_and_emit(cache->put_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, traj, execution_time, + planning_time, fraction, false), + prefix, "Put first valid trajectory, no delete_worse_trajectories, ok"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + + // Fetch matching, no tolerance + // + // Exact key match should have cache hit + prefix = "test_cartesian_trajectories.put_first.fetch_matching_no_tolerance"; + + auto fetched_trajectories = + cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + auto fetched_traj = + cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + + // Fetch with is_diff + // + // is_diff key should be equivalent to exact match if robot state did not + // change, hence should have cache hit + prefix = "test_cartesian_trajectories.put_first.fetch_is_diff_no_tolerance"; + + fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, + is_diff_cartesian_plan_req, fraction, 0, 0); + + fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, is_diff_cartesian_plan_req, + fraction, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + + // Fetch non-matching, out of tolerance + // + // Non-matching key should not have cache hit + prefix = "test_cartesian_trajectories.put_first.fetch_non_matching_out_of_tolerance"; + + fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories( + *move_group, g_robot_name, close_matching_cartesian_plan_req, fraction, 0, 0); + + fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, + close_matching_cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, only start in tolerance (but not goal) + // + // Non-matching key should not have cache hit + prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_start_in_tolerance"; + + fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories( + *move_group, g_robot_name, close_matching_cartesian_plan_req, fraction, 999, 0); + + fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, + close_matching_cartesian_plan_req, fraction, 999, 0); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, only goal in tolerance (but not start) + // + // Non-matching key should not have cache hit + prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_goal_in_tolerance"; + + fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories( + *move_group, g_robot_name, close_matching_cartesian_plan_req, fraction, 0, 999); + + fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, + close_matching_cartesian_plan_req, fraction, 0, 999); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, in tolerance + // + // Close key within tolerance limit should have cache hit + prefix = "test_cartesian_trajectories.put_first.fetch_non_matching_in_tolerance"; + + fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories( + *move_group, g_robot_name, close_matching_cartesian_plan_req, fraction, 0.1, 0.1); + + fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, + close_matching_cartesian_plan_req, fraction, 0.1, 0.1); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + + // Fetch with higher fraction + // + // Matching trajectories with more restrictive fraction requirements should not + // pull up trajectories cached for less restrictive fraction requirements + prefix = "test_cartesian_trajectories.put_first.fetch_higher_fraction"; + + fetched_trajectories = + cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, 1, 999, 999); + + fetched_traj = + cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, 1, 999, 999); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + + // Fetch with lower fraction + // + // Matching trajectories with less restrictive fraction requirements should pull up + // trajectories cached for more restrictive fraction requirements + prefix = "test_cartesian_trajectories.put_first.fetch_lower_fraction"; + + fetched_trajectories = + cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, 0, 999, 999); + + fetched_traj = + cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, 0, 999, 999); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + + // Put worse, no delete_worse_trajectories + // + // Worse trajectories should not be inserted + prefix = "test_cartesian_trajectories.put_worse"; + double worse_execution_time = execution_time + 100; + + check_and_emit(!cache->put_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, traj, + worse_execution_time, planning_time, fraction, false), + prefix, "Put worse trajectory, no delete_worse_trajectories, not ok"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + + // Put better, no delete_worse_trajectories + // + // Better trajectories should be inserted + prefix = "test_cartesian_trajectories.put_better"; + double better_execution_time = execution_time - 100; + + check_and_emit(cache->put_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, traj, + better_execution_time, planning_time, fraction, false), + prefix, "Put better trajectory, no delete_worse_trajectories, ok"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 2, prefix, "Two trajectories in cache"); + + // Fetch sorted + // + // With multiple trajectories in cache, fetches should be sorted accordingly + prefix = "test_cartesian_trajectories.put_better.fetch_sorted"; + + fetched_trajectories = + cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + fetched_traj = + cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + + check_and_emit(fetched_trajectories.at(0)->lookupDouble("execution_time_s") == better_execution_time && + fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched trajectories are sorted correctly"); + + // Put better, delete_worse_trajectories + // + // Better, different, trajectories should be inserted + prefix = "test_cartesian_trajectories.put_better_delete_worse_trajectories"; + double even_better_execution_time = better_execution_time - 100; + + check_and_emit(cache->put_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, different_traj, + even_better_execution_time, planning_time, fraction, true), + prefix, "Put better trajectory, delete_worse_trajectories, ok"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + + // Fetch better plan + prefix = "test_cartesian_trajectories.put_better_delete_worse_trajectories.fetch"; + + fetched_trajectories = + cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + fetched_traj = + cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == different_traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + + // Put different req, delete_worse_trajectories + // + // Unrelated trajectory requests should live alongside pre-existing plans + prefix = "test_cartesian_trajectories.put_different_req"; + + check_and_emit(cache->put_cartesian_trajectory(*move_group, g_robot_name, different_cartesian_plan_req, + different_traj, better_execution_time, planning_time, fraction, true), + prefix, "Put different trajectory req, delete_worse_trajectories, ok"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 2, prefix, "Two trajectories in cache"); + + // Fetch with different trajectory req + // + // With multiple trajectories in cache, fetches should be sorted accordingly + prefix = "test_cartesian_trajectories.put_different_req.fetch"; + + fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, + different_cartesian_plan_req, fraction, 0, 0); + + fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, + different_cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == different_traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + + // Fetch different robot + // + // Since we didn't populate anything, we should expect empty + prefix = "test_cartesian_trajectories.different_robot.empty"; + std::string different_robot_name = "different_robot"; + + check_and_emit(cache->count_cartesian_trajectories(different_robot_name) == 0, prefix, "No trajectories in cache"); + + // Put first for different robot, delete_worse_trajectories + // + // A different robot's cache should not interact with the original cache + prefix = "test_cartesian_trajectories.different_robot.put_first"; + check_and_emit(cache->put_cartesian_trajectory(*move_group, different_robot_name, different_cartesian_plan_req, + different_traj, better_execution_time, planning_time, fraction, true), + prefix, "Put different trajectory req, delete_worse_trajectories, ok"); + + check_and_emit(cache->count_cartesian_trajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); + + check_and_emit(cache->count_cartesian_trajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 2, prefix, + "Two trajectories in original robot's cache"); + + fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, + different_cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all on original still returns one"); +} + +int main(int argc, char** argv) +{ + // Setup + rclcpp::init(argc, argv); + + rclcpp::NodeOptions test_node_options; + test_node_options.automatically_declare_parameters_from_overrides(true); + test_node_options.arguments({"--ros-args", "-r", "__node:=test_node"}); + + + rclcpp::NodeOptions move_group_node_options; + move_group_node_options.automatically_declare_parameters_from_overrides(true); + move_group_node_options.arguments({"--ros-args", "-r", "__node:=move_group_node"}); + + auto test_node = rclcpp::Node::make_shared("test_node", test_node_options); + auto move_group_node = rclcpp::Node::make_shared("move_group_node", move_group_node_options); + + std::atomic running = true; + + std::thread spin_thread([&]() { + while (rclcpp::ok() && running) + { + rclcpp::spin_some(test_node); + rclcpp::spin_some(move_group_node); + } + }); + + // This is necessary + test_node->declare_parameter("warehouse_plugin", "warehouse_ros_sqlite::DatabaseConnection"); + + // Test proper + { + auto cache = std::make_shared(test_node); + check_and_emit(cache->init(":memory:", 0, 1e-4), "init", "Cache init"); + + auto move_group = std::make_shared(move_group_node, "panda_arm"); + + auto curr_state = move_group->getCurrentState(60); + move_group->setStartState(*curr_state); + + test_motion_trajectories(move_group, cache); + test_cartesian_trajectories(move_group, cache); + } + + running = false; + spin_thread.join(); + + test_node.reset(); + move_group_node.reset(); + + rclcpp::shutdown(); + return 0; +} diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp new file mode 100644 index 0000000000..9127604c21 --- /dev/null +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -0,0 +1,1340 @@ +// Copyright 2022 Johnson & Johnson +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "moveit/robot_state/conversions.h" +#include "moveit/robot_state/robot_state.h" +#include "moveit/warehouse/moveit_message_storage.h" +#include "warehouse_ros/message_collection.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logging.hpp" +#include "trajectory_cache.hpp" + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +using warehouse_ros::MessageWithMetadata; +using warehouse_ros::Metadata; +using warehouse_ros::Query; + +// Utils ======================================================================= + +// Append a range inclusive query with some tolerance about some center value. +void query_append_range_inclusive_with_tolerance(Query& query, const std::string& name, double center, double tolerance) +{ + query.appendRangeInclusive(name, center - tolerance / 2, center + tolerance / 2); +} + +// Sort constraint components by joint or link name. +void sort_constraints(std::vector& joint_constraints, + std::vector& position_constraints, + std::vector& orientation_constraints) +{ + std::sort(joint_constraints.begin(), joint_constraints.end(), + [](const moveit_msgs::msg::JointConstraint& l, const moveit_msgs::msg::JointConstraint& r) { + return l.joint_name < r.joint_name; + }); + + std::sort(position_constraints.begin(), position_constraints.end(), + [](const moveit_msgs::msg::PositionConstraint& l, const moveit_msgs::msg::PositionConstraint& r) { + return l.link_name < r.link_name; + }); + + std::sort(orientation_constraints.begin(), orientation_constraints.end(), + [](const moveit_msgs::msg::OrientationConstraint& l, const moveit_msgs::msg::OrientationConstraint& r) { + return l.link_name < r.link_name; + }); +} + +// Trajectory Cache ============================================================ + +TrajectoryCache::TrajectoryCache(const rclcpp::Node::SharedPtr& node) : node_(node) +{ + tf_buffer_ = std::make_unique(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); +} + +bool TrajectoryCache::init(const std::string& db_path, uint32_t db_port, double exact_match_precision) +{ + RCLCPP_INFO(node_->get_logger(), "Opening trajectory cache database at: %s (Port: %d, Precision: %f)", + db_path.c_str(), db_port, exact_match_precision); + + // If the `warehouse_plugin` parameter isn't set, defaults to warehouse_ros' + // default. + db_ = moveit_warehouse::loadDatabase(node_); + + exact_match_precision_ = exact_match_precision; + db_->setParams(db_path, db_port); + return db_->connect(); +} + +unsigned TrajectoryCache::count_trajectories(const std::string& move_group_namespace) +{ + auto coll = + db_->openCollection("move_group_trajectory_cache", move_group_namespace); + return coll.count(); +} + +unsigned TrajectoryCache::count_cartesian_trajectories(const std::string& move_group_namespace) +{ + auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", + move_group_namespace); + return coll.count(); +} + +// ============================================================================= +// MOTION PLAN TRAJECTORY CACHING +// ============================================================================= +// MOTION PLAN TRAJECTORY CACHING: TOP LEVEL OPS +std::vector::ConstPtr> +TrajectoryCache::fetch_all_matching_trajectories(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double start_tolerance, double goal_tolerance, bool metadata_only, + const std::string& sort_by) +{ + auto coll = + db_->openCollection("move_group_trajectory_cache", move_group_namespace); + + Query::Ptr query = coll.createQuery(); + + bool start_ok = this->extract_and_append_trajectory_start_to_query(*query, move_group, plan_request, start_tolerance); + bool goal_ok = this->extract_and_append_trajectory_goal_to_query(*query, move_group, plan_request, goal_tolerance); + + if (!start_ok || !goal_ok) + { + RCLCPP_ERROR(node_->get_logger(), "Could not construct trajectory query."); + return {}; + } + + return coll.queryList(query, metadata_only, sort_by, true); +} + +MessageWithMetadata::ConstPtr TrajectoryCache::fetch_best_matching_trajectory( + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, + bool metadata_only, const std::string& sort_by) +{ + // First find all matching, but metadata only. + // Then use the ID metadata of the best plan to pull the actual message. + auto matching_trajectories = this->fetch_all_matching_trajectories(move_group, move_group_namespace, plan_request, + start_tolerance, goal_tolerance, true, sort_by); + + if (matching_trajectories.empty()) + { + RCLCPP_INFO(node_->get_logger(), "No matching trajectories found."); + return nullptr; + } + + auto coll = + db_->openCollection("move_group_trajectory_cache", move_group_namespace); + + // Best plan is at first index, since the lookup query was sorted by + // execution_time. + int best_trajectory_id = matching_trajectories.at(0)->lookupInt("id"); + Query::Ptr best_query = coll.createQuery(); + best_query->append("id", best_trajectory_id); + + return coll.findOne(best_query, metadata_only); +} + +bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, + double planning_time_s, bool delete_worse_trajectories) +{ + // Check pre-conditions + if (!trajectory.multi_dof_joint_trajectory.points.empty()) + { + RCLCPP_ERROR(node_->get_logger(), "Skipping plan insert: Multi-DOF trajectory plans are not supported."); + return false; + } + if (plan_request.workspace_parameters.header.frame_id.empty() || trajectory.joint_trajectory.header.frame_id.empty()) + { + RCLCPP_ERROR(node_->get_logger(), "Skipping plan insert: Frame IDs cannot be empty."); + return false; + } + if (plan_request.workspace_parameters.header.frame_id != trajectory.joint_trajectory.header.frame_id) + { + RCLCPP_ERROR(node_->get_logger(), + "Skipping plan insert: " + "Plan request frame (%s) does not match plan frame (%s).", + plan_request.workspace_parameters.header.frame_id.c_str(), + trajectory.joint_trajectory.header.frame_id.c_str()); + return false; + } + + auto coll = + db_->openCollection("move_group_trajectory_cache", move_group_namespace); + + // Pull out trajectories "exactly" keyed by request in cache. + Query::Ptr exact_query = coll.createQuery(); + + bool start_query_ok = this->extract_and_append_trajectory_start_to_query(*exact_query, move_group, plan_request, 0); + bool goal_query_ok = this->extract_and_append_trajectory_goal_to_query(*exact_query, move_group, plan_request, 0); + + if (!start_query_ok || !goal_query_ok) + { + RCLCPP_ERROR(node_->get_logger(), "Skipping plan insert: Could not construct lookup query."); + return false; + } + + auto exact_matches = coll.queryList(exact_query, /* metadata_only */ true, /* sort_by */ "execution_time_s"); + + double best_execution_time = std::numeric_limits::infinity(); + if (!exact_matches.empty()) + { + best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); + + if (delete_worse_trajectories) + { + for (auto& match : exact_matches) + { + double match_execution_time_s = match->lookupDouble("execution_time_s"); + if (execution_time_s < match_execution_time_s) + { + int delete_id = match->lookupInt("id"); + RCLCPP_INFO(node_->get_logger(), + "Overwriting plan (id: %d): " + "execution_time (%es) > new trajectory's execution_time (%es)", + delete_id, match_execution_time_s, execution_time_s); + + Query::Ptr delete_query = coll.createQuery(); + delete_query->append("id", delete_id); + coll.removeMessages(delete_query); + } + } + } + } + + // Insert if candidate is best seen. + if (execution_time_s < best_execution_time) + { + Metadata::Ptr insert_metadata = coll.createMetadata(); + + bool start_meta_ok = + this->extract_and_append_trajectory_start_to_metadata(*insert_metadata, move_group, plan_request); + bool goal_meta_ok = + this->extract_and_append_trajectory_goal_to_metadata(*insert_metadata, move_group, plan_request); + insert_metadata->append("execution_time_s", execution_time_s); + insert_metadata->append("planning_time_s", planning_time_s); + + if (!start_meta_ok || !goal_meta_ok) + { + RCLCPP_ERROR(node_->get_logger(), "Skipping plan insert: Could not construct insert metadata."); + return false; + } + + RCLCPP_INFO(node_->get_logger(), + "Inserting trajectory: New trajectory execution_time (%es) " + "is better than best trajectory's execution_time (%es)", + execution_time_s, best_execution_time); + + coll.insert(trajectory, insert_metadata); + return true; + } + + RCLCPP_INFO(node_->get_logger(), + "Skipping plan insert: New trajectory execution_time (%es) " + "is worse than best trajectory's execution_time (%es)", + execution_time_s, best_execution_time); + return false; +} + +// MOTION PLAN TRAJECTORY CACHING: QUERY CONSTRUCTION +bool TrajectoryCache::extract_and_append_trajectory_start_to_query( + Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) +{ + match_tolerance += exact_match_precision_; + + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); + } + + // auto original = *query; // Copy not supported. + + query.append("group_name", plan_request.group_name); + + // Workspace params + // Match anything within our specified workspace limits. + query.append("workspace_parameters.header.frame_id", plan_request.workspace_parameters.header.frame_id); + query.appendGTE("workspace_parameters.min_corner.x", plan_request.workspace_parameters.min_corner.x); + query.appendGTE("workspace_parameters.min_corner.y", plan_request.workspace_parameters.min_corner.y); + query.appendGTE("workspace_parameters.min_corner.z", plan_request.workspace_parameters.min_corner.z); + query.appendLTE("workspace_parameters.max_corner.x", plan_request.workspace_parameters.max_corner.x); + query.appendLTE("workspace_parameters.max_corner.y", plan_request.workspace_parameters.max_corner.y); + query.appendLTE("workspace_parameters.max_corner.z", plan_request.workspace_parameters.max_corner.z); + + // Joint state + // Only accounts for joint_state position. Ignores velocity and effort. + if (plan_request.start_state.is_diff) + { + // If plan request states that the start_state is_diff, then we need to get + // the current state from the move_group. + + // NOTE: methyldragon - + // I think if is_diff is on, the joint states will not be populated in all + // of our motion plan requests? If this isn't the case we might need to + // apply the joint states as offsets as well. + // + // TODO: Since MoveIt also potentially does another getCurrentState() call + // when planning, there is a chance that the current state in the cache + // differs from the state used in MoveIt's plan. + // + // When upstreaming this class to MoveIt, this issue should go away once + // the class is used within the move group's Plan call. + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); + if (!current_state) + { + RCLCPP_WARN(node_->get_logger(), "Skipping start query append: Could not get robot state."); + // *query = original; // Undo our changes. (Can't. Copy not supported.) + return false; + } + + moveit_msgs::msg::RobotState current_state_msg; + robotStateToRobotStateMsg(*current_state, current_state_msg); + + for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) + { + query.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); + query_append_range_inclusive_with_tolerance(query, "start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i), match_tolerance); + } + } + else + { + for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) + { + query.append("start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); + query_append_range_inclusive_with_tolerance(query, "start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i), match_tolerance); + } + } + return true; +} + +bool TrajectoryCache::extract_and_append_trajectory_goal_to_query( + Query& query, const moveit::planning_interface::MoveGroupInterface& /* move_group */, + const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) +{ + match_tolerance += exact_match_precision_; + + // Make ignored members explicit + bool emit_position_constraint_warning = false; + for (auto& constraint : plan_request.goal_constraints) + { + for (auto& position_constraint : constraint.position_constraints) + { + if (!position_constraint.constraint_region.primitives.empty()) + { + emit_position_constraint_warning = true; + break; + } + } + if (emit_position_constraint_warning) + { + break; + } + } + if (emit_position_constraint_warning) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring goal_constraints.position_constraints.constraint_region: " + "Not supported."); + } + + // auto original = *query; // Copy not supported. + + query_append_range_inclusive_with_tolerance(query, "max_velocity_scaling_factor", + plan_request.max_velocity_scaling_factor, match_tolerance); + query_append_range_inclusive_with_tolerance(query, "max_acceleration_scaling_factor", + plan_request.max_acceleration_scaling_factor, match_tolerance); + query_append_range_inclusive_with_tolerance(query, "max_cartesian_speed", plan_request.max_cartesian_speed, + match_tolerance); + + // Extract constraints (so we don't have cardinality on goal_constraint idx.) + std::vector joint_constraints; + std::vector position_constraints; + std::vector orientation_constraints; + for (auto& constraint : plan_request.goal_constraints) + { + for (auto& joint_constraint : constraint.joint_constraints) + { + joint_constraints.push_back(joint_constraint); + } + for (auto& position_constraint : constraint.position_constraints) + { + position_constraints.push_back(position_constraint); + } + for (auto& orientation_constraint : constraint.orientation_constraints) + { + orientation_constraints.push_back(orientation_constraint); + } + + // Also sort for even less cardinality. + sort_constraints(joint_constraints, position_constraints, orientation_constraints); + } + + // Joint constraints + size_t joint_idx = 0; + for (auto& constraint : joint_constraints) + { + std::string meta_name = "goal_constraints.joint_constraints_" + std::to_string(joint_idx++); + + query.append(meta_name + ".joint_name", constraint.joint_name); + query_append_range_inclusive_with_tolerance(query, meta_name + ".position", constraint.position, match_tolerance); + query.appendGTE(meta_name + ".tolerance_above", constraint.tolerance_above); + query.appendLTE(meta_name + ".tolerance_below", constraint.tolerance_below); + } + + // Position constraints + // All offsets will be "frozen" and computed wrt. the workspace frame + // instead. + if (!position_constraints.empty()) + { + query.append("goal_constraints.position_constraints.header.frame_id", + plan_request.workspace_parameters.header.frame_id); + + size_t pos_idx = 0; + for (auto& constraint : position_constraints) + { + std::string meta_name = "goal_constraints.position_constraints_" + std::to_string(pos_idx++); + + // Compute offsets wrt. to workspace frame. + double x_offset = 0; + double y_offset = 0; + double z_offset = 0; + + if (plan_request.workspace_parameters.header.frame_id != constraint.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform( + constraint.header.frame_id, plan_request.workspace_parameters.header.frame_id, tf2::TimePointZero); + + x_offset = transform.transform.translation.x; + y_offset = transform.transform.translation.y; + z_offset = transform.transform.translation.z; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(node_->get_logger(), + "Skipping goal query append: " + "Could not get goal transform for translation %s to %s: %s", + plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), + ex.what()); + + // (Can't. Copy not supported.) + // *query = original; // Undo our changes. + return false; + } + } + + query.append(meta_name + ".link_name", constraint.link_name); + + query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.x", + x_offset + constraint.target_point_offset.x, match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.y", + y_offset + constraint.target_point_offset.y, match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.z", + z_offset + constraint.target_point_offset.z, match_tolerance); + } + } + + // Orientation constraints + // All offsets will be "frozen" and computed wrt. the workspace frame + // instead. + if (!orientation_constraints.empty()) + { + query.append("goal_constraints.orientation_constraints.header.frame_id", + plan_request.workspace_parameters.header.frame_id); + + size_t ori_idx = 0; + for (auto& constraint : orientation_constraints) + { + std::string meta_name = "goal_constraints.orientation_constraints_" + std::to_string(ori_idx++); + + // Compute offsets wrt. to workspace frame. + geometry_msgs::msg::Quaternion quat_offset; + quat_offset.x = 0; + quat_offset.y = 0; + quat_offset.z = 0; + quat_offset.w = 1; + + if (plan_request.workspace_parameters.header.frame_id != constraint.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform( + constraint.header.frame_id, plan_request.workspace_parameters.header.frame_id, tf2::TimePointZero); + + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(node_->get_logger(), + "Skipping goal query append: " + "Could not get goal transform for orientation %s to %s: %s", + plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), + ex.what()); + + // (Can't. Copy not supported.) + // *query = original; // Undo our changes. + return false; + } + } + + query.append(meta_name + ".link_name", constraint.link_name); + + // Orientation of constraint frame wrt. workspace frame + tf2::Quaternion tf2_quat_frame_offset(quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); + + // Added offset on top of the constraint frame's orientation stated in + // the constraint. + tf2::Quaternion tf2_quat_goal_offset(constraint.orientation.x, constraint.orientation.y, constraint.orientation.z, + constraint.orientation.w); + + tf2_quat_frame_offset.normalize(); + tf2_quat_goal_offset.normalize(); + + auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; + final_quat.normalize(); + + query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.x", final_quat.getX(), + match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.y", final_quat.getY(), + match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.z", final_quat.getZ(), + match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.w", final_quat.getW(), + match_tolerance); + } + } + + return true; +} + +// MOTION PLAN TRAJECTORY CACHING: METADATA CONSTRUCTION +bool TrajectoryCache::extract_and_append_trajectory_start_to_metadata( + Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request) +{ + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + metadata.append("group_name", plan_request.group_name); + + // Workspace params + metadata.append("workspace_parameters.header.frame_id", plan_request.workspace_parameters.header.frame_id); + metadata.append("workspace_parameters.min_corner.x", plan_request.workspace_parameters.min_corner.x); + metadata.append("workspace_parameters.min_corner.y", plan_request.workspace_parameters.min_corner.y); + metadata.append("workspace_parameters.min_corner.z", plan_request.workspace_parameters.min_corner.z); + metadata.append("workspace_parameters.max_corner.x", plan_request.workspace_parameters.max_corner.x); + metadata.append("workspace_parameters.max_corner.y", plan_request.workspace_parameters.max_corner.y); + metadata.append("workspace_parameters.max_corner.z", plan_request.workspace_parameters.max_corner.z); + + // Joint state + // Only accounts for joint_state position. Ignores velocity and effort. + if (plan_request.start_state.is_diff) + { + // If plan request states that the start_state is_diff, then we need to get + // the current state from the move_group. + + // NOTE: methyldragon - + // I think if is_diff is on, the joint states will not be populated in all + // of our motion plan requests? If this isn't the case we might need to + // apply the joint states as offsets as well. + // + // TODO: Since MoveIt also potentially does another getCurrentState() call + // when planning, there is a chance that the current state in the cache + // differs from the state used in MoveIt's plan. + // + // When upstreaming this class to MoveIt, this issue should go away once + // the class is used within the move group's Plan call. + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); + if (!current_state) + { + RCLCPP_WARN(node_->get_logger(), "Skipping start metadata append: Could not get robot state."); + // *metadata = original; // Undo our changes. // Copy not supported + return false; + } + + moveit_msgs::msg::RobotState current_state_msg; + robotStateToRobotStateMsg(*current_state, current_state_msg); + + for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) + { + metadata.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); + metadata.append("start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i)); + } + } + else + { + for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) + { + metadata.append("start_state.joint_state.name_" + std::to_string(i), + plan_request.start_state.joint_state.name.at(i)); + metadata.append("start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i)); + } + } + + return true; +} + +bool TrajectoryCache::extract_and_append_trajectory_goal_to_metadata( + Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& /* move_group */, + const moveit_msgs::msg::MotionPlanRequest& plan_request) +{ + // Make ignored members explicit + bool emit_position_constraint_warning = false; + for (auto& constraint : plan_request.goal_constraints) + { + for (auto& position_constraint : constraint.position_constraints) + { + if (!position_constraint.constraint_region.primitives.empty()) + { + emit_position_constraint_warning = true; + break; + } + } + if (emit_position_constraint_warning) + { + break; + } + } + if (emit_position_constraint_warning) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring goal_constraints.position_constraints.constraint_region: " + "Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + metadata.append("max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor); + metadata.append("max_acceleration_scaling_factor", plan_request.max_acceleration_scaling_factor); + metadata.append("max_cartesian_speed", plan_request.max_cartesian_speed); + + // Extract constraints (so we don't have cardinality on goal_constraint idx.) + std::vector joint_constraints; + std::vector position_constraints; + std::vector orientation_constraints; + for (auto& constraint : plan_request.goal_constraints) + { + for (auto& joint_constraint : constraint.joint_constraints) + { + joint_constraints.push_back(joint_constraint); + } + for (auto& position_constraint : constraint.position_constraints) + { + position_constraints.push_back(position_constraint); + } + for (auto& orientation_constraint : constraint.orientation_constraints) + { + orientation_constraints.push_back(orientation_constraint); + } + + // Also sort for even less cardinality. + sort_constraints(joint_constraints, position_constraints, orientation_constraints); + } + + // Joint constraints + size_t joint_idx = 0; + for (auto& constraint : joint_constraints) + { + std::string meta_name = "goal_constraints.joint_constraints_" + std::to_string(joint_idx++); + + metadata.append(meta_name + ".joint_name", constraint.joint_name); + metadata.append(meta_name + ".position", constraint.position); + metadata.append(meta_name + ".tolerance_above", constraint.tolerance_above); + metadata.append(meta_name + ".tolerance_below", constraint.tolerance_below); + } + + // Position constraints + if (!position_constraints.empty()) + { + // All offsets will be "frozen" and computed wrt. the workspace frame + // instead. + metadata.append("goal_constraints.position_constraints.header.frame_id", + plan_request.workspace_parameters.header.frame_id); + + size_t position_idx = 0; + for (auto& constraint : position_constraints) + { + std::string meta_name = "goal_constraints.position_constraints_" + std::to_string(position_idx++); + + // Compute offsets wrt. to workspace frame. + double x_offset = 0; + double y_offset = 0; + double z_offset = 0; + + if (plan_request.workspace_parameters.header.frame_id != constraint.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform( + constraint.header.frame_id, plan_request.workspace_parameters.header.frame_id, tf2::TimePointZero); + + x_offset = transform.transform.translation.x; + y_offset = transform.transform.translation.y; + z_offset = transform.transform.translation.z; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(node_->get_logger(), + "Skipping goal metadata append: " + "Could not get goal transform for translation %s to %s: %s", + plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), + ex.what()); + + // (Can't. Copy not supported.) + // *metadata = original; // Undo our changes. + return false; + } + } + + metadata.append(meta_name + ".link_name", constraint.link_name); + + metadata.append(meta_name + ".target_point_offset.x", x_offset + constraint.target_point_offset.x); + metadata.append(meta_name + ".target_point_offset.y", y_offset + constraint.target_point_offset.y); + metadata.append(meta_name + ".target_point_offset.z", z_offset + constraint.target_point_offset.z); + } + } + + // Orientation constraints + if (!orientation_constraints.empty()) + { + // All offsets will be "frozen" and computed wrt. the workspace frame + // instead. + metadata.append("goal_constraints.orientation_constraints.header.frame_id", + plan_request.workspace_parameters.header.frame_id); + + size_t ori_idx = 0; + for (auto& constraint : orientation_constraints) + { + std::string meta_name = "goal_constraints.orientation_constraints_" + std::to_string(ori_idx++); + + // Compute offsets wrt. to workspace frame. + geometry_msgs::msg::Quaternion quat_offset; + quat_offset.x = 0; + quat_offset.y = 0; + quat_offset.z = 0; + quat_offset.w = 1; + + if (plan_request.workspace_parameters.header.frame_id != constraint.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform( + constraint.header.frame_id, plan_request.workspace_parameters.header.frame_id, tf2::TimePointZero); + + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(node_->get_logger(), + "Skipping goal metadata append: " + "Could not get goal transform for orientation %s to %s: %s", + plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), + ex.what()); + + // (Can't. Copy not supported.) + // *metadata = original; // Undo our changes. + return false; + } + } + + metadata.append(meta_name + ".link_name", constraint.link_name); + + // Orientation of constraint frame wrt. workspace frame + tf2::Quaternion tf2_quat_frame_offset(quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); + + // Added offset on top of the constraint frame's orientation stated in + // the constraint. + tf2::Quaternion tf2_quat_goal_offset(constraint.orientation.x, constraint.orientation.y, constraint.orientation.z, + constraint.orientation.w); + + tf2_quat_frame_offset.normalize(); + tf2_quat_goal_offset.normalize(); + + auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; + final_quat.normalize(); + + metadata.append(meta_name + ".target_point_offset.x", final_quat.getX()); + metadata.append(meta_name + ".target_point_offset.y", final_quat.getY()); + metadata.append(meta_name + ".target_point_offset.z", final_quat.getZ()); + metadata.append(meta_name + ".target_point_offset.w", final_quat.getW()); + } + } + + return true; +} + +// ============================================================================= +// CARTESIAN TRAJECTORY CACHING +// ============================================================================= +// CARTESIAN TRAJECTORY CACHING: TOP LEVEL OPS +moveit_msgs::srv::GetCartesianPath::Request +TrajectoryCache::construct_get_cartesian_path_request(moveit::planning_interface::MoveGroupInterface& move_group, + const std::vector& waypoints, + double step, double jump_threshold, bool avoid_collisions) +{ + moveit_msgs::srv::GetCartesianPath::Request out; + + // Some of these parameters need us to pull PRIVATE values out of the + // move_group elsewhere... Yes, it is very cursed and I hate it. + // Fixing it requires fixing it in MoveIt. + moveit_msgs::msg::MotionPlanRequest tmp; + move_group.constructMotionPlanRequest(tmp); + + out.start_state = std::move(tmp.start_state); + out.group_name = std::move(tmp.group_name); + out.max_velocity_scaling_factor = tmp.max_velocity_scaling_factor; + out.max_acceleration_scaling_factor = tmp.max_acceleration_scaling_factor; + + out.header.frame_id = move_group.getPoseReferenceFrame(); + out.waypoints = waypoints; + out.max_step = step; + out.jump_threshold = jump_threshold; + out.path_constraints = moveit_msgs::msg::Constraints(); + out.avoid_collisions = avoid_collisions; + out.link_name = move_group.getEndEffectorLink(); + + // We were already cursed before, now we are double cursed... + // move_group.getNodeHandle() is UNIMPLEMENTED upstream. + out.header.stamp = node_->now(); + + return out; +} + +std::vector::ConstPtr> +TrajectoryCache::fetch_all_matching_cartesian_trajectories( + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, + double goal_tolerance, bool metadata_only, const std::string& sort_by) +{ + auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", + move_group_namespace); + + Query::Ptr query = coll.createQuery(); + + bool start_ok = + this->extract_and_append_cartesian_trajectory_start_to_query(*query, move_group, plan_request, start_tolerance); + bool goal_ok = + this->extract_and_append_cartesian_trajectory_goal_to_query(*query, move_group, plan_request, goal_tolerance); + + if (!start_ok || !goal_ok) + { + RCLCPP_ERROR(node_->get_logger(), "Could not construct cartesian trajectory query."); + return {}; + } + + query->appendGTE("fraction", min_fraction); + return coll.queryList(query, metadata_only, sort_by, true); +} + +MessageWithMetadata::ConstPtr +TrajectoryCache::fetch_best_matching_cartesian_trajectory( + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, + double goal_tolerance, bool metadata_only, const std::string& sort_by) +{ + // First find all matching, but metadata only. + // Then use the ID metadata of the best plan to pull the actual message. + auto matching_trajectories = this->fetch_all_matching_cartesian_trajectories( + move_group, move_group_namespace, plan_request, min_fraction, start_tolerance, goal_tolerance, true, sort_by); + + if (matching_trajectories.empty()) + { + RCLCPP_INFO(node_->get_logger(), "No matching cartesian trajectories found."); + return nullptr; + } + + auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", + move_group_namespace); + + // Best plan is at first index, since the lookup query was sorted by + // execution_time. + int best_trajectory_id = matching_trajectories.at(0)->lookupInt("id"); + Query::Ptr best_query = coll.createQuery(); + best_query->append("id", best_trajectory_id); + + return coll.findOne(best_query, metadata_only); +} + +bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + const moveit_msgs::msg::RobotTrajectory& trajectory, + double execution_time_s, double planning_time_s, double fraction, + bool delete_worse_trajectories) +{ + // Check pre-conditions + if (!trajectory.multi_dof_joint_trajectory.points.empty()) + { + RCLCPP_ERROR(node_->get_logger(), "Skipping cartesian trajectory insert: " + "Multi-DOF trajectory plans are not supported."); + return false; + } + if (plan_request.header.frame_id.empty() || trajectory.joint_trajectory.header.frame_id.empty()) + { + RCLCPP_ERROR(node_->get_logger(), "Skipping cartesian trajectory insert: Frame IDs cannot be empty."); + return false; + } + + auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", + move_group_namespace); + + // Pull out trajectories "exactly" keyed by request in cache. + Query::Ptr exact_query = coll.createQuery(); + + bool start_query_ok = + this->extract_and_append_cartesian_trajectory_start_to_query(*exact_query, move_group, plan_request, 0); + bool goal_query_ok = + this->extract_and_append_cartesian_trajectory_goal_to_query(*exact_query, move_group, plan_request, 0); + exact_query->append("fraction", fraction); + + if (!start_query_ok || !goal_query_ok) + { + RCLCPP_ERROR(node_->get_logger(), "Skipping cartesian trajectory insert: Could not construct lookup query."); + return false; + } + + auto exact_matches = coll.queryList(exact_query, /* metadata_only */ true, /* sort_by */ "execution_time_s"); + double best_execution_time = std::numeric_limits::infinity(); + if (!exact_matches.empty()) + { + best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); + + if (delete_worse_trajectories) + { + for (auto& match : exact_matches) + { + double match_execution_time_s = match->lookupDouble("execution_time_s"); + if (execution_time_s < match_execution_time_s) + { + int delete_id = match->lookupInt("id"); + RCLCPP_INFO(node_->get_logger(), + "Overwriting cartesian trajectory (id: %d): " + "execution_time (%es) > new trajectory's execution_time (%es)", + delete_id, match_execution_time_s, execution_time_s); + + Query::Ptr delete_query = coll.createQuery(); + delete_query->append("id", delete_id); + coll.removeMessages(delete_query); + } + } + } + } + + // Insert if candidate is best seen. + if (execution_time_s < best_execution_time) + { + Metadata::Ptr insert_metadata = coll.createMetadata(); + + bool start_meta_ok = + this->extract_and_append_cartesian_trajectory_start_to_metadata(*insert_metadata, move_group, plan_request); + bool goal_meta_ok = + this->extract_and_append_cartesian_trajectory_goal_to_metadata(*insert_metadata, move_group, plan_request); + insert_metadata->append("execution_time_s", execution_time_s); + insert_metadata->append("planning_time_s", planning_time_s); + insert_metadata->append("fraction", fraction); + + if (!start_meta_ok || !goal_meta_ok) + { + RCLCPP_ERROR(node_->get_logger(), "Skipping cartesian trajectory insert: " + "Could not construct insert metadata."); + return false; + } + + RCLCPP_INFO(node_->get_logger(), + "Inserting cartesian trajectory: New trajectory execution_time (%es) " + "is better than best trajectory's execution_time (%es) at fraction (%es)", + execution_time_s, best_execution_time, fraction); + + coll.insert(trajectory, insert_metadata); + return true; + } + + RCLCPP_INFO(node_->get_logger(), + "Skipping cartesian trajectory insert: New trajectory execution_time (%es) " + "is worse than best trajectory's execution_time (%es) at fraction (%es)", + execution_time_s, best_execution_time, fraction); + return false; +} + +// CARTESIAN TRAJECTORY CACHING: QUERY CONSTRUCTION +bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_query( + Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) +{ + match_tolerance += exact_match_precision_; + + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + query.append("group_name", plan_request.group_name); + + // Joint state + // Only accounts for joint_state position. Ignores velocity and effort. + if (plan_request.start_state.is_diff) + { + // If plan request states that the start_state is_diff, then we need to get + // the current state from the move_group. + + // NOTE: methyldragon - + // I think if is_diff is on, the joint states will not be populated in all + // of our motion plan requests? If this isn't the case we might need to + // apply the joint states as offsets as well. + // + // TODO: Since MoveIt also potentially does another getCurrentState() call + // when planning, there is a chance that the current state in the cache + // differs from the state used in MoveIt's plan. + // + // When upstreaming this class to MoveIt, this issue should go away once + // the class is used within the move group's Plan call. + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); + if (!current_state) + { + RCLCPP_WARN(node_->get_logger(), "Skipping start metadata append: Could not get robot state."); + // *metadata = original; // Undo our changes. // Copy not supported + return false; + } + + moveit_msgs::msg::RobotState current_state_msg; + robotStateToRobotStateMsg(*current_state, current_state_msg); + + for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) + { + query.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); + query_append_range_inclusive_with_tolerance(query, "start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i), match_tolerance); + } + } + else + { + for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) + { + query.append("start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); + query_append_range_inclusive_with_tolerance(query, "start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i), match_tolerance); + } + } + + return true; +} + +bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_query( + Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) +{ + match_tolerance += exact_match_precision_; + + // Make ignored members explicit + if (!plan_request.path_constraints.joint_constraints.empty() || + !plan_request.path_constraints.position_constraints.empty() || + !plan_request.path_constraints.orientation_constraints.empty() || + !plan_request.path_constraints.visibility_constraints.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring path_constraints: Not supported."); + } + if (plan_request.avoid_collisions) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring avoid_collisions: Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + query_append_range_inclusive_with_tolerance(query, "max_velocity_scaling_factor", + plan_request.max_velocity_scaling_factor, match_tolerance); + query_append_range_inclusive_with_tolerance(query, "max_acceleration_scaling_factor", + plan_request.max_acceleration_scaling_factor, match_tolerance); + query_append_range_inclusive_with_tolerance(query, "max_step", plan_request.max_step, match_tolerance); + query_append_range_inclusive_with_tolerance(query, "jump_threshold", plan_request.jump_threshold, match_tolerance); + + // Waypoints + // Restating them in terms of the robot model frame (usually base_link) + std::string base_frame = move_group.getRobotModel()->getModelFrame(); + + // Compute offsets. + double x_offset = 0; + double y_offset = 0; + double z_offset = 0; + + geometry_msgs::msg::Quaternion quat_offset; + quat_offset.x = 0; + quat_offset.y = 0; + quat_offset.z = 0; + quat_offset.w = 1; + + if (base_frame != plan_request.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform(plan_request.header.frame_id, base_frame, tf2::TimePointZero); + + x_offset = transform.transform.translation.x; + y_offset = transform.transform.translation.y; + z_offset = transform.transform.translation.z; + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(node_->get_logger(), + "Skipping goal metadata append: " + "Could not get goal transform for %s to %s: %s", + base_frame.c_str(), plan_request.header.frame_id.c_str(), ex.what()); + + // (Can't. Copy not supported.) + // *metadata = original; // Undo our changes. + return false; + } + } + + tf2::Quaternion tf2_quat_frame_offset(quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); + tf2_quat_frame_offset.normalize(); + + size_t waypoint_idx = 0; + for (auto& waypoint : plan_request.waypoints) + { + std::string meta_name = "waypoints_" + std::to_string(waypoint_idx++); + + // Apply offsets + // Position + query_append_range_inclusive_with_tolerance(query, meta_name + ".position.x", x_offset + waypoint.position.x, + match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".position.y", y_offset + waypoint.position.y, + match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".position.z", z_offset + waypoint.position.z, + match_tolerance); + + // Orientation + tf2::Quaternion tf2_quat_goal_offset(waypoint.orientation.x, waypoint.orientation.y, waypoint.orientation.z, + waypoint.orientation.w); + tf2_quat_goal_offset.normalize(); + + auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; + final_quat.normalize(); + + query_append_range_inclusive_with_tolerance(query, meta_name + ".orientation.x", final_quat.getX(), match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".orientation.y", final_quat.getY(), match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".orientation.z", final_quat.getZ(), match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".orientation.w", final_quat.getW(), match_tolerance); + } + + query.append("link_name", plan_request.link_name); + query.append("header.frame_id", base_frame); + + return true; +} + +// CARTESIAN TRAJECTORY CACHING: METADATA CONSTRUCTION +bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_metadata( + Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request) +{ + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + metadata.append("group_name", plan_request.group_name); + + // Joint state + // Only accounts for joint_state position. Ignores velocity and effort. + if (plan_request.start_state.is_diff) + { + // If plan request states that the start_state is_diff, then we need to get + // the current state from the move_group. + + // NOTE: methyldragon - + // I think if is_diff is on, the joint states will not be populated in all + // of our motion plan requests? If this isn't the case we might need to + // apply the joint states as offsets as well. + // + // TODO: Since MoveIt also potentially does another getCurrentState() call + // when planning, there is a chance that the current state in the cache + // differs from the state used in MoveIt's plan. + // + // When upstreaming this class to MoveIt, this issue should go away once + // the class is used within the move group's Plan call. + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); + if (!current_state) + { + RCLCPP_WARN(node_->get_logger(), "Skipping start metadata append: Could not get robot state."); + // *metadata = original; // Undo our changes. // Copy not supported + return false; + } + + moveit_msgs::msg::RobotState current_state_msg; + robotStateToRobotStateMsg(*current_state, current_state_msg); + + for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) + { + metadata.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); + metadata.append("start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i)); + } + } + else + { + for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) + { + metadata.append("start_state.joint_state.name_" + std::to_string(i), + plan_request.start_state.joint_state.name.at(i)); + metadata.append("start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i)); + } + } + + return true; +} + +bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_metadata( + Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request) +{ + // Make ignored members explicit + if (!plan_request.path_constraints.joint_constraints.empty() || + !plan_request.path_constraints.position_constraints.empty() || + !plan_request.path_constraints.orientation_constraints.empty() || + !plan_request.path_constraints.visibility_constraints.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring path_constraints: Not supported."); + } + if (plan_request.avoid_collisions) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring avoid_collisions: Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + metadata.append("max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor); + metadata.append("max_acceleration_scaling_factor", plan_request.max_acceleration_scaling_factor); + metadata.append("max_step", plan_request.max_step); + metadata.append("jump_threshold", plan_request.jump_threshold); + + // Waypoints + // Restating them in terms of the robot model frame (usually base_link) + std::string base_frame = move_group.getRobotModel()->getModelFrame(); + + // Compute offsets. + double x_offset = 0; + double y_offset = 0; + double z_offset = 0; + + geometry_msgs::msg::Quaternion quat_offset; + quat_offset.x = 0; + quat_offset.y = 0; + quat_offset.z = 0; + quat_offset.w = 1; + + if (base_frame != plan_request.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform(plan_request.header.frame_id, base_frame, tf2::TimePointZero); + + x_offset = transform.transform.translation.x; + y_offset = transform.transform.translation.y; + z_offset = transform.transform.translation.z; + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(node_->get_logger(), + "Skipping goal metadata append: " + "Could not get goal transform for %s to %s: %s", + base_frame.c_str(), plan_request.header.frame_id.c_str(), ex.what()); + + // (Can't. Copy not supported.) + // *metadata = original; // Undo our changes. + return false; + } + } + + tf2::Quaternion tf2_quat_frame_offset(quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); + tf2_quat_frame_offset.normalize(); + + size_t waypoint_idx = 0; + for (auto& waypoint : plan_request.waypoints) + { + std::string meta_name = "waypoints_" + std::to_string(waypoint_idx++); + + // Apply offsets + // Position + metadata.append(meta_name + ".position.x", x_offset + waypoint.position.x); + metadata.append(meta_name + ".position.y", y_offset + waypoint.position.y); + metadata.append(meta_name + ".position.z", z_offset + waypoint.position.z); + + // Orientation + tf2::Quaternion tf2_quat_goal_offset(waypoint.orientation.x, waypoint.orientation.y, waypoint.orientation.z, + waypoint.orientation.w); + tf2_quat_goal_offset.normalize(); + + auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; + final_quat.normalize(); + + metadata.append(meta_name + ".orientation.x", final_quat.getX()); + metadata.append(meta_name + ".orientation.y", final_quat.getY()); + metadata.append(meta_name + ".orientation.z", final_quat.getZ()); + metadata.append(meta_name + ".orientation.w", final_quat.getW()); + } + + metadata.append("link_name", plan_request.link_name); + metadata.append("header.frame_id", base_frame); + + return true; +} + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.hpp b/moveit_ros/trajectory_cache/src/trajectory_cache.hpp new file mode 100644 index 0000000000..a1aab3410b --- /dev/null +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.hpp @@ -0,0 +1,261 @@ +// Copyright 2022 Johnson & Johnson +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SRC__TRAJECTORY_CACHE_HPP +#define SRC__TRAJECTORY_CACHE_HPP + +#include +#include +#include + +#include + +#include +#include + +// TF2 +#include +#include + +// ROS2 Messages +#include +#include + +// moveit modules +#include +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +/** + * Trajectory Cache manager for MoveIt. + * + * This manager facilitates cache management for MoveIt 2's `MoveGroupInterface` + * by using `warehouse_ros` to manage a database of executed trajectories, keyed + * by the start and goal conditions, and sorted by how long the trajectories + * took to execute. This allows for the lookup and reuse of the best performing + * trajectories found so far. + * + * WARNING: RFE: + * !!! This cache does NOT support collision detection! + * Trajectories will be put into and fetched from the cache IGNORING + * collision. + * + * If your planning scene is expected to change between cache lookups, do NOT + * use this cache, fetched trajectories are likely to result in collision + * then. + * + * To handle collisions this class will need to hash the planning scene world + * msg (after zeroing out std_msgs/Header timestamps and sequences) and do an + * appropriate lookup, or do more complicated checks to see if the scene world + * is "close enough" or is a less obstructed version of the scene in the cache + * entry. + * + * !!! This cache does NOT support keying on joint velocities and efforts. + * The cache only keys on joint positions. + * + * !!! This cache does NOT support multi-DOF joints. + + * !!! This cache does NOT support certain constraints + * Including: path, constraint regions, everything related to collision. + * + * This is because they are difficult (but not impossible) to implement key + * logic for. + * + * Relevant ROS Parameters: + * - `warehouse_plugin`: What database to use + * - `warehouse_host`: Where the database should be created + * - `warehouse_port`: The port used for the database + * + * This class supports trajectories planned from move_group MotionPlanRequests + * as well as GetCartesianPath requests. That is, both normal motion plans and + * cartesian plans are supported. + * + * Motion plan trajectories are stored in the `move_group_trajectory_cache` + * database within the database file, with trajectories for each move group + * stored in a collection named after the relevant move group's name. + * + * For example, the "my_move_group" move group will have its cache stored in + * `move_group_trajectory_cache@my_move_group` + * + * Motion Plan Trajectories are keyed on: + * - Plan Start: robot joint state + * - Plan Goal (either of): + * - Final pose (wrt. `planning_frame` (usually `base_link`)) + * - Final robot joint states + * - Plan Constraints (but not collision) + * + * Trajectories may be looked up with some tolerance at call time. + * + * Similarly, the cartesian trajectories are stored in the + * `move_group_cartesian_trajectory_cache` database within the database file, + * with trajectories for each move group stored in a collection named after the + * relevant move group's name. + * + * Cartesian Trajectories are keyed on: + * - Plan Start: robot joint state + * - Plan Goal: + * - Pose waypoints + */ +class TrajectoryCache +{ +public: + // We explicitly need a Node::SharedPtr because warehouse_ros ONLY supports + // it... + // + // TODO: methylDragon - + // Use rclcpp::node_interfaces::NodeInterfaces<> once warehouse_ros does. + explicit TrajectoryCache(const rclcpp::Node::SharedPtr& node); + + bool init(const std::string& db_path = ":memory:", uint32_t db_port = 0, double exact_match_precision = 1e-6); + + unsigned count_trajectories(const std::string& move_group_namespace); + + unsigned count_cartesian_trajectories(const std::string& move_group_namespace); + + // =========================================================================== + // MOTION PLAN TRAJECTORY CACHING + // =========================================================================== + // TOP LEVEL OPS + + // Fetches all plans that fit within the requested tolerances for start and + // goal conditions, returning them as a vector, sorted by some db column. + std::vector::ConstPtr> + fetch_all_matching_trajectories(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, + double goal_tolerance, bool metadata_only = false, + const std::string& sort_by = "execution_time_s"); + + // Fetches the best trajectory that fits within the requested tolerances for start + // and goal conditions, by some db column. + warehouse_ros::MessageWithMetadata::ConstPtr fetch_best_matching_trajectory( + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, + bool metadata_only = false, const std::string& sort_by = "execution_time_s"); + + // Put a trajectory into the database if it is the best matching trajectory seen so far. + // + // Trajectories are matched based off their start and goal states. + // And are considered "better" if they have a smaller planned execution time + // than matching trajectories. + // + // Optionally deletes all worse trajectories by default to prune the cache. + bool put_trajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, + const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, + double planning_time_s, bool delete_worse_trajectories = true); + + // QUERY CONSTRUCTION + bool extract_and_append_trajectory_start_to_query(warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance); + + bool extract_and_append_trajectory_goal_to_query(warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance); + + // METADATA CONSTRUCTION + bool extract_and_append_trajectory_start_to_metadata(warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request); + + bool extract_and_append_trajectory_goal_to_metadata(warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request); + + // =========================================================================== + // CARTESIAN TRAJECTORY CACHING + // =========================================================================== + // TOP LEVEL OPS + + // This mimics the move group computeCartesianPath signature (without path + // constraints). + moveit_msgs::srv::GetCartesianPath::Request + construct_get_cartesian_path_request(moveit::planning_interface::MoveGroupInterface& move_group, + const std::vector& waypoints, double step, + double jump_threshold, bool avoid_collisions = true); + + // Fetches all cartesian trajectories that fit within the requested tolerances + // for start and goal conditions, returning them as a vector, sorted by some + // db column. + std::vector::ConstPtr> + fetch_all_matching_cartesian_trajectories(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double min_fraction, double start_tolerance, double goal_tolerance, + bool metadata_only = false, + const std::string& sort_by = "execution_time_s"); + + // Fetches the best cartesian trajectory that fits within the requested tolerances + // for start and goal conditions, by some db column. + warehouse_ros::MessageWithMetadata::ConstPtr + fetch_best_matching_cartesian_trajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double min_fraction, double start_tolerance, double goal_tolerance, + bool metadata_only = false, const std::string& sort_by = "execution_time_s"); + + // Put a cartesian trajectory into the database if it is the best matching + // cartesian trajectory seen so far. + // + // Cartesian trajectories are matched based off their start and goal states. + // And are considered "better" if they have a smaller planned execution time + // than matching cartesian trajectories. + // + // Optionally deletes all worse cartesian trajectories by default to prune the + // cache. + bool put_cartesian_trajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, + double planning_time_s, double fraction, bool delete_worse_trajectories = true); + + // QUERY CONSTRUCTION + bool extract_and_append_cartesian_trajectory_start_to_query( + warehouse_ros::Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance); + + bool extract_and_append_cartesian_trajectory_goal_to_query( + warehouse_ros::Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance); + + // METADATA CONSTRUCTION + bool extract_and_append_cartesian_trajectory_start_to_metadata( + warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request); + + bool extract_and_append_cartesian_trajectory_goal_to_metadata( + warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request); + +private: + rclcpp::Node::SharedPtr node_; + warehouse_ros::DatabaseConnection::Ptr db_; + + double exact_match_precision_ = 1e-6; + + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; +}; + +} // namespace trajectory_cache +} // namespace moveit_ros + +#endif // SRC__TRAJECTORY_CACHE_HPP diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.py b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py new file mode 100755 index 0000000000..a9c32c06da --- /dev/null +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py @@ -0,0 +1,154 @@ +# Copyright 2023 Johnson & Johnson +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch_pytest.tools import process as process_tools +from launch_ros.actions import Node + +from moveit_configs_utils import MoveItConfigsBuilder + +import launch_pytest + +import pytest +import os + +# This would have been a gtest, but we need a move_group instance, which +# requires some parameters loaded and a separate node started. + + +@pytest.fixture +def moveit_config(): + return ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .planning_pipelines("ompl", ["ompl"]) + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .to_moveit_configs() + ) + + +# We need this so the move_group has something to interact with +@pytest.fixture +def robot_fixture(moveit_config): + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="log", + parameters=[moveit_config.to_dict()], + ) + + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", + "0.0", "0.0", "world", "panda_link0"], + ) + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="log", + parameters=[moveit_config.robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="log", + ) + + load_controllers = [] + for controller in [ + "panda_arm_controller", + "panda_hand_controller", + "joint_state_broadcaster", + ]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner {}".format( + controller)], + shell=True, + output="log", + ) + ] + + return [ + run_move_group_node, + static_tf, + robot_state_publisher, + ros2_control_node, + *load_controllers + ] + + +@pytest.fixture +def trajectory_cache_test_runner_node(moveit_config): + return Node( + package="moveit_ros_trajectory_cache", + executable="test_trajectory_cache", + name="test_trajectory_cache_node", + output="screen", + cached_output=True, + parameters=[moveit_config.to_dict()] + ) + + +@launch_pytest.fixture +def launch_description(trajectory_cache_test_runner_node, robot_fixture): + return LaunchDescription( + robot_fixture + + [ + trajectory_cache_test_runner_node, + launch_pytest.actions.ReadyToTest() + ] + ) + + +def validate_stream(expected): + def wrapped(output): + assert expected in output.splitlines( + ), f"Did not get expected: {expected} in output:\n\n{output}" + return wrapped + + +@pytest.mark.launch(fixture=launch_description) +def test_all_tests_pass(trajectory_cache_test_runner_node, launch_context): + # Check for occurrences of [PASS] in output + assert process_tools.wait_for_output_sync( + launch_context, + trajectory_cache_test_runner_node, + lambda x: x.count("[PASS]") == 165, # All test cases passed. + timeout=30 + ) + + # Check no occurrences of [FAIL] in output + assert not process_tools.wait_for_output_sync( + launch_context, + trajectory_cache_test_runner_node, + lambda x: "[FAIL]" in x, + timeout=10 + ) + + yield From 3025ed6aa3ba743d2baedcf60d82f97cbf397bcc Mon Sep 17 00:00:00 2001 From: methylDragon Date: Sat, 18 May 2024 16:18:05 -0700 Subject: [PATCH 02/26] Add README Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/README.md | 130 ++++++++++++++++++++++++++ 1 file changed, 130 insertions(+) diff --git a/moveit_ros/trajectory_cache/README.md b/moveit_ros/trajectory_cache/README.md index d0ff499365..6ee301c871 100644 --- a/moveit_ros/trajectory_cache/README.md +++ b/moveit_ros/trajectory_cache/README.md @@ -1 +1,131 @@ # Trajectory Cache + +``` + * This cache does NOT support collision detection! + * Plans will be put into and fetched from the cache IGNORING collision. + * If your planning scene is expected to change between cache lookups, do NOT + * use this cache, fetched plans are likely to result in collision then. + * + * To handle collisions this class will need to hash the planning scene world + * msg (after zeroing out std_msgs/Header timestamps and sequences) and do an + * appropriate lookup, or otherwise find a way to determine that a planning scene + * is "less than" or "in range of" another (e.g. checking that every obstacle/mesh + * exists within the other scene's). (very out of scope) + ``` + +A trajectory cache based on [`warehouse_ros`](https://github.com/moveit/warehouse_ros) for the move_group planning interface that supports fuzzy lookup for `MotionPlanRequest` and `GetCartesianPath` requests and trajectories. + +The cache allows you to insert trajectories and fetch keyed fuzzily on the following: + +- Starting robot (joint) state +- Planning request constraints + - This includes ALL joint, position, and orientation constraints! + - And also workspace parameters, and some others. +- Planning request goal parameters + +It works generically for an arbitrary number of joints, across any number of move_groups. + +Furthermore, the strictness of the fuzzy lookup can be configured for start and goal conditions. + +## Citations + +If you use this package in your work, please cite it using the following: + +``` +@software{ong_2024_11215428, + author = {Ong, Brandon}, + title = {A Fuzzy-Matching Trajectory Cache for MoveIt 2}, + month = may, + year = 2024, + publisher = {GitHub}, + version = {0.1.0}, + doi = {10.5281/zenodo.11215428}, + url = {https://doi.org/10.5281/zenodo.11215428} +} +``` + +## Example usage + +```cpp +// Be sure to set some relevant ROS parameters: +// Relevant ROS Parameters: +// - `warehouse_plugin`: What database to use +// - `warehouse_host`: Where the database should be created +// - `warehouse_port`: The port used for the database +auto traj_cache = std::make_shared(node); + +auto fetched_trajectory = traj_cache->fetch_best_matching_trajectory( + *move_group_interface, robot_name, motion_plan_req_msg, + _cache_start_match_tolerance, _cache_goal_match_tolerance, /*sort_by=*/"execution_time_s"); + +if (fetched_trajectory) { + // Great! We got a cache hit + // Do something with the plan. +} else { + // Plan... And put it for posterity! + traj_cache->put_trajectory( + *interface, robot_name, std::move(plan_req_msg), std::move(res->result.trajectory), + rclcpp::Duration( + res->result.trajectory.joint_trajectory.points.back().time_from_start + ).seconds(), + res->result.planning_time, /*overwrite=*/true) +} +``` + +It also has the following optimizations: +- Separate caches for separate move groups +- The cache does "canonicalization" of the poses to be relative to the planning frame to increase the chances of cache hits. +- Configurable fuzzy lookup for the keys above. +- It supports "overwriting" of worse trajectories with better trajectories + - If a trajectory with keys extremely close to a pre-existing cache entry is inserted with a better planned execution time, the cache can delete all "worse" trajectories. + - #IsThisMachineLearning + - It also prunes the database and mitigates a lot of query slow-down as the cache grows + +## Working Principle + +If a plan request has start, goal, and constraint conditions that are "close enough" (configurable per request) to an entry in the cache, then the cached trajectory should be suitable (as long as obstacles have not changed). + +Goal fuzziness is a lot less lenient than start fuzziness by default. + +Finally, the databases are sharded by move group, and the constraints are added as columns in a name agnostic fashion (they are even somewhat robust to ordering, because they're sorted!) + +## Benefits + +A trajectory cache helps: +- Cut down on planning time (especially for known moves) +- Allows for consistent predictable behavior of used together with a stochastic planner + - It effectively allows you to "freeze" a move + +A user may also choose when to leverage the cache (e.g. when planning moves from a static "home" position, or repetitive/cartesian moves) to get these benefits. + +Additionally, because the cache class has the ability to sort by planned execution time, over sufficient runs, the stochastic plans eventually converge to better and better plans (execution time wise). + +You may build abstractions on top of the class, for example, to expose the following behaviors: +- `TrainingOverwrite`: Always plan, and write to cache, deleting all worse trajectories for "matching" cache keys +- `TrainingAppendOnly`: Always plan, and always add to the cache. +- `ExecuteBestEffort`: Rely on cache wherever possible, but plan otherwise. +- `ExecuteReadOnly`: Only execute if cache hit occurred. + +You can see how such behaviors effectively model the "dev" and "deploy" phases of a robot deployment, and how they could be useful. + +## Best Practices + +- Since this cache does not yet support collisions, ensure the planning scene and obstacles remain static +- Have looser start fuzziness, and stricter goal fuzziness +- Move the robot to static known poses where possible before planning to increase the chances of a cache hit +- Use the cache where repetitive, non-dynamic motion is likely to occur (e.g. known plans, short planned moves, etc.) + +## WARNING: The following are unsupported / RFE + +Since this is an initial release, the following features are unsupported because they were a little too difficult for the time I had to implement this. So I am leaving it to the community to help! + +- **!!! This cache does NOT support collision detection!** + - Trajectories will be put into and fetched from the cache IGNORING collision. + - If your planning scene is expected to change between cache lookups, do NOT use this cache, fetched trajectories are likely to result in collision then. + - To handle collisions this class will need to hash the planning scene world msg (after zeroing out std_msgs/Header timestamps and sequences) and do an appropriate lookup, or do more complicated checks to see if the scene world is "close enough" or is a less obstructed version of the scene in the cache entry. + - !!! This cache does NOT support keying on joint velocities and efforts. + - The cache only keys on joint positions. +- !!! This cache does NOT support multi-DOF joints. +- !!! This cache does NOT support certain constraints + - Including: path, constraint regions, everything related to collision. +- The fuzzy lookup can't be configured on a per-joint basis. \ No newline at end of file From d7095ba2b5a0227effe3b7bb985556cfda3cd9f0 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 23 Jul 2024 13:57:32 -0700 Subject: [PATCH 03/26] Move test cpp to test directory Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/CMakeLists.txt | 4 +++- .../moveit/trajectory_cache}/trajectory_cache.hpp | 0 moveit_ros/trajectory_cache/src/trajectory_cache.cpp | 2 +- .../trajectory_cache/{src => test}/test_trajectory_cache.cpp | 2 +- 4 files changed, 5 insertions(+), 3 deletions(-) rename moveit_ros/trajectory_cache/{src => include/moveit/trajectory_cache}/trajectory_cache.hpp (100%) rename moveit_ros/trajectory_cache/{src => test}/test_trajectory_cache.cpp (99%) diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt index 4fabec17d7..3c888c7330 100644 --- a/moveit_ros/trajectory_cache/CMakeLists.txt +++ b/moveit_ros/trajectory_cache/CMakeLists.txt @@ -37,6 +37,7 @@ set(TRAJECTORY_CACHE_LIBRARY_NAME trajectory_cache) # Motion plan cache library add_library(${TRAJECTORY_CACHE_LIBRARY_NAME} src/trajectory_cache.cpp) ament_target_dependencies(${TRAJECTORY_CACHE_LIBRARY_NAME} ${trajectory_cache_dependencies}) +target_include_directories(${TRAJECTORY_CACHE_LIBRARY_NAME} PUBLIC $) #=============================================================================== @@ -45,7 +46,8 @@ if(BUILD_TESTING) find_package(launch_testing_ament_cmake REQUIRED) find_package(rmf_utils REQUIRED) - add_executable(test_trajectory_cache src/test_trajectory_cache.cpp) + # This test executable is run by the pytest_test, since a node is required for testing move groups + add_executable(test_trajectory_cache test/test_trajectory_cache.cpp) target_link_libraries(test_trajectory_cache ${TRAJECTORY_CACHE_LIBRARY_NAME}) install(TARGETS diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp similarity index 100% rename from moveit_ros/trajectory_cache/src/trajectory_cache.hpp rename to moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index 9127604c21..4c39bbdba8 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/logging.hpp" -#include "trajectory_cache.hpp" +#include "moveit/trajectory_cache/trajectory_cache.hpp" namespace moveit_ros { diff --git a/moveit_ros/trajectory_cache/src/test_trajectory_cache.cpp b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp similarity index 99% rename from moveit_ros/trajectory_cache/src/test_trajectory_cache.cpp rename to moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp index 799b2d60c8..91410a26a0 100644 --- a/moveit_ros/trajectory_cache/src/test_trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp @@ -16,7 +16,7 @@ #include "moveit/robot_state/conversions.h" #include "moveit/robot_state/robot_state.h" -#include "trajectory_cache.hpp" +#include "moveit/trajectory_cache/trajectory_cache.hpp" #include #include From 728dce2daa90f97f70f1f6b3454aa46062491655 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 23 Jul 2024 14:29:20 -0700 Subject: [PATCH 04/26] Clean up logging and comments Signed-off-by: methylDragon --- .../trajectory_cache/trajectory_cache.hpp | 7 +- .../trajectory_cache/src/trajectory_cache.cpp | 70 ++++++++----------- .../test/test_trajectory_cache.cpp | 4 +- 3 files changed, 33 insertions(+), 48 deletions(-) diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index a1aab3410b..0b789d9809 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -12,8 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SRC__TRAJECTORY_CACHE_HPP -#define SRC__TRAJECTORY_CACHE_HPP +#pragma once #include #include @@ -256,6 +255,4 @@ class TrajectoryCache }; } // namespace trajectory_cache -} // namespace moveit_ros - -#endif // SRC__TRAJECTORY_CACHE_HPP +} // namespace moveit_ros \ No newline at end of file diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index 4c39bbdba8..225be2c78e 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -72,7 +72,7 @@ TrajectoryCache::TrajectoryCache(const rclcpp::Node::SharedPtr& node) : node_(no bool TrajectoryCache::init(const std::string& db_path, uint32_t db_port, double exact_match_precision) { - RCLCPP_INFO(node_->get_logger(), "Opening trajectory cache database at: %s (Port: %d, Precision: %f)", + RCLCPP_DEBUG(node_->get_logger(), "Opening trajectory cache database at: %s (Port: %d, Precision: %f)", db_path.c_str(), db_port, exact_match_precision); // If the `warehouse_plugin` parameter isn't set, defaults to warehouse_ros' @@ -138,7 +138,7 @@ MessageWithMetadata::ConstPtr TrajectoryCache if (matching_trajectories.empty()) { - RCLCPP_INFO(node_->get_logger(), "No matching trajectories found."); + RCLCPP_DEBUG(node_->get_logger(), "No matching trajectories found."); return nullptr; } @@ -211,7 +211,7 @@ bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroup if (execution_time_s < match_execution_time_s) { int delete_id = match->lookupInt("id"); - RCLCPP_INFO(node_->get_logger(), + RCLCPP_DEBUG(node_->get_logger(), "Overwriting plan (id: %d): " "execution_time (%es) > new trajectory's execution_time (%es)", delete_id, match_execution_time_s, execution_time_s); @@ -242,7 +242,7 @@ bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroup return false; } - RCLCPP_INFO(node_->get_logger(), + RCLCPP_DEBUG(node_->get_logger(), "Inserting trajectory: New trajectory execution_time (%es) " "is better than best trajectory's execution_time (%es)", execution_time_s, best_execution_time); @@ -251,7 +251,7 @@ bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroup return true; } - RCLCPP_INFO(node_->get_logger(), + RCLCPP_DEBUG(node_->get_logger(), "Skipping plan insert: New trajectory execution_time (%es) " "is worse than best trajectory's execution_time (%es)", execution_time_s, best_execution_time); @@ -275,8 +275,6 @@ bool TrajectoryCache::extract_and_append_trajectory_start_to_query( RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); } - // auto original = *query; // Copy not supported. - query.append("group_name", plan_request.group_name); // Workspace params @@ -311,7 +309,8 @@ bool TrajectoryCache::extract_and_append_trajectory_start_to_query( if (!current_state) { RCLCPP_WARN(node_->get_logger(), "Skipping start query append: Could not get robot state."); - // *query = original; // Undo our changes. (Can't. Copy not supported.) + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. return false; } @@ -366,8 +365,6 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_query( "Not supported."); } - // auto original = *query; // Copy not supported. - query_append_range_inclusive_with_tolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor, match_tolerance); query_append_range_inclusive_with_tolerance(query, "max_acceleration_scaling_factor", @@ -447,8 +444,8 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_query( plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); - // (Can't. Copy not supported.) - // *query = original; // Undo our changes. + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. return false; } } @@ -501,8 +498,8 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_query( plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); - // (Can't. Copy not supported.) - // *query = original; // Undo our changes. + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. return false; } } @@ -552,8 +549,6 @@ bool TrajectoryCache::extract_and_append_trajectory_start_to_metadata( RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); } - // auto original = *metadata; // Copy not supported. - metadata.append("group_name", plan_request.group_name); // Workspace params @@ -587,7 +582,8 @@ bool TrajectoryCache::extract_and_append_trajectory_start_to_metadata( if (!current_state) { RCLCPP_WARN(node_->get_logger(), "Skipping start metadata append: Could not get robot state."); - // *metadata = original; // Undo our changes. // Copy not supported + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. return false; } @@ -642,8 +638,6 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_metadata( "Not supported."); } - // auto original = *metadata; // Copy not supported. - metadata.append("max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor); metadata.append("max_acceleration_scaling_factor", plan_request.max_acceleration_scaling_factor); metadata.append("max_cartesian_speed", plan_request.max_cartesian_speed); @@ -720,8 +714,8 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_metadata( plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); - // (Can't. Copy not supported.) - // *metadata = original; // Undo our changes. + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. return false; } } @@ -771,8 +765,8 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_metadata( plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); - // (Can't. Copy not supported.) - // *metadata = original; // Undo our changes. + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. return false; } } @@ -879,7 +873,7 @@ TrajectoryCache::fetch_best_matching_cartesian_trajectory( if (matching_trajectories.empty()) { - RCLCPP_INFO(node_->get_logger(), "No matching cartesian trajectories found."); + RCLCPP_DEBUG(node_->get_logger(), "No matching cartesian trajectories found."); return nullptr; } @@ -947,7 +941,7 @@ bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface: if (execution_time_s < match_execution_time_s) { int delete_id = match->lookupInt("id"); - RCLCPP_INFO(node_->get_logger(), + RCLCPP_DEBUG(node_->get_logger(), "Overwriting cartesian trajectory (id: %d): " "execution_time (%es) > new trajectory's execution_time (%es)", delete_id, match_execution_time_s, execution_time_s); @@ -980,7 +974,7 @@ bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface: return false; } - RCLCPP_INFO(node_->get_logger(), + RCLCPP_DEBUG(node_->get_logger(), "Inserting cartesian trajectory: New trajectory execution_time (%es) " "is better than best trajectory's execution_time (%es) at fraction (%es)", execution_time_s, best_execution_time, fraction); @@ -989,7 +983,7 @@ bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface: return true; } - RCLCPP_INFO(node_->get_logger(), + RCLCPP_DEBUG(node_->get_logger(), "Skipping cartesian trajectory insert: New trajectory execution_time (%es) " "is worse than best trajectory's execution_time (%es) at fraction (%es)", execution_time_s, best_execution_time, fraction); @@ -1013,8 +1007,6 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_query( RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); } - // auto original = *metadata; // Copy not supported. - query.append("group_name", plan_request.group_name); // Joint state @@ -1039,7 +1031,8 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_query( if (!current_state) { RCLCPP_WARN(node_->get_logger(), "Skipping start metadata append: Could not get robot state."); - // *metadata = original; // Undo our changes. // Copy not supported + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. return false; } @@ -1085,8 +1078,6 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_query( RCLCPP_WARN(node_->get_logger(), "Ignoring avoid_collisions: Not supported."); } - // auto original = *metadata; // Copy not supported. - query_append_range_inclusive_with_tolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor, match_tolerance); query_append_range_inclusive_with_tolerance(query, "max_acceleration_scaling_factor", @@ -1127,8 +1118,8 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_query( "Could not get goal transform for %s to %s: %s", base_frame.c_str(), plan_request.header.frame_id.c_str(), ex.what()); - // (Can't. Copy not supported.) - // *metadata = original; // Undo our changes. + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. return false; } } @@ -1185,8 +1176,6 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_metadata( RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); } - // auto original = *metadata; // Copy not supported. - metadata.append("group_name", plan_request.group_name); // Joint state @@ -1211,7 +1200,8 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_metadata( if (!current_state) { RCLCPP_WARN(node_->get_logger(), "Skipping start metadata append: Could not get robot state."); - // *metadata = original; // Undo our changes. // Copy not supported + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. return false; } @@ -1256,8 +1246,6 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_metadata( RCLCPP_WARN(node_->get_logger(), "Ignoring avoid_collisions: Not supported."); } - // auto original = *metadata; // Copy not supported. - metadata.append("max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor); metadata.append("max_acceleration_scaling_factor", plan_request.max_acceleration_scaling_factor); metadata.append("max_step", plan_request.max_step); @@ -1296,8 +1284,8 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_metadata( "Could not get goal transform for %s to %s: %s", base_frame.c_str(), plan_request.header.frame_id.c_str(), ex.what()); - // (Can't. Copy not supported.) - // *metadata = original; // Undo our changes. + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. return false; } } diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp index 91410a26a0..63002bc7d1 100644 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp @@ -92,7 +92,7 @@ std::vector get_dummy_waypoints() void test_motion_trajectories(std::shared_ptr move_group, std::shared_ptr cache) { // Setup ===================================================================== - // All variants are copies. + // All variants are modified copies of `plan_req`. /// MotionPlanRequest @@ -577,7 +577,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, prefix, "Ok"); // Setup ===================================================================== - // All variants are copies. + // All variants are modified copies of `cartesian_plan_req`. /// GetCartesianPath::Request From 7341e64b507c75dacda73d7c8a6f03e54b81cc6f Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 23 Jul 2024 14:46:57 -0700 Subject: [PATCH 05/26] Use move_group node for time Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/src/trajectory_cache.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index 225be2c78e..292ec96d60 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -826,10 +826,7 @@ TrajectoryCache::construct_get_cartesian_path_request(moveit::planning_interface out.path_constraints = moveit_msgs::msg::Constraints(); out.avoid_collisions = avoid_collisions; out.link_name = move_group.getEndEffectorLink(); - - // We were already cursed before, now we are double cursed... - // move_group.getNodeHandle() is UNIMPLEMENTED upstream. - out.header.stamp = node_->now(); + out.header.stamp = move_group.getNode()->now(); return out; } From aa053650a18b9b8b35d23a0c3a1193632af122d2 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 23 Jul 2024 14:51:10 -0700 Subject: [PATCH 06/26] Add and use logger Signed-off-by: methylDragon --- .../trajectory_cache/trajectory_cache.hpp | 1 + .../trajectory_cache/src/trajectory_cache.cpp | 90 +++++++++---------- 2 files changed, 46 insertions(+), 45 deletions(-) diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index 0b789d9809..5bc3af13db 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -246,6 +246,7 @@ class TrajectoryCache private: rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; warehouse_ros::DatabaseConnection::Ptr db_; double exact_match_precision_ = 1e-6; diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index 292ec96d60..e29f29e465 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -64,7 +64,7 @@ void sort_constraints(std::vector& joint_cons // Trajectory Cache ============================================================ -TrajectoryCache::TrajectoryCache(const rclcpp::Node::SharedPtr& node) : node_(node) +TrajectoryCache::TrajectoryCache(const rclcpp::Node::SharedPtr& node) : node_(node), logger_(moveit::getLogger("moveit.ros.trajectory_cache")) { tf_buffer_ = std::make_unique(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -72,7 +72,7 @@ TrajectoryCache::TrajectoryCache(const rclcpp::Node::SharedPtr& node) : node_(no bool TrajectoryCache::init(const std::string& db_path, uint32_t db_port, double exact_match_precision) { - RCLCPP_DEBUG(node_->get_logger(), "Opening trajectory cache database at: %s (Port: %d, Precision: %f)", + RCLCPP_DEBUG(logger_, "Opening trajectory cache database at: %s (Port: %d, Precision: %f)", db_path.c_str(), db_port, exact_match_precision); // If the `warehouse_plugin` parameter isn't set, defaults to warehouse_ros' @@ -119,7 +119,7 @@ TrajectoryCache::fetch_all_matching_trajectories(const moveit::planning_interfac if (!start_ok || !goal_ok) { - RCLCPP_ERROR(node_->get_logger(), "Could not construct trajectory query."); + RCLCPP_ERROR(logger_, "Could not construct trajectory query."); return {}; } @@ -138,7 +138,7 @@ MessageWithMetadata::ConstPtr TrajectoryCache if (matching_trajectories.empty()) { - RCLCPP_DEBUG(node_->get_logger(), "No matching trajectories found."); + RCLCPP_DEBUG(logger_, "No matching trajectories found."); return nullptr; } @@ -163,17 +163,17 @@ bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroup // Check pre-conditions if (!trajectory.multi_dof_joint_trajectory.points.empty()) { - RCLCPP_ERROR(node_->get_logger(), "Skipping plan insert: Multi-DOF trajectory plans are not supported."); + RCLCPP_ERROR(logger_, "Skipping plan insert: Multi-DOF trajectory plans are not supported."); return false; } if (plan_request.workspace_parameters.header.frame_id.empty() || trajectory.joint_trajectory.header.frame_id.empty()) { - RCLCPP_ERROR(node_->get_logger(), "Skipping plan insert: Frame IDs cannot be empty."); + RCLCPP_ERROR(logger_, "Skipping plan insert: Frame IDs cannot be empty."); return false; } if (plan_request.workspace_parameters.header.frame_id != trajectory.joint_trajectory.header.frame_id) { - RCLCPP_ERROR(node_->get_logger(), + RCLCPP_ERROR(logger_, "Skipping plan insert: " "Plan request frame (%s) does not match plan frame (%s).", plan_request.workspace_parameters.header.frame_id.c_str(), @@ -192,7 +192,7 @@ bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroup if (!start_query_ok || !goal_query_ok) { - RCLCPP_ERROR(node_->get_logger(), "Skipping plan insert: Could not construct lookup query."); + RCLCPP_ERROR(logger_, "Skipping plan insert: Could not construct lookup query."); return false; } @@ -211,7 +211,7 @@ bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroup if (execution_time_s < match_execution_time_s) { int delete_id = match->lookupInt("id"); - RCLCPP_DEBUG(node_->get_logger(), + RCLCPP_DEBUG(logger_, "Overwriting plan (id: %d): " "execution_time (%es) > new trajectory's execution_time (%es)", delete_id, match_execution_time_s, execution_time_s); @@ -238,11 +238,11 @@ bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroup if (!start_meta_ok || !goal_meta_ok) { - RCLCPP_ERROR(node_->get_logger(), "Skipping plan insert: Could not construct insert metadata."); + RCLCPP_ERROR(logger_, "Skipping plan insert: Could not construct insert metadata."); return false; } - RCLCPP_DEBUG(node_->get_logger(), + RCLCPP_DEBUG(logger_, "Inserting trajectory: New trajectory execution_time (%es) " "is better than best trajectory's execution_time (%es)", execution_time_s, best_execution_time); @@ -251,7 +251,7 @@ bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroup return true; } - RCLCPP_DEBUG(node_->get_logger(), + RCLCPP_DEBUG(logger_, "Skipping plan insert: New trajectory execution_time (%es) " "is worse than best trajectory's execution_time (%es)", execution_time_s, best_execution_time); @@ -268,11 +268,11 @@ bool TrajectoryCache::extract_and_append_trajectory_start_to_query( // Make ignored members explicit if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) { - RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.multi_dof_joint_states: Not supported."); + RCLCPP_WARN(logger_, "Ignoring start_state.multi_dof_joint_states: Not supported."); } if (!plan_request.start_state.attached_collision_objects.empty()) { - RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); + RCLCPP_WARN(logger_, "Ignoring start_state.attached_collision_objects: Not supported."); } query.append("group_name", plan_request.group_name); @@ -308,7 +308,7 @@ bool TrajectoryCache::extract_and_append_trajectory_start_to_query( moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); if (!current_state) { - RCLCPP_WARN(node_->get_logger(), "Skipping start query append: Could not get robot state."); + RCLCPP_WARN(logger_, "Skipping start query append: Could not get robot state."); // NOTE: methyldragon - // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. return false; @@ -361,7 +361,7 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_query( } if (emit_position_constraint_warning) { - RCLCPP_WARN(node_->get_logger(), "Ignoring goal_constraints.position_constraints.constraint_region: " + RCLCPP_WARN(logger_, "Ignoring goal_constraints.position_constraints.constraint_region: " "Not supported."); } @@ -438,7 +438,7 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_query( } catch (tf2::TransformException& ex) { - RCLCPP_WARN(node_->get_logger(), + RCLCPP_WARN(logger_, "Skipping goal query append: " "Could not get goal transform for translation %s to %s: %s", plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), @@ -492,7 +492,7 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_query( } catch (tf2::TransformException& ex) { - RCLCPP_WARN(node_->get_logger(), + RCLCPP_WARN(logger_, "Skipping goal query append: " "Could not get goal transform for orientation %s to %s: %s", plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), @@ -542,11 +542,11 @@ bool TrajectoryCache::extract_and_append_trajectory_start_to_metadata( // Make ignored members explicit if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) { - RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.multi_dof_joint_states: Not supported."); + RCLCPP_WARN(logger_, "Ignoring start_state.multi_dof_joint_states: Not supported."); } if (!plan_request.start_state.attached_collision_objects.empty()) { - RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); + RCLCPP_WARN(logger_, "Ignoring start_state.attached_collision_objects: Not supported."); } metadata.append("group_name", plan_request.group_name); @@ -581,7 +581,7 @@ bool TrajectoryCache::extract_and_append_trajectory_start_to_metadata( moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); if (!current_state) { - RCLCPP_WARN(node_->get_logger(), "Skipping start metadata append: Could not get robot state."); + RCLCPP_WARN(logger_, "Skipping start metadata append: Could not get robot state."); // NOTE: methyldragon - // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. return false; @@ -634,7 +634,7 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_metadata( } if (emit_position_constraint_warning) { - RCLCPP_WARN(node_->get_logger(), "Ignoring goal_constraints.position_constraints.constraint_region: " + RCLCPP_WARN(logger_, "Ignoring goal_constraints.position_constraints.constraint_region: " "Not supported."); } @@ -708,7 +708,7 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_metadata( } catch (tf2::TransformException& ex) { - RCLCPP_WARN(node_->get_logger(), + RCLCPP_WARN(logger_, "Skipping goal metadata append: " "Could not get goal transform for translation %s to %s: %s", plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), @@ -759,7 +759,7 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_metadata( } catch (tf2::TransformException& ex) { - RCLCPP_WARN(node_->get_logger(), + RCLCPP_WARN(logger_, "Skipping goal metadata append: " "Could not get goal transform for orientation %s to %s: %s", plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), @@ -849,7 +849,7 @@ TrajectoryCache::fetch_all_matching_cartesian_trajectories( if (!start_ok || !goal_ok) { - RCLCPP_ERROR(node_->get_logger(), "Could not construct cartesian trajectory query."); + RCLCPP_ERROR(logger_, "Could not construct cartesian trajectory query."); return {}; } @@ -870,7 +870,7 @@ TrajectoryCache::fetch_best_matching_cartesian_trajectory( if (matching_trajectories.empty()) { - RCLCPP_DEBUG(node_->get_logger(), "No matching cartesian trajectories found."); + RCLCPP_DEBUG(logger_, "No matching cartesian trajectories found."); return nullptr; } @@ -896,13 +896,13 @@ bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface: // Check pre-conditions if (!trajectory.multi_dof_joint_trajectory.points.empty()) { - RCLCPP_ERROR(node_->get_logger(), "Skipping cartesian trajectory insert: " + RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: " "Multi-DOF trajectory plans are not supported."); return false; } if (plan_request.header.frame_id.empty() || trajectory.joint_trajectory.header.frame_id.empty()) { - RCLCPP_ERROR(node_->get_logger(), "Skipping cartesian trajectory insert: Frame IDs cannot be empty."); + RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: Frame IDs cannot be empty."); return false; } @@ -920,7 +920,7 @@ bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface: if (!start_query_ok || !goal_query_ok) { - RCLCPP_ERROR(node_->get_logger(), "Skipping cartesian trajectory insert: Could not construct lookup query."); + RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: Could not construct lookup query."); return false; } @@ -938,7 +938,7 @@ bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface: if (execution_time_s < match_execution_time_s) { int delete_id = match->lookupInt("id"); - RCLCPP_DEBUG(node_->get_logger(), + RCLCPP_DEBUG(logger_, "Overwriting cartesian trajectory (id: %d): " "execution_time (%es) > new trajectory's execution_time (%es)", delete_id, match_execution_time_s, execution_time_s); @@ -966,12 +966,12 @@ bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface: if (!start_meta_ok || !goal_meta_ok) { - RCLCPP_ERROR(node_->get_logger(), "Skipping cartesian trajectory insert: " + RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: " "Could not construct insert metadata."); return false; } - RCLCPP_DEBUG(node_->get_logger(), + RCLCPP_DEBUG(logger_, "Inserting cartesian trajectory: New trajectory execution_time (%es) " "is better than best trajectory's execution_time (%es) at fraction (%es)", execution_time_s, best_execution_time, fraction); @@ -980,7 +980,7 @@ bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface: return true; } - RCLCPP_DEBUG(node_->get_logger(), + RCLCPP_DEBUG(logger_, "Skipping cartesian trajectory insert: New trajectory execution_time (%es) " "is worse than best trajectory's execution_time (%es) at fraction (%es)", execution_time_s, best_execution_time, fraction); @@ -997,11 +997,11 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_query( // Make ignored members explicit if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) { - RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.multi_dof_joint_states: Not supported."); + RCLCPP_WARN(logger_, "Ignoring start_state.multi_dof_joint_states: Not supported."); } if (!plan_request.start_state.attached_collision_objects.empty()) { - RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); + RCLCPP_WARN(logger_, "Ignoring start_state.attached_collision_objects: Not supported."); } query.append("group_name", plan_request.group_name); @@ -1027,7 +1027,7 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_query( moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); if (!current_state) { - RCLCPP_WARN(node_->get_logger(), "Skipping start metadata append: Could not get robot state."); + RCLCPP_WARN(logger_, "Skipping start metadata append: Could not get robot state."); // NOTE: methyldragon - // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. return false; @@ -1068,11 +1068,11 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_query( !plan_request.path_constraints.orientation_constraints.empty() || !plan_request.path_constraints.visibility_constraints.empty()) { - RCLCPP_WARN(node_->get_logger(), "Ignoring path_constraints: Not supported."); + RCLCPP_WARN(logger_, "Ignoring path_constraints: Not supported."); } if (plan_request.avoid_collisions) { - RCLCPP_WARN(node_->get_logger(), "Ignoring avoid_collisions: Not supported."); + RCLCPP_WARN(logger_, "Ignoring avoid_collisions: Not supported."); } query_append_range_inclusive_with_tolerance(query, "max_velocity_scaling_factor", @@ -1110,7 +1110,7 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_query( } catch (tf2::TransformException& ex) { - RCLCPP_WARN(node_->get_logger(), + RCLCPP_WARN(logger_, "Skipping goal metadata append: " "Could not get goal transform for %s to %s: %s", base_frame.c_str(), plan_request.header.frame_id.c_str(), ex.what()); @@ -1166,11 +1166,11 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_metadata( // Make ignored members explicit if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) { - RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.multi_dof_joint_states: Not supported."); + RCLCPP_WARN(logger_, "Ignoring start_state.multi_dof_joint_states: Not supported."); } if (!plan_request.start_state.attached_collision_objects.empty()) { - RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); + RCLCPP_WARN(logger_, "Ignoring start_state.attached_collision_objects: Not supported."); } metadata.append("group_name", plan_request.group_name); @@ -1196,7 +1196,7 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_metadata( moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); if (!current_state) { - RCLCPP_WARN(node_->get_logger(), "Skipping start metadata append: Could not get robot state."); + RCLCPP_WARN(logger_, "Skipping start metadata append: Could not get robot state."); // NOTE: methyldragon - // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. return false; @@ -1236,11 +1236,11 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_metadata( !plan_request.path_constraints.orientation_constraints.empty() || !plan_request.path_constraints.visibility_constraints.empty()) { - RCLCPP_WARN(node_->get_logger(), "Ignoring path_constraints: Not supported."); + RCLCPP_WARN(logger_, "Ignoring path_constraints: Not supported."); } if (plan_request.avoid_collisions) { - RCLCPP_WARN(node_->get_logger(), "Ignoring avoid_collisions: Not supported."); + RCLCPP_WARN(logger_, "Ignoring avoid_collisions: Not supported."); } metadata.append("max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor); @@ -1276,7 +1276,7 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_metadata( } catch (tf2::TransformException& ex) { - RCLCPP_WARN(node_->get_logger(), + RCLCPP_WARN(logger_, "Skipping goal metadata append: " "Could not get goal transform for %s to %s: %s", base_frame.c_str(), plan_request.header.frame_id.c_str(), ex.what()); From cb8ab147d756a18a423eb3b1956eb857fa0b4eca Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 23 Jul 2024 17:39:51 -0700 Subject: [PATCH 07/26] Use new move_group accessors Signed-off-by: methylDragon --- .../trajectory_cache/src/trajectory_cache.cpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index e29f29e465..d2b59d2153 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -808,16 +808,11 @@ TrajectoryCache::construct_get_cartesian_path_request(moveit::planning_interface { moveit_msgs::srv::GetCartesianPath::Request out; - // Some of these parameters need us to pull PRIVATE values out of the - // move_group elsewhere... Yes, it is very cursed and I hate it. - // Fixing it requires fixing it in MoveIt. - moveit_msgs::msg::MotionPlanRequest tmp; - move_group.constructMotionPlanRequest(tmp); - - out.start_state = std::move(tmp.start_state); - out.group_name = std::move(tmp.group_name); - out.max_velocity_scaling_factor = tmp.max_velocity_scaling_factor; - out.max_acceleration_scaling_factor = tmp.max_acceleration_scaling_factor; + move_group.constructRobotState(out.start_state); + + out.group_name = move_group.getName(); + out.max_velocity_scaling_factor = move_group.getMaxVelocityScalingFactor(); + out.max_acceleration_scaling_factor = move_group.getMaxVelocityScalingFactor(); out.header.frame_id = move_group.getPoseReferenceFrame(); out.waypoints = waypoints; From deef7e0d01eae9c9b96396f10b17b7435ae60e6a Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 23 Jul 2024 23:05:39 -0700 Subject: [PATCH 08/26] Coerce variable and method names to style Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/README.md | 4 +- .../trajectory_cache/trajectory_cache.hpp | 117 +++---- .../trajectory_cache/src/trajectory_cache.cpp | 158 +++++----- .../test/test_trajectory_cache.cpp | 292 +++++++++--------- 4 files changed, 281 insertions(+), 290 deletions(-) diff --git a/moveit_ros/trajectory_cache/README.md b/moveit_ros/trajectory_cache/README.md index 6ee301c871..497f003ed9 100644 --- a/moveit_ros/trajectory_cache/README.md +++ b/moveit_ros/trajectory_cache/README.md @@ -54,7 +54,7 @@ If you use this package in your work, please cite it using the following: // - `warehouse_port`: The port used for the database auto traj_cache = std::make_shared(node); -auto fetched_trajectory = traj_cache->fetch_best_matching_trajectory( +auto fetched_trajectory = traj_cache->fetchBestMatchingTrajectory( *move_group_interface, robot_name, motion_plan_req_msg, _cache_start_match_tolerance, _cache_goal_match_tolerance, /*sort_by=*/"execution_time_s"); @@ -63,7 +63,7 @@ if (fetched_trajectory) { // Do something with the plan. } else { // Plan... And put it for posterity! - traj_cache->put_trajectory( + traj_cache->putTrajectory( *interface, robot_name, std::move(plan_req_msg), std::move(res->result.trajectory), rclcpp::Duration( res->result.trajectory.joint_trajectory.points.back().time_from_start diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index 5bc3af13db..22f6b45e64 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -122,9 +122,9 @@ class TrajectoryCache bool init(const std::string& db_path = ":memory:", uint32_t db_port = 0, double exact_match_precision = 1e-6); - unsigned count_trajectories(const std::string& move_group_namespace); + unsigned countTrajectories(const std::string& move_group_namespace); - unsigned count_cartesian_trajectories(const std::string& move_group_namespace); + unsigned countCartesianTrajectories(const std::string& move_group_namespace); // =========================================================================== // MOTION PLAN TRAJECTORY CACHING @@ -134,15 +134,15 @@ class TrajectoryCache // Fetches all plans that fit within the requested tolerances for start and // goal conditions, returning them as a vector, sorted by some db column. std::vector::ConstPtr> - fetch_all_matching_trajectories(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, - const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, - double goal_tolerance, bool metadata_only = false, - const std::string& sort_by = "execution_time_s"); + fetchAllMatchingTrajectories(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, + double goal_tolerance, bool metadata_only = false, + const std::string& sort_by = "execution_time_s"); // Fetches the best trajectory that fits within the requested tolerances for start // and goal conditions, by some db column. - warehouse_ros::MessageWithMetadata::ConstPtr fetch_best_matching_trajectory( + warehouse_ros::MessageWithMetadata::ConstPtr fetchBestMatchingTrajectory( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s"); @@ -154,30 +154,30 @@ class TrajectoryCache // than matching trajectories. // // Optionally deletes all worse trajectories by default to prune the cache. - bool put_trajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, - const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, - double planning_time_s, bool delete_worse_trajectories = true); + bool putTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, + const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, + double planning_time_s, bool delete_worse_trajectories = true); // QUERY CONSTRUCTION - bool extract_and_append_trajectory_start_to_query(warehouse_ros::Query& query, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request, - double match_tolerance); + bool extractAndAppendTrajectoryStartToQuery(warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance); - bool extract_and_append_trajectory_goal_to_query(warehouse_ros::Query& query, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request, - double match_tolerance); + bool extractAndAppendTrajectoryGoalToQuery(warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance); // METADATA CONSTRUCTION - bool extract_and_append_trajectory_start_to_metadata(warehouse_ros::Metadata& metadata, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request); + bool extractAndAppendTrajectoryStartToMetadata(warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request); - bool extract_and_append_trajectory_goal_to_metadata(warehouse_ros::Metadata& metadata, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request); + bool extractAndAppendTrajectoryGoalToMetadata(warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request); // =========================================================================== // CARTESIAN TRAJECTORY CACHING @@ -187,29 +187,26 @@ class TrajectoryCache // This mimics the move group computeCartesianPath signature (without path // constraints). moveit_msgs::srv::GetCartesianPath::Request - construct_get_cartesian_path_request(moveit::planning_interface::MoveGroupInterface& move_group, - const std::vector& waypoints, double step, - double jump_threshold, bool avoid_collisions = true); + constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group, + const std::vector& waypoints, double step, + double jump_threshold, bool avoid_collisions = true); // Fetches all cartesian trajectories that fit within the requested tolerances // for start and goal conditions, returning them as a vector, sorted by some // db column. std::vector::ConstPtr> - fetch_all_matching_cartesian_trajectories(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, - double min_fraction, double start_tolerance, double goal_tolerance, - bool metadata_only = false, - const std::string& sort_by = "execution_time_s"); + fetchAllMatchingCartesianTrajectories(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double min_fraction, double start_tolerance, double goal_tolerance, + bool metadata_only = false, const std::string& sort_by = "execution_time_s"); // Fetches the best cartesian trajectory that fits within the requested tolerances // for start and goal conditions, by some db column. - warehouse_ros::MessageWithMetadata::ConstPtr - fetch_best_matching_cartesian_trajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, - double min_fraction, double start_tolerance, double goal_tolerance, - bool metadata_only = false, const std::string& sort_by = "execution_time_s"); + warehouse_ros::MessageWithMetadata::ConstPtr fetchBestMatchingCartesianTrajectory( + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, + double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s"); // Put a cartesian trajectory into the database if it is the best matching // cartesian trajectory seen so far. @@ -220,29 +217,33 @@ class TrajectoryCache // // Optionally deletes all worse cartesian trajectories by default to prune the // cache. - bool put_cartesian_trajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, - const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, - double planning_time_s, double fraction, bool delete_worse_trajectories = true); + bool putCartesianTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, + double planning_time_s, double fraction, bool delete_worse_trajectories = true); // QUERY CONSTRUCTION - bool extract_and_append_cartesian_trajectory_start_to_query( - warehouse_ros::Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance); + bool extractAndAppendCartesianTrajectoryStartToQuery(warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double match_tolerance); - bool extract_and_append_cartesian_trajectory_goal_to_query( - warehouse_ros::Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance); + bool extractAndAppendCartesianTrajectoryGoalToQuery(warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double match_tolerance); // METADATA CONSTRUCTION - bool extract_and_append_cartesian_trajectory_start_to_metadata( - warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request); + bool + extractAndAppendCartesianTrajectoryStartToMetadata(warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request); - bool extract_and_append_cartesian_trajectory_goal_to_metadata( - warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request); + bool + extractAndAppendCartesianTrajectoryGoalToMetadata(warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request); private: rclcpp::Node::SharedPtr node_; diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index d2b59d2153..80527d5247 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -64,7 +64,8 @@ void sort_constraints(std::vector& joint_cons // Trajectory Cache ============================================================ -TrajectoryCache::TrajectoryCache(const rclcpp::Node::SharedPtr& node) : node_(node), logger_(moveit::getLogger("moveit.ros.trajectory_cache")) +TrajectoryCache::TrajectoryCache(const rclcpp::Node::SharedPtr& node) + : node_(node), logger_(moveit::getLogger("moveit.ros.trajectory_cache")) { tf_buffer_ = std::make_unique(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -72,8 +73,8 @@ TrajectoryCache::TrajectoryCache(const rclcpp::Node::SharedPtr& node) : node_(no bool TrajectoryCache::init(const std::string& db_path, uint32_t db_port, double exact_match_precision) { - RCLCPP_DEBUG(logger_, "Opening trajectory cache database at: %s (Port: %d, Precision: %f)", - db_path.c_str(), db_port, exact_match_precision); + RCLCPP_DEBUG(logger_, "Opening trajectory cache database at: %s (Port: %d, Precision: %f)", db_path.c_str(), db_port, + exact_match_precision); // If the `warehouse_plugin` parameter isn't set, defaults to warehouse_ros' // default. @@ -84,14 +85,14 @@ bool TrajectoryCache::init(const std::string& db_path, uint32_t db_port, double return db_->connect(); } -unsigned TrajectoryCache::count_trajectories(const std::string& move_group_namespace) +unsigned TrajectoryCache::countTrajectories(const std::string& move_group_namespace) { auto coll = db_->openCollection("move_group_trajectory_cache", move_group_namespace); return coll.count(); } -unsigned TrajectoryCache::count_cartesian_trajectories(const std::string& move_group_namespace) +unsigned TrajectoryCache::countCartesianTrajectories(const std::string& move_group_namespace) { auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", move_group_namespace); @@ -103,19 +104,19 @@ unsigned TrajectoryCache::count_cartesian_trajectories(const std::string& move_g // ============================================================================= // MOTION PLAN TRAJECTORY CACHING: TOP LEVEL OPS std::vector::ConstPtr> -TrajectoryCache::fetch_all_matching_trajectories(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, - const moveit_msgs::msg::MotionPlanRequest& plan_request, - double start_tolerance, double goal_tolerance, bool metadata_only, - const std::string& sort_by) +TrajectoryCache::fetchAllMatchingTrajectories(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double start_tolerance, double goal_tolerance, bool metadata_only, + const std::string& sort_by) { auto coll = db_->openCollection("move_group_trajectory_cache", move_group_namespace); Query::Ptr query = coll.createQuery(); - bool start_ok = this->extract_and_append_trajectory_start_to_query(*query, move_group, plan_request, start_tolerance); - bool goal_ok = this->extract_and_append_trajectory_goal_to_query(*query, move_group, plan_request, goal_tolerance); + bool start_ok = this->extractAndAppendTrajectoryStartToQuery(*query, move_group, plan_request, start_tolerance); + bool goal_ok = this->extractAndAppendTrajectoryGoalToQuery(*query, move_group, plan_request, goal_tolerance); if (!start_ok || !goal_ok) { @@ -126,15 +127,15 @@ TrajectoryCache::fetch_all_matching_trajectories(const moveit::planning_interfac return coll.queryList(query, metadata_only, sort_by, true); } -MessageWithMetadata::ConstPtr TrajectoryCache::fetch_best_matching_trajectory( +MessageWithMetadata::ConstPtr TrajectoryCache::fetchBestMatchingTrajectory( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, bool metadata_only, const std::string& sort_by) { // First find all matching, but metadata only. // Then use the ID metadata of the best plan to pull the actual message. - auto matching_trajectories = this->fetch_all_matching_trajectories(move_group, move_group_namespace, plan_request, - start_tolerance, goal_tolerance, true, sort_by); + auto matching_trajectories = this->fetchAllMatchingTrajectories(move_group, move_group_namespace, plan_request, + start_tolerance, goal_tolerance, true, sort_by); if (matching_trajectories.empty()) { @@ -154,11 +155,11 @@ MessageWithMetadata::ConstPtr TrajectoryCache return coll.findOne(best_query, metadata_only); } -bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, - const moveit_msgs::msg::MotionPlanRequest& plan_request, - const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, - double planning_time_s, bool delete_worse_trajectories) +bool TrajectoryCache::putTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, + double planning_time_s, bool delete_worse_trajectories) { // Check pre-conditions if (!trajectory.multi_dof_joint_trajectory.points.empty()) @@ -187,8 +188,8 @@ bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroup // Pull out trajectories "exactly" keyed by request in cache. Query::Ptr exact_query = coll.createQuery(); - bool start_query_ok = this->extract_and_append_trajectory_start_to_query(*exact_query, move_group, plan_request, 0); - bool goal_query_ok = this->extract_and_append_trajectory_goal_to_query(*exact_query, move_group, plan_request, 0); + bool start_query_ok = this->extractAndAppendTrajectoryStartToQuery(*exact_query, move_group, plan_request, 0); + bool goal_query_ok = this->extractAndAppendTrajectoryGoalToQuery(*exact_query, move_group, plan_request, 0); if (!start_query_ok || !goal_query_ok) { @@ -212,9 +213,9 @@ bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroup { int delete_id = match->lookupInt("id"); RCLCPP_DEBUG(logger_, - "Overwriting plan (id: %d): " - "execution_time (%es) > new trajectory's execution_time (%es)", - delete_id, match_execution_time_s, execution_time_s); + "Overwriting plan (id: %d): " + "execution_time (%es) > new trajectory's execution_time (%es)", + delete_id, match_execution_time_s, execution_time_s); Query::Ptr delete_query = coll.createQuery(); delete_query->append("id", delete_id); @@ -229,10 +230,8 @@ bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroup { Metadata::Ptr insert_metadata = coll.createMetadata(); - bool start_meta_ok = - this->extract_and_append_trajectory_start_to_metadata(*insert_metadata, move_group, plan_request); - bool goal_meta_ok = - this->extract_and_append_trajectory_goal_to_metadata(*insert_metadata, move_group, plan_request); + bool start_meta_ok = this->extractAndAppendTrajectoryStartToMetadata(*insert_metadata, move_group, plan_request); + bool goal_meta_ok = this->extractAndAppendTrajectoryGoalToMetadata(*insert_metadata, move_group, plan_request); insert_metadata->append("execution_time_s", execution_time_s); insert_metadata->append("planning_time_s", planning_time_s); @@ -243,23 +242,23 @@ bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroup } RCLCPP_DEBUG(logger_, - "Inserting trajectory: New trajectory execution_time (%es) " - "is better than best trajectory's execution_time (%es)", - execution_time_s, best_execution_time); + "Inserting trajectory: New trajectory execution_time (%es) " + "is better than best trajectory's execution_time (%es)", + execution_time_s, best_execution_time); coll.insert(trajectory, insert_metadata); return true; } RCLCPP_DEBUG(logger_, - "Skipping plan insert: New trajectory execution_time (%es) " - "is worse than best trajectory's execution_time (%es)", - execution_time_s, best_execution_time); + "Skipping plan insert: New trajectory execution_time (%es) " + "is worse than best trajectory's execution_time (%es)", + execution_time_s, best_execution_time); return false; } // MOTION PLAN TRAJECTORY CACHING: QUERY CONSTRUCTION -bool TrajectoryCache::extract_and_append_trajectory_start_to_query( +bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) { @@ -336,7 +335,7 @@ bool TrajectoryCache::extract_and_append_trajectory_start_to_query( return true; } -bool TrajectoryCache::extract_and_append_trajectory_goal_to_query( +bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( Query& query, const moveit::planning_interface::MoveGroupInterface& /* move_group */, const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) { @@ -362,7 +361,7 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_query( if (emit_position_constraint_warning) { RCLCPP_WARN(logger_, "Ignoring goal_constraints.position_constraints.constraint_region: " - "Not supported."); + "Not supported."); } query_append_range_inclusive_with_tolerance(query, "max_velocity_scaling_factor", @@ -535,7 +534,7 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_query( } // MOTION PLAN TRAJECTORY CACHING: METADATA CONSTRUCTION -bool TrajectoryCache::extract_and_append_trajectory_start_to_metadata( +bool TrajectoryCache::extractAndAppendTrajectoryStartToMetadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request) { @@ -611,7 +610,7 @@ bool TrajectoryCache::extract_and_append_trajectory_start_to_metadata( return true; } -bool TrajectoryCache::extract_and_append_trajectory_goal_to_metadata( +bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& /* move_group */, const moveit_msgs::msg::MotionPlanRequest& plan_request) { @@ -635,7 +634,7 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_metadata( if (emit_position_constraint_warning) { RCLCPP_WARN(logger_, "Ignoring goal_constraints.position_constraints.constraint_region: " - "Not supported."); + "Not supported."); } metadata.append("max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor); @@ -802,9 +801,9 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_metadata( // ============================================================================= // CARTESIAN TRAJECTORY CACHING: TOP LEVEL OPS moveit_msgs::srv::GetCartesianPath::Request -TrajectoryCache::construct_get_cartesian_path_request(moveit::planning_interface::MoveGroupInterface& move_group, - const std::vector& waypoints, - double step, double jump_threshold, bool avoid_collisions) +TrajectoryCache::constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group, + const std::vector& waypoints, double step, + double jump_threshold, bool avoid_collisions) { moveit_msgs::srv::GetCartesianPath::Request out; @@ -827,10 +826,12 @@ TrajectoryCache::construct_get_cartesian_path_request(moveit::planning_interface } std::vector::ConstPtr> -TrajectoryCache::fetch_all_matching_cartesian_trajectories( - const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, - double goal_tolerance, bool metadata_only, const std::string& sort_by) +TrajectoryCache::fetchAllMatchingCartesianTrajectories(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double min_fraction, double start_tolerance, + double goal_tolerance, bool metadata_only, + const std::string& sort_by) { auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", move_group_namespace); @@ -838,9 +839,8 @@ TrajectoryCache::fetch_all_matching_cartesian_trajectories( Query::Ptr query = coll.createQuery(); bool start_ok = - this->extract_and_append_cartesian_trajectory_start_to_query(*query, move_group, plan_request, start_tolerance); - bool goal_ok = - this->extract_and_append_cartesian_trajectory_goal_to_query(*query, move_group, plan_request, goal_tolerance); + this->extractAndAppendCartesianTrajectoryStartToQuery(*query, move_group, plan_request, start_tolerance); + bool goal_ok = this->extractAndAppendCartesianTrajectoryGoalToQuery(*query, move_group, plan_request, goal_tolerance); if (!start_ok || !goal_ok) { @@ -852,15 +852,14 @@ TrajectoryCache::fetch_all_matching_cartesian_trajectories( return coll.queryList(query, metadata_only, sort_by, true); } -MessageWithMetadata::ConstPtr -TrajectoryCache::fetch_best_matching_cartesian_trajectory( +MessageWithMetadata::ConstPtr TrajectoryCache::fetchBestMatchingCartesianTrajectory( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, double goal_tolerance, bool metadata_only, const std::string& sort_by) { // First find all matching, but metadata only. // Then use the ID metadata of the best plan to pull the actual message. - auto matching_trajectories = this->fetch_all_matching_cartesian_trajectories( + auto matching_trajectories = this->fetchAllMatchingCartesianTrajectories( move_group, move_group_namespace, plan_request, min_fraction, start_tolerance, goal_tolerance, true, sort_by); if (matching_trajectories.empty()) @@ -881,18 +880,18 @@ TrajectoryCache::fetch_best_matching_cartesian_trajectory( return coll.findOne(best_query, metadata_only); } -bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, - const moveit_msgs::msg::RobotTrajectory& trajectory, - double execution_time_s, double planning_time_s, double fraction, - bool delete_worse_trajectories) +bool TrajectoryCache::putCartesianTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + const moveit_msgs::msg::RobotTrajectory& trajectory, + double execution_time_s, double planning_time_s, double fraction, + bool delete_worse_trajectories) { // Check pre-conditions if (!trajectory.multi_dof_joint_trajectory.points.empty()) { RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: " - "Multi-DOF trajectory plans are not supported."); + "Multi-DOF trajectory plans are not supported."); return false; } if (plan_request.header.frame_id.empty() || trajectory.joint_trajectory.header.frame_id.empty()) @@ -908,9 +907,8 @@ bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface: Query::Ptr exact_query = coll.createQuery(); bool start_query_ok = - this->extract_and_append_cartesian_trajectory_start_to_query(*exact_query, move_group, plan_request, 0); - bool goal_query_ok = - this->extract_and_append_cartesian_trajectory_goal_to_query(*exact_query, move_group, plan_request, 0); + this->extractAndAppendCartesianTrajectoryStartToQuery(*exact_query, move_group, plan_request, 0); + bool goal_query_ok = this->extractAndAppendCartesianTrajectoryGoalToQuery(*exact_query, move_group, plan_request, 0); exact_query->append("fraction", fraction); if (!start_query_ok || !goal_query_ok) @@ -934,9 +932,9 @@ bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface: { int delete_id = match->lookupInt("id"); RCLCPP_DEBUG(logger_, - "Overwriting cartesian trajectory (id: %d): " - "execution_time (%es) > new trajectory's execution_time (%es)", - delete_id, match_execution_time_s, execution_time_s); + "Overwriting cartesian trajectory (id: %d): " + "execution_time (%es) > new trajectory's execution_time (%es)", + delete_id, match_execution_time_s, execution_time_s); Query::Ptr delete_query = coll.createQuery(); delete_query->append("id", delete_id); @@ -952,9 +950,9 @@ bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface: Metadata::Ptr insert_metadata = coll.createMetadata(); bool start_meta_ok = - this->extract_and_append_cartesian_trajectory_start_to_metadata(*insert_metadata, move_group, plan_request); + this->extractAndAppendCartesianTrajectoryStartToMetadata(*insert_metadata, move_group, plan_request); bool goal_meta_ok = - this->extract_and_append_cartesian_trajectory_goal_to_metadata(*insert_metadata, move_group, plan_request); + this->extractAndAppendCartesianTrajectoryGoalToMetadata(*insert_metadata, move_group, plan_request); insert_metadata->append("execution_time_s", execution_time_s); insert_metadata->append("planning_time_s", planning_time_s); insert_metadata->append("fraction", fraction); @@ -962,28 +960,28 @@ bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface: if (!start_meta_ok || !goal_meta_ok) { RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: " - "Could not construct insert metadata."); + "Could not construct insert metadata."); return false; } RCLCPP_DEBUG(logger_, - "Inserting cartesian trajectory: New trajectory execution_time (%es) " - "is better than best trajectory's execution_time (%es) at fraction (%es)", - execution_time_s, best_execution_time, fraction); + "Inserting cartesian trajectory: New trajectory execution_time (%es) " + "is better than best trajectory's execution_time (%es) at fraction (%es)", + execution_time_s, best_execution_time, fraction); coll.insert(trajectory, insert_metadata); return true; } RCLCPP_DEBUG(logger_, - "Skipping cartesian trajectory insert: New trajectory execution_time (%es) " - "is worse than best trajectory's execution_time (%es) at fraction (%es)", - execution_time_s, best_execution_time, fraction); + "Skipping cartesian trajectory insert: New trajectory execution_time (%es) " + "is worse than best trajectory's execution_time (%es) at fraction (%es)", + execution_time_s, best_execution_time, fraction); return false; } // CARTESIAN TRAJECTORY CACHING: QUERY CONSTRUCTION -bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_query( +bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) { @@ -1051,7 +1049,7 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_query( return true; } -bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_query( +bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) { @@ -1154,7 +1152,7 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_query( } // CARTESIAN TRAJECTORY CACHING: METADATA CONSTRUCTION -bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_metadata( +bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToMetadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request) { @@ -1221,7 +1219,7 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_metadata( return true; } -bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_metadata( +bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToMetadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request) { diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp index 63002bc7d1..138761c029 100644 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp @@ -24,8 +24,8 @@ using moveit::planning_interface::MoveGroupInterface; using moveit_ros::trajectory_cache::TrajectoryCache; -const std::string g_robot_name = "panda"; -const std::string g_robot_frame = "world"; +const std::string ROBOT_NAME = "panda"; +const std::string ROBOT_FRAME = "world"; // UTILS ======================================================================= // Utility function to emit a pass or fail statement. @@ -48,15 +48,15 @@ moveit_msgs::msg::RobotTrajectory get_dummy_panda_traj() moveit_msgs::msg::RobotTrajectory traj; auto trajectory = &traj.joint_trajectory; - trajectory->header.frame_id = g_robot_frame; + trajectory->header.frame_id = ROBOT_FRAME; - trajectory->joint_names.push_back(g_robot_name + "_joint1"); - trajectory->joint_names.push_back(g_robot_name + "_joint2"); - trajectory->joint_names.push_back(g_robot_name + "_joint3"); - trajectory->joint_names.push_back(g_robot_name + "_joint4"); - trajectory->joint_names.push_back(g_robot_name + "_joint5"); - trajectory->joint_names.push_back(g_robot_name + "_joint6"); - trajectory->joint_names.push_back(g_robot_name + "_joint7"); + trajectory->joint_names.push_back(ROBOT_NAME + "_joint1"); + trajectory->joint_names.push_back(ROBOT_NAME + "_joint2"); + trajectory->joint_names.push_back(ROBOT_NAME + "_joint3"); + trajectory->joint_names.push_back(ROBOT_NAME + "_joint4"); + trajectory->joint_names.push_back(ROBOT_NAME + "_joint5"); + trajectory->joint_names.push_back(ROBOT_NAME + "_joint6"); + trajectory->joint_names.push_back(ROBOT_NAME + "_joint7"); trajectory->points.emplace_back(); trajectory->points.at(0).positions = { 0, 0, 0, 0, 0, 0 }; @@ -99,7 +99,7 @@ void test_motion_trajectories(std::shared_ptr move_group, st // Plain start moveit_msgs::msg::MotionPlanRequest plan_req; move_group->constructMotionPlanRequest(plan_req); - plan_req.workspace_parameters.header.frame_id = g_robot_frame; + plan_req.workspace_parameters.header.frame_id = ROBOT_FRAME; plan_req.workspace_parameters.max_corner.x = 10; plan_req.workspace_parameters.max_corner.y = 10; plan_req.workspace_parameters.max_corner.z = 10; @@ -192,58 +192,58 @@ void test_motion_trajectories(std::shared_ptr move_group, st // Initially empty prefix = "test_motion_trajectories.empty"; - check_and_emit(cache->count_trajectories(g_robot_name) == 0, prefix, "Trajectory cache initially empty"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 0, prefix, "Trajectory cache initially empty"); - check_and_emit(cache->fetch_all_matching_trajectories(*move_group, g_robot_name, plan_req, 999, 999).empty(), prefix, + check_and_emit(cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 999, 999).empty(), prefix, "Fetch all trajectories on empty cache returns empty"); - check_and_emit(cache->fetch_best_matching_trajectory(*move_group, g_robot_name, plan_req, 999, 999) == nullptr, - prefix, "Fetch best trajectory on empty cache returns nullptr"); + check_and_emit(cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 999, 999) == nullptr, prefix, + "Fetch best trajectory on empty cache returns nullptr"); // Put trajectory empty frame // // Trajectory must have frame in joint trajectory, expect put fail - prefix = "test_motion_trajectories.put_trajectory_empty_frame"; + prefix = "test_motion_trajectories.putTrajectory_empty_frame"; double execution_time = 999; double planning_time = 999; - check_and_emit(!cache->put_trajectory(*move_group, g_robot_name, plan_req, empty_frame_traj, execution_time, - planning_time, false), + check_and_emit(!cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, empty_frame_traj, execution_time, + planning_time, false), prefix, "Put empty frame trajectory, no delete_worse_trajectories, not ok"); - check_and_emit(cache->count_trajectories(g_robot_name) == 0, prefix, "No trajectories in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 0, prefix, "No trajectories in cache"); // Put trajectory req empty frame // // Trajectory request must have frame in workspace, expect put fail - prefix = "test_motion_trajectories.put_trajectory_req_empty_frame"; + prefix = "test_motion_trajectories.putTrajectory_req_empty_frame"; execution_time = 999; planning_time = 999; - check_and_emit(!cache->put_trajectory(*move_group, g_robot_name, empty_frame_plan_req, traj, execution_time, - planning_time, false), + check_and_emit(!cache->putTrajectory(*move_group, ROBOT_NAME, empty_frame_plan_req, traj, execution_time, + planning_time, false), prefix, "Put empty frame req trajectory, no delete_worse_trajectories, not ok"); - check_and_emit(cache->count_trajectories(g_robot_name) == 0, prefix, "No trajectories in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 0, prefix, "No trajectories in cache"); // Put first, no delete_worse_trajectories prefix = "test_motion_trajectories.put_first"; execution_time = 999; planning_time = 999; - check_and_emit(cache->put_trajectory(*move_group, g_robot_name, plan_req, traj, execution_time, planning_time, false), + check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, traj, execution_time, planning_time, false), prefix, "Put first valid trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->count_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); // Fetch matching, no tolerance // // Exact key match should have cache hit prefix = "test_motion_trajectories.put_first.fetch_matching_no_tolerance"; - auto fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, plan_req, 0, 0); + auto fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0); - auto fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, plan_req, 0, 0); + auto fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0); check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); @@ -264,9 +264,9 @@ void test_motion_trajectories(std::shared_ptr move_group, st // change, hence should have cache hit prefix = "test_motion_trajectories.put_first.fetch_is_diff_no_tolerance"; - fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, is_diff_plan_req, 0, 0); + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, is_diff_plan_req, 0, 0); - fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, is_diff_plan_req, 0, 0); + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, is_diff_plan_req, 0, 0); check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); @@ -286,10 +286,9 @@ void test_motion_trajectories(std::shared_ptr move_group, st // Non-matching key should not have cache hit prefix = "test_motion_trajectories.put_first.fetch_non_matching_out_of_tolerance"; - fetched_trajectories = - cache->fetch_all_matching_trajectories(*move_group, g_robot_name, close_matching_plan_req, 0, 0); + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 0); - fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, close_matching_plan_req, 0, 0); + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 0); check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); @@ -299,10 +298,9 @@ void test_motion_trajectories(std::shared_ptr move_group, st // Non-matching key should not have cache hit prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_start_in_tolerance"; - fetched_trajectories = - cache->fetch_all_matching_trajectories(*move_group, g_robot_name, close_matching_plan_req, 999, 0); + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 999, 0); - fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, close_matching_plan_req, 999, 0); + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 999, 0); check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); @@ -312,10 +310,9 @@ void test_motion_trajectories(std::shared_ptr move_group, st // Non-matching key should not have cache hit prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_goal_in_tolerance"; - fetched_trajectories = - cache->fetch_all_matching_trajectories(*move_group, g_robot_name, close_matching_plan_req, 0, 999); + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 999); - fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, close_matching_plan_req, 0, 999); + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 999); check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); @@ -326,9 +323,9 @@ void test_motion_trajectories(std::shared_ptr move_group, st prefix = "test_motion_trajectories.put_first.fetch_non_matching_in_tolerance"; fetched_trajectories = - cache->fetch_all_matching_trajectories(*move_group, g_robot_name, close_matching_plan_req, 0.1, 0.1); + cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0.1, 0.1); - fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, close_matching_plan_req, 0.1, 0.1); + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0.1, 0.1); check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); @@ -349,10 +346,9 @@ void test_motion_trajectories(std::shared_ptr move_group, st prefix = "test_motion_trajectories.put_first.fetch_swapped"; fetched_trajectories = - cache->fetch_all_matching_trajectories(*move_group, g_robot_name, swapped_close_matching_plan_req, 0.1, 0.1); + cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, swapped_close_matching_plan_req, 0.1, 0.1); - fetched_traj = - cache->fetch_best_matching_trajectory(*move_group, g_robot_name, swapped_close_matching_plan_req, 0.1, 0.1); + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, swapped_close_matching_plan_req, 0.1, 0.1); check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); @@ -374,9 +370,9 @@ void test_motion_trajectories(std::shared_ptr move_group, st prefix = "test_motion_trajectories.put_first.fetch_smaller_workspace"; fetched_trajectories = - cache->fetch_all_matching_trajectories(*move_group, g_robot_name, smaller_workspace_plan_req, 999, 999); + cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, smaller_workspace_plan_req, 999, 999); - fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, smaller_workspace_plan_req, 999, 999); + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, smaller_workspace_plan_req, 999, 999); check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); @@ -388,9 +384,9 @@ void test_motion_trajectories(std::shared_ptr move_group, st prefix = "test_motion_trajectories.put_first.fetch_larger_workspace"; fetched_trajectories = - cache->fetch_all_matching_trajectories(*move_group, g_robot_name, larger_workspace_plan_req, 999, 999); + cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, larger_workspace_plan_req, 999, 999); - fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, larger_workspace_plan_req, 999, 999); + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, larger_workspace_plan_req, 999, 999); check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); @@ -419,11 +415,11 @@ void test_motion_trajectories(std::shared_ptr move_group, st prefix = "test_motion_trajectories.put_worse"; double worse_execution_time = execution_time + 100; - check_and_emit(!cache->put_trajectory(*move_group, g_robot_name, plan_req, traj, worse_execution_time, planning_time, - false), + check_and_emit(!cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, traj, worse_execution_time, planning_time, + false), prefix, "Put worse trajectory, no delete_worse_trajectories, not ok"); - check_and_emit(cache->count_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); // Put better, no delete_worse_trajectories // @@ -431,20 +427,20 @@ void test_motion_trajectories(std::shared_ptr move_group, st prefix = "test_motion_trajectories.put_better"; double better_execution_time = execution_time - 100; - check_and_emit(cache->put_trajectory(*move_group, g_robot_name, plan_req, traj, better_execution_time, planning_time, - false), + check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, traj, better_execution_time, planning_time, + false), prefix, "Put better trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->count_trajectories(g_robot_name) == 2, prefix, "Two trajectories in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); // Fetch sorted // // With multiple trajectories in cache, fetches should be sorted accordingly prefix = "test_motion_trajectories.put_better.fetch_sorted"; - fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, plan_req, 0, 0); + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0); - fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, plan_req, 0, 0); + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0); check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); @@ -469,18 +465,18 @@ void test_motion_trajectories(std::shared_ptr move_group, st prefix = "test_motion_trajectories.put_better_delete_worse_trajectories"; double even_better_execution_time = better_execution_time - 100; - check_and_emit(cache->put_trajectory(*move_group, g_robot_name, plan_req, different_traj, even_better_execution_time, - planning_time, true), + check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, different_traj, even_better_execution_time, + planning_time, true), prefix, "Put better trajectory, delete_worse_trajectories, ok"); - check_and_emit(cache->count_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); // Fetch better plan prefix = "test_motion_trajectories.put_better_delete_worse_trajectories.fetch"; - fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, plan_req, 0, 0); + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0); - fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, plan_req, 0, 0); + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0); check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); @@ -500,20 +496,20 @@ void test_motion_trajectories(std::shared_ptr move_group, st // Unrelated trajectory requests should live alongside pre-existing plans prefix = "test_motion_trajectories.put_different_req"; - check_and_emit(cache->put_trajectory(*move_group, g_robot_name, different_plan_req, different_traj, - better_execution_time, planning_time, true), + check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, different_plan_req, different_traj, + better_execution_time, planning_time, true), prefix, "Put different trajectory req, delete_worse_trajectories, ok"); - check_and_emit(cache->count_trajectories(g_robot_name) == 2, prefix, "Two trajectories in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); // Fetch with different trajectory req // // With multiple trajectories in cache, fetches should be sorted accordingly prefix = "test_motion_trajectories.put_different_req.fetch"; - fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, different_plan_req, 0, 0); + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, different_plan_req, 0, 0); - fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, different_plan_req, 0, 0); + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, different_plan_req, 0, 0); check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); @@ -534,23 +530,23 @@ void test_motion_trajectories(std::shared_ptr move_group, st prefix = "test_motion_trajectories.different_robot.empty"; std::string different_robot_name = "different_robot"; - check_and_emit(cache->count_cartesian_trajectories(different_robot_name) == 0, prefix, "No trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 0, prefix, "No trajectories in cache"); // Put first for different robot, delete_worse_trajectories // // A different robot's cache should not interact with the original cache prefix = "test_motion_trajectories.different_robot.put_first"; - check_and_emit(cache->put_trajectory(*move_group, different_robot_name, different_plan_req, different_traj, - better_execution_time, planning_time, true), + check_and_emit(cache->putTrajectory(*move_group, different_robot_name, different_plan_req, different_traj, + better_execution_time, planning_time, true), prefix, "Put different trajectory req, delete_worse_trajectories, ok"); - check_and_emit(cache->count_trajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); + check_and_emit(cache->countTrajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); - check_and_emit(cache->count_trajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); + check_and_emit(cache->countTrajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); - check_and_emit(cache->count_trajectories(g_robot_name) == 2, prefix, "Two trajectories in original robot's cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in original robot's cache"); - fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, different_plan_req, 0, 0); + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, different_plan_req, 0, 0); check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all on original still returns one"); } @@ -562,13 +558,13 @@ void test_cartesian_trajectories(std::shared_ptr move_group, /// First, test if construction even works... // Construct get cartesian trajectory request - prefix = "test_cartesian_trajectories.construct_get_cartesian_path_request"; + prefix = "test_cartesian_trajectories.constructGetCartesianPathRequest"; int test_step = 1; int test_jump = 2; auto test_waypoints = get_dummy_waypoints(); auto cartesian_plan_req_under_test = - cache->construct_get_cartesian_path_request(*move_group, test_waypoints, test_step, test_jump, false); + cache->constructGetCartesianPathRequest(*move_group, test_waypoints, test_step, test_jump, false); check_and_emit(cartesian_plan_req_under_test.waypoints == test_waypoints && static_cast(cartesian_plan_req_under_test.max_step) == test_step && @@ -583,7 +579,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // Plain start auto waypoints = get_dummy_waypoints(); - auto cartesian_plan_req = cache->construct_get_cartesian_path_request(*move_group, waypoints, 1, 1, false); + auto cartesian_plan_req = cache->constructGetCartesianPathRequest(*move_group, waypoints, 1, 1, false); cartesian_plan_req.start_state.multi_dof_joint_state.joint_names.clear(); cartesian_plan_req.start_state.multi_dof_joint_state.transforms.clear(); cartesian_plan_req.start_state.multi_dof_joint_state.twist.clear(); @@ -643,54 +639,53 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // Initially empty prefix = "test_cartesian_trajectories.empty"; - check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 0, prefix, "Trajectory cache initially empty"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, prefix, "Trajectory cache initially empty"); check_and_emit( - cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, 0, 999, 999) - .empty(), + cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999).empty(), prefix, "Fetch all trajectories on empty cache returns empty"); - check_and_emit(cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, 0, 999, - 999) == nullptr, + check_and_emit(cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, + 999) == nullptr, prefix, "Fetch best trajectory on empty cache returns nullptr"); // Put trajectory empty frame // // Trajectory must have frame in joint trajectory, expect put fail - prefix = "test_cartesian_trajectories.put_trajectory_empty_frame"; + prefix = "test_cartesian_trajectories.putTrajectory_empty_frame"; double execution_time = 999; double planning_time = 999; double fraction = 0.5; - check_and_emit(!cache->put_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, empty_frame_traj, - execution_time, planning_time, fraction, false), + check_and_emit(!cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, empty_frame_traj, + execution_time, planning_time, fraction, false), prefix, "Put empty frame trajectory, no delete_worse_trajectories, not ok"); - check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 0, prefix, "No trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, prefix, "No trajectories in cache"); // Put trajectory req empty frame // // Trajectory request must have frame in workspace, expect put fail - prefix = "test_cartesian_trajectories.put_trajectory_req_empty_frame"; + prefix = "test_cartesian_trajectories.putTrajectory_req_empty_frame"; execution_time = 999; planning_time = 999; - check_and_emit(!cache->put_cartesian_trajectory(*move_group, g_robot_name, empty_frame_cartesian_plan_req, traj, - execution_time, planning_time, fraction, false), + check_and_emit(!cache->putCartesianTrajectory(*move_group, ROBOT_NAME, empty_frame_cartesian_plan_req, traj, + execution_time, planning_time, fraction, false), prefix, "Put empty frame req trajectory, no delete_worse_trajectories, not ok"); - check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 0, prefix, "No trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, prefix, "No trajectories in cache"); // Put first, no delete_worse_trajectories prefix = "test_cartesian_trajectories.put_first"; execution_time = 999; planning_time = 999; - check_and_emit(cache->put_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, traj, execution_time, - planning_time, fraction, false), + check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, execution_time, + planning_time, fraction, false), prefix, "Put first valid trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); // Fetch matching, no tolerance // @@ -698,10 +693,10 @@ void test_cartesian_trajectories(std::shared_ptr move_group, prefix = "test_cartesian_trajectories.put_first.fetch_matching_no_tolerance"; auto fetched_trajectories = - cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); auto fetched_traj = - cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); @@ -724,11 +719,11 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // change, hence should have cache hit prefix = "test_cartesian_trajectories.put_first.fetch_is_diff_no_tolerance"; - fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, - is_diff_cartesian_plan_req, fraction, 0, 0); + fetched_trajectories = + cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, is_diff_cartesian_plan_req, fraction, 0, 0); - fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, is_diff_cartesian_plan_req, - fraction, 0, 0); + fetched_traj = + cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, is_diff_cartesian_plan_req, fraction, 0, 0); check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); @@ -750,11 +745,11 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // Non-matching key should not have cache hit prefix = "test_cartesian_trajectories.put_first.fetch_non_matching_out_of_tolerance"; - fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories( - *move_group, g_robot_name, close_matching_cartesian_plan_req, fraction, 0, 0); + fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( + *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 0); - fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, - close_matching_cartesian_plan_req, fraction, 0, 0); + fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, + fraction, 0, 0); check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); @@ -764,11 +759,11 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // Non-matching key should not have cache hit prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_start_in_tolerance"; - fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories( - *move_group, g_robot_name, close_matching_cartesian_plan_req, fraction, 999, 0); + fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( + *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 999, 0); - fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, - close_matching_cartesian_plan_req, fraction, 999, 0); + fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, + fraction, 999, 0); check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); @@ -778,11 +773,11 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // Non-matching key should not have cache hit prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_goal_in_tolerance"; - fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories( - *move_group, g_robot_name, close_matching_cartesian_plan_req, fraction, 0, 999); + fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( + *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 999); - fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, - close_matching_cartesian_plan_req, fraction, 0, 999); + fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, + fraction, 0, 999); check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); @@ -792,11 +787,11 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // Close key within tolerance limit should have cache hit prefix = "test_cartesian_trajectories.put_first.fetch_non_matching_in_tolerance"; - fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories( - *move_group, g_robot_name, close_matching_cartesian_plan_req, fraction, 0.1, 0.1); + fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( + *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0.1, 0.1); - fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, - close_matching_cartesian_plan_req, fraction, 0.1, 0.1); + fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, + fraction, 0.1, 0.1); check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); @@ -820,10 +815,9 @@ void test_cartesian_trajectories(std::shared_ptr move_group, prefix = "test_cartesian_trajectories.put_first.fetch_higher_fraction"; fetched_trajectories = - cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, 1, 999, 999); + cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 1, 999, 999); - fetched_traj = - cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, 1, 999, 999); + fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 1, 999, 999); check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); @@ -835,10 +829,9 @@ void test_cartesian_trajectories(std::shared_ptr move_group, prefix = "test_cartesian_trajectories.put_first.fetch_lower_fraction"; fetched_trajectories = - cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, 0, 999, 999); + cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999); - fetched_traj = - cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, 0, 999, 999); + fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999); check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); @@ -861,11 +854,11 @@ void test_cartesian_trajectories(std::shared_ptr move_group, prefix = "test_cartesian_trajectories.put_worse"; double worse_execution_time = execution_time + 100; - check_and_emit(!cache->put_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, traj, - worse_execution_time, planning_time, fraction, false), + check_and_emit(!cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, worse_execution_time, + planning_time, fraction, false), prefix, "Put worse trajectory, no delete_worse_trajectories, not ok"); - check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); // Put better, no delete_worse_trajectories // @@ -873,11 +866,11 @@ void test_cartesian_trajectories(std::shared_ptr move_group, prefix = "test_cartesian_trajectories.put_better"; double better_execution_time = execution_time - 100; - check_and_emit(cache->put_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, traj, - better_execution_time, planning_time, fraction, false), + check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, better_execution_time, + planning_time, fraction, false), prefix, "Put better trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 2, prefix, "Two trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); // Fetch sorted // @@ -885,10 +878,10 @@ void test_cartesian_trajectories(std::shared_ptr move_group, prefix = "test_cartesian_trajectories.put_better.fetch_sorted"; fetched_trajectories = - cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); fetched_traj = - cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); @@ -915,20 +908,20 @@ void test_cartesian_trajectories(std::shared_ptr move_group, prefix = "test_cartesian_trajectories.put_better_delete_worse_trajectories"; double even_better_execution_time = better_execution_time - 100; - check_and_emit(cache->put_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, different_traj, - even_better_execution_time, planning_time, fraction, true), + check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, different_traj, + even_better_execution_time, planning_time, fraction, true), prefix, "Put better trajectory, delete_worse_trajectories, ok"); - check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); // Fetch better plan prefix = "test_cartesian_trajectories.put_better_delete_worse_trajectories.fetch"; fetched_trajectories = - cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); fetched_traj = - cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); @@ -950,22 +943,22 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // Unrelated trajectory requests should live alongside pre-existing plans prefix = "test_cartesian_trajectories.put_different_req"; - check_and_emit(cache->put_cartesian_trajectory(*move_group, g_robot_name, different_cartesian_plan_req, - different_traj, better_execution_time, planning_time, fraction, true), + check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, different_cartesian_plan_req, different_traj, + better_execution_time, planning_time, fraction, true), prefix, "Put different trajectory req, delete_worse_trajectories, ok"); - check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 2, prefix, "Two trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); // Fetch with different trajectory req // // With multiple trajectories in cache, fetches should be sorted accordingly prefix = "test_cartesian_trajectories.put_different_req.fetch"; - fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, - different_cartesian_plan_req, fraction, 0, 0); + fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, + different_cartesian_plan_req, fraction, 0, 0); - fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, - different_cartesian_plan_req, fraction, 0, 0); + fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, different_cartesian_plan_req, + fraction, 0, 0); check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); @@ -988,25 +981,25 @@ void test_cartesian_trajectories(std::shared_ptr move_group, prefix = "test_cartesian_trajectories.different_robot.empty"; std::string different_robot_name = "different_robot"; - check_and_emit(cache->count_cartesian_trajectories(different_robot_name) == 0, prefix, "No trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 0, prefix, "No trajectories in cache"); // Put first for different robot, delete_worse_trajectories // // A different robot's cache should not interact with the original cache prefix = "test_cartesian_trajectories.different_robot.put_first"; - check_and_emit(cache->put_cartesian_trajectory(*move_group, different_robot_name, different_cartesian_plan_req, - different_traj, better_execution_time, planning_time, fraction, true), + check_and_emit(cache->putCartesianTrajectory(*move_group, different_robot_name, different_cartesian_plan_req, + different_traj, better_execution_time, planning_time, fraction, true), prefix, "Put different trajectory req, delete_worse_trajectories, ok"); - check_and_emit(cache->count_cartesian_trajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); - check_and_emit(cache->count_cartesian_trajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); - check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 2, prefix, + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in original robot's cache"); - fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, - different_cartesian_plan_req, fraction, 0, 0); + fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, + different_cartesian_plan_req, fraction, 0, 0); check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all on original still returns one"); } @@ -1018,12 +1011,11 @@ int main(int argc, char** argv) rclcpp::NodeOptions test_node_options; test_node_options.automatically_declare_parameters_from_overrides(true); - test_node_options.arguments({"--ros-args", "-r", "__node:=test_node"}); - + test_node_options.arguments({ "--ros-args", "-r", "__node:=test_node" }); rclcpp::NodeOptions move_group_node_options; move_group_node_options.automatically_declare_parameters_from_overrides(true); - move_group_node_options.arguments({"--ros-args", "-r", "__node:=move_group_node"}); + move_group_node_options.arguments({ "--ros-args", "-r", "__node:=move_group_node" }); auto test_node = rclcpp::Node::make_shared("test_node", test_node_options); auto move_group_node = rclcpp::Node::make_shared("move_group_node", move_group_node_options); From 7bf91297b0465b2eed697d85697745d205df9cbd Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 23 Jul 2024 23:14:26 -0700 Subject: [PATCH 09/26] Formatting pass Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/CHANGELOG.rst | 2 +- moveit_ros/trajectory_cache/CMakeLists.txt | 53 +++++++++---------- moveit_ros/trajectory_cache/README.md | 26 ++++----- .../trajectory_cache/trajectory_cache.hpp | 2 +- .../test/test_trajectory_cache.py | 25 ++++----- 5 files changed, 52 insertions(+), 56 deletions(-) diff --git a/moveit_ros/trajectory_cache/CHANGELOG.rst b/moveit_ros/trajectory_cache/CHANGELOG.rst index 82ad0f2be9..09f7aae2e9 100644 --- a/moveit_ros/trajectory_cache/CHANGELOG.rst +++ b/moveit_ros/trajectory_cache/CHANGELOG.rst @@ -4,4 +4,4 @@ Changelog for package nexus_motion_planner 0.1.0 (2024-05-17) ------------------ -* Add ``moveit_ros_trajectory_cache`` package for trajectory caching. \ No newline at end of file +* Add ``moveit_ros_trajectory_cache`` package for trajectory caching. diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt index 3c888c7330..9d55f04f2c 100644 --- a/moveit_ros/trajectory_cache/CMakeLists.txt +++ b/moveit_ros/trajectory_cache/CMakeLists.txt @@ -19,47 +19,44 @@ find_package(warehouse_ros_sqlite REQUIRED) include_directories(include) -set (trajectory_cache_dependencies - geometry_msgs - moveit_ros_planning_interface - moveit_ros_warehouse - rclcpp - tf2 - tf2_ros - trajectory_msgs - warehouse_ros - warehouse_ros_sqlite -) - -#=============================================================================== -set(TRAJECTORY_CACHE_LIBRARY_NAME trajectory_cache) +set(TRAJECTORY_CACHE_DEPENDENCIES + geometry_msgs + moveit_ros_planning_interface + moveit_ros_warehouse + rclcpp + tf2 + tf2_ros + trajectory_msgs + warehouse_ros + warehouse_ros_sqlite) + +# ============================================================================== # Motion plan cache library -add_library(${TRAJECTORY_CACHE_LIBRARY_NAME} src/trajectory_cache.cpp) -ament_target_dependencies(${TRAJECTORY_CACHE_LIBRARY_NAME} ${trajectory_cache_dependencies}) -target_include_directories(${TRAJECTORY_CACHE_LIBRARY_NAME} PUBLIC $) +add_library(trajectory_cache src/trajectory_cache.cpp) +ament_target_dependencies(trajectory_cache ${TRAJECTORY_CACHE_DEPENDENCIES}) +target_include_directories( + trajectory_cache PUBLIC $) -#=============================================================================== +# ============================================================================== if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) find_package(rmf_utils REQUIRED) - # This test executable is run by the pytest_test, since a node is required for testing move groups + # This test executable is run by the pytest_test, since a node is required for + # testing move groups add_executable(test_trajectory_cache test/test_trajectory_cache.cpp) - target_link_libraries(test_trajectory_cache ${TRAJECTORY_CACHE_LIBRARY_NAME}) + target_link_libraries(test_trajectory_cache trajectory_cache) - install(TARGETS - test_trajectory_cache - RUNTIME DESTINATION lib/${PROJECT_NAME} - ) + install(TARGETS test_trajectory_cache RUNTIME DESTINATION lib/${PROJECT_NAME}) - ament_add_pytest_test(test_trajectory_cache_py "test/test_trajectory_cache.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - ) + ament_add_pytest_test( + test_trajectory_cache_py "test/test_trajectory_cache.py" WORKING_DIRECTORY + "${CMAKE_CURRENT_BINARY_DIR}") endif() -ament_export_dependencies(${trajectory_cache_dependencies}) +ament_export_dependencies(${TRAJECTORY_CACHE_DEPENDENCIES}) ament_package() diff --git a/moveit_ros/trajectory_cache/README.md b/moveit_ros/trajectory_cache/README.md index 497f003ed9..ba5dbfd2dc 100644 --- a/moveit_ros/trajectory_cache/README.md +++ b/moveit_ros/trajectory_cache/README.md @@ -54,21 +54,23 @@ If you use this package in your work, please cite it using the following: // - `warehouse_port`: The port used for the database auto traj_cache = std::make_shared(node); -auto fetched_trajectory = traj_cache->fetchBestMatchingTrajectory( - *move_group_interface, robot_name, motion_plan_req_msg, - _cache_start_match_tolerance, _cache_goal_match_tolerance, /*sort_by=*/"execution_time_s"); +auto fetched_trajectory = + traj_cache->fetchBestMatchingTrajectory(*move_group_interface, robot_name, motion_plan_req_msg, + _cache_start_match_tolerance, _cache_goal_match_tolerance, + /*sort_by=*/"execution_time_s"); -if (fetched_trajectory) { +if (fetched_trajectory) +{ // Great! We got a cache hit // Do something with the plan. -} else { +} +else +{ // Plan... And put it for posterity! traj_cache->putTrajectory( - *interface, robot_name, std::move(plan_req_msg), std::move(res->result.trajectory), - rclcpp::Duration( - res->result.trajectory.joint_trajectory.points.back().time_from_start - ).seconds(), - res->result.planning_time, /*overwrite=*/true) + *interface, robot_name, std::move(plan_req_msg), std::move(res->result.trajectory), + rclcpp::Duration(res->result.trajectory.joint_trajectory.points.back().time_from_start).seconds(), + res->result.planning_time, /*overwrite=*/true); } ``` @@ -124,8 +126,8 @@ Since this is an initial release, the following features are unsupported because - If your planning scene is expected to change between cache lookups, do NOT use this cache, fetched trajectories are likely to result in collision then. - To handle collisions this class will need to hash the planning scene world msg (after zeroing out std_msgs/Header timestamps and sequences) and do an appropriate lookup, or do more complicated checks to see if the scene world is "close enough" or is a less obstructed version of the scene in the cache entry. - !!! This cache does NOT support keying on joint velocities and efforts. - - The cache only keys on joint positions. + - The cache only keys on joint positions. - !!! This cache does NOT support multi-DOF joints. - !!! This cache does NOT support certain constraints - Including: path, constraint regions, everything related to collision. -- The fuzzy lookup can't be configured on a per-joint basis. \ No newline at end of file +- The fuzzy lookup can't be configured on a per-joint basis. diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index 22f6b45e64..ce38049ff1 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -257,4 +257,4 @@ class TrajectoryCache }; } // namespace trajectory_cache -} // namespace moveit_ros \ No newline at end of file +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.py b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py index a9c32c06da..095f8016b6 100755 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.py +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py @@ -54,8 +54,7 @@ def robot_fixture(moveit_config): package="tf2_ros", executable="static_transform_publisher", output="log", - arguments=["0.0", "0.0", "0.0", "0.0", - "0.0", "0.0", "world", "panda_link0"], + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], ) robot_state_publisher = Node( @@ -87,8 +86,7 @@ def robot_fixture(moveit_config): ]: load_controllers += [ ExecuteProcess( - cmd=["ros2 run controller_manager spawner {}".format( - controller)], + cmd=["ros2 run controller_manager spawner {}".format(controller)], shell=True, output="log", ) @@ -99,7 +97,7 @@ def robot_fixture(moveit_config): static_tf, robot_state_publisher, ros2_control_node, - *load_controllers + *load_controllers, ] @@ -111,25 +109,24 @@ def trajectory_cache_test_runner_node(moveit_config): name="test_trajectory_cache_node", output="screen", cached_output=True, - parameters=[moveit_config.to_dict()] + parameters=[moveit_config.to_dict()], ) @launch_pytest.fixture def launch_description(trajectory_cache_test_runner_node, robot_fixture): return LaunchDescription( - robot_fixture + - [ - trajectory_cache_test_runner_node, - launch_pytest.actions.ReadyToTest() - ] + robot_fixture + + [trajectory_cache_test_runner_node, launch_pytest.actions.ReadyToTest()] ) def validate_stream(expected): def wrapped(output): - assert expected in output.splitlines( + assert ( + expected in output.splitlines() ), f"Did not get expected: {expected} in output:\n\n{output}" + return wrapped @@ -140,7 +137,7 @@ def test_all_tests_pass(trajectory_cache_test_runner_node, launch_context): launch_context, trajectory_cache_test_runner_node, lambda x: x.count("[PASS]") == 165, # All test cases passed. - timeout=30 + timeout=30, ) # Check no occurrences of [FAIL] in output @@ -148,7 +145,7 @@ def test_all_tests_pass(trajectory_cache_test_runner_node, launch_context): launch_context, trajectory_cache_test_runner_node, lambda x: "[FAIL]" in x, - timeout=10 + timeout=10, ) yield From cec778c95bcf8159a73137262fd8f40f6fa65055 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 24 Jul 2024 00:44:20 -0700 Subject: [PATCH 10/26] Add docstrings Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/README.md | 3 +- .../trajectory_cache/trajectory_cache.hpp | 388 +++++++++++++---- .../trajectory_cache/src/trajectory_cache.cpp | 412 +++++++++--------- 3 files changed, 519 insertions(+), 284 deletions(-) diff --git a/moveit_ros/trajectory_cache/README.md b/moveit_ros/trajectory_cache/README.md index ba5dbfd2dc..5b58cbcf77 100644 --- a/moveit_ros/trajectory_cache/README.md +++ b/moveit_ros/trajectory_cache/README.md @@ -50,9 +50,8 @@ If you use this package in your work, please cite it using the following: // Be sure to set some relevant ROS parameters: // Relevant ROS Parameters: // - `warehouse_plugin`: What database to use -// - `warehouse_host`: Where the database should be created -// - `warehouse_port`: The port used for the database auto traj_cache = std::make_shared(node); +traj_cache->init(/*db_host=*/":memory:", /*db_port=*/0, /*exact_match_precision=*/1e-6); auto fetched_trajectory = traj_cache->fetchBestMatchingTrajectory(*move_group_interface, robot_name, motion_plan_req_msg, diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index ce38049ff1..d79ebf0b85 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -40,8 +40,9 @@ namespace moveit_ros namespace trajectory_cache { -/** - * Trajectory Cache manager for MoveIt. +/** \class TrajectoryCache trajectory_cache.hpp moveit/trajectory_cache/trajectory_cache.hpp + * + * \brief Trajectory Cache manager for MoveIt. * * This manager facilitates cache management for MoveIt 2's `MoveGroupInterface` * by using `warehouse_ros` to manage a database of executed trajectories, keyed @@ -77,8 +78,6 @@ namespace trajectory_cache * * Relevant ROS Parameters: * - `warehouse_plugin`: What database to use - * - `warehouse_host`: Where the database should be created - * - `warehouse_port`: The port used for the database * * This class supports trajectories planned from move_group MotionPlanRequests * as well as GetCartesianPath requests. That is, both normal motion plans and @@ -113,139 +112,368 @@ namespace trajectory_cache class TrajectoryCache { public: - // We explicitly need a Node::SharedPtr because warehouse_ros ONLY supports - // it... - // - // TODO: methylDragon - - // Use rclcpp::node_interfaces::NodeInterfaces<> once warehouse_ros does. + /** + * \brief Construct a TrajectoryCache. + * + * \param[in] node. An rclcpp::Node::SharedPtr, which will be used to lookup warehouse_ros parameters, log, and listen + * for TF. + * + * TODO: methylDragon - + * We explicitly need a Node::SharedPtr because warehouse_ros ONLY supports it... + * Use rclcpp::node_interfaces::NodeInterfaces<> once warehouse_ros does. + */ explicit TrajectoryCache(const rclcpp::Node::SharedPtr& node); + /** + * \brief Initialize the TrajectoryCache. + * + * This sets up the database connection, and sets any configuration parameters. + * You must call this before calling any other method of the trajectory cache. + * + * \param[in] db_path. The database path. + * \param[in] db_port. The database port. + * \param[in] exact_match_precision. Tolerance for float precision comparison for what counts as an exact match. + * An exact match is when: + * (candidate >= value - (exact_match_precision / 2) && candidate <= value + (exact_match_precision / 2)) + * \returns true if the database was successfully connected to. + * */ bool init(const std::string& db_path = ":memory:", uint32_t db_port = 0, double exact_match_precision = 1e-6); - unsigned countTrajectories(const std::string& move_group_namespace); - - unsigned countCartesianTrajectories(const std::string& move_group_namespace); - - // =========================================================================== - // MOTION PLAN TRAJECTORY CACHING - // =========================================================================== - // TOP LEVEL OPS - - // Fetches all plans that fit within the requested tolerances for start and - // goal conditions, returning them as a vector, sorted by some db column. + /** + * \brief Count the number of non-cartesian trajectories for a particular cache namespace. + * + * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * \returns The number of non-cartesian trajectories for the cache namespace. + */ + unsigned countTrajectories(const std::string& cache_namespace); + + /** + * \brief Count the number of cartesian trajectories for a particular cache namespace. + * + * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * \returns The number of cartesian trajectories for the cache namespace. + */ + unsigned countCartesianTrajectories(const std::string& cache_namespace); + + /** + * \name Motion plan trajectory caching + */ + /**@{*/ + + /** + * \brief Fetch all plans that fit within the requested tolerances for start and goal conditions, returning them as a + * vector, sorted by some cache column. + * + * \param[in] move_group. The manipulator move group, used to get its state. + * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * \param[in] plan_request. The motion plan request to key the cache with. + * \param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. + * \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. + * \param[in] metadata_only. If true, returns only the cache entry metadata. + * \param[in] sort_by. The cache column to sort by, defaults to execution time. + * \returns A vector of cache hits, sorted by the `sort_by` param. + */ std::vector::ConstPtr> fetchAllMatchingTrajectories(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, + const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s"); - // Fetches the best trajectory that fits within the requested tolerances for start - // and goal conditions, by some db column. + /** + * \brief Fetch the best trajectory that fits within the requested tolerances for start and goal conditions, by some + * cache column. + * + * \param[in] move_group. The manipulator move group, used to get its state. + * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * \param[in] plan_request. The motion plan request to key the cache with. + * \param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. + * \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. + * \param[in] metadata_only. If true, returns only the cache entry metadata. + * \param[in] sort_by. The cache column to sort by, defaults to execution time. + * \returns The best cache hit, with respect to the `sort_by` param. + */ warehouse_ros::MessageWithMetadata::ConstPtr fetchBestMatchingTrajectory( - const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s"); - // Put a trajectory into the database if it is the best matching trajectory seen so far. - // - // Trajectories are matched based off their start and goal states. - // And are considered "better" if they have a smaller planned execution time - // than matching trajectories. - // - // Optionally deletes all worse trajectories by default to prune the cache. + /** + * \brief Put a trajectory into the database if it is the best matching trajectory seen so far. + * + * Trajectories are matched based off their start and goal states. + * And are considered "better" if they have a smaller planned execution time than exactly matching trajectories. + * + * A trajectory is "exactly matching" if its start and goal are close enough to another trajectory. + * The tolerance for this depends on the `exact_match_tolerance` arg passed in init(). + * \see init() + * + * Optionally deletes all worse trajectories by default to prune the cache. + * + * \param[in] move_group. The manipulator move group, used to get its state. + * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * \param[in] plan_request. The motion plan request to key the cache with. + * \param[in] trajectory. The trajectory to put. + * \param[in] execution_time_s. The execution time of the trajectory, in seconds. + * \param[in] planning_time_s. How long the trajectory took to plan, in seconds. + * \param[in] delete_worse_trajectories. If true, will prune the cache by deleting all cache entries that match the + * `plan_request` exactly, if they are worse than the `trajectory`, even if it was not put. + * \returns true if the trajectory was the best seen yet and hence put into the cache. + */ bool putTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, + const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, double planning_time_s, bool delete_worse_trajectories = true); - // QUERY CONSTRUCTION - bool extractAndAppendTrajectoryStartToQuery(warehouse_ros::Query& query, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request, - double match_tolerance); - - bool extractAndAppendTrajectoryGoalToQuery(warehouse_ros::Query& query, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request, - double match_tolerance); - - // METADATA CONSTRUCTION - bool extractAndAppendTrajectoryStartToMetadata(warehouse_ros::Metadata& metadata, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request); - - bool extractAndAppendTrajectoryGoalToMetadata(warehouse_ros::Metadata& metadata, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request); - - // =========================================================================== - // CARTESIAN TRAJECTORY CACHING - // =========================================================================== - // TOP LEVEL OPS - - // This mimics the move group computeCartesianPath signature (without path - // constraints). + /**@}*/ + + /** + * \name Cartesian trajectory caching + */ + /**@{*/ + + /** + * \brief Construct a GetCartesianPath request. + * + * This mimics the move group computeCartesianPath signature (without path constraints). + * + * \param[in] move_group. The manipulator move group, used to get its state, frames, and link. + * \param[in] waypoints. The cartesian waypoints to request the path for. + * \param[in] max_step. The value to populate into the `GetCartesianPath` request's max_step field. + * \param[in] jump_threshold. The value to populate into the `GetCartesianPath` request's jump_threshold field. + * \param[in] avoid_collisions. The value to populate into the `GetCartesianPath` request's avoid_collisions field. + * \returns + */ moveit_msgs::srv::GetCartesianPath::Request constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group, - const std::vector& waypoints, double step, + const std::vector& waypoints, double max_step, double jump_threshold, bool avoid_collisions = true); - // Fetches all cartesian trajectories that fit within the requested tolerances - // for start and goal conditions, returning them as a vector, sorted by some - // db column. + /** + * \brief Fetch all cartesian trajectories that fit within the requested tolerances for start and goal conditions, + * returning them as a vector, sorted by some cache column. + * + * \param[in] move_group. The manipulator move group, used to get its state. + * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * \param[in] plan_request. The cartesian plan request to key the cache with. + * \param[in] min_fraction. The minimum fraction required for a cache hit. + * \param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. + * \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. + * \param[in] metadata_only. If true, returns only the cache entry metadata. + * \param[in] sort_by. The cache column to sort by, defaults to execution time. + * \returns A vector of cache hits, sorted by the `sort_by` param. + */ std::vector::ConstPtr> fetchAllMatchingCartesianTrajectories(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, + const std::string& cache_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s"); - // Fetches the best cartesian trajectory that fits within the requested tolerances - // for start and goal conditions, by some db column. + /** + * \brief Fetch the best cartesian trajectory that fits within the requested tolerances for start and goal conditions, + * by some cache column. + * + * \param[in] move_group. The manipulator move group, used to get its state. + * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * \param[in] plan_request. The cartesian plan request to key the cache with. + * \param[in] min_fraction. The minimum fraction required for a cache hit. + * \param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. + * \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. + * \param[in] metadata_only. If true, returns only the cache entry metadata. + * \param[in] sort_by. The cache column to sort by, defaults to execution time. + * \returns The best cache hit, with respect to the `sort_by` param. + */ warehouse_ros::MessageWithMetadata::ConstPtr fetchBestMatchingCartesianTrajectory( - const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s"); - // Put a cartesian trajectory into the database if it is the best matching - // cartesian trajectory seen so far. - // - // Cartesian trajectories are matched based off their start and goal states. - // And are considered "better" if they have a smaller planned execution time - // than matching cartesian trajectories. - // - // Optionally deletes all worse cartesian trajectories by default to prune the - // cache. + /** + * \brief Put a cartesian trajectory into the database if it is the best matching cartesian trajectory seen so far. + * + * Cartesian trajectories are matched based off their start and goal states. + * And are considered "better" if they have a smaller planned execution time than exactly matching cartesian + * trajectories. + * + * A trajectory is "exactly matching" if its start and goal (and fraction) are close enough to another trajectory. + * The tolerance for this depends on the `exact_match_tolerance` arg passed in init(). + * \see init() + * + * Optionally deletes all worse cartesian trajectories by default to prune the cache. + * + * \param[in] move_group. The manipulator move group, used to get its state. + * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * \param[in] plan_request. The cartesian plan request to key the cache with. + * \param[in] trajectory. The trajectory to put. + * \param[in] execution_time_s. The execution time of the trajectory, in seconds. + * \param[in] planning_time_s. How long the trajectory took to plan, in seconds. + * \param[in] fraction. The fraction of the path that was computed. + * \param[in] delete_worse_trajectories. If true, will prune the cache by deleting all cache entries that match the + * `plan_request` exactly, if they are worse than `trajectory`, even if it was not put. + * \returns true if the trajectory was the best seen yet and hence put into the cache. + */ bool putCartesianTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, + const std::string& cache_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, double planning_time_s, double fraction, bool delete_worse_trajectories = true); - // QUERY CONSTRUCTION + /**@}*/ + +private: + /** + * \name Motion plan trajectory query and metadata construction + */ + /**@{*/ + + /** + * \brief Extract relevant parameters from a motion plan request's start parameters to populate a cache db query, with + * some match tolerance. + * + * These parameters will be used to look-up relevant sections of a cache element's key. + * + * \param[out] query. The query to add parameters to. + * \param[in] move_group. The manipulator move group, used to get its state. + * \param[in] plan_request. The motion plan request to key the cache with. + * \param[in] match_tolerance. The match tolerance (additive with exact_match_tolerance) for the query. + * \returns true if successfully added to. If false, the query might have been partially modified and should not be + * used. + */ + bool extractAndAppendTrajectoryStartToQuery(warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance); + + /** + * \brief Extract relevant parameters from a motion plan request's goal parameters to populate a cache db query, with + * some match tolerance. + * + * These parameters will be used to look-up relevant sections of a cache element's key. + * + * \param[out] query. The query to add parameters to. + * \param[in] move_group. The manipulator move group, used to get its state. + * \param[in] plan_request. The motion plan request to key the cache with. + * \param[in] match_tolerance. The match tolerance (additive with exact_match_tolerance) for the query. + * \returns true if successfully added to. If false, the query might have been partially modified and should not be + * used. + */ + bool extractAndAppendTrajectoryGoalToQuery(warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance); + + /** + * \brief Extract relevant parameters from a motion plan request's start parameters to populate a cache entry's + * metadata. + * + * These parameters will be used key the cache element. + * + * \param[out] metadata. The metadata to add paramters to. + * \param[in] move_group. The manipulator move group, used to get its state. + * \param[in] plan_request. The motion plan request to key the cache with. + * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be + * used. + */ + bool extractAndAppendTrajectoryStartToMetadata(warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request); + + /** + * \brief Extract relevant parameters from a motion plan request's goal parameters to populate a cache entry's + * metadata. + * + * These parameters will be used key the cache element. + * + * \param[out] metadata. The metadata to add paramters to. + * \param[in] move_group. The manipulator move group, used to get its state. + * \param[in] plan_request. The motion plan request to key the cache with. + * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be + * used. + */ + bool extractAndAppendTrajectoryGoalToMetadata(warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request); + + /**@}*/ + + /** + * \name Cartesian trajectory query and metadata construction + */ + /**@{*/ + + /** + * \brief Extract relevant parameters from a cartesian plan request's start parameters to populate a cache db query, + * with some match tolerance. + * + * These parameters will be used to look-up relevant sections of a cache element's key. + * + * \param[out] query. The query to add parameters to. + * \param[in] move_group. The manipulator move group, used to get its state. + * \param[in] plan_request. The cartesian plan request to key the cache with. + * \param[in] match_tolerance. The match tolerance (additive with exact_match_tolerance) for the query. + * \returns true if successfully added to. If false, the query might have been partially modified and should not be + * used. + */ bool extractAndAppendCartesianTrajectoryStartToQuery(warehouse_ros::Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance); + /** + * \brief Extract relevant parameters from a cartesian plan request's goal parameters to populate a cache db query, + * with some match tolerance. + * + * These parameters will be used to look-up relevant sections of a cache element's key. + * + * \param[out] query. The query to add parameters to. + * \param[in] move_group. The manipulator move group, used to get its state. + * \param[in] plan_request. The cartesian plan request to key the cache with. + * \param[in] match_tolerance. The match tolerance (additive with exact_match_tolerance) for the query. + * \returns true if successfully added to. If false, the query might have been partially modified and should not be + * used. + */ bool extractAndAppendCartesianTrajectoryGoalToQuery(warehouse_ros::Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance); - // METADATA CONSTRUCTION + /** + * \brief Extract relevant parameters from a cartesian plan request's goal parameters to populate a cache entry's + * metadata. + * + * These parameters will be used key the cache element. + * + * \param[out] metadata. The metadata to add paramters to. + * \param[in] move_group. The manipulator move group, used to get its state. + * \param[in] plan_request. The cartesian plan request to key the cache with. + * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be + * used. + */ bool extractAndAppendCartesianTrajectoryStartToMetadata(warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request); + /** + * \brief Extract relevant parameters from a cartesian plan request's goal parameters to populate a cache entry's + * metadata. + * + * These parameters will be used key the cache element. + * + * \param[out] metadata. The metadata to add paramters to. + * \param[in] move_group. The manipulator move group, used to get its state. + * \param[in] plan_request. The cartesian plan request to key the cache with. + * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be + * used. + */ bool extractAndAppendCartesianTrajectoryGoalToMetadata(warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request); -private: + /**@}*/ + rclcpp::Node::SharedPtr node_; rclcpp::Logger logger_; warehouse_ros::DatabaseConnection::Ptr db_; diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index 80527d5247..3bb8d688c7 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -85,33 +85,31 @@ bool TrajectoryCache::init(const std::string& db_path, uint32_t db_port, double return db_->connect(); } -unsigned TrajectoryCache::countTrajectories(const std::string& move_group_namespace) +unsigned TrajectoryCache::countTrajectories(const std::string& cache_namespace) { - auto coll = - db_->openCollection("move_group_trajectory_cache", move_group_namespace); + auto coll = db_->openCollection("move_group_trajectory_cache", cache_namespace); return coll.count(); } -unsigned TrajectoryCache::countCartesianTrajectories(const std::string& move_group_namespace) +unsigned TrajectoryCache::countCartesianTrajectories(const std::string& cache_namespace) { - auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", - move_group_namespace); + auto coll = + db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); return coll.count(); } // ============================================================================= // MOTION PLAN TRAJECTORY CACHING // ============================================================================= -// MOTION PLAN TRAJECTORY CACHING: TOP LEVEL OPS + std::vector::ConstPtr> TrajectoryCache::fetchAllMatchingTrajectories(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, + const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, bool metadata_only, const std::string& sort_by) { - auto coll = - db_->openCollection("move_group_trajectory_cache", move_group_namespace); + auto coll = db_->openCollection("move_group_trajectory_cache", cache_namespace); Query::Ptr query = coll.createQuery(); @@ -128,13 +126,13 @@ TrajectoryCache::fetchAllMatchingTrajectories(const moveit::planning_interface:: } MessageWithMetadata::ConstPtr TrajectoryCache::fetchBestMatchingTrajectory( - const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, bool metadata_only, const std::string& sort_by) { // First find all matching, but metadata only. // Then use the ID metadata of the best plan to pull the actual message. - auto matching_trajectories = this->fetchAllMatchingTrajectories(move_group, move_group_namespace, plan_request, + auto matching_trajectories = this->fetchAllMatchingTrajectories(move_group, cache_namespace, plan_request, start_tolerance, goal_tolerance, true, sort_by); if (matching_trajectories.empty()) @@ -143,8 +141,7 @@ MessageWithMetadata::ConstPtr TrajectoryCache return nullptr; } - auto coll = - db_->openCollection("move_group_trajectory_cache", move_group_namespace); + auto coll = db_->openCollection("move_group_trajectory_cache", cache_namespace); // Best plan is at first index, since the lookup query was sorted by // execution_time. @@ -156,7 +153,7 @@ MessageWithMetadata::ConstPtr TrajectoryCache } bool TrajectoryCache::putTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, + const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, double planning_time_s, bool delete_worse_trajectories) @@ -182,8 +179,7 @@ bool TrajectoryCache::putTrajectory(const moveit::planning_interface::MoveGroupI return false; } - auto coll = - db_->openCollection("move_group_trajectory_cache", move_group_namespace); + auto coll = db_->openCollection("move_group_trajectory_cache", cache_namespace); // Pull out trajectories "exactly" keyed by request in cache. Query::Ptr exact_query = coll.createQuery(); @@ -197,7 +193,7 @@ bool TrajectoryCache::putTrajectory(const moveit::planning_interface::MoveGroupI return false; } - auto exact_matches = coll.queryList(exact_query, /* metadata_only */ true, /* sort_by */ "execution_time_s"); + auto exact_matches = coll.queryList(exact_query, /*metadata_only=*/true, /*sort_by=*/"execution_time_s"); double best_execution_time = std::numeric_limits::infinity(); if (!exact_matches.empty()) @@ -257,7 +253,196 @@ bool TrajectoryCache::putTrajectory(const moveit::planning_interface::MoveGroupI return false; } -// MOTION PLAN TRAJECTORY CACHING: QUERY CONSTRUCTION +// ============================================================================= +// CARTESIAN TRAJECTORY CACHING +// ============================================================================= + +moveit_msgs::srv::GetCartesianPath::Request +TrajectoryCache::constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group, + const std::vector& waypoints, + double max_step, double jump_threshold, bool avoid_collisions) +{ + moveit_msgs::srv::GetCartesianPath::Request out; + + move_group.constructRobotState(out.start_state); + + out.group_name = move_group.getName(); + out.max_velocity_scaling_factor = move_group.getMaxVelocityScalingFactor(); + out.max_acceleration_scaling_factor = move_group.getMaxVelocityScalingFactor(); + + out.header.frame_id = move_group.getPoseReferenceFrame(); + out.waypoints = waypoints; + out.max_step = max_step; + out.jump_threshold = jump_threshold; + out.path_constraints = moveit_msgs::msg::Constraints(); + out.avoid_collisions = avoid_collisions; + out.link_name = move_group.getEndEffectorLink(); + out.header.stamp = move_group.getNode()->now(); + + return out; +} + +std::vector::ConstPtr> +TrajectoryCache::fetchAllMatchingCartesianTrajectories(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& cache_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double min_fraction, double start_tolerance, + double goal_tolerance, bool metadata_only, + const std::string& sort_by) +{ + auto coll = + db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); + + Query::Ptr query = coll.createQuery(); + + bool start_ok = + this->extractAndAppendCartesianTrajectoryStartToQuery(*query, move_group, plan_request, start_tolerance); + bool goal_ok = this->extractAndAppendCartesianTrajectoryGoalToQuery(*query, move_group, plan_request, goal_tolerance); + + if (!start_ok || !goal_ok) + { + RCLCPP_ERROR(logger_, "Could not construct cartesian trajectory query."); + return {}; + } + + query->appendGTE("fraction", min_fraction); + return coll.queryList(query, metadata_only, sort_by, true); +} + +MessageWithMetadata::ConstPtr TrajectoryCache::fetchBestMatchingCartesianTrajectory( + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, + double goal_tolerance, bool metadata_only, const std::string& sort_by) +{ + // First find all matching, but metadata only. + // Then use the ID metadata of the best plan to pull the actual message. + auto matching_trajectories = this->fetchAllMatchingCartesianTrajectories( + move_group, cache_namespace, plan_request, min_fraction, start_tolerance, goal_tolerance, true, sort_by); + + if (matching_trajectories.empty()) + { + RCLCPP_DEBUG(logger_, "No matching cartesian trajectories found."); + return nullptr; + } + + auto coll = + db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); + + // Best plan is at first index, since the lookup query was sorted by + // execution_time. + int best_trajectory_id = matching_trajectories.at(0)->lookupInt("id"); + Query::Ptr best_query = coll.createQuery(); + best_query->append("id", best_trajectory_id); + + return coll.findOne(best_query, metadata_only); +} + +bool TrajectoryCache::putCartesianTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& cache_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + const moveit_msgs::msg::RobotTrajectory& trajectory, + double execution_time_s, double planning_time_s, double fraction, + bool delete_worse_trajectories) +{ + // Check pre-conditions + if (!trajectory.multi_dof_joint_trajectory.points.empty()) + { + RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: " + "Multi-DOF trajectory plans are not supported."); + return false; + } + if (plan_request.header.frame_id.empty() || trajectory.joint_trajectory.header.frame_id.empty()) + { + RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: Frame IDs cannot be empty."); + return false; + } + + auto coll = + db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); + + // Pull out trajectories "exactly" keyed by request in cache. + Query::Ptr exact_query = coll.createQuery(); + + bool start_query_ok = + this->extractAndAppendCartesianTrajectoryStartToQuery(*exact_query, move_group, plan_request, 0); + bool goal_query_ok = this->extractAndAppendCartesianTrajectoryGoalToQuery(*exact_query, move_group, plan_request, 0); + exact_query->append("fraction", fraction); + + if (!start_query_ok || !goal_query_ok) + { + RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: Could not construct lookup query."); + return false; + } + + auto exact_matches = coll.queryList(exact_query, /*metadata_only=*/true, /*sort_by=*/"execution_time_s"); + double best_execution_time = std::numeric_limits::infinity(); + if (!exact_matches.empty()) + { + best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); + + if (delete_worse_trajectories) + { + for (auto& match : exact_matches) + { + double match_execution_time_s = match->lookupDouble("execution_time_s"); + if (execution_time_s < match_execution_time_s) + { + int delete_id = match->lookupInt("id"); + RCLCPP_DEBUG(logger_, + "Overwriting cartesian trajectory (id: %d): " + "execution_time (%es) > new trajectory's execution_time (%es)", + delete_id, match_execution_time_s, execution_time_s); + + Query::Ptr delete_query = coll.createQuery(); + delete_query->append("id", delete_id); + coll.removeMessages(delete_query); + } + } + } + } + + // Insert if candidate is best seen. + if (execution_time_s < best_execution_time) + { + Metadata::Ptr insert_metadata = coll.createMetadata(); + + bool start_meta_ok = + this->extractAndAppendCartesianTrajectoryStartToMetadata(*insert_metadata, move_group, plan_request); + bool goal_meta_ok = + this->extractAndAppendCartesianTrajectoryGoalToMetadata(*insert_metadata, move_group, plan_request); + insert_metadata->append("execution_time_s", execution_time_s); + insert_metadata->append("planning_time_s", planning_time_s); + insert_metadata->append("fraction", fraction); + + if (!start_meta_ok || !goal_meta_ok) + { + RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: " + "Could not construct insert metadata."); + return false; + } + + RCLCPP_DEBUG(logger_, + "Inserting cartesian trajectory: New trajectory execution_time (%es) " + "is better than best trajectory's execution_time (%es) at fraction (%es)", + execution_time_s, best_execution_time, fraction); + + coll.insert(trajectory, insert_metadata); + return true; + } + + RCLCPP_DEBUG(logger_, + "Skipping cartesian trajectory insert: New trajectory execution_time (%es) " + "is worse than best trajectory's execution_time (%es) at fraction (%es)", + execution_time_s, best_execution_time, fraction); + return false; +} + +// ============================================================================= +// MOTION PLAN TRAJECTORY QUERY AND METADATA CONSTRUCTION +// ============================================================================= + +// MOTION PLAN TRAJECTORY CACHING: QUERY CONSTRUCTION ========================== + bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) @@ -533,7 +718,8 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( return true; } -// MOTION PLAN TRAJECTORY CACHING: METADATA CONSTRUCTION +// MOTION PLAN TRAJECTORY CACHING: METADATA CONSTRUCTION ======================= + bool TrajectoryCache::extractAndAppendTrajectoryStartToMetadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request) @@ -797,190 +983,11 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( } // ============================================================================= -// CARTESIAN TRAJECTORY CACHING +// CARTESIAN TRAJECTORY QUERY AND METADATA CONSTRUCTION // ============================================================================= -// CARTESIAN TRAJECTORY CACHING: TOP LEVEL OPS -moveit_msgs::srv::GetCartesianPath::Request -TrajectoryCache::constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group, - const std::vector& waypoints, double step, - double jump_threshold, bool avoid_collisions) -{ - moveit_msgs::srv::GetCartesianPath::Request out; - - move_group.constructRobotState(out.start_state); - - out.group_name = move_group.getName(); - out.max_velocity_scaling_factor = move_group.getMaxVelocityScalingFactor(); - out.max_acceleration_scaling_factor = move_group.getMaxVelocityScalingFactor(); - - out.header.frame_id = move_group.getPoseReferenceFrame(); - out.waypoints = waypoints; - out.max_step = step; - out.jump_threshold = jump_threshold; - out.path_constraints = moveit_msgs::msg::Constraints(); - out.avoid_collisions = avoid_collisions; - out.link_name = move_group.getEndEffectorLink(); - out.header.stamp = move_group.getNode()->now(); - - return out; -} - -std::vector::ConstPtr> -TrajectoryCache::fetchAllMatchingCartesianTrajectories(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, - double min_fraction, double start_tolerance, - double goal_tolerance, bool metadata_only, - const std::string& sort_by) -{ - auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", - move_group_namespace); - - Query::Ptr query = coll.createQuery(); - - bool start_ok = - this->extractAndAppendCartesianTrajectoryStartToQuery(*query, move_group, plan_request, start_tolerance); - bool goal_ok = this->extractAndAppendCartesianTrajectoryGoalToQuery(*query, move_group, plan_request, goal_tolerance); - - if (!start_ok || !goal_ok) - { - RCLCPP_ERROR(logger_, "Could not construct cartesian trajectory query."); - return {}; - } - - query->appendGTE("fraction", min_fraction); - return coll.queryList(query, metadata_only, sort_by, true); -} - -MessageWithMetadata::ConstPtr TrajectoryCache::fetchBestMatchingCartesianTrajectory( - const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, - double goal_tolerance, bool metadata_only, const std::string& sort_by) -{ - // First find all matching, but metadata only. - // Then use the ID metadata of the best plan to pull the actual message. - auto matching_trajectories = this->fetchAllMatchingCartesianTrajectories( - move_group, move_group_namespace, plan_request, min_fraction, start_tolerance, goal_tolerance, true, sort_by); - - if (matching_trajectories.empty()) - { - RCLCPP_DEBUG(logger_, "No matching cartesian trajectories found."); - return nullptr; - } - auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", - move_group_namespace); - - // Best plan is at first index, since the lookup query was sorted by - // execution_time. - int best_trajectory_id = matching_trajectories.at(0)->lookupInt("id"); - Query::Ptr best_query = coll.createQuery(); - best_query->append("id", best_trajectory_id); - - return coll.findOne(best_query, metadata_only); -} +// CARTESIAN TRAJECTORY CACHING: QUERY CONSTRUCTION ============================ -bool TrajectoryCache::putCartesianTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& move_group_namespace, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, - const moveit_msgs::msg::RobotTrajectory& trajectory, - double execution_time_s, double planning_time_s, double fraction, - bool delete_worse_trajectories) -{ - // Check pre-conditions - if (!trajectory.multi_dof_joint_trajectory.points.empty()) - { - RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: " - "Multi-DOF trajectory plans are not supported."); - return false; - } - if (plan_request.header.frame_id.empty() || trajectory.joint_trajectory.header.frame_id.empty()) - { - RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: Frame IDs cannot be empty."); - return false; - } - - auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", - move_group_namespace); - - // Pull out trajectories "exactly" keyed by request in cache. - Query::Ptr exact_query = coll.createQuery(); - - bool start_query_ok = - this->extractAndAppendCartesianTrajectoryStartToQuery(*exact_query, move_group, plan_request, 0); - bool goal_query_ok = this->extractAndAppendCartesianTrajectoryGoalToQuery(*exact_query, move_group, plan_request, 0); - exact_query->append("fraction", fraction); - - if (!start_query_ok || !goal_query_ok) - { - RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: Could not construct lookup query."); - return false; - } - - auto exact_matches = coll.queryList(exact_query, /* metadata_only */ true, /* sort_by */ "execution_time_s"); - double best_execution_time = std::numeric_limits::infinity(); - if (!exact_matches.empty()) - { - best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); - - if (delete_worse_trajectories) - { - for (auto& match : exact_matches) - { - double match_execution_time_s = match->lookupDouble("execution_time_s"); - if (execution_time_s < match_execution_time_s) - { - int delete_id = match->lookupInt("id"); - RCLCPP_DEBUG(logger_, - "Overwriting cartesian trajectory (id: %d): " - "execution_time (%es) > new trajectory's execution_time (%es)", - delete_id, match_execution_time_s, execution_time_s); - - Query::Ptr delete_query = coll.createQuery(); - delete_query->append("id", delete_id); - coll.removeMessages(delete_query); - } - } - } - } - - // Insert if candidate is best seen. - if (execution_time_s < best_execution_time) - { - Metadata::Ptr insert_metadata = coll.createMetadata(); - - bool start_meta_ok = - this->extractAndAppendCartesianTrajectoryStartToMetadata(*insert_metadata, move_group, plan_request); - bool goal_meta_ok = - this->extractAndAppendCartesianTrajectoryGoalToMetadata(*insert_metadata, move_group, plan_request); - insert_metadata->append("execution_time_s", execution_time_s); - insert_metadata->append("planning_time_s", planning_time_s); - insert_metadata->append("fraction", fraction); - - if (!start_meta_ok || !goal_meta_ok) - { - RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: " - "Could not construct insert metadata."); - return false; - } - - RCLCPP_DEBUG(logger_, - "Inserting cartesian trajectory: New trajectory execution_time (%es) " - "is better than best trajectory's execution_time (%es) at fraction (%es)", - execution_time_s, best_execution_time, fraction); - - coll.insert(trajectory, insert_metadata); - return true; - } - - RCLCPP_DEBUG(logger_, - "Skipping cartesian trajectory insert: New trajectory execution_time (%es) " - "is worse than best trajectory's execution_time (%es) at fraction (%es)", - execution_time_s, best_execution_time, fraction); - return false; -} - -// CARTESIAN TRAJECTORY CACHING: QUERY CONSTRUCTION bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) @@ -1151,7 +1158,8 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( return true; } -// CARTESIAN TRAJECTORY CACHING: METADATA CONSTRUCTION +// CARTESIAN TRAJECTORY CACHING: METADATA CONSTRUCTION ========================= + bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToMetadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request) From 62228468f26a699e1de12d7f9cbff38a21abae86 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 24 Jul 2024 01:57:13 -0700 Subject: [PATCH 11/26] Add ability to sort in descending order Signed-off-by: methylDragon --- .../moveit/trajectory_cache/trajectory_cache.hpp | 16 ++++++++++------ .../trajectory_cache/src/trajectory_cache.cpp | 16 ++++++++-------- 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index d79ebf0b85..1be586160e 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -171,6 +171,7 @@ class TrajectoryCache * \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. * \param[in] metadata_only. If true, returns only the cache entry metadata. * \param[in] sort_by. The cache column to sort by, defaults to execution time. + * \param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order. * \returns A vector of cache hits, sorted by the `sort_by` param. */ std::vector::ConstPtr> @@ -178,7 +179,7 @@ class TrajectoryCache const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, bool metadata_only = false, - const std::string& sort_by = "execution_time_s"); + const std::string& sort_by = "execution_time_s", bool ascending = true); /** * \brief Fetch the best trajectory that fits within the requested tolerances for start and goal conditions, by some @@ -191,18 +192,19 @@ class TrajectoryCache * \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. * \param[in] metadata_only. If true, returns only the cache entry metadata. * \param[in] sort_by. The cache column to sort by, defaults to execution time. + * \param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order. * \returns The best cache hit, with respect to the `sort_by` param. */ warehouse_ros::MessageWithMetadata::ConstPtr fetchBestMatchingTrajectory( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, - bool metadata_only = false, const std::string& sort_by = "execution_time_s"); + bool metadata_only = false, const std::string& sort_by = "execution_time_s", bool ascending = true); /** * \brief Put a trajectory into the database if it is the best matching trajectory seen so far. * * Trajectories are matched based off their start and goal states. - * And are considered "better" if they have a smaller planned execution time than exactly matching trajectories. + * And are considered "better" if they higher priority in the sorting order specified by `sort_by` than exactly matching trajectories. * * A trajectory is "exactly matching" if its start and goal are close enough to another trajectory. * The tolerance for this depends on the `exact_match_tolerance` arg passed in init(). @@ -261,6 +263,7 @@ class TrajectoryCache * \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. * \param[in] metadata_only. If true, returns only the cache entry metadata. * \param[in] sort_by. The cache column to sort by, defaults to execution time. + * \param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order. * \returns A vector of cache hits, sorted by the `sort_by` param. */ std::vector::ConstPtr> @@ -268,7 +271,7 @@ class TrajectoryCache const std::string& cache_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, double goal_tolerance, - bool metadata_only = false, const std::string& sort_by = "execution_time_s"); + bool metadata_only = false, const std::string& sort_by = "execution_time_s", bool ascending = true); /** * \brief Fetch the best cartesian trajectory that fits within the requested tolerances for start and goal conditions, @@ -282,18 +285,19 @@ class TrajectoryCache * \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. * \param[in] metadata_only. If true, returns only the cache entry metadata. * \param[in] sort_by. The cache column to sort by, defaults to execution time. + * \param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order. * \returns The best cache hit, with respect to the `sort_by` param. */ warehouse_ros::MessageWithMetadata::ConstPtr fetchBestMatchingCartesianTrajectory( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, - double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s"); + double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s", bool ascending = true); /** * \brief Put a cartesian trajectory into the database if it is the best matching cartesian trajectory seen so far. * * Cartesian trajectories are matched based off their start and goal states. - * And are considered "better" if they have a smaller planned execution time than exactly matching cartesian + * And are considered "better" if they higher priority in the sorting order specified by `sort_by` than exactly matching cartesian * trajectories. * * A trajectory is "exactly matching" if its start and goal (and fraction) are close enough to another trajectory. diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index 3bb8d688c7..b037673eac 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -107,7 +107,7 @@ TrajectoryCache::fetchAllMatchingTrajectories(const moveit::planning_interface:: const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, bool metadata_only, - const std::string& sort_by) + const std::string& sort_by, bool ascending) { auto coll = db_->openCollection("move_group_trajectory_cache", cache_namespace); @@ -122,18 +122,18 @@ TrajectoryCache::fetchAllMatchingTrajectories(const moveit::planning_interface:: return {}; } - return coll.queryList(query, metadata_only, sort_by, true); + return coll.queryList(query, metadata_only, sort_by, ascending); } MessageWithMetadata::ConstPtr TrajectoryCache::fetchBestMatchingTrajectory( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, - bool metadata_only, const std::string& sort_by) + bool metadata_only, const std::string& sort_by, bool ascending) { // First find all matching, but metadata only. // Then use the ID metadata of the best plan to pull the actual message. auto matching_trajectories = this->fetchAllMatchingTrajectories(move_group, cache_namespace, plan_request, - start_tolerance, goal_tolerance, true, sort_by); + start_tolerance, goal_tolerance, true, sort_by, ascending); if (matching_trajectories.empty()) { @@ -288,7 +288,7 @@ TrajectoryCache::fetchAllMatchingCartesianTrajectories(const moveit::planning_in const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, double goal_tolerance, bool metadata_only, - const std::string& sort_by) + const std::string& sort_by, bool ascending) { auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); @@ -306,18 +306,18 @@ TrajectoryCache::fetchAllMatchingCartesianTrajectories(const moveit::planning_in } query->appendGTE("fraction", min_fraction); - return coll.queryList(query, metadata_only, sort_by, true); + return coll.queryList(query, metadata_only, sort_by, ascending); } MessageWithMetadata::ConstPtr TrajectoryCache::fetchBestMatchingCartesianTrajectory( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, - double goal_tolerance, bool metadata_only, const std::string& sort_by) + double goal_tolerance, bool metadata_only, const std::string& sort_by, bool ascending) { // First find all matching, but metadata only. // Then use the ID metadata of the best plan to pull the actual message. auto matching_trajectories = this->fetchAllMatchingCartesianTrajectories( - move_group, cache_namespace, plan_request, min_fraction, start_tolerance, goal_tolerance, true, sort_by); + move_group, cache_namespace, plan_request, min_fraction, start_tolerance, goal_tolerance, true, sort_by, ascending); if (matching_trajectories.empty()) { From 10c6ac3a044870da47017cd9e1c65fcf8d2ddb67 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 24 Jul 2024 02:05:04 -0700 Subject: [PATCH 12/26] Add RFE for custom cost functions Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/moveit_ros/trajectory_cache/README.md b/moveit_ros/trajectory_cache/README.md index 5b58cbcf77..c3d7c52610 100644 --- a/moveit_ros/trajectory_cache/README.md +++ b/moveit_ros/trajectory_cache/README.md @@ -130,3 +130,5 @@ Since this is an initial release, the following features are unsupported because - !!! This cache does NOT support certain constraints - Including: path, constraint regions, everything related to collision. - The fuzzy lookup can't be configured on a per-joint basis. +- Alternate ordinal lookup metrics for the cache + - Currently only execution time is explicitly supported as a way to compare cache entries. Ideally we should be able to inject lambdas to save custom cache DB metadata to represent and sort on custom cost functions (e.g. minimum jerk, path length, etc.). (e.g. https://github.com/moveit/moveit2/pull/2153) \ No newline at end of file From 692f562dfc6af7a09d07776a8c3d522671c43f61 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 24 Jul 2024 02:07:20 -0700 Subject: [PATCH 13/26] Formatting pass Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/README.md | 4 +- .../trajectory_cache/trajectory_cache.hpp | 21 ++-- .../trajectory_cache/src/trajectory_cache.cpp | 111 +++++++++--------- 3 files changed, 70 insertions(+), 66 deletions(-) diff --git a/moveit_ros/trajectory_cache/README.md b/moveit_ros/trajectory_cache/README.md index c3d7c52610..e208319dea 100644 --- a/moveit_ros/trajectory_cache/README.md +++ b/moveit_ros/trajectory_cache/README.md @@ -69,7 +69,7 @@ else traj_cache->putTrajectory( *interface, robot_name, std::move(plan_req_msg), std::move(res->result.trajectory), rclcpp::Duration(res->result.trajectory.joint_trajectory.points.back().time_from_start).seconds(), - res->result.planning_time, /*overwrite=*/true); + res->result.planning_time, /*delete_worse_trajectories=*/true); } ``` @@ -131,4 +131,4 @@ Since this is an initial release, the following features are unsupported because - Including: path, constraint regions, everything related to collision. - The fuzzy lookup can't be configured on a per-joint basis. - Alternate ordinal lookup metrics for the cache - - Currently only execution time is explicitly supported as a way to compare cache entries. Ideally we should be able to inject lambdas to save custom cache DB metadata to represent and sort on custom cost functions (e.g. minimum jerk, path length, etc.). (e.g. https://github.com/moveit/moveit2/pull/2153) \ No newline at end of file + - Currently only execution time is explicitly supported as a way to compare cache entries. Ideally we should be able to inject lambdas to save custom cache DB metadata to represent and sort on custom cost functions (e.g. minimum jerk, path length, etc.). (e.g. https://github.com/moveit/moveit2/pull/2153) diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index 1be586160e..c7b327f4e7 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -204,7 +204,8 @@ class TrajectoryCache * \brief Put a trajectory into the database if it is the best matching trajectory seen so far. * * Trajectories are matched based off their start and goal states. - * And are considered "better" if they higher priority in the sorting order specified by `sort_by` than exactly matching trajectories. + * And are considered "better" if they higher priority in the sorting order specified by `sort_by` than exactly + * matching trajectories. * * A trajectory is "exactly matching" if its start and goal are close enough to another trajectory. * The tolerance for this depends on the `exact_match_tolerance` arg passed in init(). @@ -271,7 +272,8 @@ class TrajectoryCache const std::string& cache_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, double goal_tolerance, - bool metadata_only = false, const std::string& sort_by = "execution_time_s", bool ascending = true); + bool metadata_only = false, const std::string& sort_by = "execution_time_s", + bool ascending = true); /** * \brief Fetch the best cartesian trajectory that fits within the requested tolerances for start and goal conditions, @@ -291,14 +293,15 @@ class TrajectoryCache warehouse_ros::MessageWithMetadata::ConstPtr fetchBestMatchingCartesianTrajectory( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, - double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s", bool ascending = true); + double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s", + bool ascending = true); /** * \brief Put a cartesian trajectory into the database if it is the best matching cartesian trajectory seen so far. * * Cartesian trajectories are matched based off their start and goal states. - * And are considered "better" if they higher priority in the sorting order specified by `sort_by` than exactly matching cartesian - * trajectories. + * And are considered "better" if they higher priority in the sorting order specified by `sort_by` than exactly + * matching cartesian trajectories. * * A trajectory is "exactly matching" if its start and goal (and fraction) are close enough to another trajectory. * The tolerance for this depends on the `exact_match_tolerance` arg passed in init(). @@ -373,7 +376,7 @@ class TrajectoryCache * * These parameters will be used key the cache element. * - * \param[out] metadata. The metadata to add paramters to. + * \param[out] metadata. The metadata to add parameters to. * \param[in] move_group. The manipulator move group, used to get its state. * \param[in] plan_request. The motion plan request to key the cache with. * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be @@ -389,7 +392,7 @@ class TrajectoryCache * * These parameters will be used key the cache element. * - * \param[out] metadata. The metadata to add paramters to. + * \param[out] metadata. The metadata to add parameters to. * \param[in] move_group. The manipulator move group, used to get its state. * \param[in] plan_request. The motion plan request to key the cache with. * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be @@ -448,7 +451,7 @@ class TrajectoryCache * * These parameters will be used key the cache element. * - * \param[out] metadata. The metadata to add paramters to. + * \param[out] metadata. The metadata to add parameters to. * \param[in] move_group. The manipulator move group, used to get its state. * \param[in] plan_request. The cartesian plan request to key the cache with. * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be @@ -465,7 +468,7 @@ class TrajectoryCache * * These parameters will be used key the cache element. * - * \param[out] metadata. The metadata to add paramters to. + * \param[out] metadata. The metadata to add parameters to. * \param[in] move_group. The manipulator move group, used to get its state. * \param[in] plan_request. The cartesian plan request to key the cache with. * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index b037673eac..7b793031de 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -36,15 +36,15 @@ using warehouse_ros::Query; // Utils ======================================================================= // Append a range inclusive query with some tolerance about some center value. -void query_append_range_inclusive_with_tolerance(Query& query, const std::string& name, double center, double tolerance) +void queryAppendRangeInclusiveWithTolerance(Query& query, const std::string& name, double center, double tolerance) { query.appendRangeInclusive(name, center - tolerance / 2, center + tolerance / 2); } // Sort constraint components by joint or link name. -void sort_constraints(std::vector& joint_constraints, - std::vector& position_constraints, - std::vector& orientation_constraints) +void sortConstraints(std::vector& joint_constraints, + std::vector& position_constraints, + std::vector& orientation_constraints) { std::sort(joint_constraints.begin(), joint_constraints.end(), [](const moveit_msgs::msg::JointConstraint& l, const moveit_msgs::msg::JointConstraint& r) { @@ -132,8 +132,8 @@ MessageWithMetadata::ConstPtr TrajectoryCache { // First find all matching, but metadata only. // Then use the ID metadata of the best plan to pull the actual message. - auto matching_trajectories = this->fetchAllMatchingTrajectories(move_group, cache_namespace, plan_request, - start_tolerance, goal_tolerance, true, sort_by, ascending); + auto matching_trajectories = this->fetchAllMatchingTrajectories( + move_group, cache_namespace, plan_request, start_tolerance, goal_tolerance, true, sort_by, ascending); if (matching_trajectories.empty()) { @@ -316,8 +316,9 @@ MessageWithMetadata::ConstPtr TrajectoryCache { // First find all matching, but metadata only. // Then use the ID metadata of the best plan to pull the actual message. - auto matching_trajectories = this->fetchAllMatchingCartesianTrajectories( - move_group, cache_namespace, plan_request, min_fraction, start_tolerance, goal_tolerance, true, sort_by, ascending); + auto matching_trajectories = + this->fetchAllMatchingCartesianTrajectories(move_group, cache_namespace, plan_request, min_fraction, + start_tolerance, goal_tolerance, true, sort_by, ascending); if (matching_trajectories.empty()) { @@ -504,8 +505,8 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) { query.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - query_append_range_inclusive_with_tolerance(query, "start_state.joint_state.position_" + std::to_string(i), - current_state_msg.joint_state.position.at(i), match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i), match_tolerance); } } else @@ -513,8 +514,8 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) { query.append("start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); - query_append_range_inclusive_with_tolerance(query, "start_state.joint_state.position_" + std::to_string(i), - plan_request.start_state.joint_state.position.at(i), match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i), match_tolerance); } } return true; @@ -549,12 +550,12 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( "Not supported."); } - query_append_range_inclusive_with_tolerance(query, "max_velocity_scaling_factor", - plan_request.max_velocity_scaling_factor, match_tolerance); - query_append_range_inclusive_with_tolerance(query, "max_acceleration_scaling_factor", - plan_request.max_acceleration_scaling_factor, match_tolerance); - query_append_range_inclusive_with_tolerance(query, "max_cartesian_speed", plan_request.max_cartesian_speed, - match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor, + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "max_acceleration_scaling_factor", + plan_request.max_acceleration_scaling_factor, match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "max_cartesian_speed", plan_request.max_cartesian_speed, + match_tolerance); // Extract constraints (so we don't have cardinality on goal_constraint idx.) std::vector joint_constraints; @@ -576,7 +577,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( } // Also sort for even less cardinality. - sort_constraints(joint_constraints, position_constraints, orientation_constraints); + sortConstraints(joint_constraints, position_constraints, orientation_constraints); } // Joint constraints @@ -586,7 +587,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( std::string meta_name = "goal_constraints.joint_constraints_" + std::to_string(joint_idx++); query.append(meta_name + ".joint_name", constraint.joint_name); - query_append_range_inclusive_with_tolerance(query, meta_name + ".position", constraint.position, match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position", constraint.position, match_tolerance); query.appendGTE(meta_name + ".tolerance_above", constraint.tolerance_above); query.appendLTE(meta_name + ".tolerance_below", constraint.tolerance_below); } @@ -636,12 +637,12 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( query.append(meta_name + ".link_name", constraint.link_name); - query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.x", - x_offset + constraint.target_point_offset.x, match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.y", - y_offset + constraint.target_point_offset.y, match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.z", - z_offset + constraint.target_point_offset.z, match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.x", + x_offset + constraint.target_point_offset.x, match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.y", + y_offset + constraint.target_point_offset.y, match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.z", + z_offset + constraint.target_point_offset.z, match_tolerance); } } @@ -704,14 +705,14 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; final_quat.normalize(); - query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.x", final_quat.getX(), - match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.y", final_quat.getY(), - match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.z", final_quat.getZ(), - match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.w", final_quat.getW(), - match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.x", final_quat.getX(), + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.y", final_quat.getY(), + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.z", final_quat.getZ(), + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.w", final_quat.getW(), + match_tolerance); } } @@ -847,7 +848,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( } // Also sort for even less cardinality. - sort_constraints(joint_constraints, position_constraints, orientation_constraints); + sortConstraints(joint_constraints, position_constraints, orientation_constraints); } // Joint constraints @@ -1039,8 +1040,8 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) { query.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - query_append_range_inclusive_with_tolerance(query, "start_state.joint_state.position_" + std::to_string(i), - current_state_msg.joint_state.position.at(i), match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i), match_tolerance); } } else @@ -1048,8 +1049,8 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) { query.append("start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); - query_append_range_inclusive_with_tolerance(query, "start_state.joint_state.position_" + std::to_string(i), - plan_request.start_state.joint_state.position.at(i), match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i), match_tolerance); } } @@ -1075,12 +1076,12 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( RCLCPP_WARN(logger_, "Ignoring avoid_collisions: Not supported."); } - query_append_range_inclusive_with_tolerance(query, "max_velocity_scaling_factor", - plan_request.max_velocity_scaling_factor, match_tolerance); - query_append_range_inclusive_with_tolerance(query, "max_acceleration_scaling_factor", - plan_request.max_acceleration_scaling_factor, match_tolerance); - query_append_range_inclusive_with_tolerance(query, "max_step", plan_request.max_step, match_tolerance); - query_append_range_inclusive_with_tolerance(query, "jump_threshold", plan_request.jump_threshold, match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor, + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "max_acceleration_scaling_factor", + plan_request.max_acceleration_scaling_factor, match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "max_step", plan_request.max_step, match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "jump_threshold", plan_request.jump_threshold, match_tolerance); // Waypoints // Restating them in terms of the robot model frame (usually base_link) @@ -1131,12 +1132,12 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( // Apply offsets // Position - query_append_range_inclusive_with_tolerance(query, meta_name + ".position.x", x_offset + waypoint.position.x, - match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".position.y", y_offset + waypoint.position.y, - match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".position.z", z_offset + waypoint.position.z, - match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.x", x_offset + waypoint.position.x, + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.y", y_offset + waypoint.position.y, + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.z", z_offset + waypoint.position.z, + match_tolerance); // Orientation tf2::Quaternion tf2_quat_goal_offset(waypoint.orientation.x, waypoint.orientation.y, waypoint.orientation.z, @@ -1146,10 +1147,10 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; final_quat.normalize(); - query_append_range_inclusive_with_tolerance(query, meta_name + ".orientation.x", final_quat.getX(), match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".orientation.y", final_quat.getY(), match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".orientation.z", final_quat.getZ(), match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".orientation.w", final_quat.getW(), match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.x", final_quat.getX(), match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.y", final_quat.getY(), match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.z", final_quat.getZ(), match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.w", final_quat.getW(), match_tolerance); } query.append("link_name", plan_request.link_name); From 3199c33d0aec19e6c0291127b049581f71651012 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Thu, 25 Jul 2024 20:43:54 -0700 Subject: [PATCH 14/26] Fix build for downstream packages Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/CMakeLists.txt | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt index 9d55f04f2c..907f5942f6 100644 --- a/moveit_ros/trajectory_cache/CMakeLists.txt +++ b/moveit_ros/trajectory_cache/CMakeLists.txt @@ -5,7 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) @@ -34,9 +33,20 @@ set(TRAJECTORY_CACHE_DEPENDENCIES # Motion plan cache library add_library(trajectory_cache src/trajectory_cache.cpp) -ament_target_dependencies(trajectory_cache ${TRAJECTORY_CACHE_DEPENDENCIES}) target_include_directories( - trajectory_cache PUBLIC $) + trajectory_cache + PUBLIC $ + $) +ament_target_dependencies(trajectory_cache ${TRAJECTORY_CACHE_DEPENDENCIES}) + +install( + TARGETS trajectory_cache + EXPORT moveit_ros_trajectory_cacheTargets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin) + +install(DIRECTORY include/ DESTINATION include/moveit_ros_trajectory_cache) # ============================================================================== @@ -58,5 +68,6 @@ if(BUILD_TESTING) endif() +ament_export_targets(moveit_ros_trajectory_cacheTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${TRAJECTORY_CACHE_DEPENDENCIES}) ament_package() From ac50316ab1327250e42db2421af24da05fc66da5 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Fri, 26 Jul 2024 00:19:43 -0700 Subject: [PATCH 15/26] Always get some workspace frame ID Signed-off-by: methylDragon --- .../trajectory_cache/src/trajectory_cache.cpp | 115 +++++++++++------- 1 file changed, 69 insertions(+), 46 deletions(-) diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index 7b793031de..dffee5ebd3 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -35,6 +35,20 @@ using warehouse_ros::Query; // Utils ======================================================================= +// Ensure we always have a workspace frame ID. +std::string getWorkspaceFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::WorkspaceParameters& workspace_parameters) +{ + if (workspace_parameters.header.frame_id.empty()) + { + return move_group.getRobotModel()->getModelFrame(); + } + else + { + return workspace_parameters.header.frame_id; + } +} + // Append a range inclusive query with some tolerance about some center value. void queryAppendRangeInclusiveWithTolerance(Query& query, const std::string& name, double center, double tolerance) { @@ -158,24 +172,25 @@ bool TrajectoryCache::putTrajectory(const moveit::planning_interface::MoveGroupI const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, double planning_time_s, bool delete_worse_trajectories) { + std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); + // Check pre-conditions if (!trajectory.multi_dof_joint_trajectory.points.empty()) { RCLCPP_ERROR(logger_, "Skipping plan insert: Multi-DOF trajectory plans are not supported."); return false; } - if (plan_request.workspace_parameters.header.frame_id.empty() || trajectory.joint_trajectory.header.frame_id.empty()) + if (workspace_frame_id.empty() || trajectory.joint_trajectory.header.frame_id.empty()) { RCLCPP_ERROR(logger_, "Skipping plan insert: Frame IDs cannot be empty."); return false; } - if (plan_request.workspace_parameters.header.frame_id != trajectory.joint_trajectory.header.frame_id) + if (workspace_frame_id != trajectory.joint_trajectory.header.frame_id) { RCLCPP_ERROR(logger_, "Skipping plan insert: " "Plan request frame (%s) does not match plan frame (%s).", - plan_request.workspace_parameters.header.frame_id.c_str(), - trajectory.joint_trajectory.header.frame_id.c_str()); + workspace_frame_id.c_str(), trajectory.joint_trajectory.header.frame_id.c_str()); return false; } @@ -448,6 +463,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) { + std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); match_tolerance += exact_match_precision_; // Make ignored members explicit @@ -464,7 +480,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( // Workspace params // Match anything within our specified workspace limits. - query.append("workspace_parameters.header.frame_id", plan_request.workspace_parameters.header.frame_id); + query.append("workspace_parameters.header.frame_id", workspace_frame_id); query.appendGTE("workspace_parameters.min_corner.x", plan_request.workspace_parameters.min_corner.x); query.appendGTE("workspace_parameters.min_corner.y", plan_request.workspace_parameters.min_corner.y); query.appendGTE("workspace_parameters.min_corner.z", plan_request.workspace_parameters.min_corner.z); @@ -495,7 +511,8 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( { RCLCPP_WARN(logger_, "Skipping start query append: Could not get robot state."); // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. return false; } @@ -522,9 +539,10 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( } bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( - Query& query, const moveit::planning_interface::MoveGroupInterface& /* move_group */, + Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) { + std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); match_tolerance += exact_match_precision_; // Make ignored members explicit @@ -597,8 +615,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( // instead. if (!position_constraints.empty()) { - query.append("goal_constraints.position_constraints.header.frame_id", - plan_request.workspace_parameters.header.frame_id); + query.append("goal_constraints.position_constraints.header.frame_id", workspace_frame_id); size_t pos_idx = 0; for (auto& constraint : position_constraints) @@ -610,12 +627,12 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( double y_offset = 0; double z_offset = 0; - if (plan_request.workspace_parameters.header.frame_id != constraint.header.frame_id) + if (workspace_frame_id != constraint.header.frame_id) { try { - auto transform = tf_buffer_->lookupTransform( - constraint.header.frame_id, plan_request.workspace_parameters.header.frame_id, tf2::TimePointZero); + auto transform = + tf_buffer_->lookupTransform(constraint.header.frame_id, workspace_frame_id, tf2::TimePointZero); x_offset = transform.transform.translation.x; y_offset = transform.transform.translation.y; @@ -626,11 +643,11 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( RCLCPP_WARN(logger_, "Skipping goal query append: " "Could not get goal transform for translation %s to %s: %s", - plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), - ex.what()); + workspace_frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. return false; } } @@ -651,8 +668,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( // instead. if (!orientation_constraints.empty()) { - query.append("goal_constraints.orientation_constraints.header.frame_id", - plan_request.workspace_parameters.header.frame_id); + query.append("goal_constraints.orientation_constraints.header.frame_id", workspace_frame_id); size_t ori_idx = 0; for (auto& constraint : orientation_constraints) @@ -666,12 +682,12 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( quat_offset.z = 0; quat_offset.w = 1; - if (plan_request.workspace_parameters.header.frame_id != constraint.header.frame_id) + if (workspace_frame_id != constraint.header.frame_id) { try { - auto transform = tf_buffer_->lookupTransform( - constraint.header.frame_id, plan_request.workspace_parameters.header.frame_id, tf2::TimePointZero); + auto transform = + tf_buffer_->lookupTransform(constraint.header.frame_id, workspace_frame_id, tf2::TimePointZero); quat_offset = transform.transform.rotation; } @@ -680,11 +696,11 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( RCLCPP_WARN(logger_, "Skipping goal query append: " "Could not get goal transform for orientation %s to %s: %s", - plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), - ex.what()); + workspace_frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. return false; } } @@ -725,6 +741,8 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToMetadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request) { + std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); + // Make ignored members explicit if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) { @@ -738,7 +756,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToMetadata( metadata.append("group_name", plan_request.group_name); // Workspace params - metadata.append("workspace_parameters.header.frame_id", plan_request.workspace_parameters.header.frame_id); + metadata.append("workspace_parameters.header.frame_id", workspace_frame_id); metadata.append("workspace_parameters.min_corner.x", plan_request.workspace_parameters.min_corner.x); metadata.append("workspace_parameters.min_corner.y", plan_request.workspace_parameters.min_corner.y); metadata.append("workspace_parameters.min_corner.z", plan_request.workspace_parameters.min_corner.z); @@ -769,7 +787,8 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToMetadata( { RCLCPP_WARN(logger_, "Skipping start metadata append: Could not get robot state."); // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. return false; } @@ -798,9 +817,11 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToMetadata( } bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( - Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& /* move_group */, + Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request) { + std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); + // Make ignored members explicit bool emit_position_constraint_warning = false; for (auto& constraint : plan_request.goal_constraints) @@ -868,8 +889,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( { // All offsets will be "frozen" and computed wrt. the workspace frame // instead. - metadata.append("goal_constraints.position_constraints.header.frame_id", - plan_request.workspace_parameters.header.frame_id); + metadata.append("goal_constraints.position_constraints.header.frame_id", workspace_frame_id); size_t position_idx = 0; for (auto& constraint : position_constraints) @@ -881,12 +901,12 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( double y_offset = 0; double z_offset = 0; - if (plan_request.workspace_parameters.header.frame_id != constraint.header.frame_id) + if (workspace_frame_id != constraint.header.frame_id) { try { - auto transform = tf_buffer_->lookupTransform( - constraint.header.frame_id, plan_request.workspace_parameters.header.frame_id, tf2::TimePointZero); + auto transform = + tf_buffer_->lookupTransform(constraint.header.frame_id, workspace_frame_id, tf2::TimePointZero); x_offset = transform.transform.translation.x; y_offset = transform.transform.translation.y; @@ -897,11 +917,11 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( RCLCPP_WARN(logger_, "Skipping goal metadata append: " "Could not get goal transform for translation %s to %s: %s", - plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), - ex.what()); + workspace_frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. return false; } } @@ -919,8 +939,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( { // All offsets will be "frozen" and computed wrt. the workspace frame // instead. - metadata.append("goal_constraints.orientation_constraints.header.frame_id", - plan_request.workspace_parameters.header.frame_id); + metadata.append("goal_constraints.orientation_constraints.header.frame_id", workspace_frame_id); size_t ori_idx = 0; for (auto& constraint : orientation_constraints) @@ -934,12 +953,12 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( quat_offset.z = 0; quat_offset.w = 1; - if (plan_request.workspace_parameters.header.frame_id != constraint.header.frame_id) + if (workspace_frame_id != constraint.header.frame_id) { try { - auto transform = tf_buffer_->lookupTransform( - constraint.header.frame_id, plan_request.workspace_parameters.header.frame_id, tf2::TimePointZero); + auto transform = + tf_buffer_->lookupTransform(constraint.header.frame_id, workspace_frame_id, tf2::TimePointZero); quat_offset = transform.transform.rotation; } @@ -948,11 +967,11 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( RCLCPP_WARN(logger_, "Skipping goal metadata append: " "Could not get goal transform for orientation %s to %s: %s", - plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), - ex.what()); + workspace_frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. return false; } } @@ -1030,7 +1049,8 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( { RCLCPP_WARN(logger_, "Skipping start metadata append: Could not get robot state."); // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. return false; } @@ -1117,7 +1137,8 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( base_frame.c_str(), plan_request.header.frame_id.c_str(), ex.what()); // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. return false; } } @@ -1200,7 +1221,8 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToMetadata( { RCLCPP_WARN(logger_, "Skipping start metadata append: Could not get robot state."); // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. return false; } @@ -1284,7 +1306,8 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToMetadata( base_frame.c_str(), plan_request.header.frame_id.c_str(), ex.what()); // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not supported. + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. return false; } } From 5a546280661c7b7a61f3c29ed1cb579885a2942c Mon Sep 17 00:00:00 2001 From: methylDragon Date: Fri, 26 Jul 2024 01:12:54 -0700 Subject: [PATCH 16/26] Always get some cartesian path request frame ID Signed-off-by: methylDragon --- .../trajectory_cache/src/trajectory_cache.cpp | 54 +++++++++++++++---- 1 file changed, 44 insertions(+), 10 deletions(-) diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index dffee5ebd3..2976402112 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -49,6 +49,20 @@ std::string getWorkspaceFrameId(const moveit::planning_interface::MoveGroupInter } } +// Ensure we always have a cartesian path request frame ID. +std::string getCartesianPathRequestFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& path_request) +{ + if (path_request.header.frame_id.empty()) + { + return move_group.getPoseReferenceFrame(); + } + else + { + return path_request.header.frame_id; + } +} + // Append a range inclusive query with some tolerance about some center value. void queryAppendRangeInclusiveWithTolerance(Query& query, const std::string& name, double center, double tolerance) { @@ -180,9 +194,14 @@ bool TrajectoryCache::putTrajectory(const moveit::planning_interface::MoveGroupI RCLCPP_ERROR(logger_, "Skipping plan insert: Multi-DOF trajectory plans are not supported."); return false; } - if (workspace_frame_id.empty() || trajectory.joint_trajectory.header.frame_id.empty()) + if (workspace_frame_id.empty()) { - RCLCPP_ERROR(logger_, "Skipping plan insert: Frame IDs cannot be empty."); + RCLCPP_ERROR(logger_, "Skipping plan insert: Workspace frame ID cannot be empty."); + return false; + } + if (trajectory.joint_trajectory.header.frame_id.empty()) + { + RCLCPP_ERROR(logger_, "Skipping plan insert: Trajectory frame ID cannot be empty."); return false; } if (workspace_frame_id != trajectory.joint_trajectory.header.frame_id) @@ -360,6 +379,8 @@ bool TrajectoryCache::putCartesianTrajectory(const moveit::planning_interface::M double execution_time_s, double planning_time_s, double fraction, bool delete_worse_trajectories) { + std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); + // Check pre-conditions if (!trajectory.multi_dof_joint_trajectory.points.empty()) { @@ -367,9 +388,14 @@ bool TrajectoryCache::putCartesianTrajectory(const moveit::planning_interface::M "Multi-DOF trajectory plans are not supported."); return false; } - if (plan_request.header.frame_id.empty() || trajectory.joint_trajectory.header.frame_id.empty()) + if (path_request_frame_id.empty()) + { + RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: Path request frame ID cannot be empty."); + return false; + } + if (trajectory.joint_trajectory.header.frame_id.empty()) { - RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: Frame IDs cannot be empty."); + RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: Trajectory frame ID cannot be empty."); return false; } @@ -1012,6 +1038,7 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) { + std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); match_tolerance += exact_match_precision_; // Make ignored members explicit @@ -1025,6 +1052,7 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( } query.append("group_name", plan_request.group_name); + query.append("path_request.header.frame_id", path_request_frame_id); // Joint state // Only accounts for joint_state position. Ignores velocity and effort. @@ -1081,6 +1109,7 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) { + std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); match_tolerance += exact_match_precision_; // Make ignored members explicit @@ -1118,11 +1147,11 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( quat_offset.z = 0; quat_offset.w = 1; - if (base_frame != plan_request.header.frame_id) + if (base_frame != path_request_frame_id) { try { - auto transform = tf_buffer_->lookupTransform(plan_request.header.frame_id, base_frame, tf2::TimePointZero); + auto transform = tf_buffer_->lookupTransform(path_request_frame_id, base_frame, tf2::TimePointZero); x_offset = transform.transform.translation.x; y_offset = transform.transform.translation.y; @@ -1134,7 +1163,7 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( RCLCPP_WARN(logger_, "Skipping goal metadata append: " "Could not get goal transform for %s to %s: %s", - base_frame.c_str(), plan_request.header.frame_id.c_str(), ex.what()); + base_frame.c_str(), path_request_frame_id.c_str(), ex.what()); // NOTE: methyldragon - // Ideally we would restore the original state here and undo our changes, however copy of the query is not @@ -1186,6 +1215,8 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToMetadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request) { + std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); + // Make ignored members explicit if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) { @@ -1197,6 +1228,7 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToMetadata( } metadata.append("group_name", plan_request.group_name); + metadata.append("path_request.header.frame_id", path_request_frame_id); // Joint state // Only accounts for joint_state position. Ignores velocity and effort. @@ -1254,6 +1286,8 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToMetadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request) { + std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); + // Make ignored members explicit if (!plan_request.path_constraints.joint_constraints.empty() || !plan_request.path_constraints.position_constraints.empty() || @@ -1287,11 +1321,11 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToMetadata( quat_offset.z = 0; quat_offset.w = 1; - if (base_frame != plan_request.header.frame_id) + if (base_frame != path_request_frame_id) { try { - auto transform = tf_buffer_->lookupTransform(plan_request.header.frame_id, base_frame, tf2::TimePointZero); + auto transform = tf_buffer_->lookupTransform(path_request_frame_id, base_frame, tf2::TimePointZero); x_offset = transform.transform.translation.x; y_offset = transform.transform.translation.y; @@ -1303,7 +1337,7 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToMetadata( RCLCPP_WARN(logger_, "Skipping goal metadata append: " "Could not get goal transform for %s to %s: %s", - base_frame.c_str(), plan_request.header.frame_id.c_str(), ex.what()); + base_frame.c_str(), path_request_frame_id.c_str(), ex.what()); // NOTE: methyldragon - // Ideally we would restore the original state here and undo our changes, however copy of the query is not From caa466968dabfdb68db4770a81f04b713a695ae8 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Fri, 26 Jul 2024 01:13:10 -0700 Subject: [PATCH 17/26] Fix tests Signed-off-by: methylDragon --- .../trajectory_cache/trajectory_cache.hpp | 23 ++-- .../trajectory_cache/src/trajectory_cache.cpp | 2 +- .../test/test_trajectory_cache.cpp | 117 +++++++++--------- .../test/test_trajectory_cache.py | 2 +- 4 files changed, 70 insertions(+), 74 deletions(-) diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index c7b327f4e7..9bf244922f 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -134,7 +134,8 @@ class TrajectoryCache * \param[in] db_port. The database port. * \param[in] exact_match_precision. Tolerance for float precision comparison for what counts as an exact match. * An exact match is when: - * (candidate >= value - (exact_match_precision / 2) && candidate <= value + (exact_match_precision / 2)) + * (candidate >= value - (exact_match_precision / 2) + * && candidate <= value + (exact_match_precision / 2)) * \returns true if the database was successfully connected to. * */ bool init(const std::string& db_path = ":memory:", uint32_t db_port = 0, double exact_match_precision = 1e-6); @@ -204,11 +205,11 @@ class TrajectoryCache * \brief Put a trajectory into the database if it is the best matching trajectory seen so far. * * Trajectories are matched based off their start and goal states. - * And are considered "better" if they higher priority in the sorting order specified by `sort_by` than exactly - * matching trajectories. + * And are considered "better" if they are higher priority in the sorting order specified by `sort_by` than another + * exactly matching trajectory. * * A trajectory is "exactly matching" if its start and goal are close enough to another trajectory. - * The tolerance for this depends on the `exact_match_tolerance` arg passed in init(). + * The tolerance for this depends on the `exact_match_precision` arg passed in init(). * \see init() * * Optionally deletes all worse trajectories by default to prune the cache. @@ -300,11 +301,11 @@ class TrajectoryCache * \brief Put a cartesian trajectory into the database if it is the best matching cartesian trajectory seen so far. * * Cartesian trajectories are matched based off their start and goal states. - * And are considered "better" if they higher priority in the sorting order specified by `sort_by` than exactly - * matching cartesian trajectories. + * And are considered "better" if they are higher priority in the sorting order specified by `sort_by` than another + * exactly matching cartesian trajectory. * * A trajectory is "exactly matching" if its start and goal (and fraction) are close enough to another trajectory. - * The tolerance for this depends on the `exact_match_tolerance` arg passed in init(). + * The tolerance for this depends on the `exact_match_precision` arg passed in init(). * \see init() * * Optionally deletes all worse cartesian trajectories by default to prune the cache. @@ -343,7 +344,7 @@ class TrajectoryCache * \param[out] query. The query to add parameters to. * \param[in] move_group. The manipulator move group, used to get its state. * \param[in] plan_request. The motion plan request to key the cache with. - * \param[in] match_tolerance. The match tolerance (additive with exact_match_tolerance) for the query. + * \param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. * \returns true if successfully added to. If false, the query might have been partially modified and should not be * used. */ @@ -361,7 +362,7 @@ class TrajectoryCache * \param[out] query. The query to add parameters to. * \param[in] move_group. The manipulator move group, used to get its state. * \param[in] plan_request. The motion plan request to key the cache with. - * \param[in] match_tolerance. The match tolerance (additive with exact_match_tolerance) for the query. + * \param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. * \returns true if successfully added to. If false, the query might have been partially modified and should not be * used. */ @@ -418,7 +419,7 @@ class TrajectoryCache * \param[out] query. The query to add parameters to. * \param[in] move_group. The manipulator move group, used to get its state. * \param[in] plan_request. The cartesian plan request to key the cache with. - * \param[in] match_tolerance. The match tolerance (additive with exact_match_tolerance) for the query. + * \param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. * \returns true if successfully added to. If false, the query might have been partially modified and should not be * used. */ @@ -436,7 +437,7 @@ class TrajectoryCache * \param[out] query. The query to add parameters to. * \param[in] move_group. The manipulator move group, used to get its state. * \param[in] plan_request. The cartesian plan request to key the cache with. - * \param[in] match_tolerance. The match tolerance (additive with exact_match_tolerance) for the query. + * \param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. * \returns true if successfully added to. If false, the query might have been partially modified and should not be * used. */ diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index 2976402112..0e4aa0d826 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -51,7 +51,7 @@ std::string getWorkspaceFrameId(const moveit::planning_interface::MoveGroupInter // Ensure we always have a cartesian path request frame ID. std::string getCartesianPathRequestFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& path_request) + const moveit_msgs::srv::GetCartesianPath::Request& path_request) { if (path_request.header.frame_id.empty()) { diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp index 138761c029..7e05c357c9 100644 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp @@ -38,7 +38,6 @@ void check_and_emit(bool predicate, const std::string& prefix, const std::string else { std::cout << "[FAIL] " << prefix << ": " << label << std::endl; - exit(1); } } @@ -215,37 +214,37 @@ void test_motion_trajectories(std::shared_ptr move_group, st // Put trajectory req empty frame // - // Trajectory request must have frame in workspace, expect put fail + // Trajectory request having no frame in workspace should default to robot frame prefix = "test_motion_trajectories.putTrajectory_req_empty_frame"; - execution_time = 999; - planning_time = 999; + execution_time = 1000; + planning_time = 1000; - check_and_emit(!cache->putTrajectory(*move_group, ROBOT_NAME, empty_frame_plan_req, traj, execution_time, - planning_time, false), - prefix, "Put empty frame req trajectory, no delete_worse_trajectories, not ok"); + check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, empty_frame_plan_req, traj, execution_time, + planning_time, false), + prefix, "Put empty frame req trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 0, prefix, "No trajectories in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); - // Put first, no delete_worse_trajectories - prefix = "test_motion_trajectories.put_first"; + // Put second, no delete_worse_trajectories + prefix = "test_motion_trajectories.put_second"; execution_time = 999; planning_time = 999; check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, traj, execution_time, planning_time, false), - prefix, "Put first valid trajectory, no delete_worse_trajectories, ok"); + prefix, "Put second valid trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); // Fetch matching, no tolerance // // Exact key match should have cache hit - prefix = "test_motion_trajectories.put_first.fetch_matching_no_tolerance"; + prefix = "test_motion_trajectories.put_second.fetch_matching_no_tolerance"; auto fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0); auto fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); @@ -262,13 +261,13 @@ void test_motion_trajectories(std::shared_ptr move_group, st // // is_diff key should be equivalent to exact match if robot state did not // change, hence should have cache hit - prefix = "test_motion_trajectories.put_first.fetch_is_diff_no_tolerance"; + prefix = "test_motion_trajectories.put_second.fetch_is_diff_no_tolerance"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, is_diff_plan_req, 0, 0); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, is_diff_plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); @@ -284,7 +283,7 @@ void test_motion_trajectories(std::shared_ptr move_group, st // Fetch non-matching, out of tolerance // // Non-matching key should not have cache hit - prefix = "test_motion_trajectories.put_first.fetch_non_matching_out_of_tolerance"; + prefix = "test_motion_trajectories.put_second.fetch_non_matching_out_of_tolerance"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 0); @@ -296,7 +295,7 @@ void test_motion_trajectories(std::shared_ptr move_group, st // Fetch non-matching, only start in tolerance (but not goal) // // Non-matching key should not have cache hit - prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_start_in_tolerance"; + prefix = "test_motion_trajectories.put_second.fetch_non_matching_only_start_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 999, 0); @@ -308,7 +307,7 @@ void test_motion_trajectories(std::shared_ptr move_group, st // Fetch non-matching, only goal in tolerance (but not start) // // Non-matching key should not have cache hit - prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_goal_in_tolerance"; + prefix = "test_motion_trajectories.put_second.fetch_non_matching_only_goal_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 999); @@ -320,14 +319,14 @@ void test_motion_trajectories(std::shared_ptr move_group, st // Fetch non-matching, in tolerance // // Close key within tolerance limit should have cache hit - prefix = "test_motion_trajectories.put_first.fetch_non_matching_in_tolerance"; + prefix = "test_motion_trajectories.put_second.fetch_non_matching_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0.1, 0.1); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0.1, 0.1); - check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); @@ -343,14 +342,14 @@ void test_motion_trajectories(std::shared_ptr move_group, st // Fetch swapped // // Matches should be mostly invariant to constraint ordering - prefix = "test_motion_trajectories.put_first.fetch_swapped"; + prefix = "test_motion_trajectories.put_second.fetch_swapped"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, swapped_close_matching_plan_req, 0.1, 0.1); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, swapped_close_matching_plan_req, 0.1, 0.1); - check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); @@ -367,7 +366,7 @@ void test_motion_trajectories(std::shared_ptr move_group, st // // Matching trajectories with more restrictive workspace requirements should not // pull up trajectories cached for less restrictive workspace requirements - prefix = "test_motion_trajectories.put_first.fetch_smaller_workspace"; + prefix = "test_motion_trajectories.put_second.fetch_smaller_workspace"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, smaller_workspace_plan_req, 999, 999); @@ -381,14 +380,14 @@ void test_motion_trajectories(std::shared_ptr move_group, st // // Matching trajectories with less restrictive workspace requirements should pull up // trajectories cached for more restrictive workspace requirements - prefix = "test_motion_trajectories.put_first.fetch_larger_workspace"; + prefix = "test_motion_trajectories.put_second.fetch_larger_workspace"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, larger_workspace_plan_req, 999, 999); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, larger_workspace_plan_req, 999, 999); - check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); @@ -419,7 +418,7 @@ void test_motion_trajectories(std::shared_ptr move_group, st false), prefix, "Put worse trajectory, no delete_worse_trajectories, not ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); // Put better, no delete_worse_trajectories // @@ -431,7 +430,7 @@ void test_motion_trajectories(std::shared_ptr move_group, st false), prefix, "Put better trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 3, prefix, "Three trajectories in cache"); // Fetch sorted // @@ -442,7 +441,7 @@ void test_motion_trajectories(std::shared_ptr move_group, st fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); + check_and_emit(fetched_trajectories.size() == 3, prefix, "Fetch all returns three"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); @@ -540,9 +539,7 @@ void test_motion_trajectories(std::shared_ptr move_group, st better_execution_time, planning_time, true), prefix, "Put different trajectory req, delete_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); - - check_and_emit(cache->countTrajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); + check_and_emit(cache->countTrajectories(different_robot_name) == 1, prefix, "One trajectory in cache"); check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in original robot's cache"); @@ -665,32 +662,32 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // Put trajectory req empty frame // - // Trajectory request must have frame in workspace, expect put fail + // Trajectory request having no frame in workspace should default to robot frame prefix = "test_cartesian_trajectories.putTrajectory_req_empty_frame"; - execution_time = 999; - planning_time = 999; + execution_time = 1000; + planning_time = 1000; - check_and_emit(!cache->putCartesianTrajectory(*move_group, ROBOT_NAME, empty_frame_cartesian_plan_req, traj, - execution_time, planning_time, fraction, false), - prefix, "Put empty frame req trajectory, no delete_worse_trajectories, not ok"); + check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, empty_frame_cartesian_plan_req, traj, + execution_time, planning_time, fraction, false), + prefix, "Put empty frame req trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, prefix, "No trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); - // Put first, no delete_worse_trajectories - prefix = "test_cartesian_trajectories.put_first"; + // Put second, no delete_worse_trajectories + prefix = "test_cartesian_trajectories.put_second"; execution_time = 999; planning_time = 999; check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, execution_time, planning_time, fraction, false), - prefix, "Put first valid trajectory, no delete_worse_trajectories, ok"); + prefix, "Put second valid trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); // Fetch matching, no tolerance // // Exact key match should have cache hit - prefix = "test_cartesian_trajectories.put_first.fetch_matching_no_tolerance"; + prefix = "test_cartesian_trajectories.put_second.fetch_matching_no_tolerance"; auto fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); @@ -698,7 +695,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, auto fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); @@ -717,7 +714,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // // is_diff key should be equivalent to exact match if robot state did not // change, hence should have cache hit - prefix = "test_cartesian_trajectories.put_first.fetch_is_diff_no_tolerance"; + prefix = "test_cartesian_trajectories.put_second.fetch_is_diff_no_tolerance"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, is_diff_cartesian_plan_req, fraction, 0, 0); @@ -725,7 +722,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, is_diff_cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); @@ -743,7 +740,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // Fetch non-matching, out of tolerance // // Non-matching key should not have cache hit - prefix = "test_cartesian_trajectories.put_first.fetch_non_matching_out_of_tolerance"; + prefix = "test_cartesian_trajectories.put_second.fetch_non_matching_out_of_tolerance"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 0); @@ -757,7 +754,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // Fetch non-matching, only start in tolerance (but not goal) // // Non-matching key should not have cache hit - prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_start_in_tolerance"; + prefix = "test_motion_trajectories.put_second.fetch_non_matching_only_start_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 999, 0); @@ -771,7 +768,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // Fetch non-matching, only goal in tolerance (but not start) // // Non-matching key should not have cache hit - prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_goal_in_tolerance"; + prefix = "test_motion_trajectories.put_second.fetch_non_matching_only_goal_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 999); @@ -785,7 +782,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // Fetch non-matching, in tolerance // // Close key within tolerance limit should have cache hit - prefix = "test_cartesian_trajectories.put_first.fetch_non_matching_in_tolerance"; + prefix = "test_cartesian_trajectories.put_second.fetch_non_matching_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0.1, 0.1); @@ -793,7 +790,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0.1, 0.1); - check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); @@ -812,7 +809,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // // Matching trajectories with more restrictive fraction requirements should not // pull up trajectories cached for less restrictive fraction requirements - prefix = "test_cartesian_trajectories.put_first.fetch_higher_fraction"; + prefix = "test_cartesian_trajectories.put_second.fetch_higher_fraction"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 1, 999, 999); @@ -826,14 +823,14 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // // Matching trajectories with less restrictive fraction requirements should pull up // trajectories cached for more restrictive fraction requirements - prefix = "test_cartesian_trajectories.put_first.fetch_lower_fraction"; + prefix = "test_cartesian_trajectories.put_second.fetch_lower_fraction"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999); fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999); - check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); @@ -858,7 +855,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, planning_time, fraction, false), prefix, "Put worse trajectory, no delete_worse_trajectories, not ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); // Put better, no delete_worse_trajectories // @@ -870,7 +867,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, planning_time, fraction, false), prefix, "Put better trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 3, prefix, "Three trajectories in cache"); // Fetch sorted // @@ -883,7 +880,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); + check_and_emit(fetched_trajectories.size() == 3, prefix, "Fetch all returns three"); check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); @@ -991,9 +988,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, different_traj, better_execution_time, planning_time, fraction, true), prefix, "Put different trajectory req, delete_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); - - check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 1, prefix, "One trajectory in cache"); check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in original robot's cache"); diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.py b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py index 095f8016b6..4a4de5400a 100755 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.py +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py @@ -136,7 +136,7 @@ def test_all_tests_pass(trajectory_cache_test_runner_node, launch_context): assert process_tools.wait_for_output_sync( launch_context, trajectory_cache_test_runner_node, - lambda x: x.count("[PASS]") == 165, # All test cases passed. + lambda x: x.count("[PASS]") == 163, # All test cases passed. timeout=30, ) From 8b04e2fc817424eef5d15cd710ab4d636217e951 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 29 Jul 2024 22:43:30 -0700 Subject: [PATCH 18/26] Add const qualifiers as appropriate Signed-off-by: methylDragon --- .../trajectory_cache/trajectory_cache.hpp | 34 +++++++++---------- .../trajectory_cache/src/trajectory_cache.cpp | 24 ++++++------- 2 files changed, 28 insertions(+), 30 deletions(-) diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index 9bf244922f..922be64588 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -180,7 +180,7 @@ class TrajectoryCache const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, bool metadata_only = false, - const std::string& sort_by = "execution_time_s", bool ascending = true); + const std::string& sort_by = "execution_time_s", bool ascending = true) const; /** * \brief Fetch the best trajectory that fits within the requested tolerances for start and goal conditions, by some @@ -199,7 +199,7 @@ class TrajectoryCache warehouse_ros::MessageWithMetadata::ConstPtr fetchBestMatchingTrajectory( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, - bool metadata_only = false, const std::string& sort_by = "execution_time_s", bool ascending = true); + bool metadata_only = false, const std::string& sort_by = "execution_time_s", bool ascending = true) const; /** * \brief Put a trajectory into the database if it is the best matching trajectory seen so far. @@ -274,7 +274,7 @@ class TrajectoryCache const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s", - bool ascending = true); + bool ascending = true) const; /** * \brief Fetch the best cartesian trajectory that fits within the requested tolerances for start and goal conditions, @@ -295,7 +295,7 @@ class TrajectoryCache const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s", - bool ascending = true); + bool ascending = true) const; /** * \brief Put a cartesian trajectory into the database if it is the best matching cartesian trajectory seen so far. @@ -351,7 +351,7 @@ class TrajectoryCache bool extractAndAppendTrajectoryStartToQuery(warehouse_ros::Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request, - double match_tolerance); + double match_tolerance) const; /** * \brief Extract relevant parameters from a motion plan request's goal parameters to populate a cache db query, with @@ -369,7 +369,7 @@ class TrajectoryCache bool extractAndAppendTrajectoryGoalToQuery(warehouse_ros::Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request, - double match_tolerance); + double match_tolerance) const; /** * \brief Extract relevant parameters from a motion plan request's start parameters to populate a cache entry's @@ -385,7 +385,7 @@ class TrajectoryCache */ bool extractAndAppendTrajectoryStartToMetadata(warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request); + const moveit_msgs::msg::MotionPlanRequest& plan_request) const; /** * \brief Extract relevant parameters from a motion plan request's goal parameters to populate a cache entry's @@ -401,7 +401,7 @@ class TrajectoryCache */ bool extractAndAppendTrajectoryGoalToMetadata(warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request); + const moveit_msgs::msg::MotionPlanRequest& plan_request) const; /**@}*/ @@ -426,7 +426,7 @@ class TrajectoryCache bool extractAndAppendCartesianTrajectoryStartToQuery(warehouse_ros::Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, - double match_tolerance); + double match_tolerance) const; /** * \brief Extract relevant parameters from a cartesian plan request's goal parameters to populate a cache db query, @@ -444,7 +444,7 @@ class TrajectoryCache bool extractAndAppendCartesianTrajectoryGoalToQuery(warehouse_ros::Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, - double match_tolerance); + double match_tolerance) const; /** * \brief Extract relevant parameters from a cartesian plan request's goal parameters to populate a cache entry's @@ -458,10 +458,9 @@ class TrajectoryCache * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be * used. */ - bool - extractAndAppendCartesianTrajectoryStartToMetadata(warehouse_ros::Metadata& metadata, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request); + bool extractAndAppendCartesianTrajectoryStartToMetadata( + warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request) const; /** * \brief Extract relevant parameters from a cartesian plan request's goal parameters to populate a cache entry's @@ -475,10 +474,9 @@ class TrajectoryCache * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be * used. */ - bool - extractAndAppendCartesianTrajectoryGoalToMetadata(warehouse_ros::Metadata& metadata, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request); + bool extractAndAppendCartesianTrajectoryGoalToMetadata( + warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request) const; /**@}*/ diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index 0e4aa0d826..c42057d7b0 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -135,7 +135,7 @@ TrajectoryCache::fetchAllMatchingTrajectories(const moveit::planning_interface:: const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, bool metadata_only, - const std::string& sort_by, bool ascending) + const std::string& sort_by, bool ascending) const { auto coll = db_->openCollection("move_group_trajectory_cache", cache_namespace); @@ -156,7 +156,7 @@ TrajectoryCache::fetchAllMatchingTrajectories(const moveit::planning_interface:: MessageWithMetadata::ConstPtr TrajectoryCache::fetchBestMatchingTrajectory( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, - bool metadata_only, const std::string& sort_by, bool ascending) + bool metadata_only, const std::string& sort_by, bool ascending) const { // First find all matching, but metadata only. // Then use the ID metadata of the best plan to pull the actual message. @@ -322,7 +322,7 @@ TrajectoryCache::fetchAllMatchingCartesianTrajectories(const moveit::planning_in const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, double goal_tolerance, bool metadata_only, - const std::string& sort_by, bool ascending) + const std::string& sort_by, bool ascending) const { auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); @@ -346,7 +346,7 @@ TrajectoryCache::fetchAllMatchingCartesianTrajectories(const moveit::planning_in MessageWithMetadata::ConstPtr TrajectoryCache::fetchBestMatchingCartesianTrajectory( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, - double goal_tolerance, bool metadata_only, const std::string& sort_by, bool ascending) + double goal_tolerance, bool metadata_only, const std::string& sort_by, bool ascending) const { // First find all matching, but metadata only. // Then use the ID metadata of the best plan to pull the actual message. @@ -487,7 +487,7 @@ bool TrajectoryCache::putCartesianTrajectory(const moveit::planning_interface::M bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) + const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) const { std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); match_tolerance += exact_match_precision_; @@ -566,7 +566,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) + const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) const { std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); match_tolerance += exact_match_precision_; @@ -765,7 +765,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( bool TrajectoryCache::extractAndAppendTrajectoryStartToMetadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request) + const moveit_msgs::msg::MotionPlanRequest& plan_request) const { std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); @@ -844,7 +844,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToMetadata( bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request) + const moveit_msgs::msg::MotionPlanRequest& plan_request) const { std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); @@ -1036,7 +1036,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) const { std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); match_tolerance += exact_match_precision_; @@ -1107,7 +1107,7 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) const { std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); match_tolerance += exact_match_precision_; @@ -1213,7 +1213,7 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToMetadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request) + const moveit_msgs::srv::GetCartesianPath::Request& plan_request) const { std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); @@ -1284,7 +1284,7 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToMetadata( bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToMetadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request) + const moveit_msgs::srv::GetCartesianPath::Request& plan_request) const { std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); From cf980c14ef0b3f5ede177190bb5478bcd06ea31d Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 30 Jul 2024 02:52:38 -0700 Subject: [PATCH 19/26] Add accessors, and support for preserving K plans Signed-off-by: methylDragon --- .../trajectory_cache/trajectory_cache.hpp | 61 ++- .../trajectory_cache/src/trajectory_cache.cpp | 68 ++- .../test/test_trajectory_cache.cpp | 469 ++++++++++-------- .../test/test_trajectory_cache.py | 2 +- 4 files changed, 363 insertions(+), 237 deletions(-) diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index 922be64588..98bfbfa142 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -124,21 +124,40 @@ class TrajectoryCache */ explicit TrajectoryCache(const rclcpp::Node::SharedPtr& node); + /** + * \brief Options struct for TrajectoryCache. + * + * \property db_path. The database path. + * \property db_port. The database port. + * \property exact_match_precision. Tolerance for float precision comparison for what counts as an exact match. + * An exact match is when: + * (candidate >= value - (exact_match_precision / 2) + * && candidate <= value + (exact_match_precision / 2)) + * \property num_additional_trajectories_to_preserve_when_deleting_worse. The number of additional cached trajectories + * to preserve when `delete_worse_trajectories` is true. It is useful to keep more than one matching trajectory to + * have alternative trajectories to handle obstacles. + */ + struct Options + { + std::string db_path = ":memory:"; + uint32_t db_port = 0; + + double exact_match_precision = 1e-6; + size_t num_additional_trajectories_to_preserve_when_deleting_worse = 1; + }; + /** * \brief Initialize the TrajectoryCache. * * This sets up the database connection, and sets any configuration parameters. * You must call this before calling any other method of the trajectory cache. * - * \param[in] db_path. The database path. - * \param[in] db_port. The database port. - * \param[in] exact_match_precision. Tolerance for float precision comparison for what counts as an exact match. - * An exact match is when: - * (candidate >= value - (exact_match_precision / 2) - * && candidate <= value + (exact_match_precision / 2)) + * \param[in] options. An instance of TrajectoryCache::Options to initialize the cache with. + * \see TrajectoryCache::Options * \returns true if the database was successfully connected to. + * \throws When options.num_additional_trajectories_to_preserve_when_deleting_worse is less than 1. * */ - bool init(const std::string& db_path = ":memory:", uint32_t db_port = 0, double exact_match_precision = 1e-6); + bool init(const Options& options); /** * \brief Count the number of non-cartesian trajectories for a particular cache namespace. @@ -156,6 +175,32 @@ class TrajectoryCache */ unsigned countCartesianTrajectories(const std::string& cache_namespace); + /** + * \name Getters and setters. + */ + /**@{*/ + + /// \brief Gets the database path. + std::string getDbPath(); + + /// \brief Gets the database port. + uint32_t getDbPort(); + + /// \brief Getss the exact match precision. + double getExactMatchPrecision(); + + /// \brief Sets the exact match precision. + void setExactMatchPrecision(double exact_match_precision); + + /// \brief Get the number of trajectories to preserve when deleting worse trajectories. + size_t getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse(); + + /// \brief Set the number of additional trajectories to preserve when deleting worse trajectories. + void setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse( + size_t num_additional_trajectories_to_preserve_when_deleting_worse); + + /**@}*/ + /** * \name Motion plan trajectory caching */ @@ -484,7 +529,7 @@ class TrajectoryCache rclcpp::Logger logger_; warehouse_ros::DatabaseConnection::Ptr db_; - double exact_match_precision_ = 1e-6; + Options options_; std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_; diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index c42057d7b0..ee358b8d23 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -99,17 +99,17 @@ TrajectoryCache::TrajectoryCache(const rclcpp::Node::SharedPtr& node) tf_listener_ = std::make_shared(*tf_buffer_); } -bool TrajectoryCache::init(const std::string& db_path, uint32_t db_port, double exact_match_precision) +bool TrajectoryCache::init(const TrajectoryCache::Options& options) { - RCLCPP_DEBUG(logger_, "Opening trajectory cache database at: %s (Port: %d, Precision: %f)", db_path.c_str(), db_port, - exact_match_precision); + RCLCPP_DEBUG(logger_, "Opening trajectory cache database at: %s (Port: %d, Precision: %f)", options.db_path.c_str(), + options.db_port, options.exact_match_precision); // If the `warehouse_plugin` parameter isn't set, defaults to warehouse_ros' // default. db_ = moveit_warehouse::loadDatabase(node_); + options_ = options; - exact_match_precision_ = exact_match_precision; - db_->setParams(db_path, db_port); + db_->setParams(options.db_path, options.db_port); return db_->connect(); } @@ -126,6 +126,42 @@ unsigned TrajectoryCache::countCartesianTrajectories(const std::string& cache_na return coll.count(); } +// ============================================================================= +// GETTERS AND SETTERS +// ============================================================================= + +std::string TrajectoryCache::getDbPath() +{ + return options_.db_path; +} + +uint32_t TrajectoryCache::getDbPort() +{ + return options_.db_port; +} + +double TrajectoryCache::getExactMatchPrecision() +{ + return options_.exact_match_precision; +} + +void TrajectoryCache::setExactMatchPrecision(double exact_match_precision) +{ + options_.exact_match_precision = exact_match_precision; +} + +size_t TrajectoryCache::getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() +{ + return options_.num_additional_trajectories_to_preserve_when_deleting_worse; +} + +void TrajectoryCache::setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse( + size_t num_additional_trajectories_to_preserve_when_deleting_worse) +{ + options_.num_additional_trajectories_to_preserve_when_deleting_worse = + num_additional_trajectories_to_preserve_when_deleting_worse; +} + // ============================================================================= // MOTION PLAN TRAJECTORY CACHING // ============================================================================= @@ -227,7 +263,8 @@ bool TrajectoryCache::putTrajectory(const moveit::planning_interface::MoveGroupI return false; } - auto exact_matches = coll.queryList(exact_query, /*metadata_only=*/true, /*sort_by=*/"execution_time_s"); + auto exact_matches = + coll.queryList(exact_query, /*metadata_only=*/true, /*sort_by=*/"execution_time_s", /*ascending=*/true); double best_execution_time = std::numeric_limits::infinity(); if (!exact_matches.empty()) @@ -236,10 +273,12 @@ bool TrajectoryCache::putTrajectory(const moveit::planning_interface::MoveGroupI if (delete_worse_trajectories) { + size_t preserved_count = 0; for (auto& match : exact_matches) { double match_execution_time_s = match->lookupDouble("execution_time_s"); - if (execution_time_s < match_execution_time_s) + if (++preserved_count > options_.num_additional_trajectories_to_preserve_when_deleting_worse && + execution_time_s < match_execution_time_s) { int delete_id = match->lookupInt("id"); RCLCPP_DEBUG(logger_, @@ -416,7 +455,8 @@ bool TrajectoryCache::putCartesianTrajectory(const moveit::planning_interface::M return false; } - auto exact_matches = coll.queryList(exact_query, /*metadata_only=*/true, /*sort_by=*/"execution_time_s"); + auto exact_matches = + coll.queryList(exact_query, /*metadata_only=*/true, /*sort_by=*/"execution_time_s", /*ascending=*/true); double best_execution_time = std::numeric_limits::infinity(); if (!exact_matches.empty()) { @@ -424,10 +464,12 @@ bool TrajectoryCache::putCartesianTrajectory(const moveit::planning_interface::M if (delete_worse_trajectories) { + size_t preserved_count = 0; for (auto& match : exact_matches) { double match_execution_time_s = match->lookupDouble("execution_time_s"); - if (execution_time_s < match_execution_time_s) + if (++preserved_count > options_.num_additional_trajectories_to_preserve_when_deleting_worse && + execution_time_s < match_execution_time_s) { int delete_id = match->lookupInt("id"); RCLCPP_DEBUG(logger_, @@ -490,7 +532,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) const { std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); - match_tolerance += exact_match_precision_; + match_tolerance += options_.exact_match_precision; // Make ignored members explicit if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) @@ -569,7 +611,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) const { std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); - match_tolerance += exact_match_precision_; + match_tolerance += options_.exact_match_precision; // Make ignored members explicit bool emit_position_constraint_warning = false; @@ -1039,7 +1081,7 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) const { std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); - match_tolerance += exact_match_precision_; + match_tolerance += options_.exact_match_precision; // Make ignored members explicit if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) @@ -1110,7 +1152,7 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) const { std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); - match_tolerance += exact_match_precision_; + match_tolerance += options_.exact_match_precision; // Make ignored members explicit if (!plan_request.path_constraints.joint_constraints.empty() || diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp index 7e05c357c9..8d2a692053 100644 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp @@ -29,15 +29,15 @@ const std::string ROBOT_FRAME = "world"; // UTILS ======================================================================= // Utility function to emit a pass or fail statement. -void check_and_emit(bool predicate, const std::string& prefix, const std::string& label) +void check_and_emit(bool predicate, const std::string& test_case, const std::string& label) { if (predicate) { - std::cout << "[PASS] " << prefix << ": " << label << std::endl; + std::cout << "[PASS] " << test_case << ": " << label << std::endl; } else { - std::cout << "[FAIL] " << prefix << ": " << label << std::endl; + std::cout << "[FAIL] " << test_case << ": " << label << std::endl; } } @@ -88,6 +88,24 @@ std::vector get_dummy_waypoints() return out; } +void test_getters_and_setters(std::shared_ptr cache) +{ + std::string test_case = "getters_and_setters"; + + check_and_emit(cache->getDbPath() == ":memory:", test_case, "getDbPath"); + check_and_emit(cache->getDbPort() == 0, test_case, "getDbPort"); + + check_and_emit(cache->getExactMatchPrecision() == 10, test_case, "getExactMatchPrecision"); + cache->setExactMatchPrecision(1); + check_and_emit(cache->getExactMatchPrecision() == 1, test_case, "getExactMatchPrecisionAfterSet"); + + check_and_emit(cache->getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() == 10, test_case, + "getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse"); + cache->setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse(1); + check_and_emit(cache->getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() == 1, test_case, + "getNumAdditionalTrajectoriesToPreserveWhenDeletingWorseAfterSet"); +} + void test_motion_trajectories(std::shared_ptr move_group, std::shared_ptr cache) { // Setup ===================================================================== @@ -184,378 +202,378 @@ void test_motion_trajectories(std::shared_ptr move_group, st // Test Utils - std::string prefix; + std::string test_case; // Checks ==================================================================== // Initially empty - prefix = "test_motion_trajectories.empty"; + test_case = "test_motion_trajectories.empty"; - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 0, prefix, "Trajectory cache initially empty"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 0, test_case, "Trajectory cache initially empty"); - check_and_emit(cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 999, 999).empty(), prefix, + check_and_emit(cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 999, 999).empty(), test_case, "Fetch all trajectories on empty cache returns empty"); - check_and_emit(cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 999, 999) == nullptr, prefix, + check_and_emit(cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 999, 999) == nullptr, test_case, "Fetch best trajectory on empty cache returns nullptr"); // Put trajectory empty frame // // Trajectory must have frame in joint trajectory, expect put fail - prefix = "test_motion_trajectories.putTrajectory_empty_frame"; + test_case = "test_motion_trajectories.putTrajectory_empty_frame"; double execution_time = 999; double planning_time = 999; check_and_emit(!cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, empty_frame_traj, execution_time, planning_time, false), - prefix, "Put empty frame trajectory, no delete_worse_trajectories, not ok"); + test_case, "Put empty frame trajectory, no delete_worse_trajectories, not ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 0, prefix, "No trajectories in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 0, test_case, "No trajectories in cache"); // Put trajectory req empty frame // // Trajectory request having no frame in workspace should default to robot frame - prefix = "test_motion_trajectories.putTrajectory_req_empty_frame"; + test_case = "test_motion_trajectories.putTrajectory_req_empty_frame"; execution_time = 1000; planning_time = 1000; check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, empty_frame_plan_req, traj, execution_time, planning_time, false), - prefix, "Put empty frame req trajectory, no delete_worse_trajectories, ok"); + test_case, "Put empty frame req trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); // Put second, no delete_worse_trajectories - prefix = "test_motion_trajectories.put_second"; + test_case = "test_motion_trajectories.put_second"; execution_time = 999; planning_time = 999; check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, traj, execution_time, planning_time, false), - prefix, "Put second valid trajectory, no delete_worse_trajectories, ok"); + test_case, "Put second valid trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); // Fetch matching, no tolerance // // Exact key match should have cache hit - prefix = "test_motion_trajectories.put_second.fetch_matching_no_tolerance"; + test_case = "test_motion_trajectories.put_second.fetch_matching_no_tolerance"; auto fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0); auto fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); // Fetch with is_diff // // is_diff key should be equivalent to exact match if robot state did not // change, hence should have cache hit - prefix = "test_motion_trajectories.put_second.fetch_is_diff_no_tolerance"; + test_case = "test_motion_trajectories.put_second.fetch_is_diff_no_tolerance"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, is_diff_plan_req, 0, 0); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, is_diff_plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); // Fetch non-matching, out of tolerance // // Non-matching key should not have cache hit - prefix = "test_motion_trajectories.put_second.fetch_non_matching_out_of_tolerance"; + test_case = "test_motion_trajectories.put_second.fetch_non_matching_out_of_tolerance"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 0); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch non-matching, only start in tolerance (but not goal) // // Non-matching key should not have cache hit - prefix = "test_motion_trajectories.put_second.fetch_non_matching_only_start_in_tolerance"; + test_case = "test_motion_trajectories.put_second.fetch_non_matching_only_start_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 999, 0); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 999, 0); - check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch non-matching, only goal in tolerance (but not start) // // Non-matching key should not have cache hit - prefix = "test_motion_trajectories.put_second.fetch_non_matching_only_goal_in_tolerance"; + test_case = "test_motion_trajectories.put_second.fetch_non_matching_only_goal_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 999); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 999); - check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch non-matching, in tolerance // // Close key within tolerance limit should have cache hit - prefix = "test_motion_trajectories.put_second.fetch_non_matching_in_tolerance"; + test_case = "test_motion_trajectories.put_second.fetch_non_matching_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0.1, 0.1); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0.1, 0.1); - check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); // Fetch swapped // // Matches should be mostly invariant to constraint ordering - prefix = "test_motion_trajectories.put_second.fetch_swapped"; + test_case = "test_motion_trajectories.put_second.fetch_swapped"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, swapped_close_matching_plan_req, 0.1, 0.1); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, swapped_close_matching_plan_req, 0.1, 0.1); - check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); // Fetch with smaller workspace // // Matching trajectories with more restrictive workspace requirements should not // pull up trajectories cached for less restrictive workspace requirements - prefix = "test_motion_trajectories.put_second.fetch_smaller_workspace"; + test_case = "test_motion_trajectories.put_second.fetch_smaller_workspace"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, smaller_workspace_plan_req, 999, 999); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, smaller_workspace_plan_req, 999, 999); - check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch with larger workspace // // Matching trajectories with less restrictive workspace requirements should pull up // trajectories cached for more restrictive workspace requirements - prefix = "test_motion_trajectories.put_second.fetch_larger_workspace"; + test_case = "test_motion_trajectories.put_second.fetch_larger_workspace"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, larger_workspace_plan_req, 999, 999); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, larger_workspace_plan_req, 999, 999); - check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); check_and_emit(fetched_traj->lookupDouble("workspace_parameters.max_corner.x") <= larger_workspace_plan_req.workspace_parameters.max_corner.x, - prefix, "Fetched trajectory has more restrictive workspace max_corner"); + test_case, "Fetched trajectory has more restrictive workspace max_corner"); check_and_emit(fetched_traj->lookupDouble("workspace_parameters.max_corner.x") >= larger_workspace_plan_req.workspace_parameters.min_corner.x, - prefix, "Fetched trajectory has more restrictive workspace min_corner"); + test_case, "Fetched trajectory has more restrictive workspace min_corner"); // Put worse, no delete_worse_trajectories // // Worse trajectories should not be inserted - prefix = "test_motion_trajectories.put_worse"; + test_case = "test_motion_trajectories.put_worse"; double worse_execution_time = execution_time + 100; check_and_emit(!cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, traj, worse_execution_time, planning_time, false), - prefix, "Put worse trajectory, no delete_worse_trajectories, not ok"); + test_case, "Put worse trajectory, no delete_worse_trajectories, not ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); // Put better, no delete_worse_trajectories // // Better trajectories should be inserted - prefix = "test_motion_trajectories.put_better"; + test_case = "test_motion_trajectories.put_better"; double better_execution_time = execution_time - 100; check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, traj, better_execution_time, planning_time, false), - prefix, "Put better trajectory, no delete_worse_trajectories, ok"); + test_case, "Put better trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 3, prefix, "Three trajectories in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 3, test_case, "Three trajectories in cache"); // Fetch sorted // // With multiple trajectories in cache, fetches should be sorted accordingly - prefix = "test_motion_trajectories.put_better.fetch_sorted"; + test_case = "test_motion_trajectories.put_better.fetch_sorted"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 3, prefix, "Fetch all returns three"); - check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + check_and_emit(fetched_trajectories.size() == 3, test_case, "Fetch all returns three"); + check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, prefix, + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); check_and_emit(fetched_trajectories.at(0)->lookupDouble("execution_time_s") == better_execution_time && fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time, - prefix, "Fetched trajectories are sorted correctly"); + test_case, "Fetched trajectories are sorted correctly"); // Put better, delete_worse_trajectories // // Better, different, trajectories should be inserted - prefix = "test_motion_trajectories.put_better_delete_worse_trajectories"; + test_case = "test_motion_trajectories.put_better_delete_worse_trajectories"; double even_better_execution_time = better_execution_time - 100; check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, different_traj, even_better_execution_time, planning_time, true), - prefix, "Put better trajectory, delete_worse_trajectories, ok"); + test_case, "Put better trajectory, delete_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); // Fetch better plan - prefix = "test_motion_trajectories.put_better_delete_worse_trajectories.fetch"; + test_case = "test_motion_trajectories.put_better_delete_worse_trajectories.fetch"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); - check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + check_and_emit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == different_traj, prefix, "Fetched trajectory matches original"); + check_and_emit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, prefix, + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, test_case, "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); // Put different req, delete_worse_trajectories // // Unrelated trajectory requests should live alongside pre-existing plans - prefix = "test_motion_trajectories.put_different_req"; + test_case = "test_motion_trajectories.put_different_req"; check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, different_plan_req, different_traj, better_execution_time, planning_time, true), - prefix, "Put different trajectory req, delete_worse_trajectories, ok"); + test_case, "Put different trajectory req, delete_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); // Fetch with different trajectory req // // With multiple trajectories in cache, fetches should be sorted accordingly - prefix = "test_motion_trajectories.put_different_req.fetch"; + test_case = "test_motion_trajectories.put_different_req.fetch"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, different_plan_req, 0, 0); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, different_plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); - check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + check_and_emit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == different_traj, prefix, "Fetched trajectory matches original"); + check_and_emit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, prefix, + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); // Fetch different robot // // Since we didn't populate anything, we should expect empty - prefix = "test_motion_trajectories.different_robot.empty"; + test_case = "test_motion_trajectories.different_robot.empty"; std::string different_robot_name = "different_robot"; - check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 0, prefix, "No trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 0, test_case, "No trajectories in cache"); // Put first for different robot, delete_worse_trajectories // // A different robot's cache should not interact with the original cache - prefix = "test_motion_trajectories.different_robot.put_first"; + test_case = "test_motion_trajectories.different_robot.put_first"; check_and_emit(cache->putTrajectory(*move_group, different_robot_name, different_plan_req, different_traj, better_execution_time, planning_time, true), - prefix, "Put different trajectory req, delete_worse_trajectories, ok"); + test_case, "Put different trajectory req, delete_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(different_robot_name) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countTrajectories(different_robot_name) == 1, test_case, "One trajectory in cache"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in original robot's cache"); + check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in original robot's cache"); fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, different_plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all on original still returns one"); + check_and_emit(fetched_trajectories.size() == 1, test_case, "Fetch all on original still returns one"); } void test_cartesian_trajectories(std::shared_ptr move_group, std::shared_ptr cache) { - std::string prefix; + std::string test_case; /// First, test if construction even works... // Construct get cartesian trajectory request - prefix = "test_cartesian_trajectories.constructGetCartesianPathRequest"; + test_case = "test_cartesian_trajectories.constructGetCartesianPathRequest"; int test_step = 1; int test_jump = 2; @@ -567,7 +585,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, static_cast(cartesian_plan_req_under_test.max_step) == test_step && static_cast(cartesian_plan_req_under_test.jump_threshold) == test_jump && !cartesian_plan_req_under_test.avoid_collisions, - prefix, "Ok"); + test_case, "Ok"); // Setup ===================================================================== // All variants are modified copies of `cartesian_plan_req`. @@ -634,60 +652,60 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // Checks ==================================================================== // Initially empty - prefix = "test_cartesian_trajectories.empty"; + test_case = "test_cartesian_trajectories.empty"; - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, prefix, "Trajectory cache initially empty"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, test_case, "Trajectory cache initially empty"); check_and_emit( cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999).empty(), - prefix, "Fetch all trajectories on empty cache returns empty"); + test_case, "Fetch all trajectories on empty cache returns empty"); check_and_emit(cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999) == nullptr, - prefix, "Fetch best trajectory on empty cache returns nullptr"); + test_case, "Fetch best trajectory on empty cache returns nullptr"); // Put trajectory empty frame // // Trajectory must have frame in joint trajectory, expect put fail - prefix = "test_cartesian_trajectories.putTrajectory_empty_frame"; + test_case = "test_cartesian_trajectories.putTrajectory_empty_frame"; double execution_time = 999; double planning_time = 999; double fraction = 0.5; check_and_emit(!cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, empty_frame_traj, execution_time, planning_time, fraction, false), - prefix, "Put empty frame trajectory, no delete_worse_trajectories, not ok"); + test_case, "Put empty frame trajectory, no delete_worse_trajectories, not ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, prefix, "No trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, test_case, "No trajectories in cache"); // Put trajectory req empty frame // // Trajectory request having no frame in workspace should default to robot frame - prefix = "test_cartesian_trajectories.putTrajectory_req_empty_frame"; + test_case = "test_cartesian_trajectories.putTrajectory_req_empty_frame"; execution_time = 1000; planning_time = 1000; check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, empty_frame_cartesian_plan_req, traj, execution_time, planning_time, fraction, false), - prefix, "Put empty frame req trajectory, no delete_worse_trajectories, ok"); + test_case, "Put empty frame req trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); // Put second, no delete_worse_trajectories - prefix = "test_cartesian_trajectories.put_second"; + test_case = "test_cartesian_trajectories.put_second"; execution_time = 999; planning_time = 999; check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, execution_time, planning_time, fraction, false), - prefix, "Put second valid trajectory, no delete_worse_trajectories, ok"); + test_case, "Put second valid trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); // Fetch matching, no tolerance // // Exact key match should have cache hit - prefix = "test_cartesian_trajectories.put_second.fetch_matching_no_tolerance"; + test_case = "test_cartesian_trajectories.put_second.fetch_matching_no_tolerance"; auto fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); @@ -695,26 +713,27 @@ void test_cartesian_trajectories(std::shared_ptr move_group, auto fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); - check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, + "Fetched trajectory has correct fraction"); // Fetch with is_diff // // is_diff key should be equivalent to exact match if robot state did not // change, hence should have cache hit - prefix = "test_cartesian_trajectories.put_second.fetch_is_diff_no_tolerance"; + test_case = "test_cartesian_trajectories.put_second.fetch_is_diff_no_tolerance"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, is_diff_cartesian_plan_req, fraction, 0, 0); @@ -722,25 +741,26 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, is_diff_cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); - check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, + "Fetched trajectory has correct fraction"); // Fetch non-matching, out of tolerance // // Non-matching key should not have cache hit - prefix = "test_cartesian_trajectories.put_second.fetch_non_matching_out_of_tolerance"; + test_case = "test_cartesian_trajectories.put_second.fetch_non_matching_out_of_tolerance"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 0); @@ -748,13 +768,13 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch non-matching, only start in tolerance (but not goal) // // Non-matching key should not have cache hit - prefix = "test_motion_trajectories.put_second.fetch_non_matching_only_start_in_tolerance"; + test_case = "test_motion_trajectories.put_second.fetch_non_matching_only_start_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 999, 0); @@ -762,13 +782,13 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 999, 0); - check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch non-matching, only goal in tolerance (but not start) // // Non-matching key should not have cache hit - prefix = "test_motion_trajectories.put_second.fetch_non_matching_only_goal_in_tolerance"; + test_case = "test_motion_trajectories.put_second.fetch_non_matching_only_goal_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 999); @@ -776,13 +796,13 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 999); - check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch non-matching, in tolerance // // Close key within tolerance limit should have cache hit - prefix = "test_cartesian_trajectories.put_second.fetch_non_matching_in_tolerance"; + test_case = "test_cartesian_trajectories.put_second.fetch_non_matching_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0.1, 0.1); @@ -790,89 +810,91 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0.1, 0.1); - check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); - check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, + "Fetched trajectory has correct fraction"); // Fetch with higher fraction // // Matching trajectories with more restrictive fraction requirements should not // pull up trajectories cached for less restrictive fraction requirements - prefix = "test_cartesian_trajectories.put_second.fetch_higher_fraction"; + test_case = "test_cartesian_trajectories.put_second.fetch_higher_fraction"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 1, 999, 999); fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 1, 999, 999); - check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch with lower fraction // // Matching trajectories with less restrictive fraction requirements should pull up // trajectories cached for more restrictive fraction requirements - prefix = "test_cartesian_trajectories.put_second.fetch_lower_fraction"; + test_case = "test_cartesian_trajectories.put_second.fetch_lower_fraction"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999); fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999); - check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); - check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, + "Fetched trajectory has correct fraction"); // Put worse, no delete_worse_trajectories // // Worse trajectories should not be inserted - prefix = "test_cartesian_trajectories.put_worse"; + test_case = "test_cartesian_trajectories.put_worse"; double worse_execution_time = execution_time + 100; check_and_emit(!cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, worse_execution_time, planning_time, fraction, false), - prefix, "Put worse trajectory, no delete_worse_trajectories, not ok"); + test_case, "Put worse trajectory, no delete_worse_trajectories, not ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); // Put better, no delete_worse_trajectories // // Better trajectories should be inserted - prefix = "test_cartesian_trajectories.put_better"; + test_case = "test_cartesian_trajectories.put_better"; double better_execution_time = execution_time - 100; check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, better_execution_time, planning_time, fraction, false), - prefix, "Put better trajectory, no delete_worse_trajectories, ok"); + test_case, "Put better trajectory, no delete_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 3, prefix, "Three trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 3, test_case, "Three trajectories in cache"); // Fetch sorted // // With multiple trajectories in cache, fetches should be sorted accordingly - prefix = "test_cartesian_trajectories.put_better.fetch_sorted"; + test_case = "test_cartesian_trajectories.put_better.fetch_sorted"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); @@ -880,39 +902,40 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 3, prefix, "Fetch all returns three"); - check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + check_and_emit(fetched_trajectories.size() == 3, test_case, "Fetch all returns three"); + check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, prefix, + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); - check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, + "Fetched trajectory has correct fraction"); check_and_emit(fetched_trajectories.at(0)->lookupDouble("execution_time_s") == better_execution_time && fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time, - prefix, "Fetched trajectories are sorted correctly"); + test_case, "Fetched trajectories are sorted correctly"); // Put better, delete_worse_trajectories // // Better, different, trajectories should be inserted - prefix = "test_cartesian_trajectories.put_better_delete_worse_trajectories"; + test_case = "test_cartesian_trajectories.put_better_delete_worse_trajectories"; double even_better_execution_time = better_execution_time - 100; check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, different_traj, even_better_execution_time, planning_time, fraction, true), - prefix, "Put better trajectory, delete_worse_trajectories, ok"); + test_case, "Put better trajectory, delete_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); // Fetch better plan - prefix = "test_cartesian_trajectories.put_better_delete_worse_trajectories.fetch"; + test_case = "test_cartesian_trajectories.put_better_delete_worse_trajectories.fetch"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); @@ -920,36 +943,37 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); - check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + check_and_emit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == different_traj, prefix, "Fetched trajectory matches original"); + check_and_emit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, prefix, + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, test_case, "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); - check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, + "Fetched trajectory has correct fraction"); // Put different req, delete_worse_trajectories // // Unrelated trajectory requests should live alongside pre-existing plans - prefix = "test_cartesian_trajectories.put_different_req"; + test_case = "test_cartesian_trajectories.put_different_req"; check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, different_cartesian_plan_req, different_traj, better_execution_time, planning_time, fraction, true), - prefix, "Put different trajectory req, delete_worse_trajectories, ok"); + test_case, "Put different trajectory req, delete_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, prefix, "Two trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); // Fetch with different trajectory req // // With multiple trajectories in cache, fetches should be sorted accordingly - prefix = "test_cartesian_trajectories.put_different_req.fetch"; + test_case = "test_cartesian_trajectories.put_different_req.fetch"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, different_cartesian_plan_req, fraction, 0, 0); @@ -957,46 +981,47 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, different_cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); - check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + check_and_emit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == different_traj, prefix, "Fetched trajectory matches original"); + check_and_emit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, prefix, + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); - check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, + "Fetched trajectory has correct fraction"); // Fetch different robot // // Since we didn't populate anything, we should expect empty - prefix = "test_cartesian_trajectories.different_robot.empty"; + test_case = "test_cartesian_trajectories.different_robot.empty"; std::string different_robot_name = "different_robot"; - check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 0, prefix, "No trajectories in cache"); + check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 0, test_case, "No trajectories in cache"); // Put first for different robot, delete_worse_trajectories // // A different robot's cache should not interact with the original cache - prefix = "test_cartesian_trajectories.different_robot.put_first"; + test_case = "test_cartesian_trajectories.different_robot.put_first"; check_and_emit(cache->putCartesianTrajectory(*move_group, different_robot_name, different_cartesian_plan_req, different_traj, better_execution_time, planning_time, fraction, true), - prefix, "Put different trajectory req, delete_worse_trajectories, ok"); + test_case, "Put different trajectory req, delete_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 1, prefix, "One trajectory in cache"); + check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 1, test_case, "One trajectory in cache"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, prefix, + check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in original robot's cache"); fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, different_cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all on original still returns one"); + check_and_emit(fetched_trajectories.size() == 1, test_case, "Fetch all on original still returns one"); } int main(int argc, char** argv) @@ -1028,16 +1053,30 @@ int main(int argc, char** argv) // This is necessary test_node->declare_parameter("warehouse_plugin", "warehouse_ros_sqlite::DatabaseConnection"); - // Test proper { - auto cache = std::make_shared(test_node); - check_and_emit(cache->init(":memory:", 0, 1e-4), "init", "Cache init"); + // Init. auto move_group = std::make_shared(move_group_node, "panda_arm"); - auto curr_state = move_group->getCurrentState(60); move_group->setStartState(*curr_state); + auto cache = std::make_shared(test_node); + + TrajectoryCache::Options options; + options.db_path = ":memory:"; + options.db_port = 0; + options.exact_match_precision = 10; + options.num_additional_trajectories_to_preserve_when_deleting_worse = 10; + + // Tests. + + check_and_emit(cache->init(options), "init", "Cache init"); + + test_getters_and_setters(cache); + + cache->setExactMatchPrecision(1e-4); + cache->setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse(0); + test_motion_trajectories(move_group, cache); test_cartesian_trajectories(move_group, cache); } diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.py b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py index 4a4de5400a..8396872ccf 100755 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.py +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py @@ -136,7 +136,7 @@ def test_all_tests_pass(trajectory_cache_test_runner_node, launch_context): assert process_tools.wait_for_output_sync( launch_context, trajectory_cache_test_runner_node, - lambda x: x.count("[PASS]") == 163, # All test cases passed. + lambda x: x.count("[PASS]") == 169, # All test cases passed. timeout=30, ) From cefcecac223bec9413bc76dcd109491acd98c45f Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 30 Jul 2024 23:24:44 -0700 Subject: [PATCH 20/26] Edit docs and rename puts to inserts Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/README.md | 4 +- .../trajectory_cache/trajectory_cache.hpp | 327 +++++++++--------- .../trajectory_cache/src/trajectory_cache.cpp | 47 +-- .../test/test_trajectory_cache.cpp | 146 ++++---- .../test/test_trajectory_cache.py | 4 +- 5 files changed, 272 insertions(+), 256 deletions(-) diff --git a/moveit_ros/trajectory_cache/README.md b/moveit_ros/trajectory_cache/README.md index e208319dea..c0dc20de7d 100644 --- a/moveit_ros/trajectory_cache/README.md +++ b/moveit_ros/trajectory_cache/README.md @@ -66,10 +66,10 @@ if (fetched_trajectory) else { // Plan... And put it for posterity! - traj_cache->putTrajectory( + traj_cache->insertTrajectory( *interface, robot_name, std::move(plan_req_msg), std::move(res->result.trajectory), rclcpp::Duration(res->result.trajectory.joint_trajectory.points.back().time_from_start).seconds(), - res->result.planning_time, /*delete_worse_trajectories=*/true); + res->result.planning_time, /*prune_worse_trajectories=*/true); } ``` diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index 98bfbfa142..db28f7414b 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Johnson & Johnson +// Copyright 2024 Intrinsic Innovation LLC. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,6 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +/** @file + * @brief Fuzzy-Matching Trajectory Cache. + * @author methylDragon + */ + #pragma once #include @@ -40,9 +45,9 @@ namespace moveit_ros namespace trajectory_cache { -/** \class TrajectoryCache trajectory_cache.hpp moveit/trajectory_cache/trajectory_cache.hpp +/** @class TrajectoryCache trajectory_cache.hpp moveit/trajectory_cache/trajectory_cache.hpp * - * \brief Trajectory Cache manager for MoveIt. + * @brief Trajectory Cache manager for MoveIt. * * This manager facilitates cache management for MoveIt 2's `MoveGroupInterface` * by using `warehouse_ros` to manage a database of executed trajectories, keyed @@ -113,9 +118,9 @@ class TrajectoryCache { public: /** - * \brief Construct a TrajectoryCache. + * @brief Constructs a TrajectoryCache. * - * \param[in] node. An rclcpp::Node::SharedPtr, which will be used to lookup warehouse_ros parameters, log, and listen + * @param[in] node. An rclcpp::Node::SharedPtr, which will be used to lookup warehouse_ros parameters, log, and listen * for TF. * * TODO: methylDragon - @@ -125,16 +130,16 @@ class TrajectoryCache explicit TrajectoryCache(const rclcpp::Node::SharedPtr& node); /** - * \brief Options struct for TrajectoryCache. + * @brief Options struct for TrajectoryCache. * - * \property db_path. The database path. - * \property db_port. The database port. - * \property exact_match_precision. Tolerance for float precision comparison for what counts as an exact match. + * @property db_path. The database path. + * @property db_port. The database port. + * @property exact_match_precision. Tolerance for float precision comparison for what counts as an exact match. * An exact match is when: * (candidate >= value - (exact_match_precision / 2) * && candidate <= value + (exact_match_precision / 2)) - * \property num_additional_trajectories_to_preserve_when_deleting_worse. The number of additional cached trajectories - * to preserve when `delete_worse_trajectories` is true. It is useful to keep more than one matching trajectory to + * @property num_additional_trajectories_to_preserve_when_deleting_worse. The number of additional cached trajectories + * to preserve when `prune_worse_trajectories` is true. It is useful to keep more than one matching trajectory to * have alternative trajectories to handle obstacles. */ struct Options @@ -147,78 +152,78 @@ class TrajectoryCache }; /** - * \brief Initialize the TrajectoryCache. + * @brief Initializes the TrajectoryCache. * * This sets up the database connection, and sets any configuration parameters. * You must call this before calling any other method of the trajectory cache. * - * \param[in] options. An instance of TrajectoryCache::Options to initialize the cache with. - * \see TrajectoryCache::Options - * \returns true if the database was successfully connected to. - * \throws When options.num_additional_trajectories_to_preserve_when_deleting_worse is less than 1. + * @param[in] options. An instance of TrajectoryCache::Options to initialize the cache with. + * @see TrajectoryCache::Options + * @returns true if the database was successfully connected to. + * @throws When options.num_additional_trajectories_to_preserve_when_deleting_worse is less than 1. * */ bool init(const Options& options); /** - * \brief Count the number of non-cartesian trajectories for a particular cache namespace. + * @brief Count the number of non-cartesian trajectories for a particular cache namespace. * - * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. - * \returns The number of non-cartesian trajectories for the cache namespace. + * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * @returns The number of non-cartesian trajectories for the cache namespace. */ unsigned countTrajectories(const std::string& cache_namespace); /** - * \brief Count the number of cartesian trajectories for a particular cache namespace. + * @brief Count the number of cartesian trajectories for a particular cache namespace. * - * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. - * \returns The number of cartesian trajectories for the cache namespace. + * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * @returns The number of cartesian trajectories for the cache namespace. */ unsigned countCartesianTrajectories(const std::string& cache_namespace); /** - * \name Getters and setters. + * @name Getters and setters. */ /**@{*/ - /// \brief Gets the database path. + /// @brief Gets the database path. std::string getDbPath(); - /// \brief Gets the database port. + /// @brief Gets the database port. uint32_t getDbPort(); - /// \brief Getss the exact match precision. + /// @brief Gets the exact match precision. double getExactMatchPrecision(); - /// \brief Sets the exact match precision. + /// @brief Sets the exact match precision. void setExactMatchPrecision(double exact_match_precision); - /// \brief Get the number of trajectories to preserve when deleting worse trajectories. + /// @brief Get the number of trajectories to preserve when deleting worse trajectories. size_t getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse(); - /// \brief Set the number of additional trajectories to preserve when deleting worse trajectories. + /// @brief Set the number of additional trajectories to preserve when deleting worse trajectories. void setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse( size_t num_additional_trajectories_to_preserve_when_deleting_worse); /**@}*/ /** - * \name Motion plan trajectory caching + * @name Motion plan trajectory caching */ /**@{*/ /** - * \brief Fetch all plans that fit within the requested tolerances for start and goal conditions, returning them as a - * vector, sorted by some cache column. - * - * \param[in] move_group. The manipulator move group, used to get its state. - * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. - * \param[in] plan_request. The motion plan request to key the cache with. - * \param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. - * \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. - * \param[in] metadata_only. If true, returns only the cache entry metadata. - * \param[in] sort_by. The cache column to sort by, defaults to execution time. - * \param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order. - * \returns A vector of cache hits, sorted by the `sort_by` param. + * @brief Fetches all plans that fit within the requested tolerances for start and goal conditions, returning them as + * a vector, sorted by some cache column. + * + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * @param[in] plan_request. The motion plan request to key the cache with. + * @param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. + * @param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. + * @param[in] metadata_only. If true, returns only the cache entry metadata. + * @param[in] sort_by. The cache column to sort by, defaults to execution time. + * @param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order. + * @returns A vector of cache hits, sorted by the `sort_by` param. */ std::vector::ConstPtr> fetchAllMatchingTrajectories(const moveit::planning_interface::MoveGroupInterface& move_group, @@ -228,18 +233,18 @@ class TrajectoryCache const std::string& sort_by = "execution_time_s", bool ascending = true) const; /** - * \brief Fetch the best trajectory that fits within the requested tolerances for start and goal conditions, by some + * @brief Fetches the best trajectory that fits within the requested tolerances for start and goal conditions, by some * cache column. * - * \param[in] move_group. The manipulator move group, used to get its state. - * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. - * \param[in] plan_request. The motion plan request to key the cache with. - * \param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. - * \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. - * \param[in] metadata_only. If true, returns only the cache entry metadata. - * \param[in] sort_by. The cache column to sort by, defaults to execution time. - * \param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order. - * \returns The best cache hit, with respect to the `sort_by` param. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * @param[in] plan_request. The motion plan request to key the cache with. + * @param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. + * @param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. + * @param[in] metadata_only. If true, returns only the cache entry metadata. + * @param[in] sort_by. The cache column to sort by, defaults to execution time. + * @param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order. + * @returns The best cache hit, with respect to the `sort_by` param. */ warehouse_ros::MessageWithMetadata::ConstPtr fetchBestMatchingTrajectory( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, @@ -247,7 +252,7 @@ class TrajectoryCache bool metadata_only = false, const std::string& sort_by = "execution_time_s", bool ascending = true) const; /** - * \brief Put a trajectory into the database if it is the best matching trajectory seen so far. + * @brief Inserts a trajectory into the database if it is the best matching trajectory seen so far. * * Trajectories are matched based off their start and goal states. * And are considered "better" if they are higher priority in the sorting order specified by `sort_by` than another @@ -255,43 +260,43 @@ class TrajectoryCache * * A trajectory is "exactly matching" if its start and goal are close enough to another trajectory. * The tolerance for this depends on the `exact_match_precision` arg passed in init(). - * \see init() + * @see init() * * Optionally deletes all worse trajectories by default to prune the cache. * - * \param[in] move_group. The manipulator move group, used to get its state. - * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. - * \param[in] plan_request. The motion plan request to key the cache with. - * \param[in] trajectory. The trajectory to put. - * \param[in] execution_time_s. The execution time of the trajectory, in seconds. - * \param[in] planning_time_s. How long the trajectory took to plan, in seconds. - * \param[in] delete_worse_trajectories. If true, will prune the cache by deleting all cache entries that match the + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * @param[in] plan_request. The motion plan request to key the cache with. + * @param[in] trajectory. The trajectory to put. + * @param[in] execution_time_s. The execution time of the trajectory, in seconds. + * @param[in] planning_time_s. How long the trajectory took to plan, in seconds. + * @param[in] prune_worse_trajectories. If true, will prune the cache by deleting all cache entries that match the * `plan_request` exactly, if they are worse than the `trajectory`, even if it was not put. - * \returns true if the trajectory was the best seen yet and hence put into the cache. + * @returns true if the trajectory was the best seen yet and hence put into the cache. */ - bool putTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, - const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, - double planning_time_s, bool delete_worse_trajectories = true); + bool insertTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, + const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, + double planning_time_s, bool prune_worse_trajectories = true); /**@}*/ /** - * \name Cartesian trajectory caching + * @name Cartesian trajectory caching */ /**@{*/ /** - * \brief Construct a GetCartesianPath request. + * @brief Constructs a GetCartesianPath request. * * This mimics the move group computeCartesianPath signature (without path constraints). * - * \param[in] move_group. The manipulator move group, used to get its state, frames, and link. - * \param[in] waypoints. The cartesian waypoints to request the path for. - * \param[in] max_step. The value to populate into the `GetCartesianPath` request's max_step field. - * \param[in] jump_threshold. The value to populate into the `GetCartesianPath` request's jump_threshold field. - * \param[in] avoid_collisions. The value to populate into the `GetCartesianPath` request's avoid_collisions field. - * \returns + * @param[in] move_group. The manipulator move group, used to get its state, frames, and link. + * @param[in] waypoints. The cartesian waypoints to request the path for. + * @param[in] max_step. The value to populate into the `GetCartesianPath` request's max_step field. + * @param[in] jump_threshold. The value to populate into the `GetCartesianPath` request's jump_threshold field. + * @param[in] avoid_collisions. The value to populate into the `GetCartesianPath` request's avoid_collisions field. + * @returns */ moveit_msgs::srv::GetCartesianPath::Request constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group, @@ -299,19 +304,19 @@ class TrajectoryCache double jump_threshold, bool avoid_collisions = true); /** - * \brief Fetch all cartesian trajectories that fit within the requested tolerances for start and goal conditions, + * @brief Fetches all cartesian trajectories that fit within the requested tolerances for start and goal conditions, * returning them as a vector, sorted by some cache column. * - * \param[in] move_group. The manipulator move group, used to get its state. - * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. - * \param[in] plan_request. The cartesian plan request to key the cache with. - * \param[in] min_fraction. The minimum fraction required for a cache hit. - * \param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. - * \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. - * \param[in] metadata_only. If true, returns only the cache entry metadata. - * \param[in] sort_by. The cache column to sort by, defaults to execution time. - * \param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order. - * \returns A vector of cache hits, sorted by the `sort_by` param. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * @param[in] plan_request. The cartesian plan request to key the cache with. + * @param[in] min_fraction. The minimum fraction required for a cache hit. + * @param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. + * @param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. + * @param[in] metadata_only. If true, returns only the cache entry metadata. + * @param[in] sort_by. The cache column to sort by, defaults to execution time. + * @param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order. + * @returns A vector of cache hits, sorted by the `sort_by` param. */ std::vector::ConstPtr> fetchAllMatchingCartesianTrajectories(const moveit::planning_interface::MoveGroupInterface& move_group, @@ -322,19 +327,19 @@ class TrajectoryCache bool ascending = true) const; /** - * \brief Fetch the best cartesian trajectory that fits within the requested tolerances for start and goal conditions, - * by some cache column. - * - * \param[in] move_group. The manipulator move group, used to get its state. - * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. - * \param[in] plan_request. The cartesian plan request to key the cache with. - * \param[in] min_fraction. The minimum fraction required for a cache hit. - * \param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. - * \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. - * \param[in] metadata_only. If true, returns only the cache entry metadata. - * \param[in] sort_by. The cache column to sort by, defaults to execution time. - * \param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order. - * \returns The best cache hit, with respect to the `sort_by` param. + * @brief Fetches the best cartesian trajectory that fits within the requested tolerances for start and goal + * conditions, by some cache column. + * + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * @param[in] plan_request. The cartesian plan request to key the cache with. + * @param[in] min_fraction. The minimum fraction required for a cache hit. + * @param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. + * @param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. + * @param[in] metadata_only. If true, returns only the cache entry metadata. + * @param[in] sort_by. The cache column to sort by, defaults to execution time. + * @param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order. + * @returns The best cache hit, with respect to the `sort_by` param. */ warehouse_ros::MessageWithMetadata::ConstPtr fetchBestMatchingCartesianTrajectory( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, @@ -343,7 +348,7 @@ class TrajectoryCache bool ascending = true) const; /** - * \brief Put a cartesian trajectory into the database if it is the best matching cartesian trajectory seen so far. + * @brief Inserts a cartesian trajectory into the database if it is the best matching cartesian trajectory seen so far. * * Cartesian trajectories are matched based off their start and goal states. * And are considered "better" if they are higher priority in the sorting order specified by `sort_by` than another @@ -351,46 +356,46 @@ class TrajectoryCache * * A trajectory is "exactly matching" if its start and goal (and fraction) are close enough to another trajectory. * The tolerance for this depends on the `exact_match_precision` arg passed in init(). - * \see init() + * @see init() * * Optionally deletes all worse cartesian trajectories by default to prune the cache. * - * \param[in] move_group. The manipulator move group, used to get its state. - * \param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. - * \param[in] plan_request. The cartesian plan request to key the cache with. - * \param[in] trajectory. The trajectory to put. - * \param[in] execution_time_s. The execution time of the trajectory, in seconds. - * \param[in] planning_time_s. How long the trajectory took to plan, in seconds. - * \param[in] fraction. The fraction of the path that was computed. - * \param[in] delete_worse_trajectories. If true, will prune the cache by deleting all cache entries that match the + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * @param[in] plan_request. The cartesian plan request to key the cache with. + * @param[in] trajectory. The trajectory to put. + * @param[in] execution_time_s. The execution time of the trajectory, in seconds. + * @param[in] planning_time_s. How long the trajectory took to plan, in seconds. + * @param[in] fraction. The fraction of the path that was computed. + * @param[in] prune_worse_trajectories. If true, will prune the cache by deleting all cache entries that match the * `plan_request` exactly, if they are worse than `trajectory`, even if it was not put. - * \returns true if the trajectory was the best seen yet and hence put into the cache. + * @returns true if the trajectory was the best seen yet and hence put into the cache. */ - bool putCartesianTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& cache_namespace, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, - const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, - double planning_time_s, double fraction, bool delete_worse_trajectories = true); + bool insertCartesianTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& cache_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, + double planning_time_s, double fraction, bool prune_worse_trajectories = true); /**@}*/ private: /** - * \name Motion plan trajectory query and metadata construction + * @name Motion plan trajectory query and metadata construction */ /**@{*/ /** - * \brief Extract relevant parameters from a motion plan request's start parameters to populate a cache db query, with - * some match tolerance. + * @brief Extracts relevant parameters from a motion plan request's start parameters to populate a cache db query, + * with some match tolerance. * * These parameters will be used to look-up relevant sections of a cache element's key. * - * \param[out] query. The query to add parameters to. - * \param[in] move_group. The manipulator move group, used to get its state. - * \param[in] plan_request. The motion plan request to key the cache with. - * \param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. - * \returns true if successfully added to. If false, the query might have been partially modified and should not be + * @param[out] query. The query to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The motion plan request to key the cache with. + * @param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. + * @returns true if successfully added to. If false, the query might have been partially modified and should not be * used. */ bool extractAndAppendTrajectoryStartToQuery(warehouse_ros::Query& query, @@ -399,16 +404,16 @@ class TrajectoryCache double match_tolerance) const; /** - * \brief Extract relevant parameters from a motion plan request's goal parameters to populate a cache db query, with + * @brief Extracts relevant parameters from a motion plan request's goal parameters to populate a cache db query, with * some match tolerance. * * These parameters will be used to look-up relevant sections of a cache element's key. * - * \param[out] query. The query to add parameters to. - * \param[in] move_group. The manipulator move group, used to get its state. - * \param[in] plan_request. The motion plan request to key the cache with. - * \param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. - * \returns true if successfully added to. If false, the query might have been partially modified and should not be + * @param[out] query. The query to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The motion plan request to key the cache with. + * @param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. + * @returns true if successfully added to. If false, the query might have been partially modified and should not be * used. */ bool extractAndAppendTrajectoryGoalToQuery(warehouse_ros::Query& query, @@ -417,15 +422,15 @@ class TrajectoryCache double match_tolerance) const; /** - * \brief Extract relevant parameters from a motion plan request's start parameters to populate a cache entry's + * @brief Extracts relevant parameters from a motion plan request's start parameters to populate a cache entry's * metadata. * * These parameters will be used key the cache element. * - * \param[out] metadata. The metadata to add parameters to. - * \param[in] move_group. The manipulator move group, used to get its state. - * \param[in] plan_request. The motion plan request to key the cache with. - * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be + * @param[out] metadata. The metadata to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The motion plan request to key the cache with. + * @returns true if successfully added to. If false, the metadata might have been partially modified and should not be * used. */ bool extractAndAppendTrajectoryStartToMetadata(warehouse_ros::Metadata& metadata, @@ -433,15 +438,15 @@ class TrajectoryCache const moveit_msgs::msg::MotionPlanRequest& plan_request) const; /** - * \brief Extract relevant parameters from a motion plan request's goal parameters to populate a cache entry's + * @brief Extracts relevant parameters from a motion plan request's goal parameters to populate a cache entry's * metadata. * * These parameters will be used key the cache element. * - * \param[out] metadata. The metadata to add parameters to. - * \param[in] move_group. The manipulator move group, used to get its state. - * \param[in] plan_request. The motion plan request to key the cache with. - * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be + * @param[out] metadata. The metadata to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The motion plan request to key the cache with. + * @returns true if successfully added to. If false, the metadata might have been partially modified and should not be * used. */ bool extractAndAppendTrajectoryGoalToMetadata(warehouse_ros::Metadata& metadata, @@ -451,21 +456,21 @@ class TrajectoryCache /**@}*/ /** - * \name Cartesian trajectory query and metadata construction + * @name Cartesian trajectory query and metadata construction */ /**@{*/ /** - * \brief Extract relevant parameters from a cartesian plan request's start parameters to populate a cache db query, + * @brief Extracts relevant parameters from a cartesian plan request's start parameters to populate a cache db query, * with some match tolerance. * * These parameters will be used to look-up relevant sections of a cache element's key. * - * \param[out] query. The query to add parameters to. - * \param[in] move_group. The manipulator move group, used to get its state. - * \param[in] plan_request. The cartesian plan request to key the cache with. - * \param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. - * \returns true if successfully added to. If false, the query might have been partially modified and should not be + * @param[out] query. The query to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The cartesian plan request to key the cache with. + * @param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. + * @returns true if successfully added to. If false, the query might have been partially modified and should not be * used. */ bool extractAndAppendCartesianTrajectoryStartToQuery(warehouse_ros::Query& query, @@ -474,16 +479,16 @@ class TrajectoryCache double match_tolerance) const; /** - * \brief Extract relevant parameters from a cartesian plan request's goal parameters to populate a cache db query, + * @brief Extracts relevant parameters from a cartesian plan request's goal parameters to populate a cache db query, * with some match tolerance. * * These parameters will be used to look-up relevant sections of a cache element's key. * - * \param[out] query. The query to add parameters to. - * \param[in] move_group. The manipulator move group, used to get its state. - * \param[in] plan_request. The cartesian plan request to key the cache with. - * \param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. - * \returns true if successfully added to. If false, the query might have been partially modified and should not be + * @param[out] query. The query to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The cartesian plan request to key the cache with. + * @param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. + * @returns true if successfully added to. If false, the query might have been partially modified and should not be * used. */ bool extractAndAppendCartesianTrajectoryGoalToQuery(warehouse_ros::Query& query, @@ -492,15 +497,15 @@ class TrajectoryCache double match_tolerance) const; /** - * \brief Extract relevant parameters from a cartesian plan request's goal parameters to populate a cache entry's + * @brief Extracts relevant parameters from a cartesian plan request's goal parameters to populate a cache entry's * metadata. * * These parameters will be used key the cache element. * - * \param[out] metadata. The metadata to add parameters to. - * \param[in] move_group. The manipulator move group, used to get its state. - * \param[in] plan_request. The cartesian plan request to key the cache with. - * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be + * @param[out] metadata. The metadata to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The cartesian plan request to key the cache with. + * @returns true if successfully added to. If false, the metadata might have been partially modified and should not be * used. */ bool extractAndAppendCartesianTrajectoryStartToMetadata( @@ -508,15 +513,15 @@ class TrajectoryCache const moveit_msgs::srv::GetCartesianPath::Request& plan_request) const; /** - * \brief Extract relevant parameters from a cartesian plan request's goal parameters to populate a cache entry's + * @brief Extracts relevant parameters from a cartesian plan request's goal parameters to populate a cache entry's * metadata. * * These parameters will be used key the cache element. * - * \param[out] metadata. The metadata to add parameters to. - * \param[in] move_group. The manipulator move group, used to get its state. - * \param[in] plan_request. The cartesian plan request to key the cache with. - * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be + * @param[out] metadata. The metadata to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The cartesian plan request to key the cache with. + * @returns true if successfully added to. If false, the metadata might have been partially modified and should not be * used. */ bool extractAndAppendCartesianTrajectoryGoalToMetadata( diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index ee358b8d23..21365cb9e7 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Johnson & Johnson +// Copyright 2024 Intrinsic Innovation LLC. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,17 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. +/** @file + * @brief Implementation of the Fuzzy-Matching Trajectory Cache. + * @author methylDragon + */ + #include #include -#include "moveit/robot_state/conversions.h" -#include "moveit/robot_state/robot_state.h" -#include "moveit/warehouse/moveit_message_storage.h" -#include "warehouse_ros/message_collection.h" +#include +#include +#include +#include -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/logging.hpp" -#include "moveit/trajectory_cache/trajectory_cache.hpp" +#include +#include +#include namespace moveit_ros { @@ -216,11 +221,11 @@ MessageWithMetadata::ConstPtr TrajectoryCache return coll.findOne(best_query, metadata_only); } -bool TrajectoryCache::putTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& cache_namespace, - const moveit_msgs::msg::MotionPlanRequest& plan_request, - const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, - double planning_time_s, bool delete_worse_trajectories) +bool TrajectoryCache::insertTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& cache_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, + double planning_time_s, bool prune_worse_trajectories) { std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); @@ -271,7 +276,7 @@ bool TrajectoryCache::putTrajectory(const moveit::planning_interface::MoveGroupI { best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); - if (delete_worse_trajectories) + if (prune_worse_trajectories) { size_t preserved_count = 0; for (auto& match : exact_matches) @@ -411,12 +416,12 @@ MessageWithMetadata::ConstPtr TrajectoryCache return coll.findOne(best_query, metadata_only); } -bool TrajectoryCache::putCartesianTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& cache_namespace, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, - const moveit_msgs::msg::RobotTrajectory& trajectory, - double execution_time_s, double planning_time_s, double fraction, - bool delete_worse_trajectories) +bool TrajectoryCache::insertCartesianTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& cache_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + const moveit_msgs::msg::RobotTrajectory& trajectory, + double execution_time_s, double planning_time_s, double fraction, + bool prune_worse_trajectories) { std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); @@ -462,7 +467,7 @@ bool TrajectoryCache::putCartesianTrajectory(const moveit::planning_interface::M { best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); - if (delete_worse_trajectories) + if (prune_worse_trajectories) { size_t preserved_count = 0; for (auto& match : exact_matches) diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp index 8d2a692053..d68ca82607 100644 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Johnson & Johnson +// Copyright 2024 Intrinsic Innovation LLC. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,11 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +/** @file + * @author methylDragon + */ + #include -#include "moveit/robot_state/conversions.h" -#include "moveit/robot_state/robot_state.h" -#include "moveit/trajectory_cache/trajectory_cache.hpp" +#include +#include +#include #include #include @@ -220,36 +224,36 @@ void test_motion_trajectories(std::shared_ptr move_group, st // Put trajectory empty frame // // Trajectory must have frame in joint trajectory, expect put fail - test_case = "test_motion_trajectories.putTrajectory_empty_frame"; + test_case = "test_motion_trajectories.insertTrajectory_empty_frame"; double execution_time = 999; double planning_time = 999; - check_and_emit(!cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, empty_frame_traj, execution_time, - planning_time, false), - test_case, "Put empty frame trajectory, no delete_worse_trajectories, not ok"); + check_and_emit(!cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, empty_frame_traj, execution_time, + planning_time, false), + test_case, "Put empty frame trajectory, no prune_worse_trajectories, not ok"); check_and_emit(cache->countTrajectories(ROBOT_NAME) == 0, test_case, "No trajectories in cache"); // Put trajectory req empty frame // // Trajectory request having no frame in workspace should default to robot frame - test_case = "test_motion_trajectories.putTrajectory_req_empty_frame"; + test_case = "test_motion_trajectories.insertTrajectory_req_empty_frame"; execution_time = 1000; planning_time = 1000; - check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, empty_frame_plan_req, traj, execution_time, - planning_time, false), - test_case, "Put empty frame req trajectory, no delete_worse_trajectories, ok"); + check_and_emit(cache->insertTrajectory(*move_group, ROBOT_NAME, empty_frame_plan_req, traj, execution_time, + planning_time, false), + test_case, "Put empty frame req trajectory, no prune_worse_trajectories, ok"); check_and_emit(cache->countTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); - // Put second, no delete_worse_trajectories + // Put second, no prune_worse_trajectories test_case = "test_motion_trajectories.put_second"; execution_time = 999; planning_time = 999; - check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, traj, execution_time, planning_time, false), - test_case, "Put second valid trajectory, no delete_worse_trajectories, ok"); + check_and_emit(cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, traj, execution_time, planning_time, false), + test_case, "Put second valid trajectory, no prune_worse_trajectories, ok"); check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); @@ -426,27 +430,27 @@ void test_motion_trajectories(std::shared_ptr move_group, st larger_workspace_plan_req.workspace_parameters.min_corner.x, test_case, "Fetched trajectory has more restrictive workspace min_corner"); - // Put worse, no delete_worse_trajectories + // Put worse, no prune_worse_trajectories // // Worse trajectories should not be inserted test_case = "test_motion_trajectories.put_worse"; double worse_execution_time = execution_time + 100; - check_and_emit(!cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, traj, worse_execution_time, planning_time, - false), - test_case, "Put worse trajectory, no delete_worse_trajectories, not ok"); + check_and_emit(!cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, traj, worse_execution_time, planning_time, + false), + test_case, "Put worse trajectory, no prune_worse_trajectories, not ok"); check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); - // Put better, no delete_worse_trajectories + // Put better, no prune_worse_trajectories // // Better trajectories should be inserted test_case = "test_motion_trajectories.put_better"; double better_execution_time = execution_time - 100; - check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, traj, better_execution_time, planning_time, - false), - test_case, "Put better trajectory, no delete_worse_trajectories, ok"); + check_and_emit(cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, traj, better_execution_time, planning_time, + false), + test_case, "Put better trajectory, no prune_worse_trajectories, ok"); check_and_emit(cache->countTrajectories(ROBOT_NAME) == 3, test_case, "Three trajectories in cache"); @@ -476,20 +480,20 @@ void test_motion_trajectories(std::shared_ptr move_group, st fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time, test_case, "Fetched trajectories are sorted correctly"); - // Put better, delete_worse_trajectories + // Put better, prune_worse_trajectories // // Better, different, trajectories should be inserted - test_case = "test_motion_trajectories.put_better_delete_worse_trajectories"; + test_case = "test_motion_trajectories.put_better_prune_worse_trajectories"; double even_better_execution_time = better_execution_time - 100; - check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, plan_req, different_traj, even_better_execution_time, - planning_time, true), - test_case, "Put better trajectory, delete_worse_trajectories, ok"); + check_and_emit(cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, different_traj, even_better_execution_time, + planning_time, true), + test_case, "Put better trajectory, prune_worse_trajectories, ok"); check_and_emit(cache->countTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); // Fetch better plan - test_case = "test_motion_trajectories.put_better_delete_worse_trajectories.fetch"; + test_case = "test_motion_trajectories.put_better_prune_worse_trajectories.fetch"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0); @@ -508,14 +512,14 @@ void test_motion_trajectories(std::shared_ptr move_group, st check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, "Fetched trajectory has correct planning time"); - // Put different req, delete_worse_trajectories + // Put different req, prune_worse_trajectories // // Unrelated trajectory requests should live alongside pre-existing plans test_case = "test_motion_trajectories.put_different_req"; - check_and_emit(cache->putTrajectory(*move_group, ROBOT_NAME, different_plan_req, different_traj, - better_execution_time, planning_time, true), - test_case, "Put different trajectory req, delete_worse_trajectories, ok"); + check_and_emit(cache->insertTrajectory(*move_group, ROBOT_NAME, different_plan_req, different_traj, + better_execution_time, planning_time, true), + test_case, "Put different trajectory req, prune_worse_trajectories, ok"); check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); @@ -549,13 +553,13 @@ void test_motion_trajectories(std::shared_ptr move_group, st check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 0, test_case, "No trajectories in cache"); - // Put first for different robot, delete_worse_trajectories + // Put first for different robot, prune_worse_trajectories // // A different robot's cache should not interact with the original cache test_case = "test_motion_trajectories.different_robot.put_first"; - check_and_emit(cache->putTrajectory(*move_group, different_robot_name, different_plan_req, different_traj, - better_execution_time, planning_time, true), - test_case, "Put different trajectory req, delete_worse_trajectories, ok"); + check_and_emit(cache->insertTrajectory(*move_group, different_robot_name, different_plan_req, different_traj, + better_execution_time, planning_time, true), + test_case, "Put different trajectory req, prune_worse_trajectories, ok"); check_and_emit(cache->countTrajectories(different_robot_name) == 1, test_case, "One trajectory in cache"); @@ -667,38 +671,38 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // Put trajectory empty frame // // Trajectory must have frame in joint trajectory, expect put fail - test_case = "test_cartesian_trajectories.putTrajectory_empty_frame"; + test_case = "test_cartesian_trajectories.insertTrajectory_empty_frame"; double execution_time = 999; double planning_time = 999; double fraction = 0.5; - check_and_emit(!cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, empty_frame_traj, - execution_time, planning_time, fraction, false), - test_case, "Put empty frame trajectory, no delete_worse_trajectories, not ok"); + check_and_emit(!cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, empty_frame_traj, + execution_time, planning_time, fraction, false), + test_case, "Put empty frame trajectory, no prune_worse_trajectories, not ok"); check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, test_case, "No trajectories in cache"); // Put trajectory req empty frame // // Trajectory request having no frame in workspace should default to robot frame - test_case = "test_cartesian_trajectories.putTrajectory_req_empty_frame"; + test_case = "test_cartesian_trajectories.insertTrajectory_req_empty_frame"; execution_time = 1000; planning_time = 1000; - check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, empty_frame_cartesian_plan_req, traj, - execution_time, planning_time, fraction, false), - test_case, "Put empty frame req trajectory, no delete_worse_trajectories, ok"); + check_and_emit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, empty_frame_cartesian_plan_req, traj, + execution_time, planning_time, fraction, false), + test_case, "Put empty frame req trajectory, no prune_worse_trajectories, ok"); check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); - // Put second, no delete_worse_trajectories + // Put second, no prune_worse_trajectories test_case = "test_cartesian_trajectories.put_second"; execution_time = 999; planning_time = 999; - check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, execution_time, - planning_time, fraction, false), - test_case, "Put second valid trajectory, no delete_worse_trajectories, ok"); + check_and_emit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, execution_time, + planning_time, fraction, false), + test_case, "Put second valid trajectory, no prune_worse_trajectories, ok"); check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); @@ -867,27 +871,27 @@ void test_cartesian_trajectories(std::shared_ptr move_group, check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); - // Put worse, no delete_worse_trajectories + // Put worse, no prune_worse_trajectories // // Worse trajectories should not be inserted test_case = "test_cartesian_trajectories.put_worse"; double worse_execution_time = execution_time + 100; - check_and_emit(!cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, worse_execution_time, - planning_time, fraction, false), - test_case, "Put worse trajectory, no delete_worse_trajectories, not ok"); + check_and_emit(!cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, + worse_execution_time, planning_time, fraction, false), + test_case, "Put worse trajectory, no prune_worse_trajectories, not ok"); check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); - // Put better, no delete_worse_trajectories + // Put better, no prune_worse_trajectories // // Better trajectories should be inserted test_case = "test_cartesian_trajectories.put_better"; double better_execution_time = execution_time - 100; - check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, better_execution_time, - planning_time, fraction, false), - test_case, "Put better trajectory, no delete_worse_trajectories, ok"); + check_and_emit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, + better_execution_time, planning_time, fraction, false), + test_case, "Put better trajectory, no prune_worse_trajectories, ok"); check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 3, test_case, "Three trajectories in cache"); @@ -922,20 +926,20 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time, test_case, "Fetched trajectories are sorted correctly"); - // Put better, delete_worse_trajectories + // Put better, prune_worse_trajectories // // Better, different, trajectories should be inserted - test_case = "test_cartesian_trajectories.put_better_delete_worse_trajectories"; + test_case = "test_cartesian_trajectories.put_better_prune_worse_trajectories"; double even_better_execution_time = better_execution_time - 100; - check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, different_traj, - even_better_execution_time, planning_time, fraction, true), - test_case, "Put better trajectory, delete_worse_trajectories, ok"); + check_and_emit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, different_traj, + even_better_execution_time, planning_time, fraction, true), + test_case, "Put better trajectory, prune_worse_trajectories, ok"); check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); // Fetch better plan - test_case = "test_cartesian_trajectories.put_better_delete_worse_trajectories.fetch"; + test_case = "test_cartesian_trajectories.put_better_prune_worse_trajectories.fetch"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); @@ -959,14 +963,14 @@ void test_cartesian_trajectories(std::shared_ptr move_group, check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); - // Put different req, delete_worse_trajectories + // Put different req, prune_worse_trajectories // // Unrelated trajectory requests should live alongside pre-existing plans test_case = "test_cartesian_trajectories.put_different_req"; - check_and_emit(cache->putCartesianTrajectory(*move_group, ROBOT_NAME, different_cartesian_plan_req, different_traj, - better_execution_time, planning_time, fraction, true), - test_case, "Put different trajectory req, delete_worse_trajectories, ok"); + check_and_emit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, different_cartesian_plan_req, different_traj, + better_execution_time, planning_time, fraction, true), + test_case, "Put different trajectory req, prune_worse_trajectories, ok"); check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); @@ -1005,13 +1009,13 @@ void test_cartesian_trajectories(std::shared_ptr move_group, check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 0, test_case, "No trajectories in cache"); - // Put first for different robot, delete_worse_trajectories + // Put first for different robot, prune_worse_trajectories // // A different robot's cache should not interact with the original cache test_case = "test_cartesian_trajectories.different_robot.put_first"; - check_and_emit(cache->putCartesianTrajectory(*move_group, different_robot_name, different_cartesian_plan_req, - different_traj, better_execution_time, planning_time, fraction, true), - test_case, "Put different trajectory req, delete_worse_trajectories, ok"); + check_and_emit(cache->insertCartesianTrajectory(*move_group, different_robot_name, different_cartesian_plan_req, + different_traj, better_execution_time, planning_time, fraction, true), + test_case, "Put different trajectory req, prune_worse_trajectories, ok"); check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 1, test_case, "One trajectory in cache"); diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.py b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py index 8396872ccf..05cfcabd25 100755 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.py +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py @@ -1,4 +1,4 @@ -# Copyright 2023 Johnson & Johnson +# Copyright 2024 Intrinsic Innovation LLC. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,6 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +# Author: methylDragon + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ExecuteProcess From 307fa98e6909eea1efc5ac82f8077da4ce5aa8af Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 31 Jul 2024 01:44:13 -0700 Subject: [PATCH 21/26] Make clang tidy happy Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/CMakeLists.txt | 22 +- .../test/test_trajectory_cache.cpp | 625 +++++++++--------- 2 files changed, 325 insertions(+), 322 deletions(-) diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt index 907f5942f6..49e3a51a91 100644 --- a/moveit_ros/trajectory_cache/CMakeLists.txt +++ b/moveit_ros/trajectory_cache/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(trajectory_msgs REQUIRED) find_package(warehouse_ros REQUIRED) find_package(warehouse_ros_sqlite REQUIRED) +include(GenerateExportHeader) include_directories(include) set(TRAJECTORY_CACHE_DEPENDENCIES @@ -31,22 +32,29 @@ set(TRAJECTORY_CACHE_DEPENDENCIES # ============================================================================== -# Motion plan cache library -add_library(trajectory_cache src/trajectory_cache.cpp) +# Trajectory cache library +add_library(moveit_ros_trajectory_cache_lib src/trajectory_cache.cpp) +generate_export_header(moveit_ros_trajectory_cache_lib) target_include_directories( - trajectory_cache + moveit_ros_trajectory_cache_lib PUBLIC $ $) -ament_target_dependencies(trajectory_cache ${TRAJECTORY_CACHE_DEPENDENCIES}) +ament_target_dependencies(moveit_ros_trajectory_cache_lib + ${TRAJECTORY_CACHE_DEPENDENCIES}) install( - TARGETS trajectory_cache + TARGETS moveit_ros_trajectory_cache_lib EXPORT moveit_ros_trajectory_cacheTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin) + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include/moveit_ros_trajectory_cache) install(DIRECTORY include/ DESTINATION include/moveit_ros_trajectory_cache) +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_ros_trajectory_cache_lib_export.h + DESTINATION include/moveit_ros_trajectory_cache) # ============================================================================== @@ -58,7 +66,7 @@ if(BUILD_TESTING) # This test executable is run by the pytest_test, since a node is required for # testing move groups add_executable(test_trajectory_cache test/test_trajectory_cache.cpp) - target_link_libraries(test_trajectory_cache trajectory_cache) + target_link_libraries(test_trajectory_cache moveit_ros_trajectory_cache_lib) install(TARGETS test_trajectory_cache RUNTIME DESTINATION lib/${PROJECT_NAME}) diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp index d68ca82607..dfe8ef7e47 100644 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp @@ -33,19 +33,19 @@ const std::string ROBOT_FRAME = "world"; // UTILS ======================================================================= // Utility function to emit a pass or fail statement. -void check_and_emit(bool predicate, const std::string& test_case, const std::string& label) +void checkAndEmit(bool predicate, const std::string& test_case, const std::string& label) { if (predicate) { - std::cout << "[PASS] " << test_case << ": " << label << std::endl; + std::cout << "[PASS] " << test_case << ": " << label << "\n"; } else { - std::cout << "[FAIL] " << test_case << ": " << label << std::endl; + std::cout << "[FAIL] " << test_case << ": " << label << "\n"; } } -moveit_msgs::msg::RobotTrajectory get_dummy_panda_traj() +moveit_msgs::msg::RobotTrajectory getDummyPandaTraj() { static moveit_msgs::msg::RobotTrajectory out = []() { moveit_msgs::msg::RobotTrajectory traj; @@ -72,7 +72,7 @@ moveit_msgs::msg::RobotTrajectory get_dummy_panda_traj() return out; } -std::vector get_dummy_waypoints() +std::vector getDummyWaypoints() { static std::vector out = []() { std::vector waypoints; @@ -92,25 +92,26 @@ std::vector get_dummy_waypoints() return out; } -void test_getters_and_setters(std::shared_ptr cache) +void testGettersAndSetters(const std::shared_ptr& cache) { std::string test_case = "getters_and_setters"; - check_and_emit(cache->getDbPath() == ":memory:", test_case, "getDbPath"); - check_and_emit(cache->getDbPort() == 0, test_case, "getDbPort"); + checkAndEmit(cache->getDbPath() == ":memory:", test_case, "getDbPath"); + checkAndEmit(cache->getDbPort() == 0, test_case, "getDbPort"); - check_and_emit(cache->getExactMatchPrecision() == 10, test_case, "getExactMatchPrecision"); + checkAndEmit(cache->getExactMatchPrecision() == 10, test_case, "getExactMatchPrecision"); cache->setExactMatchPrecision(1); - check_and_emit(cache->getExactMatchPrecision() == 1, test_case, "getExactMatchPrecisionAfterSet"); + checkAndEmit(cache->getExactMatchPrecision() == 1, test_case, "getExactMatchPrecisionAfterSet"); - check_and_emit(cache->getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() == 10, test_case, - "getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse"); + checkAndEmit(cache->getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() == 10, test_case, + "getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse"); cache->setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse(1); - check_and_emit(cache->getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() == 1, test_case, - "getNumAdditionalTrajectoriesToPreserveWhenDeletingWorseAfterSet"); + checkAndEmit(cache->getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() == 1, test_case, + "getNumAdditionalTrajectoriesToPreserveWhenDeletingWorseAfterSet"); } -void test_motion_trajectories(std::shared_ptr move_group, std::shared_ptr cache) +void testMotionTrajectories(const std::shared_ptr& move_group, + const std::shared_ptr& cache) { // Setup ===================================================================== // All variants are modified copies of `plan_req`. @@ -193,7 +194,7 @@ void test_motion_trajectories(std::shared_ptr move_group, st /// RobotTrajectory // Trajectory - auto traj = get_dummy_panda_traj(); + auto traj = getDummyPandaTraj(); // Trajectory with no frame_id in its trajectory header auto empty_frame_traj = traj; @@ -211,385 +212,386 @@ void test_motion_trajectories(std::shared_ptr move_group, st // Checks ==================================================================== // Initially empty - test_case = "test_motion_trajectories.empty"; + test_case = "testMotionTrajectories.empty"; - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 0, test_case, "Trajectory cache initially empty"); + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 0, test_case, "Trajectory cache initially empty"); - check_and_emit(cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 999, 999).empty(), test_case, - "Fetch all trajectories on empty cache returns empty"); + checkAndEmit(cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 999, 999).empty(), test_case, + "Fetch all trajectories on empty cache returns empty"); - check_and_emit(cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 999, 999) == nullptr, test_case, - "Fetch best trajectory on empty cache returns nullptr"); + checkAndEmit(cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 999, 999) == nullptr, test_case, + "Fetch best trajectory on empty cache returns nullptr"); // Put trajectory empty frame // // Trajectory must have frame in joint trajectory, expect put fail - test_case = "test_motion_trajectories.insertTrajectory_empty_frame"; + test_case = "testMotionTrajectories.insertTrajectory_empty_frame"; double execution_time = 999; double planning_time = 999; - check_and_emit(!cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, empty_frame_traj, execution_time, - planning_time, false), - test_case, "Put empty frame trajectory, no prune_worse_trajectories, not ok"); + checkAndEmit(!cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, empty_frame_traj, execution_time, + planning_time, false), + test_case, "Put empty frame trajectory, no prune_worse_trajectories, not ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 0, test_case, "No trajectories in cache"); + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 0, test_case, "No trajectories in cache"); // Put trajectory req empty frame // // Trajectory request having no frame in workspace should default to robot frame - test_case = "test_motion_trajectories.insertTrajectory_req_empty_frame"; + test_case = "testMotionTrajectories.insertTrajectory_req_empty_frame"; execution_time = 1000; planning_time = 1000; - check_and_emit(cache->insertTrajectory(*move_group, ROBOT_NAME, empty_frame_plan_req, traj, execution_time, - planning_time, false), - test_case, "Put empty frame req trajectory, no prune_worse_trajectories, ok"); + checkAndEmit(cache->insertTrajectory(*move_group, ROBOT_NAME, empty_frame_plan_req, traj, execution_time, + planning_time, false), + test_case, "Put empty frame req trajectory, no prune_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); // Put second, no prune_worse_trajectories - test_case = "test_motion_trajectories.put_second"; + test_case = "testMotionTrajectories.put_second"; execution_time = 999; planning_time = 999; - check_and_emit(cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, traj, execution_time, planning_time, false), - test_case, "Put second valid trajectory, no prune_worse_trajectories, ok"); + checkAndEmit(cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, traj, execution_time, planning_time, false), + test_case, "Put second valid trajectory, no prune_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); // Fetch matching, no tolerance // // Exact key match should have cache hit - test_case = "test_motion_trajectories.put_second.fetch_matching_no_tolerance"; + test_case = "testMotionTrajectories.put_second.fetch_matching_no_tolerance"; auto fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0); auto fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, - "Fetched trajectory has correct execution time"); + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, - "Fetched trajectory has correct planning time"); + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); // Fetch with is_diff // // is_diff key should be equivalent to exact match if robot state did not // change, hence should have cache hit - test_case = "test_motion_trajectories.put_second.fetch_is_diff_no_tolerance"; + test_case = "testMotionTrajectories.put_second.fetch_is_diff_no_tolerance"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, is_diff_plan_req, 0, 0); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, is_diff_plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, - "Fetched trajectory has correct execution time"); + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, - "Fetched trajectory has correct planning time"); + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); // Fetch non-matching, out of tolerance // // Non-matching key should not have cache hit - test_case = "test_motion_trajectories.put_second.fetch_non_matching_out_of_tolerance"; + test_case = "testMotionTrajectories.put_second.fetch_non_matching_out_of_tolerance"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 0); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch non-matching, only start in tolerance (but not goal) // // Non-matching key should not have cache hit - test_case = "test_motion_trajectories.put_second.fetch_non_matching_only_start_in_tolerance"; + test_case = "testMotionTrajectories.put_second.fetch_non_matching_only_start_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 999, 0); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 999, 0); - check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch non-matching, only goal in tolerance (but not start) // // Non-matching key should not have cache hit - test_case = "test_motion_trajectories.put_second.fetch_non_matching_only_goal_in_tolerance"; + test_case = "testMotionTrajectories.put_second.fetch_non_matching_only_goal_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 999); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 999); - check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch non-matching, in tolerance // // Close key within tolerance limit should have cache hit - test_case = "test_motion_trajectories.put_second.fetch_non_matching_in_tolerance"; + test_case = "testMotionTrajectories.put_second.fetch_non_matching_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0.1, 0.1); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0.1, 0.1); - check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, - "Fetched trajectory has correct execution time"); + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, - "Fetched trajectory has correct planning time"); + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); // Fetch swapped // // Matches should be mostly invariant to constraint ordering - test_case = "test_motion_trajectories.put_second.fetch_swapped"; + test_case = "testMotionTrajectories.put_second.fetch_swapped"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, swapped_close_matching_plan_req, 0.1, 0.1); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, swapped_close_matching_plan_req, 0.1, 0.1); - check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, - "Fetched trajectory has correct execution time"); + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, - "Fetched trajectory has correct planning time"); + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); // Fetch with smaller workspace // // Matching trajectories with more restrictive workspace requirements should not // pull up trajectories cached for less restrictive workspace requirements - test_case = "test_motion_trajectories.put_second.fetch_smaller_workspace"; + test_case = "testMotionTrajectories.put_second.fetch_smaller_workspace"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, smaller_workspace_plan_req, 999, 999); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, smaller_workspace_plan_req, 999, 999); - check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch with larger workspace // // Matching trajectories with less restrictive workspace requirements should pull up // trajectories cached for more restrictive workspace requirements - test_case = "test_motion_trajectories.put_second.fetch_larger_workspace"; + test_case = "testMotionTrajectories.put_second.fetch_larger_workspace"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, larger_workspace_plan_req, 999, 999); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, larger_workspace_plan_req, 999, 999); - check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, - "Fetched trajectory has correct execution time"); + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, - "Fetched trajectory has correct planning time"); + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); - check_and_emit(fetched_traj->lookupDouble("workspace_parameters.max_corner.x") <= - larger_workspace_plan_req.workspace_parameters.max_corner.x, - test_case, "Fetched trajectory has more restrictive workspace max_corner"); + checkAndEmit(fetched_traj->lookupDouble("workspace_parameters.max_corner.x") <= + larger_workspace_plan_req.workspace_parameters.max_corner.x, + test_case, "Fetched trajectory has more restrictive workspace max_corner"); - check_and_emit(fetched_traj->lookupDouble("workspace_parameters.max_corner.x") >= - larger_workspace_plan_req.workspace_parameters.min_corner.x, - test_case, "Fetched trajectory has more restrictive workspace min_corner"); + checkAndEmit(fetched_traj->lookupDouble("workspace_parameters.max_corner.x") >= + larger_workspace_plan_req.workspace_parameters.min_corner.x, + test_case, "Fetched trajectory has more restrictive workspace min_corner"); // Put worse, no prune_worse_trajectories // // Worse trajectories should not be inserted - test_case = "test_motion_trajectories.put_worse"; + test_case = "testMotionTrajectories.put_worse"; double worse_execution_time = execution_time + 100; - check_and_emit(!cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, traj, worse_execution_time, planning_time, - false), - test_case, "Put worse trajectory, no prune_worse_trajectories, not ok"); + checkAndEmit(!cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, traj, worse_execution_time, planning_time, + false), + test_case, "Put worse trajectory, no prune_worse_trajectories, not ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); // Put better, no prune_worse_trajectories // // Better trajectories should be inserted - test_case = "test_motion_trajectories.put_better"; + test_case = "testMotionTrajectories.put_better"; double better_execution_time = execution_time - 100; - check_and_emit(cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, traj, better_execution_time, planning_time, - false), - test_case, "Put better trajectory, no prune_worse_trajectories, ok"); + checkAndEmit(cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, traj, better_execution_time, planning_time, + false), + test_case, "Put better trajectory, no prune_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 3, test_case, "Three trajectories in cache"); + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 3, test_case, "Three trajectories in cache"); // Fetch sorted // // With multiple trajectories in cache, fetches should be sorted accordingly - test_case = "test_motion_trajectories.put_better.fetch_sorted"; + test_case = "testMotionTrajectories.put_better.fetch_sorted"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 3, test_case, "Fetch all returns three"); - check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + checkAndEmit(fetched_trajectories.size() == 3, test_case, "Fetch all returns three"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, - "Fetched trajectory has correct execution time"); + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, + "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, - "Fetched trajectory has correct planning time"); + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); - check_and_emit(fetched_trajectories.at(0)->lookupDouble("execution_time_s") == better_execution_time && - fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time, - test_case, "Fetched trajectories are sorted correctly"); + checkAndEmit(fetched_trajectories.at(0)->lookupDouble("execution_time_s") == better_execution_time && + fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time, + test_case, "Fetched trajectories are sorted correctly"); // Put better, prune_worse_trajectories // // Better, different, trajectories should be inserted - test_case = "test_motion_trajectories.put_better_prune_worse_trajectories"; + test_case = "testMotionTrajectories.put_better_prune_worse_trajectories"; double even_better_execution_time = better_execution_time - 100; - check_and_emit(cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, different_traj, even_better_execution_time, - planning_time, true), - test_case, "Put better trajectory, prune_worse_trajectories, ok"); + checkAndEmit(cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, different_traj, even_better_execution_time, + planning_time, true), + test_case, "Put better trajectory, prune_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); // Fetch better plan - test_case = "test_motion_trajectories.put_better_prune_worse_trajectories.fetch"; + test_case = "testMotionTrajectories.put_better_prune_worse_trajectories.fetch"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); - check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); + checkAndEmit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, test_case, - "Fetched trajectory has correct execution time"); + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, test_case, + "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, - "Fetched trajectory has correct planning time"); + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); // Put different req, prune_worse_trajectories // // Unrelated trajectory requests should live alongside pre-existing plans - test_case = "test_motion_trajectories.put_different_req"; + test_case = "testMotionTrajectories.put_different_req"; - check_and_emit(cache->insertTrajectory(*move_group, ROBOT_NAME, different_plan_req, different_traj, - better_execution_time, planning_time, true), - test_case, "Put different trajectory req, prune_worse_trajectories, ok"); + checkAndEmit(cache->insertTrajectory(*move_group, ROBOT_NAME, different_plan_req, different_traj, + better_execution_time, planning_time, true), + test_case, "Put different trajectory req, prune_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); // Fetch with different trajectory req // // With multiple trajectories in cache, fetches should be sorted accordingly - test_case = "test_motion_trajectories.put_different_req.fetch"; + test_case = "testMotionTrajectories.put_different_req.fetch"; fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, different_plan_req, 0, 0); fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, different_plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); - check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); + checkAndEmit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, - "Fetched trajectory has correct execution time"); + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, + "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, - "Fetched trajectory has correct planning time"); + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); // Fetch different robot // // Since we didn't populate anything, we should expect empty - test_case = "test_motion_trajectories.different_robot.empty"; + test_case = "testMotionTrajectories.different_robot.empty"; std::string different_robot_name = "different_robot"; - check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 0, test_case, "No trajectories in cache"); + checkAndEmit(cache->countCartesianTrajectories(different_robot_name) == 0, test_case, "No trajectories in cache"); // Put first for different robot, prune_worse_trajectories // // A different robot's cache should not interact with the original cache - test_case = "test_motion_trajectories.different_robot.put_first"; - check_and_emit(cache->insertTrajectory(*move_group, different_robot_name, different_plan_req, different_traj, - better_execution_time, planning_time, true), - test_case, "Put different trajectory req, prune_worse_trajectories, ok"); + test_case = "testMotionTrajectories.different_robot.put_first"; + checkAndEmit(cache->insertTrajectory(*move_group, different_robot_name, different_plan_req, different_traj, + better_execution_time, planning_time, true), + test_case, "Put different trajectory req, prune_worse_trajectories, ok"); - check_and_emit(cache->countTrajectories(different_robot_name) == 1, test_case, "One trajectory in cache"); + checkAndEmit(cache->countTrajectories(different_robot_name) == 1, test_case, "One trajectory in cache"); - check_and_emit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in original robot's cache"); + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in original robot's cache"); fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, different_plan_req, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, test_case, "Fetch all on original still returns one"); + checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all on original still returns one"); } -void test_cartesian_trajectories(std::shared_ptr move_group, std::shared_ptr cache) +void testCartesianTrajectories(const std::shared_ptr& move_group, + const std::shared_ptr& cache) { std::string test_case; /// First, test if construction even works... // Construct get cartesian trajectory request - test_case = "test_cartesian_trajectories.constructGetCartesianPathRequest"; + test_case = "testCartesianTrajectories.constructGetCartesianPathRequest"; int test_step = 1; int test_jump = 2; - auto test_waypoints = get_dummy_waypoints(); + auto test_waypoints = getDummyWaypoints(); auto cartesian_plan_req_under_test = cache->constructGetCartesianPathRequest(*move_group, test_waypoints, test_step, test_jump, false); - check_and_emit(cartesian_plan_req_under_test.waypoints == test_waypoints && - static_cast(cartesian_plan_req_under_test.max_step) == test_step && - static_cast(cartesian_plan_req_under_test.jump_threshold) == test_jump && - !cartesian_plan_req_under_test.avoid_collisions, - test_case, "Ok"); + checkAndEmit(cartesian_plan_req_under_test.waypoints == test_waypoints && + static_cast(cartesian_plan_req_under_test.max_step) == test_step && + static_cast(cartesian_plan_req_under_test.jump_threshold) == test_jump && + !cartesian_plan_req_under_test.avoid_collisions, + test_case, "Ok"); // Setup ===================================================================== // All variants are modified copies of `cartesian_plan_req`. @@ -597,7 +599,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, /// GetCartesianPath::Request // Plain start - auto waypoints = get_dummy_waypoints(); + auto waypoints = getDummyWaypoints(); auto cartesian_plan_req = cache->constructGetCartesianPathRequest(*move_group, waypoints, 1, 1, false); cartesian_plan_req.start_state.multi_dof_joint_state.joint_names.clear(); cartesian_plan_req.start_state.multi_dof_joint_state.transforms.clear(); @@ -642,7 +644,7 @@ void test_cartesian_trajectories(std::shared_ptr move_group, /// RobotTrajectory // Trajectory - auto traj = get_dummy_panda_traj(); + auto traj = getDummyPandaTraj(); // Trajectory with no frame_id in its trajectory header auto empty_frame_traj = traj; @@ -656,60 +658,60 @@ void test_cartesian_trajectories(std::shared_ptr move_group, // Checks ==================================================================== // Initially empty - test_case = "test_cartesian_trajectories.empty"; + test_case = "testCartesianTrajectories.empty"; - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, test_case, "Trajectory cache initially empty"); + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, test_case, "Trajectory cache initially empty"); - check_and_emit( + checkAndEmit( cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999).empty(), test_case, "Fetch all trajectories on empty cache returns empty"); - check_and_emit(cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, - 999) == nullptr, - test_case, "Fetch best trajectory on empty cache returns nullptr"); + checkAndEmit(cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999) == + nullptr, + test_case, "Fetch best trajectory on empty cache returns nullptr"); // Put trajectory empty frame // // Trajectory must have frame in joint trajectory, expect put fail - test_case = "test_cartesian_trajectories.insertTrajectory_empty_frame"; + test_case = "testCartesianTrajectories.insertTrajectory_empty_frame"; double execution_time = 999; double planning_time = 999; double fraction = 0.5; - check_and_emit(!cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, empty_frame_traj, - execution_time, planning_time, fraction, false), - test_case, "Put empty frame trajectory, no prune_worse_trajectories, not ok"); + checkAndEmit(!cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, empty_frame_traj, + execution_time, planning_time, fraction, false), + test_case, "Put empty frame trajectory, no prune_worse_trajectories, not ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, test_case, "No trajectories in cache"); + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, test_case, "No trajectories in cache"); // Put trajectory req empty frame // // Trajectory request having no frame in workspace should default to robot frame - test_case = "test_cartesian_trajectories.insertTrajectory_req_empty_frame"; + test_case = "testCartesianTrajectories.insertTrajectory_req_empty_frame"; execution_time = 1000; planning_time = 1000; - check_and_emit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, empty_frame_cartesian_plan_req, traj, - execution_time, planning_time, fraction, false), - test_case, "Put empty frame req trajectory, no prune_worse_trajectories, ok"); + checkAndEmit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, empty_frame_cartesian_plan_req, traj, + execution_time, planning_time, fraction, false), + test_case, "Put empty frame req trajectory, no prune_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); // Put second, no prune_worse_trajectories - test_case = "test_cartesian_trajectories.put_second"; + test_case = "testCartesianTrajectories.put_second"; execution_time = 999; planning_time = 999; - check_and_emit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, execution_time, - planning_time, fraction, false), - test_case, "Put second valid trajectory, no prune_worse_trajectories, ok"); + checkAndEmit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, execution_time, + planning_time, fraction, false), + test_case, "Put second valid trajectory, no prune_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); // Fetch matching, no tolerance // // Exact key match should have cache hit - test_case = "test_cartesian_trajectories.put_second.fetch_matching_no_tolerance"; + test_case = "testCartesianTrajectories.put_second.fetch_matching_no_tolerance"; auto fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); @@ -717,27 +719,26 @@ void test_cartesian_trajectories(std::shared_ptr move_group, auto fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, - "Fetched trajectory has correct execution time"); + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, - "Fetched trajectory has correct planning time"); + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); - check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, - "Fetched trajectory has correct fraction"); + checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); // Fetch with is_diff // // is_diff key should be equivalent to exact match if robot state did not // change, hence should have cache hit - test_case = "test_cartesian_trajectories.put_second.fetch_is_diff_no_tolerance"; + test_case = "testCartesianTrajectories.put_second.fetch_is_diff_no_tolerance"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, is_diff_cartesian_plan_req, fraction, 0, 0); @@ -745,26 +746,25 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, is_diff_cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, - "Fetched trajectory has correct execution time"); + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, - "Fetched trajectory has correct planning time"); + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); - check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, - "Fetched trajectory has correct fraction"); + checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); // Fetch non-matching, out of tolerance // // Non-matching key should not have cache hit - test_case = "test_cartesian_trajectories.put_second.fetch_non_matching_out_of_tolerance"; + test_case = "testCartesianTrajectories.put_second.fetch_non_matching_out_of_tolerance"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 0); @@ -772,13 +772,13 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch non-matching, only start in tolerance (but not goal) // // Non-matching key should not have cache hit - test_case = "test_motion_trajectories.put_second.fetch_non_matching_only_start_in_tolerance"; + test_case = "testMotionTrajectories.put_second.fetch_non_matching_only_start_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 999, 0); @@ -786,13 +786,13 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 999, 0); - check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch non-matching, only goal in tolerance (but not start) // // Non-matching key should not have cache hit - test_case = "test_motion_trajectories.put_second.fetch_non_matching_only_goal_in_tolerance"; + test_case = "testMotionTrajectories.put_second.fetch_non_matching_only_goal_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 999); @@ -800,13 +800,13 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 999); - check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch non-matching, in tolerance // // Close key within tolerance limit should have cache hit - test_case = "test_cartesian_trajectories.put_second.fetch_non_matching_in_tolerance"; + test_case = "testCartesianTrajectories.put_second.fetch_non_matching_in_tolerance"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0.1, 0.1); @@ -814,91 +814,89 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0.1, 0.1); - check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, - "Fetched trajectory has correct execution time"); + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, - "Fetched trajectory has correct planning time"); + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); - check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, - "Fetched trajectory has correct fraction"); + checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); // Fetch with higher fraction // // Matching trajectories with more restrictive fraction requirements should not // pull up trajectories cached for less restrictive fraction requirements - test_case = "test_cartesian_trajectories.put_second.fetch_higher_fraction"; + test_case = "testCartesianTrajectories.put_second.fetch_higher_fraction"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 1, 999, 999); fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 1, 999, 999); - check_and_emit(fetched_trajectories.size() == 0, test_case, "Fetch all returns empty"); - check_and_emit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); // Fetch with lower fraction // // Matching trajectories with less restrictive fraction requirements should pull up // trajectories cached for more restrictive fraction requirements - test_case = "test_cartesian_trajectories.put_second.fetch_lower_fraction"; + test_case = "testCartesianTrajectories.put_second.fetch_lower_fraction"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999); fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999); - check_and_emit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); - check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, - "Fetched trajectory has correct execution time"); + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, - "Fetched trajectory has correct planning time"); + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); - check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, - "Fetched trajectory has correct fraction"); + checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); // Put worse, no prune_worse_trajectories // // Worse trajectories should not be inserted - test_case = "test_cartesian_trajectories.put_worse"; + test_case = "testCartesianTrajectories.put_worse"; double worse_execution_time = execution_time + 100; - check_and_emit(!cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, - worse_execution_time, planning_time, fraction, false), - test_case, "Put worse trajectory, no prune_worse_trajectories, not ok"); + checkAndEmit(!cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, + worse_execution_time, planning_time, fraction, false), + test_case, "Put worse trajectory, no prune_worse_trajectories, not ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); // Put better, no prune_worse_trajectories // // Better trajectories should be inserted - test_case = "test_cartesian_trajectories.put_better"; + test_case = "testCartesianTrajectories.put_better"; double better_execution_time = execution_time - 100; - check_and_emit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, - better_execution_time, planning_time, fraction, false), - test_case, "Put better trajectory, no prune_worse_trajectories, ok"); + checkAndEmit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, + better_execution_time, planning_time, fraction, false), + test_case, "Put better trajectory, no prune_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 3, test_case, "Three trajectories in cache"); + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 3, test_case, "Three trajectories in cache"); // Fetch sorted // // With multiple trajectories in cache, fetches should be sorted accordingly - test_case = "test_cartesian_trajectories.put_better.fetch_sorted"; + test_case = "testCartesianTrajectories.put_better.fetch_sorted"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); @@ -906,40 +904,39 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 3, test_case, "Fetch all returns three"); - check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + checkAndEmit(fetched_trajectories.size() == 3, test_case, "Fetch all returns three"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, - "Fetched trajectory has correct execution time"); + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, + "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, - "Fetched trajectory has correct planning time"); + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); - check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, - "Fetched trajectory has correct fraction"); + checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); - check_and_emit(fetched_trajectories.at(0)->lookupDouble("execution_time_s") == better_execution_time && - fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time, - test_case, "Fetched trajectories are sorted correctly"); + checkAndEmit(fetched_trajectories.at(0)->lookupDouble("execution_time_s") == better_execution_time && + fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time, + test_case, "Fetched trajectories are sorted correctly"); // Put better, prune_worse_trajectories // // Better, different, trajectories should be inserted - test_case = "test_cartesian_trajectories.put_better_prune_worse_trajectories"; + test_case = "testCartesianTrajectories.put_better_prune_worse_trajectories"; double even_better_execution_time = better_execution_time - 100; - check_and_emit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, different_traj, - even_better_execution_time, planning_time, fraction, true), - test_case, "Put better trajectory, prune_worse_trajectories, ok"); + checkAndEmit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, different_traj, + even_better_execution_time, planning_time, fraction, true), + test_case, "Put better trajectory, prune_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); // Fetch better plan - test_case = "test_cartesian_trajectories.put_better_prune_worse_trajectories.fetch"; + test_case = "testCartesianTrajectories.put_better_prune_worse_trajectories.fetch"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); @@ -947,37 +944,36 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); - check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); + checkAndEmit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, test_case, - "Fetched trajectory has correct execution time"); + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, test_case, + "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, - "Fetched trajectory has correct planning time"); + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); - check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, - "Fetched trajectory has correct fraction"); + checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); // Put different req, prune_worse_trajectories // // Unrelated trajectory requests should live alongside pre-existing plans - test_case = "test_cartesian_trajectories.put_different_req"; + test_case = "testCartesianTrajectories.put_different_req"; - check_and_emit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, different_cartesian_plan_req, different_traj, - better_execution_time, planning_time, fraction, true), - test_case, "Put different trajectory req, prune_worse_trajectories, ok"); + checkAndEmit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, different_cartesian_plan_req, different_traj, + better_execution_time, planning_time, fraction, true), + test_case, "Put different trajectory req, prune_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); // Fetch with different trajectory req // // With multiple trajectories in cache, fetches should be sorted accordingly - test_case = "test_cartesian_trajectories.put_different_req.fetch"; + test_case = "testCartesianTrajectories.put_different_req.fetch"; fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, different_cartesian_plan_req, fraction, 0, 0); @@ -985,47 +981,46 @@ void test_cartesian_trajectories(std::shared_ptr move_group, fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, different_cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); - check_and_emit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); - check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); - check_and_emit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); + checkAndEmit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); - check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, - "Fetched trajectory has correct execution time"); + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, + "Fetched trajectory has correct execution time"); - check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, - "Fetched trajectory has correct planning time"); + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); - check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, test_case, - "Fetched trajectory has correct fraction"); + checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); // Fetch different robot // // Since we didn't populate anything, we should expect empty - test_case = "test_cartesian_trajectories.different_robot.empty"; + test_case = "testCartesianTrajectories.different_robot.empty"; std::string different_robot_name = "different_robot"; - check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 0, test_case, "No trajectories in cache"); + checkAndEmit(cache->countCartesianTrajectories(different_robot_name) == 0, test_case, "No trajectories in cache"); // Put first for different robot, prune_worse_trajectories // // A different robot's cache should not interact with the original cache - test_case = "test_cartesian_trajectories.different_robot.put_first"; - check_and_emit(cache->insertCartesianTrajectory(*move_group, different_robot_name, different_cartesian_plan_req, - different_traj, better_execution_time, planning_time, fraction, true), - test_case, "Put different trajectory req, prune_worse_trajectories, ok"); + test_case = "testCartesianTrajectories.different_robot.put_first"; + checkAndEmit(cache->insertCartesianTrajectory(*move_group, different_robot_name, different_cartesian_plan_req, + different_traj, better_execution_time, planning_time, fraction, true), + test_case, "Put different trajectory req, prune_worse_trajectories, ok"); - check_and_emit(cache->countCartesianTrajectories(different_robot_name) == 1, test_case, "One trajectory in cache"); + checkAndEmit(cache->countCartesianTrajectories(different_robot_name) == 1, test_case, "One trajectory in cache"); - check_and_emit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, - "Two trajectories in original robot's cache"); + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, + "Two trajectories in original robot's cache"); fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, different_cartesian_plan_req, fraction, 0, 0); - check_and_emit(fetched_trajectories.size() == 1, test_case, "Fetch all on original still returns one"); + checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all on original still returns one"); } int main(int argc, char** argv) @@ -1074,15 +1069,15 @@ int main(int argc, char** argv) // Tests. - check_and_emit(cache->init(options), "init", "Cache init"); + checkAndEmit(cache->init(options), "init", "Cache init"); - test_getters_and_setters(cache); + testGettersAndSetters(cache); cache->setExactMatchPrecision(1e-4); cache->setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse(0); - test_motion_trajectories(move_group, cache); - test_cartesian_trajectories(move_group, cache); + testMotionTrajectories(move_group, cache); + testCartesianTrajectories(move_group, cache); } running = false; From 70fc4878cd5b5e5db4da330bcf8b6a144cbdc156 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Thu, 8 Aug 2024 16:05:04 -0700 Subject: [PATCH 22/26] Fix CMakeLists.txt Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/CMakeLists.txt | 16 ++++------------ moveit_ros/trajectory_cache/package.xml | 2 +- 2 files changed, 5 insertions(+), 13 deletions(-) diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt index 49e3a51a91..8841fed3e4 100644 --- a/moveit_ros/trajectory_cache/CMakeLists.txt +++ b/moveit_ros/trajectory_cache/CMakeLists.txt @@ -1,10 +1,6 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.22) project(moveit_ros_trajectory_cache) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) @@ -14,7 +10,6 @@ find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(trajectory_msgs REQUIRED) find_package(warehouse_ros REQUIRED) -find_package(warehouse_ros_sqlite REQUIRED) include(GenerateExportHeader) include_directories(include) @@ -27,10 +22,7 @@ set(TRAJECTORY_CACHE_DEPENDENCIES tf2 tf2_ros trajectory_msgs - warehouse_ros - warehouse_ros_sqlite) - -# ============================================================================== + warehouse_ros) # Trajectory cache library add_library(moveit_ros_trajectory_cache_lib src/trajectory_cache.cpp) @@ -56,17 +48,17 @@ install( FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_ros_trajectory_cache_lib_export.h DESTINATION include/moveit_ros_trajectory_cache) -# ============================================================================== - if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) find_package(rmf_utils REQUIRED) + find_package(warehouse_ros_sqlite REQUIRED) # This test executable is run by the pytest_test, since a node is required for # testing move groups add_executable(test_trajectory_cache test/test_trajectory_cache.cpp) target_link_libraries(test_trajectory_cache moveit_ros_trajectory_cache_lib) + ament_target_dependencies(test_trajectory_cache warehouse_ros_sqlite) install(TARGETS test_trajectory_cache RUNTIME DESTINATION lib/${PROJECT_NAME}) diff --git a/moveit_ros/trajectory_cache/package.xml b/moveit_ros/trajectory_cache/package.xml index 9a1edf74b3..956002d2fd 100644 --- a/moveit_ros/trajectory_cache/package.xml +++ b/moveit_ros/trajectory_cache/package.xml @@ -18,7 +18,6 @@ moveit_ros python3-yaml - warehouse_ros_sqlite xacro ament_cmake_pytest @@ -33,6 +32,7 @@ rmf_utils robot_state_publisher ros2_control + warehouse_ros_sqlite ament_cmake From d12ad95e424c8caf253e27b0af844e722ea2674c Mon Sep 17 00:00:00 2001 From: methylDragon Date: Thu, 8 Aug 2024 16:09:05 -0700 Subject: [PATCH 23/26] Make getters const Signed-off-by: methylDragon --- .../include/moveit/trajectory_cache/trajectory_cache.hpp | 8 ++++---- moveit_ros/trajectory_cache/src/trajectory_cache.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index db28f7414b..2af5058840 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -186,19 +186,19 @@ class TrajectoryCache /**@{*/ /// @brief Gets the database path. - std::string getDbPath(); + std::string getDbPath() const; /// @brief Gets the database port. - uint32_t getDbPort(); + uint32_t getDbPort() const; /// @brief Gets the exact match precision. - double getExactMatchPrecision(); + double getExactMatchPrecision() const; /// @brief Sets the exact match precision. void setExactMatchPrecision(double exact_match_precision); /// @brief Get the number of trajectories to preserve when deleting worse trajectories. - size_t getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse(); + size_t getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() const; /// @brief Set the number of additional trajectories to preserve when deleting worse trajectories. void setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse( diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index 21365cb9e7..d5c3ee51c8 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -135,17 +135,17 @@ unsigned TrajectoryCache::countCartesianTrajectories(const std::string& cache_na // GETTERS AND SETTERS // ============================================================================= -std::string TrajectoryCache::getDbPath() +std::string TrajectoryCache::getDbPath() const { return options_.db_path; } -uint32_t TrajectoryCache::getDbPort() +uint32_t TrajectoryCache::getDbPort() const { return options_.db_port; } -double TrajectoryCache::getExactMatchPrecision() +double TrajectoryCache::getExactMatchPrecision() const { return options_.exact_match_precision; } @@ -155,7 +155,7 @@ void TrajectoryCache::setExactMatchPrecision(double exact_match_precision) options_.exact_match_precision = exact_match_precision; } -size_t TrajectoryCache::getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() +size_t TrajectoryCache::getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() const { return options_.num_additional_trajectories_to_preserve_when_deleting_worse; } From 5e4a622b8dd97cd5d1723a18413518300a3cd1b8 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Thu, 8 Aug 2024 16:14:29 -0700 Subject: [PATCH 24/26] Clarify frame ID utils docstrings Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/src/trajectory_cache.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index d5c3ee51c8..81bceebe5c 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -41,6 +41,10 @@ using warehouse_ros::Query; // Utils ======================================================================= // Ensure we always have a workspace frame ID. +// +// It makes sense to use getModelFrame() in the absence of a workspace parameter frame ID because the same method is +// used to populate the workspace header frame ID in the MoveGroupInterface's setWorkspace() method, which this function +// is associated with. std::string getWorkspaceFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::WorkspaceParameters& workspace_parameters) { @@ -55,6 +59,9 @@ std::string getWorkspaceFrameId(const moveit::planning_interface::MoveGroupInter } // Ensure we always have a cartesian path request frame ID. +// +// It makes sense to use getPoseReferenceFrame() in the absence of a frame ID in the request because the same method is +// used to populate the header frame ID in the MoveGroupInterface's computeCartesianPath() method, which this function is associated with. std::string getCartesianPathRequestFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& path_request) { From 9a59882f728092f4feb4ef90f35c2ad9b9fa2fdb Mon Sep 17 00:00:00 2001 From: methylDragon Date: Thu, 8 Aug 2024 16:18:47 -0700 Subject: [PATCH 25/26] Elaborate on trajectory cache benefits Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/moveit_ros/trajectory_cache/README.md b/moveit_ros/trajectory_cache/README.md index c0dc20de7d..659ee0f7a2 100644 --- a/moveit_ros/trajectory_cache/README.md +++ b/moveit_ros/trajectory_cache/README.md @@ -97,6 +97,8 @@ A trajectory cache helps: - Allows for consistent predictable behavior of used together with a stochastic planner - It effectively allows you to "freeze" a move +These benefits come from the fact that the cache acts as a lookup table of plans that were already made for a given scenario and constraints, allowing the cache to be substituted for a planner call. The reuse of cached plans then allow you to get predictable execution behavior. + A user may also choose when to leverage the cache (e.g. when planning moves from a static "home" position, or repetitive/cartesian moves) to get these benefits. Additionally, because the cache class has the ability to sort by planned execution time, over sufficient runs, the stochastic plans eventually converge to better and better plans (execution time wise). From 85aa3f792825af979f4b3a13ac14391b0746fc59 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Thu, 22 Aug 2024 08:55:44 -0700 Subject: [PATCH 26/26] Fix CHANGELOG, and make library shared Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/CHANGELOG.rst | 6 +++--- moveit_ros/trajectory_cache/CMakeLists.txt | 6 +++++- moveit_ros/trajectory_cache/package.xml | 1 + 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/moveit_ros/trajectory_cache/CHANGELOG.rst b/moveit_ros/trajectory_cache/CHANGELOG.rst index 09f7aae2e9..bbb2a3679d 100644 --- a/moveit_ros/trajectory_cache/CHANGELOG.rst +++ b/moveit_ros/trajectory_cache/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package nexus_motion_planner -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_ros_trajectory_cache +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.1.0 (2024-05-17) ------------------ diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt index 8841fed3e4..d013111c53 100644 --- a/moveit_ros/trajectory_cache/CMakeLists.txt +++ b/moveit_ros/trajectory_cache/CMakeLists.txt @@ -1,6 +1,10 @@ cmake_minimum_required(VERSION 3.22) project(moveit_ros_trajectory_cache) +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() + find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) @@ -25,7 +29,7 @@ set(TRAJECTORY_CACHE_DEPENDENCIES warehouse_ros) # Trajectory cache library -add_library(moveit_ros_trajectory_cache_lib src/trajectory_cache.cpp) +add_library(moveit_ros_trajectory_cache_lib SHARED src/trajectory_cache.cpp) generate_export_header(moveit_ros_trajectory_cache_lib) target_include_directories( moveit_ros_trajectory_cache_lib diff --git a/moveit_ros/trajectory_cache/package.xml b/moveit_ros/trajectory_cache/package.xml index 956002d2fd..cbd862e050 100644 --- a/moveit_ros/trajectory_cache/package.xml +++ b/moveit_ros/trajectory_cache/package.xml @@ -9,6 +9,7 @@ ament_cmake + moveit_common geometry_msgs moveit_ros_planning_interface rclcpp