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