cse4568@cse4568-VM:~/catkin_ws$ rostopic list
/car_1/base/footprint
/car_1/base/odom
/car_1/camera/left/camera_info
/car_1/camera/left/image_raw
/car_1/camera/left/image_raw/compressed
/car_1/camera/left/image_raw/compressed/parameter_descriptions
/car_1/camera/left/image_raw/compressed/parameter_updates
/car_1/camera/left/image_raw/compressedDepth
/car_1/camera/left/image_raw/compressedDepth/parameter_descriptions
/car_1/camera/left/image_raw/compressedDepth/parameter_updates
/car_1/camera/left/image_raw/theora
/car_1/camera/left/image_raw/theora/parameter_descriptions
/car_1/camera/left/image_raw/theora/parameter_updates
/car_1/camera/left/parameter_descriptions
/car_1/camera/left/parameter_updates
/car_1/camera/right/camera_info
/car_1/camera/right/image_raw
/car_1/camera/right/image_raw/compressed
/car_1/camera/right/image_raw/compressed/parameter_descriptions
/car_1/camera/right/image_raw/compressed/parameter_updates
/car_1/camera/right/image_raw/compressedDepth
/car_1/camera/right/image_raw/compressedDepth/parameter_descriptions
/car_1/camera/right/image_raw/compressedDepth/parameter_updates
/car_1/camera/right/image_raw/theora
/car_1/camera/right/image_raw/theora/parameter_descriptions
/car_1/camera/right/image_raw/theora/parameter_updates
/car_1/camera/right/parameter_descriptions
/car_1/camera/right/parameter_updates
/car_1/command
/car_1/ground_truth
/car_1/ground_velocity
/car_1/joint_states
/car_1/left_rear_wheel_velocity_controller/command
/car_1/left_rear_wheel_velocity_controller/pid/parameter_descriptions
/car_1/left_rear_wheel_velocity_controller/pid/parameter_updates
/car_1/left_rear_wheel_velocity_controller/state
/car_1/left_steering_hinge_position_controller/command
/car_1/left_steering_hinge_position_controller/pid/parameter_descriptions
/car_1/left_steering_hinge_position_controller/pid/parameter_updates
/car_1/left_steering_hinge_position_controller/state
/car_1/right_rear_wheel_velocity_controller/command
/car_1/right_rear_wheel_velocity_controller/pid/parameter_descriptions
/car_1/right_rear_wheel_velocity_controller/pid/parameter_updates
/car_1/right_rear_wheel_velocity_controller/state
/car_1/right_steering_hinge_position_controller/command
/car_1/right_steering_hinge_position_controller/pid/parameter_descriptions
/car_1/right_steering_hinge_position_controller/pid/parameter_updates
/car_1/right_steering_hinge_position_controller/state
/car_1/scan
/clear_path_msg
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/performance_metrics
/gazebo/set_link_state
/gazebo/set_model_state
/joy
/path/filtered
/rosout
/rosout_agg
/tf
These are the topics and I want to get the gridspace to set the observation space for my DQNAgent. SO, how to get the grid space my car enlcosed in. #! /usr/bin/env python3 

import tf
import copy
import time
import rospy
import random
import numpy as np
import math
from collections import deque
from sensor_msgs.msg import LaserScan
from ackermann_msgs.msg import AckermannDrive
from nav_msgs.msg import Odometry
import torch
import torch.nn as nn
import torch.optim as optim


FORWARD = 0
LEFT = 1
RIGHT = 2
REVERSE = 3
ACTION_SPACE = [FORWARD, LEFT, RIGHT, REVERSE]

DISTANCE_THRESHOLD = 2  # meters


class DQNAgentPytorch:
    def __init__(self, action_space, observation_space, memory_size=10000, epsilon=1.0, gamma=0.99, learning_rate=0.001):
        self.observation_space = observation_space
        self.action_space = action_space
        self.memory = deque(maxlen=memory_size)
        self.epsilon = epsilon
        self.gamma = gamma
        self.lr = learning_rate
        self.model = self.build_model()
        self.target_model = self.build_model()
        self.optimizer = optim.AdamW(self.model.parameters(), lr=self.lr, amsgrad=True)
        self.criterion = nn.SmoothL1Loss()
        self.update_target_net()

    def build_model(self):
        model = nn.Sequential(
            nn.Linear(self.observation_space.shape[0], 128),
            nn.ReLU(),
            nn.Linear(128, 128),
            nn.ReLU(),
            nn.Linear(128, len(self.action_space))
        )
        return model

    def add_experience(self, state, action, reward, next_state, done):
        self.memory.append((state, action, reward, next_state, done))

    def choose_action(self, state, test=False):
        if test or random.random() > self.epsilon:
            with torch.no_grad():
                state_t = torch.tensor(state, dtype=torch.float32).unsqueeze(0)
                q_values = self.model(state_t)
                return torch.argmax(q_values).item()
        else:
            return np.random.choice(self.action_space)

    def train(self, batch_size):
        if len(self.memory) < batch_size:
            return

        minibatch = random.sample(self.memory, batch_size)
        for state, action, reward, next_state, done in minibatch:
            target = self.target_model(torch.tensor(state, dtype=torch.float32).unsqueeze(0)).squeeze(0)
            if done:
                target[action] = reward
            else:
                next_q_values = self.target_model(torch.tensor(next_state, dtype=torch.float32).unsqueeze(0)).squeeze(0)
                target[action] = reward + self.gamma * torch.max(next_q_values)

            inputs = torch.tensor(state, dtype=torch.float32).unsqueeze(0)
            targets = target.unsqueeze(0)

            self.optimizer.zero_grad()

            outputs = self.model(inputs)
            loss = self.criterion(outputs, targets)
            loss.backward()

            # In-place gradient clipping
            torch.nn.utils.clip_grad_value_(self.model.parameters(), 100)
            self.optimizer.step()

    def update_target_net(self):
        self.target_model.load_state_dict(self.model.state_dict())

    def update_epsilon(self, new_epsilon):
        self.epsilon = new_epsilon

    def save_weights(self, file_name):
        torch.save(self.model.state_dict(), file_name)

    def load_weights(self, file_name):
        self.epsilon = 0
        self.model.load_state_dict(torch.load(file_name))

    def calculate_reward(self, state, action, min_distance, reward_collision):
        reward = 0
        if action == REVERSE and reward_collision:
            reward -= 1  # decrease the reward for reversing due to collision
        else:
            reward += max(0, (min_distance - DISTANCE_THRESHOLD) / DISTANCE_THRESHOLD)  # increase the reward for avoiding obstacles
        return reward


class EvaderNode:
    def __init__(self, agent):
        rospy.init_node('evader_node', anonymous=True)
        rospy.Subscriber('car_1/scan', LaserScan, self.scan_callback)

        rospy.Subscriber('car_1/base/odom', Odometry, self.odom_callback)
        self.drive_pub = rospy.Publisher('car_1/command', AckermannDrive, queue_size=10)

        # self.current_location = copy.deepcopy(self.init_location)


        self.is_training = False # Flag to indicate if the agent is being trained

        self.init_location = None  # Set the initial location in odom_callback when available
        self.final_destination = (10, 10)  # Set the final destination here
        self.current_location = None  # Will be updated in update_current_location method
        
        self.distance_threshold = 2  # meters
        self.angle_range = [(-150, -30), (30, 150)]

        self.agent = agent
        self.collision_threshold = 0.2

        self.is_training = True  # Flag to indicate if the agent is being trained
        self.init_odom_received = False
        self.latest_odom_msg = None
        self.reached_destination = False

    def process_scan(self, scan):
        # Process the laser scan data to get the state input for the DQN
        state = np.array(scan.ranges)[::10]  # Downsample the scan data for faster processing
        # return np.concatenate((state, self.current_location))
        return state

    def check_collision(self, scan):
        for i in range(len(scan.ranges)):
            if scan.ranges[i] < self.collision_threshold:
                return True
        return False
    def align_to_initial_point(self):
        init_x, init_y, _ = self.init_location
        curr_x, curr_y, curr_yaw = self.current_location
        target_angle = math.atan2(init_y - curr_y, init_x - curr_x)
        angle_diff = target_angle - curr_yaw

        rospy.loginfo('Initializing alignment sequence…')
        align_drive_msg = AckermannDrive()
        align_drive_msg.speed = 0
        align_drive_msg.steering_angle = angle_diff
        self.drive_pub.publish(align_drive_msg)
        rospy.sleep(1)  # Time for steering angle to update
    

    def action_to_drive_msg(self, action):
        drive_msg = AckermannDrive()
        if action == FORWARD:
            drive_msg.speed = 2.0
            drive_msg.steering_angle = 0.0
        elif action == LEFT:
            drive_msg.speed = 2.0
            drive_msg.steering_angle = float(random.uniform(math.pi/4, math.pi/2))
        elif action == RIGHT:
            drive_msg.speed = 2.0
            drive_msg.steering_angle = float(random.uniform(-math.pi/2, -math.pi/4))
        elif action == REVERSE:
            drive_msg.speed = -2.0
            drive_msg.steering_angle = 0.0
        return drive_msg

    def reverse_car(self, reverse_distance):
        reverse_drive_msg = AckermannDrive()
        reverse_drive_msg.speed = -2.0
        reverse_drive_msg.steering_angle = 0.0
        reverse_duration = reverse_distance / (-reverse_drive_msg.speed)

        start_time = time.time()
        while time.time() - start_time < reverse_duration:
            self.drive_pub.publish(reverse_drive_msg)
            rospy.sleep(0.1)

    def scan_callback(self, scan):
        state = self.process_scan(scan)
        action = self.agent.choose_action(state, test=not self.is_training)

        collision_detected = self.check_collision(scan)
        if collision_detected and not self.reached_destination:
            self.reverse_car(0.2)  # Set the reverse distance here

        drive_msg = self.action_to_drive_msg(action)

        if self.latest_odom_msg is not None:
            self.update_current_location(self.latest_odom_msg)


        if not self.is_training:
            if np.linalg.norm(np.array(self.final_destination) - np.array(self.current_location)) < self.distance_threshold:
                self.current_location = copy.deepcopy(self.init_location)
            self.drive_pub.publish(drive_msg)
            return

        min_distance = min(state)
        reward_collision = collision_detected and not self.reached_destination
        reward = self.agent.calculate_reward(state, action, min_distance, reward_collision)

        done = False  # set this flag to True if a termination condition is met
        self.agent.add_experience(state, action, reward, state, done)
        self.agent.train(32)  # Set the batch size here

        self.drive_pub.publish(drive_msg)

        # Set the termination condition for training
        # Set the termination condition for training
        if np.linalg.norm(np.array(self.final_destination) - np.array(self.current_location)[:2]) < 0.5:
            if not self.reached_destination:
                print('Reached final destination!')
                self.reached_destination = True
                self.align_to_initial_point()
            else:
                print('Reached the initial point!')
                self.reached_destination = False
            if self.is_training:
                self.is_training = False
                done = True
                self.agent.save_weights('model_weights.pth')
                self.agent.load_weights('model_weights.pth')

    def update_current_location(self, odom_msg):
        position = odom_msg.pose.pose.position
        orientation = odom_msg.pose.pose.orientation
        euler_angles = tf.transformations.euler_from_quaternion((orientation.x, orientation.y, orientation.z, orientation.w))

        self.current_location = (position.x, position.y, euler_angles[2])  # (x, y, yaw)

    def odom_callback(self, odom_msg):

        if not self.init_odom_received:
    
            self.init_location = (odom_msg.pose.pose.position.x,
                                  odom_msg.pose.pose.position.y)
            print(f"Intial Location of the Car: {self.init_location}")
            self.init_odom_received = True

        self.latest_odom_msg = odom_msg


if __name__ == '__main__':
    try:
        # Initialize the DQNAgentPytorch instance with the necessary environment and parameters
        action_space = ACTION_SPACE
        observation_space = np.zeros((360 + 3,))  # 360 laser scan equidistant points
        agent = DQNAgentPytorch(action_space, observation_space)
        

        # Initialize the EvaderNode instance with the DQNAgent
        node = EvaderNode(agent)
        
        # Wait for initial odometry information
        while not node.init_odom_received:
            rospy.sleep(0.1)
        
        rospy.spin()

    except rospy.ROSInterruptException:
        pass