Writing ROS2 Nodes

Course Progression

Why Write Your Own Nodes?

Anatomy of a ROS2 Python Node

import rclpy
from rclpy.node import Node

class MyNode(Node):
    def __init__(self):
        super().__init__('my_node_name')
        self.get_logger().info('Node started!')

def main(args=None):
    rclpy.init(args=args)        # Initialize ROS2
    node = MyNode()               # Create the node
    rclpy.spin(node)              # Keep it running
    rclpy.shutdown()              # Clean up

• rclpy — the ROS2 Python client library
• Node — base class that gives you publishers, subscribers, timers, etc.
• spin() — event loop that processes callbacks (like Arduino's loop())

Communication: Topics Review

# Publishing velocity commands
from geometry_msgs.msg import Twist

self.publisher = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)

msg = Twist()
msg.linear.x = 1.0   # Forward at 1.0 m/s
msg.angular.z = 0.0   # No rotation
self.publisher.publish(msg)

Your First Node: A Velocity Publisher

class DriveForward(Node):
    def __init__(self):
        super().__init__('drive_forward')
        self.pub = self.create_publisher(
            Twist, '/turtle1/cmd_vel', 10)
        self.timer = self.create_timer(0.1, self.on_timer)
        self.state = 'forward'  # simple state machine

    def on_timer(self):
        msg = Twist()
        if self.state == 'forward':
            msg.linear.x = 1.0   # drive forward
        elif self.state == 'turn':
            msg.angular.z = 1.0  # rotate in place
        self.pub.publish(msg)

From Direct Control to Smart Navigation

Communication: Actions

The NavigateToPose Action

from rclpy.action import ActionClient
from nav2_msgs.action import NavigateToPose
from geometry_msgs.msg import PoseStamped

self._client = ActionClient(self, NavigateToPose, 'navigate_to_pose')

goal = NavigateToPose.Goal()
goal.pose = PoseStamped()
goal.pose.header.frame_id = 'map'
goal.pose.pose.position.x = 2.0
goal.pose.pose.position.y = -1.0

self._client.wait_for_server()
future = self._client.send_goal_async(goal)

Handling the Response (Callbacks)

def goal_response_callback(self, future):
    goal_handle = future.result()
    if not goal_handle.accepted:
        self.get_logger().warn('Goal rejected!')
        return
    self.get_logger().info('Goal accepted!')
    result_future = goal_handle.get_result_async()
    result_future.add_done_callback(self.result_callback)

def result_callback(self, future):
    self.get_logger().info('Goal reached!')
    # Send the next waypoint here...

Pattern: Sequential Waypoints

class WaypointNavigator(Node):
    def __init__(self):
        super().__init__('waypoint_navigator')
        self._client = ActionClient(
            self, NavigateToPose, 'navigate_to_pose')
        self.waypoints = [
            (0.5, -1.0, 0.0),    # (x, y, yaw)
            (2.0, -1.0, 1.57),
            (2.0,  0.5, 3.14),
            (0.0,  0.0, 0.0),    # Back to start
        ]
        self.current = 0

    def result_callback(self, future):
        self.current += 1
        if self.current < len(self.waypoints):
            self.send_next_goal()
        else:
            self.get_logger().info('Route complete!')

Specifying Orientation

import math
yaw = 1.57  # radians (90 degrees)
orientation_z = math.sin(yaw / 2.0)
orientation_w = math.cos(yaw / 2.0)
goal.pose.pose.orientation.z = orientation_z
goal.pose.pose.orientation.w = orientation_w

How to Find Coordinates

• Hover method: Hover over the map in RViz — coordinates appear at the bottom
• Topic method: Click a "Nav2 Goal" and check the terminal output
• Echo method: ros2 topic echo /goal_pose to see goals as they're sent

Creating a ROS2 Python Package

# Create workspace and package
mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python waypoint_nav

# Structure: waypoint_nav/
# ├── package.xml, setup.py, setup.cfg
# └── waypoint_nav/
#     ├── __init__.py
#     └── navigate_waypoints.py  ← your node

# Register in setup.py → entry_points:
'console_scripts': [
    'navigate_waypoints = waypoint_nav.navigate_waypoints:main',
]

# Build and run:
cd ~/ros2_ws && colcon build --packages-select waypoint_nav
source install/setup.bash
ros2 run waypoint_nav navigate_waypoints

Common Pitfalls

Arduino vs. ROS2: A Comparison

Summary