22 Tutorial - PID Control

In this tutorial, we will develop a node that controls our vessel in a closed-loop manner using a PID controller.

After completing this tutorial, you will be able to:

Develop a PID controller that executes waypoint tracking
Understand the effect of PID gains on the controller performance
Understand the importance of cascaded control loops

22.1 Getting Started

We will be using the same repository as the previous tutorial. Once the previous tutorial deadline is over, your instructor will update the upstream repository with the necessary files for this tutorial and raise a pull request on your team repository. You will need to pull in the changes from the upstream repository to your team repository to start this tutorial. The instructions to pull in the changes are given in Appendix B.

After you complete the steps shown in Appendix B to synchronize your repository with the upstream repository, navigate to the root directory of your repository. Pull the latest changes made to your team repository onto your local machine.

cd <your-repository-name>
git pull origin main

The relevant folder structure of the repository for this tutorial is shown below.

<your-repository-name>
├── ros2_ws/
│   ├── src/
│   │   ├── student/
│   │   │   ├── tut_02/
│   │   │   ├── tut_03/
│   │   │   ├── tut_04/
│   │   │   ├── tut_05/
│   │   │   ├── tut_06/
│   │   │   ├── tut_07/
│   │   │   ├── tut_08/
│   │   │   ├── tut_09/
│   │   │   └── tut_10/
│   │   │       └── Tutorial_10.md
├── README.md
├── ros2_run_devdocker.sh
├── ros2_run_devdocker.bat
└── LICENSE

Start your own branch from the main branch.

git checkout -b <your-name>

All your group members can work independently on their own branches. However, you will have to merge your branches back to the main branch before the end of the tutorial. We learnt how to do this in Chapter 14 using pull requests.

First start docker desktop and then follow the instructions below to run the course docker container. Make sure to navigate in the terminal to the root folder of your repository.

For Windows:

./ros2_run_devdocker.bat

For Ubuntu (or MacOS):

./ros2_run_devdocker.sh

Source the ROS 2 environment by running the following command:

sros2

Notice that this is an alias for sourcing your ROS 2 workspace as well as the ROS 2 installation in the docker container. Remember that every time you perform a colcon build on your workspace, you need to source the ROS 2 environment again with the command above.

22.2 Create a new ROS2 package

Create a new ROS2 package inside /workspaces/mavlab/ros2_ws/src/student/tut_10

cd /workspaces/mavlab/ros2_ws/src/student/tut_10
ros2 pkg create --build-type ament_python mav_pid_controller --dependencies rclpy std_msgs nav_msgs sensor_msgs geometry_msgs python3-yaml --license MIT

22.3 Create a new node

Create a two new nodes inside the package mav_pid_controller

cd /workspaces/mavlab/ros2_ws/src/student/tut_10/mav_pid_controller/mav_pid_controller
touch mav_pid_controller.py
touch waypoints_publisher.py
touch module_control.py
touch waypoints.yml

Copy the following code into the mav_pid_controller.py file:

Code

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from std_msgs.msg import Float64
import yaml
import sys
sys.path.append('/workspaces/mavlab/')

from ros2_ws.src.ref.tut_03.module_kinematics import quat_to_eul
from ros2_ws.src.ref.tut_10.mav_pid_controller.mav_pid_controller import module_control as con
import numpy as np

