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()