Ros2-Humble-Introduction/examples/python_examples/turtle_controller.py at main · JosiahSK/Ros2-Humble-Introduction · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
#!/usr/bin/env python3
"""
turtle_controller.py — Controls the Turtlesim Turtle

Makes the turtle move in a circular pattern by publishing
velocity commands to /turtle1/cmd_vel.

Requirements:
  - Turtlesim must be running: ros2 run turtlesim turtlesim_node

Usage:
  1. Terminal 1: ros2 run turtlesim turtlesim_node
  2. Terminal 2: ros2 run my_beginner_pkg turtle_controller

The turtle will start moving in a circle!

Key concepts demonstrated:
  - Publishing geometry_msgs/msg/Twist (standard robot velocity command)
  - Understanding linear.x (forward speed) and angular.z (turn rate)
  - Graceful shutdown with Ctrl+C
"""

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import math


class TurtleController(Node):
    """
    Controls the turtlesim turtle via velocity commands.

    The Twist message is the universal ROS 2 velocity command:

    linear.x  = forward/backward speed (m/s)
                positive = forward, negative = backward
    linear.y  = left/right speed (rarely used for ground robots)
    linear.z  = up/down speed (used for drones)

    angular.x = roll rate (rarely used)
    angular.y = pitch rate (rarely used)
    angular.z = yaw/turn rate (rad/s)
                positive = counter-clockwise (left turn)
                negative = clockwise (right turn)
    """

    def __init__(self):
        super().__init__('turtle_controller')

        # Publisher for velocity commands
        # Topic: /turtle1/cmd_vel
        # Message: geometry_msgs/msg/Twist
        self.cmd_vel_publisher = self.create_publisher(
            Twist,
            '/turtle1/cmd_vel',
            10
        )

        # Publish at 10 Hz (every 0.1 seconds) for smooth motion
        self.timer = self.create_timer(0.1, self.publish_velocity)

        # Configuration
        self.linear_speed = 2.0   # m/s — forward speed
        self.angular_speed = 1.0  # rad/s — turn rate

        self.get_logger().info(
            'TurtleController started!\n'
            f'  Linear speed:  {self.linear_speed} m/s\n'
            f'  Angular speed: {self.angular_speed} rad/s\n'
            '  Make sure turtlesim_node is running!'
        )

    def publish_velocity(self):
        """Publish a Twist command to move the turtle in a circle."""
        cmd = Twist()

        # Set forward speed (linear.x)
        cmd.linear.x = self.linear_speed

        # Set turn rate (angular.z)
        # For a circle: radius = linear.x / angular.z
        # radius = 2.0 / 1.0 = 2.0 units
        cmd.angular.z = self.angular_speed

        # Other fields default to 0.0 (no sideways or vertical movement)

        self.cmd_vel_publisher.publish(cmd)

    def stop(self):
        """Publish zero velocity to stop the turtle."""
        stop_cmd = Twist()
        # All fields are 0.0 by default — this stops the turtle
        self.cmd_vel_publisher.publish(stop_cmd)
        self.get_logger().info('Turtle stopped. Goodbye! 🐢')


def main(args=None):
    rclpy.init(args=args)
    controller = TurtleController()

    try:
        rclpy.spin(controller)
    except KeyboardInterrupt:
        # Stop the turtle before shutting down
        controller.stop()
    finally:
        controller.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()