class MAVTurningCircleController(Node):
    def __init__(self):
        super().__init__('mav_turning_circle_controller')
        
        # Create subscriber for odometry
        self.odom_sub = self.create_subscription(
            Odometry,
            'mav_odom',
            self.odom_callback,
            10)
            
        # Create publisher for rudder command
        self.rudder_pub = self.create_publisher(
            Float64,
            'mav_rudder_command',
            10)
        
        # Read waypoints
        self.waypoints, self.waypoints_type = self.read_waypoints()        
        self.waypoint_idx = 1
        
        # Initialize time
        self.start_time = self.get_clock().now()

        # Initialize integrator
        self.ye_int = 0.0
        self.te_int = 0.0
        
        # Select control mode
        self.control_mode = con.pid_control
        
        self.get_logger().info('MAV Turning Circle Controller initialized')

    def read_waypoints(self):
        with open('/workspaces/mavlab/ros2_ws/src/ref/tut_10/mav_pid_controller/mav_pid_controller/waypoints.yml', 'r') as file:
            wps_file = yaml.safe_load(file)
            waypoints = np.array(wps_file['waypoints'])
            waypoints_type = wps_file['waypoints_type']
        return waypoints, waypoints_type
    
    def odom_callback(self, msg):
        # Get current time in seconds
        current_time = self.get_clock().now()
        t = (current_time - self.start_time).nanoseconds / 1e9

        quat = np.array([
            msg.pose.pose.orientation.w,
            msg.pose.pose.orientation.x,
            msg.pose.pose.orientation.y,
            msg.pose.pose.orientation.z
        ])

        eul = quat_to_eul(quat, order='ZYX')
        
        # Create state vector from odometry
        state = np.array([            
            msg.twist.twist.linear.x,
            msg.twist.twist.linear.y,
            msg.twist.twist.linear.z,
            msg.twist.twist.angular.x,
            msg.twist.twist.angular.y,
            msg.twist.twist.angular.z,
            msg.pose.pose.position.x,
            msg.pose.pose.position.y,
            msg.pose.pose.position.z,
            eul[0],
            eul[1],
            eul[2],
            0.0 # rudder angle
        ])

        # Calculate control command
        rudder_cmd, ye, self.waypoint_idx = self.control_mode(t, state, self.waypoints, self.waypoint_idx, self.ye_int)

        # Calculate time difference for integration
        dt = t - self.te_int
        if dt > 0:  # Avoid division by zero or negative time
            # Update the integral term
            self.ye_int += ye * dt
            self.te_int = t
        
        # Publish command
        cmd_msg = Float64()
        cmd_msg.data = float(rudder_cmd)
        self.rudder_pub.publish(cmd_msg)
        
        # Log info
        self.get_logger().info(
            f'\nTime: {t:.2f}s\n'            
            f'Rudder Command: {rudder_cmd*180/np.pi:.2f} deg\n'
        )

def main(args=None):
    rclpy.init(args=args)    
    controller = MAVTurningCircleController()
    rclpy.spin(controller)
    controller.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Copy the following code into the waypoints_publisher.py file:

Code

#!/usr/bin/env python3

import os
import yaml
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseArray, Pose
from ament_index_python.packages import get_package_share_directory

class WaypointsPublisher(Node):
    def __init__(self):
        super().__init__('waypoints_publisher')
        self.publisher = self.create_publisher(PoseArray, '/mav_waypoints', 10)
        self.timer = self.create_timer(1.0, self.publish_waypoints)  # Publish every second
        
        # Try to find waypoints.yml
        try:
            package_dir = get_package_share_directory('mav_pid_controller')
            waypoints_path = os.path.join(package_dir, 'waypoints.yml')
            
            if not os.path.exists(waypoints_path):
                # Try in the package source directory
                package_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
                waypoints_path = os.path.join(package_dir, 'waypoints.yml')
            
            self.get_logger().info(f'Loading waypoints from: {waypoints_path}')
            with open(waypoints_path, 'r') as file:
                self.waypoints_data = yaml.safe_load(file)
                self.get_logger().info(f'Loaded waypoints: {self.waypoints_data}')
        except Exception as e:
            self.get_logger().error(f'Failed to load waypoints: {str(e)}')
            self.waypoints_data = None
    
    def publish_waypoints(self):
        if not self.waypoints_data:
            return
            
        pose_array = PoseArray()
        pose_array.header.stamp = self.get_clock().now().to_msg()
        pose_array.header.frame_id = "map"
        
        for waypoint in self.waypoints_data.get('waypoints', []):
            pose = Pose()
            pose.position.x = float(waypoint[0])
            pose.position.y = float(waypoint[1])
            pose.position.z = float(waypoint[2])
            pose_array.poses.append(pose)
        
        self.publisher.publish(pose_array)
        
def main(args=None):
    rclpy.init(args=args)
    node = WaypointsPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Copy the following code into the module_control.py file:

Code

import numpy as np

"""
Rudder control module for vessel simulation.

This module provides different rudder control strategies for vessel steering:
"""

def ssa(ang, deg=False):
    """
    Smallest signed angle that lies between -pi and pi.
    If deg is True, the angle is assumed to be in degrees and is converted to radians.
    If deg is False, the angle is assumed to be in radians.

    Args:
        ang (float): Angle in degrees or radians
        deg (bool): If True, the angle is assumed to be in degrees

    Returns:
        float: Smallest signed angle between -pi and pi. The angle is in radians if deg is False, and in degrees if deg is True.
    """

    #===========================================================================
    # TODO: Implement the ssa function
    #===========================================================================
    # Write your code here

    pass

    #===========================================================================
    return ang

