学習時間を短くする処理の追加

launch/nav_cloning_sim.launch

@@ -38,7 +38,7 @@
         name="timed_roslaunch" output="screen" />
 
   <node pkg="timed_roslaunch" type="timed_roslaunch.sh"
-        args="8 nav_cloning turtlebot3_navigation.launch model:=waffle map_file:=$(find nav_cloning)/maps/$(arg map_file) waypoints_file:=$(find nav_cloning)/maps/$(arg waypoints_file) dist_err:=$(arg dist_err) initial_pose_x:=$(arg initial_pose_x) initial_pose_y:=$(arg initial_pose_y) initial_pose_a:=$(arg initial_pose_a) use_waypoint_nav:=$(arg use_waypoint_nav)"
+        args="8 nav_cloning turtlebot3_navigation.launch model:=waffle_pi map_file:=$(find nav_cloning)/maps/$(arg map_file) waypoints_file:=$(find nav_cloning)/maps/$(arg waypoints_file) dist_err:=$(arg dist_err) initial_pose_x:=$(arg initial_pose_x) initial_pose_y:=$(arg initial_pose_y) initial_pose_a:=$(arg initial_pose_a) use_waypoint_nav:=$(arg use_waypoint_nav)"
         name="timed_roslaunch2" >
   </node>
 

scripts/nav_cloning_with_direction_net.py

@@ -14,7 +14,7 @@
 
 # HYPER PARAM
 BATCH_SIZE = 8
-MAX_DATA = 10000
+MAX_DATA = 100000
 
 class Net(chainer.Chain):
     def __init__(self, n_channel=3, n_action=1):
@@ -58,14 +58,15 @@ def __init__(self, n_channel=3, n_action=1):
         self.cmd =[]
         self.target_angles = []
 
-    def act_and_trains(self, imgobj, cmd_dir, target_angle):
+    def act_and_trains(self, imgobj, cmd_dir, target_angle, times):
             x = [self.phi(s) for s in [imgobj]]
             c = np.array([cmd_dir], np.float32)
             t = np.array([target_angle], np.float32)
-            self.data.append(x[0])
-            self.cmd.append(c[0])
-            self.target_angles.append(t[0])
-            if len(self.data) > MAX_DATA:
+            for i in range(times):
+                self.data.append(x[0])
+                self.cmd.append(c[0])
+                self.target_angles.append(t[0])
+            while len(self.data) > MAX_DATA:
                 del self.data[0]
                 del self.cmd[0]
                 del self.target_angles[0]
@@ -92,6 +93,27 @@ def act_and_trains(self, imgobj, cmd_dir, target_angle):
                 action_value = self.net(x_test, c_test)
             return action_value.data[0][0], loss_train.array
 
+    def trains(self, iteration):
+            dataset = TupleDataset(self.data, self.cmd, self.target_angles)
+            train_iter = SerialIterator(dataset, batch_size = BATCH_SIZE, repeat=True, shuffle=True)
+            for i in range(iteration):
+                train_batch  = train_iter.next()
+                x_train, c_train, t_train = chainer.dataset.concat_examples(train_batch, -1)
+
+                y_train = self.net(x_train, c_train)
+                loss_train = F.mean_squared_error(y_train, Variable(t_train.reshape(BATCH_SIZE, 1)))
+
+                self.loss_list.append(loss_train.array)
+
+                self.net.cleargrads()
+                loss_train.backward()
+                self.optimizer.update()
+
+                self.count += 1
+
+                self.results_train['loss'] .append(loss_train.array)
+                print("trains: "+str(self.count));
+
     def act(self, imgobj, cmd_dir):
             x = [self.phi(s) for s in [imgobj]]
             c = np.array([cmd_dir],np.float32)

scripts/nav_cloning_with_direction_node.py

@@ -24,6 +24,7 @@
 import sys
 import tf
 from nav_msgs.msg import Odometry
+import tf2_ros
 
 class nav_cloning_node:
     def __init__(self):
@@ -39,7 +40,7 @@ def __init__(self):
         self.action_pub = rospy.Publisher("action", Int8, queue_size=1)
         self.nav_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
         self.srv = rospy.Service('/training', SetBool, self.callback_dl_training)
