Autonomous Driving Vehicle Simulation

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
import rclpy
from rclpy.node import Node
from rcl_interfaces.srv import GetParameters
from rcl_interfaces.msg import ParameterValue, ParameterType
from typing import Any, List, Tuple


class ParametersClientNode(Node):
    """
    Base class for ROS 2 nodes that need to communicate with another node
    to retrieve parameters.

    This class simplifies the process of querying parameters from other ROS 2 nodes.
    It provides synchronous service calls to the GetParameters service and returns
    the requested values.
    """

    def __init__(self, node_name: str) -> None:
        """
        @brief Initializes the parameter client node.

        @param node_name Name to assign to this node in the ROS graph.
        """
        super().__init__(node_name)

    def request_parameters(self, target_node_name: str, param_names: List[str], 
                           timeout_sec: float = 2.0) -> Any | Tuple[Any, ...]:
        """
        @brief Synchronously requests parameters from a remote ROS 2 node.

        This method blocks until the GetParameters service responds or the
        specified timeout expires. Returned values are converted into native
        Python types and returned as a tuple in the same order as requested.

        @param target_node_name Name of the node providing the parameters.
        @param param_names List of parameter names to request.
        @param timeout_sec Maximum time to wait for the service response.

        @return Parameter value or tuple of parameter values.
        """
        # Create a client for the GetParameters service
        service_name = f"/{target_node_name}/get_parameters"
        client = self.create_client(GetParameters, service_name)

        # Wait for the service to become available
        if not client.wait_for_service(timeout_sec=timeout_sec):
            raise TimeoutError(f"GetParameters service not available: {service_name}")

        # Create the service request and set parameter names
        request = GetParameters.Request()
        request.names = param_names

        # Send the request asynchronously
        future = client.call_async(request)

        # Spin the node until the service response arrives or times out
        rclpy.spin_until_future_complete(self, future, timeout_sec=timeout_sec)

        # Fail if the service did not respond in time
        if not future.done():
            raise TimeoutError(f"Timeout while requesting parameters {param_names} "
                               f"from node '{target_node_name}'")

        # Get the result response
        response = future.result()
        
        # Validate response size matches requested parameters
        if response is None or len(response.values) != len(param_names):
            raise RuntimeError(f"Invalid parameter response from node '{target_node_name}'")

        # Convert ROS ParameterValue messages into native Python values
        values = tuple(self._extract_parameter_value(value) for value in response.values)

        # Return a single value or a tuple depending on request size
        return values[0] if len(values) == 1 else values

    def _extract_parameter_value(self, value: ParameterValue) -> Any:
        """
        @brief Converts a ROS 2 ParameterValue message into a Python-native value.

        @param value ParameterValue message returned by GetParameters.
        @return Corresponding Python value, or None if the type is unsupported.
        """
        match value.type:
            case ParameterType.PARAMETER_BOOL:
                return value.bool_value
            case ParameterType.PARAMETER_INTEGER:
                return value.integer_value
            case ParameterType.PARAMETER_DOUBLE:
                return value.double_value
            case ParameterType.PARAMETER_STRING:
                return value.string_value
            case ParameterType.PARAMETER_BYTE_ARRAY:
                return bytes(value.byte_array_value)
            case ParameterType.PARAMETER_INTEGER_ARRAY:
                return list(value.integer_array_value)
            case ParameterType.PARAMETER_DOUBLE_ARRAY:
                return list(value.double_array_value)
            case ParameterType.PARAMETER_BOOL_ARRAY:
                return list(value.bool_array_value)
            case ParameterType.PARAMETER_STRING_ARRAY:
                return list(value.string_array_value)
            case _:
                self.get_logger().warn(f"Unknown parameter type received: {value.type}")
                return None

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
import os
import rclpy
import csv
import cv2

from std_msgs.msg import Float64
from sensor_msgs.msg import Image
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy

from autopilot_neural_network.parameters_client_node import ParametersClientNode
from dataclasses import dataclass
from cv_bridge import CvBridge


@dataclass
class VehicleState:
    steering_angle = None
    velocity = None
    image = None
    steering_angle_time = None
    velocity_time = None
    image_time = None
    

class CsvDataStorage:
    def __init__(self, dataset_path: str):
        """
        @brief Initializes the CSV data storage handler.

        Creates the dataset directory if it does not exist, and prepares the CSV file path.

        @param dataset_path Path to the dataset directory where images and CSV files are stored.
        """
        self.dataset_path = dataset_path
        self.csv_file_path = os.path.join(self.dataset_path, 'dataset.csv')
        self.bridge = CvBridge()

        if not os.path.exists(self.dataset_path):
            os.makedirs(self.dataset_path)

    def _get_next_image_id(self) -> int:
        """
        @brief Retrieves the next available image ID for saving a new image.

        Scans the dataset directory for existing images and determines the next sequential ID.

        @return The next image ID as an integer.
        """
        os.makedirs(self.dataset_path, exist_ok=True)  # Ensure directory exists

        existing_images = [f for f in os.listdir(self.dataset_path) if f.endswith('.png')]
        if not existing_images:
            return 0

        existing_ids = [int(os.path.splitext(f)[0]) for f in existing_images if f.split('.')[0].isdigit()]
        
        return max(existing_ids) + 1

    def _save_image(self, image_id: int, image: Image) -> str:
        """
        @brief Saves a ROS Image message as a PNG file.

        Converts the ROS Image message to an OpenCV image and writes it to the dataset directory.

        @param image_id The numeric ID of the image to save.
        @param image The Image message received from ROS.
        @return The full path to the saved image file.
        """
        cv_image = self.bridge.imgmsg_to_cv2(image, desired_encoding='bgr8')
        image_path = os.path.join(self.dataset_path, f'{image_id}.png')
        cv2.imwrite(image_path, cv_image)
        return image_path

    def _append_csv_row(self, image_id: int, state: VehicleState, 
                        max_velocity: float, max_steering_angle: float): 
        """
        @brief Appends a new row of vehicle state data to the dataset CSV file.

        If the CSV file does not exist yet, a header row is written before appending data.

        @param image_id The ID associated with the image for this data sample.
        @param state The VehicleState object containing the state of the vehicle.
        """
        write_header = not os.path.exists(self.csv_file_path) 
        
        with open(self.csv_file_path, 'a', newline='') as csvfile: 
            writer = csv.writer(csvfile) 
            
            if write_header: 
                writer.writerow(['image_id', 'velocity', 'steering_angle', 
                                 'image_time', 'velocity_time', 'steering_angle_time',
                                 'max_velocity', 'max_steering_angle']) 
                
            writer.writerow([image_id, 
                             state.velocity, 
                             state.steering_angle, 
                             state.image_time.nanoseconds / 1e9, # To UNIX timestamp
                             state.velocity_time.nanoseconds / 1e9, # To UNIX timestamp
                             state.steering_angle_time.nanoseconds / 1e9, # To UNIX timestamp
                             max_velocity, 
                             max_steering_angle])

    def save_sample(self, state: VehicleState, max_velocity: float, max_steering_angle: float):
        """
        @brief Saves a complete sample, including the image and corresponding state data.

        This method assigns an image ID, saves the image file, and appends metadata to the CSV file.

        @param state The VehicleState object containing vehicle state data.
        """
        image_id = self._get_next_image_id()
        self._save_image(image_id, state.image)
        self._append_csv_row(image_id, state, max_velocity, max_steering_angle)
    
    
class DataCollector(ParametersClientNode):
    """
    This node subscribes to vehicle-related topics, retrieves parameters from the 
    vehicle controller, and periodically saves synchronized image and state data to a dataset.
    """
        
    def __init__(self):
        super().__init__("data_collector")

        # Load ROS parameters
        self.declare_parameter("min_velocity_factor", 0.25)
        self.declare_parameter("timeout_time", 1_000_000_000)
        self.declare_parameter("update_period", 0.5)
        self.declare_parameter("dataset_path", "/tmp/autopilot_neural_network/dataset")
        
        self.declare_parameter("vehicle_node", "vehicle_controller")
        self.declare_parameter("velocity_topic", "vehicle_controller")
        self.declare_parameter("steering_angle_topic", "vehicle_controller")
        self.declare_parameter("image_topic", "vehicle_controller")
        
        self.declare_parameter("max_velocity_parameter", "max_velocity")
        self.declare_parameter("max_steering_angle_parameter", "max_steering_angle")

        # Retrieve parameters
        self.min_velocity_factor = self.get_parameter("min_velocity_factor").value
        self.timeout_time = self.get_parameter("timeout_time").value
        self.update_period = self.get_parameter("update_period").value
        self.dataset_path = self.get_parameter("dataset_path").value
       
        self.vehicle_node = self.get_parameter("vehicle_node").value
        self.velocity_topic = self.get_parameter("velocity_topic").value
        self.steering_angle_topic = self.get_parameter("steering_angle_topic").value
        self.image_topic = self.get_parameter("image_topic").value
        
        max_velocity_parameter = self.get_parameter("max_velocity_parameter").value
        max_steering_angle_parameter = self.get_parameter("max_steering_angle_parameter").value

        # Vehicle controller parameters
        self.max_velocity = None
        self.max_steering_angle = None

        # Request vehicle controller parameters (max_velocity, max_steering_angle)
        self.max_velocity, self.max_steering_angle = self.request_parameters(
            self.vehicle_node, [max_velocity_parameter, max_steering_angle_parameter])

        # Storage layer
        self.storage = CsvDataStorage(self.dataset_path)
        self.last_state = VehicleState()

        # QoS: keep only the latest message, replacing older ones
        qos_profile = QoSProfile(reliability=QoSReliabilityPolicy.BEST_EFFORT,
                                 history=QoSHistoryPolicy.KEEP_LAST,
                                 depth=1)

        # Subscribers
        self.create_subscription(Float64, 
                                 self.steering_angle_topic, 
                                 self._steering_callback, qos_profile)
        
        self.create_subscription(Float64, 
                                 self.velocity_topic, 
                                 self._velocity_callback, qos_profile)
        
        self.create_subscription(Image, 
                                 self.image_topic, 
                                 self._image_callback, qos_profile)

        # Timer for periodic global loop execution
        self.timer = self.create_timer(self.update_period, self._update)

    def _has_invalid_state(self) -> bool:
        """
        @brief Checks if the current state is invalid.

        Evaluates timeout, missing data, or low-speed conditions to determine
        if the current vehicle state is invalid for recording.

        @retval True If any invalid condition is met (timeout, missing data, or low speed).
        @retval False If the current state is valid.
        """
        now = self.get_clock().now()

        # Missing data check (return immediately)
        required_fields = (
            self.last_state.image,
            self.last_state.image_time,
            self.last_state.steering_angle,
            self.last_state.steering_angle_time,
            self.last_state.velocity,
            self.last_state.velocity_time,
            self.max_velocity,
            self.max_steering_angle,
        )

        if any(value is None for value in required_fields):
            return True

        # Timeout check (safe because timestamps are guaranteed valid if we reached here)
        timestamps = [
            self.last_state.image_time,
            self.last_state.steering_angle_time,
            self.last_state.velocity_time,
        ]

        if (now - min(timestamps)).nanoseconds > self.timeout_time:
            return True

        # Low-speed check
        if (self.max_velocity is not None and
            self.last_state.velocity <
            self.min_velocity_factor * self.max_velocity):
            return True

        # All good
        return False
        
    def _steering_callback(self, msg: Float64):
        """
        @brief Callback for receiving steering angle data.

        @param msg Float64 message containing the current steering angle.
        """
        self.last_state.steering_angle = msg.data
        self.last_state.steering_angle_time = self.get_clock().now()

    def _velocity_callback(self, msg: Float64):
        """
        @brief Callback for receiving vehicle velocity data.

        @param msg Float64 message containing the current vehicle velocity.
        """
        self.last_state.velocity = msg.data
        self.last_state.velocity_time = self.get_clock().now()

    def _image_callback(self, msg: Image):
        """
        @brief Callback for receiving camera images.

        @param msg Image message from the camera topic.
        """
        self.last_state.image = msg
        self.last_state.image_time = self.get_clock().now()

    def _update(self):
        """
        @brief Periodic update loop executed by the node's timer.

        Checks the validity of the current vehicle state and, if valid,
        saves a synchronized sample (image + state data) to the dataset.
        """
        if self._has_invalid_state():
            return

        self.storage.save_sample(self.last_state, self.max_velocity, self.max_steering_angle) 

        self.get_logger().info(f"Velocity: {self.last_state.velocity}, " 
                               f"Steering Angle: {self.last_state.steering_angle}")


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

    # Instantiate the node with the given vehicle node name
    node = DataCollector()

    try:
        # Spin the node to process callbacks
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    # Cleanup
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    """
    Launches the data_collector node with parameters loaded from a YAML file.
    """

    package_name = 'autopilot_neural_network'
    package_share = get_package_share_directory(package_name)

    # Path to the YAML configuration file
    params_file = os.path.join(package_share, 'config', 'parameters.yaml')

    # Define the node to launch
    data_collector_node = Node(package=package_name,
                               executable='data_collector',
                               name='data_collector',
                               output='screen',
                               parameters=[params_file])

    return LaunchDescription([data_collector_node])

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
from setuptools import setup, find_packages
import os
from glob import glob

package_name = 'autopilot_neural_network'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(),  # includes scripts/ automatically
    data_files=[
        # Package index
        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),

        # package.xml
        ('share/' + package_name, ['package.xml']),

        # Launch files
        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),

        # Config files
        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Lucas Mazzetto',
    maintainer_email='workabotic@gmail.com',
    description='An end-to-end deep learning pipeline for autonomous driving in ROS 2.',
    license='MIT',
    entry_points={
        'console_scripts': [
            'data_collector = autopilot_neural_network.data_collector:main',
        ],
    },
)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
import torch
import torch.nn as nn


class AutopilotNet(nn.Module):
    """
    This model takes a single-channel image (e.g., grayscale) as input and
    outputs two values: the predicted velocity and steering angle. It uses a
    multi-head architecture where a shared convolutional base extracts features
    from the input image, and two separate fully connected heads predict the
    final outputs.
    """
    def __init__(self, h=96, w=128, inputs=1):
        """
        @brief Initializes the AutopilotNet model layers.

        @param h The height of the input images.
        @param w The width of the input images.
        @param inputs The number of channels in the input images.
        """
        super(AutopilotNet, self).__init__()

        # Convolutional base for feature extraction
        self.convolutional_layers = nn.Sequential(nn.Conv2d(inputs, 6, kernel_size=5, stride=1),
                                                  nn.ReLU(),
                                                  nn.MaxPool2d(kernel_size=2, stride=2),
                                                  nn.Conv2d(6, 16, kernel_size=5, stride=1),
                                                  nn.ReLU(),
                                                  nn.MaxPool2d(kernel_size=2, stride=2),
                                                  nn.Dropout(0.5))

        # Get the number of features produced by the convolutional block
        conv_output_size = self._get_conv_output_size(h, w, inputs)

        # Shared fully connected layers to process features from the convolutional base
        self.shared_layers = nn.Sequential(nn.Linear(conv_output_size, 120),
                                           nn.ReLU())

        # Head for predicting velocity
        self.velocity_head = nn.Sequential(nn.Linear(120, 84),
                                           nn.ReLU(),
                                           nn.Linear(84, 1))
        
        # Head for predicting steering angle
        self.steering_head = nn.Sequential(nn.Linear(120, 84),
                                           nn.ReLU(),
                                           nn.Linear(84, 1))

    def _get_conv_output_size(self, h, w, inputs):
        """
        @brief Computes the number of output features produced by the convolutional layers.

        This method generates a dummy tensor with the same shape as the actual
        input images and feeds it through the convolutional base. By examining
        the resulting tensor size, it determines the exact number of features
        required by the first fully connected layer.

        @param h The input image height.
        @param w The input image width.
        @param inputs The number of input channels.
        @return The flattened feature size after all convolutional layers.
        """
        # Create a dummy tensor
        x = torch.zeros(1, inputs, h, w)
        
        # Pass dummy input through the convolutional layers
        x = self.convolutional_layers(x)
        
        # Get the total number of elements in x
        return x.numel()
    
    def forward(self, x):
        """
        @brief Defines the forward pass of the AutopilotNet.

        @param x The input tensor, which is a batch of images.
        @return A tensor containing the concatenated predictions for velocity and steering angle.
        """
        # Pass input through the convolutional layers
        x = self.convolutional_layers(x)
        
        # Flatten the output for the fully connected layers
        x = x.view(x.size(0), -1)
        
        # Pass flattened features through the shared layers
        x = self.shared_layers(x)

        # Get predictions from each head
        vel = self.velocity_head(x)
        steer = self.steering_head(x)

        # Concatenate the outputs into a single tensor
        return torch.cat([vel, steer], dim=1)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
import os
import torch
import csv
import numpy as np
import torchvision.transforms as T
from torch.utils.data import Dataset
from PIL import Image


def preprocessing_transform(height, width):
    """
    @brief Creates the preprocessing transformation pipeline.
    
    @param height Desired image height after resizing.
    @param width Desired image width after resizing.

    @return A torchvision Compose object with the preprocessing transforms.
    """
    return T.Compose([T.Grayscale(),
                      T.Resize((height, width)),
                      T.ToTensor(),
                      T.Normalize((0.5,), (0.5,))])


class Data(Dataset):
    """
    @brief Custom PyTorch dataset for autonomous driving images and labels.

    This dataset loads image paths and target values from a CSV file,
    applies optional data augmentation, normalizes steering and velocity using their 
    max values from the CSV, and returns (image_tensor, normalized_velocity, normalized_steering).

    @param dataset_path Path to the folder containing images and dataset.csv.
    @param height Preprocessing image height.
    @param width Preprocessing image width.
    @param augment Whether to apply online augmentation during training.
    """

    def __init__(self, dataset_path, height=96, width=128, augment=True):
        # Path to the dataset CSV file
        self.dataset_path = dataset_path
        self.csv_file_path = os.path.join(self.dataset_path, 'dataset.csv')
        
        self.augment = augment

        # Image augmentation pipeline (used only if augment=True)
        self.augmentation = T.Compose([
            T.ColorJitter(brightness=(0.5, 1.5),
                          contrast=(0.5, 1.5),
                          saturation=(0.5, 1.5)),
            
            T.RandomPerspective(distortion_scale=0.1, p=0.1),
            T.RandomRotation(degrees=(-3, 3)),
            T.GaussianBlur(kernel_size=5, sigma=(0.1, 0.8)),
        ])

        self.transform = preprocessing_transform(height, width)

        # Storage for loaded sample metadata
        self.samples = []
        self.load_samples() # Parse CSV and fill samples list


    def load_samples(self):
        """
        @brief Loads sample metadata from dataset.csv and populates self.samples.
        """
        if not os.path.exists(self.csv_file_path):
            raise FileNotFoundError(f"CSV file not found: {self.csv_file_path}")

        # Open and parse the CSV file
        with open(self.csv_file_path, 'r') as f:
            reader = csv.DictReader(f)

            for i, row in enumerate(reader, start=2):  # Start at line 2 (after header)

                # Skip rows that are empty or malformed
                if not any(row.values()):
                    continue

                # Build the full path to the corresponding image
                image_path = os.path.join(self.dataset_path, f"{row['image_id']}.png")

                # Append structured sample info
                self.samples.append({
                    'image_path': image_path,
                    'velocity': float(row['velocity']),
                    'steering_angle': float(row['steering_angle']),
                    'max_velocity': float(row['max_velocity']),
                    'max_steering_angle': float(row['max_steering_angle'])
                })


    def random_flip(self, image, steering, p=0.5):
        """
        @brief Horizontally flips the image and inverts steering with probability p.

        @param image Torch tensor image.
        @param steering Steering angle value.
        @param p Probability of flipping.

        @return (image, steering) Possibly flipped image and modified steering.
        """
        if np.random.rand() < p:       
            image = torch.flip(image, (2,))  # Flip along width dimension
            steering = -steering # Invert steering sign
            
        return image, steering


    def __len__(self):
        return len(self.samples)


    def __getitem__(self, idx):
        # Retrieve metadata for this index
        sample = self.samples[idx]

        # Load image from disk (RGB mode)
        image = Image.open(sample['image_path']).convert('RGB')

        # Raw velocity and steering values
        velocity = sample['velocity']
        steering = sample['steering_angle']

        # Apply augmentation if enabled (training only)
        if self.augment:
            image = self.augmentation(image)

        # Apply preprocessing (resize, grayscale, tensor, normalize)
        image = self.transform(image)

        # Apply random horizontal flip augment
        if self.augment:
            image, steering = self.random_flip(image, steering)

        # Normalize values using dataset-provided max values
        max_velocity = sample['max_velocity']
        max_steering_angle = sample['max_steering_angle']

        velocity /= max_velocity if max_velocity != 0 else 1
        steering /= max_steering_angle if max_steering_angle != 0 else 1

        # Convert to tensors
        velocity = torch.tensor(velocity, dtype=torch.float32)
        steering = torch.tensor(steering, dtype=torch.float32)

        return image, velocity, steering

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
from torch.utils.data import Sampler
import numpy as np


class SteeringBalancedSampler(Sampler):
    """
    This sampler is designed for steering-angle datasets where the majority of samples
    correspond to near-zero steering values (vehicle driving straight).
    """

    def __init__(self, dataset, low_fraction=0.10, threshold_ratio=0.10, shuffle=True):
        """
        @brief Constructor for SteeringBalancedSampler.

        The constructor analyzes steering angles, identifies low-steering 
        and high-steering samples, and keeps: all high-steering samples and at most 
        low_fraction * total dataset size of low-steering samples

        @param dataset The dataset to sample from.
        @param low_fraction The maximum fraction of low-steering samples to keep.
        @param threshold_ratio Fraction of max steering defining "low steering".
        @param shuffle Whether to shuffle the indices after balancing.
        """

        self.low_fraction = low_fraction
        self.shuffle = shuffle
        
        # Extract raw samples from the original dataset
        samples = [dataset.dataset.samples[i] for i in dataset.indices]

        # Collect steering data
        steer = np.array([s["steering_angle"] for s in samples])
        max_steer = np.array([s["max_steering_angle"] for s in samples])

        # Normalize steering angles to [-1, 1]
        norm_steer = steer / max_steer
        abs_norm = np.abs(norm_steer)

        # Identify low and high steering samples using threshold as fraction
        low_indices = np.where(abs_norm < threshold_ratio)[0]
        high_indices = np.where(abs_norm >= threshold_ratio)[0]

        # Keep at most (low_fraction * total_dataset) low-steering samples
        max_low_allowed = int(len(samples) * low_fraction)

        # How many low samples we will actually keep
        keep_low = min(len(low_indices), max_low_allowed)

        # Downsample low-steering indices
        if keep_low > 0:
            low_indices = np.random.choice(low_indices, size=keep_low, replace=False)
        else:
            low_indices = np.array([], dtype=np.int64)

        # Combine all high-steering samples with selected low-steering samples
        self.indices = np.concatenate([high_indices, low_indices])

        # Shuffle final indices if required
        if shuffle:
            np.random.shuffle(self.indices)

    def __iter__(self):
        return iter(self.indices)

    def __len__(self):
        return len(self.indices)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
import os
import time
import argparse
import multiprocessing

import numpy as np
import torch
import torch.nn as nn
import torch.optim as optim
from torch.utils.data import DataLoader, random_split
from torch.optim.lr_scheduler import ReduceLROnPlateau
from torch.utils.tensorboard import SummaryWriter
from tqdm import tqdm

from model import AutopilotNet
from dataset import Data
from sampler import SteeringBalancedSampler


def setup_tensorboard(args):
    """
    @brief Initializes and configures a TensorBoard SummaryWriter.

    This function sets up a directory for TensorBoard logs and records the
    hyperparameters used for the training session.

    @param args Arguments containing hyperparameters.
    @return The configured TensorBoard writer object.
    """
    log_dir = os.path.join("runs", f"autopilot_neural_network_{int(time.time())}")
    writer = SummaryWriter(log_dir=log_dir)
    
    writer.add_text("Hyperparameters", 
                    f"learning_rate: {args.learning_rate}, "
                    f"epochs: {args.epochs}, " 
                    f"batch_size: {args.batch_size}")
    
    return writer


def save_model(model, optimizer, scheduler, epoch, path):
    """
    @brief Saves the model checkpoint.

    Saves the state of the model, optimizer, and scheduler to a file. 
    This function is used to save the best performing model based on validation loss.

    @param model The model to save.
    @param optimizer The optimizer state to save.
    @param scheduler The scheduler state to save.
    @param epoch The current epoch number.
    @param path The file path to save the checkpoint to.
    """
    os.makedirs(os.path.dirname(path), exist_ok=True)
    
    state = {'epoch': epoch,
             'state_dict': model.state_dict(),
             'optimizer': optimizer.state_dict(),
             'scheduler': scheduler.state_dict()}
    
    torch.save(state, path)
    print(f"Model saved at epoch {epoch} to {path}")


def compute_weighted_loss(predicted_velocities, predicted_steering_angles, 
                          velocities, steering_angles, criterion, alpha=1, beta=1):
    """
    @brief Computes a weighted loss for velocity and steering angle.

    This function calculates the error for both predicted velocities
    and steering angles against their ground truth values, and then combines them
    into a single weighted loss.
    
    @param predicted_velocities The predicted velocities from the model.
    @param predicted_steering_angles The predicted steering angles from the model.
    @param velocities The ground truth velocities.
    @param steering_angles The ground truth steering angles.
    @param criterion The loss function.
    @param alpha The weight for the velocity loss component.
    @param beta The weight for the steering angle loss component.
    
    @return The final computed weighted loss.
    """
    # Compute losses separately
    velocity_loss = criterion(predicted_velocities, velocities)
    steering_loss = criterion(predicted_steering_angles, steering_angles)
    
    # Weighted total loss
    loss = (alpha * velocity_loss) + (beta * steering_loss)
    
    return loss


def validate(model, val_loader, criterion, device, alpha, beta):
    """
    @brief Evaluates the model on the validation dataset.

    This function iterates through the validation set, computes the loss, and
    calculates the mean absolute error for both velocity and steering angle predictions.

    @param model The model to be evaluated.
    @param val_loader DataLoader for the validation data.
    @param criterion The loss function used for evaluation.
    @param device The device (CPU or CUDA) to run evaluation on.
    @param alpha The weight for the velocity loss component.
    @param beta The weight for the steering angle loss component.
    
    @return A tuple containing the mean validation loss, mean velocity error,
            and mean steering angle error.
    """
    # Set the model to evaluation mode
    model.eval()
    
    # Initialize accumulators for loss and error metrics
    val_loss = 0
    velocity_error = 0
    steering_angle_error = 0
    
    # Disable gradient calculations to save memory and computations
    with torch.no_grad():
        for i, data in enumerate(val_loader):
            # Unpack data from the validation loader
            images, velocities, steering_angles = data
            
            # Reshape and cast labels to match the model's output format
            velocities = velocities.float().view(len(velocities), 1)
            steering_angles = steering_angles.float().view(len(steering_angles), 1)

            # Move data to the appropriate device (GPU or CPU)
            if device.type == "cuda":
                images = images.cuda()
                velocities = velocities.cuda()
                steering_angles = steering_angles.cuda()

            # Get model predictions
            outputs = model(images)
            
            # Extract predictions
            predicted_velocities = outputs[:, 0:1]
            predicted_steering_angles = outputs[:, 1:2]
            
            # Compute the weighted loss for the current batch
            loss = compute_weighted_loss(predicted_velocities=predicted_velocities,
                                         predicted_steering_angles=predicted_steering_angles,
                                         velocities=velocities,
                                         steering_angles=steering_angles,
                                         criterion=criterion,
                                         alpha=alpha,
                                         beta=beta)
            
            # Accumulate the validation loss
            val_loss += loss.item()
            
            # Accumulate the mean absolute error for velocity and steering angle
            velocity_error += torch.mean(
                torch.abs(velocities - predicted_velocities)).item()
            
            steering_angle_error += torch.mean(
                torch.abs(steering_angles - predicted_steering_angles)).item()
    
    # Calculate the mean validation loss and errors across all batches
    mean_val_loss = val_loss / (i + 1)
    mean_velocity_error = velocity_error / (i + 1)
    mean_steering_angle_error = steering_angle_error / (i + 1)
    
    return mean_val_loss, mean_velocity_error, mean_steering_angle_error


def train(args, model, train_dataset, val_dataset):
    """
    @brief Main training and validation loop.

    This function orchestrates the entire training process. It sets up the optimizer,
    learning rate scheduler, data loaders (including a balanced sampler for training),
    and the main training loop. For each epoch, it trains the model, evaluates it on
    the validation set, logs metrics to TensorBoard, and saves the best model
    checkpoint based on validation loss.

    @param args Command-line arguments for training configuration.
    @param model The neural network model to train.
    @param train_dataset The dataset for training.
    @param val_dataset The dataset for validation.
    """
    # Determine the device to use for training (GPU if available, otherwise CPU)
    device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
    
    # Move the model to the selected device
    model.to(device)

    # Initialize the Adam optimizer with the model's parameters and learning rate
    optimizer = optim.Adam(model.parameters(), lr=args.learning_rate)
    
    # Initialize a learning rate scheduler that reduces the learning rate when validation loss plateaus
    scheduler = ReduceLROnPlateau(optimizer, mode='min', 
                                  factor=args.lr_factor, 
                                  patience=args.lr_patience)
    
    # Define the loss function
    criterion = nn.MSELoss()
    
    # Set up a balanced sampler for the training data to handle imbalanced steering angles
    train_sampler = SteeringBalancedSampler(dataset=train_dataset,
                                            low_fraction=args.sampler_low_fraction,
                                            threshold_ratio=args.sampler_threshold_ratio,
                                            shuffle=True)
    
    print(f"Using {len(train_sampler)} training samples after balanced sampling.")
    
    # Create a DataLoader for the training set, using the balanced sampler
    train_loader = DataLoader(train_dataset, 
                              batch_size=args.batch_size, 
                              sampler=train_sampler, 
                              pin_memory=True, # Speeds up data transfer to GPU
                              num_workers=args.num_workers)
    
    # Create a DataLoader for the validation set
    val_loader = DataLoader(val_dataset, 
                            batch_size=args.val_batch_size, 
                            shuffle=False, # No need to shuffle validation data
                            pin_memory=True, 
                            num_workers=args.num_workers)

    # Set up TensorBoard for logging
    writer = setup_tensorboard(args)
    
    # Initialize the best validation loss to infinity for tracking the best model
    best_val_loss = float('inf')
    best_model_path = args.model_path
    
    # Start the training loop for the specified number of epochs
    for epoch in range(args.epochs):
        # Set the model to training mode
        model.train()
        
        # Initialize accumulators for training metrics for the current epoch
        train_loss = 0
        velocity_error = 0
        steering_angle_error = 0
    
        # Iterate over the training data in batches
        for i, data in enumerate(tqdm(train_loader, desc=f"Epoch {epoch}")):
            # Unpack data from the training loader
            images, velocities, steering_angles = data
            
            # Reshape and cast labels to match the model's output format
            velocities = velocities.float().view(len(velocities), 1)
            steering_angles = steering_angles.float().view(len(steering_angles), 1)

            # Move data to the appropriate device (GPU or CPU)
            if device.type == "cuda":
                images = images.cuda()
                velocities = velocities.cuda()
                steering_angles = steering_angles.cuda()
            
            # Get model predictions
            outputs = model(images)
            
            # Extract predictions
            predicted_velocities = outputs[:, 0:1]
            predicted_steering_angles = outputs[:, 1:2]
            
            # Compute the weighted loss for the current batch
            loss = compute_weighted_loss(predicted_velocities=predicted_velocities,
                                         predicted_steering_angles=predicted_steering_angles,
                                         velocities=velocities,
                                         steering_angles=steering_angles,
                                         criterion=criterion,
                                         alpha=args.alpha,
                                         beta=args.beta)
            
            # Backpropagate the loss and update model weights
            loss.backward()
            optimizer.step()
            optimizer.zero_grad()

            # Accumulate the training loss and error metrics
            train_loss += loss.item()
            velocity_error += torch.mean(
                torch.abs(velocities - predicted_velocities)).item()
            
            steering_angle_error += torch.mean(
                torch.abs(steering_angles - predicted_steering_angles)).item()
            
        # Calculate the mean training loss and errors for the epoch
        mean_train_loss = train_loss / (i + 1)
        mean_velocity_error = velocity_error / (i + 1)
        mean_steering_angle_error = steering_angle_error / (i + 1)
        
        # Evaluate the model on the validation set
        mean_val_loss, mean_val_velocity_error, mean_val_steering_angle_error = \
            validate(model, val_loader, criterion, device, args.alpha, args.beta)

        print(f"Epoch {epoch} | Train Loss: {mean_train_loss:.4f} | Val Loss: {mean_val_loss:.4f}")

        # Log metrics to TensorBoard
        writer.add_scalar("Loss/Train", mean_train_loss, epoch)
        writer.add_scalar("Loss/Validation", mean_val_loss, epoch)
        writer.add_scalar("Velocity Error/Train", mean_velocity_error, epoch)
        writer.add_scalar("Steering Angle Error/Train", mean_steering_angle_error, epoch)
        writer.add_scalar("Velocity Error/Validation", mean_val_velocity_error, epoch)
        writer.add_scalar("Steering Angle Error/Validation", mean_val_steering_angle_error, epoch)
        writer.add_scalar("Learning Rate", scheduler.get_last_lr()[0], epoch)

        # Update the learning rate based on the validation loss
        scheduler.step(mean_val_loss)

        # Save the model if the validation loss has improved
        if mean_val_loss < best_val_loss:
            best_val_loss = mean_val_loss
            save_model(model, optimizer, scheduler, epoch, best_model_path)

    writer.close()


def main(args):
    """
    @brief Entry point for the training script.

    Parses command-line arguments, initializes the model, creates the datasets,
    splits them into training and validation sets, and then starts the training
    process by calling the train function.

    @param args The parsed command-line arguments.
    """
    # Initialize model
    model = AutopilotNet(h=args.height, w=args.width, inputs=1)  
    
    # Training dataset with augmentation
    train_dataset = Data(args.dataset_path, 
                         width=args.width, 
                         height=args.height, 
                         augment=True)  
    
    # Validation dataset without augmentation
    val_dataset = Data(args.dataset_path,
                       width=args.width, 
                       height=args.height, 
                       augment=False)
    
    # Split the dataset between training and validation
    val_fraction = args.val_fraction # Validation percentage
    val_size = int(len(train_dataset) * val_fraction)  # Number of validation samples
    train_size = len(train_dataset) - val_size  # Number of training samples

    # Generate random train/val indices
    train_indices, val_indices = random_split(range(len(train_dataset)),
                                              [train_size, val_size])
    
    # Apply the generated train and validation indices to create subset datasets
    train_dataset = torch.utils.data.Subset(train_dataset, train_indices)  
    val_dataset = torch.utils.data.Subset(val_dataset, val_indices)

    # Start training
    train(args, model, train_dataset, val_dataset)  


if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    
    parser.add_argument('--dataset_path', type=str, required=True,
                        help="Path to the directory containing the dataset.")
    
    parser.add_argument('--model_path', type=str, required=True,
                        help="Path to save the best model checkpoint.")
    
    parser.add_argument('--epochs', type=int, default=100,
                        help="Number of training epochs.")
    
    parser.add_argument('--batch_size', type=int, default=1024,
                        help="Batch size for the training data loader.")
    
    parser.add_argument('--val_batch_size', type=int, default=256,
                        help="Batch size for the validation data loader.")
    
    parser.add_argument('--val_fraction', type=float, default=0.2,
                        help="Fraction of the dataset to use for validation (e.g., 0.2 for 20%).")

    parser.add_argument('--learning_rate', type=float, default=1.0e-3)
    
    parser.add_argument('--lr_patience', type=int, default=2, 
                        help="Epochs to wait for loss improvement before reducing learning rate.")
    
    parser.add_argument('--lr_factor', type=float, default=0.5,
                        help="Factor by which to reduce learning rate on plateau.")
    
    parser.add_argument('--alpha', type=float, default=0.1,
                        help="Weight for the velocity component of the loss function.")
    
    parser.add_argument('--beta', type=float, default=1.0,
                        help="Weight for the steering angle component of the loss function.")
    
    parser.add_argument('--num_workers', type=int, default=(multiprocessing.cpu_count() - 1),
                        help="Number of worker processes for data loading.")
    
    parser.add_argument('--height', type=int, default=96,
                        help="Height to which input images will be resized.")
    
    parser.add_argument('--width', type=int, default=128,
                        help="Width to which input images will be resized.")

    parser.add_argument('--sampler_low_fraction', type=float, default=0.05, 
                        help="Fraction of low-steering samples to keep.")
    
    parser.add_argument('--sampler_threshold_ratio', type=float, default=0.10,
                        help="Steering fraction of max steering to define low steering region.")

    args = parser.parse_args()

    main(args)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
import threading
import time

import torch
import PIL

import rclpy
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from std_msgs.msg import Float64
from sensor_msgs.msg import Image

from autopilot_neural_network.parameters_client_node import ParametersClientNode
from scripts.model import AutopilotNet
from scripts.dataset import preprocessing_transform

from cv_bridge import CvBridge
import cv2