def clip(val, min_val, max_val):
    """
    Clip a value to a range.
    """
    return max(min_val, min(val, max_val))


def pid_control(t, state, waypoints, waypoint_idx, ye_int=0.0):
    """
    Implement a PID control strategy to follow the waypoints. 
    
    Args:
        t (float): Current simulation time [s]
        state (ndarray): Current vessel state vector
        waypoints (ndarray): Waypoints array
        waypoint_idx (int): Current waypoint index
        ye_int (float): Integral term of cross-track error
        
    Returns:
        float: Commanded rudder angle in radians
        int: Next waypoint index
    """

    # Check if we have reached the last waypoint
    if waypoint_idx == len(waypoints):
        return 0.0, waypoint_idx

    # Current state
    u, v, r = state[3], state[4], state[5]
    x, y, psi = state[6], state[7], state[11]
    
    # Current and previous waypoints
    wp_xn, wp_yn, _ = waypoints[waypoint_idx]
    wp_xn1, wp_yn1, _ = waypoints[waypoint_idx - 1]

    delta_c = 0.0
    ye = 0.0
    #===========================================================================
    # TODO: Implement the PID control
    #===========================================================================
    # Write your code here

    pass

    #===========================================================================
    
    # Clip the rudder angle
    delta_c = clip(delta_c, -35*np.pi/180, 35*np.pi/180)

    # Distance to waypoint
    wp_dist = np.linalg.norm(np.array([x - wp_xn, y - wp_yn, 0.0]))
    if wp_dist < 0.5:
        waypoint_idx += 1

    # Do not change the following line (the negative sign is due to the sign convention of the rudder angle)
    return -delta_c, ye, waypoint_idx

Copy the following lines into the waypoints.yml file:

waypoints_type: XYZ
waypoints:
  - [0.0, 0.0, 0.0]
  - [0.0, 20.0, 0.0]
  - [20.0, 20.0, 0.0]
  - [20.0, 0.0, 0.0]
  - [0.0, 0.0, 0.0]

The file mav_pid_controller.py starts a ROS2 node that subscribes to the odometry data from the topic mav_odom and publishes the rudder angle to the topic mav_rudder_command. The file waypoints_publisher.py starts a ROS2 node that publishes the waypoints to the topic mav_waypoints. The module_control.py file contains the implementation of the PID control strategy that you need to complete. The waypoints.yml file contains the waypoints that the vehicle should follow.

22.4 PID Control Strategy

In this tutorial, we will be using a PID control strategy to follow the waypoints. The traditional way to implement the PID controller is to have a single loop control strategy. The rudder angle in this case would be computed as a function of the heading error and the error in position. However, in this tutorial, we will be using a cascaded control strategy.

We shall separate the control strategy into two parts:

The inner loop control strategy that computes the rudder angle to track a desired heading angle and yaw rate
The outer loop control strategy that computes the desired heading angle and yaw rate based on the waypoints

22.4.1 Inner Loop Control Strategy

The inner loop control strategy aims to track a desired heading angle and yaw rate. It is assumed that the desired heading angle and desired yaw rate are known from the outer loop control strategy. The inner loop only aims to ensure that the desired heading angle and desired yaw rate are tracked. We will use a PD controller to achieve this. The commanded rudder angle \(\delta_c\) is computed as a function of the heading error and the error in yaw rate and is shown below:

\[\begin{align} \delta_c = K_{pi} (\psi_{des} - \psi) + K_{di} (r_{des} - r) \end{align}\]

where \(\psi_{des}\) is the desired heading angle, \(\psi\) is the current heading angle, \(r_{des}\) is the desired yaw rate, \(r\) is the current yaw rate, \(K_{pi}\) is the inner loop proportional gain, and \(K_{di}\) is the inner loop derivative gain.

Note that the proportional term contains the difference between the desired heading angle and the current heading angle. Since angles wrap around at \(\pm \pi\), we need to be careful when computing the difference. Consider the following two cases to understand this better:

If the desired heading angle is \(170^\circ\) and the current heading angle is \(180^\circ\), the difference is \(-10^\circ\).
If the desired heading angle is \(170^\circ\) and the current heading angle is \(-170^\circ\), the difference is \(340^\circ\).

Notice that in the second case, the difference is actually \(-20^\circ\) and not \(340^\circ\). When practically implementing heading control, we need to ensure that the difference is computed correctly. The ssa() function in the module_control.py file implements this logic. Thus, when implementing the pid_control() function, you should use the ssa() function to compute the heading error.

