Merge pull request #75 from ROBOTIS-GIT/feature-jazzy-update

Update machine learning package for Jazzy

Author: GyuH13

.github/workflows/ros-ci.yml

@@ -45,6 +45,10 @@ jobs:
         with:
           required-ros-distributions: ${{ matrix.ros_distribution }}
 
+      - name: Add pip break-system-packages for rosdep
+        run: |
+          printf "[install]\nbreak-system-packages = true\n" | sudo tee /etc/pip.conf
+
       - name: Build and Test
         uses: ros-tooling/[email protected]
         env:

turtlebot3_dqn/CHANGELOG.rst

@@ -2,9 +2,15 @@
 Changelog for package turtlebot3_dqn
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.1 (2025-05-02)
+------------------
+* Support for ROS 2 Jazzy version
+* Gazebo simulation support for the package
+* Contributors: ChanHyeong Lee
+
 1.0.0 (2025-04-17)
 ------------------
 * Support for ROS 2 Humble version
 * Renewal of package structure
 * Improved behavioral rewards for agents
-Contributors: ChanHyeong Lee
+* Contributors: ChanHyeong Lee

turtlebot3_dqn/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>turtlebot3_dqn</name>
-  <version>1.0.0</version>
+  <version>1.0.1</version>
   <description>
     The turtlebot3_dqn package using reinforcement learning with DQN (Deep Q-Learning).
   </description>
@@ -12,13 +12,18 @@
   <url type="bugtracker">https://github.com/ROBOTIS-GIT/turtlebot3_machine_learning/issues</url>
   <author email="[email protected]">Gilbert</author>
   <author email="[email protected]">ChanHyeong Lee</author>
-  <depend>geometry_msgs</depend>
-  <depend>rclpy</depend>
-  <depend>sensor_msgs</depend>
-  <depend>std_srvs</depend>
-  <depend>turtlebot3_msgs</depend>
-  <depend>python-keras-pip</depend>
-  <depend>python-tensorflow-pip</depend>
+  <exec_depend>python3-pip</exec_depend>
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>python-tensorflow-pip</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-pyqt5</exec_depend>
+  <exec_depend>python3-pyqtgraph</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>turtlebot3_msgs</exec_depend>
   <export>
     <build_type>ament_python</build_type>
   </export>

turtlebot3_dqn/setup.py

