Getting Started
===============

Prerequisites
-------------

**System requirements**

- ROS 2 Jazzy Jalisco
- Ubuntu 24.04 (Noble)
- C++17 compiler (GCC 13+ or Clang 16+)
- Python 3.12+

**Dependencies** (installed in the CI base image or via apt)

.. code-block:: bash

   sudo apt-get install -y \
     ros-jazzy-ros-base \
     ros-jazzy-ros-gz-bridge \
     libnlohmann-json3-dev \
     liborocos-kdl-dev \
     libeigen3-dev \
     libfcl-dev \
     libompl-dev \
     libtrac-ik-dev

**colcon mixins** (one-time workspace setup)

.. code-block:: bash

   sudo apt-get install python3-colcon-mixin python3-vcstool
   colcon mixin add local file://$(pwd)/ci/colcon_mixins/index.yaml
   colcon mixin update local

Building
--------

All commands run from the workspace root (``jazzy_sorting_ws/``), not from within this package.

.. code-block:: bash

   # Import dependency repos
   vcs import src < src/deps/core_motion_planning/ci/deps.repos

   # Build everything
   colcon build --mixin dev

   # Build only this stack (faster iteration)
   colcon build --mixin dev --packages-select \
     motion_planning_types \
     core_robot \
     core_arm_planning \
     core_motion_planning \
     motion_planning

   # Source the install space
   source install/setup.bash

Launching the Node
------------------

Use the provided launch file. ``arm_planning_config`` and ``motion_planning_config`` are required; OPW params and inverse dynamics config are optional but recommended.

.. code-block:: bash

   ros2 launch motion_planning motion_planning.launch.py \
     arm_planning_config:=/path/to/arm_planning_config.json \
     motion_planning_config:=/path/to/motion_planning_config.json \
     opw_params:=/path/to/opw_params.json \
     inverse_dynamics_config:=/path/to/inverse_dynamics_config.json

Launch Arguments
~~~~~~~~~~~~~~~~

.. list-table::
   :header-rows: 1
   :widths: 28 12 60

   * - Argument
     - Required
     - Description
   * - ``arm_planning_config``
     - Yes
     - Absolute path to the arm planning JSON config. Controls IK solver, path planner, parameterizer, and collision settings.
   * - ``motion_planning_config``
     - Yes
     - Absolute path to the motion planning JSON config. Controls server pool sizes and ROS 2 service settings.
   * - ``opw_params``
     - No (``''``)
     - Absolute path to OPW kinematics parameters. Enables the fast analytic OPW IK solver; falls back to TRAC-IK if omitted.
   * - ``inverse_dynamics_config``
     - No (``''``)
     - Absolute path to inverse dynamics config. Required only when using TOPPRA with torque constraints.

.. note::

   Omitting ``opw_params`` logs a warning and uses TRAC-IK for all inverse kinematics. OPW is 10-100× faster for supported 6-DOF industrial robots.

Quick Smoke Test
----------------

With the node running, verify it is alive:

.. code-block:: bash

   source install/setup.bash

   # Check node is up
   ros2 node list | grep motion_planning

   # List available services
   ros2 service list | grep -E "compute|check|get"

   # Call forward kinematics (MH50 robot joints at zero)
   ros2 service call /compute_fk motion_planning_interfaces/srv/ComputeFK \
     "{robot_state: {joint_state: {name: ['s_joint','l_joint','u_joint','r_joint','b_joint','t_joint'], position: [0.0,0.0,0.0,0.0,0.0,0.0]}}, root_frame: 'base_link', tip_frame: 'tool0'}"

A ``SUCCESS`` error code (value ``1``) confirms the node is functional.

Testing
-------

.. code-block:: bash

   source /opt/ros/jazzy/setup.bash && source install/setup.bash

   # C++ gtests (unit + integration)
   colcon test --packages-select core_arm_planning core_motion_planning \
     --event-handlers console_direct+ --return-code-on-test-failure

   # Python integration tests (requires running node + robot workcell)
   colcon test --packages-select motion_planning \
     --event-handlers console_direct+ --return-code-on-test-failure

   # Python tests in parallel
   colcon test --packages-select motion_planning --pytest-args "-n auto" \
     --event-handlers console_direct+ --return-code-on-test-failure