Thx for you suggestion ! But let’s stick to our original implementation. I would like to mention few key mistakes: 1. After launching gazebo with my simulation world which is a wall enclosed plain field with a car in it’s initial position. After that I create some obstacles on the field and once it’s completed then I launch my evader code. The evader node is given with a final destination point or a goal to reach in my case I’ve taken as (10,10). So, the car now should change it’s orientation in the direction of my final point and should start moving towards it at 2m/s. If it comes across any obstacle, it should take 0.2 m reverse and again should align in the direction of final point and move forward in that direction. Meanwhile, my agent should learn based on the actions taken which leads to the best reward and which action leads to negative rewards. You can also add reward like after avoiding 2 consecutive obstacles it should be awarded double the normal reward . So, by the time it reaches destination it should learn something . Now the real one starts, the car should align in the initial point direction and should move in that direction avoiding all the obstacles by choosing actions from the learnt policy .Feel free to make all necessary modifications to my code. I made some mess. I want you to implement clean code by following a good code structure.  IMake all the modifications and give me end to end complete code.                                                                                                                                                  #! /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 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.init_location = None  # Set the initial location in odom_callback when available
       
        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
        
        self.final_destination = (10, 10)  # Set the final destination here
    


    # Aligns the car orientation to the final destination using the initial position
    def initialize_final_destination(self):
        init_x, init_y = self.init_location
        dest_x, dest_y = self.final_destination

        angle_to_dest = math.atan2(dest_y - init_y, dest_x - init_x)

        align_drive_msg = AckermannDrive()
        align_drive_msg.steering_angle = angle_to_dest
        # align_drive_msg.speed = 2

        self.drive_pub.publish(align_drive_msg)

    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)


        drive_msg = self.action_to_drive_msg(action)

        collision_detected = self.check_collision(scan)

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

        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
        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
              
                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)
        
        # if self.init_odom_received:
            # self.initialize_final_destination()

    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
        
        if self.init_odom_received:
            self.initialize_final_destination()

        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,))  # 360 laser scan equidistant points + (x, y) position + grid

        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