@@ -14,14 +14,14 @@
 
 setup(
     name=package_name,
-    version='1.0.0',
+    version='1.0.1',
     packages=find_packages(),
     data_files=[
         ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
         ('share/' + package_name + '/launch', glob.glob('launch/*.py')),
     ],
-    install_requires=['setuptools', 'launch',],
+    install_requires=['setuptools', 'launch'],
     zip_safe=True,
     author=authors,
     author_email=author_emails,

turtlebot3_dqn/turtlebot3_dqn/dqn_agent.py

@@ -26,16 +26,18 @@
 import sys
 import time
 
-from keras.api.layers import Dense
-from keras.api.models import load_model
-from keras.api.models import Sequential
-from keras.api.optimizers import Adam
 import numpy
 import rclpy
 from rclpy.node import Node
 from std_msgs.msg import Float32MultiArray
 from std_srvs.srv import Empty
 import tensorflow
+from tensorflow.keras.layers import Dense
+from tensorflow.keras.layers import Input
+from tensorflow.keras.losses import MeanSquaredError
+from tensorflow.keras.models import load_model
+from tensorflow.keras.models import Sequential
+from tensorflow.keras.optimizers import Adam
 
 from turtlebot3_msgs.srv import Dqn
 
@@ -84,7 +86,7 @@ def __init__(self, stage_num, max_training_episodes):
         self.learning_rate = 0.0007
         self.epsilon = 1.0
         self.step_counter = 0
-        self.epsilon_decay = 20000
+        self.epsilon_decay = 6000 * self.stage
         self.epsilon_min = 0.05
         self.batch_size = 128
 
@@ -120,7 +122,10 @@ def __init__(self, stage_num, max_training_episodes):
 
         if LOGGING:
             tensorboard_file_name = current_time + ' dqn_stage' + str(self.stage) + '_reward'
-            dqn_reward_log_dir = 'logs/gradient_tape/' + tensorboard_file_name
+            home_dir = os.path.expanduser('~')
+            dqn_reward_log_dir = os.path.join(
+                home_dir, 'turtlebot3_dqn_logs', 'gradient_tape', tensorboard_file_name
+            )
             self.dqn_reward_writer = tensorflow.summary.create_file_writer(dqn_reward_log_dir)
             self.dqn_reward_metric = DQNMetric()
 
@@ -276,11 +281,12 @@ def step(self, action):
 
     def create_qnetwork(self):
         model = Sequential()
-        model.add(Dense(512, input_shape=(self.state_size,), activation='relu'))
+        model.add(Input(shape=(self.state_size,)))
+        model.add(Dense(512, activation='relu'))
         model.add(Dense(256, activation='relu'))
         model.add(Dense(128, activation='relu'))
         model.add(Dense(self.action_size, activation='linear'))
-        model.compile(loss='mse', optimizer=Adam(learning_rate=self.learning_rate))
+        model.compile(loss=MeanSquaredError(), optimizer=Adam(learning_rate=self.learning_rate))
         model.summary()
 
         return model

turtlebot3_dqn/turtlebot3_dqn/dqn_environment.py

@@ -18,8 +18,10 @@
 # Authors: Ryan Shim, Gilbert, ChanHyeong Lee
 
 import math
+import os
 
 from geometry_msgs.msg import Twist
+from geometry_msgs.msg import TwistStamped
 from nav_msgs.msg import Odometry
 import numpy
 import rclpy
@@ -34,11 +36,13 @@
 from turtlebot3_msgs.srv import Goal
 
 
+ROS_DISTRO = os.environ.get('ROS_DISTRO')
+
+
 class RLEnvironment(Node):
 
     def __init__(self):
         super().__init__('rl_environment')
-        self.train_mode = True
         self.goal_pose_x = 0.0
         self.goal_pose_y = 0.0
         self.robot_pose_x = 0.0
@@ -55,15 +59,20 @@ def __init__(self):
         self.goal_distance = 1.0
         self.init_goal_distance = 0.5
         self.scan_ranges = []
+        self.front_ranges = []
         self.min_obstacle_distance = 10.0
+        self.is_front_min_actual_front = False
 
         self.local_step = 0
         self.stop_cmd_vel_timer = None
         self.angular_vel = [1.5, 0.75, 0.0, -0.75, -1.5]
 
         qos = QoSProfile(depth=10)
 
-        self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', qos)
+        if ROS_DISTRO == 'humble':
+            self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', qos)
+        else:
+            self.cmd_vel_pub = self.create_publisher(TwistStamped, 'cmd_vel', qos)
 
         self.odom_sub = self.create_subscription(
             Odometry,
@@ -112,6 +121,7 @@ def __init__(self):
         )
 
     def make_environment_callback(self, request, response):
+        self.get_logger().info('Make environment called')
         while not self.initialize_environment_client.wait_for_service(timeout_sec=1.0):
             self.get_logger().warn(
                 'service for initialize the environment is not available, waiting ...'
@@ -166,17 +176,32 @@ def call_task_failed(self):
 
     def scan_sub_callback(self, scan):
         self.scan_ranges = []
+        self.front_ranges = []
+        self.front_angles = []
+
         num_of_lidar_rays = len(scan.ranges)
+        angle_min = scan.angle_min
+        angle_increment = scan.angle_increment
+
+        self.front_distance = scan.ranges[0]
 
         for i in range(num_of_lidar_rays):
-            if scan.ranges[i] == float('Inf'):
-                self.scan_ranges.append(3.5)
-            elif numpy.isnan(scan.ranges[i]):
-                self.scan_ranges.append(0)
-            else:
-                self.scan_ranges.append(scan.ranges[i])
+            angle = angle_min + i * angle_increment
+            distance = scan.ranges[i]
+
+            if distance == float('Inf'):
+                distance = 3.5
+            elif numpy.isnan(distance):
+                distance = 0.0
+
+            self.scan_ranges.append(distance)
+
+            if (0 <= angle <= math.pi/2) or (3*math.pi/2 <= angle <= 2*math.pi):
+                self.front_ranges.append(distance)
+                self.front_angles.append(angle)
 
         self.min_obstacle_distance = min(self.scan_ranges)
+        self.front_min_obstacle_distance = min(self.front_ranges) if self.front_ranges else 10.0
 
     def odom_sub_callback(self, msg):
         self.robot_pose_x = msg.pose.pose.position.x
@@ -204,81 +229,112 @@ def calculate_state(self):
         state = []
         state.append(float(self.goal_distance))
         state.append(float(self.goal_angle))
-
-        for var in self.scan_ranges:
+        for var in self.front_ranges:
             state.append(float(var))
         self.local_step += 1
 
         if self.goal_distance < 0.20:
             self.get_logger().info('Goal Reached')
             self.succeed = True
             self.done = True
-            self.cmd_vel_pub.publish(Twist())
+            if ROS_DISTRO == 'humble':
+                self.cmd_vel_pub.publish(Twist())
+            else:
+                self.cmd_vel_pub.publish(TwistStamped())
             self.local_step = 0
             self.call_task_succeed()
 
         if self.min_obstacle_distance < 0.15:
             self.get_logger().info('Collision happened')
             self.fail = True
             self.done = True
-            self.cmd_vel_pub.publish(Twist())
+            if ROS_DISTRO == 'humble':
+                self.cmd_vel_pub.publish(Twist())
+            else:
+                self.cmd_vel_pub.publish(TwistStamped())
             self.local_step = 0
             self.call_task_failed()
 
         if self.local_step == self.max_step:
             self.get_logger().info('Time out!')
             self.fail = True
             self.done = True
-            self.cmd_vel_pub.publish(Twist())
+            if ROS_DISTRO == 'humble':
+                self.cmd_vel_pub.publish(Twist())
+            else:
+                self.cmd_vel_pub.publish(TwistStamped())
             self.local_step = 0
             self.call_task_failed()
 
         return state
 
-    def calculate_reward(self):
-        if self.train_mode:
+    def compute_directional_weights(self, relative_angles, max_weight=10.0):
+        power = 6
+        raw_weights = (numpy.cos(relative_angles))**power + 0.1
+        scaled_weights = raw_weights * (max_weight / numpy.max(raw_weights))
+        normalized_weights = scaled_weights / numpy.sum(scaled_weights)
+        return normalized_weights
 
-            if not hasattr(self, 'prev_goal_distance'):
-                self.prev_goal_distance = self.init_goal_distance
+    def compute_weighted_obstacle_reward(self):
+        if not self.front_ranges or not self.front_angles:
+            return 0.0
 
-            distance_reward = self.prev_goal_distance - self.goal_distance
-            self.prev_goal_distance = self.goal_distance
+        front_ranges = numpy.array(self.front_ranges)
+        front_angles = numpy.array(self.front_angles)
 
-            yaw_reward = (1 - 2 * math.sqrt(math.fabs(self.goal_angle / math.pi)))
+        valid_mask = front_ranges <= 0.5
+        if not numpy.any(valid_mask):
+            return 0.0
 
-            obstacle_reward = 0.0
-            if self.min_obstacle_distance < 0.50:
-                obstacle_reward = -1.0
+        front_ranges = front_ranges[valid_mask]
+        front_angles = front_angles[valid_mask]
 
-            reward = (distance_reward * 10) + (yaw_reward / 5) + obstacle_reward
+        relative_angles = numpy.unwrap(front_angles)
+        relative_angles[relative_angles > numpy.pi] -= 2 * numpy.pi
 
-            if self.succeed:
-                reward = 30.0
-            elif self.fail:
-                reward = -10.0
+        weights = self.compute_directional_weights(relative_angles, max_weight=10.0)
 
-        else:
-            if self.succeed:
-                reward = 5.0
-            elif self.fail:
-                reward = -5.0
-            else:
-                reward = 0.0
+        safe_dists = numpy.clip(front_ranges - 0.25, 1e-2, 3.5)
+        decay = numpy.exp(-3.0 * safe_dists)
+
+        weighted_decay = numpy.dot(weights, decay)
+
+        reward = - (1.0 + 4.0 * weighted_decay)
+
+        return reward
+
+    def calculate_reward(self):
+        yaw_reward = 1 - (2 * abs(self.goal_angle) / math.pi)
+        obstacle_reward = self.compute_weighted_obstacle_reward()
+
+        print('directional_reward: %f, obstacle_reward: %f' % (yaw_reward, obstacle_reward))
+        reward = yaw_reward + obstacle_reward
+
+        if self.succeed:
+            reward = 100.0
+        elif self.fail:
+            reward = -50.0
 
         return reward
 
     def rl_agent_interface_callback(self, request, response):
         action = request.action
-        twist = Twist()
-        twist.linear.x = 0.15
-        twist.angular.z = self.angular_vel[action]
-        self.cmd_vel_pub.publish(twist)
+        if ROS_DISTRO == 'humble':
+            msg = Twist()
+            msg.linear.x = 0.2
+            msg.angular.z = self.angular_vel[action]
+        else:
+            msg = TwistStamped()
+            msg.twist.linear.x = 0.2
+            msg.twist.angular.z = self.angular_vel[action]
+
+        self.cmd_vel_pub.publish(msg)
         if self.stop_cmd_vel_timer is None:
             self.prev_goal_distance = self.init_goal_distance
-            self.stop_cmd_vel_timer = self.create_timer(1.8, self.timer_callback)
+            self.stop_cmd_vel_timer = self.create_timer(0.8, self.timer_callback)
         else:
             self.destroy_timer(self.stop_cmd_vel_timer)
-            self.stop_cmd_vel_timer = self.create_timer(1.8, self.timer_callback)
+            self.stop_cmd_vel_timer = self.create_timer(0.8, self.timer_callback)
 
         response.state = self.calculate_state()
         response.reward = self.calculate_reward()
@@ -293,7 +349,10 @@ def rl_agent_interface_callback(self, request, response):
 
     def timer_callback(self):
         self.get_logger().info('Stop called')
-        self.cmd_vel_pub.publish(Twist())
+        if ROS_DISTRO == 'humble':
+            self.cmd_vel_pub.publish(Twist())
+        else:
+            self.cmd_vel_pub.publish(TwistStamped())
         self.destroy_timer(self.stop_cmd_vel_timer)
 
     def euler_from_quaternion(self, quat):