def getLimpParametersStatic(pose_x, pose_y, current_vel, prev_traj_self,
prev_param_self):
# NEW
prev_traj = np.copy(prev_traj_self)
prev_param = np.copy(prev_param_self)
s_tot = np.linspace(0, prev_param[0], trajectory_generator.NUM_STEPS)
s_tot -= s_tot[0]
_, _, _, idx = pure_pursuit_utils.nearest_point_on_trajectory_py2(
np.array([pose_x, pose_y]), prev_traj[:, :2])
idx = min(idx, prev_traj.shape[0] - 3)
s_idx = s_tot[idx:]
s_idx -= s_idx[0]
temp_traj = np.empty((trajectory_generator.NUM_STEPS, 5))
temp_traj[:, :4] = trajectory_generator_utils.multiInterp2(
np.linspace(0, s_idx[-1], trajectory_generator.NUM_STEPS), s_idx,
prev_traj[idx:, :4])
temp_traj[:, 4] = np.concatenate([
np.geomspace(current_vel, 0.4, BRAKE_STEPS), 0.4 * np.ones(
(temp_traj.shape[0] - BRAKE_STEPS, ))
])
prev_traj = temp_traj
extra_limp_s = s_tot[idx]
prev_param[0] = s_idx[-1]
# END NEW
# OLD
#self.prev_traj[:,4] = np.concatenate([np.geomspace(self.prev_traj[0,4],0.4,BRAKE_STEPS), 0.4*np.ones((self.prev_traj.shape[0]-BRAKE_STEPS,))])
# END OLD
pp_traj = np.empty((prev_traj.shape[0], 4))
pp_traj[:, 0:2] = prev_traj[:, 0:2]
pp_traj[:, 2] = prev_traj[:, 4]
pp_traj[:, 3] = prev_traj[:, 2]
return pp_traj, prev_traj, prev_param, extra_limp_s
def _get_current_speed(self, position, theta):
wpts = self.waypoints[:, 0:2]
position = position[0:2]
nearest_point, nearest_dist, t, i = pure_pursuit_utils.nearest_point_on_trajectory_py2(
position, wpts)
speed = self.waypoints[i, 2]
return speed, nearest_dist
def getLimpParameters(self, pose_x, pose_y, current_vel):
s_tot = np.linspace(0, self.prev_param[0],
trajectory_generator.NUM_STEPS)
s_tot -= s_tot[0]
_, _, _, idx = pure_pursuit_utils.nearest_point_on_trajectory_py2(
np.array([pose_x, pose_y]), self.prev_traj[:, :2])
idx = min(idx, self.prev_traj.shape[0] - 3)
s_idx = s_tot[idx:]
s_idx -= s_idx[0]
temp_traj = np.empty((trajectory_generator.NUM_STEPS, 5))
temp_traj[:, :4] = trajectory_generator_utils.multiInterp2(
np.linspace(0, s_idx[-1], trajectory_generator.NUM_STEPS), s_idx,
self.prev_traj[idx:, :4])
temp_traj[:, 4] = np.concatenate([
np.geomspace(current_vel, 0.4, BRAKE_STEPS), 0.4 * np.ones(
(temp_traj.shape[0] - BRAKE_STEPS, ))
])
self.prev_traj = temp_traj
self.limp_s += s_tot[idx]
self.prev_param[0] = s_idx[-1]
self.is_limping = True
prev_traj_plot = self.prev_traj
pp_traj = np.empty((self.prev_traj.shape[0], 4))
pp_traj[:, 0:2] = self.prev_traj[:, 0:2]
pp_traj[:, 2] = self.prev_traj[:, 4]
pp_traj[:, 3] = self.prev_traj[:, 2]
return pp_traj, prev_traj_plot
def _get_current_waypoint(self, waypoints, lookahead_distance, position,
theta):
wpts = waypoints[:, 0:2]
nearest_point, nearest_dist, t, i = pure_pursuit_utils.nearest_point_on_trajectory_py2(
position, wpts)
if nearest_dist < lookahead_distance:
lookahead_point, i2, t2 = pure_pursuit_utils.first_point_on_trajectory_intersecting_circle(
position, lookahead_distance, wpts, i + t, wrap=True)
if i2 == None:
return None
current_waypoint = np.empty(waypoints[i2, :].shape)
current_waypoint[0:2] = waypoints[i2, 0:2]
current_waypoint[3] = waypoints[i2, 3]
current_waypoint[2] = waypoints[i2, 2]
return current_waypoint
elif nearest_dist < self.max_reacquire:
return waypoints[i, :]
else:
return None
def updateBeliefHelper(picked_idx_unique,glob_prev_s,glob_prev_opp_pose,waypoints): end_idx_start=np.searchsorted(glob_prev_s,glob_prev_s[0]+2,side='right') hi=range(end_idx_start,len(glob_prev_s)) if len(hi)==0: return None flow_samples=np.empty((len(hi),6)) for end_idx in range(end_idx_start,len(glob_prev_s)): knots=np.linspace(0,end_idx,4).astype(np.int32) knots=knots[1:] vels=np.empty((3,)) for i in range(knots.shape[0]): vels[i]=glob_prev_opp_pose[knots[i],3] _,min_dist,min_frac_t,min_i=pure_pursuit_utils.nearest_point_on_trajectory_py2(glob_prev_opp_pose[knots[-1],0:2],waypoints[:,0:2]) nearest_waypoint=waypoints[min_i] end_theta=glob_prev_opp_pose[knots[-1],2]-nearest_waypoint[3] end_s=glob_prev_s[knots[-1]]-glob_prev_s[0] vec_to_pt=glob_prev_opp_pose[knots[-1],0:2]-nearest_waypoint[0:2] wpt_pt=np.array([np.cos(end_theta),np.sin(end_theta)]) if cross(wpt_pt,vec_to_pt)<0: end_t=-min_dist else: end_t=min_dist flow_samples[end_idx-end_idx_start]=np.concatenate((np.array([end_s,end_t,end_theta]),vels)) return flow_samples