Assuming a Nomoto’s first order model, you can tune the gains of the inner loop controller. The dynamics of the inner loop is given by:

\[\begin{align} T \ddot{\psi} + \dot{\psi} = K K_{pi} (\psi_{des} - \psi) + K K_{di} (\dot{\psi}_{des} - \dot{\psi}) \end{align}\]

where \(T\) is the Nomoto time constant of the vehicle and \(K\) is the Nomoto gain. This is a second order system and the methods discussed in Chapter 11 and Chapter 12 can be used to design the gains of the inner loop controller.

22.4.2 Outer Loop Control Strategy

As long as the gains of the inner loop PD controller are chosen correctly, the vehicle should follow the desired heading angle and yaw rate. The outer loop control strategy will now compute the desired heading angle and yaw rate based on the position of the vehicle with respect to the waypoints. Ideally, the vehicle should follow the waypoints in a straight line. However, in practice, the vehicle at any arbitrary time will not necessarily be on the straight line connecting the previous and next waypoints. We define the cross-track error \(y_e\) as the perpendicular distance from the vehicle to the line connecting the previous and next waypoints. The desired heading angle and desired yaw rate are specified as a PI controller as follows:

\[\begin{align} \psi_{des} &= K_{po} (0 - y_e) + K_{io} \int (0 - y_e) dt \\ r_{des} &= 0 \end{align}\]

where \(K_{po}\) is the outer loop proportional gain, \(K_{io}\) is the outer loop integral gain, \(y_e\) is the cross-track error, \(\psi_{des}\) is the desired heading angle and \(r_{des}\) is the desired yaw rate. Notice that we have assumed the desired yaw rate to be zero, which is the case when the vessel is on the line connecting the previous and next waypoints. You could however, calculate the desired yaw rate by differentiating the desired heading angle and include it in the PI controller.

During the practical implementation pay attention to the convention of cross-track error \(y_e\). If the vehicle is to the right of the line connecting the previous and next waypoints, the cross-track error is positive. If the vehicle is to the left of the line connecting the previous and next waypoints, the cross-track error is negative.

22.5 Implementation Tasks

There are two tasks to complete in this tutorial.

Task 1: Complete the `ssa` function

Location: module_control.py, ssa() function

Description: The ssa() function computes the smallest signed angle that lies between \(-\pi\) and \(\pi\). Thus if an angle of \(350^\circ\) is input (with input option deg=True), the function returns a value of \(-5^\circ\). If instead the angle is input in radians (with input option deg=False), the function returns a value of \(-\pi + 0.174533\) radians.

Input:

ang: Angle in degrees or radians
deg: If True, the angle is assumed to be in degrees and is converted to radians. If False, the angle is assumed to be in radians.

Output:

ang_rad: Smallest signed angle between \(-\pi\) and \(\pi\) if deg is False, and between \(-180^\circ\) and \(180^\circ\) if deg is True.

Task 2: Complete the `pid_control` function

Location: module_control.py, pid_control() function

Description: The pid_control() function implements a PID control strategy to follow the waypoints.

Input:

t: Current simulation time [s]
state: Current vessel state vector
waypoints: Waypoints array
waypoint_idx: Current waypoint index
ye_int: Integral term of cross-track error

Output:

delta_c: Commanded rudder angle in radians
ye: Cross-track error
waypoint_idx: Next waypoint index

22.6 Create launch file

Create a launch file inside /workspaces/mavlab/ros2_ws/src/student/tut_10/mav_pid_controller/launch

cd /workspaces/mavlab/ros2_ws/src/student/tut_10/mav_pid_controller/
mkdir launch
cd launch
touch mav_pid_controller_launch.py

Copy the following code into the mav_pid_controller_launch.py file:

Code

from launch import LaunchDescription
from launch.actions import ExecuteProcess, IncludeLaunchDescription
from launch.launch_description_sources import AnyLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node