-        self.pose_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.callback_pose)
+#        self.pose_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.callback_pose)
         self.path_sub = rospy.Subscriber("/move_base/NavfnROS/plan", Path, self.callback_path)
         self.cmd_dir_sub = rospy.Subscriber("/cmd_dir", Int8MultiArray, self.callback_cmd,queue_size=1)
         self.min_distance = 0.0
@@ -69,6 +70,9 @@ def __init__(self):
             writer.writerow(['step', 'mode', 'loss', 'angle_error(rad)', 'distance(m)','x(m)','y(m)', 'the(rad)', 'direction'])
         self.tracker_sub = rospy.Subscriber("/tracker", Odometry, self.callback_tracker)
 
+        self.tfBuffer = tf2_ros.Buffer()
+        self.listener = tf2_ros.TransformListener(self.tfBuffer)
+
     def callback(self, data):
         try:
             self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
@@ -108,6 +112,23 @@ def callback_pose(self, data):
         if distance_list:
             self.min_distance = min(distance_list)
 
+    def calc_distance(self):
+        distance_list = []
+
+        try:
+            trans = self.tfBuffer.lookup_transform('map', 'base_link', rospy.Time())
+        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
+            self.min_distance = 0.0
+            return
+
+        pos = trans.transform.translation
+        for pose in self.path_pose.poses:
+            path = pose.pose.position
+            distance = np.sqrt(abs((pos.x - path.x)**2 + (pos.y - path.y)**2))
+            distance_list.append(distance)
+        if distance_list:
+            self.min_distance = min(distance_list)
+
     def callback_cmd(self, data):
         self.cmd_dir_data = data.data
 
@@ -147,17 +168,28 @@ def loop(self):
         cmd_dir = np.asanyarray(self.cmd_dir_data)
         ros_time = str(rospy.Time.now())
 
-        if self.episode == 4000:
+        if self.episode == 20000:
             self.learning = False
             self.dl.save(self.save_path)
-            #self.dl.load(self.load_path)
+#            self.vel.linear.x = 0.0
+#            self.vel.angular.z = 0.0
+#            self.nav_pub.publish(self.vel)
+#            self.dl.load("/home/haya/catkin_ws/src/nav_cloning/data/model_with_dir_selected_training/20220607_09:33:20/model.net")
+#            self.dl.load(self.load_path)
+#            self.episode += 1
+#            return
+#        
+#        if self.episode == 20000+1:
+#            self.dl.trains(10000)
+#            self.dl.save(self.save_path)
 
-        if self.episode == 12000:
+        if self.episode == 30000:
             os.system('killall roslaunch')
             sys.exit()
 
         if self.learning:
             target_action = self.action
+            self.calc_distance()
             distance = self.min_distance
 
             if self.mode == "manual":
@@ -210,14 +242,18 @@ def loop(self):
                 action = self.dl.act(imgobj, cmd_dir)
                 angle_error = abs(action - target_action)
                 loss = 0
+                times = 1 if (cmd_dir == np.array([100, 0, 0, 0])).all() else 5
                 if angle_error > 0.05:
-                    action, loss = self.dl.act_and_trains(imgobj, cmd_dir, target_action)
+                    action, loss = self.dl.act_and_trains(imgobj, cmd_dir, target_action, times)
+                    action = max(min(action, 0.8), -0.8)
                     if abs(target_action) < 0.1:
-                        action_left,  loss_left  = self.dl.act_and_trains(imgobj_left, cmd_dir, target_action - 0.2)
-                        action_right, loss_right = self.dl.act_and_trains(imgobj_right, cmd_dir, target_action + 0.2)
-                if distance > 0.1:
+                        action_left,  loss_left  = self.dl.act_and_trains(imgobj_left, cmd_dir, target_action - 0.2, 1)
+                        action_right, loss_right = self.dl.act_and_trains(imgobj_right, cmd_dir, target_action + 0.2, 1)
+                else:
+                    self.dl.trains(2)
+                if distance > 0.2 or angle_error > 0.4:
                     self.select_dl = False
-                elif distance < 0.05:
+                elif distance < 0.1:
                     self.select_dl = True
                 if self.select_dl and self.episode >= 0:
                     target_action = action
@@ -259,7 +295,7 @@ def loop(self):
 
 if __name__ == '__main__':
     rg = nav_cloning_node()
-    DURATION = 0.25
+    DURATION = 0.2
     r = rospy.Rate(1 / DURATION)
     while not rospy.is_shutdown():
         rg.loop()