def __init__(self, args, silent=False):
self.worldstate = Worldstate()
map_ = CMap2D(args.map_folder, args.map_name, silent=silent)
for _ in range(args.map_downsampling_passes):
map_ = map_.as_coarse_map2d()
self.worldstate.map = map_
self.args = args
def __init__(self,
scenario='test',
silent=False,
legacy_mode=False,
adaptive=True,
lidar_legs=True,
collect_statistics=True):
# gym env definition
super(NavRepTrainEnv, self).__init__()
self.action_space = spaces.Box(low=-1,
high=1,
shape=(3, ),
dtype=np.float32)
self.observation_space = spaces.Tuple((spaces.Box(low=-np.inf,
high=np.inf,
shape=(1080, ),
dtype=np.float32),
spaces.Box(low=-np.inf,
high=np.inf,
shape=(5, ),
dtype=np.float32)))
# parameters
self.lidar_legs = lidar_legs
self.silent = silent
self.scenario = scenario
self.adaptive = adaptive
self.LEGACY_MODE = legacy_mode # used for testing the original SOADRL policy
self.collect_statistics = collect_statistics
# other tools
self.viewer = None
self.episode_statistics = None
if self.collect_statistics:
self.episode_statistics = DataFrame(columns=[
"total_steps",
"scenario",
"damage",
"steps",
"goal_reached",
"reward",
"num_agents",
"num_walls",
"wall_time",
])
self.total_steps = 0
self.steps_since_reset = None
self.episode_reward = None
# conversion variables
self.kLidarAngleIncrement = 0.00581718236208
self.kLidarMergedMinAngle = 0
self.kLidarMergedMaxAngle = 6.27543783188 + self.kLidarAngleIncrement
self.n_angles = 1080
self.lidar_scan = None
self.lidar_angles = None
self.border = None
self.converter_cmap2d = CMap2D()
self.converter_cmap2d.set_resolution(1.)
self.distances_travelled_in_base_frame = None
# environment
self._make_env(silent=self.silent)
def __init__(self):
# agent state
self._agentstate = {
"agents": [], # list of agent prototypes
"xystates": [], # [n_agents, 2]
"uvstates": [],
"innerstates": [],
}
# map state
self.map = CMap2D()
# others
self.derived_state = DerivedWorldstate()
def __init__(self,
map_folder,
map_filename,
reference_map_frame,
frame_to_track,
refmap_update_callback=default_refmap_update_callback):
self.tf_frame_in_refmap = None
self.map_ = None
self.lock = Lock() # for avoiding race conditions
self.refmap_is_dynamic = False
# update callback
self.refmap_update_callback = refmap_update_callback
# get frame info
self.kRefMapFrame = reference_map_frame
self.kFrame = frame_to_track
# Load map
if map_folder == "rostopic":
self.refmap_is_dynamic = True
rospy.logwarn(
"Getting reference map from topic '{}'".format(map_filename))
rospy.Subscriber(map_filename,
OccupancyGrid,
self.map_callback,
queue_size=1)
else:
# loads map based on ros params
folder = map_folder
filename = map_filename
try:
self.map_ = CMap2D(folder, filename)
except IOError as e:
rospy.logwarn(rospy.get_namespace())
rospy.logwarn(
"Failed to load reference map. Make sure {}.yaml and {}.pgm"
" are in the {} folder.".format(filename, filename,
folder))
rospy.logwarn(
"Disabling. No global localization or reference map will be available."
)
return
self.refmap_update_callback(self)
# launch callbacks
self.tf_listener = tf.TransformListener()
rospy.Timer(rospy.Duration(0.01), self.tf_callback)
self.has_last_failed = False
def map_callback(self, msg):
with self.lock:
self.map_ = CMap2D()
self.map_.from_msg(msg)
self.refmap_update_callback(self)
goals_reached.append(np.any(
np.linalg.norm(np.array(trajectory)[:,:2] - goal, axis=1) < GOAL_REACHED_DIST))
trajectory = []
# set goal
goal = goal_in_fix
if topic in map_topics:
p2_map_in_fix = Pose2D(bag_transformer.lookupTransform(
FIXED_FRAME, msg.header.frame_id, msg.header.stamp))
mapmsg = msg
fig, ax = plt.subplots(1, 1)
if mapmsg is not None:
map2d = CMap2D()
map2d.from_msg(mapmsg)
assert mapmsg.header.frame_id == FIXED_FRAME
contours = map2d.as_closed_obst_vertices()
for c in contours:
cplus = np.concatenate((c, c[:1, :]), axis=0)
ax.plot(cplus[:,0], cplus[:,1], color='k')
plt.axis('equal')
for t, g, s in zip(trajectories, goals, goals_reached):
line_color = blue(len(t)/1000.) # if s else orange(len(t)/1000.)
zorder = 2 if s else 1
if ANIMATE:
yanim = np.ones_like(t[:,1]) * np.nan
line, = ax.plot(t[:,0], yanim, color=line_color, zorder=zorder)
ax.add_artist(plt.Circle((g[0], g[1]), 0.3, color="red", zorder=2))