def reset_state(self):
'''
Utility function for resetting internal states.
'''
with self.path_lock:
self.trajs = self.get_control_trajectories(self.T)
assert self.trajs.shape == (self.K, self.T, 2)
self.scaled = np.zeros((self.K * self.T, 3))
self.bbox_map = np.zeros((self.K * self.T, 2, 4))
self.perm = np.zeros(self.K * self.T).astype(np.int)
self.map = self.get_map()
self.perm_reg = self.load_permissible_region(self.map)
self.map_x = self.map.info.origin.position.x
self.map_y = self.map.info.origin.position.y
self.map_angle = utils.rosquaternion_to_angle(self.map.info.origin.orientation)
self.map_c = np.cos(self.map_angle)
self.map_s = np.sin(self.map_angle)
if self.ENABLE_DYNAMIC_TRAJ:
self.trajs_long = self.trajs
self.trajs_short = self.get_control_trajectories(self.Tshort)
self.scaled_long = self.scaled
self.bbox_map_long = self.bbox_map
self.perm_long = self.perm
self.scaled_short = self.scaled[:self.K*self.Tshort,:]
self.bbox_map_short = self.bbox_map[:self.K*self.Tshort,:,:]
self.perm_short = self.perm[:self.K*self.Tshort]
def reset_state(self):
with self.path_lock:
self.sampled_controls = self.sample_controls()
self.scaled = np.zeros((self.K * self.T, 3))
self.bbox_map = np.zeros((self.K * self.T, 2, 4))
self.perm = np.zeros(self.K * self.T).astype(np.int)
self.map = self.get_map()
self.perm_reg = self.load_permissible_region(self.map)
self.map_x = self.map.info.origin.position.x
self.map_y = self.map.info.origin.position.y
self.map_angle = utils.rosquaternion_to_angle(
self.map.info.origin.orientation)
self.map_c = np.cos(self.map_angle)
self.map_s = np.sin(self.map_angle)
def reset_state(self):
with self.path_lock:
self.sampled_controls = self.sample_controls()
self.scaled = np.zeros((self.K * self.T, 3))
self.bbox_map = np.zeros((self.K * self.T, 2, 4))
self.perm = np.zeros(self.K * self.T).astype(np.int)
self.map = self.get_map()
self.perm_reg = self.load_permissible_region(self.map)
self.map_x = self.map.info.origin.position.x
self.map_y = self.map.info.origin.position.y
self.map_angle = utils.rosquaternion_to_angle(self.map.info.origin.orientation)
self.map_c = np.cos(self.map_angle)
self.map_s = np.sin(self.map_angle)
L = self.car_length
W = self.car_width
# Specify specs of bounding box
self.bbox = np.array([
[L / 2.0, W / 2.0],
[L / 2.0, -W / 2.0],
[-L / 2.0, W / 2.0],
[-L / 2.0, -W / 2.0]
]) / (self.map.info.resolution)
def cb_pose(self, msg):
self.inferred_pose = [
msg.pose.position.x,
msg.pose.position.y,
utils.rosquaternion_to_angle(msg.pose.orientation)]
