From bcbb978c0da3c19d7e2020e1551465030c07cd91 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 22 Nov 2023 20:10:09 -0800 Subject: [PATCH] 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: