Hi, I’ve implemented the following code. 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 . I’m not sure if the DQN is a right choice for this since I don’t know what data to feed to neural net. 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. Here is the implementation.
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.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