Custom Nodes

cd ./ros2_ws/src
ros2 pkg create --build-type ament_python kachaka_simple_control --dependencies rclpy geometry_msgs --node-name kachaka_simple_control

kachaka_simple_control/
├── resource/
│   └── kachaka_simple_control
├── kachaka_simple_control/
│   ├── __init__.py
│   └── kachaka_simple_control.py
├── test/
├── package.xml
└── setup.py

#!/usr/bin/env python3
# This line (shebang) tells the system to use Python 3 to execute this script

# Import necessary ROS 2 libraries
import rclpy  # ROS 2 Python client library - provides core ROS 2 functionality
from rclpy.node import Node  # Base class for all ROS 2 nodes
from geometry_msgs.msg import Twist  # Message type for velocity commands (linear and angular)
from rclpy.duration import Duration  # For handling time durations

class KachakaSimpleControl(Node):
    """
    A ROS 2 node class for simple robot control.
    This class inherits from Node, which provides all the ROS 2 node functionality.
    """
    def __init__(self):
        # Initialize the parent Node class with the node name
        super().__init__('kachaka_simple_control')
        
        # Create a Publisher that sends Twist type messages to the /kachaka/manual_control/cmd_vel topic
        # Parameters:
        #   - Twist: The message type (contains linear and angular velocity)
        #   - '/kachaka/manual_control/cmd_vel': The topic name where velocity commands are published
        #   - 10: The queue size (how many messages to buffer if subscribers are slow)
        self.publisher = self.create_publisher(Twist, '/kachaka/manual_control/cmd_vel', 10)
        
        # Log an informational message that the node has started
        self.get_logger().info('Robot control node has started')

    def move_robot(self, linear_x, angular_z, duration):
        """
        Move the robot at specified speed and time using feedforward control.
        
        This method publishes velocity commands continuously for the specified duration.
        In ROS 2, velocity commands must be published regularly; if you stop publishing,
        the robot will stop moving (safety feature).
        
        Parameters:
            linear_x: Forward/backward speed [m/s]
                - Positive values: forward motion
                - Negative values: backward motion
                - Zero: no linear motion
            angular_z: Rotation speed [rad/s]
                - Positive values: counterclockwise rotation (left turn when moving forward)
                - Negative values: clockwise rotation (right turn when moving forward)
                - Zero: no rotation
            duration: Movement duration [seconds]
                - How long to maintain this velocity command
        """
        # Create a new Twist message to hold velocity commands
        vel = Twist()
        # Set the linear velocity in the x direction (forward/backward)
        vel.linear.x = linear_x
        # Set the angular velocity around the z-axis (rotation)
        vel.angular.z = angular_z
        
        # Calculate when the movement should end
        # get_clock().now() gets the current ROS 2 time
        # Duration(seconds=duration) creates a time duration
        # Adding them gives us the target end time
        end_time = self.get_clock().now() + Duration(seconds=duration)
        
        # Continue publishing velocity commands until the specified time
        # This loop ensures commands are sent regularly (every 0.1 seconds)
        while self.get_clock().now() < end_time:
            # Publish the velocity command
            self.publisher.publish(vel)
            # Process any pending callbacks and wait 0.1 seconds
            # This allows ROS 2 to handle other tasks and prevents the loop from running too fast
            rclpy.spin_once(self, timeout_sec=0.1)
        
        # Stop after the movement is complete
        # This ensures the robot doesn't continue moving after the duration expires
        self.stop_robot()
        
        # Log the completed movement for debugging/monitoring
        self.get_logger().info(f'Movement completed: Forward={linear_x}, Rotation={angular_z}, Time={duration} seconds')
    
    def stop_robot(self):
        """
        Stop the robot by publishing zero velocities.
        
        This method explicitly sets all velocities to zero and publishes the command.
        It's important to explicitly stop the robot to ensure it doesn't continue
        moving due to any buffered commands.
        """
        # Create a new Twist message with zero velocities
        vel = Twist()
        vel.linear.x = 0.0   # No forward/backward motion
        vel.angular.z = 0.0  # No rotation
        # Publish the stop command
        self.publisher.publish(vel)
        # Log that the robot has stopped
        self.get_logger().info('Robot has stopped')

def main(args=None):
    """
    Main function that initializes ROS 2 and runs the robot control sequence.
    
    This function:
    1. Initializes ROS 2
    2. Creates the robot control node
    3. Executes a predefined movement sequence
    4. Handles interruptions gracefully
    5. Cleans up resources
    """
    # Initialize ROS 2 - this must be called before creating any nodes
    rclpy.init(args=args)
    
    # Create an instance of our robot control node
    robot_control = KachakaSimpleControl()

    try:
        # Execute the following movement sequence
        # The sequence demonstrates basic movements: forward, rotation, and stopping
        robot_control.move_robot(0.0, 0.0, 1.0)  # Stop for 1 second (initial pause)
        robot_control.move_robot(0.1, 0.0, 1.0)  # Move forward at 0.1 m/s for 1 second
        robot_control.move_robot(0.0, 0.0, 1.0)  # Stop for 1 second
        robot_control.move_robot(0.0, 0.5, 2.0)  # Rotate left at 0.5 rad/s for 2 seconds
        robot_control.move_robot(0.0, 0.0, 1.0)  # Stop for 1 second
        robot_control.move_robot(0.0, -0.5, 2.0)  # Rotate right at -0.5 rad/s for 2 seconds
        robot_control.move_robot(0.0, 0.0, 1.0)  # Final stop for 1 second
    except KeyboardInterrupt:
        # Handle Ctrl+C gracefully
        # This allows the user to stop the program safely
        robot_control.get_logger().info('Program was interrupted')
    finally:
        # Always execute cleanup, even if an error occurred
        # This ensures the robot stops and resources are freed
        robot_control.stop_robot()      # Make sure robot stops
        robot_control.destroy_node()   # Clean up the node
        rclpy.shutdown()               # Shutdown ROS 2

if __name__ == '__main__':
    main()

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

cd ~/ros2_ws
colcon build --packages-select kachaka_simple_control

source install/setup.bash

ros2 run kachaka_simple_control kachaka_simple_control