21 Tutorial - Open-loop Control

In this tutorial, we will develop a node that controls our vessel in an open-loop manner.

After completing this tutorial, you will be able to:

Develop a controller that executes a turning circle maneuver
Develop a controller that executes a zigzag maneuver
Develop a controller that executes a spiral maneuver

21.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/
│   │   │       └── Tutorial_09.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.

21.2 Create a new ROS2 package

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

cd /workspaces/mavlab/ros2_ws/src/student/tut_09
ros2 pkg create --build-type ament_python mav_openloop_controller --dependencies rclpy std_msgs nav_msgs sensor_msgs geometry_msgs --license MIT

21.3 Create a new node

Create a three nodes inside the package mav_openloop_controller

cd /workspaces/mavlab/ros2_ws/src/student/tut_09/mav_openloop_controller/mav_openloop_controller
touch mav_turning_circle.py
touch mav_zigzag.py
touch mav_spiral.py
touch module_control.py

Copy the following code into the mav_turning_circle.py file:

Code

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from std_msgs.msg import Float64

import sys
sys.path.append('/workspaces/mavlab/')

from ros2_ws.src.student.tut_03.module_kinematics import quat_to_eul
from ros2_ws.src.student.tut_09.mav_openloop_controller.mav_openloop_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)
            
        # Initialize time
        self.start_time = self.get_clock().now()
        
        # Select control mode
        self.control_mode = con.turning_circle
        
        self.get_logger().info('MAV Turning Circle Controller initialized')

    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 = self.control_mode(t, state, delta0=35.0)
        
        # 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 mav_zigzag.py file:

Code

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from std_msgs.msg import Float64

import sys
sys.path.append('/workspaces/mavlab/')

from ros2_ws.src.student.tut_09.mav_openloop_controller.mav_openloop_controller import module_control as con
from ros2_ws.src.student.tut_03.module_kinematics import quat_to_eul
import numpy as np

