PHASE 1: CÀI ĐẶT MÔI TRƯỜNG

Bước 1: Cài ROS2 Humble

bash
# Thêm ROS2 repo
sudo apt install software-properties-common
sudo add-apt-repository universe
sudo apt update && sudo apt install curl -y

curl -sSL [Link] \

-o /usr/share/keyrings/[Link]

echo "deb [arch=$(dpkg --print-architecture) \

signed-by=/usr/share/keyrings/[Link]] \
[Link] \
$(. /etc/os-release && echo $UBUNTU_CODENAME) main" \
| sudo tee /etc/apt/[Link].d/[Link] > /dev/null

# Cài ROS2 Humble Desktop

sudo apt update
sudo apt install ros-humble-desktop -y

# Source ROS2 (thêm vào ~/.bashrc để tự động)

echo "source /opt/ros/humble/[Link]" >> ~/.bashrc
source ~/.bashrc

Bước 2: Cài Gazebo Fortress

bash
# Thêm Gazebo repo
sudo curl [Link] \
--output /usr/share/keyrings/[Link]

echo "deb [arch=$(dpkg --print-architecture) \

signed-by=/usr/share/keyrings/[Link]] \
[Link] \
$(lsb_release -cs) main" \
| sudo tee /etc/apt/[Link].d/[Link] > /dev/null

sudo apt update

sudo apt install ignition-fortress -y

# Kiểm tra
ign gazebo --version # phải ra "Ignition Gazebo, version 6.x.x"

Bước 3: Cài các package ROS2 cần thiết

bash
sudo apt install -y \
 ros-humble-ros-gz \
ros-humble-ros-gz-bridge \
ros-humble-ros-gz-sim \
ros-humble-ros-gz-interfaces \
ros-humble-tf2-ros \
ros-humble-tf2-tools \
ros-humble-cv-bridge \
ros-humble-tf-transformations \
ros-humble-laser-geometry \
python3-pip

pip3 install torch torchvision numpy matplotlib \

opencv-python tensorboard pyyaml

Bước 4: Tạo Workspace

bash
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

# Tạo package
ros2 pkg create qr_dqn_nav \
--build-type ament_python \
--dependencies rclpy sensor_msgs geometry_msgs nav_msgs tf2_ros

cd ~/ros2_ws
colcon build
source install/[Link]
echo "source ~/ros2_ws/install/[Link]" >> ~/.bashrc

PHASE 2: TẠO ROBOT (URDF/SDF)

Bước 5: Tạo file robot SDF
bash
mkdir -p ~/ros2_ws/src/qr_dqn_nav/models/my_robot
bash
# Tạo file
cat > ~/ros2_ws/src/qr_dqn_nav/models/my_robot/[Link] << 'EOF'
<?xml version="1.0" ?>
<sdf version="1.8">
<model name="my_robot">
<static>false</static>

<!-- BASE LINK -->

<link name="base_link">
<inertial>
<mass>5.0</mass>
<inertia>
 <ixx>0.1</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.1</iyy><iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<collision name="base_collision">
<geometry>
<box><size>0.4 0.3 0.15</size></box>
</geometry>
</collision>
<visual name="base_visual">
<geometry>
<box><size>0.4 0.3 0.15</size></box>
</geometry>
<material>
<ambient>0.2 0.4 0.8 1</ambient>
<diffuse>0.2 0.4 0.8 1</diffuse>
</material>
</visual>
</link>

<!-- WHEEL LEFT -->

<link name="wheel_left">
<pose>0.0 0.18 -0.05 1.5707 0 0</pose>
<inertial><mass>0.5</mass>
<inertia><ixx>0.01</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.01</iyy><iyz>0</iyz><izz>0.01</izz></inertia>
</inertial>
<collision name="col"><geometry>
<cylinder><radius>0.08</radius><length>0.04</length></cylinder>
</geometry></collision>
<visual name="vis"><geometry>
<cylinder><radius>0.08</radius><length>0.04</length></cylinder>
</geometry>
<material><ambient>0.1 0.1 0.1 1</ambient></material>
</visual>
</link>

<!-- WHEEL RIGHT -->

<link name="wheel_right">
<pose>0.0 -0.18 -0.05 1.5707 0 0</pose>
<inertial><mass>0.5</mass>
<inertia><ixx>0.01</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.01</iyy><iyz>0</iyz><izz>0.01</izz></inertia>
</inertial>
<collision name="col"><geometry>
<cylinder><radius>0.08</radius><length>0.04</length></cylinder>
</geometry></collision>
<visual name="vis"><geometry>
<cylinder><radius>0.08</radius><length>0.04</length></cylinder>
</geometry>
<material><ambient>0.1 0.1 0.1 1</ambient></material>
</visual>
</link>
<!-- CASTER (bánh đa hướng) -->
<link name="caster">
<pose>-0.15 0 -0.07 0 0 0</pose>
<inertial><mass>0.1</mass>
<inertia><ixx>0.001</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.001</iyy><iyz>0</iyz><izz>0.001</izz></inertia>
</inertial>
<collision name="col">
<geometry><sphere><radius>0.03</radius></sphere></geometry>
<surface><friction><ode>
<mu>0.0</mu><mu2>0.0</mu2>
</ode></friction></surface>
</collision>
<visual name="vis">
<geometry><sphere><radius>0.03</radius></sphere></geometry>
</visual>
</link>

<!-- JOINTS -->

<joint name="left_wheel_joint" type="revolute">
<parent>base_link</parent><child>wheel_left</child>
<axis><xyz>0 0 1</xyz></axis>
</joint>
<joint name="right_wheel_joint" type="revolute">
<parent>base_link</parent><child>wheel_right</child>
<axis><xyz>0 0 1</xyz></axis>
</joint>
<joint name="caster_joint" type="ball">
<parent>base_link</parent><child>caster</child>
</joint>

<!-- LIDAR LINK -->

<link name="lidar_link">
<pose>0.15 0 0.1 0 0 0</pose>
<visual name="vis">
<geometry><cylinder>
<radius>0.03</radius><length>0.04</length>
</cylinder></geometry>
<material><ambient>0.8 0.2 0.2 1</ambient></material>
</visual>
<sensor name="lidar" type="gpu_lidar">
<topic>/lidar</topic>
<update_rate>10</update_rate>
<lidar>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1</resolution>
<min_angle>-3.14159</min_angle>
<max_angle>3.14159</max_angle>
</horizontal>
</scan>
<range>
<min>0.12</min><max>10.0</max>
<resolution>0.015</resolution>
 </range>
