# Tutorial 3.5

# Gap Following

# Concept

This tutorial will focus on the concept of staying within a space created between the two walls of the racecourse. This is one of many solutions to vehicle path planning and is more practical when applied to the racing environment you are currently working in. As long as there is a gap created between two collision points, the vehicle will be able to successfully navigate within it.

# Step 1: Setting up the message

Similar to the Wall Follower tutorial, in order to identify the gap you need to asses the lidar beams given by the LaserScan or '/scan' topic, but instead of only identifying the right most lidar beam index you will asses a range of lidar beams.

In order to do this you must firstly setup your message structure. In your 'pid_msgs/msg' create another .msg file and name this "KeyValue". Within this file it should look like the following:

int32 key
float32 value

Next enter your "PIDInput.msg" file and make sure it looks like the following:

float32 pid_vel
KeyValue[] pid_error

Finally navigate to your "CMakeLists.txt" and it should look similar to below:

cmake_minimum_required(VERSION 3.5)

project(my_messages)

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)

find_package(std_msgs REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/PIDInput.msg"
  "msg/KeyValue.msg"
  DEPENDENCIES std_msgs
  ADD_LINTER_TESTS
)

ament_export_dependencies(rosidl_default_runtime)

ament_package()

You are now finished setting up your message system and can now continue on to figuring out how to use it. The purpose of this is to make sure that you can communicate large clusters of data in an easier way through nodes.

# Step 2: Identifying the Gap

You are now ready to use the message structure created above and apply it to identifying the gap. To make it easier to understand what exactly you are creating here I personally like to imagine a top down view of the car. From there imagine a trangular area just in front of the car that analyzes the gap of the course.

This system should be able to identify the gap of the track and make sure that the car does not get too close to the wall.

to begin you must first import the necessary assets:

import rclpy
import math
from rclpy.node import Node

from sensor_msgs.msg import LaserScan
from pid_msgs.msg import PIDInput, KeyValue

Next in your WallDistanceFinder(Node) class under def init(self) you need to identify the two beams that intersect the rightmost front corner of the vehicle and the leftmost. You can also adjust these numbers depending on how much data you want to asses.

You should identify the following:

# the rightmost and leftmost lidar front corner beam range
self.wall_projection_area = range(350, 731)

Now in the def callback() we are going to loop through the range of the lidar beams, run each one through your parameters, and then record the data.

Your callback should look similar:

def callback(self, msg):
    beam_dict = {}

    for beam in self.wall_projection_area:
        # reducing the sensativity of the refernce by halving the distance of the beam
        all_distance = (0.5)*((msg.ranges)[beam])

        if wall_distance > 1.5: # This caps the farthest the wall dstance goes
            wall_distance = 1.5
        
        # calculate the error - difference between the value target and the real one
        beam_dict[beam] = wall_distance
    
    # An empty msg is created of the type pid_input
    pid_msg = PIDInput()

    # this is the error that you want to send to the PID for steering correction.
    for beamer, steering_errors in beam_dict.items():
        pid_msg.pid_error.append(KeyValue(key=beamer, value=steering_errors))
    
    # Publish the msg
    self.publisher_.publish(pid_msg)

Overall, your wall_distance_finder.py should now look like the following:

import rclpy
import math
from rclpy.node import Node

from sensor_msgs.msg import LaserScan
from pid_msgs.msg import PIDInput, KeyValue

class WallDistanceFinder(Node):

    def __init__(self):
        super().__init__('wall_distance_finder')
        self.subscription = self.create_subscription(LaserScan, '/scan', self.callback, 10)

        self.publisher_ = self.create_publisher(PIDInput, '/error', 10)

        # the rightmost and leftmost lidar front corner beam range
        self.wall_projection_area = range(350, 731)

        # since the reference has been halved, the target needs to follow suit
        self.target_distance = 0.7
        
        # initialize the steering error
        self.error = 0.0
    
    def callback(self, msg):
        beam_dict = {}

        for beam in self.wall_projection_area:
            # reducing the sensativity of the refernce by halving the distance of the beam
            all_distance = (0.5)*((msg.ranges)[beam])

            if wall_distance > 1.5: # This caps the farthest the wall dstance goes
                wall_distance = 1.5
            
            # calculate the error - difference between the value target and the real one
            beam_dict[beam] = wall_distance
    
        # An empty msg is created of the type pid_input
        pid_msg = PIDInput()

        # this is the error that you want to send to the PID for steering correction.
        for beamer, steering_errors in beam_dict.items():
            pid_msg.pid_error.append(KeyValue(key=beamer, value=steering_errors))
        
        # Publish the msg
        self.publisher_.publish(pid_msg)
    
    def main(args=None):
        rclpy.init(args=args)

        wall_distance_finder = WallDistanceFinder()

        rclpy.spin(wall_distance_finder)

        wall_distance_finder.destroy_node()
        rclpy.shutdown()
    
    if __name__ == "__main__":
        main()

