im.save('screen.png') if done1 or done2: print('done!') env1.reset() env2.reset() Car1.reset() env1.render() env2.render() env1.render() env2.render() # Save image from PIL import Image im1 = Image.fromarray(obs1) im2 = Image.fromarray(obs2) im1.save("img/car1/_" + str(i) + ".jpg") im2.save("img/car2/" + str(i) + ".jpg") i = i + 1 pyglet.clock.schedule_interval(update, 1.0 / env1.unwrapped.frame_rate) # Enter main event loop pyglet.app.run() env1.close() env2.close()
# 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() # Se muestra en una ventana llamada "patos" la observación del simulador cv2.imshow("patos", cv2.cvtColor(obs, cv2.COLOR_RGB2BGR)) # Se cierra el environment y termina el programa env.close()
def main(duckie_env: DuckietownEnv, debug: bool): """ Main loop that allows to control duckiebot with keyboard and uses visual servo when bot is detected Args: duckie_env: the environment in which our duckiebot evolves debug: will log debug message if True """ duckie_env.reset() duckie_env.render() logger = logging.getLogger(__name__) if debug: logger.setLevel("DEBUG") else: logger.setLevel("INFO") pose_estimator = PoseEstimator(min_area=CIRCLE_MIN_AREA, min_dist_between_blobs=CIRCLE_MIN_DISTANCE, height=CIRCLE_PATTERN_HEIGHT, width=CIRCLE_PATTERN_WIDTH, target_distance=TARGET_DIST, camera_mode=CAMERA_MODE) # This is the object that computes the next command from the estimated pose trajectory = Trajectory() @duckie_env.unwrapped.window.event def on_key_press(symbol, modifier): """ This handler processes keyboard commands that control the simulation Args: symbol: key pressed """ if symbol in [key.BACKSPACE, key.SLASH]: logger.info("RESET") trajectory.reset() duckie_env.reset() duckie_env.render() elif symbol == key.PAGEUP: duckie_env.unwrapped.cam_angle[0] = 0 elif symbol == key.ESCAPE: duckie_env.close() sys.exit(0) # Register a keyboard handler key_handler = key.KeyStateHandler() duckie_env.unwrapped.window.push_handlers(key_handler) def update(dt: float): """ This function is called at every frame to handle movement/stepping and redrawing Args: dt: change in time (in secs) since last update """ action = np.array([0.0, 0.0]) if key_handler[key.UP]: action += np.array([0.44, 0.0]) if key_handler[key.DOWN]: action -= np.array([0.44, 0]) if key_handler[key.LEFT]: action += np.array([0, 1]) if key_handler[key.RIGHT]: action -= np.array([0, 1]) # Speed boost if key_handler[key.LSHIFT]: action *= 1.5 # TODO get observation before taking step # For now, we do nothing, get image, compute action, execute it. So 2 steps for one action # It should be get observations, compute action, take step, repeat. (1 action/step) obs, reward, done, info = duckie_env.step(action) target_detected, estimated_pose = pose_estimator.get_pose(obs) # Only for debugging, slows things down considerably and is not necessary # if detect: # cv2.drawChessboardCorners(obs, # (8, 3), centers, detection) # im = Image.fromarray(obs) # im.save("circle_grid.png") # Here we get the ground_truth to see the accuracy of our estimate # Note: This is in global frame, while the estimate is in robot frame. # Also, the exact distance from the center of the duck to the bumper is unknown so it is set approximately goal_position = np.array([2.5 - TARGET_DIST - BUMPER_TO_CENTER_DIST, 0, 2.5]) # Because duckie is at [2.5, 0. 2.5] in visual_servo.yaml env file goal_angle = 0 # Because duckie faces 0 angle in visual_servo.yaml env file cur_position = np.array(info["Simulator"]["cur_pos"]) cur_angle_rad = info["Simulator"]["cur_angle"] cur_angle_deg = np.rad2deg(cur_angle_rad) if cur_angle_deg > 179: cur_angle_deg -= 360 relative_position = goal_position - cur_position relative_angle = goal_angle - cur_angle_deg relative_pose = [relative_position, relative_angle] np.set_printoptions(precision=2) logger.debug(f"gt: {relative_pose}, estimate: {estimated_pose}") if target_detected: trajectory.update(estimated_pose) elif trajectory.is_initialized(): trajectory.predict(dt) else: logger.warning("object not found, cannot compute initial trajectory") # TODO for now we can move the duckie with the arrows. Eventually we just want # to reset the environment, and maybe log the starting pose to plot where we detect or not. if trajectory.is_initialized(): action = trajectory.get_commands() obs, reward, done, info = duckie_env.step(action) if key_handler[key.RETURN]: im = Image.fromarray(obs) im.save("screen.png") if done: logger.info("done!") duckie_env.reset() duckie_env.render() duckie_env.render() pyglet.clock.schedule_interval(update, 1.0 / duckie_env.unwrapped.frame_rate) # Enter main event loop pyglet.app.run() duckie_env.close()
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()
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]
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()