</lidar>
<always_on>true</always_on>
<visualize>true</visualize>
</sensor>
</link>
<joint name="lidar_joint" type="fixed">
<parent>base_link</parent><child>lidar_link</child>
</joint>

<!-- DEPTH CAMERA LINK -->

<link name="camera_link">
<pose>0.2 0 0.08 0 0 0</pose>
<visual name="vis">
<geometry><box><size>0.02 0.08 0.03</size></box></geometry>
<material><ambient>0.1 0.1 0.1 1</ambient></material>
</visual>
<sensor name="depth_camera" type="depth_camera">
<topic>/depth_camera</topic>
<update_rate>15</update_rate>
<camera>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>84</width><height>84</height>
<format>R_FLOAT32</format>
</image>
<clip><near>0.1</near><far>5.0</far></clip>
</camera>
<always_on>true</always_on>
</sensor>
</link>
<joint name="camera_joint" type="fixed">
<parent>base_link</parent><child>camera_link</child>
</joint>

<!-- DIFF DRIVE PLUGIN -->

<plugin filename="ignition-gazebo-diff-drive-system"
name="ignition::gazebo::systems::DiffDrive">
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>0.36</wheel_separation>
<wheel_radius>0.08</wheel_radius>
<odom_publish_frequency>20</odom_publish_frequency>
<topic>/cmd_vel</topic>
<odom_topic>/odom</odom_topic>
<frame_id>odom</frame_id>
<child_frame_id>base_link</child_frame_id>
</plugin>

<!-- POSE PUBLISHER PLUGIN -->

<plugin filename="ignition-gazebo-pose-publisher-system"
name="ignition::gazebo::systems::PosePublisher">
<publish_link_pose>false</publish_link_pose>
<publish_model_pose>true</publish_model_pose>
<update_frequency>20</update_frequency>
 </plugin>

</model>
</sdf>
EOF

PHASE 3: TẠO WORLD FILE

Bước 6: Download model người đi bộ từ Fuel
bash
# Tạo thư mục models
mkdir -p ~/.ignition/fuel/[Link]/openrobotics/models/

# Download actor (người đi bộ)

ign fuel download \
-u [Link] \
--jobs 4

Bước 7: Tạo World SDF

bash
mkdir -p ~/ros2_ws/src/qr_dqn_nav/worlds
cat > ~/ros2_ws/src/qr_dqn_nav/worlds/training_world.sdf << 'EOF'
<?xml version="1.0" ?>
<sdf version="1.8">
<world name="training_world">

<!-- PHYSICS -->

<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>

<!-- PLUGINS CẦN THIẾT -->

<plugin filename="ignition-gazebo-physics-system"
name="ignition::gazebo::systems::Physics"/>
<plugin filename="ignition-gazebo-sensors-system"
name="ignition::gazebo::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
<plugin filename="ignition-gazebo-scene-broadcaster-system"
name="ignition::gazebo::systems::SceneBroadcaster"/>
<plugin filename="ignition-gazebo-user-commands-system"
name="ignition::gazebo::systems::UserCommands"/>
<plugin filename="ignition-gazebo-pose-publisher-system"
name="ignition::gazebo::systems::PosePublisher">
<publish_model_pose>true</publish_model_pose>
<update_frequency>10</update_frequency>
</plugin>
<!-- ÁNH SÁNG -->
<light name="sun" type="directional">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<direction>-0.5 0.1 -0.9</direction>
</light>

<!-- SÀN NHÀ -->

<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="col">
<geometry><plane>
<normal>0 0 1</normal>
<size>20 20</size>
</plane></geometry>
</collision>
<visual name="vis">
<geometry><plane>
<normal>0 0 1</normal>
<size>20 20</size>
</plane></geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
</material>
</visual>
</link>
</model>

<!-- ======= TƯỜNG BAO (4 bức tường) ======= -->

<!-- Tường Bắc -->
<model name="wall_north">
<static>true</static>
<pose>0 3 0.5 0 0 0</pose>
<link name="link">
<collision name="col"><geometry>
<box><size>6 0.1 1.0</size></box>
</geometry></collision>
<visual name="vis"><geometry>
<box><size>6 0.1 1.0</size></box>
</geometry>
<material><ambient>0.5 0.5 0.5 1</ambient></material>
</visual>
</link>
</model>

<!-- Tường Nam -->

<model name="wall_south">
<static>true</static>
<pose>0 -3 0.5 0 0 0</pose>
<link name="link">
 <collision name="col"><geometry>
<box><size>6 0.1 1.0</size></box>
</geometry></collision>
<visual name="vis"><geometry>
<box><size>6 0.1 1.0</size></box>
</geometry>
<material><ambient>0.5 0.5 0.5 1</ambient></material>
</visual>
</link>
</model>

<!-- Tường Đông -->

<model name="wall_east">
<static>true</static>
<pose>3 0 0.5 0 0 1.5707</pose>
<link name="link">
<collision name="col"><geometry>
<box><size>6 0.1 1.0</size></box>
</geometry></collision>
<visual name="vis"><geometry>
<box><size>6 0.1 1.0</size></box>
</geometry>
<material><ambient>0.5 0.5 0.5 1</ambient></material>
</visual>
</link>
</model>

<!-- Tường Tây -->

<model name="wall_west">
<static>true</static>
<pose>-3 0 0.5 0 0 1.5707</pose>
<link name="link">
<collision name="col"><geometry>
<box><size>6 0.1 1.0</size></box>
</geometry></collision>
<visual name="vis"><geometry>
<box><size>6 0.1 1.0</size></box>
</geometry>
<material><ambient>0.5 0.5 0.5 1</ambient></material>
</visual>
</link>
</model>

<!-- ======= VẬT CẢN TĨNH ======= -->

<model name="pillar_1">
<static>true</static>
<pose>1.0 1.0 0.5 0 0 0</pose>
<link name="link">
<collision name="col"><geometry>
<cylinder><radius>0.15</radius><length>1.0</length></cylinder>
</geometry></collision>
<visual name="vis"><geometry>
<cylinder><radius>0.15</radius><length>1.0</length></cylinder>
</geometry>
<material><ambient>0.6 0.4 0.2 1</ambient></material>
 </visual>
</link>
</model>

<model name="pillar_2">
<static>true</static>
<pose>-1.0 -1.0 0.5 0 0 0</pose>
<link name="link">
<collision name="col"><geometry>
<cylinder><radius>0.15</radius><length>1.0</length></cylinder>
</geometry></collision>
<visual name="vis"><geometry>
<cylinder><radius>0.15</radius><length>1.0</length></cylinder>
</geometry>
<material><ambient>0.6 0.4 0.2 1</ambient></material>
</visual>
</link>
</model>

<model name="box_obstacle">
<static>true</static>
<pose>0 1.5 0.25 0 0 0</pose>
<link name="link">
<collision name="col"><geometry>
<box><size>0.5 0.5 0.5</size></box>
</geometry></collision>
<visual name="vis"><geometry>
<box><size>0.5 0.5 0.5</size></box>
</geometry>
<material><ambient>0.3 0.6 0.3 1</ambient></material>
</visual>
</link>
</model>

<!-- ======= ROBOT ======= -->

<include>
<uri>model://my_robot</uri>
<name>my_robot</name>
<pose>-2.0 0.0 0.1 0 0 0</pose>
</include>

<!-- ======= NGƯỜI ĐI BỘ 1 ======= -->

<actor name="human_1">
<skin>
<filename>[Link]</filename>
<scale>1.0</scale>
</skin>
<animation name="walking">
<filename>[Link]</filename>
<interpolate_x>true</interpolate_x>
</animation>
<script>
<loop>true</loop>
<auto_start>true</auto_start>
<trajectory id="0" type="walking">
 <waypoint><time>0</time>
<pose>0 -2 0 0 0 1.57</pose></waypoint>
<waypoint><time>5</time>
<pose>0 2 0 0 0 1.57</pose></waypoint>
<waypoint><time>10</time>
<pose>0 -2 0 0 0 -1.57</pose></waypoint>
</trajectory>
</script>
</actor>

<!-- ======= NGƯỜI ĐI BỘ 2 ======= -->

<actor name="human_2">
<skin>
<filename>[Link]</filename>
<scale>1.0</scale>
</skin>
<animation name="walking">
<filename>[Link]</filename>
<interpolate_x>true</interpolate_x>
</animation>
<script>
<loop>true</loop>
<auto_start>true</auto_start>
<trajectory id="0" type="walking">
<waypoint><time>0</time>
<pose>-2 0 0 0 0 0</pose></waypoint>
<waypoint><time>6</time>
<pose>2 0 0 0 0 0</pose></waypoint>
<waypoint><time>12</time>
<pose>-2 0 0 0 0 3.14</pose></waypoint>
</trajectory>
</script>
</actor>

</world>
</sdf>
EOF

PHASE 4: CẤU HÌNH ROS-GZ BRIDGE

Bước 8: Tạo bridge config
bash
mkdir -p ~/ros2_ws/src/qr_dqn_nav/config
cat > ~/ros2_ws/src/qr_dqn_nav/config/[Link] << 'EOF'
- ros_topic_name: /cmd_vel
gz_topic_name: /cmd_vel
ros_type_name: geometry_msgs/msg/Twist
gz_type_name: [Link]
direction: ROS_TO_GZ
- ros_topic_name: /odom
gz_topic_name: /odom
ros_type_name: nav_msgs/msg/Odometry
gz_type_name: [Link]
direction: GZ_TO_ROS

- ros_topic_name: /tf
gz_topic_name: /tf
ros_type_name: tf2_msgs/msg/TFMessage
gz_type_name: [Link].Pose_V
direction: GZ_TO_ROS

- ros_topic_name: /lidar
gz_topic_name: /lidar
ros_type_name: sensor_msgs/msg/LaserScan
gz_type_name: [Link]
direction: GZ_TO_ROS

- ros_topic_name: /depth/image_raw
gz_topic_name: /depth_camera/depth_image
ros_type_name: sensor_msgs/msg/Image
gz_type_name: [Link]
direction: GZ_TO_ROS

- ros_topic_name: /gazebo_poses
gz_topic_name: /world/training_world/pose/info
ros_type_name: tf2_msgs/msg/TFMessage
gz_type_name: [Link].Pose_V
direction: GZ_TO_ROS
EOF

Bước 9: Tạo Launch file

bash
mkdir -p ~/ros2_ws/src/qr_dqn_nav/launch
cat > ~/ros2_ws/src/qr_dqn_nav/launch/[Link] << 'EOF'
import os
from launch import LaunchDescription
from [Link] import ExecuteProcess, TimerAction
from launch_ros.actions import Node

WORLD_FILE = [Link](
'~/ros2_ws/src/qr_dqn_nav/worlds/training_world.sdf'
)
BRIDGE_CONFIG = [Link](
'~/ros2_ws/src/qr_dqn_nav/config/[Link]'
)
MODEL_PATH = [Link](
'~/ros2_ws/src/qr_dqn_nav/models'
)

def generate_launch_description():
# 1. Khởi động Gazebo
gazebo = ExecuteProcess(
 cmd=['ign', 'gazebo', '-r', WORLD_FILE],
additional_env={
'IGN_GAZEBO_RESOURCE_PATH': MODEL_PATH,
'IGN_GAZEBO_MODEL_PATH': MODEL_PATH,
},
output='screen'
)

# 2. ROS-GZ Bridge (delay 3s cho Gazebo khởi động)

bridge = TimerAction(period=3.0, actions=[
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=[
'--ros-args', '-p',
f'config_file:={BRIDGE_CONFIG}'
],
output='screen'
)
])

# 3. Training node (delay 5s)

training = TimerAction(period=5.0, actions=[
ExecuteProcess(
cmd=['python3',
[Link](
'~/ros2_ws/src/qr_dqn_nav/[Link]'
)],
output='screen'
)
])

return LaunchDescription([gazebo, bridge, training])

EOF

PHASE 5: VIẾT CODE RL

Bước 10: Tạo model_config.yaml
bash
cat > ~/ros2_ws/src/qr_dqn_nav/src/model_config.yaml << 'EOF'
pos:
init_x: -2.0
init_y: 0.0
init_angle: 0.0
goal_x: 2.0
goal_y: 0.0
goal_angle: 0.0

num_episodes: 5000
replay_buffer_size: 50000
EOF

Bước 11: Tạo src/ros_module.py

