Feedback Control

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python kachaka_feedback_control --dependencies rclpy geometry_msgs nav_msgs tf_transformations numpy --node-name kachaka_feedback_control

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

#!/usr/bin/env python3
import math
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import numpy as np
from tf_transformations import euler_from_quaternion

# Imports for QoS settings
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy

class KachakaFeedbackControl(Node):
    def __init__(self):
        super().__init__('kachaka_feedback_control')

        # Define a QoS profile with BEST_EFFORT reliability
        # QoS (Quality of Service) settings control how messages are delivered
        # BEST_EFFORT means messages may be dropped if the system is busy
        # This is appropriate for odometry data where real-time performance is more
        # important than guaranteed delivery of every message
        qos_profile = QoSProfile(
            history=QoSHistoryPolicy.KEEP_LAST,  # Keep only the last N messages
            depth=10,  # Keep the last 10 messages in the queue
            reliability=QoSReliabilityPolicy.BEST_EFFORT  # Allow message loss for speed
        )

        # Publisher for velocity commands
        self.cmd_vel_pub = self.create_publisher(Twist, '/kachaka/manual_control/cmd_vel', 10)

        # Subscriber for odometry data with BEST_EFFORT QoS
        self.odom_sub = self.create_subscription(
            Odometry,
            '/kachaka/odometry/odometry',
            self.callback_odom,
            qos_profile
        )

        # Initialize position variables to None
        # These will be set when we receive the first odometry message
        self.x = None   # X position in meters
        self.y = None   # Y position in meters
        self.yaw = None # Orientation angle in radians

        # Wait for the first odometry message to be received
        # This is important because we need initial position data before we can
        # calculate movements. Without this wait, the control methods would fail
        # because they depend on knowing the starting position.
        while self.x is None or self.y is None or self.yaw is None:
            # Process callbacks to receive the odometry message
            rclpy.spin_once(self, timeout_sec=0.1)

    def callback_odom(self, msg):
        """
        Callback function that is called whenever a new odometry message is received.
        
        Odometry provides the robot's estimated position and orientation based on
        wheel encoders and other sensors. This callback updates our knowledge of
        where the robot currently is.
        
        Parameters:
            msg: Odometry message containing pose (position + orientation) information
        """
        # Extract the X coordinate from the odometry message
        # The message structure is: msg.pose.pose.position.x
        self.x = msg.pose.pose.position.x
        # Extract the Y coordinate
        self.y = msg.pose.pose.position.y
        # Extract the yaw (rotation around Z-axis) from the quaternion
        # Quaternions are used to represent 3D rotations, but we only need the yaw angle
        self.yaw = self.get_yaw_from_quaternion(msg.pose.pose.orientation)

    def get_yaw_from_quaternion(self, quaternion):
        """
        Convert a quaternion to Euler angles and extract the yaw angle.
        
        Quaternions are a way to represent 3D rotations without gimbal lock issues.
        For a mobile robot moving on a flat surface, we only need the yaw angle
        (rotation around the vertical Z-axis), which tells us which direction
        the robot is facing.
        
        Parameters:
            quaternion: Quaternion object with x, y, z, w components
        
        Returns:
            yaw: Rotation angle around Z-axis in radians [0 to 2π]
        """
        # Convert quaternion to Euler angles (roll, pitch, yaw)
        # The function returns a tuple: (roll, pitch, yaw)
        e = euler_from_quaternion((quaternion.x, quaternion.y, quaternion.z, quaternion.w))
        # Return only the yaw angle (index 2)
        return e[2]

    def go_straight(self, distance, velocity=0.3):
        """
        Move straight for the specified distance using feedback control.
        
        This method uses the robot's current position (from odometry) to determine
        when it has traveled the target distance. This is feedback control because
        we're measuring the output (actual distance traveled) and adjusting until
        we reach the target.
        
        Parameters:
            distance: Target distance to travel in meters
            velocity: Speed at which to travel [m/s] (default: 0.3 m/s)
        """
        vel = Twist()
        # Record the starting position
        # We'll compare the current position to this to calculate distance traveled
        x0 = self.x
        y0 = self.y
        
        # Continue moving until we've traveled the target distance
        # Calculate Euclidean distance: sqrt((x-x0)² + (y-y0)²)
        # This gives us the straight-line distance from the starting point
        while np.sqrt((self.x - x0)**2 + (self.y - y0)**2) < distance:
            # Set velocity: forward motion, no rotation
            vel.linear.x = velocity
            vel.angular.z = 0.0
            # Publish the velocity command
            self.cmd_vel_pub.publish(vel)
            # Process callbacks to update position from odometry
            rclpy.spin_once(self, timeout_sec=0.1)
        
        # Stop when we've reached the target distance
        self.stop()

    def turn_right(self, angle_degree, yawrate=-0.5):
        """
        Turn right by the specified angle using feedback control.
        
        This method measures the actual rotation angle and continues turning until
        the target angle is reached. The angle difference calculation handles
        the wrap-around at 2π radians (360 degrees).
        
        Parameters:
            angle_degree: Target rotation angle in degrees
            yawrate: Rotation speed [rad/s] (default: -0.5 rad/s, negative = right turn)
        """
        vel = Twist()
        # Record the starting orientation
        yaw0 = self.yaw
        # Convert target angle from degrees to radians
        target_angle = math.radians(angle_degree)
        
        # Continue turning until we've rotated the target angle
        # The angle difference calculation uses atan2 to handle wrap-around correctly
        # This ensures the calculation works even if we cross the 0/2π boundary
        while abs(math.atan2(math.sin(self.yaw - yaw0), math.cos(self.yaw - yaw0))) < target_angle:
            # Set velocity: no forward motion, rotate right (negative angular velocity)
            vel.linear.x = 0.0
            vel.angular.z = yawrate
            # Publish the velocity command
            self.cmd_vel_pub.publish(vel)
            # Process callbacks to update orientation from odometry
            rclpy.spin_once(self, timeout_sec=0.1)
        
        # Stop when we've reached the target angle
        self.stop()

    def turn_left(self, angle_degree, yawrate=0.5):
        """
        Turn left by the specified angle (in degrees) using a positive yaw rate.
        """
        vel = Twist()
        yaw0 = self.yaw
        target_angle = math.radians(angle_degree)
        # Normalize the angle difference using atan2
        while abs(math.atan2(math.sin(self.yaw - yaw0), math.cos(self.yaw - yaw0))) < target_angle:
            vel.linear.x = 0.0
            vel.angular.z = yawrate
            self.cmd_vel_pub.publish(vel)
            rclpy.spin_once(self, timeout_sec=0.1)
        self.stop()

    def stop(self):
        """Stop the robot by publishing zero velocities."""
        vel = Twist()
        vel.linear.x = 0.0
        vel.angular.z = 0.0
        self.cmd_vel_pub.publish(vel)

def main(args=None):
    rclpy.init(args=args)
    kachaka_feedback_control = KachakaFeedbackControl()

    try:
        # Example movement commands: go straight, turn left, then turn right
        kachaka_feedback_control.go_straight(1.0)
        kachaka_feedback_control.turn_left(90)
        kachaka_feedback_control.turn_right(90)
    finally:
        kachaka_feedback_control.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

    entry_points={
        'console_scripts': [
            'kachaka_feedback_control = kachaka_feedback_control.kachaka_feedback_control:main'
        ],

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

source install/setup.bash

ros2 run kachaka_feedback_control kachaka_feedback_control