def generate_launch_description():
    # Path to the rosbridge launch file
    rosbridge_launch = PathJoinSubstitution([
        FindPackageShare('rosbridge_server'),
        'launch',
        'rosbridge_websocket_launch.xml'
    ])

    # Path to the built web directory
    web_build_dir = PathJoinSubstitution([
        FindPackageShare('mav_pid_controller'),
        'web'
    ])

    return LaunchDescription([
        # Start the HTTP server to serve web files
        ExecuteProcess(
            cmd=['python3', '-m', 'http.server', '8000', '--directory', web_build_dir],
            name='http_server',
            output='screen'
        ),
        # Start the rosbridge server
        IncludeLaunchDescription(
            AnyLaunchDescriptionSource(rosbridge_launch)
        ),
        # Start the MAV simulator
        Node(
            package='mav_simulator',
            executable='mav_simulate',
            name='mav_simulate',
            output='screen'
        ),
        # Add waypoints publisher node
        Node(
            package='mav_pid_controller',
            executable='waypoints_publisher',
            name='waypoints_publisher',
            output='screen'
        ),
        # Start the MAV turning circle controller
        Node(
            package='mav_pid_controller',
            executable='mav_pid_controller',
            name='mav_pid_controller',
            output='screen'
        ),
    ])

The launch file starts a HTTP server in the web folder of the package mav_openloop_controller (developed in Chapter 21) to serve the web files and a rosbridge server to communicate with the ROS 2 nodes. As we will be using javascript to visualize the data in our web application, we need to start the rosbridge server to communicate with the ROS 2 nodes. We also start the MAV simulator node from the mav_simulator package we developed in Chapter 16. Finally we start the PID controller node from the mav_pid_controller package.

22.7 Create a new web directory

Create a new web directory inside /workspaces/mavlab/ros2_ws/src/student/tut_10/mav_pid_controller

cd /workspaces/mavlab/ros2_ws/src/student/tut_10/mav_pid_controller
mkdir web

Create a new index.html file inside /workspaces/mavlab/ros2_ws/src/student/tut_10/mav_pid_controller/web

cd /workspaces/mavlab/ros2_ws/src/student/tut_10/mav_pid_controller/web
touch index.html

Copy the following code into the index.html file:

<!DOCTYPE html>
<html lang="en">
<head>
  <meta charset="UTF-8" />
  <title>Live Odometry Visualization</title>
  <!-- Include Chart.js (from CDN or your local node_modules build) -->
  <script src="https://cdn.jsdelivr.net/npm/chart.js"></script>
  <!-- Include ROSLIBJS (from your node_modules or a CDN) -->  
  <script src="https://cdnjs.cloudflare.com/ajax/libs/roslibjs/1.1.0/roslib.min.js" integrity="sha512-x2Owc9WayRcRj80Znkau58shVfXN2OIX+gQAlrx6KPugZBKrIC6AwgEWQQCI06p2Q8RB4ilxD+y+1BdNd+1fQA==" crossorigin="anonymous" referrerpolicy="no-referrer"></script>
  <style>
    body {
      font-family: 'Segoe UI', Tahoma, Geneva, Verdana, sans-serif;
      max-width: 1200px;
      margin: 0 auto;
      padding: 20px;
      background-color: #f5f5f5;
    }
    
    h1, h2 {
      color: #2c3e50;
      text-align: center;
      margin-top: 30px;
    }

    h1 {
      font-size: 2.5em;
      margin-bottom: 40px;
      border-bottom: 2px solid #3498db;
      padding-bottom: 10px;
    }

    .chart-container {
      background: white;
      border-radius: 10px;
      box-shadow: 0 4px 6px rgba(0, 0, 0, 0.1);
      padding: 20px;
      margin: 20px auto;
      max-width: 800px;
    }

    canvas {
      max-width: 100%;
      height: auto;
      margin: 10px auto;
      display: block;
    }
  </style>
</head>
<body>
  <h1>Odometry Data Visualization</h1>

  <div class="chart-container">
    <h2>Path</h2>
    <canvas id="pathChart"></canvas>
  </div>

  <div class="chart-container">
    <h2>Position (m)</h2>
    <canvas id="positionChart"></canvas>
  </div>

  <div class="chart-container">
    <h2>Velocity (m/s)</h2>
    <canvas id="velocityChart"></canvas>
  </div>

  <div class="chart-container">
    <h2>Angular Velocity (rad/s)</h2>
    <canvas id="angularVelocityChart"></canvas>
  </div>

  <div class="chart-container">
    <h2>Euler Angles (degrees)</h2>
    <canvas id="orientationChart"></canvas>    
  </div>

  <div class="chart-container">
    <h2>Rudder (degrees)</h2>
    <canvas id="rudderChart"></canvas>
  </div>

  <!-- Main JavaScript file -->
  <script src="main.js"></script>
</body>
</html>

Create a new main.js file inside /workspaces/mavlab/ros2_ws/src/student/tut_10/mav_pid_controller/web

cd /workspaces/mavlab/ros2_ws/src/student/tut_10/mav_pid_controller/web
touch main.js