bash
cat > ~/ros2_ws/src/qr_dqn_nav/src/ros_module.py << 'EOF'
import math
import time
import rclpy
from [Link] import Node
from [Link] import QoSProfile, ReliabilityPolicy, HistoryPolicy
from geometry_msgs.msg import Twist, Pose, Point, Quaternion
from sensor_msgs.msg import LaserScan, Image
from nav_msgs.msg import Odometry, Path
from tf2_msgs.msg import TFMessage
from std_msgs.msg import Float32MultiArray
from visualization_msgs.msg import Marker
from geometry_msgs.msg import PoseStamped
from ros_gz_interfaces.srv import SetEntityPose
from ros_gz_interfaces.msg import Entity

class RosHandler(Node):
def __init__(self):
super().__init__('rl_ros_handler')

# Sensor data
[Link] = None
[Link] = None
[Link] = None
[Link] = None

# Human actor poses

self.human_poses = {} # {"human_1": (x,y), ...}

qos = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=1
)

# Publishers
self.pub_cmd_vel = self.create_publisher(Twist, '/cmd_vel', 10)
self.pub_result = self.create_publisher(Float32MultiArray, 'result', 10)
self.pub_get_action= self.create_publisher(Float32MultiArray, 'get_action', 10)
self.pub_path = self.create_publisher(Path, '/robot_trajectory', 10)
self.pub_goal = self.create_publisher(Marker, '/goal_marker', 10)

# Subscribers
self.create_subscription(TFMessage, '/tf', self.tf_callback, qos)
self.create_subscription(Odometry, '/odom', self.odom_callback, qos)
self.create_subscription(LaserScan, '/lidar', self.lidar_callback, qos)
self.create_subscription(Image, '/depth/image_raw',self.depth_callback, qos)
self.create_subscription(TFMessage, '/gazebo_poses', self.poses_callback, 10)
 # Set pose service (để teleport robot)
self.set_pose_client = self.create_client(
SetEntityPose, '/world/training_world/set_pose'
)

# Path visualization
self.robot_path = Path()
self.robot_path.header.frame_id = 'odom'

# ---------- Callbacks ----------

def tf_callback(self, msg):
for t in [Link]:
if (t.child_frame_id == 'base_link' and
[Link].frame_id == 'odom'):
[Link] = t

def odom_callback(self, msg):

[Link] = msg

def lidar_callback(self, msg):

[Link] = msg

def depth_callback(self, msg):

[Link] = msg

def poses_callback(self, msg):

"""Track vị trí tất cả human actors"""
for t in [Link]:
name = t.child_frame_id
if [Link]('human_'):
self.human_poses[name] = (
[Link].x,
[Link].y
)

# ---------- Helpers ----------

def get_nearest_human(self, robot_x, robot_y):
if not self.human_poses:
return (10.0, 10.0), 10.0

min_dist = 999.0
nearest = (10.0, 10.0)
for (hx, hy) in self.human_poses.values():
d = [Link]((hx-robot_x)**2 + (hy-robot_y)**2)
if d < min_dist:
min_dist = d
nearest = (hx, hy)
return nearest, min_dist

def set_model_pose(self, name, x, y, z, qx, qy, qz, qw):

req = [Link]()
[Link] = name
[Link] = [Link]
[Link] = Pose()
 [Link] = Point(x=float(x), y=float(y), z=float(z))
[Link] = Quaternion(
x=float(qx), y=float(qy), z=float(qz), w=float(qw)
)
self.set_pose_client.call_async(req)

def clear_data(self):
[Link] = None
[Link] = None
[Link] = None
[Link] = None
EOF

Bước 12: Tạo src/[Link]

bash
cat > ~/ros2_ws/src/qr_dqn_nav/src/[Link] << 'EOF'
import torch
import [Link] as nn
import [Link] as F

class RandomShiftsAug([Link]):
"""Data augmentation: dịch chuyển ngẫu nhiên ±pad pixels"""
def __init__(self, pad=4):
super().__init__()
[Link] = pad

def forward(self, x):

n, c, h, w = [Link]()
padding = tuple([[Link]] * 4)
x = [Link](x, padding, 'replicate')
eps = 1.0 / (h + 2 * [Link])
arange = [Link](
-1.0 + eps, 1.0 - eps,
h + 2 * [Link], device=[Link], dtype=[Link]
)[:h]
arange = [Link](0).repeat(h, 1).unsqueeze(2)
base_grid = [Link]([arange, [Link](1, 0)], dim=2)
base_grid = base_grid.unsqueeze(0).repeat(n, 1, 1, 1)
shift = [Link](
0, 2 * [Link] + 1, size=(n, 1, 1, 2),
device=[Link], dtype=[Link]
)
shift *= 2.0 / (h + 2 * [Link])
return F.grid_sample(
x, base_grid + shift,
padding_mode='zeros', align_corners=False
)

class QR_DQN_Network([Link]):
"""
Multimodal QR-DQN:
 - CNN encoder cho depth image
- MLP encoder cho pose (9 dim với human info)
- MLP encoder cho LiDAR
- Attention fusion visual + lidar
- GMU fusion với pose
- Output: [batch, actions, quantiles]
"""
def __init__(self, num_actions=5, num_quantiles=32,
pose_dim=9, frame_stack=4, lidar_dim=360):
super().__init__()
[Link] = num_actions
[Link] = num_quantiles

# Visual CNN (DQN-style)

[Link] = [Link](
nn.Conv2d(frame_stack, 32, 8, stride=4), [Link](),
nn.Conv2d(32, 64, 4, stride=2), [Link](),
nn.Conv2d(64, 64, 3, stride=1), [Link](),
[Link]()
)
cnn_out = 64 * 7 * 7 # = 3136

# Projections
self.visual_proj = [Link](
[Link](cnn_out, 256), [Link]()
)
self.lidar_enc = [Link](
[Link](lidar_dim, 512), [Link](),
[Link](512, 256), [Link]()
)
self.pose_enc = [Link](
[Link](pose_dim, 128), [Link](),
[Link](128, 64), [Link]()
)

# Attention fusion (visual + lidar)

[Link] = [Link](
d_model=256, nhead=4,
dim_feedforward=512, dropout=0.1,
batch_first=True
)
self.post_attn = [Link](
[Link](512, 256), [Link]()
)

# GMU fusion
self.gmu_visual = [Link](256, 256)
self.gmu_pose = [Link](64, 256)
self.gmu_gate = [Link](256 + 64, 1)

# Output head
self.fc1 = [Link](256, 512)
self.fc2 = [Link](512, num_actions * num_quantiles)

self._init_weights()
 # Load pretrained encoder nếu có
