def launch_env(map_name, randomize_maps_on_reset=False, domain_rand=False, frame_stacking=1): environment = DuckietownEnv(domain_rand=domain_rand, max_steps=math.inf, map_name=map_name, randomize_maps_on_reset=False) tmp = environment._get_tile if frame_stacking != 1: environment = FrameStack(environment, frame_stacking) environment._get_tile = tmp environment.reset() # Force reset to get fake frame buffer return environment
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))
# Definición del environment if args.env_name and args.env_name.find('Duckietown') != -1: env = DuckietownEnv( seed=args.seed, map_name=args.map_name, draw_curve=args.draw_curve, draw_bbox=args.draw_bbox, domain_rand=args.domain_rand, frame_skip=args.frame_skip, distortion=args.distortion, ) else: env = gym.make(args.env_name) # 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
parser.add_argument('--map-name', required=True) parser.add_argument('--no-random', action='store_true', help='disable domain randomization') parser.add_argument('--no-pause', action='store_true', help="don't pause on failure") args = parser.parse_args() if args.env_name == 'SimpleSim-v0': env = DuckietownEnv( map_name = args.map_name, domain_rand = not args.no_random ) #env.max_steps = math.inf env.max_steps = 500 else: env = gym.make(args.env_name) obs = env.reset() env.render() avg_frame_time = 0 max_frame_time = 0 def load_model(): global model model = Model() try: state_dict = torch.load('trained_models/imitate.pt', map_location=lambda storage, loc: storage) model.load_state_dict(state_dict) except: print('failed to load model')
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)
parser.add_argument('--seed', default=1, type=int, help='seed') args = parser.parse_args() env1 = DuckietownEnv( seed=args.seed, map_name=args.map_name, draw_curve=args.draw_curve, draw_bbox=args.draw_bbox, domain_rand=args.domain_rand, frame_skip=args.frame_skip, distortion=args.distortion, ) env1.do_color_relabeling = False env1.reset() env1.render() env2 = DuckietownEnv( seed=args.seed, map_name=args.map_name, draw_curve=args.draw_curve, draw_bbox=args.draw_bbox, domain_rand=args.domain_rand, frame_skip=args.frame_skip, distortion=args.distortion, ) env2.do_color_relabeling = True env2.reset()
domain_rand=False, camera_width=640, camera_height=480, accept_start_angle_deg=4, # start close to straight full_transparency=True, ) model = TensorflowModel( observation_shape=OBSERVATIONS_SHAPE, # from the logs we've got action_shape=ACTIONS_SHAPE, # same 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()
if args.env_name and args.env_name.find('Duckietown') != -1: env = DuckietownEnv(seed=None, map_name=args.map_name, draw_curve=args.draw_curve, draw_bbox=args.draw_bbox, domain_rand=False, user_tile_start=(3, 1), randomize_maps_on_reset=False, accept_start_angle_deg=5, frame_skip=args.frame_skip, distortion=args.distortion, full_transparency=True) else: env = gym.make(args.env_name) obs = env.reset() env.render() total_reward = 0 while True: action = agent.action obs, reward, done, info = env.step(action) total_reward += reward agent._publish_img(obs) env.render() if done:
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]
"""docstring forBufferReplay.""" def __init__(self, buffersize): super(BufferReplay, self).__init__() self.buffer = deque(maxlen=memory_size) def push(self, state, action, reward, next_state, done): self.buffer.append([state, action, reward, next_state, done]) def sample(self, batch_size): state, action, reward, next_state, done = zip(*random.sample(self.buffer, batch_size)) return state, action, reward, next_state, done def __len__(self): return len(self.buffer) replay_buffer = BufferReplay(memory_size) #initialise network and optimizers state = env.reset() DQN = DeepQNet(state).cuda() DQN_optimizer = optim.Adam(DQN.parameters(), lr = 0.0002) loss_list = [] #####------------training loop-----------------######### for i in range(total_episodes): step = 0 decay_step = 0 episode_rewards = [] state = env.reset() state = state.transpose((2, 0, 1))#convert from HWC to CHW state, stacked_frames = stack_images(stacked_frames, state, True) while step < total_steps:
parser.add_argument('--no-pause', action='store_true', help="don't pause on failure") args = parser.parse_args() if args.env_name is None: env = DuckietownEnv(map_name=args.map_name, domain_rand=False, draw_bbox=False) buf_env = DuckietownEnv(map_name=args.map_name, domain_rand=False, draw_bbox=False) else: env = gym.make(args.env_name) obs = env.reset() buf_env.reset() env.render() speed = 0.1 total_reward = 0 #circle radius r = 0.07 #steering module steering = speed / r #to regulate the period k_T = 0.75 #period
def main(): # pulling out of the environment MAP_NAME = os.getenv('DTG_MAP') environment = os.environ.copy() def env_as_json(name): if not name in environment: msg = 'Could not find variable "%s"; I know:\n%s' % (name, json.dumps(environment, indent=4)) raise Exception(msg) return json.loads(environment[name]) DOMAIN_RAND = env_as_json('DTG_DOMAIN_RAND') EPISODES = env_as_json('DTG_EPISODES') STEPS_PER_EPISODE = env_as_json('DTG_STEPS_PER_EPISODE') challenge = environment['DTG_CHALLENGE'] LOG_DIR = environment['DTG_LOG_DIR'] camera_width = env_as_json('DTG_CAMERA_WIDTH') camera_height = env_as_json('DTG_CAMERA_HEIGHT') misc = {} # init of info field for additional gym data misc['challenge'] = challenge # if challenge in ["LF", "LFV"]: # logger.debug("Launching challenge: {}".format(challenge)) # from gym_duckietown.config import DEFAULTS # # MAP_NAME = DEFAULTS["challenges"][challenge] # misc["challenge"] = challenge # else: # pass # XXX: what if not? error? logger.debug("Using map: {}".format(MAP_NAME)) env_parameters = dict(map_name=MAP_NAME, max_steps=STEPS_PER_EPISODE * EPISODES, domain_rand=DOMAIN_RAND, camera_width=camera_width, camera_height=camera_height, ) env = DuckietownEnv(**env_parameters) command_socket, command_poll = make_pull_socket() logger.debug("Simulator listening to incoming connections...") observations = env.reset() logger.debug('Logging gym state to: {}'.format(LOG_DIR)) data_logger = ROSLogger(logdir=LOG_DIR) min_nsteps = 10 MAX_FAILURES = 5 nfailures = 0 episodes = ['ep%03d' % _ for _ in range(EPISODES)] try: while episodes: if nfailures >= MAX_FAILURES: msg = 'Too many failures: %s' % nfailures raise Exception(msg) # XXX episode_name = episodes[0] logger.info('Starting episode %s' % episode_name) data_logger.start_episode(episode_name) data_logger.log_misc(env_parameters) try: nsteps = run_episode(env, data_logger, max_steps_per_episode=STEPS_PER_EPISODE, command_socket=command_socket, command_poll=command_poll, misc=misc) logger.info('Finished episode %s' % episode_name) if nsteps >= min_nsteps: logger.info('%d steps are enough' % nsteps) episodes.pop(0) else: logger.error('episode too short with %s steps' % nsteps) nfailures += 1 finally: data_logger.end_episode() finally: data_logger.close() logger.info('Simulation done.') misc['simulation_done'] = True send_gym( socket=Global.publisher_socket, img=observations, reward=0.0, done=True, misc=misc ) logger.info('Clean exit.')
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()