def __init__(self): logger.info('started __init__()') # Now, initialize the ROS stuff here: # logger.info('Configuring logging') uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) # roslaunch.configure_logging(uuid) # print('configured logging 2') our_launch_path = "custom_ws/src/codequackers/packages/pure_pursuit/launch/pure_pursuit.launch" roslaunch_path = os.path.join(os.getcwd(), our_launch_path) # roslaunch_path = os.path.join(os.getcwd(), "lf_slim.launch") logger.info('Creating ROSLaunchParent') self.launch = roslaunch.parent.ROSLaunchParent(uuid, [roslaunch_path]) logger.info('about to call start()') self.launch.start() logger.info('returning from start()') # Start the ROSAgent, which handles publishing images and subscribing to action logger.info('starting ROSAgent()') self.agent = ROSAgent() logger.info('started ROSAgent()') logger.info('completed __init__()')
def __init__(self, in_sim, launch_file): # Now, initialize the ROS stuff here: vehicle_name = os.getenv('VEHICLE_NAME') # The in_sim switch is used for local development # in that case, we do not start a launch file if not in_sim: # logger.info('Configuring logging') uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) # roslaunch.configure_logging(uuid) # print('configured logging 2') roslaunch_path = os.path.join(os.getcwd(), launch_file) logger.info('Creating ROSLaunchParent') self.launch = roslaunch.parent.ROSLaunchParent( uuid, [roslaunch_path]) logger.info('about to call start()') self.launch.start() logger.info('returning from start()') # Start the ROSAgent, which handles publishing images and subscribing to action logger.info('starting ROSAgent()') self.agent = ROSAgent() logger.info('started ROSAgent()') logger.info('completed __init__()')
def __init__(self): # Now, initialize the ROS stuff here: # uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) # roslaunch.configure_logging(uuid) # roslaunch_path = os.path.join(os.getcwd(), "template.launch") # self.launch = roslaunch.parent.ROSLaunchParent(uuid, [roslaunch_path]) # self.launch.start() # Start the ROSAgent, which handles publishing images and subscribing to action self.agent = ROSAgent()
class ROSTemplateAgent: def __init__(self): # Now, initialize the ROS stuff here: # uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) # roslaunch.configure_logging(uuid) # roslaunch_path = os.path.join(os.getcwd(), "template.launch") # self.launch = roslaunch.parent.ROSLaunchParent(uuid, [roslaunch_path]) # self.launch.start() # Start the ROSAgent, which handles publishing images and subscribing to action self.agent = ROSAgent() def init(self, context: Context): context.info("init()") def on_received_seed(self, context: Context, data: int): np.random.seed(data) def on_received_episode_start(self, context: Context, data: EpisodeStart): context.info("Starting episode %s." % data) def on_received_observations(self, context: Context, data: Duckiebot1Observations): jpg_data = data.camera.jpg_data obs = jpg2rgb(jpg_data) self.obs_to_agent(obs) def obs_to_agent(self, obs): self.agent._publish_img(obs) self.agent._publish_info() def on_received_get_commands(self, context: Context, data: GetCommands): if not self.agent.initialized: pwm_left, pwm_right = [0, 0] else: # TODO: let's use a queue here. Performance suffers otherwise. # What you should do is: *get the last command*, if available # otherwise, wait for one command. while not self.agent.updated: time.sleep(0.01) pwm_left, pwm_right = self.agent.action self.agent.updated = False grey = RGB(0.5, 0.5, 0.5) led_commands = LEDSCommands(grey, grey, grey, grey, grey) pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right) commands = Duckiebot1Commands(pwm_commands, led_commands) context.write("commands", commands) def finish(self, context): context.info("finish()")
def solve(gym_environment, cis): # python has dynamic typing, the line below can help IDEs with autocompletion assert isinstance(cis, ChallengeInterfaceSolution) # after this cis. will provide you with some autocompletion in some IDEs (e.g.: pycharm) cis.info('Creating model.') # you can have logging capabilties through the solution interface (cis). # the info you log can be retrieved from your submission files. # We get environment from the Evaluation Engine cis.info('Making environment') env = gym.make(gym_environment) # If you want to use a wrapper, just import it! # from utils.wrappers import ResizeWrapper # env = ResizeWrapper(env) # Then we make sure we have a connection with the environment and it is ready to go cis.info('Reset environment') observation = env.reset() # While there are no signal of completion (simulation done) # we run the predictions for a number of episodes, don't worry, we have the control on this part # We need to launch the ROS stuff in the background # ROSLaunch API doesn't play well with our environment setup, so we use subprocess import subprocess subprocess.Popen(["roslaunch lf_slim.launch"], shell=True) # Start the ROSAgent, which handles publishing images and subscribing to action agent = ROSAgent() r = rospy.Rate(15) while not rospy.is_shutdown(): # we passe the observation to our model, and we get an action in return # we tell the environment to perform this action and we get some info back in OpenAI Gym style # To trigger the lane following pipeline, we publish the image # and camera_infos to the correct topics defined in rosagent agent._publish_img(observation) agent._publish_info() # The action is updated inside of agent by other nodes asynchronously action = agent.action # Edge case - if the nodes aren't ready yet if np.array_equal(action, np.array([0, 0])): continue observation, reward, done, info = env.step(action) # here you may want to compute some stats, like how much reward are you getting # notice, this reward may no be associated with the challenge score. # it is important to check for this flag, the Evalution Engine will let us know when should we finish # if we are not careful with this the Evaluation Engine will kill our container and we will get no score # from this submission if 'simulation_done' in info: break if done: env.reset() # Run the main loop at 15Hz r.sleep()
class ROSTemplateAgent: def __init__(self): # Start the ROSAgent, which handles publishing images and subscribing to action self.agent = ROSAgent() def init(self, context: Context): context.info("init()") def on_received_seed(self, context: Context, data: int): np.random.seed(data) def on_received_episode_start(self, context: Context, data: EpisodeStart): context.info("Starting episode %s." % data) self.agent._publish_episode_start() def on_received_observations(self, context: Context, data: DB20Observations): jpg_data = data.camera.jpg_data obs = jpg2rgb(jpg_data) self.agent._publish_img(obs) self.agent._publish_info() odometry = data.odometry self.agent._publish_odometry(odometry.resolution_rad, odometry.axis_left_rad, odometry.axis_right_rad) def on_received_get_commands(self, context: Context, data: GetCommands): if not self.agent.initialized: pwm_left, pwm_right = [0, 0] else: # TODO: let's use a queue here. Performance suffers otherwise. # What you should do is: *get the last command*, if available # otherwise, wait for one command. while not self.agent.updated: time.sleep(0.01) pwm_left, pwm_right = self.agent.action self.agent.updated = False grey = RGB(0.5, 0.5, 0.5) led_commands = LEDSCommands(grey, grey, grey, grey, grey) pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right) commands = DB20Commands(pwm_commands, led_commands) context.write("commands", commands) def finish(self, context): context.info("finish()")
def launch_local_experiment(gym_environment=None): # Use our environment launcher env = launch_env(gym_environment) # If you want to use a wrapper, just import it! # from utils.wrappers import ResizeWrapper # env = ResizeWrapper(env) observation = env.reset() # While there are no signal of completion (simulation done) # we run the predictions for a number of episodes, don't worry, we have the control on this part # We need to launch the ROS stuff in the background # ROSLaunch API doesn't play well with our environment setup, so we use subprocess import subprocess subprocess.Popen(["roslaunch lf_slim.launch"], shell=True) # Start the ROSAgent, which handles publishing images and subscribing to action agent = ROSAgent() r = rospy.Rate(15) while not rospy.is_shutdown(): # we passe the observation to our model, and we get an action in return # we tell the environment to perform this action and we get some info back in OpenAI Gym style # To trigger the lane following pipeline, we publish the image # and camera_infos to the correct topics defined in rosagent agent._publish_img(observation) agent._publish_info() # The action is updated inside of agent by other nodes asynchronously action = agent.action # Edge case - if the nodes aren't ready yet if np.array_equal(action, np.array([0, 0])): continue observation, reward, done, info = env.step(action) # here you may want to compute some stats, like how much reward are you getting # notice, this reward may no be associated with the challenge score. # it is important to check for this flag, the Evalution Engine will let us know when should we finish # if we are not careful with this the Evaluation Engine will kill our container and we will get no score # from this submission if 'simulation_done' in info: break if done: env.reset() # Run the main loop at 15Hz r.sleep()
class ROSBaselineAgent(object): def __init__(self): logger.info('started __init__()') # Now, initialize the ROS stuff here: # logger.info('Configuring logging') uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) # roslaunch.configure_logging(uuid) # print('configured logging 2') roslaunch_path = os.path.join(os.getcwd(), "lf_slim.launch") logger.info('Creating ROSLaunchParent') self.launch = roslaunch.parent.ROSLaunchParent(uuid, [roslaunch_path]) logger.info('about to call start()') self.launch.start() logger.info('returning from start()') # Start the ROSAgent, which handles publishing images and subscribing to action logger.info('starting ROSAgent()') self.agent = ROSAgent() logger.info('started ROSAgent()') logger.info('completed __init__()') def on_received_seed(self, context, data): np.random.seed(data) def on_received_episode_start(self, context, data): context.info('Starting episode %s.' % data) def on_received_observations(self, context, data): logger.info("received observation") jpg_data = data['camera']['jpg_data'] obs = jpg2rgb(jpg_data) self.agent._publish_img(obs) self.agent._publish_info() def on_received_get_commands(self, context, data): while not self.agent.updated: time.sleep(0.01) pwm_left, pwm_right = self.agent.action self.agent.updated = False rgb = {'r': 0.5, 'g': 0.5, 'b': 0.5} commands = { 'wheels': { 'motor_left': pwm_left, 'motor_right': pwm_right }, 'LEDS': { 'center': rgb, 'front_left': rgb, 'front_right': rgb, 'back_left': rgb, 'back_right': rgb } } context.write('commands', commands) def finish(self, context): context.info('finish()')
def init(self, context: Context): context.info("init()") # Start the ROSAgent, which handles publishing images and subscribing to action self.agent = ROSAgent() context.info("inited")
class ROSTemplateAgent: last_camera_timestamp: float last_odometry_timestamp: float agent: ROSAgent def __init__(self): self.last_camera_timestamp = -1 self.last_odometry_timestamp = -1 def init(self, context: Context): context.info("init()") # Start the ROSAgent, which handles publishing images and subscribing to action self.agent = ROSAgent() context.info("inited") def on_received_seed(self, context: Context, data: int): np.random.seed(data) def on_received_episode_start(self, context: Context, data: EpisodeStart): context.info(f"Starting episode {data.episode_name}.") yaml_payload = getattr(data, "yaml_payload", "{}") self.agent.publish_episode_start(data.episode_name, yaml_payload) def on_received_observations(self, data: DB20ObservationsWithTimestamp, context: Context): camera = data.camera odometry = data.odometry # context.info(f'received obs camera {camera.timestamp} odometry {odometry.timestamp}') if camera.timestamp != self.last_camera_timestamp or True: self.agent.publish_img(camera.jpg_data, camera.timestamp) self.agent.publish_info(camera.timestamp) self.last_camera_timestamp = camera.timestamp if odometry.timestamp != self.last_odometry_timestamp or True: self.agent.publish_odometry(odometry.resolution_rad, odometry.axis_left_rad, odometry.axis_right_rad, odometry.timestamp) self.last_odometry_timestamp = odometry.timestamp def on_received_get_commands(self, context: Context, data: GetCommands): # context.info(f'on_received_get_commands') if not self.agent.initialized: pwm_left, pwm_right = [0, 0] else: # TODO: let's use a queue here. Performance suffers otherwise. # What you should do is: *get the last command*, if available # otherwise, wait for one command. t0 = time.time() while not self.agent.updated: dt = time.time() - t0 if dt > 2.0: context.info(f"agent not ready since {dt:.1f} s") time.sleep(0.5) if dt > 60: msg = "I have been waiting for commands from the ROS part" f" since {int(dt)} s" context.error(msg) raise Exception(msg) time.sleep(0.02) dt = time.time() - t0 if dt > 2.0: context.info(f"obtained agent commands after {dt:.1f} s") time.sleep(0.2) pwm_left, pwm_right = self.agent.action self.agent.updated = False grey = RGB(0.5, 0.5, 0.5) led_commands = LEDSCommands(grey, grey, grey, grey, grey) pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right) commands = DB20Commands(pwm_commands, led_commands) context.write("commands", commands) def finish(self, context): context.info("finish()") rospy.signal_shutdown("My job here is done.")
from gym_duckietown.envs import DuckietownEnv from gym_duckietown.simulator import Simulator from rosagent import ROSAgent def jpg2rgb(image_data): print(image_data) im = Image.open(io.BytesIO(image_data[10:])) im = im.convert('RGB') data = np.array(im) assert data.ndim == 3 assert data.dtype == np.uint8 return data agent = ROSAgent() parser = argparse.ArgumentParser() parser.add_argument('--env-name', default='Duckietown-udem1-v0') parser.add_argument('--map-name', default='udem1') parser.add_argument('--distortion', default=False, action='store_true') parser.add_argument('--draw-curve', action='store_true', help='draw the lane following curve') parser.add_argument('--draw-bbox', action='store_true', help='draw collision detection bounding boxes') parser.add_argument('--domain-rand', action='store_true', help='enable domain randomization') parser.add_argument('--frame-skip',
class ROSBaselineAgent(object): def __init__(self, in_sim, launch_file): # Now, initialize the ROS stuff here: vehicle_name = os.getenv('VEHICLE_NAME') # The in_sim switch is used for local development # in that case, we do not start a launch file if not in_sim: # logger.info('Configuring logging') uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) # roslaunch.configure_logging(uuid) # print('configured logging 2') roslaunch_path = os.path.join(os.getcwd(), launch_file) logger.info('Creating ROSLaunchParent') self.launch = roslaunch.parent.ROSLaunchParent( uuid, [roslaunch_path]) logger.info('about to call start()') self.launch.start() logger.info('returning from start()') # Start the ROSAgent, which handles publishing images and subscribing to action logger.info('starting ROSAgent()') self.agent = ROSAgent() logger.info('started ROSAgent()') logger.info('completed __init__()') def on_received_seed(self, context, data): logger.info('Received seed from pipes') np.random.seed(data) def on_received_episode_start(self, context, data): logger.info("Starting episode") context.info('Starting episode %s.' % data) def on_received_observations(self, context, data): logger.info("received observation") jpg_data = data['camera']['jpg_data'] obs = jpg2rgb(jpg_data) self.agent._publish_img(obs) self.agent._publish_info() def on_received_get_commands(self, context, data): logger.info("Agent received GetCommand request") while not self.agent.updated: time.sleep(0.01) pwm_left, pwm_right = self.agent.action if self.agent.started: # before starting, we send empty commnands to keep connection self.agent.updated = False rgb = {'r': 0.5, 'g': 0.5, 'b': 0.5} commands = { 'wheels': { 'motor_left': pwm_left, 'motor_right': pwm_right }, 'LEDS': { 'center': rgb, 'front_left': rgb, 'front_right': rgb, 'back_left': rgb, 'back_right': rgb } } context.write('commands', commands) def finish(self, context): context.info('finish()')
seed(args.seed) device = torch.device("cuda" if torch.cuda.is_available() else "cpu") env = launch_env() state_dim = env.observation_space.shape action_dim = env.action_space.shape[0] max_action = float(env.action_space.high[0]) # env = ActionWrapper(env) # env = DtRewardWrapper(env) # env = SteeringToWheelVelWrapper(env) # env = MotionBlurWrapper(env) rosagent = ROSAgent(dont_init_rl=True) ddpg_agent = rosagent.rl_policy rosagent.rl_policy = BurnInRLAgent() burnin_agent = BurnInRLAgent() random_agent = RandomAgent() def send_obs(obs): rosagent._publish_img(obs, wait_for_callback=True) def sample_action(obs): rl_action_clean = None if total_timesteps < args.exploration_timesteps: print("BURN IN")
def __init__(self): # Start the ROSAgent, which handles publishing images and subscribing to action self.agent = ROSAgent()