enc_path = '/home/vuminh/ros2_ws/src/qr_dqn_nav/pretrained_encoder.pt'
try:
[Link].load_state_dict([Link](enc_path))
print('[Network] Loaded pretrained encoder!')
for i in range(3): # Freeze 3 conv layers
for p in [Link][i].parameters():
p.requires_grad = False
except FileNotFoundError:
print('[Network] No pretrained encoder, training from scratch.')

def _init_weights(self):
for m in [Link]():
if isinstance(m, nn.Conv2d):
[Link].kaiming_normal_([Link], nonlinearity='relu')
if [Link] is not None: [Link].zeros_([Link])
elif isinstance(m, [Link]):
if m.out_features == [Link] * [Link]:
[Link].uniform_([Link], -1e-3, 1e-3)
else:
[Link].kaiming_normal_([Link], nonlinearity='relu')
if [Link] is not None: [Link].zeros_([Link])

def forward(self, pose, img, lidar):

if [Link]() == 5:
img = [Link](2)

v = self.visual_proj([Link](img)) # [B, 256]

l = self.lidar_enc(lidar) # [B, 256]
p = self.pose_enc(pose) # [B, 64]

# Attention
seq = [Link]([v, l], dim=1) # [B, 2, 256]
attn = [Link](seq)
env = self.post_attn([Link]([Link](0), -1)) # [B, 256]

# GMU gate
h_v = [Link](self.gmu_visual(env))
h_p = [Link](self.gmu_pose(p))
z = [Link](self.gmu_gate([Link]([env, p], dim=1)))
fused = z * h_v + (1 - z) * h_p # [B, 256]

x = [Link](self.fc1(fused))
out = self.fc2(x).view(-1, [Link], [Link])
return out, [Link]()
EOF

Bước 13: Tạo src/[Link]

bash
cat > ~/ros2_ws/src/qr_dqn_nav/src/[Link] << 'EOF'
#!/usr/bin/env python3
import math, time, random, subprocess
import numpy as np
import cv2
from collections import deque
from cv_bridge import CvBridge
from geometry_msgs.msg import Twist
from visualization_msgs.msg import Marker
from geometry_msgs.msg import PoseStamped
from tf_transformations import euler_from_quaternion
from src.ros_module import RosHandler
from [Link] import *

class QR_DQN_ENV:
def __init__(self, action_size, handle: RosHandler):
[Link] = handle
self.action_size = action_size
[Link] = CvBridge()

# Config từ yaml
self.goal_x = goal_x
self.goal_y = goal_y
self.init_x = init_x
self.init_y = init_y

# Frame stack
self.frame_stack = deque(maxlen=4)

# Robot state
[Link] = type('P', (), {'x': 0.0, 'y': 0.0})()
self.current_psi = 0.0
self.current_v = 0.0
self.current_w = 0.0
[Link] = 0.6
self.prev_dist = 0.0
self.min_lidar = 10.0
self.goal_counter= 0
self.done_counter= 0
self.step_counter= 0

# 5 actions: (linear_v, angular_w)

self.actions_table = [
(0.30, 0.0 ), # 0: thẳng
(0.15, 0.5 ), # 1: trái nhẹ
(0.15, -0.5 ), # 2: phải nhẹ
(0.10, 1.0 ), # 3: trái mạnh
(0.10, -1.0 ), # 4: phải mạnh
]

self.D_MAX = 10.0
[Link] = 0.45 # khoảng cách đến goal để coi là thành công
self.MIN_RANGE = 0.25 # khoảng cách tối thiểu với vật cản

# =================== UTILITIES ===================

def normalize_angle(self, a):
while a > [Link]: a -= 2*[Link]
 while a < -[Link]: a += 2*[Link]
return a

def get_odom(self):
odom = [Link]
tf = [Link]
if tf is None or odom is None:
return False
[Link] = [Link]
q = [Link]
_, _, psi = euler_from_quaternion([q.x, q.y, q.z, q.w])
self.current_psi = psi

# Publish path cho RViz

ps = PoseStamped()
[Link] = [Link].get_clock().now().to_msg()
[Link].frame_id = 'odom'
[Link].x = [Link].x
[Link].y = [Link].y
[Link].robot_path.[Link](ps)
[Link].pub_path.publish([Link].robot_path)
return True

def calc_goal_info(self):
dx = self.goal_x - [Link].x
dy = self.goal_y - [Link].y
dist = [Link](dx**2 + dy**2)
angle_to_goal = math.atan2(dy, dx)
heading_error = self.normalize_angle(angle_to_goal - self.current_psi)
return dist, heading_error

def publish_goal_marker(self):
m = Marker()
[Link].frame_id = 'odom'
[Link] = [Link].get_clock().now().to_msg()
[Link] = [Link]
[Link] = [Link]
[Link].x = self.goal_x
[Link].y = self.goal_y
[Link].z = 0.01
[Link].x = [Link].y = 0.6
[Link].z = 0.02
[Link].a = 0.8
[Link].r = 1.0; [Link].g = 0.5
[Link].pub_goal.publish(m)

# =================== GET STATE ===================

def get_state(self, scan, image_msg):
done = False

# -- LiDAR --
ranges = [Link]([Link], dtype=np.float32)
ranges = [Link]([Link](ranges) | [Link](ranges), 10.0, ranges)
self.min_lidar = float([Link](ranges))
lidar_state = [Link](1.0 - ranges / 10.0, 0.0, 1.0)
if self.min_lidar < self.MIN_RANGE:
done = True

# -- Depth image --
img = [Link].imgmsg_to_cv2(image_msg, '32FC1')
img = [Link](img, (84, 84))
img = np.nan_to_num(img, nan=5.0, posinf=5.0, neginf=0.0)
img = [Link](img, 0.0, 5.0)
img = 1.0 - (img / 5.0) # invert: gần = sáng

if len(self.frame_stack) == 0:
for _ in range(4): self.frame_stack.append(img)
else:
self.frame_stack.append(img)

image_state = [Link](self.frame_stack, axis=0)[:, None, :, :] # [4,1,84,84]

# -- Pose features --
dist, heading_err = self.calc_goal_info()
norm_dist = [Link](dist / self.D_MAX, 0.0, 1.0)
norm_head = heading_err / [Link]

