Skip to content

Analysis to move the robot refactoring #15

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
yasuohayashibara wants to merge 1 commit into
base: analysis_to_move_the_robot
Choose a base branch
from
Open

Analysis to move the robot refactoring #15

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 0 additions & 5 deletions experiments/experiment_rate_change
View file
Open in desktop

This file was deleted.

5 changes: 5 additions & 0 deletions experiments/experiment_variable_sampling_time
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
for i in `seq 10`
do
roslaunch nav_cloning nav_cloning_sim.launch mode:=variable_sampling_time
sleep 20
done
18 changes: 9 additions & 9 deletions scripts/change_dataset_balance_chainer.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -232,13 +232,13 @@ def loop(self):
probability_of_append_dataset = 1
print(probability_of_append_dataset)
make_dataset_timing = np.random.choice(a = [True,False], size = 1,p = [probability_of_append_dataset,1-probability_of_append_dataset])
self.dl.make_dataset(imgobj, target_action)
self.dl.add_data(imgobj, target_action)
line = [str(self.episode), "training", str(distance), str(self.pos_x), str(self.pos_y), str(self.pos_the)]
with open(self.path + self.start_time + '/' + 'training.csv', 'a') as f:
writer = csv.writer(f, lineterminator='\n')
writer.writerow(line)
if make_dataset_timing[0]:
self.dl.make_dataset(imgobj, target_action)
self.dl.add_data(imgobj, target_action)
with open(self.path + self.start_time + '/' + 'training.csv', 'a') as f:
writer = csv.writer(f, lineterminator='\n')
writer.writerow(line)
Expand All @@ -248,11 +248,11 @@ def loop(self):

if abs(target_action) < 0.1:
if make_dataset_timing[0]:
self.dl.make_dataset(imgobj_left, target_action - 0.2)
self.dl.make_dataset(imgobj_right, target_action + 0.2)
self.dl.make_dataset(imgobj_left, target_action - 0.2)
self.dl.add_data(imgobj_left, target_action - 0.2)
self.dl.add_data(imgobj_right, target_action + 0.2)
self.dl.add_data(imgobj_left, target_action - 0.2)
loss = self.dl.trains()
self.dl.make_dataset(imgobj_right, target_action + 0.2)
self.dl.add_data(imgobj_right, target_action + 0.2)
loss = self.dl.trains()
# loss = self.dl.trains()
# loss = self.dl.trains()
Expand Down Expand Up @@ -282,7 +282,7 @@ def loop(self):
loss = 0
if angle_error > 0.05:
if make_dataset_timing[0]:
self.dl.make_dataset(img,target_action)
self.dl.add_data(img, target_action)
line = [str(self.episode), "training", str(distance), str(self.pos_x), str(self.pos_y), str(self.pos_the)]
with open(self.path + self.start_time + '/' + 'training.csv', 'a') as f:
writer = csv.writer(f, lineterminator='\n')
Expand All @@ -294,8 +294,8 @@ def loop(self):
writer.writerow(line)
if abs(target_action) < 0.1:
if make_dataset_timing[0]:
self.dl.make_dataset(img_left,target_action - 0.2)
self.dl.make_dataset(img_right,target_action + 0.2)
self.dl.add_data(img_left, target_action - 0.2)
self.dl.add_data(img_right, target_action + 0.2)
action_left, loss_left = self.dl.act_and_trains(img_left, target_action - 0.2)
action_right, loss_right = self.dl.act_and_trains(img_right, target_action + 0.2)
if distance > 0.1:
Expand Down
28 changes: 12 additions & 16 deletions scripts/nav_cloning_net.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
from os.path import expanduser

# HYPER PARAM
BATCH_SIZE = 8
BATCH_SIZE = 8
MAX_DATA = 10000

class Net(chainer.Chain):
Expand Down Expand Up @@ -52,18 +52,18 @@ def __init__(self, n_channel=3, n_action=1):
self.data = []
self.target_angles = []

def make_dataset(self, imgobj, target_angle):
self.x = [self.phi(s) for s in [imgobj]]
self.t = np.array([target_angle], np.float32)
self.data.append(self.x[0])
self.target_angles.append(self.t[0])
def add_data(self, imgobj, target_angle):
x = [self.phi(s) for s in [imgobj]]
t = np.array([target_angle], np.float32)
self.data.append(x[0])
self.target_angles.append(t[0])
if len(self.data) > MAX_DATA:
del self.data[0]
del self.target_angles[0]
self.dataset = TupleDataset(self.data, self.target_angles)

def trains(self):
train_iter = SerialIterator(self.dataset, batch_size = BATCH_SIZE, repeat=True, shuffle=True)
dataset = TupleDataset(self.data, self.target_angles)
train_iter = SerialIterator(dataset, batch_size = BATCH_SIZE, repeat=True, shuffle=True)
train_batch = train_iter.next()
x_train, t_train = chainer.dataset.concat_examples(train_batch, -1)

Expand All @@ -82,17 +82,13 @@ def trains(self):
return loss_train.array

def act_and_trains(self, imgobj, target_angle):
self.make_dataset(imgobj, target_angle)
self.add_data(imgobj, target_angle)
loss = self.trains()

x_test = chainer.dataset.concat_examples(self.x, -1)
with chainer.using_config('train', False), chainer.using_config('enable_backprop', False):
action_value = self.net(x_test)
return action_value.data[0][0], loss
return self.act(imgobj), loss

def act(self, imgobj):
self.x = [self.phi(s) for s in [imgobj]]
x_test = chainer.dataset.concat_examples(self.x, -1)
x = [self.phi(s) for s in [imgobj]]
x_test = chainer.dataset.concat_examples(x, -1)

