🐶 ROS 2 Servo Control for Robot Dog

🧠 Previously

Raspberry Pi setup ✅
https://qiita.com/tri-edge/items/df545c0aaac6975101ed
ROS 2 setup ✅
https://qiita.com/tri-edge/items/fa72f52c0a7a9ef7af1f

📝 Introduction

Now that you have installed ROS 2 on your Raspberry Pi, the next step is to control hardware.
In this guide, you will learn how to:

• Control a servo motor using Python
• Integrate servo control with ROS 2
• Send commands to move a single servo

👉 By the end of this guide, you will be able to control a servo motor using ROS 2 commands.

💡 Important Concept

ROS 2 does NOT directly control hardware. Instead:

ROS 2 → sends command → Python node → controls servo

🔧 Requirements

🔌 Hardware

• Raspberry Pi 4
• FreeNove Robot Dog (or any servo motor)

💻 Software

• Raspberry Pi OS (64-bit)
• ROS 2 (Humble installed)
• Python 3

💡 Note

In the previous tutorial, we installed python3-smbus, which is a specific Python library used for I2C communication. In this guide, we install python3-pip, which is a tool used to install Python libraries. This is important because we need pip to install additional packages like gpiozero.

In short, python3-smbus installs one library, while python3-pip allows you to install many Python libraries as needed.

🚀 Setup

① Install required Python libraries

sudo apt update
sudo apt install python3-pip -y
pip3 install gpiozero

✅ This installs the Python package manager (pip) and the servo control library

② Test servo (without ROS 2)

👉 First, confirm the servo works properly

nano servo_test.py

Add code:

from gpiozero import AngularServo
from time import sleep

servo = AngularServo(18, min_angle=-90, max_angle=90)

while True:
    servo.angle = -45
    sleep(2)
    servo.angle = 45
    sleep(2)

Save and run:

python3 servo_test.py

✅ The servo should move left and right
👉 If this does NOT work, fix it before continuing

③ Create ROS 2 workspace

Run:

mkdir -p ~/ros2_ws/src

📁 ros2_ws

• This is your workspace name
• You can name it anything, but ros2_ws is standard

Example location: /home/pi/ros2_ws

📁 src (VERY important)

Inside the workspace:

ros2_ws/
 └── src/

This is where your ROS 2 packages go.

🧠 Why src exists

ROS expects your packages to be inside: workspace/src/

If you don’t follow this structure: ❌ ROS won’t find your packages

Run:

cd ~/ros2_ws
colcon build

🧱 What colcon build does

✅ Builds your ROS 2 workspace. It:

• compiles packages
• prepares ROS environment
• generates setup files

📁 After build, your workspace looks like:

ros2_ws/
 ├── src/
 ├── build/
 ├── install/
 └── log/

What these folders mean:

Folder	Purpose
src	your code (packages)
build	temporary build files
install	final usable ROS packages
log	build logs

✅ Final step

source install/setup.bash

💡 Tip: Run this each time you open a new terminal

source ~/ros2_ws/install/setup.bash

🧠 What this does

✅ Tells your terminal:
“Use packages from this workspace”
Without this:
❌ ROS won’t see your package
❌ ros2 run won’t work

🧠 Full flow (simple mental model)

Create folder → add code → build → enable

④ Create ROS 2 package

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python robot_dog_control

cd robot_dog_control/robot_dog_control

🐶 Relating to your robot dog

You will create the following structure:

ros2_ws/
 └── src/
     └── **robot_dog_control/**

👉 This package contains your robot control code, such as:

• the servo control node
• future movement logic

⑤ Create servo control node

In ROS 2, a node is a program that performs a specific task.
In this step, you will create a node that receives commands and controls a servo.

📄 Create the file:

nano servo_node.py

👉 This creates a Python file where your ROS 2 code will live. This node will:

• Listen to a ROS 2 topic (servo_angle)
• Receive angle values (e.g., 30, -45)
• Move the servo to that angle

🧠 ROS 2 Servo Node

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32
from gpiozero import AngularServo

class ServoNode(Node):
    def __init__(self):
        super().__init__('servo_node')

        self.servo = AngularServo(18, min_angle=-90, max_angle=90)

        self.subscription = self.create_subscription(
            Float32,
            'servo_angle',
            self.set_angle,
            10
        )

    def set_angle(self, msg):
        angle = msg.data
        self.get_logger().info(f'Setting angle: {angle}')
        self.servo.angle = angle


def main(args=None):
    rclpy.init(args=args)
    node = ServoNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

💡 Key explanation

• Float32 → the type of message (a number)
• servo_angle → topic name
• set_angle() → function that runs when a message is received

Example flow:

[ You type command ]
        ↓
[ ROS topic: servo_angle ]
        ↓
[ Your node receives message ]
        ↓
[ Servo angle is updated ]
        ↓
🐶 Robot moves

👉 A command is sent to a ROS topic, the node receives it, and then controls the servo accordingly.

⑥ Update setup.py

👉 This step tells ROS 2 how to run your node.

📄 Open setup.py:

nano ~/ros2_ws/src/robot_dog_control/setup.py

🧠 Add:

entry_points={
    'console_scripts': [
        'servo_node = robot_dog_control.servo_node:main',
    ],
},

💡 What this means

・servo_node → command name you will use
・robot_dog_control.servo_node → Python file location
・main → function that starts the program

✅ Result

After this, you can run:

ros2 run robot_dog_control servo_node

👉 Without this step, ROS 2 will NOT find your node ❌

⑦ Build the package

👉 After creating or updating code, you must build the workspace.

🔧 Build command

cd ~/ros2_ws
colcon build

🧠 What this does

compiles your package
registers your node with ROS 2
prepares it for execution

⚠️ Important step

After building:

source install/setup.bash

It tells the terminal:
“Use my newly created package and node”

💡 Note:

You will often run colcon build and source install/setup.bash after making changes. Repeating these steps is normal in ROS 2 development.

⑧ ✅ Run and test

🖥️ Open two terminals

Terminal A:

ros2 run robot_dog_control servo_node

Terminal B:

ros2 topic pub /servo_angle std_msgs/msg/Float32 "{data: 30}"

Expected result

• Servo moves to the specified angle
• Terminal shows:

Setting angle: 30.0

This confirms ROS 2 can successfully control hardware.
👉 If this works, your ROS 2 + hardware setup is fully functional.

⚠️ Warning

Be careful not to force large or rapid angle changes, as this may damage the servo.

⚡ Key Points

• Always test hardware BEFORE ROS
• ROS 2 sends commands, Python controls hardware
• Use small angle values to avoid damage
• Servos may require an external power supply

🧠 What is happening?

Command → ROS topic → Servo node → Servo motor

• Topic = communication
• Node = logic
• Servo = hardware

👉 This is the foundation of robot control

✅ Summary

You now have:

• ✅ Working servo control
• ✅ ROS 2 node controlling hardware
• ✅ Communication between ROS and servo

Your Raspberry Pi can now control real hardware using ROS 2.
This is the fundamental mechanism behind all robot control in ROS 2.
You are now ready to start controlling real robot movement using ROS 2.

🚀 Next Step

👉 Move a full leg