def get_integration_dataset(num_samples):
period = 10
data = np.zeros(shape=(num_samples, 1, 480, 640, 3))
labels = np.zeros(shape=(num_samples, 1))
k_p = 1
k_d = 1
speed = 0.2
env = DuckietownEnv(domain_rand=False, draw_bbox=False)
iterations = env.max_steps = num_samples * period
env.reset()
for i in range(iterations):
percent = round(i * 100 / iterations, 2)
print(f'\rsimulator running: {percent} %', end='\r')
lane_pose = get_lane_pos(env)
distance_to_road_center = lane_pose.dist
angle_from_straight_in_rad = lane_pose.angle_rad
steering = k_p * distance_to_road_center + k_d * angle_from_straight_in_rad
command = np.array([speed, steering])
obs, _, done, _ = env.step(command)
obs = integration_preprocess(obs)
if i % period == 0:
data[i // period, 0, :, :, :] = obs
labels[i // period][0] = lane_pose.dist_to_edge * 100
if done:
env.reset()
return list(zip(data, labels))
# then find the closest point to to that
follow_point = closest_point + closest_tangent * follow_dist
curve_point, _ = env.closest_curve_point(follow_point)
# If we have a valid point on the curve, stop
if curve_point is not None:
break
follow_dist *= 0.5
# Compute a normalized vector to the curve point
point_vec = curve_point - env.cur_pos
point_vec /= np.linalg.norm(point_vec)
dot = np.dot(env.get_right_vec(), point_vec)
velocity = 0.35
steering = 2 * -dot
obs, reward, done, info = env.step([velocity, steering])
#print('stepCount = %s, reward=%.3f' % (env.stepCount, reward))
env.render()
if done:
if reward < 0:
print('*** FAILED ***')
if not args.no_pause:
time.sleep(0.7)
obs = env.reset()
env.render()
curve_left_errors.append(omega_err)
elif kind == 'curve_right':
curve_right_errors.append(omega_err)
elif kind.startswith('3way'):
three_way_errors.append(omega_err)
elif kind.startswith('4way'):
four_way_errors.append(omega_err)
if nn_control:
correction = omega_hat
else:
correction = omega
action = np.array([speed, correction])
obs, _, done, _ = env.step(action)
obs = preprocess(obs)
if visual:
rendered = env.render(mode='rgb_array')
# _, t = env.closest_curve_point(env.cur_pos, env.cur_angle)
# rendered = plot_lanes(rendered, env, env.cur_pos, env.cur_angle, t, dist_err)
# rendered = plot_lanes(rendered, env, env.cur_pos, env.cur_angle, t, 0, error_frame=rendered)
own_render(env, rendered)
if done:
print("***CRASHED***")
crashes += 1
env.reset()
controls_model = Deterministic(
lambda time, inputs: [tf.constant([[0.2, 0.]])])
# ======== CONTROL LOOP ==========
# Choose which dynamics model to use. The simplified one is faster but less accurate.
dynamics_model = simplified_dynamics_model
# dynamics_model = complex_dynamics_model
# Feel free to experiment with the hyperparameters
control_loop = iLQR(cost_model=cost_model,
dynamics_model=dynamics_model,
init_controls_model=controls_model,
waypoints=trajectory,
dt=env.delta_time,
threshold=0.2,
debug=True,
lookahead=10,
max_rollout=9,
num_iter=30,
trust_region=0.1,
deterministic=True)
while not control_loop.is_finished:
state = [env.cur_pos[0], env.cur_pos[2], env.cur_angle]
action = control_loop.get_action(state)
env.step(action)
env.render(top_down=True)
print("FINISHED")
load_model()
while True:
start_time = time.time()
obs = obs.transpose(2, 0, 1)
obs = make_var(obs).unsqueeze(0)
vels = model(obs)
vels = vels.squeeze().data.cpu().numpy()
print(vels)
#vels[1] *= 0.85
obs, reward, done, info = env.step(vels)
#print('stepCount = %s, reward=%.3f' % (env.stepCount, reward))
env.render()
end_time = time.time()
frame_time = 1000 * (end_time - start_time)
avg_frame_time = avg_frame_time * 0.95 + frame_time * 0.05
max_frame_time = 0.99 * max(max_frame_time, frame_time) + 0.01 * frame_time
fps = 1 / (frame_time / 1000)
print('avg frame time: %d' % int(avg_frame_time))
print('max frame time: %d' % int(max_frame_time))
print('fps: %.1f' % fps)
if done:
direction2 = None
print(x, z, 'Direzione presa')
if x == 0 and z == 1:
direction2 = 'S'
elif x == 0 and z == -1:
direction2 = 'N'
elif x == 1 and z == 0:
direction2 = 'E'
elif x == -1 and z == 0:
direction2 = 'O'
while i != i_rounded or j != j_rounded:
lane_pose = env.get_lane_pos2(env.cur_pos, env.cur_angle)
distance_to_road_center = lane_pose.dist
angle_from_straight_in_rads = lane_pose.angle_rad
steering = k_p * distance_to_road_center + k_d * angle_from_straight_in_rads
obs, reward, done, info = env.step([speed, steering])
total_reward += reward
print('Steps = %s, Timestep Reward=%.3f, Total Reward=%.3f' %
(env.step_count, reward, total_reward))
env.render()
i, j = env.get_grid_coords(env.cur_pos)
mask = cv2.inRange(obs, lower_red, upper_red)
#Immagine della maschera su schermo
if mask_enable:
cv2.imshow('Red_Lines_Mask', mask)
cv2.waitKey(1)
starting_angle = env.cur_angle
#Qui viene gestita la svolta a destra
while i == i_rounded and j == j_rounded:
tile = env._get_tile(i, j)
assert first_obs.shape == second_obs.shape
# Try stepping a few times
for i in range(0, 10):
obs, _, _, _ = env.step(np.array([0.1, 0.1]))
# Try loading each of the available map files
for map_file in os.listdir('gym_duckietown/maps'):
map_name = map_file.split('.')[0]
env = DuckietownEnv(map_name=map_name)
env.reset()
# Test the multi-map environment
env = MultiMapEnv()
for i in range(0, 50):
env.reset()
# Check that we do not spawn too close to obstacles
env = DuckietownEnv(map_name='loop_obstacles')
for i in range(0, 75):
obs = env.reset()
assert not env._collision(), "collision on spawn"
env.step(np.array([0.05, 0]))
assert not env._collision(), "collision after one step"
# Test the draw_bbox mode
env = DuckietownEnv(map_name='udem1', draw_bbox=True)
env.render('rgb_array')
env = DuckietownEnv(map_name='loop_obstacles', draw_bbox=True)
env.render('rgb_array')
lane_pose = env.get_lane_pos2(env.cur_pos, env.cur_angle)
diff_center = lane_pose.dist
angle_straight_rads = lane_pose.angle_rad
speed = 0.2
timer = time.time() - tmp_time
direction = (k_p * diff_center + k_d * angle_straight_rads + k_p_1 *\
(diff_center - tmp_dist) / timer + k_d_1 * \
(angle_straight_rads - tmp_angle) / timer )
tmp_time = time.time()
tmp_dist = diff_center
tmp_angle = angle_straight_rads
obs, reward, done, info = env.step([speed, direction])
total_reward += reward
im = Image.fromarray(obs)
img = im.convert('RGB')
cv_img = np.array(img)
cv_img = cv_img[:, :, ::-1].copy()
cv_img_BGR = cv2.cvtColor(cv_img, cv2.COLOR_RGB2BGR)
cv_img_grayScaled = cv2.cvtColor(cv_img_BGR, cv2.COLOR_BGR2GRAY)
queryKP, queryDesc = detector.detectAndCompute(cv_img_grayScaled, None)
matches = flann.knnMatch(queryDesc, trainDecs, k=2)
if env.step_count == 550:
agent.remove_grid_from_maze(2, 3)
path = agent.find_new_path((1, 3), (5, 5))
highMatch = []
decay_step += 1
#exponential decay of exploration rate
epsilon = explore_stop + (explore_start - explore_stop) * np.exp(-decay_rate * decay_step)
#if buffer size crosses batch_size
if len(replay_buffer) > batch_size:
if len(replay_buffer) == batch_size:
loss = 0
e = random.random()
state_small = state
#sample mini_batch from buffer
state, action_sample, reward_sample, next_state_sample, done_sample = replay_buffer.sample(batch_size)
#print("argato",state.shape)
action = DQN.act(state, epsilon, e)
print("Action:", action)
next_state, reward, done, _ = env.step(action)
print("Reward:", reward)
next_state, stacked_frames = stack_images(stacked_frames, next_state.transpose((2, 0, 1)), False)
#push Experience to buffer
replay_buffer.push(state_small, action, reward, next_state, done)
episode_rewards.append(reward)
DQN_optimizer.zero_grad()
loss = compute_loss(batch_size, state, action_sample, reward_sample, next_state_sample, done_sample)
loss.backward()
DQN_optimizer.step()
loss_list.append(loss)
print(f'Episode number is {i}/{total_episodes} | Step number is {step} | Loss is {loss}')
else:
#for making agent explore
while True:
nro_iter = nro_iter + 1
pose_del_pato = env.get_lane_pos2(env.cur_pos, env.cur_angle)
distancia_de_manejo = pose_del_pato.dist
angulo_recto = pose_del_pato.angle_rad
k_p = 10
k_d = 1
speed = 0.2
# angulo del volante, que corresponde a la velocidad angular en rad/s
direccion = k_p*distancia_de_manejo + k_d*angulo_recto
obs, reward, done, info = env.step([speed, direccion_angulo])
# Correccion de la imagen externa del pato
original = Image.fromarray(obs)
original = np.ascontiguousarray(np.flip(original, axis=0))
cv_img = cv2.cvtColor(np.array(original), cv2.COLOR_RGB2BGR)
imagen_hsv = cv2.cvtColor(cv_img, cv2.COLOR_BGR2HSV)
erode_it=1
dilate_it=1
altura_y=75
correccion_y=150
limites=6/7
#Definición del rango de amarillos
amarillo_claro = np.array([18, 41, 133])
while current_time - start_time < 5:
time = datetime.datetime.utcnow()
current_time = time.second + time.microsecond / (10**len(
str(time.microsecond)))
lane_pose = env.get_lane_pos2(env.cur_pos, env.cur_angle)
distance_to_road_center = lane_pose.dist
angle_from_straight_in_rads = lane_pose.angle_rad
k_p = 10
k_d = 1
steering = k_p * distance_to_road_center + k_d * angle_from_straight_in_rads # TODO: You should overwrite this value
obs, reward, done, info = env.step([0.2, steering])
total_reward += reward
env.render()
#without checking the position for staying on the road and without checking reward
while True:
time = datetime.datetime.utcnow()
start_time = time.second + time.microsecond / (10**len(
str(time.microsecond)))
current_time = 0
steering = speed / r
while current_time - start_time < T:
distance_to_road_edge = lane_pose.dist_to_edge * 100
distance_to_road_center = lane_pose.dist
angle_from_straight_in_rad = lane_pose.angle_rad
angle_from_straight_in_deg = lane_pose.angle_deg
y_hat = model(obs)
d_hat = y_hat[0][0].numpy()
a_hat = y_hat[0][1].numpy()
d_hat_to_center = (d_hat - 25.) / 100.
a_hat_in_rad = (a_hat * 2 * np.pi) / 360.
dist_err = round(distance_to_road_edge - d_hat, 2)
angle_err = round(angle_from_straight_in_deg - a_hat, 2)
print("\rerror: {}, {}".format(dist_err, angle_err), end='\r')
steering = pid.update(d_hat)
# steering = k_p * d_hat_to_center + k_d * a_hat_in_rad
obs, _, done, _ = env.step(np.array([speed, steering]))
obs = preprocess(obs)
env.render()
if done:
env.reset()
class gymDuckiebotSimRRService(object):
def __init__(self):
# Initialize Robot Simulation
# Other Maps: udem1, straight_road, small_loop, loop_empty, 4way, zigzag_dists, loop_obstacles
# loop_pedestrians, loop_dyn_duckiebots, regress_4way_adam
self.env = DuckietownEnv(seed=1,
max_steps=5000,
map_name='zigzag_dists',
draw_curve=False,
draw_bbox=False,
distortion=True)
self.env.reset()
self.env.render()
self.action = np.array([0.0, 0.0])
self.framerate = self.env.unwrapped.frame_rate
# Initialize the camera images and control streaming
self._lock = threading.RLock()
self._streaming = False
self._framestream = None
self._framestream_endpoints = dict()
self._framestream_endpoints_lock = threading.RLock()
#Capture a frame, apply the action and return a CamImage structure to the client
def CaptureFrame_n_Action(self):
with self._lock:
image = RRN.NewStructure("experimental.gymDuckiebotSim.CamImage")
# Grab image from simulation and apply the action
obs, reward, done, info = self.env.step(self.action)
#if done:
# self.env.reset()
frame = cv2.cvtColor(obs,
cv2.COLOR_RGB2BGR) # Correct color for cv2
# frame = obs
image.width = frame.shape[1]
image.height = frame.shape[0]
image.step = frame.shape[1] * 3
image.data = frame.reshape(frame.size, order='C')
return image
#Start the thread that captures images and sends them through connected
#FrameStream pipes
def StartStreaming(self):
if (self._streaming):
raise Exception("Already streaming")
self._streaming = True
t = threading.Thread(target=self.frame_threadfunc)
t.start()
#Stop the streaming thread
def StopStreaming(self):
if (not self._streaming):
raise Exception("Not streaming")
self._streaming = False
#FrameStream pipe member property getter and setter
@property
def FrameStream(self):
return self._framestream
@FrameStream.setter
def FrameStream(self, value):
self._framestream = value
#Create the PipeBroadcaster and set backlog to 3 so packets
#will be dropped if the transport is overloaded
self._framestream_broadcaster = RR.PipeBroadcaster(value, 3)
#Function that will send a frame at ideally (self.framerate) fps, although in reality it
#will be lower because Python is quite slow. This is for
#demonstration only...
def frame_threadfunc(self):
#Loop as long as we are streaming
while (self._streaming):
#Capture a frame
try:
frame = self.CaptureFrame_n_Action()
except:
#TODO: notify the client that streaming has failed
self._streaming = False
return
#Send the new frame to the broadcaster. Use AsyncSendPacket
#and a blank handler. We really don't care when the send finishes
#since we are using the "backlog" flow control in the broadcaster.
self._framestream_broadcaster.AsyncSendPacket(frame, lambda: None)
# Put in a 100 ms delay
time.sleep(1.0 / self.framerate)
def setAction(self, v, w):
with self._lock:
# v = Forward Velocity [-1 1]
# w = Steering angle [-1 1]
self.action = np.array([v, w])
def Shutdown(self):
print("Duckiebot Simulation RR Service Shutdown.")
self._streaming = False
self.env.close()
class DuckiebotSim(object):
def __init__(self):
# Initialize Robot Simulation
self.env = DuckietownEnv(seed=1,
map_name='loop_sid',
draw_curve=False,
draw_bbox=False,
distortion=True,
domain_rand=False,
camera_width=640,
camera_height=480,
user_tile_start=(1, 3))
self.env.reset()
self.env.render()
# self.action = np.array([0.44, 0.0])
self.action = np.array([0.0, 0.0])
# For image to ros
self.bridge = CvBridge()
# Initialize ROS Node
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" % (self.node_name))
# Setup parameters
self.framerate = self.setupParam("~framerate",
self.env.unwrapped.frame_rate)
# Setup Publisher
#self.pub_img= rospy.Publisher("cv_camera/image_raw",Image,queue_size=1)
#self.pub_cam = rospy.Publisher("cv_camera/camera_info", CameraInfo, queue_size=1)
self.pub_img = rospy.Publisher("image_raw", Image, queue_size=1)
self.pub_cam = rospy.Publisher("camera_info", CameraInfo, queue_size=1)
self.has_published = False
# Setup Subscriber
self.sub_cmd = rospy.Subscriber("/cmd_vel",
Twist,
self.cbCmd,
queue_size=1)
# Setup timer
self.timer_img_low = rospy.Timer(
rospy.Duration.from_sec(1.0 / self.framerate), self.cbTimer)
#self.timer_img_low = rospy.Timer(rospy.Duration.from_sec(1.0/10.0),self.cbTimer)
rospy.loginfo("[%s] Initialized." % (self.node_name))
def setupParam(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
rospy.set_param(param_name,
value) #Write to parameter server for transparancy
rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
return value
def cbTimer(self, event):
if not rospy.is_shutdown():
self.grabAndPublish(self.pub_img)
def grabAndPublish(self, publisher):
# Grab image from simulation and apply the action
obs, reward, done, info = self.env.step(self.action)
#rospy.loginfo('[%s] step_count = %s, reward=%.3f' % (self.node_name, self.env.unwrapped.step_count, reward))
image = cv2.cvtColor(obs, cv2.COLOR_BGR2RGB) # Correct color for cv2
"""
##show image
cv2.namedWindow("Image")
if (not image is None):
cv2.imshow("Image",image)
if cv2.waitKey(1)!=-1: #Burak, needs to modify this line to work on your computer, THANKS!
cv2.destroyAllWindows()
"""
# Publish raw image
image_msg = self.bridge.cv2_to_imgmsg(image, "bgr8")
image_msg.header.stamp = rospy.Time.now()
# Publish
publisher.publish(image_msg)
cam_msg = CameraInfo()
cam_msg.height = 480
cam_msg.width = 640
#cam_msg.height = 240
#cam_msg.width= 320
cam_msg.header.stamp = image_msg.header.stamp
cam_msg.header.frame_id = 'cam'
cam_msg.distortion_model = 'plumb_bob'
cam_msg.D = [
-0.2, 0.0305, 0.0005859930422629722, -0.0006697840226199427, 0
]
cam_msg.K = [
305.5718893575089,
0,
303.0797142544728,
0,
308.8338858195428,
231.8845403702499,
0,
0,
1,
]
cam_msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
cam_msg.P = [
220.2460277141687,
0,
301.8668918355899,
0,
0,
238.6758484095299,
227.0880056118307,
0,
0,
0,
1,
0,
]
self.pub_cam.publish(cam_msg)
if not self.has_published:
rospy.loginfo("[%s] Published the first image." % (self.node_name))
self.has_published = True
#if done and (self.env.unwrapped.step_count != 1500):
#rospy.logwarn("[%s] DONE! RESETTING." %(self.node_name))
#self.env.reset()
self.env.render()
def cbCmd(self, cmd_msg):
linear_vel = cmd_msg.linear.x # Forward Velocity [-1 1]
angular_vel = cmd_msg.angular.z # Steering angle [-1 1]
self.action = np.array([linear_vel, angular_vel])
def onShutdown(self):
rospy.loginfo("[%s] Shutdown." % (self.node_name))
self.env.close()
graph_location=
STORAGE_LOCATION, # where do we want to store our trained models
seed=SEED # to seed all random operations in the model (e.g., dropout)
)
observation = env.reset()
# we can use the gym reward to get an idea of the performance of our model
cumulative_reward = 0.0
for episode in range(0, EPISODES):
for steps in range(0, STEPS):
action = model.predict(observation)
#print('Action', action)
#action = np.abs(action)
observation, reward, done, info = env.step(action)
env.render()
cumulative_reward += reward
if done:
break
# env.render()
# we reset after each episode, or not, this really depends on you
env.reset()
print('total reward: {}, mean reward: {}'.format(
cumulative_reward, cumulative_reward // EPISODES))
# didn't look good, ah? Well, that's where you come into the stage... good luck!
env.close()
model.close()
has_intersection=False,
has_stop_sign=True)
if args.load_actions and policy is not None:
loaded_actions = np.loadtxt(args.load_actions, delimiter=',')
policy.load_actions(loaded_actions)
done = False
printed_msg = False
if from_file:
for (speed, steering) in actions:
print(speed, steering)
obs, reward, done, info = env.step([speed, steering])
total_reward += reward
print('Steps = %s, Timestep Reward=%.3f, Total Reward=%.3f' %
(env.step_count, reward, total_reward))
env.render()
time.sleep(0.1)
else:
try:
while not done:
rgbobs = env.render('rgb_array')
# if env.step_count == len(loaded_actions):
# policy.turn_left_incoming = policy.turn_right_incoming = False
# policy.seeing_stop = True
# TODO: Décide comment calculer la vitesse et la direction
k_p = 10
k_d = 1
# La vitesse est une valeur entre 0 et 1 (correspond à une vitesse réelle de 0 à 1,2m/s)
vitesse = 0.2 # You should overwrite this value
# l'angle du volant, c'est-à-dire le changement d'angle de la voiture en rads/s
braquage = (
k_p * distance_to_road_center + k_d * angle_from_straight_in_rads
) # You should overwrite this value
###### Fini à remplir le code ici
obs, recompense, fini, info = env.step([vitesse, braquage])
total_recompense += recompense
print(
"étape = %s, recompense instantanée=%.3f, recompense totale=%.3f"
% (env.step_count, recompense, total_recompense)
)
env.render()
if fini:
if recompense < 0:
print("*** CRASHED ***")
print("recompense finale = %.3f" % total_recompense)
break
elif kind == 'curve_right':
curve_right_errors.append([dist_err, angle_err])
elif kind.startswith('3way'):
three_way_errors.append([dist_err, angle_err])
elif kind.startswith('4way'):
four_way_errors.append([dist_err, angle_err])
if nn_control:
d_hat_to_center = (d - 20.5) / 100.
a_hat_in_rad = (a * 2 * np.pi) / 360.
steering = k_p * d_hat_to_center + k_d * a_hat_in_rad
else:
steering = k_p * distance_to_road_center + k_d * angle_from_straight_in_rad
command = np.array([speed, steering])
obs, _, done, _ = env.step(command)
obs = preprocess(obs)
if visual:
rendered = env.render(mode='rgb_array')
# _, t = env.closest_curve_point(env.cur_pos, env.cur_angle)
# rendered = plot_lanes(rendered, env, env.cur_pos, env.cur_angle, t, dist_err)
# rendered = plot_lanes(rendered, env, env.cur_pos, env.cur_angle, t, 0, error_frame=rendered)
own_render(env, rendered)
if done:
print("***CRASHED***")
crashes += 1
env.reset()
Elapsed = 0
Speed = 0.6
Steering = 2.0
i = 0
while True:
# Changing the directions in every 2 sec
Elapsed = time.time()
if ((Elapsed - Start) > 2):
Speed = -1 * Speed
Steering = -1 * Steering
Start = time.time()
i += 1
# Generating the change in the simulation environment
obs, reward, done, info = env.step([Speed, Steering])
## obs Is the NxMx3 Array of pixels seen through the camera. We will use this as input in our network.
obs = obs[100:, :, :]
# Visualizing the cropped image
cv2.imshow("Cropped", obs)
key = cv2.waitKey(1) & 0xFF
if (key == ord('q')):
cv2.destroyAllWindows()
break
# Rendering the actual frame
env.render()
# After 10 seconds the program closes
if (i == 5):
class DuckiebotSim(object):
def __init__(self):
# Initialize Robot Simulation
self.env = DuckietownEnv( seed=1, map_name = 'udem1', draw_curve = False, draw_bbox = False, distortion = False, domain_rand = False)
self.env.reset()
self.env.render()
# self.action = np.array([0.44, 0.0])
self.action = np.array([0.0, 0.0])
# For image to ros
self.bridge = CvBridge()
# Initialize ROS Node
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" %(self.node_name))
# Setup parameters
self.framerate = self.setupParam("~framerate",self.env.unwrapped.frame_rate)
# Setup Publisher
self.pub_img= rospy.Publisher("image_raw",Image,queue_size=1)
self.has_published = False
# Setup Subscriber
#self.sub_cmd = rospy.Subscriber("/cmd_vel", Twist, self.cbCmd, queue_size=1)
# Setup timer
self.timer_img_low = rospy.Timer(rospy.Duration.from_sec(1.0/self.framerate),self.cbTimer)
rospy.loginfo("[%s] Initialized." %(self.node_name))
def setupParam(self,param_name,default_value):
value = rospy.get_param(param_name,default_value)
rospy.set_param(param_name,value) #Write to parameter server for transparancy
rospy.loginfo("[%s] %s = %s " %(self.node_name,param_name,value))
return value
def cbTimer(self,event):
if not rospy.is_shutdown():
self.grabAndPublish(self.pub_img)
def grabAndPublish(self,publisher):
self.action = self.basic_control()
# Grab image from simulation and apply the action
obs, reward, done, info = self.env.step(self.action)
#rospy.loginfo('[%s] step_count = %s, reward=%.3f' % (self.node_name, self.env.unwrapped.step_count, reward))
image = cv2.cvtColor(obs, cv2.COLOR_BGR2RGB) # Correct color for cv2
# Publish raw image
image_msg = self.bridge.cv2_to_imgmsg(image)
image_msg.header.stamp = rospy.Time.now()
# Publish
publisher.publish(image_msg)
if not self.has_published:
rospy.loginfo("[%s] Published the first image." %(self.node_name))
self.has_published = True
# if done and (self.env.unwrapped.step_count != 1500):
# rospy.logwarn("[%s] DONE! RESETTING." %(self.node_name))
# self.env.reset()
#self.env.render()
def cbCmd(self,cmd_msg):
linear_vel = cmd_msg.linear.x # Forward Velocity [-1 1]
angular_vel = cmd_msg.angular.z # Steering angle [-1 1]
self.action = np.array([linear_vel, angular_vel])
def onShutdown(self):
rospy.loginfo("[%s] Shutdown." %(self.node_name))
self.env.close()
def basic_control(self):
lane_pose = self.env.get_lane_pos2(self.env.cur_pos, self.env.cur_angle)
distance_to_road_center = lane_pose.dist
angle_from_straight_in_rads = lane_pose.angle_rad
rospy.loginfo(" dist = %.2f" %(distance_to_road_center))
rospy.loginfo("theta = %.2f" %(angle_from_straight_in_rads))
k_p = 10
k_d = 5
speed = 0.2
steering = k_p*distance_to_road_center + k_d*angle_from_straight_in_rads
return [speed, steering]
def main():
""" Main launcher that starts the gym thread when the command 'duckietown-start-gym' is invoked
"""
# get parameters from environment (set during docker launch) otherwise take default
map = os.getenv('DUCKIETOWN_MAP', DEFAULTS["map"])
domain_rand = bool(
os.getenv('DUCKIETOWN_DOMAIN_RAND', DEFAULTS["domain_rand"]))
max_steps = os.getenv('DUCKIETOWN_MAX_STEPS', DEFAULTS["max_steps"])
# if a challenge is set, it overrides the map selection
misc = {} # init of info field for additional gym data
challenge = os.getenv('DUCKIETOWN_CHALLENGE', "")
if challenge in ["LF", "LFV"]:
print("Launching challenge:", challenge)
map = DEFAULTS["challenges"][challenge]
misc["challenge"] = challenge
print("Using map:", map)
env = DuckietownEnv(
map_name=map,
# draw_curve = args.draw_curve,
# draw_bbox = args.draw_bbox,
max_steps=max_steps,
domain_rand=domain_rand)
obs = env.reset()
publisher_socket = None
command_socket, command_poll = make_pull_socket()
print("Simulator listening to incoming connections...")
while True:
if has_pull_message(command_socket, command_poll):
success, data = receive_data(command_socket)
if not success:
print(data) # in error case, this will contain the err msg
continue
reward = 0 # in case it's just a ping, not a motor command, we are sending a 0 reward
done = False # same thing... just for gym-compatibility
misc_ = {} # same same
if data["topic"] == 0:
obs, reward, done, misc_ = env.step(data["msg"])
if DEBUG:
print("challenge={}, step_count={}, reward={}, done={}".
format(challenge, env.unwrapped.step_count,
np.around(reward, 3), done))
if done:
env.reset()
if data["topic"] == 1:
print("received ping:", data)
if data["topic"] == 2:
obs = env.reset()
# can only initialize socket after first listener is connected - weird ZMQ bug
if publisher_socket is None:
publisher_socket = make_pub_socket(for_images=True)
if data["topic"] in [0, 1]:
misc.update(misc_)
send_gym(publisher_socket, obs, reward, done, misc)
class DuckiebotSim(object):
def __init__(self):
# Initialize Robot Simulation
self.env = DuckietownEnv(seed=1,
map_name='udem1',
draw_curve=False,
draw_bbox=False,
distortion=False)
self.env.reset()
self.env.render()
# self.action = np.array([0.44, 0.0])
self.action = np.array([0.0, 0.0])
# For image to ros
self.bridge = CvBridge()
# Initialize ROS Node
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" % (self.node_name))
# Setup parameters
self.framerate = self.setupParam("~framerate",
self.env.unwrapped.frame_rate)
# Setup Publisher
self.pub_img = rospy.Publisher("image_raw", Image, queue_size=1)
self.has_published = False
# Setup Subscriber
self.sub_cmd = rospy.Subscriber("/cmd_vel",
Twist,
self.cbCmd,
queue_size=1)
# Setup timer
self.timer_img_low = rospy.Timer(
rospy.Duration.from_sec(1.0 / self.framerate), self.cbTimer)
rospy.loginfo("[%s] Initialized." % (self.node_name))
def setupParam(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
rospy.set_param(param_name,
value) #Write to parameter server for transparancy
rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
return value
def cbTimer(self, event):
if not rospy.is_shutdown():
self.grabAndPublish(self.pub_img)
def grabAndPublish(self, publisher):
# Grab image from simulation and apply the action
obs, reward, done, info = self.env.step(self.action)
rospy.loginfo('[%s] step_count = %s, reward=%.3f' %
(self.node_name, self.env.unwrapped.step_count, reward))
image = cv2.cvtColor(obs, cv2.COLOR_BGR2RGB) # Correct color for cv2
"""
##show image
cv2.namedWindow("Image")
if (not image is None):
cv2.imshow("Image",image)
if cv2.waitKey(1)!=-1: #Burak, needs to modify this line to work on your computer, THANKS!
cv2.destroyAllWindows()
"""
# Publish raw image
image_msg = self.bridge.cv2_to_imgmsg(image)
image_msg.header.stamp = rospy.Time.now()
# Publish
publisher.publish(image_msg)
if not self.has_published:
rospy.loginfo("[%s] Published the first image." % (self.node_name))
self.has_published = True
if done and (self.env.unwrapped.step_count != 1500):
rospy.logwarn("[%s] DONE! RESETTING." % (self.node_name))
self.env.reset()
self.env.render()
def cbCmd(self, cmd_msg):
linear_vel = cmd_msg.linear.x # Forward Velocity [-1 1]
angular_vel = cmd_msg.angular.z # Steering angle [-1 1]
self.action = np.array([linear_vel, angular_vel])
def onShutdown(self):
rospy.loginfo("[%s] Shutdown." % (self.node_name))
self.env.close()
###### Start changing the code here.
# TODO: Decide how to calculate the speed and direction.
k_p = 10
k_d = 1
# The speed is a value between [0, 1] (which corresponds to a real speed between 0m/s and 1.2m/s)
speed = 0.2 # TODO: You should overwrite this value
# angle of the steering wheel, which corresponds to the angular velocity in rad/s
steering = k_p * distance_to_road_center + k_d * angle_from_straight_in_rads # TODO: You should overwrite this value
print("stering real : \n" + str(steering))
###### No need to edit code below.
obs, reward, done, info = env.step([speed, steering_angle])
### line detector
original = Image.fromarray(obs)
cv_img = cv2.cvtColor(np.array(original), cv2.COLOR_RGB2BGR)
hsv_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2HSV)
erode_it = 1
dilate_it = 1
height_y = 75
offset_y = 150
boundary = 6 / 7
#Definición del rango de amarillos
lower_yellow = np.array([18, 41, 133])
upper_yellow = np.array([30, 255, 255])
(3.5, 0, 1.3), (3.0, 0, 1.3)])
route = route * env.road_tile_size
pt_index = np.argmin([np.linalg.norm(pt - env.cur_pos) for pt in route])
while True:
direction = -1 if args.backwards else 1
pt_dest = route[pt_index]
dist = np.linalg.norm(pt_dest - env.cur_pos)
if dist < 0.13:
pt_index = (pt_index + direction) % len(route)
pt_dest = route[pt_index]
dist = np.linalg.norm(pt_dest - env.cur_pos)
cur_dir = np.delete(env.get_dir_vec(), 1)
v = np.delete(pt_dest - env.cur_pos, 1)
desired_dir = direction * v / np.linalg.norm(v)
rot = np.arctan2(
-cur_dir[0] * desired_dir[1] + cur_dir[1] * desired_dir[0],
np.dot(cur_dir, desired_dir))
# print('pos {}, dest {}, dist {}'.format(np.delete(env.cur_pos, 1), pt_dest, dist))
# print('cd {}, dd {}, rot {}, ang {}'.format(cur_dir, desired_dir, rot, env.cur_angle))
speed = direction * 0.3 * (1 - 0.71 * np.clip(abs(rot) - 0.35, 0, 0.4))
steering = 2 * rot
env.step([speed, steering])
env.render(mode=render_mode)
# Se reinicia el environment
env.reset()
#Se crea una llave auxiliar que permite partir quieto
_key = '6'
while True:
# Captura la tecla que está siendo apretada y almacena su valor en key
key = cv2.waitKey(30)
# Si la tecla es Esc, se sale del loop y termina el programa
if key == 27:
break
# Se ejecuta la acción definida anteriormente
action = mov_duckiebot(_key)
# Se retorna la observación (obs), la evaluación (reward), etc
obs, reward, done, info = env.step(action)
# obs consiste en un imagen RGB de 640 x 480 x 3
dim = (160, 120)
# Se estandariza a imagen a las dimensiones que acepta el modelo
resized = cv2.resize(obs, dim, interpolation=cv2.INTER_AREA)
obs_ = np.expand_dims(resized, axis=0)
#Se ejecuta la predicción
_key = model.predict(obs_)
_key = np.argmax(_key[0])
print(_key)
# done significa que el Duckiebot chocó con un objeto o se salió del camino
if done:
print('done!')
# En ese caso se reinicia el simulador
env.reset()
print("env.cur_pose =")
print(env.cur_pos)
print("\n")
distance_to_road_center = lane_pose.dist
angle_from_straight_in_rads = lane_pose.angle_rad
print("dist = ", distance_to_road_center, " rad = ", angle_from_straight_in_rads)
speed = 0.3
if(abs(env.cur_pos[0] - p[io][0]) <= tol and abs(env.cur_pos[2] - p[io][1]) <= tol):
io = io + 1
if(io >= len(p)):
io = 0
obs, reward, done, info = env.step([speed, global_angle_arr(p, io)*2])
#obs, reward, done, info = env.step([speed, loca_angle(angle_from_straight_in_rads, distance_to_road_center)])
#obs, reward, done, info = env.step([speed, 0])
#total_reward += reward
print("info = ", info)
#print('Steps = %s, Timestep Reward=%.3f, Total Reward=%.3f' % (env.step_count, reward, total_reward))
env.render()
if done:
if reward < 0:
print('*** CRASHED ***')
print ('Final Reward = %.3f' % total_reward)
break
