From 488df63980c86768da0df233f75f2cce0f8caf62 Mon Sep 17 00:00:00 2001
From: methylDragon <methylDragon@google.com>
Date: Tue, 21 Nov 2023 19:22:18 -0800
Subject: [PATCH] Add tests for motion plan cache (but not cartesian)

Signed-off-by: methylDragon <methylDragon@gmail.com>
---
 .../src/motion_plan_cache.cpp                 |   7 +-
 .../src/test_motion_plan_cache.cpp            | 490 ++++++++++++++++++
 .../test/test_motion_plan_cache.py            | 239 +++++++++
 3 files changed, 732 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<moveit_msgs::msg::RobotTrajectory>(
     "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<moveit_msgs::msg::RobotTrajectory>(
     "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 <rclcpp/rclcpp.hpp>
+
+#include "moveit/robot_state/conversions.h"
+#include "moveit/robot_state/robot_state.h"
+#include "motion_plan_cache.hpp"
+
+#include <thread>
+
+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<MoveGroupInterface> move_group,
+  std::shared_ptr<MotionPlanCache> 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<std::string>(
+    "warehouse_plugin", "warehouse_ros_sqlite::DatabaseConnection");
+
+  // Test proper
+  {
+    auto cache = std::make_shared<MotionPlanCache>(test_node);
+    check_and_emit(cache->init(":memory:", 0, 1e-4), "init", "Cache init");
+
+    auto move_group =
+      std::make_shared<MoveGroupInterface>(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..022768b
--- /dev/null
+++ b/nexus_motion_planner/test/test_motion_plan_cache.py
@@ -0,0 +1,239 @@
+# 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