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..95ed65f --- /dev/null +++ b/nexus_motion_planner/src/test_motion_plan_cache.cpp @@ -0,0 +1,490 @@ +// 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.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)); + + /// RobotTrajectory + + // Plan + auto plan = get_dummy_panda_plan(); + + auto empty_frame_plan = plan; + empty_frame_plan.joint_trajectory.header.frame_id = ""; + + // 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"); + + // 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_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 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, 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 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") == even_better_execution_time, + prefix, "Fetched plan has correct execution 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..542861b --- /dev/null +++ b/nexus_motion_planner/test/test_motion_plan_cache.py @@ -0,0 +1,245 @@ +# 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_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 plans are sorted correctly' + ], + + 'test_motion_plans.put_better_overwrite': [ + 'Put better plan, overwrite, ok', + 'One plan in cache' + ], + } + + for prefix, labels in test_cases.items(): + 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