Copy the following code into the main.js file:

// Connect to rosbridge websocket (adjust the URL as needed)
var ros = new ROSLIB.Ros({
    url: 'ws://localhost:9090'
  });
  
  ros.on('connection', function () {
    console.log('Connected to rosbridge.');
  });
  ros.on('error', function (error) {
    console.error('Error connecting to rosbridge: ', error);
  });
  
  // Create a subscriber to the /imu/data topic
  var odomTopic = new ROSLIB.Topic({
    ros: ros,
    name: '/mav_odom',
    messageType: 'nav_msgs/Odometry'
  });

  var rudderTopic = new ROSLIB.Topic({
    ros: ros,
    name: '/mav_rudder',
    messageType: 'std_msgs/Float64'
  });
  
  // Create a subscriber for waypoints
  var waypointsTopic = new ROSLIB.Topic({
    ros: ros,
    name: '/mav_waypoints',
    messageType: 'geometry_msgs/PoseArray'
  });
  
  // Helper: convert quaternion to Euler angles
  function quaternionToEuler(q) {
    var x = q.x, y = q.y, z = q.z, w = q.w;
    var sinr_cosp = 2 * (w * x + y * z);
    var cosr_cosp = 1 - 2 * (x * x + y * y);
    var roll = Math.atan2(sinr_cosp, cosr_cosp) * 180 / Math.PI;
  
    var sinp = 2 * (w * y - z * x);
    var pitch = Math.abs(sinp) >= 1 ? Math.sign(sinp) * Math.PI / 2 : Math.asin(sinp) * 180 / Math.PI;
  
    var siny_cosp = 2 * (w * z + x * y);
    var cosy_cosp = 1 - 2 * (y * y + z * z);
    var yaw = Math.atan2(siny_cosp, cosy_cosp) * 180 / Math.PI;
    
    return { roll: roll, pitch: pitch, yaw: yaw };
  }
  
  // Set up Chart.js charts
  function createChart(ctx, label, borderColor) {
    return new Chart(ctx, {
      type: 'line',
      data: {
        labels: [], // time or sample index
        datasets: [{
          label: label,
          data: [],
          borderColor: borderColor,
          fill: false,
          tension: 0.1,
        }]
      },
      options: {
        animation: false,
        responsive: true,
        scales: { x: { display: false } }
      }
    });
  }
  
  // Get canvas contexts
  var pathCtx = document.getElementById('pathChart').getContext('2d');
  var positionCtx = document.getElementById('positionChart').getContext('2d');
  var velocityCtx = document.getElementById('velocityChart').getContext('2d');
  var orientationCtx = document.getElementById('orientationChart').getContext('2d');
  var angularVelocityCtx = document.getElementById('angularVelocityChart').getContext('2d');
  var rudderCtx = document.getElementById('rudderChart').getContext('2d');
  
  // Create charts for each data type.
  var pathChart = new Chart(pathCtx, {
    type: 'scatter',
    data: {
      datasets: [
        {
          label: 'Path',
          data: [],
          borderColor: 'red',
          backgroundColor: 'red',
          showLine: true,
          fill: false
        },
        {
          label: 'Waypoints',
          data: [],
          borderColor: 'blue',
          backgroundColor: 'blue',
          pointRadius: 6,
          pointStyle: 'circle',
          showLine: true,
          borderDash: [5, 5]
        }
      ]
    },
    options: {
      animation: false,
      responsive: true,
      aspectRatio: 1,
      scales: {
        x: { display: true, title: { display: true, text: 'X Position' } },
        y: { display: true, title: { display: true, text: 'Y Position' } }
      }
    }
  });

  // For acceleration and angular velocity, we use one chart per topic (each with three datasets for x, y, z).
  var positionChart = new Chart(positionCtx, {
    type: 'line',
    data: {
      labels: [],
      datasets: [
        { label: 'x', data: [], borderColor: 'red', fill: false },
        { label: 'y', data: [], borderColor: 'green', fill: false },
        { label: 'z', data: [], borderColor: 'blue', fill: false },
      ]
    },
    options: { animation: false, responsive: true, scales: { x: { display: false } } }
  });
  
  var velocityChart = new Chart(velocityCtx, {
    type: 'line',
    data: {
      labels: [],
      datasets: [
        { label: 'u', data: [], borderColor: 'red', fill: false },
        { label: 'v', data: [], borderColor: 'green', fill: false },
        { label: 'w', data: [], borderColor: 'blue', fill: false },
      ]
    },
    options: { animation: false, responsive: true, scales: { x: { display: false } } }
  });
  
  // Create one chart for all orientation angles
  var orientationChart = new Chart(orientationCtx, {
    type: 'line',
    data: {
      labels: [],
      datasets: [
        { label: 'Roll', data: [], borderColor: 'red', fill: false },
        { label: 'Pitch', data: [], borderColor: 'green', fill: false },
        { label: 'Yaw', data: [], borderColor: 'blue', fill: false },
      ]
    },
    options: { animation: false, responsive: true, scales: { x: { display: false } } }
  });

  var angularVelocityChart = new Chart(angularVelocityCtx, {
    type: 'line',
    data: {
      labels: [],
      datasets: [
        { label: 'p', data: [], borderColor: 'red', fill: false },
        { label: 'q', data: [], borderColor: 'green', fill: false },
        { label: 'r', data: [], borderColor: 'blue', fill: false },
      ]
    },
    options: { animation: false, responsive: true, scales: { x: { display: false } } }
  });

  var rudderChart = new Chart(rudderCtx, {
    type: 'line',
    data: {
      labels: [],
      datasets: [
        { label: 'delta', data: [], borderColor: 'red', fill: false }        
      ]
    },
    options: { animation: false, responsive: true, scales: { x: { display: false } } }
  });
  
  var sampleIndex = 0;

  function updatePathChart(x, y) {
    pathChart.data.datasets[0].data.push({x: x, y: y});
    if (pathChart.data.datasets[0].data.length > 100) {
      pathChart.data.datasets[0].data.shift();
    }
    pathChart.update();
  }
  
  function updatePositionChart(x, y, z) {
    positionChart.data.labels.push(sampleIndex);
    positionChart.data.datasets[0].data.push(x);
    positionChart.data.datasets[1].data.push(y);
    positionChart.data.datasets[2].data.push(z);
    if (positionChart.data.labels.length > 100) {
      positionChart.data.labels.shift();
      positionChart.data.datasets.forEach(ds => ds.data.shift());
    }
    positionChart.update();
  }
  
  function updateVelocityChart(u, v, w) {
    velocityChart.data.labels.push(sampleIndex);
    velocityChart.data.datasets[0].data.push(u);
    velocityChart.data.datasets[1].data.push(v);
    velocityChart.data.datasets[2].data.push(w);
    if (velocityChart.data.labels.length > 100) {
      velocityChart.data.labels.shift();
      velocityChart.data.datasets.forEach(ds => ds.data.shift());
    }
    velocityChart.update();
  }

  function updateAngularVelocityChart(p, q, r) {
    angularVelocityChart.data.labels.push(sampleIndex);
    angularVelocityChart.data.datasets[0].data.push(p);
    angularVelocityChart.data.datasets[1].data.push(q);
    angularVelocityChart.data.datasets[2].data.push(r);
    if (angularVelocityChart.data.labels.length > 100) {
      angularVelocityChart.data.labels.shift();
      angularVelocityChart.data.datasets.forEach(ds => ds.data.shift());
    }
    angularVelocityChart.update();
  }

  function updateOrientationChart(roll, pitch, yaw) {
    orientationChart.data.labels.push(sampleIndex);
    orientationChart.data.datasets[0].data.push(roll);
    orientationChart.data.datasets[1].data.push(pitch);
    orientationChart.data.datasets[2].data.push(yaw);
    if (orientationChart.data.labels.length > 100) {
      orientationChart.data.labels.shift();
      orientationChart.data.datasets.forEach(ds => ds.data.shift());
    }
    orientationChart.update();
  }

  function updateRudderChart(delta) {
    rudderChart.data.labels.push(sampleIndex);
    rudderChart.data.datasets[0].data.push(delta);
    if (rudderChart.data.labels.length > 100) {
      rudderChart.data.labels.shift();
      rudderChart.data.datasets.forEach(ds => ds.data.shift());
    }
    rudderChart.update();
  }
  
  // Subscribe to the /imu/data topic
  odomTopic.subscribe(function (message) {
    // Increment our sample index for the x-axis
    sampleIndex++;
  
    // Extract accelerations and angular velocities
    var x = message.pose.pose.position.x;
    var y = message.pose.pose.position.y;
    var z = message.pose.pose.position.z;
    var u = message.twist.twist.linear.x;
    var v = message.twist.twist.linear.y;
    var w = message.twist.twist.linear.z;
    var p = message.twist.twist.angular.x;
    var q = message.twist.twist.angular.y;
    var r = message.twist.twist.angular.z;
    
    // Convert quaternion to Euler angles
    var euler = quaternionToEuler(message.pose.pose.orientation); // returns {roll, pitch, yaw}
    
    // Update the charts
    updatePathChart(x, y);
    updatePositionChart(x, y, z);
    updateVelocityChart(u, v, w);
    updateAngularVelocityChart(p, q, r);
    updateOrientationChart(euler.roll, euler.pitch, euler.yaw);
  });

  rudderTopic.subscribe(function (message) {
    
    var delta = message.data;

    updateRudderChart(delta);
  });

  // Subscribe to waypoints topic
  waypointsTopic.subscribe(function (message) {
    // Clear existing waypoints
    pathChart.data.datasets[1].data = [];
    
    // Add each waypoint to the chart
    if (message.poses) {
      message.poses.forEach(function(pose) {
        pathChart.data.datasets[1].data.push({
          x: pose.position.x,
          y: pose.position.y
        });
      });
      
      pathChart.update();
    }
  });

