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

Communication: Topics Review

# Publishing to a topic (e.g., velocity commands)
from geometry_msgs.msg import Twist

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

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

# Subscribing to a topic (e.g., lidar data)
from sensor_msgs.msg import LaserScan

self.subscription = self.create_subscription(
    LaserScan, '/scan', self.scan_callback, 10)

def scan_callback(self, msg):
    min_distance = min(msg.ranges)
    self.get_logger().info(f'Closest obstacle: {min_distance:.2f}m')

Communication: Actions

The NavigateToPose Action

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

# Create the action client
self._client = ActionClient(self, NavigateToPose, 'navigate_to_pose')

# Build a goal
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

# Send it
self._client.wait_for_server()
future = self._client.send_goal_async(goal)

Handling the Response (Callbacks)

# When goal is accepted/rejected:
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!')
    # Now wait for the result
    result_future = goal_handle.get_result_async()
    result_future.add_done_callback(self.result_callback)

# When navigation completes:
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')
        
        # Define your route
        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()   # Keep going
        else:
            self.get_logger().info('Route complete!')

Specifying Orientation

import math

yaw = 1.57  # radians (90 degrees)

# Convert yaw to quaternion:
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 your mouse over the map in RViz — coordinates appear at the bottom of the RViz window
• Topic method: Click a "Nav2 Goal" and check the terminal for the goal coordinates
• 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

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

# Register node 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