# -- Human features --
(hx, hy), h_dist = [Link].get_nearest_human(
[Link].x, [Link].y
)
dx = hx - [Link].x
dy = hy - [Link].y
# Chuyển sang frame robot
local_hx = ( dx*[Link](-self.current_psi)
- dy*[Link](-self.current_psi))
local_hy = ( dx*[Link](-self.current_psi)
+ dy*[Link](-self.current_psi))
norm_hx = [Link](local_hx / 5.0, -1.0, 1.0)
norm_hy = [Link](local_hy / 5.0, -1.0, 1.0)
norm_hdist= [Link](h_dist / 5.0, 0.0, 1.0)

# pose_state [9]
pose_state = [Link]([
norm_dist, # dist to goal
[Link](self.current_v / 0.3, 0.0, 1.0),# linear vel
[Link](self.current_w / 1.0,-1.0, 1.0),# angular vel
norm_head, # heading error
norm_hdist, # dist to human
norm_hx, # human local x
norm_hy, # human local y
[Link](self.min_lidar / 5.0, 0.0, 1.0),# min lidar norm
float(len([Link].human_poses)), # số người phát hiện
], dtype=np.float32)

if norm_dist < 0.04: # đến goal

done = True

return pose_state, image_state, lidar_state, done

# =================== REWARD ===================
def get_reward(self, pose_state, done, current_dist):
if done:
self.done_counter += 1
if pose_state[0] < 0.04: # goal
self.goal_counter += 1
[Link].get_logger().info('✅ Goal reached!')
return 10.0
else: # collision
[Link].get_logger().info('💥 Collision!')
return -10.0

# Shaping rewards
progress = self.prev_dist - current_dist
r_progress = 3.0 * progress

heading_err = pose_state[3]
r_heading = 0.4 * [Link](-2.0 * abs(heading_err))

# Repulsive từ LiDAR (vật cản tĩnh + tường)

safety = 0.4
if self.min_lidar < safety:
r_repulsive = -2.0 * [Link](
-(self.min_lidar**2) / (2 * 0.5 * safety**2)
)
else:
r_repulsive = 0.0

# Penalty từ người đi bộ (pose-based)

h_dist = pose_state[4] * 5.0 # denormalize
if h_dist < 0.5:
r_human = -2.0 * [Link](-h_dist / 0.5)
elif h_dist < 1.5:
r_human = -0.5 * (1.5 - h_dist) / 1.5
else:
r_human = 0.0

r_step = -0.05 # time penalty

reward = r_heading + r_progress + r_repulsive + r_human + r_step

self.prev_dist = current_dist
return reward

# =================== STEP ===================

def step(self, action):
self.step_counter += 1
target_v, target_w = self.actions_table[action]

# Smooth velocity
self.current_v = ([Link])*self.current_v + [Link]*target_v
self.current_w = ([Link])*self.current_w + [Link]*target_w

cmd = Twist()
[Link].x = self.current_v
 [Link].z = self.current_w

# Frame skip: gửi lệnh 4 lần (≈80ms)

for _ in range(4):
[Link].pub_cmd_vel.publish(cmd)
[Link](0.02)

# Đợi sensor data mới

[Link].clear_data()
t0 = [Link]()
while ([Link] is None or
[Link] is None or
[Link] is None):
if [Link]() - t0 > 5.0:
[Link].get_logger().warn('Sensor timeout!')
t0 = [Link]()
[Link](0.01)

self.get_odom()
pose_state, image_state, lidar_state, done = self.get_state(
[Link], [Link]
)
dist, _ = self.calc_goal_info()

# Stuck detection
if target_v > 0 and abs([Link].x) < 0.005:
self._stuck_count = getattr(self, '_stuck_count', 0) + 1
if self._stuck_count > 20:
done = True
self._stuck_count = 0
else:
self._stuck_count = 0

reward = self.get_reward(pose_state, done, dist)

return (pose_state, image_state, lidar_state), done, reward

# =================== RESET ===================

def reset(self):
# Random goal trong arena
while True:
gx = [Link](-2.5, 2.5)
gy = [Link](-2.5, 2.5)
sx = [Link](-2.5, 2.5)
sy = [Link](-2.5, 2.5)
if [Link]((gx-sx)**2 + (gy-sy)**2) > 2.0:
break

self.goal_x, self.goal_y = gx, gy

self.publish_goal_marker()

# Teleport robot
[Link]([
'ign', 'service',
'-s', '/world/training_world/set_pose',
'--reqtype', '[Link]',
 '--reptype', '[Link]',
'--timeout', '2000',
'--req',
f'name: "my_robot", '
f'position: {{x: {sx}, y: {sy}, z: 0.1}}, '
f'orientation: {{w: 1.0}}'
], capture_output=True)

# Reset state
self.frame_stack.clear()
self.current_v = 0.0
self.current_w = 0.0
self.step_counter = 0
self._stuck_count = 0
[Link].robot_path.[Link]()

[Link](1.5)
[Link].clear_data()

while ([Link] is None or

[Link] is None or
[Link] is None or
[Link] is None):
[Link](0.05)

self.get_odom()
dist, _ = self.calc_goal_info()
self.prev_dist = dist

pose_state, image_state, lidar_state, _ = self.get_state(

[Link], [Link]
)
return pose_state, image_state, lidar_state
EOF

Bước 14: Tạo src/[Link]

bash
cat > ~/ros2_ws/src/qr_dqn_nav/src/[Link] << 'EOF'
import math, random
import numpy as np
import torch
import [Link] as optim
from collections import deque
from [Link] import QR_DQN_Network, RandomShiftsAug
from src.ros_module import RosHandler

# ========== PRIORITIZED REPLAY BUFFER ==========

class SumTree:
def __init__(self, capacity):
[Link] = capacity
[Link] = [Link](2 * capacity - 1)
[Link] = [Link]([None] * capacity, dtype=object)
 [Link] = 0
self.n_entries = 0

def add(self, p, data):

idx = [Link] + [Link] - 1
[Link][[Link]] = data
[Link](idx, p)
[Link] = ([Link] + 1) % [Link]
if self.n_entries < [Link]:
self.n_entries += 1

def update(self, idx, p):

delta = p - [Link][idx]
[Link][idx] = p
while idx != 0:
idx = (idx - 1) // 2
[Link][idx] += delta

def _retrieve(self, idx, s):

left = 2 * idx + 1
if left >= len([Link]): return idx
return (self._retrieve(left, s) if s <= [Link][left]
else self._retrieve(left+1, s - [Link][left]))

def get(self, s):

idx = self._retrieve(0, s)
data_idx= idx - [Link] + 1
return idx, [Link][idx], [Link][data_idx]

