ROS2 C++ robot forward-inverse kinematic build

Forward/Inverse kinematics demo for a planar 3-DOF arm. All design processes and the explanation of coding structure are explained in design_process.txt.

Includes:

• FK publisher node: publishes an end-effector pose on /ee_pose
• IK subscriber node: subscribes to /ee_pose, computes elbow-up and elbow-down solutions, and prints them
• Robot class (include/robot.hpp, src/robot.cpp) with FK, analytical IK, and a damped least-squares IK
• Unit tests (GoogleTest) in src/main.cpp

---

1) Install prerequisites

On Ubuntu 24.04 (Noble) with ROS 2 Jazzy:

sudo apt update
# Core ROS 2 and dev tools
sudo apt install -y ros-jazzy-ros-base \
  python3-colcon-common-extensions python3-rosdep python3-vcstool \
  build-essential cmake git libeigen3-dev
# Runtime ROS dependencies
sudo apt install -y ros-jazzy-rclcpp ros-jazzy-geometry-msgs

Initialize rosdep (if it's your first time running this):

sudo rosdep init || true
rosdep update

Source ROS 2 automatically in new terminals:

echo 'source /opt/ros/jazzy/setup.bash' >> ~/.bashrc
source ~/.bashrc

2) Create a workspace and add this package

After you have downloaded the robot_demo.zip file, run:

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

# Unzip the repo into src (adjust the path to your ZIP)
unzip /path/to/robot_demo.zip -d .

Your tree will look like this:

~/ros2_ws/src/robot_demo
├── CMakeLists.txt
├── package.xml
├── include/
│   └── robot.hpp
├── src/
│   ├── robot.cpp
│   ├── fk_publisher_node.cpp
│   ├── ik_subscriber_node.cpp
│   └── main.cpp
└── launch/
    └── robot_demo_launch.py

3) Install package dependencies

From your workspace root, run:

cd ~/ros2_ws
rosdep install --from-paths src --ignore-src --rosdistro jazzy -yr

4) Build

cd ~/ros2_ws
colcon build --packages-select robot_demo --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
source install/setup.bash

This builds the following:

1. robot_lib (library with FK/IK)
2. fk_publisher_node
3. ik_subscriber_node (Note that the IK solver is the analytical one)
4. installs launch files to install/robot_demo/share/robot_demo/launch/

5) Run the demo

Start the FK node with chosen joint angles (radians). If the launch also starts the IK node, it will print both branches.

# Terminal 1 NOTE: YOU CAN CHANGE theta1:=0.6 theta2:=-0.3 theta3:=1.1 AS DESIRED
source /opt/ros/jazzy/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 launch robot_demo robot_demo_launch.py theta1:=0.6 theta2:=-0.3 theta3:=1.1

# Terminal 2 (observe FK pose)
source /opt/ros/jazzy/setup.bash
ros2 topic echo /ee_pose

6) Run the gunit tests (GoogleTest)

Since we use src/main.cpp as the GoogleTest entry, make sure tests are enabled in CMake and the test dependency is declared in package.xml.

Add these lines of code near the bottom of CMakeLists.txt, before ament_package():

# --- Tests ---
if(BUILD_TESTING)
  find_package(ament_cmake_gtest REQUIRED)
  ament_add_gtest(robot_demo_tests
    src/main.cpp
  )
  target_link_libraries(robot_demo_tests
    robot_lib
    Eigen3::Eigen
  )
  # If tests also use ROS APIs, you can add:
  # ament_target_dependencies(robot_demo_tests rclcpp geometry_msgs)
endif()

In package.xml, add:

<test_depend>ament_cmake_gtest</test_depend>

Build and run tests:

cd ~/ros2_ws
colcon build --packages-select robot_demo
source install/setup.bash

# Run tests via colcon to only view success/failure cases
colcon test --packages-select robot_demo
colcon test-result --verbose

# Run tests via colcon to stream the raw gtest output
colcon test --packages-select robot_demo --event-handlers console_direct+
colcon test-result --verbose