class Autopilot(ParametersClientNode):
    """
    Subscribes to camera images and continuously runs inference in a separate thread
    to generate steering and velocity commands. Older frames are dropped to ensure
    real-time processing. Uses parameters from a vehicle controller node to scale network 
    outputs to actual vehicle commands.
    """

    def __init__(self):
        """
        @brief Initializes the Autopilot node.

        Loads the neural network model, sets up image preprocessing, ROS publishers and subscribers,
        and starts the inference thread.
        """
        super().__init__("autopilot")

        # Load ROS parameters
        self.declare_parameter("vehicle_node", "vehicle_controller")
        self.declare_parameter("velocity_topic", "/velocity")
        self.declare_parameter("steering_angle_topic", "/steering_angle")
        self.declare_parameter("image_topic", "/camera/image_raw")
        
        self.declare_parameter("image_width", 128)
        self.declare_parameter("image_height", 96)
        
        self.declare_parameter('model_path', '/tmp/autopilot_neural_network/model.pt')
        
        self.declare_parameter("max_velocity_parameter", "max_velocity")
        self.declare_parameter("max_steering_angle_parameter", "max_steering_angle")
        
        # Retrieve parameters
        self.vehicle_node = self.get_parameter("vehicle_node").value
        self.velocity_topic = self.get_parameter("velocity_topic").value
        self.steering_angle_topic = self.get_parameter("steering_angle_topic").value
        self.image_topic = self.get_parameter("image_topic").value
        self.image_width = self.get_parameter("image_width").value
        self.image_height = self.get_parameter("image_height").value
        self.model_path = self.get_parameter("model_path").value
        
        max_velocity_parameter = self.get_parameter("max_velocity_parameter").value
        max_steering_angle_parameter = self.get_parameter("max_steering_angle_parameter").value

        # Vehicle controller parameters
        self.max_velocity = None
        self.max_steering_angle = None

        # Request vehicle controller parameters (max_velocity, max_steering_angle)
        self.max_velocity, self.max_steering_angle = self.request_parameters(
            self.vehicle_node, [max_velocity_parameter, max_steering_angle_parameter])

        self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu")

        # Load neural network model
        self.get_logger().info(f"Loading model from: {self.model_path}")

        self.model = AutopilotNet()
        
        checkpoint = torch.load(self.model_path, map_location=self.device)
        
        self.model.load_state_dict(checkpoint['state_dict'])
        self.model.eval()
        self.model.to(self.device)

        # Image preprocessing
        self.bridge = CvBridge()
        
        # Initialize the image preprocessing pipeline to match the training configuration.
        self.transform = preprocessing_transform(self.image_height, 
                                                 self.image_width)
        
        # QoS for subscriber: keep only the latest message
        qos_profile = QoSProfile(reliability=QoSReliabilityPolicy.BEST_EFFORT,
                                 history=QoSHistoryPolicy.KEEP_LAST,
                                 depth=1)

        # Publishers
        self.velocity_pub = self.create_publisher(
            Float64, self.velocity_topic, 1)
        
        self.steering_pub = self.create_publisher(
            Float64, self.steering_angle_topic, 1)

        # Subscriber
        self.create_subscription(Image, self.image_topic, 
                                 self._image_callback, qos_profile)

        # Inference management
        self.latest_frame = None # Stores the latest camera frame
        self.lock = threading.Lock() # Protects access to latest_frame
        self.running = True # Controls inference thread
        self.inference_thread = threading.Thread(target=self._inference_loop,
                                                 daemon=True)
        self.inference_thread.start()

        self.get_logger().info("Autopilot initialized.")

    def _image_callback(self, msg: Image):
        """
        @brief Callback for incoming camera images.

        Stores the latest frame and discards older ones, implementing UDP-like behavior.

        @param msg ROS Image message from camera topic
        """
        with self.lock:
            self.latest_frame = msg

    def _inference_loop(self):
        """
        @brief Continuous loop that processes the latest frame.

        Runs in a separate thread, checking for new frames and running inference.
        Older frames are dropped to ensure real-time performance.
        """
        while self.running and rclpy.ok():
            frame = None
            with self.lock:
                if self.latest_frame is not None:
                    frame = self.latest_frame
                    self.latest_frame = None  # Drop older frames

            if frame is not None:
                self._process_frame(frame)
            else:
                time.sleep(0.005)  # Small delay to avoid busy-waiting

    def _process_frame(self, msg: Image):
        """
        @brief Converts the ROS Image message, runs inference, and publishes commands.

        Converts the image to grayscale, applies preprocessing, runs the neural network,
        scales outputs by vehicle parameters, and publishes velocity and steering messages.

        @param msg ROS Image message to process
        """
        try:
            cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
        except Exception as e:
            self.get_logger().error(f"CvBridge error: {e}")
            return

        gray_image = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY) # Convert to grayscale
        pil_gray = PIL.Image.fromarray(gray_image) # Convert to PIL Image
        input_tensor = self.transform(pil_gray).unsqueeze(0).to(self.device)
        
        # Inference
        with torch.no_grad():
            output = self.model(input_tensor)
            pred_velocity = output[0, 0]
            pred_steering = output[0, 1]

        # Scale predictions to actual vehicle commands
        velocity = float(pred_velocity.item()) * self.max_velocity
        steering = float(pred_steering.item()) * self.max_steering_angle

        # Publish results
        self.velocity_pub.publish(Float64(data=velocity))
        self.steering_pub.publish(Float64(data=steering))

        # self.get_logger().info(f"Published - Velocity: {velocity:.3f}, Steering: {steering:.3f}")

    def destroy_node(self):
        """
        @brief Stops the inference thread before destroying the ROS2 node.

        Ensures the inference thread is safely joined to avoid dangling threads.
        """
        self.running = False
        self.inference_thread.join()
        super().destroy_node()


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

    node = Autopilot()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    """
    Launches the autopilot node with parameters loaded from a YAML file.
    """

    package_name = 'autopilot_neural_network'
    package_share = get_package_share_directory(package_name)

    # Path to the YAML configuration file
    params_file = os.path.join(package_share, 'config', 'parameters.yaml')

    # Define the node to launch
    autopilot_node = Node(package=package_name,
                          executable='autopilot',
                          name='autopilot',
                          output='screen',
                          parameters=[params_file])

    return LaunchDescription([autopilot_node])

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
from setuptools import setup, find_packages
import os
from glob import glob

package_name = 'autopilot_neural_network'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(),  # includes scripts/ automatically
    data_files=[
        # Package index
        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),

        # package.xml
        ('share/' + package_name, ['package.xml']),

        # Launch files
        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),

        # Config files
        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Lucas Mazzetto',
    maintainer_email='workabotic@gmail.com',
    description='An end-to-end deep learning pipeline for autonomous driving in ROS 2.',
    license='MIT',
    entry_points={
        'console_scripts': [
            'autopilot = autopilot_neural_network.autopilot:main',
            'data_collector = autopilot_neural_network.data_collector:main',
        ],
    },
)