def total(self): return [Link][0]

class ReplayBuffer:
def __init__(self, size):
[Link] = SumTree(size)
self.e = 0.01
self.a = 0.4
[Link] = 0.4
self.beta_inc= 5e-5
self.max_p = 1.0

def add(self, s, a, r, ns, done):

def _np(x):
if isinstance(x, [Link]): return [Link]().numpy()
return [Link](x)

ps, pi, pd = s
ns0, ni, nd= ns
t = (_np(ps), _np(pi), _np(pd),
int(a), float(r),
_np(ns0), _np(ni), _np(nd),
float(done))
[Link](self.max_p, t)

def sample(self, B):

 [Link] = min(1.0, [Link] + self.beta_inc)
seg = [Link]() / B
idxs, prios, batch = [], [], []
for i in range(B):
s = [Link](seg*i, seg*(i+1))
idx, p, data = [Link](s)
if data is None:
idx, p, data = [Link]([Link]()-1e-6)
[Link](idx); [Link](p); [Link](data)

prios = [Link](prios, dtype=np.float32)

probs = prios / ([Link]() + 1e-6)
weights = [Link]([Link].n_entries * probs + 1e-10, -[Link])
weights /= [Link]() + 1e-10

def stack(fn): return [Link]([fn(t) for t in batch], dtype=np.float32)

return (stack(lambda t: t[0]), # pose

stack(lambda t: t[1]), # img
stack(lambda t: t[2]), # lidar
[Link]([t[3] for t in batch], dtype=np.int64),
[Link]([t[4] for t in batch], dtype=np.float32),
stack(lambda t: t[5]), # next pose
stack(lambda t: t[6]), # next img
stack(lambda t: t[7]), # next lidar
[Link]([t[8] for t in batch], dtype=np.float32),
idxs, weights)

def update_priorities(self, idxs, prios):

for idx, p in zip(idxs, prios):
[Link](idx, p)
self.max_p = max(self.max_p, p)

def __len__(self): return [Link].n_entries

# ========== QR-DQN AGENT ==========

class QR_DQN_Agent:
def __init__(self, action_size, handle: RosHandler, pose_dim=9):
[Link] = [Link]('cuda' if [Link].is_available() else 'cpu')
self.action_size = action_size
[Link] = handle

# Hyperparameters
self.num_quantiles = 32
[Link] = 0.99
[Link] = 1e-4
self.batch_size = 64
self.train_start = 1000
self.memory_size = 50000
[Link] = 0.001 # soft update rate
[Link] = 1.0
self.epsilon_min = 0.05
self.epsilon_decay = 3000 # episodes
 # Quantile midpoints τ
[Link] = (
([Link](self.num_quantiles, device=[Link]) + 0.5)
/ self.num_quantiles
).view(1, self.num_quantiles, 1)

# Models
self.pred_net = QR_DQN_Network(
num_actions=action_size,
num_quantiles=self.num_quantiles,
pose_dim=pose_dim
).to([Link])
self.target_net = QR_DQN_Network(
num_actions=action_size,
num_quantiles=self.num_quantiles,
pose_dim=pose_dim
).to([Link])
self.target_net.load_state_dict(self.pred_net.state_dict())
self.target_net.eval()

[Link] = [Link](self.pred_net.parameters(), lr=[Link])

[Link] = RandomShiftsAug(pad=4).to([Link])
[Link] = ReplayBuffer(self.memory_size)

# Logging
self.last_q_mean = 0.0
self.last_q_max = 0.0
self.last_q_std = 0.0

def update_epsilon(self, episode):

if episode >= self.epsilon_decay:
[Link] = self.epsilon_min
else:
[Link] = 1.0 - (1.0 - self.epsilon_min) * episode / self.epsilon_decay

def get_action(self, state, training=True):

pose, img, lidar = state
self.pred_net.eval()
with torch.no_grad():
p = [Link](pose, dtype=torch.float32).unsqueeze(0).to([Link])
i = [Link](img, dtype=torch.float32).unsqueeze(0).to([Link])
l = [Link](lidar, dtype=torch.float32).unsqueeze(0).to([Link])
dist, _ = self.pred_net(p, i, l)
q = [Link](dim=2) # [1, actions]
self.last_q_mean = [Link]().item()
self.last_q_max = [Link]().item()
self.last_q_std = [Link]().item()
self.pred_net.train()

if training and [Link]() < [Link]:

return [Link](self.action_size)
return int([Link]().item())

def train_step(self):
if len([Link]) < self.train_start:
 return None

(poses, imgs, lidars, actions, rewards,

nposes, nimgs, nlidars, dones, idxs, weights) = [Link](self.batch_size)

# Tensor conversion
def T(x, dtype=torch.float32): return [Link](x, dtype=dtype).to([Link])

poses = T(poses); imgs = T(imgs); lidars = T(lidars)

nposes = T(nposes); nimgs = T(nimgs); nlidars = T(nlidars)
actions = T(actions, torch.int64).unsqueeze(-1)
rewards = T(rewards).unsqueeze(-1)
dones = T(dones).unsqueeze(-1)
weights = T(weights)

B = [Link][0]

# Augmentation
B, T_f, C, H, W = [Link]
imgs = [Link]([Link](B, T_f*C, H, W)).view(B, T_f, C, H, W)
nimgs = [Link]([Link](B, T_f*C, H, W)).view(B, T_f, C, H, W)

# Double DQN target

with torch.no_grad():
next_dist, _ = self.pred_net(nposes, nimgs, nlidars)
next_action = next_dist.mean(dim=2).argmax(dim=1)
next_target, _= self.target_net(nposes, nimgs, nlidars)
bi = [Link](B).to([Link])
next_q = next_target[bi, next_action] # [B, Q]
target = rewards + [Link] * next_q * (1 - dones)
target = [Link](1) # [B, 1, Q]

# Current quantiles
curr_dist, gate = self.pred_net(poses, imgs, lidars)
act_expand = [Link](-1).expand(-1, 1, self.num_quantiles)
pred = curr_dist.gather(1, act_expand) # [B, 1, Q]

# Quantile Huber Loss

u = target - pred
abs_u = [Link]()
huber = [Link](abs_u <= 1.0, 0.5*u**2, abs_u - 0.5)
q_weight= ([Link] - ([Link]() < 0).float()).abs()
loss_per= (q_weight * huber).sum(dim=2).mean(dim=1) # [B]
loss = (loss_per * weights).mean()

