def __init__(self):
# Fetch parameters
self.map_topic = rospy.get_param("~map_topic")
self.num_beams_per_particle = rospy.get_param("~num_beams_per_particle")
self.scan_theta_discretization = rospy.get_param("~scan_theta_discretization")
self.scan_field_of_view = rospy.get_param("~scan_field_of_view")
####################################
# TODO
# Precompute the sensor model here
# (You should probably write a
# function for this)
####################################
# Create a simulated laser scan
self.scan_sim = PyScanSimulator2D(
self.num_beams_per_particle,
self.scan_field_of_view,
0, # This is not the simulator, don't add noise
0.01, # This is used as an epsilon
self.scan_theta_discretization)
# Subscribe to the map
self.map_set = False
rospy.Subscriber(
self.map_topic,
OccupancyGrid,
self.map_callback,
queue_size=1)
def __init__(self):
self.save_path = os.path.join(rospy.get_param("~save_path"), time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") #%Y-%m-%d-%H-%M-%S
self.trajectory = LineTrajectory("/built_trajectory")
'''
Insert appropriate subscribers/publishers here
'''
self.data_points = []
self.count = 0
self.num_waypoints = 4
self.waypoints = []
self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=10)
self.traj_pub = rospy.Publisher("/trajectory/current", PolygonStamped, queue_size=10)
self.trajectory_points = rospy.Publisher("/traj_pts", Marker, queue_size=20)
self.trajectory.publish_viz() #duration=40.0
self.rtt_pub = rospy.Publisher("/rtt/final", Marker, queue_size = 10)
self.rtt_tree_pub = rospy.Publisher("/rtt/tree", Marker, queue_size = 10000)
# save the built trajectory on shutdown
rospy.on_shutdown(self.saveTrajectory)
# Create a simulated laser scan
self.scan_sim = PyScanSimulator2D(
2, # number of beams to send out
0, # field of view centered around theta = 0
0, # don't add noise
0.01, # used as an epsilon
500) # discretize the theta space for faster ray tracing
#subscribe to map
self.map_set = False
self.permissible_region = None
self.resolution = None
self.origin = None
self.width = None
self.height = None
self.permissible_indices = None
rospy.Subscriber(
"/map",
OccupancyGrid,
self.map_callback,
queue_size = 1)
self.rrt = RRT(self.rtt_tree_pub,self.scan_sim)
def __init__(self):
# Fetch parameters
self.map_topic = rospy.get_param("~map_topic")
self.num_beams_per_particle = rospy.get_param(
"~num_beams_per_particle")
self.scan_theta_discretization = rospy.get_param(
"~scan_theta_discretization")
self.scan_field_of_view = rospy.get_param("~scan_field_of_view")
####################################
# TODO
# Adjust these parameters
self.alpha_hit = 0
self.alpha_short = 0
self.alpha_max = 0
self.alpha_rand = 0
self.sigma_hit = 0
# Your sensor table will be a `table_width` x `table_width` np array:
self.table_width = 201
####################################
# Precompute the sensor model table
self.sensor_model_table = None
self.precompute_sensor_model()
# Create a simulated laser scan
self.scan_sim = PyScanSimulator2D(
self.num_beams_per_particle,
self.scan_field_of_view,
0, # This is not the simulator, don't add noise
0.01, # This is used as an epsilon
self.scan_theta_discretization)
# Subscribe to the map
self.map = None
self.map_set = False
rospy.Subscriber(self.map_topic,
OccupancyGrid,
self.map_callback,
queue_size=1)
def __init__(self):
# Fetch parameters
self.map_topic = rospy.get_param("~map_topic")
self.num_beams_per_particle = rospy.get_param("~num_beams_per_particle")
self.scan_theta_discretization = rospy.get_param("~scan_theta_discretization")
self.scan_field_of_view = rospy.get_param("~scan_field_of_view")
self.INV_SQUASH_FACTOR = 1.0 / float(rospy.get_param("~squash_factor"))
####################################
# Precompute the sensor model here
# (You should probably write a
# function for this)
self.MAX_RANGE_PX = 400
self.LIDAR_SCALE_TO_MAP_SCALE = rospy.get_param("~lidar_scale_to_map_scale", 1.0)
self.sensor_model_table = None
self.particle_scans = None
self.precompute_sensor_model()
####################################
# Create a simulated laser scan
self.scan_sim = PyScanSimulator2D(
self.num_beams_per_particle,
self.scan_field_of_view,
0, # This is not the simulator, don't add noise
0.01, # This is used as an epsilon
self.scan_theta_discretization)
# Subscribe to the map
self.map_set = False
self.map = None
self.permissible_region = None
self.map_info = None
rospy.Subscriber(
self.map_topic,
OccupancyGrid,
self.map_callback,
queue_size=1)
def __init__(self):
self.on_robot = rospy.get_param("~on_robot")
# Fetch parameters
self.map_topic = rospy.get_param("~map_topic")
#self.num_beams_per_particle = rospy.get_param("~num_beams_per_particle")
if self.on_robot == 1:
self.num_beams_per_particle = 109 #1081
else:
self.num_beams_per_particle = 100
self.scan_theta_discretization = rospy.get_param(
"~scan_theta_discretization")
self.scan_field_of_view = rospy.get_param("~scan_field_of_view")
####################################
def p_hit(zt, z_star, z_max, sigma):
output = 1 / np.sqrt(
2 * np.pi * sigma**2) * np.e**(-(zt - z_star)**2 /
(2 * sigma**2))
mask = ((zt >= 0) & (zt <= z_max))
output[~mask] = 0
return output
def p_short(zt, z_star):
with np.errstate(divide='ignore', invalid='ignore'):
output = 2 / z_star * (1 - zt / z_star)
output[output == np.inf] = 0
output = np.nan_to_num(output)
mask = ((zt >= 0) & (zt <= z_star))
output[~mask] = 0
return output
def p_max(zt, z_max):
return (zt == z_max) * 1
def p_rand(zt, z_max):
output = np.ones((zt.shape[0], 1))
output = output / z_max
mask = ((zt >= 0) & (zt < z_max))
output[~mask] = 0
return output
def p(zt, z_star, z_max, sigma, a_hit, a_short, a_max, a_rand):
answer = a_hit * p_hit(zt, z_star, z_max, sigma) \
+ a_short * p_short(zt, z_star) \
+ a_max * p_max(zt, z_max) \
+ a_rand * p_rand(zt, z_max)
# print("Hit: ", p_hit(zt, z_star, z_max, sigma))
# print("Short: ", p_short(zt, z_star))
# print("Max: ", p_max(zt, z_max))
# print("Rand: ", p_rand(zt, z_max))
return answer
self.z_max = 10
sigma = 0.5
a_hit = rospy.get_param("~a_hit")
a_short = rospy.get_param("~a_short")
a_max = rospy.get_param("~a_max")
a_rand = rospy.get_param("~a_rand")
self.increment = 0.01
elements = np.arange(0, self.z_max + self.increment, self.increment)
pairs = np.array(np.meshgrid(elements, elements)).T.reshape(-1, 2)
zt = pairs[:, 0:
1] # Takes the first col of pairs (Ex. like the x value of a coordinate pair)
z_star = pairs[:, 1:
2] # Takes the second col of pairs (Ex. like the y value of a coordinate pair)
grid = p(zt, z_star, self.z_max, sigma, a_hit, a_short, a_max, a_rand)
grid = self.normalize(grid)
self.grid = grid.reshape((elements.shape[0], elements.shape[0])).T
####################################
# Create a simulated laser scan
self.scan_sim = PyScanSimulator2D(
self.num_beams_per_particle,
self.scan_field_of_view,
0, # This is not the simulator, don't add noise
0.01, # This is used as an epsilon
self.scan_theta_discretization)
# Subscribe to the map
self.map_set = False
rospy.Subscriber(self.map_topic,
OccupancyGrid,
self.map_callback,
queue_size=1)
def __init__(self):
# Fetch parameters
self.map_topic = rospy.get_param("~map_topic")
self.num_beams_per_particle = rospy.get_param("~num_beams_per_particle")
self.scan_theta_discretization = rospy.get_param("~scan_theta_discretization")
self.scan_field_of_view = rospy.get_param("~scan_field_of_view")
####################################
# TODO
# Precompute the sensor model here
# (You should probably write a
# function for this)
#with open('index_data.pkl', 'rb') as f: # *** MAKE SURE PKL FILE IS IN src DIRECTORY ***
with open('/home/racecar/racecar_ws/src/localization/sensor_model/index_data.pkl', 'rb') as f:
self.table = pickle.load(f) # lookup table for probabilities
# key = (zt, zt_star) = (measured distance, ground truth distance)
# value = probability(measured | ground truth)
def prob_lookup(zt, zt_star):
'''
look up probability given zt (measured) and zt_star (ground truth)
floor indices to match values stored in the table
-- zt
min: 0.5
max: 100
num_samples: 200
-- zt_star
min: 0.5
max: 100
num_samples: 200
:param zt: measured distance (m)
:param zt_star: ground truth distance
:return: probability (zt | zt_star)
'''
n = 200
zmax = 100
# TODO: use round() instead of math.floor()? Will it have an index out of range error if rounds up at max index?
zt_index = int(math.floor((zt - 0.5)*(n-1)/(zmax-0)))
zt_star_index = int(math.floor((zt_star-0.5)*(n-1)/(zmax-0.1)))
return self.table[zt_index][zt_star_index]
# self.prob_lookup is a function that can take a nested sequence of objects or numpy arrays as inputs and return a single numpy array as output
self.prob_lookup = np.vectorize(prob_lookup)
####################################
# Create a simulated laser scan
self.scan_sim = PyScanSimulator2D(
self.num_beams_per_particle,
self.scan_field_of_view,
0, # This is not the simulator, don't add noise
0.01, # This is used as an epsilon
self.scan_theta_discretization)
# Subscribe to the map
self.map_set = False
rospy.Subscriber(
self.map_topic,
OccupancyGrid,
self.map_callback,
queue_size=1)
rospy.loginfo('sensor model initialized')