with chainer.using_config('train', False), chainer.using_config('enable_backprop', False):
action_value = self.net(x_test)
Expand Down
76 changes: 66 additions & 10 deletions scripts/nav_cloning_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,9 +23,10 @@
import sys
import tf
from nav_msgs.msg import Odometry
import tf2_ros

class nav_cloning_node:
def __init__(self):
def __init__(self, duration):
rospy.init_node('nav_cloning_node', anonymous=True)
self.mode = rospy.get_param("/nav_cloning_node/mode", "use_dl_output")
self.action_num = 1
Expand Down Expand Up @@ -60,12 +61,18 @@ def __init__(self):
self.is_started = False
self.start_time_s = rospy.get_time()
os.makedirs(self.path + self.start_time)
self.step = 0
self.default_duration = duration
self.duration = duration

with open(self.path + self.start_time + '/' + 'training.csv', 'w') as f:
writer = csv.writer(f, lineterminator='\n')
writer.writerow(['step', 'mode', 'loss', 'angle_error(rad)', 'distance(m)','x(m)','y(m)', 'the(rad)'])
writer.writerow(['episode', 'mode', 'loss', 'angle_error(rad)', 'distance(m)','x(m)','y(m)', 'the(rad)','step'])
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")
Expand Down Expand Up @@ -105,6 +112,26 @@ 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

def callback_vel(self, data):
self.vel = data
self.action = self.vel.angular.z
Expand Down Expand Up @@ -141,17 +168,18 @@ def loop(self):

ros_time = str(rospy.Time.now())

if self.episode == 4000:
if self.episode == 5000:
self.learning = False
self.dl.save(self.save_path)
#self.dl.load(self.load_path)

if self.episode == 6000:
if self.episode == 7000:
os.system('killall roslaunch')
sys.exit()

if self.learning:
target_action = self.action
self.calc_distance()
distance = self.min_distance

if self.mode == "manual":
Expand Down Expand Up @@ -182,9 +210,11 @@ def loop(self):

elif self.mode == "use_dl_output":
action, loss = self.dl.act_and_trains(imgobj, target_action)
self.step += 1
if abs(target_action) < 0.1:
action_left, loss_left = self.dl.act_and_trains(imgobj_left, target_action - 0.2)
action_right, loss_right = self.dl.act_and_trains(imgobj_right, target_action + 0.2)
self.step += 2
angle_error = abs(action - target_action)
if distance > 0.1:
self.select_dl = False
Expand Down Expand Up @@ -216,11 +246,33 @@ def loop(self):
if self.select_dl and self.episode >= 0:
target_action = action

elif self.mode == "variable_sampling_time":
if distance < 0.1:
self.duration = self.default_duration * (0.1 - distance) * 10 + self.default_duration
elif distance >= 0.1:
self.duration = self.default_duration

action, loss = self.dl.act_and_trains(imgobj , target_action)
self.step += 1
if abs(target_action) < 0.1:
action_left, loss_left = self.dl.act_and_trains(imgobj_left , target_action - 0.2)
action_right, loss_right = self.dl.act_and_trains(imgobj_right , target_action + 0.2)
self.step += 2

angle_error = abs(action - target_action)
if distance > 0.1:
self.select_dl = False
elif distance < 0.05:
self.select_dl = True
if self.select_dl and self.episode >= 0:
target_action = action
print("DURATION: " + str(self.duration))

# end mode

print(str(self.episode) + ", training, loss: " + str(loss) + ", angle_error: " + str(angle_error) + ", distance: " + str(distance))
print(str(self.episode) + ", step: " + str(self.step) + ", training, loss: " + str(loss) + ", angle_error: " + str(angle_error) + ", distance: " + str(distance))
self.episode += 1
line = [str(self.episode), "training", str(loss), str(angle_error), str(distance), str(self.pos_x), str(self.pos_y), str(self.pos_the)]
line = [str(self.episode), "training", str(loss), str(angle_error), str(distance), str(self.pos_x), str(self.pos_y), str(self.pos_the), str(self.step)]
with open(self.path + self.start_time + '/' + 'training.csv', 'a') as f:
writer = csv.writer(f, lineterminator='\n')
writer.writerow(line)
Expand All @@ -235,13 +287,14 @@ def loop(self):

self.episode += 1
angle_error = abs(self.action - target_action)
line = [str(self.episode), "test", "0", str(angle_error), str(distance), str(self.pos_x), str(self.pos_y), str(self.pos_the)]
line = [str(self.episode), "test", "0", str(angle_error), str(distance), str(self.pos_x), str(self.pos_y), str(self.pos_the), str(self.step)]
with open(self.path + self.start_time + '/' + 'training.csv', 'a') as f:
writer = csv.writer(f, lineterminator='\n')
writer.writerow(line)
self.vel.linear.x = 0.2
self.vel.angular.z = target_action
self.nav_pub.publish(self.vel)
self.duration = self.default_duration

temp = copy.deepcopy(img)
cv2.imshow("Resized Image", temp)
Expand All @@ -251,10 +304,13 @@ def loop(self):
cv2.imshow("Resized Right Image", temp)
cv2.waitKey(1)

def duration(self):
return self.duration

if __name__ == '__main__':
rg = nav_cloning_node()
DURATION = 0.2
r = rospy.Rate(1 / DURATION)
DURATION = 0.25
rg = nav_cloning_node(DURATION)
while not rospy.is_shutdown():
rg.loop()
r = rospy.Rate(1 / rg.duration)
r.sleep()