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)

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)

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.

# 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.

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

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:

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

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