# Step 3: Following the Gap

The final step in the process is to follow the gap and navigate the car safely within the track without crashing. To do this navigate to your control.py file where you will follow a similar process to the Wall Following tutorial.

Make sure you have the proper imports from the previous tutorial. Everything should be the same except for unpacking the message and your vehicle navigation logic.

Within your PIDController() class under the def callback() unpack the msg published by your WallDistanceFinder:

right_side_sum = 0
left_side_sum = 0
middle_beam = 0

# Grab the error from the WallDistanceFinder and divide into sections
for kv_pair in msg.pid_error:
    beam = kv_pair.key
    wall_distance = kv_pair.value

    if 350 <= beam < 540:
        right_side_sum += wall_distance
    elif 540 < beam <= 730:
        left_side_sum += wall_distance
    else:
        middle_beam = middle_beam + wall_distance

Now since we are using a area based system based on the different wall distances we now need to find the error derivative which should ultimately look like so:

steering_error = lef_side_sum - right_side_sum

angle = (self.s_kp * steering_error) + (self.s_kd * (steering_error self.prev_steering_error))
angle = angle/100
angle = max(min(angle, 0.85), -0.85)

Your final code should look like this:

import rclpy
import math
from rclpy.node import Node

from ackermann_msgs.msg import AckermannDriveStamped
from pid_msgs.msg import PIDInput

class PIDController(Node):

    def __init__(self):
        super().__init__('pid_controller')
        self.subscription = self.create_subscription(
            PIDInput, 
            '/error', 
            self.callback,
            10
        )

        self.publisher_ = self.create_publisher(AckermannDriveStamped, '/drive', 1)

        self.prev_steering_error = 0.0
        
        # velocity value
        self.vel_input = 4.0

        # PID params
        self.s_kp = 0.0 #TODO
        self.s_kd = 0.0 #TODO
        self.s_ki = 0.0 #TODO

    def callback(self, msg):
        
        self.get_logger().info(f"Subscribing Steering Error: \"{msg.pid_error}\"")

        right_side_sum = 0
        left_side_sum = 0
        middle_beam = 0

        # grab the error from the WallDistanceFinder node
        for kv_pair in msg.pid_error:
            beam = kv_pair.key
            wall_distance = kv_pair.value

            if 350 <= beam < 540:
                right_side_sum += wall_distance
            elif 540 < beam <= 730:
                left_side_sum += wall_distance
            else:
                middle_beam = middle_beam + wall_distance
        
        # Calculate the error derivative (change in error)
        steering_error = left_side_sum - right_side_sum
        
        # Update the previous error for the next iteration
        self.prev_steering_error = steering_error
        
        # calculate desired steering angle from steering error (P and D)
        angle = (self.s_kp * steering_error) + (self.s_kd * (steering_error - self.prev_steering_error))
        angle = angle/100
        
        # saturate the steering angle
        angle = max(min(angle, 0.85), -0.85)

        # initialize the Ackermann message
        stamped_command = AckermannDriveStamped()

        # modify the steering angle element of the /drive message
        stamped_command.drive.steering_angle = angle

        # modify the velocity of the /drive message 
        stamped_command.drive.speed = self.vel_input

        # publish the message (includes the modified elements)
        self.publisher_.publish(stamped_command)

        self.get_logger().info(f"Publishing Steering Angle: \"{stamped_command.drive.steering_angle}\"")
        
        self.get_logger().info(f"Publishing Speed: \"{stamped_command.drive.speed}\"")

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

    pid_controller = PIDController()

    rclpy.spin(pid_controller)

    pid_controller.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    main()

# Closing

So, overall this logic should make it so that the vehicle identifies the gap and divides it into a right sum, left sum, and middle beam. It then finds the error based on the difference of the two areas, becasue if the wall impedes one of the areas it will adjust the other way to avoid crashing. This avoidence is determined within the angle variable and is then published as a command to the car. Your car should now stay within the gap and not crash.