class MAVZigzagController(Node):
    def __init__(self):
        super().__init__('mav_zigzag_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)
            
        # Initialize time
        self.start_time = self.get_clock().now()
        
        # Select control mode
        self.control_mode = con.zigzag
        
        self.get_logger().info('MAV Zigzag Controller initialized')

    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 = self.control_mode(t, state, delta0=20.0, psi0=10.0)
        
        # 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 = MAVZigzagController()
    rclpy.spin(controller)
    controller.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Copy the following code into the mav_spiral.py file:

Code

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from std_msgs.msg import Float64

import sys
sys.path.append('/workspaces/mavlab/')

from ros2_ws.src.student.tut_09.mav_openloop_controller.mav_openloop_controller import module_control as con
from ros2_ws.src.student.tut_03.module_kinematics import quat_to_eul
import numpy as np

class MAVSpiralController(Node):
    def __init__(self):
        super().__init__('mav_spiral_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)
            
        # Initialize time
        self.start_time = self.get_clock().now()
        
        # Select control mode
        self.control_mode = con.spiral
        
        self.get_logger().info('MAV Spiral Controller initialized')

    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 = self.control_mode(t, state, wait_time=10.0, start_angle=-35.0, end_angle=35.0, increment=5.0)
        
        # 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 = MAVSpiralController()
    rclpy.spin(controller)
    controller.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 turning_circle(t, state, delta0):
    """
    Implement a turning circle control strategy. 
    
    For the first 20 seconds, the rudder is fixed at 0 degrees. 
    After 20 seconds, the rudder is fixed at user-defined angle delta0.

    Args:
        t (float): Current simulation time [s]
        state (ndarray): Current vessel state vector
        delta0 (float): User-defined rudder angle in degrees

    Returns:
        float: Commanded rudder angle in radians
    """
    delta_c = 0.0
    #===========================================================================
    # TODO: Implement the turning circle control
    #===========================================================================
    # Write your code here

    pass

    #===========================================================================

    return delta_c

def zigzag(t, state, delta0, psi0):
    """
    Implement a zigzag control strategy. 
    
    For the first 20 seconds, the rudder is fixed at 0 degrees. 
    After 20 seconds, the rudder is moved to a user-defined angle delta0.
    The rudder is held at this angle until the vessel has turned by a user-defined heading angle psi0.
    Then the rudder is moved to -delta0 and held at this angle until the vessel has turned to -psi0.
    This cycle repeats indefinitely.

    Args:
        t (float): Current simulation time [s]
        state (ndarray): Current vessel state vector
        delta0 (float): User-defined rudder angle in degrees
        psi0 (float): User-defined heading angle in degrees

    Returns:
        float: Commanded rudder angle in radians
    """
    delta_c = 0.0    
    
    # Get current heading from state vector (index 11 is the yaw/heading angle)
    psi = state[11]
    
    # Convert user-defined angles to radians
    delta0_rad = delta0 * np.pi / 180.0
    psi0_rad = psi0 * np.pi / 180.0
    
    # Static variable to track the current phase of zigzag
    if not hasattr(zigzag, "phase"):
        zigzag.phase = 0  # 0: initial, 1: turning starboard, 2: turning port
        zigzag.initial_heading = psi

    #===========================================================================
    # TODO: Implement the zigzag control
    #===========================================================================
    # Write your code inside the following block

    
    # Initial phase (first 20 seconds)
    if t <= 20:
        delta_c = 0.0
    else:
        # After 20 seconds, start zigzag maneuver
        if zigzag.phase == 0:
            # Start turning starboard
            delta_c = 0.0
            zigzag.phase = 1
        elif zigzag.phase == 1:
            # Continue turning starboard until reaching psi0
            delta_c = 0.0
            # Check if we've turned enough to the starboard            
        elif zigzag.phase == 2:
            # Continue turning port until reaching -psi0
            delta_c = 0.0
            # Check if we've turned enough to the port             
    #===========================================================================
    return delta_c

def spiral(t, state, wait_time, start_angle, end_angle, increment):
    """
    Implement a spiral control strategy.
    
    For the first 20 seconds, the rudder is fixed at 0 degrees. 
    After 20 seconds, the rudder is moved to a user-defined start angle.
    The rudder is then incrementally increased by a user-defined increment until the end angle is reached.
    The rudder is held at each incremented angle for a user-defined wait time.
    The rudder is returned to zero after the end angle is reached.

    Args:
        t (float): Current simulation time [s]
        state (ndarray): Current vessel state vector
        wait_time (float): Time to wait before incrementing the rudder angle [s]
        start_angle (float): User-defined start angle in degrees
        end_angle (float): User-defined end angle in degrees
        increment (float): User-defined increment in degrees

    Returns:
        float: Commanded rudder angle in radians
    """

    # Verify that increment is in the correct direction
    if start_angle < end_angle and increment <= 0:
        raise ValueError("Increment must be positive when start_angle is less than end_angle")
    elif start_angle > end_angle and increment >= 0:
        raise ValueError("Increment must be negative when start_angle is greater than end_angle")
    elif increment == 0:
        raise ValueError("Increment must be non-zero")

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

    pass

    #===========================================================================

    return delta_c

The three files mav_turning_circle.py, mav_zigzag.py and mav_spiral.py each start 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. Each of these nodes implements a different control strategy. The module_control.py file contains the implementation of the three control strategies that you need to complete.

21.3.1 Implentation Tasks

There are four tasks to complete in this tutorial. In each of these tasks, the vessel state vector is assumed to be \(13 \times 1\) vector as defined in Chapter 15. The first three elements denote the components of the linear velocity of the vessel in the BCS. The next three elements denote the components of the angular velocity of the vessel in the BCS. The next three elements denote the position of the vessel in the GCS. The next three elements denote the Euler angles of the vessel in the GCS. The last element denotes the actual rudder angle.

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 `turning_circle` function

Location: module_control.py, turning_circle() function

Description: The turning_circle() function implements a turning circle control strategy. For the first \(20\) seconds, the rudder is fixed at \(0^\circ\). After \(20\) seconds, the rudder is fixed at a user-defined angle \(\delta_0\). We call upon this function from the mav_turning_circle.py node with a value of \(\delta_0 = 35^\circ\).

Input:

t: Current simulation time [s]
state: Current vessel state vector
delta0: User-defined rudder angle in degrees

Output:

delta_c: Commanded rudder angle in radians

Task 3: Complete the `zigzag` function

Location: module_control.py, zigzag() function

Description: The zigzag() function implements a zigzag control strategy. For the first \(20\) seconds, the rudder is fixed at \(0^\circ\). After \(20\) seconds, the rudder is moved to a user-defined angle \(\delta_0\). The rudder is held at this angle until the vessel has turned by a user-defined heading angle \(\psi_0\). Then the rudder is moved to \(-\delta_0\) and held at this angle until the vessel has turned to \(-\psi_0\). This cycle repeats indefinitely. We call upon this function from the mav_zigzag.py node with a value of \(\delta_0 = 20^\circ\) and \(\psi_0 = 10^\circ\).

Input:

t: Current simulation time [s]
state: Current vessel state vector
delta0: User-defined rudder angle in degrees
psi0: User-defined heading angle in degrees

Output:

delta_c: Commanded rudder angle in radians

Task 4: Complete the `spiral` function

Location: module_control.py, spiral() function

Description: The spiral() function implements a spiral control strategy. For the first \(20\) seconds, the rudder is fixed at \(0^\circ\). After \(20\) seconds, the rudder is moved to a user-defined start angle. The rudder is then incrementally increased by a user-defined increment until the end angle is reached. The rudder is held at each incremented angle for a user-defined wait time. The rudder is returned to zero after the end angle is reached. We call upon this function from the mav_spiral.py node with a wait time of \(10\) seconds, start angle of \(-35^\circ\), end angle of \(35^\circ\), and increment of \(5^\circ\).

Input:

t: Current simulation time [s]
state: Current vessel state vector
wait_time: Time to wait before incrementing the rudder angle [s]
start_angle: User-defined start angle in degrees
end_angle: User-defined end angle in degrees
increment: User-defined increment in degrees

Output:

delta_c: Commanded rudder angle in radians

21.4 Create launch files

Create launch files for each control strategy inside /workspaces/mavlab/ros2_ws/src/student/tut_09/mav_openloop_controller/mav_openloop_controller/launch

cd /workspaces/mavlab/ros2_ws/src/student/tut_09/mav_openloop_controller/
mkdir launch
cd launch
touch mav_turning_circle_launch.py
touch mav_zigzag_launch.py
touch mav_spiral_launch.py

Copy the following code into the mav_turning_circle_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_openloop_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'
        ),
        # Start the MAV turning circle controller
        Node(
            package='mav_openloop_controller',
            executable='mav_turning_circle',
            name='mav_turning_circle',
            output='screen'
        ),
    ])

Copy the following code into the mav_zigzag_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_openloop_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'
        ),
        # Start the MAV turning circle controller
        Node(
            package='mav_openloop_controller',
            executable='mav_zigzag',
            name='mav_zigzag',
            output='screen'
        ),
    ])

Copy the following code into the mav_spiral_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_openloop_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'
        ),
        # Start the MAV turning circle controller
        Node(
            package='mav_openloop_controller',
            executable='mav_spiral',
            name='mav_spiral',
            output='screen'
        ),
    ])

Each launch file will start a HTTP server 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 open-loop controller nodes from the mav_openloop_controller package.

21.5 Create a new web directory

Create a new web directory inside /workspaces/mavlab/ros2_ws/src/student/tut_09/mav_openloop_controller

cd /workspaces/mavlab/ros2_ws/src/student/tut_09/mav_openloop_controller
mkdir web

Create a new index.html file inside /workspaces/mavlab/ros2_ws/src/student/tut_09/mav_openloop_controller/web

cd /workspaces/mavlab/ros2_ws/src/student/tut_09/mav_openloop_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_09/mav_openloop_controller/web

cd /workspaces/mavlab/ros2_ws/src/student/tut_09/mav_openloop_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'
  });
  
  // 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
      }]
    },
    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);
  });

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.

21.6 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_openloop_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/*')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Abhilash Somayajula',
    maintainer_email='abhilash@iitm.ac.in',
    description='MAV Open-Loop Controller',
    license='MIT',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'mav_turning_circle = mav_openloop_controller.mav_turning_circle:main',
            'mav_zigzag = mav_openloop_controller.mav_zigzag:main',
            'mav_spiral = mav_openloop_controller.mav_spiral:main',
        ],
    },
)

21.7 Build the ROS2 package

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

21.8 Run the ROS2 package

ros2 launch mav_openloop_controller mav_turning_circle_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. The above command should result in the vehicle performing a turning circle.

Repeat the same steps for the mav_zigzag and mav_spiral launch files. Make sure to stop the already running launch file before running a new one.

21.9 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 seems to the steady turning radius for the turning circle?
What is the overshoot value in the zigzag maneuver?
From the spiral maneuver can you comment if the vessel is straight line stable or not?
Can we implement these open-loop tests in our wave basin (size \(L = 30\) m \(\times\) \(B = 30\) m)?
In the zigzag maneuver, is the switch of the rudder happening at the correct time? Which plots do we need to observe to answer this question?

21.10 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.

21.1 Getting Started

21.2 Create a new ROS2 package

21.3 Create a new node

21.3.1 Implentation Tasks

Task 1: Complete the ssa function

Task 2: Complete the turning_circle function

Task 3: Complete the zigzag function

Task 4: Complete the spiral function

21.4 Create launch files

21.5 Create a new web directory

21.6 Modify the setup.py file

21.7 Build the ROS2 package

21.8 Run the ROS2 package

21.9 Evaluation

21.10 Instructor Feedback through Pull Requests

Task 1: Complete the `ssa` function

Task 2: Complete the `turning_circle` function

Task 3: Complete the `zigzag` function

Task 4: Complete the `spiral` function