The main.js file is a javascript file that will be used to visualize the data in our web application. It contains subscribers to the topics mav_odom and mav_rudder and a function to update the charts with the latest data.

22.8 Modify the setup.py file

Replace the existing code in the setup.py file with the following code:

Code

from setuptools import find_packages, setup
import os
from glob import glob

package_name = 'mav_pid_controller'

setup(
    name=package_name,
    version='0.0.1',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
        (os.path.join('share', package_name, 'web'), glob('web/*')),
        ('share/mav_pid_controller', ['mav_pid_controller/waypoints.yml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Abhilash Somayajula',
    maintainer_email='abhilash@iitm.ac.in',
    description='MAV PID Controller',
    license='MIT',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'mav_pid_controller = mav_pid_controller.mav_pid_controller:main',
            'waypoints_publisher = mav_pid_controller.waypoints_publisher:main',
        ],
    },
)

22.9 Build the package

Build the package using the following command:

cd /workspaces/mavlab/ros2_ws
rm -rf build install log
colcon build
sros2

22.10 Run the launch file

Run the launch file using the following command:

ros2 launch mav_pid_controller mav_pid_controller_launch.py

Open the web application by navigating to the following URL:

http://localhost:8000

You should see the data from the odometry topic being visualized in the web application. By default, the vehicle should simply be moving forward as no control algorithm is implemented yet. After completing the tasks, the vehicle should follow the waypoints.

