def __init__(self):
# Get parameters
self.particle_filter_frame = \
rospy.get_param("~particle_filter_frame")
# Initialize the models
self.motion_model = MotionModel()
self.sensor_model = SensorModel()
def do_long_range_message(config, robot1, robot2, motion1, motion2):
"""Potentially transfer long range messages between robots.
Depending on whether the robots are within thresh distance, they will
exchange long range measurements. The long range measurements are sent in
the form of LongRangeMeasurement objects.
Args:
config: Configuration file.
robot1: First Robot object.
robot2: Second Robot object.
motion1: First RobotMotion object.
motion2: Second RobotMotion object.
"""
# If within long range, exchange long range sensor measurements.
if dist(motion1.pos, motion2.pos) < config['long_thresh']:
# Get message data from each robot.
message1to2_data = robot1.get_long_range_message()
message2to1_data = robot2.get_long_range_message()
# Create the sensor model, which will be used to make all measurements.
sm = SensorModel(config)
# Compute the pairwise sensor measurements between each robot.
num_sensors = len(config['sensor_parameters'])
robot1_measurements = [
sm.get_measurement(motion1, i, motion2) for i in range(num_sensors)
]
robot2_measurements = [
sm.get_measurement(motion2, i, motion1) for i in range(num_sensors)
]
logging.debug("Robot 1 Measurements: %s", robot1_measurements)
logging.debug("Robot 2 Measurements: %s", robot2_measurements)
# Stuff the data into the LongRangeMessages
message1to2 = LongRangeMessage(message1to2_data, robot2_measurements)
message2to1 = LongRangeMessage(message2to1_data, robot1_measurements)
# And then transmit both of the messages.
# Do it this way to ensure that receiving a message doesn't
# modify some state inside the robot.
robot1.receive_long_range_message(message2to1)
robot2.receive_long_range_message(message1to2)
# indicate whether these two robots communicated
return True
return False
def test_hyperparameter():
np.random.seed(10008)
map_obj = MapReader('../data/map/wean.dat')
occupancy_map = map_obj.get_map()
# generate a random particle, then sent out beams from that location
# indices = np.where(occupancy_map.flatten() == 0)[0]
# ind = np.random.choice(indices, 1)[0]
# y, x = ind // w, ind % w
# theta = -np.pi / 2
# angle = np.pi * (40 / 180)
X = init_particles_freespace(1, occupancy_map)
sensor = SensorModel(occupancy_map)
X = X.reshape(1, -1)
z_t_star = sensor.ray_casting(X[:, :3], num_beams=10)
print(z_t_star)
print(z_t_star.max())
z = np.arange(sensor._max_range + 2).astype(np.float)
p_hit, p_short, p_max, p_rand = sensor.estimate_density(z, z_t_star[0][5])
# plot(1, p_hit)
# plot(2, p_short)
# plot(3, p_max)
# plot(4, p_rand)
# w_hit = 3 # 99 / 2 / 2.5 / 4 # 1.
# w_short = 0.05 # 2 * 198 / 4 / 2.5 / 4 # 1
# w_max = 0.1 # 49 / 2.5 / 4 # 0.5
# w_rand = 10 # 990 / 4 # 5
# self._z_hit = 99 / 2 / 2.5 / 4 # 1.
# self._z_short = 198 / 4 // 2.5 / 4 # 1
# self._z_max = 49 / 2.5 / 4 # 0.5
# self._z_rand = 990 / 4 # 5
# w_hit = 1.
# w_short = 0.1
# w_max = 0.5
# w_rand = 10
w_hit = 1000 # 99 / 2 / 2.5 / 4 # 1.
w_short = 0.01 # 2 * 198 / 4 / 2.5 / 4 # 1
w_max = 0.03 # 49 / 4 / 4 # 0.5
w_rand = 12500
p = sensor._z_hit * p_hit + sensor._z_short * p_short + sensor._z_max * p_max + sensor._z_rand * p_rand
plot(5, p)
plt.show()
def __init__(self, map_file='../data/map/wean.dat'):
self.map = Map(map_file)
#self.map.display_gaussian([], 'Gaussian Blurred Map')
self._number_particles = 1000
self.particles = list()
self._particle_probabilities = []
for _ in range(0, self._number_particles):
print 'Initializing particle', _
particle = Particle(self.map)
#particle.x = 4080 + np.random.normal(scale=35)
#particle.y = 3980 + np.random.normal(scale=15)
#particle.theta = math.pi + 0.1 + np.random.normal(scale=.1)
self.particles.append(particle)
self._particle_probabilities.append(1)
self._motion_model = MotionModel(0.001, 0.001, 0.1, 0.1)
self._sensor_model = SensorModel(self.map)
self._ranges = []
def __init__(self):
# Get parameters
self.particle_filter_frame = \
rospy.get_param("~particle_filter_frame")
# Initialize publishers/subscribers
#
# *Important Note #1:* It is critical for your particle
# filter to obtain the following topic names from the
# parameters for the autograder to work correctly. Note
# that while the Odometry message contains both a pose and
# a twist component, you will only be provided with the
# twist component, so you should rely only on that
# information, and *not* use the pose component.
scan_topic = rospy.get_param("~scan_topic", "/scan")
odom_topic = rospy.get_param("~odom_topic", "/odom")
self.laser_sub = rospy.Subscriber(scan_topic, LaserScan,
YOUR_LIDAR_CALLBACK, # TODO: Fill this in
queue_size=1)
self.odom_sub = rospy.Subscriber(odom_topic, Odometry,
YOUR_ODOM_CALLBACK, # TODO: Fill this in
queue_size=1)
# *Important Note #2:* You must respond to pose
# initialization requests sent to the /initialpose
# topic. You can test that this works properly using the
# "Pose Estimate" feature in RViz, which publishes to
# /initialpose.
self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped,
YOUR_POSE_INITIALIZATION_CALLBACK, # TODO: Fill this in
queue_size=1)
# *Important Note #3:* You must publish your pose estimate to
# the following topic. In particular, you must use the
# pose field of the Odometry message. You do not need to
# provide the twist part of the Odometry message. The
# odometry you publish here should be with respect to the
# "/map" frame.
self.odom_pub = rospy.Publisher("/pf/pose/odom", Odometry, queue_size = 1)
# Initialize the models
self.motion_model = MotionModel()
self.sensor_model = SensorModel()
def test_raycasting_vectorize():
np.random.seed(10008)
map_obj = MapReader('../data/map/wean.dat')
occupancy_map = map_obj.get_map()
# generate a random particle, then sent out beams from that location
h, w = occupancy_map.shape
indices = np.where(occupancy_map.flatten() == 0)[0]
ind = np.random.choice(indices, 1)[0]
y, x = ind // w, ind % w
theta = np.pi / 2
X = np.array([[x, y, theta]])
X = np.repeat(X, 2, axis=0)
X[:, :2] *= 10
num_beams = 180
sensor = SensorModel(occupancy_map)
z_t_star = sensor.ray_casting(X, num_beams=num_beams)
x0, y0 = X[0, :2]
angle = np.arange(num_beams) * (np.pi / num_beams)
angle = theta + angle - np.pi / 2
x1 = x0 + z_t_star * np.cos(angle)
y1 = y0 - z_t_star * np.sin(angle)
x0, y0 = x0 / 10, y0 / 10
x1, y1 = x1 / 10, y1 / 10
# plot figure
fig = plt.figure()
plt.imshow(occupancy_map)
plt.scatter(x0, y0, c='red')
plt.scatter(x1, y1, c='yellow')
print(f'(x0, y0): ({x0}, {y0}), (x1, y1): ({x1}, {y1})')
# plt.plot((x0, x1), (y0, y1), color='yellow')
plt.show()
print(z_t_star)
def __init__(self):
rospy.init_node('pf')
real_robot = False
# create instances of two helper objects that are provided to you
# as part of the project
self.particle_filter = ParticleFilter()
self.occupancy_field = OccupancyField()
self.TFHelper = TFHelper()
self.sensor_model = sensor_model = SensorModel(
model_noise_rate=0.5,
odometry_noise_rate=0.15,
world_model=self.occupancy_field,
TFHelper=self.TFHelper)
self.position_delta = None # Pose, delta from current to previous odometry reading
self.last_scan = None # list of ranges
self.odom = None # Pose, current odometry reading
self.x_y_spread = 0.3 # Spread constant for x-y initialization of particles
self.z_spread = 0.2 # Spread constant for z initialization of particles
self.n_particles = 150 # number of particles
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
rospy.Subscriber("odom", Odometry, self.update_position)
rospy.Subscriber("stable_scan", LaserScan, self.update_scan)
# publisher for the particle cloud for visualizing in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
config = {
'sensor_sigma': 0.1,
'sensor_parameters': [
{
'delta': [1, 0],
'orientation': 0,
'fov': 180,
},
{
'delta': [-1, 0],
'orientation': 180,
'fov': 180,
},
]
}
sm = SensorModel(config)
config1 = {
'start': [0, 0],
'sigma_initial': [[0.1, 0],
[0, 0.1]],
'id': 1
}
motion1 = RobotMotion(config1)
motion1.pos = np.array([0, 0])
motion1.th = math.radians(0)
motion2 = RobotMotion(config1)
motion2.pos = np.array([10, 0])
print(sm.get_measurement(motion1, 0, motion2))
def __init__(self):
# Get parameters
self.particle_filter_frame = rospy.get_param("~particle_filter_frame")
self.MAX_PARTICLES = int(rospy.get_param("~num_particles"))
self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles"))
self.PUBLISH_ODOM = bool(rospy.get_param("~publish_odom", "1"))
self.ANGLE_STEP = int(rospy.get_param("~angle_step"))
self.DO_VIZ = bool(rospy.get_param("~do_viz"))
# MCL algorithm
self.iters = 0
self.lidar_initialized = False
self.odom_initialized = False
self.map_initialized = False
self.last_odom_pose = None # last received odom pose
self.last_stamp = None
self.laser_angles = None
self.downsampled_angles = None
self.state_lock = Lock()
# paritcles
self.inferred_pose = None
self.particles = np.zeros((self.MAX_PARTICLES, 3))
self.particle_indices = np.arange(self.MAX_PARTICLES)
self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES)
# Initialize the models
self.motion_model = MotionModel()
self.sensor_model = SensorModel()
# initialize the state
self.smoothing = Utils.CircularArray(10)
self.timer = Utils.Timer(10)
self.initialize_global()
# map
self.permissible_region = None
self.SHOW_FINE_TIMING = False
# these topics are for visualization
self.pose_pub = rospy.Publisher("/pf/viz/inferred_pose",
PoseStamped,
queue_size=1)
self.particle_pub = rospy.Publisher("/pf/viz/particles",
PoseArray,
queue_size=1)
self.pub_fake_scan = rospy.Publisher("/pf/viz/fake_scan",
LaserScan,
queue_size=1)
self.path_pub = rospy.Publisher('/pf/viz/path', Path, queue_size=1)
self.path = Path()
if self.PUBLISH_ODOM:
self.odom_pub = rospy.Publisher("/pf/pose/odom",
Odometry,
queue_size=1)
# these topics are for coordinate space things
self.pub_tf = tf.TransformBroadcaster()
# these topics are to receive data from the racecar
self.laser_sub = rospy.Subscriber(rospy.get_param(
"~scan_topic", "/scan"),
LaserScan,
self.callback_lidar,
queue_size=1)
self.odom_sub = rospy.Subscriber(rospy.get_param(
"~odom_topic", "/odom"),
Odometry,
self.callback_odom,
queue_size=1)
self.pose_sub = rospy.Subscriber("/initialpose",
PoseWithCovarianceStamped,
self.clicked_pose,
queue_size=1)
self.click_sub = rospy.Subscriber("/clicked_point",
PointStamped,
self.clicked_pose,
queue_size=1)
print "Finished initializing, waiting on messages..."
parser.add_argument('--path_to_log', default='../data/log/robotdata1.log')
parser.add_argument('--output', default='results')
parser.add_argument('--num_particles', default=500, type=int)
parser.add_argument('--visualize', action='store_true')
args = parser.parse_args()
src_path_map = args.path_to_map
src_path_log = args.path_to_log
os.makedirs(args.output, exist_ok=True)
map_obj = MapReader(src_path_map)
occupancy_map = map_obj.get_map()
logfile = open(src_path_log, 'r')
motion_model = MotionModel()
sensor_model = SensorModel(occupancy_map)
resampler = Resampling()
map_model = ReadTable()
ray_map = map_model.return_Map()
print(ray_map.shape)
# ray_map = 0
bool_occ_map = (occupancy_map < 0.35) & (occupancy_map >= 0)
num_particles = args.num_particles
initial_num = num_particles
# X_bar = init_particles_random(num_particles, occupancy_map)
X_bar = init_particles_freespace(num_particles, occupancy_map)
# initialize mesh map module
print('Load mesh map and initialize map module...')
map_module = MapModule(map_poses, map_file)
# initialize particles
print('Monte Carlo localization initializing...')
map_size, road_coords = gen_coords_given_poses(map_poses)
if config['pose_tracking']:
particles = init_particles_pose_tracking(numParticles,
poses[start_idx])
else:
particles = init_particles_given_coords(numParticles, road_coords)
# initialize sensor model
sensor_model = SensorModel(map_module, scan_folder, config['range_image'])
if config['range_image']['render_instanced']:
update_weights = sensor_model.update_weights_instanced
else:
update_weights = sensor_model.update_weights
# generate odom commands
commands = gen_commands(poses)
# initialize a visualizer
if visualize:
plt.ion()
visualizer = Visualizer(map_size,
poses,
map_poses,
grid_res=grid_res,
def __init__(
self,
publish_tf,
n_particles,
n_viz_particles,
odometry_topic,
motor_state_topic,
servo_state_topic,
scan_topic,
laser_ray_step,
exclude_max_range_rays,
max_range_meters,
speed_to_erpm_offset,
speed_to_erpm_gain,
steering_angle_to_servo_offset,
steering_angle_to_servo_gain,
car_length,
):
"""
Initializes the particle filter
publish_tf: Whether or not to publish the tf. Should be false in sim, true on real robot
n_particles: The number of particles
n_viz_particles: The number of particles to visualize
odometry_topic: The topic containing odometry information
motor_state_topic: The topic containing motor state information
servo_state_topic: The topic containing servo state information
scan_topic: The topic containing laser scans
laser_ray_step: Step for downsampling laser scans
exclude_max_range_rays: Whether to exclude rays that are beyond the max range
max_range_meters: The max range of the laser
speed_to_erpm_offset: Offset conversion param from rpm to speed
speed_to_erpm_gain: Gain conversion param from rpm to speed
steering_angle_to_servo_offset: Offset conversion param from servo position to steering angle
steering_angle_to_servo_gain: Gain conversion param from servo position to steering angle
car_length: The length of the car
"""
self.PUBLISH_TF = publish_tf
# The number of particles in this implementation, the total number of particles is constant.
self.N_PARTICLES = n_particles
self.N_VIZ_PARTICLES = n_viz_particles # The number of particles to visualize
# Cached list of particle indices
self.particle_indices = np.arange(self.N_PARTICLES)
# Numpy matrix of dimension N_PARTICLES x 3
self.particles = np.zeros((self.N_PARTICLES, 3))
# Numpy matrix containing weight for each particle
self.weights = np.ones(self.N_PARTICLES) / float(self.N_PARTICLES)
# A lock used to prevent concurrency issues. You do not need to worry about this
self.state_lock = Lock()
self.tfl = tf.TransformListener(
) # Transforms points between coordinate frames
# Get the map
map_msg = rospy.wait_for_message(MAP_TOPIC, OccupancyGrid)
self.map_info = map_msg.info # Save info about map for later use
# Create numpy array representing map for later use
array_255 = np.array(map_msg.data).reshape(
(map_msg.info.height, map_msg.info.width))
self.permissible_region = np.zeros_like(array_255, dtype=bool)
# Numpy array of dimension (map_msg.info.height, map_msg.info.width), with values 0: not permissible, 1: permissible
self.permissible_region[array_255 == 0] = 1
# Globally initialize the particles
# Publish particle filter state
# Used to create a tf between the map and the laser for visualization
self.pub_tf = tf.TransformBroadcaster()
# Publishes the expected pose
self.pose_pub = rospy.Publisher(PUBLISH_PREFIX + "/inferred_pose",
PoseStamped,
queue_size=1)
# Publishes a subsample of the particles
self.particle_pub = rospy.Publisher(PUBLISH_PREFIX + "/particles",
PoseArray,
queue_size=1)
# Publishes the most recent laser scan
self.pub_laser = rospy.Publisher(PUBLISH_PREFIX + "/scan",
LaserScan,
queue_size=1)
# Publishes the path of the car
self.pub_odom = rospy.Publisher(PUBLISH_PREFIX + "/odom",
Odometry,
queue_size=1)
rospy.sleep(1.0)
self.initialize_global()
# An object used for resampling
self.resampler = ReSampler(self.particles, self.weights,
self.state_lock)
# An object used for applying sensor model
self.sensor_model = SensorModel(
scan_topic,
laser_ray_step,
exclude_max_range_rays,
max_range_meters,
map_msg,
self.particles,
self.weights,
car_length,
self.state_lock,
)
# An object used for applying kinematic motion model
self.motion_model = KinematicMotionModel(
motor_state_topic,
servo_state_topic,
speed_to_erpm_offset,
speed_to_erpm_gain,
steering_angle_to_servo_offset,
steering_angle_to_servo_gain,
car_length,
self.particles,
self.state_lock,
)
self.permissible_x, self.permissible_y = np.where(
self.permissible_region == 1)
# Parameters/flags/vars for global localization
self.global_localize = False
self.global_suspend = False
self.ents = None
self.ents_sum = 0.0
self.noisy_cnt = 0
# number of regions to partition. Simulation: 25, Real: 5.
self.NUM_REGIONS = 25
# number of updates for regional localization. Simulation 5, Real: 3.
self.REGIONAL_ROUNDS = 5
self.regions = []
self.click_mode = True
self.debug_mode = False
if self.debug_mode:
self.global_localize = True # True when doing global localization
# precompute regions. Each region is represented by arrays of x, y indices on the map
region_size = len(self.permissible_x) / self.NUM_REGIONS
idx = np.argsort(self.permissible_y) # column-major
_px, _py = self.permissible_x[idx], self.permissible_y[idx]
for i in range(self.NUM_REGIONS):
self.regions.append((
_px[i * region_size:(i + 1) * region_size],
_py[i * region_size:(i + 1) * region_size],
))
# Subscribe to the '/initialpose' topic. Publised by RVIZ. See clicked_pose_cb function in this file for more info
# three different reactions to click on rviz
self.click_sub = rospy.Subscriber(
"/initialpose",
PoseWithCovarianceStamped,
self.clicked_pose_cb,
queue_size=1,
)
self.init_sub = rospy.Subscriber("/initialpose",
PoseWithCovarianceStamped,
self.reinit_cb,
queue_size=1)
rospy.wait_for_message(scan_topic, LaserScan)
print("Initialization complete")
def __init__(self):
# using locks so threads can't access self.particles at the same time (avoiding race condition)
self.lock = Lock()
self.lock.acquire()
# Get parameters
self.particle_filter_frame = \
rospy.get_param("~particle_filter_frame") # should be '/base_link_pf' in sim and '/base_link' on car
# Initialize the models
self.motion_model = MotionModel()
self.sensor_model = SensorModel()
# Topics
self.odom_topic = rospy.get_param("~odom_topic")
self.scan_topic = rospy.get_param("~scan_topic")
self.particle_marker_topic = '/particle_marker'
self.click_topic = '/move_base_simple/goal'
self.pose_array_topic = '/pose_array'
self.inferred_pose_topic = '/inferred_pose'
self.error_x_topic = '/error_x'
self.error_y_topic = '/error_y'
self.error_t_topic = '/error_t'
# Subscribers and Publishers
rospy.Subscriber(
self.odom_topic, Odometry,
self.odom_callback) # '/odom' for sim and 'vesc/odom' for car
rospy.Subscriber(self.scan_topic, LaserScan, self.scan_callback)
self.tf_broadcaster = TransformBroadcaster()
# Listening to clicks in rviz and publishing particle markers to rviz
rospy.Subscriber(self.click_topic, PoseStamped, self.click_callback)
self.particle_marker_pub = rospy.Publisher(self.particle_marker_topic,
Marker,
queue_size=1)
self.pose_array_pub = rospy.Publisher(self.pose_array_topic,
PoseArray,
queue_size=1)
self.inferred_pose_pub = rospy.Publisher(self.inferred_pose_topic,
Marker,
queue_size=1)
self.error_x_pub = rospy.Publisher(self.error_x_topic,
Float32,
queue_size=1)
self.error_y_pub = rospy.Publisher(self.error_y_topic,
Float32,
queue_size=1)
self.error_t_pub = rospy.Publisher(self.error_t_topic,
Float32,
queue_size=1)
self.rate = rospy.Rate(30) # should be greater than 20 Hz
# Implement the MCL algorithm
# using the sensor model and the motion model
#
# Make sure you include some way to initialize
# your particles, ideally with some sort
# of interactive interface in rviz
#
# Publish a transformation frame between the map
# and the particle_filter_frame.
# Initializing particles and weights
self.num_particles = rospy.get_param('~num_particles')
# currently intializing all particles as origin
self.particles = np.zeros((self.num_particles, 3))
# all particles are initially equally likely
self.weights = np.full((self.num_particles), 1. / self.num_particles)
# for error calculations
self.groundtruth = [0, 0, 0]
# send drive commands to test in sim
self.drive_pub = rospy.Publisher('/drive',
AckermannDriveStamped,
queue_size=1)
self.drive_msg = AckermannDriveStamped()
self.drive_msg.drive.steering_angle = 0
self.drive_msg.drive.speed = 1
if self.SIM == False: # don't toggle on driving if working on real robot
self.DRIVE = False
self.publish_transform()
self.lock.release()
def setUp(self):
self.sensor_model = SensorModel(map)
self.particle = Particle(map)
self.particle.x = 250
self.particle.y = 250
self.particle.theta = 0
def init_world_once(self, do_test=True):
# create a blank world, PRM style
num_nodes = self.config["num_nodes"]
connection_radius = self.config["connection_radius"]
environment_size = self.config["environment_size"]
self.sensor_range = self.config["sensor_range"]
# vertices
self.vertices_surface = []
self.vertices = []
count = 0
for vertex_idx in xrange(num_nodes):
x = random.uniform(0, environment_size[0])
y = random.uniform(0, environment_size[1])
for z in [-10, self.surface_level]:
v = Vertex(x, y, z, count)
count += 1
self.vertices.append(v)
if z == self.surface_level:
self.vertices_surface.append(True)
else:
self.vertices_surface.append(False)
# edges, stored as a matrix indexed as [vertex_start, vertex_end]
self.num_nodes = len(
self.vertices
) #doubled the input num_nodes in creating two layers of vertices instead of one
self.edge_matrix = [None] * self.num_nodes
self.edge_adjacency_idx_lists = [None] * self.num_nodes
self.edge_adjacency_edge_lists = [None] * self.num_nodes
for vertex_start_idx in xrange(self.num_nodes):
self.edge_matrix[vertex_start_idx] = [None] * self.num_nodes
self.edge_adjacency_idx_lists[vertex_start_idx] = []
self.edge_adjacency_edge_lists[vertex_start_idx] = []
for vertex_end_idx in xrange(self.num_nodes):
cost = distance(self.vertices[vertex_start_idx],
self.vertices[vertex_end_idx])
if vertex_start_idx != vertex_end_idx and cost <= connection_radius and not (
self.vertices_surface[vertex_start_idx]
and self.vertices_surface[vertex_end_idx]):
exists = True
else:
exists = False
edge = Edge(vertex_start_idx, vertex_end_idx, cost, exists)
self.edge_matrix[vertex_start_idx][vertex_end_idx] = edge
if exists:
self.edge_adjacency_idx_lists[vertex_start_idx].append(
vertex_end_idx)
self.edge_adjacency_edge_lists[vertex_start_idx].append(
edge)
if do_test:
self.test_indices()
#self.vertex_target_idx = self.create_target_idx() #single robot
# Define prior distribution
self.num_classes = self.config["num_classes"]
self.prob_of_class0 = self.config["prob_of_class0"]
self.prob_of_other_classes = (1 - self.prob_of_class0) / (
self.num_classes - 1)
self.prior = np.zeros(self.num_classes) #p_y0, p_y1, ...
for i in range(self.num_classes):
if i == 0:
self.prior[i] = self.prob_of_class0
else:
self.prior[i] = self.prob_of_other_classes
self.randomize_targets()
self.original_classes_y = copy.copy(self.classes_y)
# Define single random drop-off location (on surface) per world
temp = random.randint(0, len(self.vertices) - 1)
while self.vertices_surface[temp] != True:
temp = random.randint(0, len(self.vertices) - 1)
self.drop_off_idx = temp
'''
# Set all class 2 vertices to is_armed = True
self.is_armed_array = np.zeros(self.num_nodes, dtype=bool) #default, False for all
for v in xrange(len(self.classes_y)):
if self.classes_y[v] == 2: #it is a mine
self.is_armed_array[v] = True
'''
'''
# Vertex classes
# Each vertex has a number between 0 and n, which represents the class of the vertex
num_v_y0 = int(self.num_nodes*self.prob_of_class0)
num_v_other = self.num_nodes - num_v_y0
num_v_each = num_v_other/(self.num_classes-1)
classes_y = []
classes_y.extend([0]*num_v_y0) # add the non-target vertex classes
for i in range(1,self.num_classes):
classes_y.extend([i]*num_v_each) # add the target vertex classes
# G round truth vertex classes
random.shuffle(classes_y) #shuffle the list to randomize location of targets
'''
self.comms_range = self.config["comms_range"]
self.vertices_in_comms_range = self.generateCommsRangeVertices()
# Setup sensor model
self.sensor_model = SensorModel(self.config, self.num_nodes, self)