From b705556091394b985050154ee134690a94fa3c01 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 23:50:00 -0800 Subject: [PATCH 01/13] Add count methods Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_plan_cache.cpp | 16 ++++++++++++++++ nexus_motion_planner/src/motion_plan_cache.hpp | 7 ++++--- 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index b2e8096..26f0e60 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -103,6 +103,22 @@ bool MotionPlanCache::init( return db_->connect(); } +unsigned +MotionPlanCache::count_plans(const std::string& move_group_namespace) +{ + auto coll = db_->openCollection( + "move_group_plan_cache", move_group_namespace); + return coll.count(); +} + +unsigned +MotionPlanCache::count_cartesian_plans(const std::string& move_group_namespace) +{ + auto coll = db_->openCollection( + "move_group_cartesian_plan_cache", move_group_namespace); + return coll.count(); +} + // ============================================================================= // MOTION PLAN CACHING // ============================================================================= diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index fc7bbff..fc6143c 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -37,9 +37,6 @@ #include #include -// NEXUS messages -#include - namespace nexus { namespace motion_planner { @@ -93,6 +90,10 @@ class MotionPlanCache uint32_t db_port = 0, double exact_match_precision = 1e-6); + unsigned count_plans(const std::string& move_group_namespace); + + unsigned count_cartesian_plans(const std::string& move_group_namespace); + // =========================================================================== // MOTION PLAN CACHING // =========================================================================== From ae9cbc6f6cc8e664ab5d687dc0252eb3669ed817 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 21 Nov 2023 14:11:33 -0800 Subject: [PATCH 02/13] Enable shared from this for cache class Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_plan_cache.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index fc6143c..dcf7ed1 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -78,7 +78,7 @@ namespace motion_planner { * * Motion plans may be looked up with some tolerance at call time. */ -class MotionPlanCache +class MotionPlanCache : public std::enable_shared_from_this { public: // We explicitly need a Node::SharedPtr because warehouse_ros ONLY supports From 0af131fb8c94fadcc0b6e497fb63ec7c379ed1e4 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 21 Nov 2023 18:20:20 -0800 Subject: [PATCH 03/13] Add unit test build rules Signed-off-by: methylDragon --- nexus_motion_planner/CMakeLists.txt | 27 +++++++++++++++++++++++---- nexus_motion_planner/package.xml | 6 ++++++ 2 files changed, 29 insertions(+), 4 deletions(-) diff --git a/nexus_motion_planner/CMakeLists.txt b/nexus_motion_planner/CMakeLists.txt index ca94c1e..af87314 100644 --- a/nexus_motion_planner/CMakeLists.txt +++ b/nexus_motion_planner/CMakeLists.txt @@ -28,12 +28,17 @@ set (motion_planner_server_dependencies rclcpp rclcpp_action rclcpp_lifecycle - warehouse_ros - warehouse_ros_sqlite tf2 tf2_ros ) +set (motion_plan_cache_dependencies + moveit_ros_planning_interface + rclcpp + warehouse_ros + warehouse_ros_sqlite +) + set (test_request_dependencies moveit_msgs nexus_endpoints @@ -48,7 +53,7 @@ set(EXECUTABLE_NAME motion_planner_server) # Motion plan cache library add_library(${MOTION_PLAN_CACHE_LIBRARY_NAME} src/motion_plan_cache.cpp) -ament_target_dependencies(${MOTION_PLAN_CACHE_LIBRARY_NAME} ${motion_planner_server_dependencies}) +ament_target_dependencies(${MOTION_PLAN_CACHE_LIBRARY_NAME} ${motion_plan_cache_dependencies}) # Server executable add_executable(${EXECUTABLE_NAME} src/main.cpp src/motion_planner_server.cpp) @@ -72,6 +77,8 @@ install( ) if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) find_package(ament_cmake_uncrustify REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) find_package(rmf_utils REQUIRED) @@ -80,6 +87,14 @@ if(BUILD_TESTING) NAMES "rmf_code_style.cfg" PATHS "${rmf_utils_DIR}/../../../../share/rmf_utils/") + add_executable(test_motion_plan_cache src/test_motion_plan_cache.cpp) + target_link_libraries(test_motion_plan_cache ${MOTION_PLAN_CACHE_LIBRARY_NAME}) + + install(TARGETS + test_motion_plan_cache + RUNTIME DESTINATION lib/${PROJECT_NAME} + ) + ament_uncrustify( ARGN include src test CONFIG_FILE ${uncrustify_config_file} @@ -87,6 +102,10 @@ if(BUILD_TESTING) LANGUAGE CPP ) + ament_add_pytest_test(test_motion_plan_cache_py "test/test_motion_plan_cache.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ) + # TODO(YV): Fix these tests. # add_launch_test( # test/test_request.test.py @@ -94,5 +113,5 @@ if(BUILD_TESTING) # ) endif() -ament_export_dependencies(${motion_planner_server_dependencies}) +ament_export_dependencies(${motion_planner_server_dependencies} ${motion_plan_cache_dependencies}) ament_package() diff --git a/nexus_motion_planner/package.xml b/nexus_motion_planner/package.xml index d23de1d..57e2739 100644 --- a/nexus_motion_planner/package.xml +++ b/nexus_motion_planner/package.xml @@ -28,8 +28,14 @@ abb_irb1300_10_115_moveit_config abb_irb1300_support + ament_cmake_pytest ament_cmake_uncrustify + launch_pytest launch_testing_ament_cmake + + moveit_configs_utils + moveit_resources + python3-pytest rmf_utils From 237fb50f93cccbae69fdeddb8eba93f39caa20b2 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 21 Nov 2023 19:22:18 -0800 Subject: [PATCH 04/13] Add tests for motion plan cache (but not cartesian) Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 7 +- .../src/test_motion_plan_cache.cpp | 646 ++++++++++++++++++ .../test/test_motion_plan_cache.py | 282 ++++++++ 3 files changed, 931 insertions(+), 4 deletions(-) create mode 100644 nexus_motion_planner/src/test_motion_plan_cache.cpp create mode 100755 nexus_motion_planner/test/test_motion_plan_cache.py diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 26f0e60..ce3f1f3 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -220,7 +220,7 @@ MotionPlanCache::put_plan( auto coll = db_->openCollection( "move_group_plan_cache", move_group_namespace); - // Pull out "exact" matching plans in cache. + // Pull out plans "exactly" keyed by request in cache. Query::Ptr exact_query = coll.createQuery(); bool start_query_ok = this->extract_and_append_plan_start_to_query( @@ -1111,15 +1111,14 @@ MotionPlanCache::put_cartesian_plan( { RCLCPP_ERROR( node_->get_logger(), - "Skipping cartesian plan insert: " - "Frame IDs cannot be empty."); + "Skipping cartesian plan insert: Frame IDs cannot be empty."); return false; } auto coll = db_->openCollection( "move_group_cartesian_plan_cache", move_group_namespace); - // Pull out "exact" matching plans in cache. + // Pull out plans "exactly" keyed by request in cache. Query::Ptr exact_query = coll.createQuery(); bool start_query_ok = this->extract_and_append_cartesian_plan_start_to_query( diff --git a/nexus_motion_planner/src/test_motion_plan_cache.cpp b/nexus_motion_planner/src/test_motion_plan_cache.cpp new file mode 100644 index 0000000..e554621 --- /dev/null +++ b/nexus_motion_planner/src/test_motion_plan_cache.cpp @@ -0,0 +1,646 @@ +// 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 "motion_plan_cache.hpp" + +#include + +using moveit::planning_interface::MoveGroupInterface; +using nexus::motion_planner::MotionPlanCache; + +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; + } +} + +moveit_msgs::msg::RobotTrajectory get_dummy_panda_plan() +{ + static moveit_msgs::msg::RobotTrajectory out = []() + { + moveit_msgs::msg::RobotTrajectory plan; + + auto traj = &plan.joint_trajectory; + traj->header.frame_id = g_robot_frame; + + traj->joint_names.push_back(g_robot_name + "_joint1"); + traj->joint_names.push_back(g_robot_name + "_joint2"); + traj->joint_names.push_back(g_robot_name + "_joint3"); + traj->joint_names.push_back(g_robot_name + "_joint4"); + traj->joint_names.push_back(g_robot_name + "_joint5"); + traj->joint_names.push_back(g_robot_name + "_joint6"); + traj->joint_names.push_back(g_robot_name + "_joint7"); + + traj->points.emplace_back(); + traj->points.at(0).positions = {0, 0, 0, 0, 0, 0}; + traj->points.at(0).velocities = {0, 0, 0, 0, 0, 0}; + traj->points.at(0).accelerations = {0, 0, 0, 0, 0, 0}; + traj->points.at(0).time_from_start.sec = 999999; + + return plan; + }(); + + return out; +} + +void test_motion_plans( + std::shared_ptr move_group, + std::shared_ptr cache) +{ + // Setup ===================================================================== + // All variants are copies. + + /// MotionPlanRequest + + // Plain start + moveit_msgs::msg::MotionPlanRequest plan_req_msg; + move_group->constructMotionPlanRequest(plan_req_msg); + plan_req_msg.workspace_parameters.header.frame_id = g_robot_frame; + plan_req_msg.workspace_parameters.max_corner.x = 10; + plan_req_msg.workspace_parameters.max_corner.y = 10; + plan_req_msg.workspace_parameters.max_corner.z = 10; + plan_req_msg.workspace_parameters.min_corner.x = -10; + plan_req_msg.workspace_parameters.min_corner.y = -10; + plan_req_msg.workspace_parameters.min_corner.z = -10; + plan_req_msg.start_state.multi_dof_joint_state.joint_names.clear(); + plan_req_msg.start_state.multi_dof_joint_state.transforms.clear(); + plan_req_msg.start_state.multi_dof_joint_state.twist.clear(); + plan_req_msg.start_state.multi_dof_joint_state.wrench.clear(); + + // Empty frame start + moveit_msgs::msg::MotionPlanRequest empty_frame_plan_req_msg = plan_req_msg; + empty_frame_plan_req_msg.workspace_parameters.header.frame_id = ""; + + // is_diff = true + auto is_diff_plan_req_msg = plan_req_msg; + is_diff_plan_req_msg.start_state.is_diff = true; + is_diff_plan_req_msg.start_state.joint_state.header.frame_id = ""; + is_diff_plan_req_msg.start_state.joint_state.name.clear(); + is_diff_plan_req_msg.start_state.joint_state.position.clear(); + is_diff_plan_req_msg.start_state.joint_state.velocity.clear(); + is_diff_plan_req_msg.start_state.joint_state.effort.clear(); + + // Something close enough (mod 0.1 away) + auto close_matching_plan_req_msg = plan_req_msg; + close_matching_plan_req_msg.workspace_parameters.max_corner.x += 0.05; + close_matching_plan_req_msg.workspace_parameters.min_corner.x -= 0.05; + close_matching_plan_req_msg.start_state.joint_state.position.at(0) -= 0.05; + close_matching_plan_req_msg.start_state.joint_state.position.at(1) += 0.05; + close_matching_plan_req_msg.start_state.joint_state.position.at(2) -= 0.05; + close_matching_plan_req_msg.goal_constraints + .at(0).joint_constraints.at(0).position -= 0.05; + close_matching_plan_req_msg.goal_constraints + .at(0).joint_constraints.at(1).position += 0.05; + close_matching_plan_req_msg.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_msg = close_matching_plan_req_msg; + std::swap( + swapped_close_matching_plan_req_msg.goal_constraints.at( + 0).joint_constraints.at(0), + swapped_close_matching_plan_req_msg.goal_constraints.at( + 0).joint_constraints.at(1)); + + // Smaller workspace start + auto smaller_workspace_plan_req_msg = plan_req_msg; + smaller_workspace_plan_req_msg.workspace_parameters.max_corner.x = 1; + smaller_workspace_plan_req_msg.workspace_parameters.max_corner.y = 1; + smaller_workspace_plan_req_msg.workspace_parameters.max_corner.z = 1; + smaller_workspace_plan_req_msg.workspace_parameters.min_corner.x = -1; + smaller_workspace_plan_req_msg.workspace_parameters.min_corner.y = -1; + smaller_workspace_plan_req_msg.workspace_parameters.min_corner.z = -1; + + // Larger workspace start + auto larger_workspace_plan_req_msg = plan_req_msg; + larger_workspace_plan_req_msg.workspace_parameters.max_corner.x = 50; + larger_workspace_plan_req_msg.workspace_parameters.max_corner.y = 50; + larger_workspace_plan_req_msg.workspace_parameters.max_corner.z = 50; + larger_workspace_plan_req_msg.workspace_parameters.min_corner.x = -50; + larger_workspace_plan_req_msg.workspace_parameters.min_corner.y = -50; + larger_workspace_plan_req_msg.workspace_parameters.min_corner.z = -50; + + // Different + auto different_plan_req_msg = plan_req_msg; + different_plan_req_msg.workspace_parameters.max_corner.x += 1.05; + different_plan_req_msg.workspace_parameters.min_corner.x -= 2.05; + different_plan_req_msg.start_state.joint_state.position.at(0) -= 3.05; + different_plan_req_msg.start_state.joint_state.position.at(1) += 4.05; + different_plan_req_msg.start_state.joint_state.position.at(2) -= 5.05; + different_plan_req_msg.goal_constraints + .at(0).joint_constraints.at(0).position -= 6.05; + different_plan_req_msg.goal_constraints + .at(0).joint_constraints.at(1).position += 7.05; + different_plan_req_msg.goal_constraints + .at(0).joint_constraints.at(2).position -= 8.05; + + /// RobotTrajectory + + // Plan + auto plan = get_dummy_panda_plan(); + + // Plan with no frame_id in its trajectory header + auto empty_frame_plan = plan; + empty_frame_plan.joint_trajectory.header.frame_id = ""; + + auto different_plan = plan; + different_plan.joint_trajectory.points.at(0).positions.at(0) = 999; + different_plan.joint_trajectory.points.at(0).positions.at(1) = 999; + different_plan.joint_trajectory.points.at(0).positions.at(2) = 999; + + // Test Utils + + std::string prefix; + + // Checks ==================================================================== + + // Initially empty + prefix = "test_motion_plans.empty"; + + check_and_emit( + cache->count_plans(g_robot_name) == 0, + prefix, "Plan cache initially empty"); + + check_and_emit( + cache->fetch_all_matching_plans( + *move_group, g_robot_name, plan_req_msg, 999, 999).empty(), + prefix, "Fetch all plans on empty cache returns empty"); + + check_and_emit( + cache->fetch_best_matching_plan( + *move_group, g_robot_name, plan_req_msg, 999, 999) == nullptr, + prefix, "Fetch best plan on empty cache returns nullptr"); + + // Put plan empty frame + // + // Plan must have frame in joint trajectory + prefix = "test_motion_plans.put_plan_empty_frame"; + double execution_time = 999; + double planning_time = 999; + + check_and_emit( + !cache->put_plan( + *move_group, g_robot_name, plan_req_msg, empty_frame_plan, + execution_time, planning_time, false), + prefix, "Put empty frame plan, no overwrite, not ok"); + + check_and_emit( + cache->count_plans(g_robot_name) == 0, prefix, "No plans in cache"); + + // Put plan req empty frame + // + // Plan request must have frame in workspace + prefix = "test_motion_plans.put_plan_req_empty_frame"; + execution_time = 999; + planning_time = 999; + + check_and_emit( + !cache->put_plan( + *move_group, g_robot_name, empty_frame_plan_req_msg, plan, + execution_time, planning_time, false), + prefix, "Put empty frame req plan, no overwrite, not ok"); + + check_and_emit( + cache->count_plans(g_robot_name) == 0, prefix, "No plans in cache"); + + // Put first, no overwrite + prefix = "test_motion_plans.put_first"; + execution_time = 999; + planning_time = 999; + + check_and_emit( + cache->put_plan( + *move_group, g_robot_name, plan_req_msg, plan, + execution_time, planning_time, false), + prefix, "Put first valid plan, no overwrite, ok"); + + check_and_emit( + cache->count_plans(g_robot_name) == 1, prefix, "One plan in cache"); + + // Fetch matching, no tolerance + // + // Exact key match should have cache hit + prefix = "test_motion_plans.put_first.fetch_matching_no_tolerance"; + + auto fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, plan_req_msg, 0, 0); + + auto fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, plan_req_msg, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan 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_plans.put_first.fetch_is_diff_no_tolerance"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, is_diff_plan_req_msg, 0, 0); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, is_diff_plan_req_msg, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + // Fetch non-matching, out of tolerance + // + // Non-matching key should not have cache hit + prefix = "test_motion_plans.put_first.fetch_non_matching_out_of_tolerance"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, close_matching_plan_req_msg, 0, 0); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, close_matching_plan_req_msg, 0, 0); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + + // Fetch non-matching, in tolerance + // + // Close key within tolerance limit should have cache hit + prefix = "test_motion_plans.put_first.fetch_non_matching_in_tolerance"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, close_matching_plan_req_msg, 0.1, 0.1); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, close_matching_plan_req_msg, 0.1, 0.1); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + // Fetch swapped + // + // Matches should be mostly invariant to constraint ordering + prefix = "test_motion_plans.put_first.fetch_swapped"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, swapped_close_matching_plan_req_msg, 0.1, 0.1); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, swapped_close_matching_plan_req_msg, 0.1, 0.1); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + // Fetch with smaller workspace + // + // Matching plans with more restrictive workspace requirements should not + // pull up plans cached for less restrictive workspace requirements + prefix = "test_motion_plans.put_first.fetch_smaller_workspace"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, smaller_workspace_plan_req_msg, 999, 999); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, smaller_workspace_plan_req_msg, 999, 999); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + + // Fetch with larger workspace + // + // Matching plans with less restrictive workspace requirements should pull up + // plans cached for more restrictive workspace requirements + prefix = "test_motion_plans.put_first.fetch_larger_workspace"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, larger_workspace_plan_req_msg, 999, 999); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, larger_workspace_plan_req_msg, 999, 999); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble( + "workspace_parameters.max_corner.x") + <= larger_workspace_plan_req_msg.workspace_parameters.max_corner.x, + prefix, "Fetched plan has more restrictive workspace max_corner"); + + check_and_emit( + fetched_plan->lookupDouble( + "workspace_parameters.max_corner.x") + >= larger_workspace_plan_req_msg.workspace_parameters.min_corner.x, + prefix, "Fetched plan has more restrictive workspace min_corner"); + + // Put worse, no overwrite + // + // Worse plans should not be inserted + prefix = "test_motion_plans.put_worse"; + double worse_execution_time = execution_time + 100; + + check_and_emit( + !cache->put_plan( + *move_group, g_robot_name, plan_req_msg, plan, + worse_execution_time, planning_time, false), + prefix, "Put worse plan, no overwrite, not ok"); + + check_and_emit( + cache->count_plans(g_robot_name) == 1, prefix, "One plan in cache"); + + // Put better, no overwrite + // + // Better plans should be inserted + prefix = "test_motion_plans.put_better"; + double better_execution_time = execution_time - 100; + + check_and_emit( + cache->put_plan( + *move_group, g_robot_name, plan_req_msg, plan, + better_execution_time, planning_time, false), + prefix, "Put better plan, no overwrite, ok"); + + check_and_emit( + cache->count_plans(g_robot_name) == 2, prefix, "Two plans in cache"); + + // Fetch sorted + // + // With multiple plans in cache, fetches should be sorted accordingly + prefix = "test_motion_plans.put_better.fetch_sorted"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, plan_req_msg, 0, 0); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, plan_req_msg, 0, 0); + + check_and_emit(fetched_plans.size() == 2, prefix, "Fetch all returns two"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == better_execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plans.at(0)->lookupDouble( + "execution_time_s") == better_execution_time + && fetched_plans.at(1)->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plans are sorted correctly"); + + // Put better, overwrite + // + // Better, different, plans should be inserted + prefix = "test_motion_plans.put_better_overwrite"; + double even_better_execution_time = better_execution_time - 100; + + check_and_emit( + cache->put_plan( + *move_group, g_robot_name, plan_req_msg, different_plan, + even_better_execution_time, planning_time, true), + prefix, "Put better plan, overwrite, ok"); + + check_and_emit( + cache->count_plans(g_robot_name) == 1, prefix, "One plan in cache"); + + // Fetch sorted + // + // With multiple plans in cache, fetches should be sorted accordingly + prefix = "test_motion_plans.put_better_overwrite.fetch"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, plan_req_msg, 0, 0); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, plan_req_msg, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == different_plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble( + "execution_time_s") == even_better_execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + // Put different req, overwrite + // + // Unrelated plan requests should live alongside pre-existing plans + prefix = "test_motion_plans.put_different_req"; + + check_and_emit( + cache->put_plan( + *move_group, g_robot_name, different_plan_req_msg, different_plan, + better_execution_time, planning_time, true), + prefix, "Put different plan req, overwrite, ok"); + + check_and_emit( + cache->count_plans(g_robot_name) == 2, prefix, "Two plans in cache"); + + // Fetch with different plan req + // + // With multiple plans in cache, fetches should be sorted accordingly + prefix = "test_motion_plans.put_different_req.fetch"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, different_plan_req_msg, 0, 0); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, different_plan_req_msg, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == different_plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble( + "execution_time_s") == better_execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); +} + +int main(int argc, char** argv) +{ + // Setup + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + + auto test_node = rclcpp::Node::make_shared("test_node", node_options); + std::thread spin_thread( + [&]() + { + while (rclcpp::ok()) + { + rclcpp::spin_some(test_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(test_node, "panda_arm"); + + auto curr_state = move_group->getCurrentState(); + move_group->setStartState(*curr_state); + + test_motion_plans(move_group, cache); + // test_cartesian_plans(move_group, cache); + } + + rclcpp::shutdown(); + spin_thread.join(); + + return 0; +} + +// CARTESIAN PLANS === + +// Check construct plan request +// Remember to check fraction too! \ No newline at end of file diff --git a/nexus_motion_planner/test/test_motion_plan_cache.py b/nexus_motion_planner/test/test_motion_plan_cache.py new file mode 100755 index 0000000..87b99b3 --- /dev/null +++ b/nexus_motion_planner/test/test_motion_plan_cache.py @@ -0,0 +1,282 @@ +# 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"]) # Sadly necessary + .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 motion_cache_test_runner_node(moveit_config): + return Node( + package="nexus_motion_planner", + executable="test_motion_plan_cache", + name="test_motion_plan_cache_node", + output="screen", + cached_output=True, + parameters=[moveit_config.to_dict()] + ) + + +@launch_pytest.fixture +def launch_description(motion_cache_test_runner_node, robot_fixture): + return LaunchDescription( + robot_fixture + + [ + motion_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_read_stdout(motion_cache_test_runner_node, launch_context): + test_cases = [ + # Cache init + ('init', [ + 'Cache init', + ]), + + # Motion plan cache + ('test_motion_plans.empty', [ + 'Plan cache initially empty', + 'Fetch all plans on empty cache returns empty', + 'Fetch best plan on empty cache returns nullptr', + ]), + + ('test_motion_plans.put_plan_empty_frame', [ + 'Put empty frame plan, no overwrite, not ok', + 'No plans in cache', + ]), + + ('test_motion_plans.put_plan_req_empty_frame', [ + 'Put empty frame req plan, no overwrite, not ok', + 'No plans in cache', + ]), + + ('test_motion_plans.put_first', [ + 'Put first valid plan, no overwrite, ok', + 'One plan in cache', + ]), + ('test_motion_plans.put_first.fetch_matching_no_tolerance', [ + 'Fetch all returns one', + 'Fetch best plan is not nullptr', + 'Fetched plan on both fetches match', + 'Fetched plan matches original', + 'Fetched plan has correct execution time', + 'Fetched plan has correct planning time', + ]), + ('test_motion_plans.put_first.fetch_is_diff_no_tolerance', [ + 'Fetch all returns one', + 'Fetch best plan is not nullptr', + 'Fetched plan on both fetches match', + 'Fetched plan matches original', + 'Fetched plan has correct execution time', + 'Fetched plan has correct planning time', + ]), + ('test_motion_plans.put_first.fetch_non_matching_out_of_tolerance', [ + 'Fetch all returns empty', + 'Fetch best plan is nullptr', + ]), + ('test_motion_plans.put_first.fetch_non_matching_in_tolerance', [ + 'Fetch all returns one', + 'Fetch best plan is not nullptr', + 'Fetched plan on both fetches match', + 'Fetched plan matches original', + 'Fetched plan has correct execution time', + 'Fetched plan has correct planning time', + ]), + ('test_motion_plans.put_first.fetch_swapped', [ + 'Fetch all returns one', + 'Fetch best plan is not nullptr', + 'Fetched plan on both fetches match', + 'Fetched plan matches original', + 'Fetched plan has correct execution time', + 'Fetched plan has correct planning time', + ]), + ('test_motion_plans.put_first.fetch_smaller_workspace', [ + 'Fetch all returns empty', + 'Fetch best plan is nullptr', + ]), + ('test_motion_plans.put_first.fetch_larger_workspace', [ + 'Fetch all returns one', + 'Fetch best plan is not nullptr', + 'Fetched plan on both fetches match', + 'Fetched plan matches original', + 'Fetched plan has correct execution time', + 'Fetched plan has correct planning time', + 'Fetched plan has more restrictive workspace max_corner', + 'Fetched plan has more restrictive workspace min_corner', + ]), + + ('test_motion_plans.put_worse', [ + 'Put worse plan, no overwrite, not ok', + 'One plan in cache', + ]), + + ('test_motion_plans.put_better', [ + 'Put better plan, no overwrite, ok', + 'Two plans in cache', + ]), + ('test_motion_plans.put_better.fetch_sorted', [ + 'Fetch all returns two', + 'Fetch best plan is not nullptr', + 'Fetched plan on both fetches match', + 'Fetched plan matches original', + 'Fetched plan has correct execution time', + 'Fetched plan has correct planning time', + 'Fetched plans are sorted correctly', + ]), + + ('test_motion_plans.put_better_overwrite', [ + 'Put better plan, overwrite, ok', + 'One plan in cache', + ]), + ('test_motion_plans.put_better_overwrite.fetch', [ + 'Fetch all returns one', + 'Fetch best plan is not nullptr', + 'Fetched plan on both fetches match', + 'Fetched plan matches original', + 'Fetched plan has correct execution time', + 'Fetched plan has correct planning time', + ]), + + ('test_motion_plans.put_different_req', [ + 'Put different plan req, overwrite, ok', + 'Two plans in cache', + ]), + ('test_motion_plans.put_different_req.fetch', [ + 'Fetch all returns one', + 'Fetch best plan is not nullptr', + 'Fetched plan on both fetches match', + 'Fetched plan matches original', + 'Fetched plan has correct execution time', + 'Fetched plan has correct planning time', + ]), + + ] + + for prefix, labels in test_cases: + for label in labels: + process_tools.assert_output_sync( + launch_context, + motion_cache_test_runner_node, + validate_stream(f"[PASS] {prefix}: {label}"), + timeout=10 + ) + + # Check no occurrences of [FAIL] in output + assert not process_tools.wait_for_output_sync( + launch_context, + motion_cache_test_runner_node, + lambda x: "[FAIL]" in x, + timeout=10 + ) + + # Wait for process to end and check for graceful exit + yield + assert motion_cache_test_runner_node.return_code == 0 From c461fd7086c77981608f90df216cf6d16ca8328a Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 22 Nov 2023 19:51:41 -0800 Subject: [PATCH 05/13] Fix bugs in cartesian caching Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index ce3f1f3..bee939f 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -1075,7 +1075,7 @@ MotionPlanCache::fetch_best_matching_cartesian_plan( } auto coll = db_->openCollection( - "move_group_plan_cache", move_group_namespace); + "move_group_cartesian_plan_cache", move_group_namespace); // Best plan is at first index, since the lookup query was sorted by // execution_time. @@ -1302,10 +1302,10 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query( 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()) + 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."); @@ -1387,10 +1387,10 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query( 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); + 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); + z_offset + waypoint.position.z, match_tolerance); // Orientation tf2::Quaternion tf2_quat_goal_offset( @@ -1414,6 +1414,7 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query( } query.append("link_name", plan_request.link_name); + query.append("header.frame_id", base_frame); return true; } @@ -1509,10 +1510,10 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_metadata( 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()) + 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."); From bcbb978c0da3c19d7e2020e1551465030c07cd91 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 22 Nov 2023 20:10:09 -0800 Subject: [PATCH 06/13] Add tests for cartesian plan cache Signed-off-by: methylDragon --- .../src/test_motion_plan_cache.cpp | 734 +++++++++++++++--- .../test/test_motion_plan_cache.py | 134 +++- 2 files changed, 751 insertions(+), 117 deletions(-) diff --git a/nexus_motion_planner/src/test_motion_plan_cache.cpp b/nexus_motion_planner/src/test_motion_plan_cache.cpp index e554621..3650e5a 100644 --- a/nexus_motion_planner/src/test_motion_plan_cache.cpp +++ b/nexus_motion_planner/src/test_motion_plan_cache.cpp @@ -66,10 +66,31 @@ moveit_msgs::msg::RobotTrajectory get_dummy_panda_plan() return plan; }(); + 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_plans( std::shared_ptr move_group, std::shared_ptr cache) @@ -80,85 +101,85 @@ void test_motion_plans( /// MotionPlanRequest // Plain start - moveit_msgs::msg::MotionPlanRequest plan_req_msg; - move_group->constructMotionPlanRequest(plan_req_msg); - plan_req_msg.workspace_parameters.header.frame_id = g_robot_frame; - plan_req_msg.workspace_parameters.max_corner.x = 10; - plan_req_msg.workspace_parameters.max_corner.y = 10; - plan_req_msg.workspace_parameters.max_corner.z = 10; - plan_req_msg.workspace_parameters.min_corner.x = -10; - plan_req_msg.workspace_parameters.min_corner.y = -10; - plan_req_msg.workspace_parameters.min_corner.z = -10; - plan_req_msg.start_state.multi_dof_joint_state.joint_names.clear(); - plan_req_msg.start_state.multi_dof_joint_state.transforms.clear(); - plan_req_msg.start_state.multi_dof_joint_state.twist.clear(); - plan_req_msg.start_state.multi_dof_joint_state.wrench.clear(); + 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_msg = plan_req_msg; - empty_frame_plan_req_msg.workspace_parameters.header.frame_id = ""; + 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_msg = plan_req_msg; - is_diff_plan_req_msg.start_state.is_diff = true; - is_diff_plan_req_msg.start_state.joint_state.header.frame_id = ""; - is_diff_plan_req_msg.start_state.joint_state.name.clear(); - is_diff_plan_req_msg.start_state.joint_state.position.clear(); - is_diff_plan_req_msg.start_state.joint_state.velocity.clear(); - is_diff_plan_req_msg.start_state.joint_state.effort.clear(); + 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_msg = plan_req_msg; - close_matching_plan_req_msg.workspace_parameters.max_corner.x += 0.05; - close_matching_plan_req_msg.workspace_parameters.min_corner.x -= 0.05; - close_matching_plan_req_msg.start_state.joint_state.position.at(0) -= 0.05; - close_matching_plan_req_msg.start_state.joint_state.position.at(1) += 0.05; - close_matching_plan_req_msg.start_state.joint_state.position.at(2) -= 0.05; - close_matching_plan_req_msg.goal_constraints + 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_msg.goal_constraints + close_matching_plan_req.goal_constraints .at(0).joint_constraints.at(1).position += 0.05; - close_matching_plan_req_msg.goal_constraints + 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_msg = close_matching_plan_req_msg; + auto swapped_close_matching_plan_req = close_matching_plan_req; std::swap( - swapped_close_matching_plan_req_msg.goal_constraints.at( + swapped_close_matching_plan_req.goal_constraints.at( 0).joint_constraints.at(0), - swapped_close_matching_plan_req_msg.goal_constraints.at( + swapped_close_matching_plan_req.goal_constraints.at( 0).joint_constraints.at(1)); // Smaller workspace start - auto smaller_workspace_plan_req_msg = plan_req_msg; - smaller_workspace_plan_req_msg.workspace_parameters.max_corner.x = 1; - smaller_workspace_plan_req_msg.workspace_parameters.max_corner.y = 1; - smaller_workspace_plan_req_msg.workspace_parameters.max_corner.z = 1; - smaller_workspace_plan_req_msg.workspace_parameters.min_corner.x = -1; - smaller_workspace_plan_req_msg.workspace_parameters.min_corner.y = -1; - smaller_workspace_plan_req_msg.workspace_parameters.min_corner.z = -1; + 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_msg = plan_req_msg; - larger_workspace_plan_req_msg.workspace_parameters.max_corner.x = 50; - larger_workspace_plan_req_msg.workspace_parameters.max_corner.y = 50; - larger_workspace_plan_req_msg.workspace_parameters.max_corner.z = 50; - larger_workspace_plan_req_msg.workspace_parameters.min_corner.x = -50; - larger_workspace_plan_req_msg.workspace_parameters.min_corner.y = -50; - larger_workspace_plan_req_msg.workspace_parameters.min_corner.z = -50; + 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_msg = plan_req_msg; - different_plan_req_msg.workspace_parameters.max_corner.x += 1.05; - different_plan_req_msg.workspace_parameters.min_corner.x -= 2.05; - different_plan_req_msg.start_state.joint_state.position.at(0) -= 3.05; - different_plan_req_msg.start_state.joint_state.position.at(1) += 4.05; - different_plan_req_msg.start_state.joint_state.position.at(2) -= 5.05; - different_plan_req_msg.goal_constraints + 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_msg.goal_constraints + different_plan_req.goal_constraints .at(0).joint_constraints.at(1).position += 7.05; - different_plan_req_msg.goal_constraints + different_plan_req.goal_constraints .at(0).joint_constraints.at(2).position -= 8.05; /// RobotTrajectory @@ -190,56 +211,56 @@ void test_motion_plans( check_and_emit( cache->fetch_all_matching_plans( - *move_group, g_robot_name, plan_req_msg, 999, 999).empty(), + *move_group, g_robot_name, plan_req, 999, 999).empty(), prefix, "Fetch all plans on empty cache returns empty"); check_and_emit( cache->fetch_best_matching_plan( - *move_group, g_robot_name, plan_req_msg, 999, 999) == nullptr, + *move_group, g_robot_name, plan_req, 999, 999) == nullptr, prefix, "Fetch best plan on empty cache returns nullptr"); // Put plan empty frame // - // Plan must have frame in joint trajectory + // Plan must have frame in joint trajectory, expect put fail prefix = "test_motion_plans.put_plan_empty_frame"; double execution_time = 999; double planning_time = 999; check_and_emit( !cache->put_plan( - *move_group, g_robot_name, plan_req_msg, empty_frame_plan, + *move_group, g_robot_name, plan_req, empty_frame_plan, execution_time, planning_time, false), - prefix, "Put empty frame plan, no overwrite, not ok"); + prefix, "Put empty frame plan, no delete_worse_plans, not ok"); check_and_emit( cache->count_plans(g_robot_name) == 0, prefix, "No plans in cache"); // Put plan req empty frame // - // Plan request must have frame in workspace + // Plan request must have frame in workspace, expect put fail prefix = "test_motion_plans.put_plan_req_empty_frame"; execution_time = 999; planning_time = 999; check_and_emit( !cache->put_plan( - *move_group, g_robot_name, empty_frame_plan_req_msg, plan, + *move_group, g_robot_name, empty_frame_plan_req, plan, execution_time, planning_time, false), - prefix, "Put empty frame req plan, no overwrite, not ok"); + prefix, "Put empty frame req plan, no delete_worse_plans, not ok"); check_and_emit( cache->count_plans(g_robot_name) == 0, prefix, "No plans in cache"); - // Put first, no overwrite + // Put first, no delete_worse_plans prefix = "test_motion_plans.put_first"; execution_time = 999; planning_time = 999; check_and_emit( cache->put_plan( - *move_group, g_robot_name, plan_req_msg, plan, + *move_group, g_robot_name, plan_req, plan, execution_time, planning_time, false), - prefix, "Put first valid plan, no overwrite, ok"); + prefix, "Put first valid plan, no delete_worse_plans, ok"); check_and_emit( cache->count_plans(g_robot_name) == 1, prefix, "One plan in cache"); @@ -250,10 +271,10 @@ void test_motion_plans( prefix = "test_motion_plans.put_first.fetch_matching_no_tolerance"; auto fetched_plans = cache->fetch_all_matching_plans( - *move_group, g_robot_name, plan_req_msg, 0, 0); + *move_group, g_robot_name, plan_req, 0, 0); auto fetched_plan = cache->fetch_best_matching_plan( - *move_group, g_robot_name, plan_req_msg, 0, 0); + *move_group, g_robot_name, plan_req, 0, 0); check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); check_and_emit( @@ -281,10 +302,10 @@ void test_motion_plans( prefix = "test_motion_plans.put_first.fetch_is_diff_no_tolerance"; fetched_plans = cache->fetch_all_matching_plans( - *move_group, g_robot_name, is_diff_plan_req_msg, 0, 0); + *move_group, g_robot_name, is_diff_plan_req, 0, 0); fetched_plan = cache->fetch_best_matching_plan( - *move_group, g_robot_name, is_diff_plan_req_msg, 0, 0); + *move_group, g_robot_name, is_diff_plan_req, 0, 0); check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); check_and_emit( @@ -311,10 +332,10 @@ void test_motion_plans( prefix = "test_motion_plans.put_first.fetch_non_matching_out_of_tolerance"; fetched_plans = cache->fetch_all_matching_plans( - *move_group, g_robot_name, close_matching_plan_req_msg, 0, 0); + *move_group, g_robot_name, close_matching_plan_req, 0, 0); fetched_plan = cache->fetch_best_matching_plan( - *move_group, g_robot_name, close_matching_plan_req_msg, 0, 0); + *move_group, g_robot_name, close_matching_plan_req, 0, 0); check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); check_and_emit( @@ -326,10 +347,10 @@ void test_motion_plans( prefix = "test_motion_plans.put_first.fetch_non_matching_in_tolerance"; fetched_plans = cache->fetch_all_matching_plans( - *move_group, g_robot_name, close_matching_plan_req_msg, 0.1, 0.1); + *move_group, g_robot_name, close_matching_plan_req, 0.1, 0.1); fetched_plan = cache->fetch_best_matching_plan( - *move_group, g_robot_name, close_matching_plan_req_msg, 0.1, 0.1); + *move_group, g_robot_name, close_matching_plan_req, 0.1, 0.1); check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); check_and_emit( @@ -356,10 +377,10 @@ void test_motion_plans( prefix = "test_motion_plans.put_first.fetch_swapped"; fetched_plans = cache->fetch_all_matching_plans( - *move_group, g_robot_name, swapped_close_matching_plan_req_msg, 0.1, 0.1); + *move_group, g_robot_name, swapped_close_matching_plan_req, 0.1, 0.1); fetched_plan = cache->fetch_best_matching_plan( - *move_group, g_robot_name, swapped_close_matching_plan_req_msg, 0.1, 0.1); + *move_group, g_robot_name, swapped_close_matching_plan_req, 0.1, 0.1); check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); check_and_emit( @@ -387,10 +408,10 @@ void test_motion_plans( prefix = "test_motion_plans.put_first.fetch_smaller_workspace"; fetched_plans = cache->fetch_all_matching_plans( - *move_group, g_robot_name, smaller_workspace_plan_req_msg, 999, 999); + *move_group, g_robot_name, smaller_workspace_plan_req, 999, 999); fetched_plan = cache->fetch_best_matching_plan( - *move_group, g_robot_name, smaller_workspace_plan_req_msg, 999, 999); + *move_group, g_robot_name, smaller_workspace_plan_req, 999, 999); check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); check_and_emit( @@ -403,10 +424,10 @@ void test_motion_plans( prefix = "test_motion_plans.put_first.fetch_larger_workspace"; fetched_plans = cache->fetch_all_matching_plans( - *move_group, g_robot_name, larger_workspace_plan_req_msg, 999, 999); + *move_group, g_robot_name, larger_workspace_plan_req, 999, 999); fetched_plan = cache->fetch_best_matching_plan( - *move_group, g_robot_name, larger_workspace_plan_req_msg, 999, 999); + *move_group, g_robot_name, larger_workspace_plan_req, 999, 999); check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); check_and_emit( @@ -430,16 +451,16 @@ void test_motion_plans( check_and_emit( fetched_plan->lookupDouble( "workspace_parameters.max_corner.x") - <= larger_workspace_plan_req_msg.workspace_parameters.max_corner.x, + <= larger_workspace_plan_req.workspace_parameters.max_corner.x, prefix, "Fetched plan has more restrictive workspace max_corner"); check_and_emit( fetched_plan->lookupDouble( "workspace_parameters.max_corner.x") - >= larger_workspace_plan_req_msg.workspace_parameters.min_corner.x, + >= larger_workspace_plan_req.workspace_parameters.min_corner.x, prefix, "Fetched plan has more restrictive workspace min_corner"); - // Put worse, no overwrite + // Put worse, no delete_worse_plans // // Worse plans should not be inserted prefix = "test_motion_plans.put_worse"; @@ -447,14 +468,14 @@ void test_motion_plans( check_and_emit( !cache->put_plan( - *move_group, g_robot_name, plan_req_msg, plan, + *move_group, g_robot_name, plan_req, plan, worse_execution_time, planning_time, false), - prefix, "Put worse plan, no overwrite, not ok"); + prefix, "Put worse plan, no delete_worse_plans, not ok"); check_and_emit( cache->count_plans(g_robot_name) == 1, prefix, "One plan in cache"); - // Put better, no overwrite + // Put better, no delete_worse_plans // // Better plans should be inserted prefix = "test_motion_plans.put_better"; @@ -462,9 +483,9 @@ void test_motion_plans( check_and_emit( cache->put_plan( - *move_group, g_robot_name, plan_req_msg, plan, + *move_group, g_robot_name, plan_req, plan, better_execution_time, planning_time, false), - prefix, "Put better plan, no overwrite, ok"); + prefix, "Put better plan, no delete_worse_plans, ok"); check_and_emit( cache->count_plans(g_robot_name) == 2, prefix, "Two plans in cache"); @@ -475,10 +496,10 @@ void test_motion_plans( prefix = "test_motion_plans.put_better.fetch_sorted"; fetched_plans = cache->fetch_all_matching_plans( - *move_group, g_robot_name, plan_req_msg, 0, 0); + *move_group, g_robot_name, plan_req, 0, 0); fetched_plan = cache->fetch_best_matching_plan( - *move_group, g_robot_name, plan_req_msg, 0, 0); + *move_group, g_robot_name, plan_req, 0, 0); check_and_emit(fetched_plans.size() == 2, prefix, "Fetch all returns two"); check_and_emit( @@ -505,31 +526,29 @@ void test_motion_plans( && fetched_plans.at(1)->lookupDouble("execution_time_s") == execution_time, prefix, "Fetched plans are sorted correctly"); - // Put better, overwrite + // Put better, delete_worse_plans // // Better, different, plans should be inserted - prefix = "test_motion_plans.put_better_overwrite"; + prefix = "test_motion_plans.put_better_delete_worse_plans"; double even_better_execution_time = better_execution_time - 100; check_and_emit( cache->put_plan( - *move_group, g_robot_name, plan_req_msg, different_plan, + *move_group, g_robot_name, plan_req, different_plan, even_better_execution_time, planning_time, true), - prefix, "Put better plan, overwrite, ok"); + prefix, "Put better plan, delete_worse_plans, ok"); check_and_emit( cache->count_plans(g_robot_name) == 1, prefix, "One plan in cache"); - // Fetch sorted - // - // With multiple plans in cache, fetches should be sorted accordingly - prefix = "test_motion_plans.put_better_overwrite.fetch"; + // Fetch better plan + prefix = "test_motion_plans.put_better_delete_worse_plans.fetch"; fetched_plans = cache->fetch_all_matching_plans( - *move_group, g_robot_name, plan_req_msg, 0, 0); + *move_group, g_robot_name, plan_req, 0, 0); fetched_plan = cache->fetch_best_matching_plan( - *move_group, g_robot_name, plan_req_msg, 0, 0); + *move_group, g_robot_name, plan_req, 0, 0); check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); check_and_emit( @@ -551,16 +570,16 @@ void test_motion_plans( fetched_plan->lookupDouble("planning_time_s") == planning_time, prefix, "Fetched plan has correct planning time"); - // Put different req, overwrite + // Put different req, delete_worse_plans // // Unrelated plan requests should live alongside pre-existing plans prefix = "test_motion_plans.put_different_req"; check_and_emit( cache->put_plan( - *move_group, g_robot_name, different_plan_req_msg, different_plan, + *move_group, g_robot_name, different_plan_req, different_plan, better_execution_time, planning_time, true), - prefix, "Put different plan req, overwrite, ok"); + prefix, "Put different plan req, delete_worse_plans, ok"); check_and_emit( cache->count_plans(g_robot_name) == 2, prefix, "Two plans in cache"); @@ -571,10 +590,505 @@ void test_motion_plans( prefix = "test_motion_plans.put_different_req.fetch"; fetched_plans = cache->fetch_all_matching_plans( - *move_group, g_robot_name, different_plan_req_msg, 0, 0); + *move_group, g_robot_name, different_plan_req, 0, 0); fetched_plan = cache->fetch_best_matching_plan( - *move_group, g_robot_name, different_plan_req_msg, 0, 0); + *move_group, g_robot_name, different_plan_req, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == different_plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble( + "execution_time_s") == better_execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); +} + +void test_cartesian_plans( + std::shared_ptr move_group, + std::shared_ptr cache) +{ + std::string prefix; + + /// First, test if construction even works... + + // Construct get cartesian plan request + prefix = "test_cartesian_plans.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_plan_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_plan_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.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.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 + + // Plan + auto plan = get_dummy_panda_plan(); + + // Plan with no frame_id in its trajectory header + auto empty_frame_plan = plan; + empty_frame_plan.joint_trajectory.header.frame_id = ""; + + auto different_plan = plan; + different_plan.joint_trajectory.points.at(0).positions.at(0) = 999; + different_plan.joint_trajectory.points.at(0).positions.at(1) = 999; + different_plan.joint_trajectory.points.at(0).positions.at(2) = 999; + + // Checks ==================================================================== + + // Initially empty + prefix = "test_cartesian_plans.empty"; + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 0, + prefix, "Plan cache initially empty"); + + check_and_emit( + cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, cartesian_plan_req, 0, 999, 999).empty(), + prefix, "Fetch all plans on empty cache returns empty"); + + check_and_emit( + cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, 0, 999, 999) == nullptr, + prefix, "Fetch best plan on empty cache returns nullptr"); + + // Put plan empty frame + // + // Plan must have frame in joint trajectory, expect put fail + prefix = "test_cartesian_plans.put_plan_empty_frame"; + double execution_time = 999; + double planning_time = 999; + double fraction = 0.5; + + check_and_emit( + !cache->put_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, empty_frame_plan, + execution_time, planning_time, fraction, false), + prefix, "Put empty frame plan, no delete_worse_plans, not ok"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 0, + prefix, "No plans in cache"); + + // Put plan req empty frame + // + // Plan request must have frame in workspace, expect put fail + prefix = "test_cartesian_plans.put_plan_req_empty_frame"; + execution_time = 999; + planning_time = 999; + + check_and_emit( + !cache->put_cartesian_plan( + *move_group, g_robot_name, empty_frame_cartesian_plan_req, plan, + execution_time, planning_time, fraction, false), + prefix, "Put empty frame req plan, no delete_worse_plans, not ok"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 0, + prefix, "No plans in cache"); + + // Put first, no delete_worse_plans + prefix = "test_cartesian_plans.put_first"; + execution_time = 999; + planning_time = 999; + + check_and_emit( + cache->put_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, plan, + execution_time, planning_time, fraction, false), + prefix, "Put first valid plan, no delete_worse_plans, ok"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 1, + prefix, "One plan in cache"); + + // Fetch matching, no tolerance + // + // Exact key match should have cache hit + prefix = "test_cartesian_plans.put_first.fetch_matching_no_tolerance"; + + auto fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + auto fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble("fraction") == fraction, + prefix, "Fetched plan 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_plans.put_first.fetch_is_diff_no_tolerance"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, is_diff_cartesian_plan_req, fraction, 0, 0); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, is_diff_cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble("fraction") == fraction, + prefix, "Fetched plan has correct fraction"); + + // Fetch non-matching, out of tolerance + // + // Non-matching key should not have cache hit + prefix = "test_cartesian_plans.put_first.fetch_non_matching_out_of_tolerance"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 0, 0); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 0, 0); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + + // Fetch non-matching, in tolerance + // + // Close key within tolerance limit should have cache hit + prefix = "test_cartesian_plans.put_first.fetch_non_matching_in_tolerance"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 0.1, 0.1); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 0.1, 0.1); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble("fraction") == fraction, + prefix, "Fetched plan has correct fraction"); + + // Fetch with higher fraction + // + // Matching plans with more restrictive fraction requirements should not + // pull up plans cached for less restrictive fraction requirements + prefix = "test_cartesian_plans.put_first.fetch_higher_fraction"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, cartesian_plan_req, 1, 999, 999); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, 1, 999, 999); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + + // Fetch with lower fraction + // + // Matching plans with less restrictive fraction requirements should pull up + // plans cached for more restrictive fraction requirements + prefix = "test_cartesian_plans.put_first.fetch_lower_fraction"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, cartesian_plan_req, 0, 999, 999); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, 0, 999, 999); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble("fraction") == fraction, + prefix, "Fetched plan has correct fraction"); + + // Put worse, no delete_worse_plans + // + // Worse plans should not be inserted + prefix = "test_cartesian_plans.put_worse"; + double worse_execution_time = execution_time + 100; + + check_and_emit( + !cache->put_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, plan, + worse_execution_time, planning_time, fraction, false), + prefix, "Put worse plan, no delete_worse_plans, not ok"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 1, + prefix, "One plan in cache"); + + // Put better, no delete_worse_plans + // + // Better plans should be inserted + prefix = "test_cartesian_plans.put_better"; + double better_execution_time = execution_time - 100; + + check_and_emit( + cache->put_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, plan, + better_execution_time, planning_time, fraction, false), + prefix, "Put better plan, no delete_worse_plans, ok"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 2, + prefix, "Two plans in cache"); + + // Fetch sorted + // + // With multiple plans in cache, fetches should be sorted accordingly + prefix = "test_cartesian_plans.put_better.fetch_sorted"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_plans.size() == 2, prefix, "Fetch all returns two"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == better_execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble("fraction") == fraction, + prefix, "Fetched plan has correct fraction"); + + check_and_emit( + fetched_plans.at(0)->lookupDouble( + "execution_time_s") == better_execution_time + && fetched_plans.at(1)->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plans are sorted correctly"); + + // Put better, delete_worse_plans + // + // Better, different, plans should be inserted + prefix = "test_cartesian_plans.put_better_delete_worse_plans"; + double even_better_execution_time = better_execution_time - 100; + + check_and_emit( + cache->put_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, different_plan, + even_better_execution_time, planning_time, fraction, true), + prefix, "Put better plan, delete_worse_plans, ok"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 1, + prefix, "One plan in cache"); + + // Fetch better plan + prefix = "test_cartesian_plans.put_better_delete_worse_plans.fetch"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == different_plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble( + "execution_time_s") == even_better_execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble("fraction") == fraction, + prefix, "Fetched plan has correct fraction"); + + // Put different req, delete_worse_plans + // + // Unrelated plan requests should live alongside pre-existing plans + prefix = "test_cartesian_plans.put_different_req"; + + check_and_emit( + cache->put_cartesian_plan( + *move_group, g_robot_name, different_cartesian_plan_req, + different_plan, + better_execution_time, planning_time, fraction, true), + prefix, "Put different plan req, delete_worse_plans, ok"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 2, + prefix, "Two plans in cache"); + + // Fetch with different plan req + // + // With multiple plans in cache, fetches should be sorted accordingly + prefix = "test_cartesian_plans.put_different_req.fetch"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, different_cartesian_plan_req, fraction, 0, 0); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, different_cartesian_plan_req, fraction, 0, 0); check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); check_and_emit( @@ -595,6 +1109,10 @@ void test_motion_plans( check_and_emit( fetched_plan->lookupDouble("planning_time_s") == planning_time, prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble("fraction") == fraction, + prefix, "Fetched plan has correct fraction"); } int main(int argc, char** argv) @@ -631,7 +1149,7 @@ int main(int argc, char** argv) move_group->setStartState(*curr_state); test_motion_plans(move_group, cache); - // test_cartesian_plans(move_group, cache); + test_cartesian_plans(move_group, cache); } rclcpp::shutdown(); diff --git a/nexus_motion_planner/test/test_motion_plan_cache.py b/nexus_motion_planner/test/test_motion_plan_cache.py index 87b99b3..c4ceb41 100755 --- a/nexus_motion_planner/test/test_motion_plan_cache.py +++ b/nexus_motion_planner/test/test_motion_plan_cache.py @@ -149,17 +149,17 @@ def test_read_stdout(motion_cache_test_runner_node, launch_context): ]), ('test_motion_plans.put_plan_empty_frame', [ - 'Put empty frame plan, no overwrite, not ok', + 'Put empty frame plan, no delete_worse_plans, not ok', 'No plans in cache', ]), ('test_motion_plans.put_plan_req_empty_frame', [ - 'Put empty frame req plan, no overwrite, not ok', + 'Put empty frame req plan, no delete_worse_plans, not ok', 'No plans in cache', ]), ('test_motion_plans.put_first', [ - 'Put first valid plan, no overwrite, ok', + 'Put first valid plan, no delete_worse_plans, ok', 'One plan in cache', ]), ('test_motion_plans.put_first.fetch_matching_no_tolerance', [ @@ -214,12 +214,12 @@ def test_read_stdout(motion_cache_test_runner_node, launch_context): ]), ('test_motion_plans.put_worse', [ - 'Put worse plan, no overwrite, not ok', + 'Put worse plan, no delete_worse_plans, not ok', 'One plan in cache', ]), ('test_motion_plans.put_better', [ - 'Put better plan, no overwrite, ok', + 'Put better plan, no delete_worse_plans, ok', 'Two plans in cache', ]), ('test_motion_plans.put_better.fetch_sorted', [ @@ -232,11 +232,11 @@ def test_read_stdout(motion_cache_test_runner_node, launch_context): 'Fetched plans are sorted correctly', ]), - ('test_motion_plans.put_better_overwrite', [ - 'Put better plan, overwrite, ok', + ('test_motion_plans.put_better_delete_worse_plans', [ + 'Put better plan, delete_worse_plans, ok', 'One plan in cache', ]), - ('test_motion_plans.put_better_overwrite.fetch', [ + ('test_motion_plans.put_better_delete_worse_plans.fetch', [ 'Fetch all returns one', 'Fetch best plan is not nullptr', 'Fetched plan on both fetches match', @@ -246,7 +246,7 @@ def test_read_stdout(motion_cache_test_runner_node, launch_context): ]), ('test_motion_plans.put_different_req', [ - 'Put different plan req, overwrite, ok', + 'Put different plan req, delete_worse_plans, ok', 'Two plans in cache', ]), ('test_motion_plans.put_different_req.fetch', [ @@ -258,6 +258,122 @@ def test_read_stdout(motion_cache_test_runner_node, launch_context): 'Fetched plan has correct planning time', ]), + # Cartesian plan cache + ('test_cartesian_plans.construct_get_cartesian_path_request', [ + 'Ok', + ]), + + ('test_cartesian_plans.empty', [ + 'Plan cache initially empty', + 'Fetch all plans on empty cache returns empty', + 'Fetch best plan on empty cache returns nullptr', + ]), + + ('test_cartesian_plans.put_plan_empty_frame', [ + 'Put empty frame plan, no delete_worse_plans, not ok', + 'No plans in cache', + ]), + + ('test_cartesian_plans.put_plan_req_empty_frame', [ + 'Put empty frame req plan, no delete_worse_plans, not ok', + 'No plans in cache', + ]), + + ('test_cartesian_plans.put_first', [ + 'Put first valid plan, no delete_worse_plans, ok', + 'One plan in cache', + ]), + ('test_cartesian_plans.put_first.fetch_matching_no_tolerance', [ + 'Fetch all returns one', + 'Fetch best plan is not nullptr', + 'Fetched plan on both fetches match', + 'Fetched plan matches original', + 'Fetched plan has correct execution time', + 'Fetched plan has correct planning time', + 'Fetched plan has correct fraction', + ]), + ('test_cartesian_plans.put_first.fetch_is_diff_no_tolerance', [ + 'Fetch all returns one', + 'Fetch best plan is not nullptr', + 'Fetched plan on both fetches match', + 'Fetched plan matches original', + 'Fetched plan has correct execution time', + 'Fetched plan has correct planning time', + 'Fetched plan has correct fraction', + ]), + ('test_cartesian_plans.put_first.fetch_non_matching_out_of_tolerance', [ + 'Fetch all returns empty', + 'Fetch best plan is nullptr', + ]), + ('test_cartesian_plans.put_first.fetch_non_matching_in_tolerance', [ + 'Fetch all returns one', + 'Fetch best plan is not nullptr', + 'Fetched plan on both fetches match', + 'Fetched plan matches original', + 'Fetched plan has correct execution time', + 'Fetched plan has correct planning time', + 'Fetched plan has correct fraction', + ]), + ('test_cartesian_plans.put_first.fetch_higher_fraction', [ + 'Fetch all returns empty', + 'Fetch best plan is nullptr', + ]), + ('test_cartesian_plans.put_first.fetch_lower_fraction', [ + 'Fetch all returns one', + 'Fetch best plan is not nullptr', + 'Fetched plan on both fetches match', + 'Fetched plan matches original', + 'Fetched plan has correct execution time', + 'Fetched plan has correct planning time', + 'Fetched plan has correct fraction', + ]), + + ('test_cartesian_plans.put_worse', [ + 'Put worse plan, no delete_worse_plans, not ok', + 'One plan in cache', + ]), + + ('test_cartesian_plans.put_better', [ + 'Put better plan, no delete_worse_plans, ok', + 'Two plans in cache', + ]), + ('test_cartesian_plans.put_better.fetch_sorted', [ + 'Fetch all returns two', + 'Fetch best plan is not nullptr', + 'Fetched plan on both fetches match', + 'Fetched plan matches original', + 'Fetched plan has correct execution time', + 'Fetched plan has correct planning time', + 'Fetched plan has correct fraction', + 'Fetched plans are sorted correctly', + ]), + ('test_cartesian_plans.put_better_delete_worse_plans', [ + 'Put better plan, delete_worse_plans, ok', + 'One plan in cache', + ]), + ('test_cartesian_plans.put_better_delete_worse_plans.fetch', [ + 'Fetch all returns one', + 'Fetch best plan is not nullptr', + 'Fetched plan on both fetches match', + 'Fetched plan matches original', + 'Fetched plan has correct execution time', + 'Fetched plan has correct planning time', + 'Fetched plan has correct fraction', + ]), + + ('test_cartesian_plans.put_different_req', [ + 'Put different plan req, delete_worse_plans, ok', + 'Two plans in cache', + ]), + ('test_cartesian_plans.put_different_req.fetch', [ + 'Fetch all returns one', + 'Fetch best plan is not nullptr', + 'Fetched plan on both fetches match', + 'Fetched plan matches original', + 'Fetched plan has correct execution time', + 'Fetched plan has correct planning time', + 'Fetched plan has correct fraction', + ]), ] for prefix, labels in test_cases: From a06c57815925a4911231b86974f896b6e03928db Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 22 Nov 2023 20:36:58 -0800 Subject: [PATCH 07/13] Exit if a test fails Signed-off-by: methylDragon --- nexus_motion_planner/src/test_motion_plan_cache.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nexus_motion_planner/src/test_motion_plan_cache.cpp b/nexus_motion_planner/src/test_motion_plan_cache.cpp index 3650e5a..d7fb667 100644 --- a/nexus_motion_planner/src/test_motion_plan_cache.cpp +++ b/nexus_motion_planner/src/test_motion_plan_cache.cpp @@ -38,6 +38,7 @@ void check_and_emit( else { std::cout << "[FAIL] " << prefix << ": " << label << std::endl; + exit(1); } } From 3e2bb014750b8a350cb747804d6f43fae188d814 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 22 Nov 2023 20:38:12 -0800 Subject: [PATCH 08/13] Remove gtest import Signed-off-by: methylDragon --- nexus_motion_planner/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/nexus_motion_planner/CMakeLists.txt b/nexus_motion_planner/CMakeLists.txt index af87314..bcb51ee 100644 --- a/nexus_motion_planner/CMakeLists.txt +++ b/nexus_motion_planner/CMakeLists.txt @@ -77,7 +77,6 @@ install( ) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) find_package(ament_cmake_uncrustify REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) From bf53bf7a4906794ff48fd6a2d05ecd39d4191fa3 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Sat, 25 Nov 2023 19:21:51 -0800 Subject: [PATCH 09/13] Remove enable_shared_from_this Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_plan_cache.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index dcf7ed1..fc6143c 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -78,7 +78,7 @@ namespace motion_planner { * * Motion plans may be looked up with some tolerance at call time. */ -class MotionPlanCache : public std::enable_shared_from_this +class MotionPlanCache { public: // We explicitly need a Node::SharedPtr because warehouse_ros ONLY supports From e23f2022bff0008267e0c3daed60ec1417c802b4 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Sat, 25 Nov 2023 19:45:33 -0800 Subject: [PATCH 10/13] Only check for failure Signed-off-by: methylDragon --- .../test/test_motion_plan_cache.py | 252 +----------------- 1 file changed, 1 insertion(+), 251 deletions(-) diff --git a/nexus_motion_planner/test/test_motion_plan_cache.py b/nexus_motion_planner/test/test_motion_plan_cache.py index c4ceb41..e73cbb1 100755 --- a/nexus_motion_planner/test/test_motion_plan_cache.py +++ b/nexus_motion_planner/test/test_motion_plan_cache.py @@ -134,257 +134,7 @@ def wrapped(output): @pytest.mark.launch(fixture=launch_description) -def test_read_stdout(motion_cache_test_runner_node, launch_context): - test_cases = [ - # Cache init - ('init', [ - 'Cache init', - ]), - - # Motion plan cache - ('test_motion_plans.empty', [ - 'Plan cache initially empty', - 'Fetch all plans on empty cache returns empty', - 'Fetch best plan on empty cache returns nullptr', - ]), - - ('test_motion_plans.put_plan_empty_frame', [ - 'Put empty frame plan, no delete_worse_plans, not ok', - 'No plans in cache', - ]), - - ('test_motion_plans.put_plan_req_empty_frame', [ - 'Put empty frame req plan, no delete_worse_plans, not ok', - 'No plans in cache', - ]), - - ('test_motion_plans.put_first', [ - 'Put first valid plan, no delete_worse_plans, ok', - 'One plan in cache', - ]), - ('test_motion_plans.put_first.fetch_matching_no_tolerance', [ - 'Fetch all returns one', - 'Fetch best plan is not nullptr', - 'Fetched plan on both fetches match', - 'Fetched plan matches original', - 'Fetched plan has correct execution time', - 'Fetched plan has correct planning time', - ]), - ('test_motion_plans.put_first.fetch_is_diff_no_tolerance', [ - 'Fetch all returns one', - 'Fetch best plan is not nullptr', - 'Fetched plan on both fetches match', - 'Fetched plan matches original', - 'Fetched plan has correct execution time', - 'Fetched plan has correct planning time', - ]), - ('test_motion_plans.put_first.fetch_non_matching_out_of_tolerance', [ - 'Fetch all returns empty', - 'Fetch best plan is nullptr', - ]), - ('test_motion_plans.put_first.fetch_non_matching_in_tolerance', [ - 'Fetch all returns one', - 'Fetch best plan is not nullptr', - 'Fetched plan on both fetches match', - 'Fetched plan matches original', - 'Fetched plan has correct execution time', - 'Fetched plan has correct planning time', - ]), - ('test_motion_plans.put_first.fetch_swapped', [ - 'Fetch all returns one', - 'Fetch best plan is not nullptr', - 'Fetched plan on both fetches match', - 'Fetched plan matches original', - 'Fetched plan has correct execution time', - 'Fetched plan has correct planning time', - ]), - ('test_motion_plans.put_first.fetch_smaller_workspace', [ - 'Fetch all returns empty', - 'Fetch best plan is nullptr', - ]), - ('test_motion_plans.put_first.fetch_larger_workspace', [ - 'Fetch all returns one', - 'Fetch best plan is not nullptr', - 'Fetched plan on both fetches match', - 'Fetched plan matches original', - 'Fetched plan has correct execution time', - 'Fetched plan has correct planning time', - 'Fetched plan has more restrictive workspace max_corner', - 'Fetched plan has more restrictive workspace min_corner', - ]), - - ('test_motion_plans.put_worse', [ - 'Put worse plan, no delete_worse_plans, not ok', - 'One plan in cache', - ]), - - ('test_motion_plans.put_better', [ - 'Put better plan, no delete_worse_plans, ok', - 'Two plans in cache', - ]), - ('test_motion_plans.put_better.fetch_sorted', [ - 'Fetch all returns two', - 'Fetch best plan is not nullptr', - 'Fetched plan on both fetches match', - 'Fetched plan matches original', - 'Fetched plan has correct execution time', - 'Fetched plan has correct planning time', - 'Fetched plans are sorted correctly', - ]), - - ('test_motion_plans.put_better_delete_worse_plans', [ - 'Put better plan, delete_worse_plans, ok', - 'One plan in cache', - ]), - ('test_motion_plans.put_better_delete_worse_plans.fetch', [ - 'Fetch all returns one', - 'Fetch best plan is not nullptr', - 'Fetched plan on both fetches match', - 'Fetched plan matches original', - 'Fetched plan has correct execution time', - 'Fetched plan has correct planning time', - ]), - - ('test_motion_plans.put_different_req', [ - 'Put different plan req, delete_worse_plans, ok', - 'Two plans in cache', - ]), - ('test_motion_plans.put_different_req.fetch', [ - 'Fetch all returns one', - 'Fetch best plan is not nullptr', - 'Fetched plan on both fetches match', - 'Fetched plan matches original', - 'Fetched plan has correct execution time', - 'Fetched plan has correct planning time', - ]), - - # Cartesian plan cache - ('test_cartesian_plans.construct_get_cartesian_path_request', [ - 'Ok', - ]), - - ('test_cartesian_plans.empty', [ - 'Plan cache initially empty', - 'Fetch all plans on empty cache returns empty', - 'Fetch best plan on empty cache returns nullptr', - ]), - - ('test_cartesian_plans.put_plan_empty_frame', [ - 'Put empty frame plan, no delete_worse_plans, not ok', - 'No plans in cache', - ]), - - ('test_cartesian_plans.put_plan_req_empty_frame', [ - 'Put empty frame req plan, no delete_worse_plans, not ok', - 'No plans in cache', - ]), - - ('test_cartesian_plans.put_first', [ - 'Put first valid plan, no delete_worse_plans, ok', - 'One plan in cache', - ]), - ('test_cartesian_plans.put_first.fetch_matching_no_tolerance', [ - 'Fetch all returns one', - 'Fetch best plan is not nullptr', - 'Fetched plan on both fetches match', - 'Fetched plan matches original', - 'Fetched plan has correct execution time', - 'Fetched plan has correct planning time', - 'Fetched plan has correct fraction', - ]), - ('test_cartesian_plans.put_first.fetch_is_diff_no_tolerance', [ - 'Fetch all returns one', - 'Fetch best plan is not nullptr', - 'Fetched plan on both fetches match', - 'Fetched plan matches original', - 'Fetched plan has correct execution time', - 'Fetched plan has correct planning time', - 'Fetched plan has correct fraction', - ]), - ('test_cartesian_plans.put_first.fetch_non_matching_out_of_tolerance', [ - 'Fetch all returns empty', - 'Fetch best plan is nullptr', - ]), - ('test_cartesian_plans.put_first.fetch_non_matching_in_tolerance', [ - 'Fetch all returns one', - 'Fetch best plan is not nullptr', - 'Fetched plan on both fetches match', - 'Fetched plan matches original', - 'Fetched plan has correct execution time', - 'Fetched plan has correct planning time', - 'Fetched plan has correct fraction', - ]), - ('test_cartesian_plans.put_first.fetch_higher_fraction', [ - 'Fetch all returns empty', - 'Fetch best plan is nullptr', - ]), - ('test_cartesian_plans.put_first.fetch_lower_fraction', [ - 'Fetch all returns one', - 'Fetch best plan is not nullptr', - 'Fetched plan on both fetches match', - 'Fetched plan matches original', - 'Fetched plan has correct execution time', - 'Fetched plan has correct planning time', - 'Fetched plan has correct fraction', - ]), - - ('test_cartesian_plans.put_worse', [ - 'Put worse plan, no delete_worse_plans, not ok', - 'One plan in cache', - ]), - - ('test_cartesian_plans.put_better', [ - 'Put better plan, no delete_worse_plans, ok', - 'Two plans in cache', - ]), - ('test_cartesian_plans.put_better.fetch_sorted', [ - 'Fetch all returns two', - 'Fetch best plan is not nullptr', - 'Fetched plan on both fetches match', - 'Fetched plan matches original', - 'Fetched plan has correct execution time', - 'Fetched plan has correct planning time', - 'Fetched plan has correct fraction', - 'Fetched plans are sorted correctly', - ]), - ('test_cartesian_plans.put_better_delete_worse_plans', [ - 'Put better plan, delete_worse_plans, ok', - 'One plan in cache', - ]), - ('test_cartesian_plans.put_better_delete_worse_plans.fetch', [ - 'Fetch all returns one', - 'Fetch best plan is not nullptr', - 'Fetched plan on both fetches match', - 'Fetched plan matches original', - 'Fetched plan has correct execution time', - 'Fetched plan has correct planning time', - 'Fetched plan has correct fraction', - ]), - - ('test_cartesian_plans.put_different_req', [ - 'Put different plan req, delete_worse_plans, ok', - 'Two plans in cache', - ]), - ('test_cartesian_plans.put_different_req.fetch', [ - 'Fetch all returns one', - 'Fetch best plan is not nullptr', - 'Fetched plan on both fetches match', - 'Fetched plan matches original', - 'Fetched plan has correct execution time', - 'Fetched plan has correct planning time', - 'Fetched plan has correct fraction', - ]), - ] - - for prefix, labels in test_cases: - for label in labels: - process_tools.assert_output_sync( - launch_context, - motion_cache_test_runner_node, - validate_stream(f"[PASS] {prefix}: {label}"), - timeout=10 - ) - +def test_all_tests_pass(motion_cache_test_runner_node, launch_context): # Check no occurrences of [FAIL] in output assert not process_tools.wait_for_output_sync( launch_context, From 841a7ca098186a3c6679d3248dbd1b477b22fdf0 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Sat, 25 Nov 2023 20:03:59 -0800 Subject: [PATCH 11/13] Test half in-tolerance Signed-off-by: methylDragon --- .../src/test_motion_plan_cache.cpp | 77 +++++++++++++++++++ 1 file changed, 77 insertions(+) diff --git a/nexus_motion_planner/src/test_motion_plan_cache.cpp b/nexus_motion_planner/src/test_motion_plan_cache.cpp index d7fb667..f5391a3 100644 --- a/nexus_motion_planner/src/test_motion_plan_cache.cpp +++ b/nexus_motion_planner/src/test_motion_plan_cache.cpp @@ -342,6 +342,38 @@ void test_motion_plans( check_and_emit( fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + // Fetch non-matching, only start in tolerance (but not goal) + // + // Non-matching key should not have cache hit + prefix = + "test_motion_plans.put_first.fetch_non_matching_only_start_in_tolerance"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, close_matching_plan_req, 999, 0); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, close_matching_plan_req, 999, 0); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + + // Fetch non-matching, only goal in tolerance (but not start) + // + // Non-matching key should not have cache hit + prefix = + "test_motion_plans.put_first.fetch_non_matching_only_goal_in_tolerance"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, close_matching_plan_req, 0, 999); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, close_matching_plan_req, 0, 999); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + // Fetch non-matching, in tolerance // // Close key within tolerance limit should have cache hit @@ -677,12 +709,21 @@ void test_cartesian_plans( // 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; @@ -856,6 +897,42 @@ void test_cartesian_plans( check_and_emit( fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + // Fetch non-matching, only start in tolerance (but not goal) + // + // Non-matching key should not have cache hit + prefix = + "test_motion_plans.put_first.fetch_non_matching_only_start_in_tolerance"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 999, 0); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 999, 0); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + + // Fetch non-matching, only goal in tolerance (but not start) + // + // Non-matching key should not have cache hit + prefix = + "test_motion_plans.put_first.fetch_non_matching_only_goal_in_tolerance"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 0, 999); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 0, 999); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + // Fetch non-matching, in tolerance // // Close key within tolerance limit should have cache hit From 776c5fdbefa8443ca9a73b87c17ae22d24d710de Mon Sep 17 00:00:00 2001 From: methylDragon Date: Sat, 25 Nov 2023 21:50:13 -0800 Subject: [PATCH 12/13] Test different robot cache Signed-off-by: methylDragon --- .../src/test_motion_plan_cache.cpp | 79 ++++++++++++++++++- 1 file changed, 77 insertions(+), 2 deletions(-) diff --git a/nexus_motion_planner/src/test_motion_plan_cache.cpp b/nexus_motion_planner/src/test_motion_plan_cache.cpp index f5391a3..64e71c7 100644 --- a/nexus_motion_planner/src/test_motion_plan_cache.cpp +++ b/nexus_motion_planner/src/test_motion_plan_cache.cpp @@ -647,6 +647,44 @@ void test_motion_plans( check_and_emit( fetched_plan->lookupDouble("planning_time_s") == planning_time, prefix, "Fetched plan has correct planning time"); + + // Fetch different robot + // + // Since we didn't populate anything, we should expect empty + prefix = "test_motion_plans.different_robot.empty"; + std::string different_robot_name = "different_robot"; + + check_and_emit( + cache->count_cartesian_plans(different_robot_name) == 0, + prefix, "No plans in cache"); + + // Put first for different robot, delete_worse_plans + // + // A different robot's cache should not interact with the original cache + prefix = "test_motion_plans.different_robot.put_first"; + check_and_emit( + cache->put_plan( + *move_group, different_robot_name, different_plan_req, + different_plan, better_execution_time, planning_time, true), + prefix, "Put different plan req, delete_worse_plans, ok"); + + check_and_emit( + cache->count_plans(different_robot_name) == 1, + prefix, "One plans in cache"); + + check_and_emit( + cache->count_plans(different_robot_name) == 1, + prefix, "One plans in cache"); + + check_and_emit( + cache->count_plans(g_robot_name) == 2, + prefix, "Two plans in original robot's cache"); + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, different_plan_req, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, + "Fetch all on original still returns one"); } void test_cartesian_plans( @@ -1148,8 +1186,7 @@ void test_cartesian_plans( check_and_emit( cache->put_cartesian_plan( - *move_group, g_robot_name, different_cartesian_plan_req, - different_plan, + *move_group, g_robot_name, different_cartesian_plan_req, different_plan, better_execution_time, planning_time, fraction, true), prefix, "Put different plan req, delete_worse_plans, ok"); @@ -1191,6 +1228,44 @@ void test_cartesian_plans( check_and_emit( fetched_plan->lookupDouble("fraction") == fraction, prefix, "Fetched plan has correct fraction"); + + // Fetch different robot + // + // Since we didn't populate anything, we should expect empty + prefix = "test_cartesian_plans.different_robot.empty"; + std::string different_robot_name = "different_robot"; + + check_and_emit( + cache->count_cartesian_plans(different_robot_name) == 0, + prefix, "No plans in cache"); + + // Put first for different robot, delete_worse_plans + // + // A different robot's cache should not interact with the original cache + prefix = "test_cartesian_plans.different_robot.put_first"; + check_and_emit( + cache->put_cartesian_plan( + *move_group, different_robot_name, different_cartesian_plan_req, + different_plan, better_execution_time, planning_time, fraction, true), + prefix, "Put different plan req, delete_worse_plans, ok"); + + check_and_emit( + cache->count_cartesian_plans(different_robot_name) == 1, + prefix, "One plans in cache"); + + check_and_emit( + cache->count_cartesian_plans(different_robot_name) == 1, + prefix, "One plans in cache"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 2, + prefix, "Two plans in original robot's cache"); + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, different_cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, + "Fetch all on original still returns one"); } int main(int argc, char** argv) From 644b5b4469479acc8dd1bd49e75e9ada34768935 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 27 Nov 2023 16:33:46 -0600 Subject: [PATCH 13/13] Add force_cache_mode_execute_read_only (#29) * Add force_cache_mode_execute_read_only Signed-off-by: methylDragon * Add force_cache_mode_execute_read_only input port Signed-off-by: methylDragon --------- Signed-off-by: methylDragon --- nexus_capabilities/src/capabilities/plan_motion.cpp | 11 +++++++++++ nexus_capabilities/src/capabilities/plan_motion.hpp | 3 +++ nexus_motion_planner/src/motion_planner_server.cpp | 6 ++++-- .../nexus_motion_planner_msgs/srv/GetMotionPlan.srv | 5 +++++ nexus_tree_nodes.xml | 1 + 5 files changed, 24 insertions(+), 2 deletions(-) diff --git a/nexus_capabilities/src/capabilities/plan_motion.cpp b/nexus_capabilities/src/capabilities/plan_motion.cpp index 4d83079..b762a93 100644 --- a/nexus_capabilities/src/capabilities/plan_motion.cpp +++ b/nexus_capabilities/src/capabilities/plan_motion.cpp @@ -120,6 +120,17 @@ make_request() req->max_velocity_scaling_factor = scale_speed; req->max_acceleration_scaling_factor = scale_speed; + auto maybe_read_only = + this->getInput("force_cache_mode_execute_read_only"); + if (maybe_read_only.has_value()) + { + req->force_cache_mode_execute_read_only = maybe_read_only.value(); + } + else + { + req->force_cache_mode_execute_read_only = false; + } + RCLCPP_DEBUG_STREAM(this->_logger, "planning to " << req->goal_pose.pose << " at frame " << req->goal_pose.header.frame_id << " with cartesian " << req->cartesian << "and scaled speed of " << diff --git a/nexus_capabilities/src/capabilities/plan_motion.hpp b/nexus_capabilities/src/capabilities/plan_motion.hpp index ce72073..57dcc58 100644 --- a/nexus_capabilities/src/capabilities/plan_motion.hpp +++ b/nexus_capabilities/src/capabilities/plan_motion.hpp @@ -66,6 +66,9 @@ public: static BT::PortsList providedPorts() BT::InputPort>( "start_constraints", "OPTIONAL. If provided the the joint_constraints are used as the start condition. Else, the current state of the robot will be used as the start."), + BT::InputPort( + "force_cache_mode_execute_read_only", + "OPTIONAL. Set true to force cache mode to ExecuteReadOnly for this request."), BT::OutputPort( "result", "The resulting trajectory.")}; } diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 2c3ce5e..d24b442 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -615,7 +615,8 @@ void MotionPlannerServer::plan_with_move_group( _collision_aware_cartesian_path); // Fetch if in execute mode. - if (cache_mode_is_execute(_cache_mode)) + if (cache_mode_is_execute(_cache_mode) + || req.force_cache_mode_execute_read_only) { auto fetch_start = this->now(); auto fetched_cartesian_plan = @@ -640,7 +641,8 @@ void MotionPlannerServer::plan_with_move_group( fetched_cartesian_plan->lookupDouble("planning_time_s")); } // Fail if ReadOnly mode and no cached cartesian plan was fetched. - else if (_cache_mode == PlannerDatabaseMode::ExecuteReadOnly) + else if (_cache_mode == PlannerDatabaseMode::ExecuteReadOnly + || req.force_cache_mode_execute_read_only) { RCLCPP_ERROR( this->get_logger(), diff --git a/nexus_msgs/nexus_motion_planner_msgs/srv/GetMotionPlan.srv b/nexus_msgs/nexus_motion_planner_msgs/srv/GetMotionPlan.srv index 87cc18f..a883d18 100644 --- a/nexus_msgs/nexus_motion_planner_msgs/srv/GetMotionPlan.srv +++ b/nexus_msgs/nexus_motion_planner_msgs/srv/GetMotionPlan.srv @@ -52,6 +52,11 @@ string payload float64 max_velocity_scaling_factor float64 max_acceleration_scaling_factor +# Set to true in order to force the server to use the motion cache as if the +# cache mode were set to ExecuteReadOnly, regardless of the original setting +# for this request. +bool force_cache_mode_execute_read_only + --- # Motion planning result diff --git a/nexus_tree_nodes.xml b/nexus_tree_nodes.xml index e9e7efa..9d36669 100644 --- a/nexus_tree_nodes.xml +++ b/nexus_tree_nodes.xml @@ -52,6 +52,7 @@ An optional fraction [0,1] to scale the acceleraton and speed of the generated trajectory True if a cartesian plan is required If provided the joint_constraints will be used as the start state of the robot. By default the current state is set as start + True to force cache mode to ExecuteReadOnly for this request. The generated motion plan