def reset(self):
self.last50actions = [0] * 50 # used for looping avoidance
# === POSE ===
pos = random.choice(list(enumerate(positions)))[0]
self.position = pos
self.set_new_pose(pos)
# === RESET ===
# Resets the state of the environment and returns an initial observation.
time.sleep(0.05)
# self._gazebo_reset()
self._gazebo_unpause()
image_data = None
cv_image = None
success = False
while image_data is None or success is False:
image_data = rospy.wait_for_message('/F1ROS/cameraL/image_raw', Image, timeout=5)
# Transform the image data from ROS to CVMat
cv_image = CvBridge().imgmsg_to_cv2(image_data, "bgr8")
f1_image_camera = self.image_msg_to_image(image_data, cv_image)
if f1_image_camera:
success = True
self._gazebo_pause()
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
cv_image = cv2.resize(cv_image, (self.img_rows, self.img_cols))
# cv_image = cv_image[(self.img_rows/20):self.img_rows-(self.img_rows/20),(self.img_cols/10):self.img_cols] #crop image
# cv_image = skimage.exposure.rescale_intensity(cv_image,out_range=(0,255))
state = cv_image.reshape(1, 1, cv_image.shape[0], cv_image.shape[1])
return state, pos
def step(self, action):
self._gazebo_unpause()
# === ACTIONS === - 5 actions
vel_cmd = Twist()
vel_cmd.linear.x = 3 # self.action_space[action][0]
vel_cmd.angular.z = self.action_space[action] # [1]
self.vel_pub.publish(vel_cmd)
# print("Action: {} - V_Lineal: {} - W_Angular: {}".format(action, vel_cmd.linear.x, vel_cmd.angular.z))
# === IMAGE ===
image_data = None
success = False
cv_image = None
f1_image_camera = None
while image_data is None or success is False:
image_data = rospy.wait_for_message('/F1ROS/cameraL/image_raw', Image, timeout=5)
# Transform the image data from ROS to CVMat
cv_image = CvBridge().imgmsg_to_cv2(image_data, "bgr8")
f1_image_camera = self.image_msg_to_image(image_data, cv_image)
if f1_image_camera:
success = True
point_1, point_2, point_3 = self.processed_image(f1_image_camera.data)
# DONE
done = self.is_game_over(point_1, point_2, point_3)
self._gazebo_pause()
self.last50actions.pop(0) # remove oldest
if action == 0:
self.last50actions.append(0)
else:
self.last50actions.append(1)
# action_sum = sum(self.last50actions)
# == CALCULATE ERROR ==
error_1, error_2, error_3 = self.calculate_error(point_1, point_2, point_3)
# == REWARD ==
if not done:
reward = self.calculate_reward(error_1, error_2, error_3)
else:
reward = -200
# == TELEMETRY ==
if telemetry:
self.show_telemetry(f1_image_camera.data, point_1, point_2, point_3, action, reward)
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
cv_image = cv2.resize(cv_image, (self.img_rows, self.img_cols))
observation = cv_image.reshape(1, 1, cv_image.shape[0], cv_image.shape[1])
# info = [vel_cmd.linear.x, vel_cmd.angular.z, error_1, error_2, error_3]
# OpenAI standard return: observation, reward, done, info
return observation, reward, done, {}
def discretize_observation(self, data):
image_data = None
cv_image = None
n = 0
while image_data is None:
try:
image_data = rospy.wait_for_message('/camera/rgb/image_raw',
Image,
timeout=5)
h = image_data.height
w = image_data.width
cv_image = CvBridge().imgmsg_to_cv2(image_data, "bgr8")
except:
n += 1
if n == 10:
print "Camera error"
state = []
done = True
return state
# cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
cv_image = cv2.resize(cv_image, (self.height, self.width))
cv_image = cv_image.reshape(self.width, self.height, self.channel)
# print cv_image.shape
# cv2.imshow("image", cv_image)
# cv2.waitKey(3)
return cv_image
def _reset(self):
self.last50actions = [0] * 50 # used for looping avoidance
# Resets the state of the environment and returns an initial observation.
rospy.wait_for_service('/gazebo/reset_simulation')
try:
# reset_proxy.call()
self.reset_proxy()
except (rospy.ServiceException) as e:
print("/gazebo/reset_simulation service call failed")
# Unpause simulation to make observation
rospy.wait_for_service('/gazebo/unpause_physics')
try:
# resp_pause = pause.call()
self.unpause()
except (rospy.ServiceException) as e:
print("/gazebo/unpause_physics service call failed")
image_data = None
success = False
cv_image = None
while image_data is None or success is False:
try:
image_data = rospy.wait_for_message('/front_cam/camera/image',
Image,
timeout=5)
h = image_data.height
w = image_data.width
cv_image = CvBridge().imgmsg_to_cv2(image_data, "bgr8")
# temporal fix, check image is not corrupted
if not (cv_image[h / 2, w / 2, 0] == 178
and cv_image[h / 2, w / 2, 1] == 178
and cv_image[h / 2, w / 2, 2] == 178):
success = True
else:
pass
# print("/camera/rgb/image_raw ERROR, retrying")
except:
pass
rospy.wait_for_service('/gazebo/pause_physics')
try:
# resp_pause = pause.call()
self.pause()
except (rospy.ServiceException) as e:
print("/gazebo/pause_physics service call failed")
'''x_t = skimage.color.rgb2gray(cv_image)
x_t = skimage.transform.resize(x_t,(32,32))
x_t = skimage.exposure.rescale_intensity(x_t,out_range=(0,255))'''
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
cv_image = cv2.resize(cv_image, (self.img_rows, self.img_cols))
# cv_image = cv_image[(self.img_rows/20):self.img_rows-(self.img_rows/20),(self.img_cols/10):self.img_cols] #crop image
# cv_image = skimage.exposure.rescale_intensity(cv_image,out_range=(0,255))
state = cv_image.reshape(1, 1, cv_image.shape[0], cv_image.shape[1])
return state
def discretize_observation(self, data):
"""image_data = None
cv_image = None
n = 0
while image_data is None:
try:
image_data = rospy.wait_for_message('/camera/depth/image_raw', Image, timeout=5)
h = image_data.height
w = image_data.width
cv_image = CvBridge().imgmsg_to_cv2(image_data, "32FC1")
except:
n += 1
if n == 10:
print "Depth error"
state = []
done = True
return state, done
cv_image = np.array(cv_image, dtype=np.float32)
cv2.normalize(cv_image, cv_image, 0, 1, cv2.NORM_MINMAX)
cv_image = cv2.resize(cv_image, (160, 120))
for i in range(120):
for j in range(160):
if np.isnan(cv_image[i][j]):
cv_image[i][j] = 0
elif np.isinf(cv_image[i][j]):
cv_image[i][j] = 1
"""
image_data = None
cv_image = None
n = 0
while image_data is None:
try:
image_data = rospy.wait_for_message('/camera/rgb/image_raw',
Image,
timeout=5)
h = image_data.height
w = image_data.width
cv_image = CvBridge().imgmsg_to_cv2(image_data, "bgr8")
except:
n += 1
if n == 10:
print "Camera error"
state = []
done = True
return state
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
cv_image = cv2.resize(cv_image, (self.height, self.width))
cv_image = cv_image.reshape(self.width, self.height, self.channel)
# print cv_image.shape
#cv2.imshow("image", cv_image)
#cv2.waitKey(3)
return cv_image
def reset(self):
#print('reset')
self.last50actions = [0] * 50 #used for looping avoidance
# Resets the state of the environment and returns an initial observation.
rospy.wait_for_service('/pioneer3at/supervisor/simulation_reset')
self.reset_service(1)
#wait until the simulation is reset
# Unpause simulation to make observation
time.sleep(1)
self.enable_sensors()
self.stop_robot()
self.enable_motors()
image_data = None
success=False
cv_image = None
while image_data is None or success is False:
try:
image_data = rospy.wait_for_message('/pioneer3at/camera/image', Image, timeout=5)
h = image_data.height
w = image_data.width
cv_image = CvBridge().imgmsg_to_cv2(image_data, "bgr8")
#temporal fix, check image is not corrupted
if not (cv_image[h//2,w//2,0]==178 and cv_image[h//2,w//2,1]==178 and cv_image[h//2,w//2,2]==178):
success = True
else:
print("/pioneer3at/camera/image ERROR, retrying")
#self.step_sim()
except:
print("/pioneer3at/camera/image ERROR, retrying")
#self.step_sim()
'''x_t = skimage.color.rgb2gray(cv_image)
x_t = skimage.transform.resize(x_t,(32,32))
x_t = skimage.exposure.rescale_intensity(x_t,out_range=(0,255))'''
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
cv_image = cv2.resize(cv_image, (self.img_rows, self.img_cols))
#cv_image = cv_image[(self.img_rows/20):self.img_rows-(self.img_rows/20),(self.img_cols/10):self.img_cols] #crop image
#cv_image = skimage.exposure.rescale_intensity(cv_image,out_range=(0,255))
state = cv_image.reshape(1, 1, cv_image.shape[0], cv_image.shape[1])
return state
def discretize_observation(self, data):
image_data = None
cv_image = None
n = 0
while image_data is None:
try:
image_data = rospy.wait_for_message('/camera/rgb/image_raw',
Image,
timeout=5)
h = image_data.height
w = image_data.width
cv_image = CvBridge().imgmsg_to_cv2(image_data, "bgr8")
except:
n += 1
if n == 10:
print "Camera error"
state = []
done = True
return state
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
cv_image = cv2.resize(cv_image, (self.height, self.width))
cv_image = cv_image.reshape(self.width, self.height, self.channel)
# print cv_image.shape
# cv2.imshow("image", cv_image)
# cv2.waitKey(3)
observation = []
mod = len(data.ranges) / 20
for i, item in enumerate(data.ranges):
if (i % mod == 0):
if data.ranges[i] == float('Inf') or np.isinf(data.ranges[i]):
observation.append(21)
elif np.isnan(data.ranges[i]):
observation.append(0)
else:
observation.append(data.ranges[i])
return cv_image, observation
cv_image = CvBridge().imgmsg_to_cv2(image_data, "bgr8")
#np.save('cv_image.npy',cv_image)
#print(h_img)
#print(w_img)
#print(cv_image[h_img/2,w_img/2])
if not (cv_image[h_img / 2, w_img / 2, 0] == 178
and cv_image[h_img / 2, w_img / 2, 1] == 178
and cv_image[h_img / 2, w_img / 2, 2] == 178):
success_img = True
else:
pass
except:
pass
rospy.wait_for_service('/gazebo/pause_physics')
try:
self.pause()
except rospy.ServiceException, e:
print("/gazebo/pause_physics service call failed")
cv_image_out = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
cv_image_out = cv2.resize(cv_image_out, (self.img_rows, self.img_cols))
state_out = cv_image_out.reshape(1, cv_image_out.shape[0],
cv_image_out.shape[1], 1)
cv_depth = cv2.resize(cv_depth, (self.img_rows, self.img_cols))
state = cv_depth.reshape(1, cv_depth.shape[0], cv_depth.shape[1], 1)
return state
def _step(self, action):
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except (rospy.ServiceException) as e:
print("/gazebo/unpause_physics service call failed")
# 3 actions
if action == 0: # FORWARD AND UP
vel_cmd = Twist()
vel_cmd.linear.x = 2
vel_cmd.linear.y = 0
vel_cmd.linear.z = 1
vel_cmd.angular.x = 0
vel_cmd.angular.y = 0
vel_cmd.angular.z = 0
self.cmd_pub.publish(vel_cmd)
elif action == 1: # LEFT
vel_cmd = Twist()
vel_cmd.linear.x = 1
vel_cmd.angular.z = 2
vel_cmd.linear.y = 0
vel_cmd.angular.x = 0
vel_cmd.angular.y = 0
vel_cmd.angular.z = 2
self.cmd_pub.publish(vel_cmd)
elif action == 2: # RIGHT
vel_cmd = Twist()
vel_cmd.linear.x = 1
vel_cmd.angular.z = 1
vel_cmd.linear.y = 0
vel_cmd.angular.x = 0
vel_cmd.angular.y = 0
vel_cmd.angular.z = -2
self.cmd_pub.publish(vel_cmd)
elif action == 3: # BACK
vel_cmd = Twist()
vel_cmd.linear.x = -2
vel_cmd.linear.z = 1
vel_cmd.linear.y = 0
vel_cmd.angular.x = 0
vel_cmd.angular.y = 0
vel_cmd.angular.z = 0
self.cmd_pub.publish(vel_cmd)
elif action == 4: # DOWN
vel_cmd = Twist()
vel_cmd.linear.x = 0
vel_cmd.angular.z = -2
vel_cmd.linear.y = 0
vel_cmd.angular.x = 0
vel_cmd.angular.y = 0
vel_cmd.angular.z = 0
self.cmd_pub.publish(vel_cmd)
elif action == 5: # TURN LEFT
vel_cmd = Twist()
vel_cmd.linear.x = 0
vel_cmd.linear.z = 1
vel_cmd.linear.y = -2
vel_cmd.angular.x = 0
vel_cmd.angular.y = 0
vel_cmd.angular.z = 0
self.cmd_pub.publish(vel_cmd)
elif action == 6: # TURN RIGHT
vel_cmd = Twist()
vel_cmd.linear.x = 0
vel_cmd.linear.z = 1
vel_cmd.linear.y = 2
vel_cmd.angular.x = 0
vel_cmd.angular.y = 0
vel_cmd.angular.z = 0
self.cmd_pub.publish(vel_cmd)
elif action == 7: # UP
vel_cmd = Twist()
vel_cmd.linear.x = 0
vel_cmd.linear.z = 2
vel_cmd.linear.y = 0
vel_cmd.angular.x = 0
vel_cmd.angular.y = 0
vel_cmd.angular.z = 0
self.cmd_pub.publish(vel_cmd)
elif action == 8: # STAND STILL
print("gets here")
vel_cmd = Twist()
vel_cmd.linear.x = 0
vel_cmd.linear.z = 0
vel_cmd.linear.y = 0
vel_cmd.angular.x = 0
vel_cmd.angular.y = 0
vel_cmd.angular.z = 0
self.cmd_pub.publish(vel_cmd)
data = None
while data is None:
try:
data = rospy.wait_for_message('/scan', LaserScan, timeout=5)
except:
pass
done = self.calculate_observation(data)
image_data = None
success = False
cv_image = None
while image_data is None or success is False:
try:
image_data = rospy.wait_for_message('/front_cam/camera/image',
Image,
timeout=5)
h = image_data.height
w = image_data.width
cv_image = CvBridge().imgmsg_to_cv2(image_data, "bgr8")
# temporal fix, check image is not corrupted
if not (cv_image[h / 2, w / 2, 0] == 178
and cv_image[h / 2, w / 2, 1] == 178
and cv_image[h / 2, w / 2, 2] == 178):
success = True
else:
pass
# print("/camera/rgb/image_raw ERROR, retrying")
except:
pass
rospy.wait_for_service('/gazebo/pause_physics')
try:
# resp_pause = pause.call()
self.pause()
except (rospy.ServiceException) as e:
print("/gazebo/pause_physics service call failed")
self.last50actions.pop(0) # remove oldest
if action == 0:
self.last50actions.append(0)
else:
self.last50actions.append(1)
action_sum = sum(self.last50actions)
'''
# Add center of the track reward
# len(data.ranges) = 100
laser_len = len(data.ranges)
tup = []
for value in data.ranges:
if value != float("inf"):
tup.append(value)
left_sum = sum(tuple(tup[laser_len - (laser_len / 5):laser_len - (laser_len / 10)])) # 80-90
right_sum = sum(tuple(tup[(laser_len / 10):(laser_len / 5)])) # 10-20
center_detour = abs(right_sum - left_sum) / 5
# 3 actions
if not done:
if action == 0:
reward = 1 / float(center_detour + 1)
elif action_sum > 45: # L or R looping
reward = -0.5
else: # L or R no looping
reward = 0.5 / float(center_detour + 1)
else:
reward = -1
x_t = skimage.color.rgb2gray(cv_image)
x_t = skimage.transform.resize(x_t,(32,32))
x_t = skimage.exposure.rescale_intensity(x_t,out_range=(0,255))'''
rospy.Subscriber("/pose", PoseStamped, get_location)
distance = math.sqrt((20 - point.position.x)**2 +
(20 - point.position.y)**2)
reward = 10 / distance
reward = reward * 10
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
cv_image = cv2.resize(cv_image, (self.img_rows, self.img_cols))
# cv_image = cv_image[(self.img_rows/20):self.img_rows-(self.img_rows/20),(self.img_cols/10):self.img_cols] #crop image
# cv_image = skimage.exposure.rescale_intensity(cv_image,out_range=(0,255))
state = cv_image.reshape(1, 1, cv_image.shape[0], cv_image.shape[1])
return state, reward, done, {}
pass
#print("/camera/rgb/image_raw ERROR, retrying")
except:
pass
rospy.wait_for_service('/gazebo/pause_physics')
try:
#resp_pause = pause.call()
self.pause()
except rospy.ServiceException, e:
print ("/gazebo/pause_physics service call failed")
'''x_t = skimage.color.rgb2gray(cv_image)
x_t = skimage.transform.resize(x_t,(32,32))
x_t = skimage.exposure.rescale_intensity(x_t,out_range=(0,255))'''
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
cv_image = cv2.resize(cv_image, (self.img_rows, self.img_cols))
#cv_image = cv_image[(self.img_rows/20):self.img_rows-(self.img_rows/20),(self.img_cols/10):self.img_cols] #crop image
#cv_image = skimage.exposure.rescale_intensity(cv_image,out_range=(0,255))
state = cv_image.reshape(1,cv_image.shape[0], cv_image.shape[1], 1)
#state = cv_image.reshape(1, 1, cv_image.shape[0], cv_image.shape[1])
return state
# test STACK 4
#self.s_t = np.stack((cv_image, cv_image, cv_image, cv_image), axis=0)
#self.s_t = self.s_t.reshape(1, self.s_t.shape[0], self.s_t.shape[1], self.s_t.shape[2])
#return self.s_t
def step(self, action):
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except (rospy.ServiceException) as e:
print("/gazebo/unpause_physics service call failed")
'''# 21 actions
max_ang_speed = 0.3
ang_vel = (action-10)*max_ang_speed*0.1 #from (-0.33 to + 0.33)
vel_cmd = Twist()
vel_cmd.linear.x = 0.2
vel_cmd.angular.z = ang_vel
self.vel_pub.publish(vel_cmd)'''
# 3 actions
if action == 0: #FORWARD
vel_cmd = Twist()
vel_cmd.linear.x = 0.2
vel_cmd.angular.z = 0.0
self.vel_pub.publish(vel_cmd)
elif action == 1: #LEFT
vel_cmd = Twist()
vel_cmd.linear.x = 0.05
vel_cmd.angular.z = 0.2
self.vel_pub.publish(vel_cmd)
elif action == 2: #RIGHT
vel_cmd = Twist()
vel_cmd.linear.x = 0.05
vel_cmd.angular.z = -0.2
self.vel_pub.publish(vel_cmd)
data = None
while data is None:
try:
data = rospy.wait_for_message('/scan', LaserScan, timeout=5)
except:
pass
done = self.calculate_observation(data)
image_data = None
success = False
cv_image = None
while image_data is None or success is False:
try:
image_data = rospy.wait_for_message('/camera/rgb/image_raw',
Image,
timeout=5)
h = image_data.height
w = image_data.width
cv_image = CvBridge().imgmsg_to_cv2(image_data, "bgr8")
#temporal fix, check image is not corrupted
if not (cv_image[h // 2, w // 2, 0] == 178
and cv_image[h // 2, w // 2, 1] == 178
and cv_image[h // 2, w // 2, 2] == 178):
success = True
else:
pass
#print("/camera/rgb/image_raw ERROR, retrying")
except:
pass
rospy.wait_for_service('/gazebo/pause_physics')
try:
#resp_pause = pause.call()
self.pause()
except (rospy.ServiceException) as e:
print("/gazebo/pause_physics service call failed")
self.last50actions.pop(0) #remove oldest
if action == 0:
self.last50actions.append(0)
else:
self.last50actions.append(1)
action_sum = sum(self.last50actions)
'''# 21 actions
if not done:
# Straight reward = 5, Max angle reward = 0.5
reward = round(15*(max_ang_speed - abs(ang_vel) +0.0335), 2)
# print ("Action : "+str(action)+" Ang_vel : "+str(ang_vel)+" reward="+str(reward))
if action_sum > 45: #L or R looping
#print("90 percent of the last 50 actions were turns. LOOPING")
reward = -5
else:
reward = -200'''
# Add center of the track reward
# len(data.ranges) = 100
laser_len = len(data.ranges)
left_sum = sum(data.ranges[laser_len - (laser_len / 5):laser_len -
(laser_len / 10)]) #80-90
right_sum = sum(data.ranges[(laser_len / 10):(laser_len / 5)]) #10-20
center_detour = abs(right_sum - left_sum) / 5
# 3 actions
if not done:
if action == 0:
reward = 1 / float(center_detour + 1)
elif action_sum > 45: #L or R looping
reward = -0.5
else: #L or R no looping
reward = 0.5 / float(center_detour + 1)
else:
reward = -1
#print("detour= "+str(center_detour)+" :: reward= "+str(reward)+" ::action="+str(action))
'''x_t = skimage.color.rgb2gray(cv_image)
x_t = skimage.transform.resize(x_t,(32,32))
x_t = skimage.exposure.rescale_intensity(x_t,out_range=(0,255))'''
# cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
cv_image = cv2.resize(cv_image, (self.img_rows, self.img_cols))
#cv_image = cv_image[(self.img_rows/20):self.img_rows-(self.img_rows/20),(self.img_cols/10):self.img_cols] #crop image
#cv_image = skimage.exposure.rescale_intensity(cv_image,out_range=(0,255))
# state = cv_image.reshape(1, 3, cv_image.shape[0], cv_image.shape[1])
state = cv_image.reshape(1, cv_image.shape[0], cv_image.shape[1],
cv_image.shape[2])
return state, reward, done, {}
def step(self, action):
#print('step')
front_left_wheel_service = self.vel_services[0]
back_left_wheel_service = self.vel_services[2]
front_right_wheel_service = self.vel_services[1]
back_right_wheel_service = self.vel_services[3]
# 3 actions
if action == 0: #FORWARD
front_left_wheel_service(6)
back_left_wheel_service(6)
front_right_wheel_service(6)
back_right_wheel_service(6)
elif action == 1: #RIGTH
front_left_wheel_service(6)
back_left_wheel_service(6)
front_right_wheel_service(0.3)
back_right_wheel_service(0.3)
elif action == 2: #LEFT
front_left_wheel_service(0.3)
back_left_wheel_service(0.3)
front_right_wheel_service(6)
back_right_wheel_service(6)
#self.step_sim()
data = None
while data is None:
try:
data = rospy.wait_for_message('/pioneer3at/Sick_LMS_291/laser_scan/layer0', LaserScan, timeout=5)
except:
print("/pioneer3at/Sick_LMS_291/laser_scan/layer0 ERROR, retrying")
#self.step_sim()
image_data = None
success=False
cv_image = None
while image_data is None or success is False:
try:
image_data = rospy.wait_for_message('/pioneer3at/camera/image', Image, timeout=5)
h = image_data.height
w = image_data.width
cv_image = CvBridge().imgmsg_to_cv2(image_data, "bgr8")
#temporal fix, check image is not corrupted
if not (cv_image[h//2,w//2,0]==178 and cv_image[h//2,w//2,1]==178 and cv_image[h//2,w//2,2]==178):
success = True
else:
print("/pioneer3at/camera/image ERROR, retrying")
#self.step_sim()
except:
print("/pioneer3at/camera/image ERROR, retrying")
#self.step_sim()
self.last50actions.pop(0) #remove oldest
if action == 0:
self.last50actions.append(0)
else:
self.last50actions.append(1)
action_sum = sum(self.last50actions)
'''# 21 actions
if not done:
# Straight reward = 5, Max angle reward = 0.5
reward = round(15*(max_ang_speed - abs(ang_vel) +0.0335), 2)
# print ("Action : "+str(action)+" Ang_vel : "+str(ang_vel)+" reward="+str(reward))
if action_sum > 45: #L or R looping
#print("90 percent of the last 50 actions were turns. LOOPING")
reward = -5
else:
reward = -200'''
# Add center of the track reward
# len(data.ranges) = 100
laser_len = len(data.ranges)
left_sum = sum(data.ranges[laser_len-(laser_len/5):laser_len-(laser_len/10)]) #80-90
right_sum = sum(data.ranges[(laser_len/10):(laser_len/5)]) #10-20
center_detour = abs(right_sum - left_sum)/5
done = self.calculate_observation(data)
# 3 actions
if not done:
if action == 0:
reward = 1 / float(center_detour+1)
elif action_sum > 45: #L or R looping
reward = -0.5
else: #L or R no looping
reward = 0.5 / float(center_detour+1)
else:
reward = -1
#print("detour= "+str(center_detour)+" :: reward= "+str(reward)+" ::action="+str(action))
'''x_t = skimage.color.rgb2gray(cv_image)
x_t = skimage.transform.resize(x_t,(32,32))
x_t = skimage.exposure.rescale_intensity(x_t,out_range=(0,255))'''
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
cv_image = cv2.resize(cv_image, (self.img_rows, self.img_cols))
#cv_image = cv_image[(self.img_rows/20):self.img_rows-(self.img_rows/20),(self.img_cols/10):self.img_cols] #crop image
#cv_image = skimage.exposure.rescale_intensity(cv_image,out_range=(0,255))
state = cv_image.reshape(1, 1, cv_image.shape[0], cv_image.shape[1])
return state, reward, done, {}
