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