22.11 Evaluation

The evaluation of the tutorial will be performed in the classroom. Notice that this is a group activity. You will be working in a group of 4 students. The evaluation will be performed on the group level. In order to pass the tutorial, the group must pass all the tests in the GitHub Actions workflow and also be able to answer all the questions during the evaluation in the classroom. Those who are not present in the classroom on the day of evaluation will not be able to participate in the evaluation and will automatically fail the tutorial.

While one of the objectives of these tutorials is to give you hands on experience with coding and practical implementation, the more important goal is to help you relate what you are doing in your tutorials to the underlying concepts discussed in the class. Therefore, while you are working on the implementation tasks, make sure you are also thinking about the underlying concepts and how they are being applied in the code.

Some questions to think about while you are working on the implementation tasks:

What is the effect of the PID gains on the controller performance?
How does the PID controller compare to the open-loop controller used in the previous tutorial?
What is the effect of the integral term on the controller performance?
What is the effect of the derivative term on the controller performance?
Does the cascaded control strategy offer any advantages over a single loop PID controller?
Why do we not implement the integral term in the inner loop?

22.12 Instructor Feedback through Pull Requests

Commit your changes to the remote repository often. This will help you to track your progress and also help you to revert to a working version if you make a mistake. In addition, the instructor and TA will be able to provide you with feedback on your code through the Feedback pull request available in your repository. Please do not close this pull request.

22.1 Getting Started

22.2 Create a new ROS2 package

22.3 Create a new node

22.4 PID Control Strategy

22.4.1 Inner Loop Control Strategy

22.4.2 Outer Loop Control Strategy

22.5 Implementation Tasks

Task 1: Complete the ssa function

Task 2: Complete the pid_control function

22.6 Create launch file

22.7 Create a new web directory

22.8 Modify the setup.py file

22.9 Build the package

22.10 Run the launch file

22.11 Evaluation

22.12 Instructor Feedback through Pull Requests

Task 1: Complete the `ssa` function

Task 2: Complete the `pid_control` function