Architecture
============

Package Structure
-----------------

The repository contains 8 packages organised in three layers: types and interfaces at the
base, core planning libraries in the middle, and ROS 2 nodes and agents at the top.

.. graphviz::

   digraph architecture {
      rankdir=TB;
      node [shape=box, style=filled, fontname="Helvetica"];
      edge [fontname="Helvetica", fontsize=10];

      // Layer 0 – interfaces
      motion_planning_interfaces [label="motion_planning_interfaces\n(ROS 2 msg/srv)", fillcolor="#d0e8ff"];
      motion_planning_types      [label="motion_planning_types\n(C++ type headers)", fillcolor="#d0e8ff"];

      // Layer 1 – core libs
      core_robot       [label="core_robot\n(Robot model)", fillcolor="#ffe0b2"];
      core_arm_planning[label="core_arm_planning\n(Path planning, IK, TOPPRA)", fillcolor="#ffe0b2"];
      core_motion_planning_lib [label="core_motion_planning\n(Server pool, collision util)", fillcolor="#ffe0b2"];

      // Layer 2 – ROS 2 node
      motion_planning_ros2_conversions [label="motion_planning_ros2_conversions\n(msg↔type conversions)", fillcolor="#e8f5e9"];
      motion_planning [label="motion_planning\n(ROS 2 node servers)", fillcolor="#c8e6c9"];

      // Layer 3 – agent
      motion_planning_agent [label="motion_planning_agent\n(Lifecycle node)", fillcolor="#f3e5f5"];

      // Edges
      motion_planning_types       -> core_robot;
      motion_planning_types       -> core_arm_planning;
      motion_planning_types       -> core_motion_planning_lib;
      core_robot                  -> core_arm_planning;
      core_arm_planning           -> core_motion_planning_lib;
      core_motion_planning_lib    -> motion_planning;
      motion_planning_interfaces  -> motion_planning_ros2_conversions;
      motion_planning_types       -> motion_planning_ros2_conversions;
      motion_planning_ros2_conversions -> motion_planning;
      motion_planning             -> motion_planning_agent;
   }

Server Pool
-----------

``ServerManager`` maintains a pool of pre-constructed servers for each capability.
Callers acquire a server, use it, and return it — enabling parallel request handling
without re-initialisation overhead.

.. graphviz::

   digraph server_pool {
      rankdir=LR;
      node [shape=box, style=filled, fontname="Helvetica"];

      client  [label="Client request", shape=ellipse, fillcolor="#fff9c4"];
      sm      [label="ServerManager", fillcolor="#ffe0b2"];
      ms      [label="MotionServer\n(pool)", fillcolor="#c8e6c9"];
      iks     [label="IKServer\n(pool)", fillcolor="#c8e6c9"];
      fks     [label="FKServer\n(pool)", fillcolor="#c8e6c9"];
      ps      [label="ParameterizerServer\n(pool)", fillcolor="#c8e6c9"];
      cs      [label="CollisionServer\n(pool)", fillcolor="#c8e6c9"];

      client -> sm  [label="acquire"];
      sm -> ms;
      sm -> iks;
      sm -> fks;
      sm -> ps;
      sm -> cs;
      ms -> sm  [label="return", style=dashed];
      iks -> sm [style=dashed];
      fks -> sm [style=dashed];
      ps  -> sm [style=dashed];
      cs  -> sm [style=dashed];
   }

Planning Pipeline
-----------------

A motion plan request flows through four stages from goal specification to a
time-parameterised joint trajectory.

.. graphviz::

   digraph pipeline {
      rankdir=LR;
      node [shape=box, style=filled, fontname="Helvetica"];

      goal   [label="Goal\n(pose / joints)", shape=ellipse, fillcolor="#fff9c4"];
      ik     [label="IK\n(TRAC-IK / OPW)", fillcolor="#d0e8ff"];
      plan   [label="Path planning\n(OMPL)", fillcolor="#ffe0b2"];
      col    [label="Collision check\n(FCL)", fillcolor="#ffccbc"];
      param  [label="Time parameterisation\n(TOPPRA / TOTG)", fillcolor="#c8e6c9"];
      traj   [label="JointTrajectory", shape=ellipse, fillcolor="#fff9c4"];

      goal  -> ik   [label="if Cartesian goal"];
      ik    -> plan;
      goal  -> plan [label="if joint goal"];
      plan  -> col;
      col   -> param [label="collision-free path"];
      param -> traj;
   }