# TD error → priority update

td_err = ([Link](dim=2) - [Link](dim=1)).abs().view(-1)
prios = (td_err.detach().cpu().numpy() + [Link].e) ** [Link].a
[Link].update_priorities(idxs, prios)

[Link].zero_grad()
[Link]()
[Link].clip_grad_norm_(self.pred_net.parameters(), 1.0)
[Link]()
 # Soft update target
with torch.no_grad():
for tp, pp in zip(self.target_net.parameters(),
self.pred_net.parameters()):
tp.mul_(1 - [Link]).add_([Link] * pp)

return [Link](), [Link]()

EOF

Bước 15: Tạo [Link]

bash
cat > ~/ros2_ws/src/qr_dqn_nav/[Link] << 'EOF'
#!/usr/bin/env python3
import os, time, threading
import torch
import rclpy
from [Link] import MultiThreadedExecutor
from [Link] import SummaryWriter
from collections import deque
from geometry_msgs.msg import Twist
from src.ros_module import RosHandler
from [Link] import QR_DQN_ENV
from [Link] import QR_DQN_Agent

SAVE_DIR = [Link]([Link](__file__))
RUN_NAME = 'human_avoidance_v1'
EPISODES = 5000

def training_loop(ros: RosHandler):

writer = SummaryWriter(f'{SAVE_DIR}/runs/{RUN_NAME}')
env = QR_DQN_ENV(action_size=5, handle=ros)
agent = QR_DQN_Agent(action_size=5, handle=ros, pose_dim=9)

best_avg = -999.0
best_rate = 0.0
avg_buf = deque(maxlen=50)
suc_buf = deque(maxlen=100)

for ep in range(1, EPISODES+1):

state = [Link]()
agent.update_epsilon(ep)

ep_reward = 0.0
step = 0
done = False
success = False

while not done and step < 1000:

action = agent.get_action(state, training=True)
ns, done, reward = [Link](action)
[Link](state, action, reward, ns, done)
 out = agent.train_step()
ep_reward += reward
state = ns
step += 1

if reward >= 9.0: success = True

suc_buf.append(1.0 if success else 0.0)

avg_buf.append(ep_reward)
suc_rate = sum(suc_buf) / len(suc_buf)
avg_rew = sum(avg_buf) / len(avg_buf)

# TensorBoard
writer.add_scalar('Train/Reward', ep_reward, ep)
writer.add_scalar('Train/Epsilon', [Link], ep)
writer.add_scalar('Train/Steps', step, ep)
writer.add_scalar('Train/SuccessRate', suc_rate, ep)
writer.add_scalar('Train/AvgReward50', avg_rew, ep)
writer.add_scalar('Q/Mean', agent.last_q_mean, ep)

print(f'[EP {ep:4d}] R={ep_reward:7.2f} | '

f'Suc={suc_rate*100:.1f}% | '
f'Eps={[Link]:.3f} | '
f'Buf={len([Link])}')

# Save best models

if ep > 100 and avg_rew > best_avg:
best_avg = avg_rew
[Link](agent.pred_net.state_dict(),
f'{SAVE_DIR}/{RUN_NAME}_best_avg.pt')
print(f' ✅ Best avg saved: {best_avg:.2f}')

if suc_rate > best_rate and suc_rate > 0:

best_rate = suc_rate
[Link](agent.pred_net.state_dict(),
f'{SAVE_DIR}/{RUN_NAME}_best_rate.pt')
print(f' ✅ Best rate saved: {best_rate*100:.1f}%')

[Link](agent.pred_net.state_dict(),
f'{SAVE_DIR}/{RUN_NAME}_final.pt')
[Link]()
print('Training complete!')

if __name__ == '__main__':
[Link]()
ros = RosHandler()
exe = MultiThreadedExecutor()
exe.add_node(ros)

t = [Link](target=training_loop, args=(ros,))
[Link]()

try:
[Link]()
 except KeyboardInterrupt:
pass
finally:
ros.destroy_node()
[Link]()
EOF

PHASE 6: BUILD VÀ CHẠY

Bước 16: Build package
bash
cd ~/ros2_ws
colcon build --packages-select qr_dqn_nav
source install/[Link]

Bước 17: Set environment variables

bash
# Thêm vào ~/.bashrc
cat >> ~/.bashrc << 'EOF'
export IGN_GAZEBO_RESOURCE_PATH=$HOME/ros2_ws/src/qr_dqn_nav/models:
$IGN_GAZEBO_RESOURCE_PATH
export IGN_GAZEBO_MODEL_PATH=$HOME/ros2_ws/src/qr_dqn_nav/models:
$IGN_GAZEBO_MODEL_PATH
EOF
source ~/.bashrc

Bước 18: Chạy hệ thống (3 terminal)

bash
# ===== Terminal 1: Gazebo =====
ign gazebo ~/ros2_ws/src/qr_dqn_nav/worlds/training_world.sdf -r

# ===== Terminal 2: ROS-GZ Bridge =====

source /opt/ros/humble/[Link]
source ~/ros2_ws/install/[Link]
ros2 run ros_gz_bridge parameter_bridge \
--ros-args -p \
config_file:=$HOME/ros2_ws/src/qr_dqn_nav/config/[Link]

# ===== Terminal 3: Training =====

source /opt/ros/humble/[Link]
source ~/ros2_ws/install/[Link]
cd ~/ros2_ws/src/qr_dqn_nav
python3 [Link]

# ===== Terminal 4 (optional): TensorBoard =====

tensorboard --logdir ~/ros2_ws/src/qr_dqn_nav/runs

Bước 19: Kiểm tra sensor data

bash
# Kiểm tra các topic đang publish
ros2 topic list
ros2 topic hz /lidar
ros2 topic hz /depth/image_raw
ros2 topic hz /odom

# Xem raw data

ros2 topic echo /odom --once

PHASE 7: DEBUG THƯỜNG GẶP

bash
# Lỗi 1: Không nhận được /depth/image_raw
# → Kiểm tra bridge config, topic name trong SDF

# Lỗi 2: Robot không di chuyển

ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \
"{linear: {x: 0.2}, angular: {z: 0.0}}" --once

# Lỗi 3: Actor người không hiện

ign fuel download -u [Link]

# Lỗi 4: CUDA out of memory

# → Giảm batch_size xuống 32 trong [Link]

# Lỗi 5: Xem log TensorBoard

tensorboard --logdir runs/ --port 6006
# Mở browser: [Link]