class Robot():
def __init__(self):
self.config = read_config()
rospy.init_node('robot')
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.header.frame_id = 'map'
self.pose_array.poses = []
self.moves = self.config["move_list"]
#Particle update values to use
self.isFirstMove = True
self.first_sigma_x = read.config["first_move_sigma_x"]
self.first_sigma_y = read.config["first_move_sigma_y"]
self.first_sigma_angle = read.config["first_move_sigma_angle"]
self.resample_sigma_x = read.config["resample_sigma_x"]
self.resample_sigma_y = read.config["resample_sigma_y"]
self.resample_sigma_angle = read.config["resample_sigma_angle"]
laser_sub = rospy.Subscriber("/base_scan", LaserScan,
self.sensorUpdate)
self.likely_pub = rospy.Publisher("/likelihood_field",
OccupancyGrid,
queue_size=10,
latch=True)
self.pose_pub = rospy.Publisher("/particlecloud",
PoseArray,
queue_size=10)
map_sub = rospy.Subscriber("/map", OccupancyGrid, self.mapCallBack)
rospy.sleep(1)
self.width = self.map.width
self.height = self.map.height
# particle init
for i in range(0, self.config["num_particles"]):
x = random.randint(0, self.width)
y = random.randint(0, self.height)
theta = random.uniform(0, 2 * m.pi)
obstacle = self.map.get_cell(x, y)
if (obstacle < 0.5):
pose = get_pose(x, y, theta)
self.pose_array.poses.append(pose)
else:
i -= 1
rospy.sleep(1)
self.pose_pub.publish(self.pose_array)
self.moveRobot(self.moves)
rospy.spin()
def mapCallBack(self, message):
# 2
self.map = Map(message)
self.width = self.map.width
self.height = self.map.height
sig = self.config['laser_sigma_hit']
coordinates = []
queryPts = []
rospy.sleep(1)
# 3
for col in range(0, self.width):
for row in range(0, self.height):
x, y = self.map.cell_position(row, col)
cellVal = self.map.get_cell(x, y)
if (cellVal > 0.5):
coordinates.append([x, y])
self.kdt = KDTree(coordinates)
for col in range(0, self.width):
for row in range(0, self.height):
dist, ind = self.kdt.query([col, row], k=1)
gauss = (1 /
(sig * m.sqrt(2 * m.pi))) * ((m.e)**-(((dist)**2) /
(2 * (sig**2))))
self.map.set_cell(col, row, gauss)
self.likely_pub.publish(self.map.to_message())
def sensorUpdate(self, laser):
self.laserMsg = laser
def moveRobot(self, motion_cmd):
for i in range(0, len(self.moves)): #4
motion = self.moves[i]
a = motion[0]
d = motion[1]
n = motion[2]
move_function(a, 0)
for t in range(0, n):
move_function(0, d)
updateParticles(a, d)
def updateParticles(a, d):
new_pose_array = []
for j in range(0, self.config["num_particles"]):
particle = self.pose_array.poses[j]
x = particle[0]
y = particle[1]
theta = particle[2]
if (self.isFirstMove):
x = x + random.gauss(0, first_move_sigma_x)
y = y + random.gauss(0, first_move_sigma_y)
theta = theta + random.gauss(0, first_move_sigma_theta)
self.isFirstMove = False
x = x + d * math.cos(a)
y = y + d * math.sin(a)
theta = theta + a
pose = get_pose(x, y, theta)
new_pose_array.append(pose)
self.pose_array.poses = new_pose_array
#Reweight
def motionUpdate(self, motion_cmd, particle):
a = motion_cmd[0]
d = motion_cmd[1]
n = motion_cmd[2]
class Robot():
def __init__(self):
rospy.init_node("robot")
self.config = read_config()
#r.seed(self.config['seed'])
self.num_particles = self.config['num_particles']
self.laser_sigma_hit = self.config['laser_sigma_hit']
self.move_list = self.config['move_list']
self.move_list_length = len(self.move_list)
self.first_move_sigma_x = self.config['first_move_sigma_x']
self.first_move_sigma_y = self.config['first_move_sigma_y']
self.first_move_sigma_angle = self.config['first_move_sigma_angle']
self.laser_z_hit = self.config['laser_z_hit']
self.laser_z_rand = self.config['laser_z_rand']
self.resample_sigma_x = self.config['resample_sigma_x']
self.resample_sigma_y = self.config['resample_sigma_y']
self.resample_sigma_theta = self.config['resample_sigma_angle']
self.handle_map_first_called = 1
self.move_made = 0
self.map_data_sub = rospy.Subscriber(
"/map",
OccupancyGrid,
self.handle_map_data
)
self.base_scan_data = rospy.Subscriber(
"/base_scan",
LaserScan,
self.handle_base_scan_data
)
self.particle_pose_pub = rospy.Publisher(
"/particlecloud",
PoseArray,
queue_size = 10
)
self.likelihood_pub = rospy.Publisher(
"/likelihood_field",
OccupancyGrid,
queue_size = 1,
latch = True
)
self.result_update_pub = rospy.Publisher(
"/result_update",
Bool,
queue_size = 10
)
self.sim_complete_pub = rospy.Publisher(
"/sim_complete",
Bool,
queue_size = 10
)
self.scan_data_avai = 0
self.sensor_loop()
def sensor_loop(self):
while not rospy.is_shutdown():
if ( self.handle_map_first_called == 0):
#print "went in while(true) loop"
self.create_particles()
self.construct_field()
#print "finished construct field"
# publish OccupancyMap
grid_mssg = self.my_map.to_message()
rospy.sleep(1)
self.likelihood_pub.publish(grid_mssg)
self.make_all_moves()
break
rospy.sleep(1)
self.sim_complete_pub.publish(True)
rospy.sleep(1)
rospy.signal_shutdown(Robot)
def handle_base_scan_data (self, data):
self.scan_info = data
self.scan_data_avai = 1
def process_scan_data (self):
while( self.scan_data_avai == 0 ):
rospy.sleep(0.1)
#print "process data"
#self.scan_data_avai = 0
self.scan_data = self.scan_info
for i in range (self.num_particles):
if( np.isnan(self.particle_array[i].x) or np.isnan(self.particle_array[i].y) or np.isinf(self.particle_array[i].x) or np.isinf(self.particle_array[i].y) ):
continue
cell_prob1 = self.my_map.get_cell(self.particle_array[i].x, self.particle_array[i].y)
cell_prob2 = self.true_map.get_cell(self.particle_array[i].x, self.particle_array[i].y)
if (np.isnan(cell_prob1) or cell_prob2 == 1.0):
self.particle_array[i].weight = 0.0
continue
pz_array = []
for j in range (10):
angle = self.particle_array[i].theta + (self.scan_data.angle_min + (self.scan_data.angle_increment * j))
x = self.particle_array[i].x + (self.scan_data.ranges[j] * np.cos(angle))
y = self.particle_array[i].y + (self.scan_data.ranges[j] * np.sin(angle))
if( np.isnan(x) or np.isnan(y) or np.isinf(x) or np.isinf(y) ):
#lp = 0
pz = 0
else:
lp = self.my_map.get_cell( x, y )
if( np.isnan(lp) ):
pz = 0
else:
pz = (self.laser_z_hit * lp) + self.laser_z_rand
# if( np.isnan(lp) ):
# #lp = 0
# pz = 0
#pz = (self.laser_z_hit * lp) + self.laser_z_rand
pz_array.append(pz)
p_tot = 0
for a in range (len(pz_array)):
value_cubed = math.pow(pz_array[a],3)
if(np.isnan(value_cubed)):
continue
else:
p_tot = p_tot + value_cubed
#sigmoid function for calculating weight
self.particle_array[i].weight = self.particle_array[i].weight * (1.0/(1.0+math.pow(math.e, -1*p_tot)))
self.normalize_weight()
def create_particles(self):
self.particle_array = []
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.header.frame_id = 'map'
self.pose_array.poses = []
for i in range (self.num_particles):
# x and y are coordinates
col = r.randint(0, self.my_map_width)
row = r.randint(0, self.my_map_height)
x, y = self.my_map.cell_position (row,col)
theta = math.radians(r.random() * 360)
pose = get_pose (x,y,theta)
particle = Particle()
particle.x = x
particle.y = y
particle.theta = theta
particle.pose = pose
particle.weight = (1.0/self.num_particles)
self.particle_array.append(particle)
self.pose_array.poses.append(pose)
rospy.sleep(1)
self.particle_pose_pub.publish(self.pose_array)
def handle_map_data(self, data):
if( self.handle_map_first_called == 1 ):
self.my_map = Map(data)
self.true_map = Map(data)
self.my_map_width = self.my_map.width
self.my_map_height = self.my_map.height
self.handle_map_first_called = 0
#print "handle map is called"
def make_all_moves(self):
self.num_moves = len(self.move_list)
for i in range (self.num_moves):
#print "make_move"
self.make_move()
rospy.sleep(1)
self.result_update_pub.publish(True)
self.move_made = self.move_made + 1
def make_move(self):
i = self.move_made
#add noise to x, y, theta when first move
move_function(self.move_list[i][0], 0)
for j in range(len(self.particle_array)):
self.particle_array[j].theta += math.radians(self.move_list[i][0])
self.particle_array[i].theta = self.particle_array[i].theta % (math.radians(360))
for a in range(self.move_list[i][2]):
move_function(0, self.move_list[i][1])
self.particle_update(i)
if(i == 0):
self.particle_add_noise_first_move()
self.process_scan_data()
self.resampling_particle()
def normalize_weight(self):
total = 0
for k in range (self.num_particles):
total += self.particle_array[k].weight
for j in range (self.num_particles):
self.particle_array[j].weight /= total
total = 0
for m in range (self.num_particles):
total += self.particle_array[m].weight
#print "total should be 1: ", total
def resampling_particle(self):
#print "resampling"
self.weight_array = []
new_array = []
for i in range (self.num_particles):
self.weight_array.append(self.particle_array[i].weight)
for j in range (self.num_particles):
particle = np.random.choice(self.particle_array, None, True, self.weight_array)
new_particle = Particle()
new_particle.x = particle.x
new_particle.y = particle.y
new_particle.theta = particle.theta
new_particle.weight = particle.weight
new_particle.pose = particle.pose
new_x = self.add_resample_noise(new_particle.x, self.resample_sigma_x)
new_y = self.add_resample_noise(new_particle.y, self.resample_sigma_y)
new_theta = self.add_resample_noise(new_particle.theta, self.resample_sigma_theta)
new_pose = get_pose(new_x, new_y, new_theta)
new_particle.x = new_x
new_particle.y = new_y
new_particle.theta = new_theta
new_particle.pose = new_pose
new_array.append(new_particle)
for m in range (self.num_particles):
self.particle_array[m] = new_array[m]
self.pose_array.poses[m] = new_array[m].pose
#self.normalize_weight()
# publish the pose array
rospy.sleep(1)
self.particle_pose_pub.publish(self.pose_array)
def particle_update (self, i):
for a in range (self.num_particles):
update_x = self.particle_array[a].x + self.move_list[i][1] * np.cos(self.particle_array[a].theta)
update_y = self.particle_array[a].y + self.move_list[i][1] * np.sin(self.particle_array[a].theta)
self.particle_array[a].x = update_x
self.particle_array[a].y = update_y
self.particle_array[a].pose = get_pose(update_x, update_y, self.particle_array[a].theta)
def particle_add_noise_first_move(self):
for a in range (self.num_particles):
self.particle_array[a].x = self.add_first_move_noise(self.particle_array[a].x, self.first_move_sigma_x)
self.particle_array[a].y = self.add_first_move_noise(self.particle_array[a].y, self.first_move_sigma_y)
self.particle_array[a].theta = self.add_first_move_noise(self.particle_array[a].theta, self.first_move_sigma_angle)
self.particle_array[a].pose = get_pose(self.particle_array[a].x, self.particle_array[a].y, self.particle_array[a].theta)
def add_first_move_noise(self, coordinate, sd):
# noise = r.gauss(0, sd)*100.)/100.
noise = r.gauss(0, sd)
added_noise = coordinate + noise
return added_noise
def add_resample_noise(self, coordinate, sd):
# noise = math.ceil(r.gauss(0, sd) * 100.) /100.
noise = r.gauss(0, sd)
added_noise = coordinate + noise
return added_noise
def construct_field(self):
self.kdtree_array = []
for i in range (self.my_map_height):
for j in range (self.my_map_width):
x, y = self.my_map.cell_position(i,j)
value = self.my_map.get_cell(x, y)
if( value == 1.0 ):
self.kdtree_array.append([x, y])
self.kdtree = KDTree (self.kdtree_array)
self.update_field()
def update_field(self):
for i in range (self.my_map_height):
for j in range (self.my_map_width):
coordinate = self.my_map.cell_position(i,j)
dist, idx = self.kdtree.query([[coordinate[0],coordinate[1]]], k=1)
new_value = self.calculate (dist)
coordinate = self.my_map.cell_position(i,j)
#if( self.my_map.get_cell(coordinate[0], coordinate[1]) == 0.0):
self.my_map.set_cell(coordinate[0], coordinate[1], new_value)
def calculate (self, distance):
power = (-1.0 * ((distance*distance)/(2.0*self.laser_sigma_hit*self.laser_sigma_hit)))
value = math.pow( math.e, power)
return value
class Robot():
def __init__(self):
self.config = read_config()
rospy.init_node("robot")
self.rate = rospy.Rate(1)
self.rate.sleep()
self.result_publisher = rospy.Publisher(
"/result_update",
Bool,
queue_size = 10
)
self.complete_publisher = rospy.Publisher(
"/sim_complete",
Bool,
queue_size = 10
)
self.poseArray_publisher = rospy.Publisher(
"/particlecloud",
PoseArray,
queue_size = 10
)
self.likelihood_publisher = rospy.Publisher(
"/likelihood_field",
OccupancyGrid,
queue_size = 10,
latch = True
)
self.map_service = rospy.Subscriber(
"/map",
OccupancyGrid,
self.handle_map_message
)
self.laser_service = rospy.Subscriber(
"/base_scan",
LaserScan,
self.handle_laser_message
)
self.initialized = False
self.mess = Bool()
self.mess.data = True
self.originX = 0
self.originY = 0
self.width = 0
self.height = 0
self.resolution = 0
self.numPart = self.config['num_particles']
self.laser_sigma_hit = self.config['laser_sigma_hit']
self.laser_z_hit = self.config['laser_z_hit']
self.laser_z_rand = self.config['laser_z_rand']
self.move_list = self.config['move_list']
self.first_move_sigma_x = self.config['first_move_sigma_x']
self.first_move_sigma_y = self.config['first_move_sigma_y']
self.first_move_sigma_angle = self.config['first_move_sigma_angle']
self.resample_sigma_x = self.config['resample_sigma_x']
self.resample_sigma_y = self.config['resample_sigma_y']
self.resample_sigma_angle = self.config['resample_sigma_angle']
self.mapLH = None
self.mapOri = None
self.particleX = []
self.particleY = []
self.particleTheta = []
self.particleWeight = []
self.pose_array = None
self.initializeParticle()
self.move()
rospy.spin()
def get_nearest_obstacle(self, x, y, k=1):
# Check if KDTree is initialized
if not hasattr(self, 'obstacle_tree'):
self._initialize_obstacle_KDTree()
# Query for the given location
dist, ind = self.obstacle_tree.query((x, y), k)
# Transform index to actual locations
obstacles = self.obstacle_tree.get_arrays()[0]
obst = [obstacles[i] for i in ind]
return dist, obst
def handle_map_message(self, message):
if(self.initialized == False):
self.resolution = message.info.resolution
self.origin_x = message.info.origin.position.x
self.origin_y = message.info.origin.position.y
self.width = message.info.width
self.height = message.info.height
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.header.frame_id = 'map'
self.pose_array.poses = []
self.mapOri = Map(message)
self.mapLH = Map(message)
self.initialized = True
def initializeParticle(self):
self.rate.sleep()
for i in range(self.numPart):
x = random.uniform(0,self.width)
y = random.uniform(0,self.height)
while(self.mapOri.get_cell(x,y) != 0):
x = random.uniform(0,self.width)
y = random.uniform(0,self.height)
self.particleX.append(x)
self.particleY.append(y)
self.particleTheta.append(random.uniform(0,2*np.pi))
self.particleWeight.append(1/self.numPart)
self.pose_array.poses.append(get_pose(self.particleX[i],self.particleY[i],self.particleTheta[i]))
self.poseArray_publisher.publish(self.pose_array)
obstalces = filter(lambda x: self.mapOri.get_cell(x[0], x[1]) != 0.0, [(x, y) for x in range(self.width) for y in range(self.height)])
self.obstacle_tree = KDTree(obstalces, leaf_size=20)
self.mapLH.grid = np.array([[GaussianPDF(self.get_nearest_obstacle(x,y)[0][0][0], 0, self.laser_sigma_hit)
for x in range(self.width)]
for y in range(self.height)])
self.likelihood_publisher.publish(self.mapLH.to_message())
def move(self):
first_move = True
for moveStep in self.move_list:
angle = moveStep[0]
dist = moveStep[1]
num = moveStep[2]
print(angle)
print(dist)
print(num)
if(first_move == True):
move_function(angle,0)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
self.particleTheta[i] = self.particleTheta[i]+(angle / 180.0 * self.angle_max)+random.gauss(0,self.first_move_sigma_angle)
self.particleX[i] = (self.particleX[i]+random.gauss(0,self.first_move_sigma_x))
self.particleY[i] = (self.particleY[i]+random.gauss(0,self.first_move_sigma_y))
particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i]/wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if(re<=resampleProb[j]):
index = j
break
resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x))
resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y))
resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
for manyTimes in range(num):
move_function(0,dist)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
self.particleX[i] = self.particleX[i]+random.gauss(0,self.first_move_sigma_x)
self.particleY[i] = self.particleY[i]+random.gauss(0,self.first_move_sigma_y)
self.particleTheta[i] = self.particleTheta[i]+random.gauss(0,self.first_move_sigma_angle)
particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
else:
self.particleX[i] = self.particleX[i]+math.cos(self.particleTheta[i])*dist
self.particleY[i] = self.particleY[i]+math.sin(self.particleTheta[i])*dist
index = 0
pTot = 1
for z in self.ranges:
laserAngle = self.angle_min + self.angle_incr*index
index+=1
if(z>=self.range_min and z<=self.range_max):
xz = self.particleX[i]+math.cos(self.particleTheta[i]+laserAngle)*z
yz = self.particleY[i]+math.sin(self.particleTheta[i]+laserAngle)*z
prob = self.mapLH.get_cell(xz,yz)
if(math.isnan(prob)):
prob = self.laser_z_rand
else:
prob = self.laser_z_hit * prob + self.laser_z_rand
pTot *= prob
self.particleWeight[i] = self.particleWeight[i]*(pTot+0.001)
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i]/wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if(re<=resampleProb[j]):
index = j
break
resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x))
resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y))
resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
first_move = False
else:
move_function(angle,0)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
self.particleTheta[i] = self.particleTheta[i]+(angle / 180.0 * self.angle_max)
particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i]/wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if(re<=resampleProb[j]):
index = j
break
resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x))
resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y))
resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
for manyTimes in range(num):
move_function(0,dist)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
else:
self.particleX[i] = (self.particleX[i]+math.cos(self.particleTheta[i])*dist)
self.particleY[i] = (self.particleY[i]+math.sin(self.particleTheta[i])*dist)
index = 0
pTot = 1
for z in self.ranges:
laserAngle = self.angle_min + self.angle_incr*index
index+=1
if(z>=self.range_min and z<=self.range_max):
xz = self.particleX[i]+math.cos(self.particleTheta[i]+laserAngle)*z
yz = self.particleY[i]+math.sin(self.particleTheta[i]+laserAngle)*z
prob = self.mapLH.get_cell(xz,yz)
if(math.isnan(prob)):
prob = self.laser_z_rand
else:
prob = self.laser_z_hit * prob + self.laser_z_rand
pTot *= prob
self.particleWeight[i] = self.particleWeight[i]*(-pTot+0.001)
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i]/wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if(re<=resampleProb[j]):
index = j
break
resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x))
resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y))
resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
self.result_publisher.publish(self.mess)
self.complete_publisher.publish(self.mess)
def handle_laser_message(self,message):
self.angle_min = message.angle_min
self.angle_max = message.angle_max
self.angle_incr = message.angle_increment
self.range_min = message.range_min
self.range_max = message.range_max
self.ranges = message.ranges
class Robot():
def __init__(self):
""" This will read the config files and set up the different
listeners and what not
"""
self.config = read_config()
self.moveList = self.config['move_list']
self.particles = list()
self.isMapInit = False
random.seed(self.config['seed'])
np.random.seed(self.config['seed'])
self.baseMap = None
self.likelihood_field = None
self.laserScanFlag = False
self.laserScanData = None
self.robotInit()
rospy.init_node("robot")
self.start()
def robotInit(self):
""" Init Subscribers and Publishers"""
self.mapSub = rospy.Subscriber(
"/map",
OccupancyGrid,
self.mapInit
)
self.laserScanSub = rospy.Subscriber(
"/base_scan",
LaserScan,
self.laserScanUpdate
)
#Do I need latch? TODO
self.particleCloudPub = rospy.Publisher(
"/particlecloud",
PoseArray,
queue_size = 10
)
self.likelihoodFieldPub = rospy.Publisher(
"/likelihood_field",
OccupancyGrid,
queue_size = 10,
latch = True
)
self.resultUpdatePub = rospy.Publisher(
"/result_update",
Bool,
queue_size = 10
)
self.simCompletePub = rospy.Publisher(
"/sim_complete",
Bool,
queue_size = 10
)
def mapInit(self, mapData):
if not self.isMapInit:
self.isMapInit = True
self.baseMap = Map(mapData)
self.likelihood_field = Map(mapData)
lsh = self.config['laser_sigma_hit']
width = self.baseMap.width
height = self.baseMap.height
occPoints = list()
points = list()
unoccPoints = list()
for r in range(height):
for c in range(width):
x, y = self.baseMap.cell_position(r, c)
if self.baseMap.get_cell(x, y) == 1:
occPoints.append((x,y))
else:
unoccPoints.append((x,y))
points.append((x,y))
self.occPointsSet = set(occPoints)
occPoints = np.array(occPoints)
points = np.array(points)
self.occPoints = occPoints
self.points = points
self.unoccPoints = unoccPoints
self.tree = KDTree(occPoints)
distance, index = self.tree.query(points, k = 1)
print "Points ", len(points), " should match ", width*height
for d, i, p in zip(distance, index, points):
x, y = p
self.likelihood_field.set_cell(x, y, likeProb(d, lsh))
self.likelihoodFieldPub.publish(self.likelihood_field.to_message())
self.particleInit()
def particleInit(self):
default_weight = 1.0/(self.config["num_particles"])
numPart = self.config["num_particles"]
randPoints = np.random.choice(range(len(self.unoccPoints)), numPart, replace=True)
randPoints = [self.unoccPoints[i] for i in randPoints]
for x, y in randPoints:
p = Particle(x, y, m.pi*random.random(), default_weight)
self.particles.append(p)
self.publishParticles()
self.move()
def publishParticles(self):
pose_array = self.poseArrayTemplate()
for p in self.particles:
pose_array.poses.append(p.get_pose())
self.particleCloudPub.publish(pose_array)
def laserScanUpdate(self, laserScanMessage):
if not self.laserScanFlag:
self.laserScanFlag = True
self.laserScanData = laserScanMessage
def move(self):
moves = self.config["move_list"]
for i, (a, d, n) in enumerate(moves):
print "Move %d, Turn at Angle %.2f, then move %.2f meters %d times." % (i, a, d, n)
print "First turn at angle %.2f" % a
helper_functions.move_function(a, 0)
self.particles = self.updateMove(self.particles, 0.0, a)
self.publishParticles()
self.particles = self.commit(self.particles)
self.publishParticles()
for __ in xrange(n):
print "Moving forward with distance %d" % d
helper_functions.move_function(0, d)
self.particles = self.updateMove(self.particles, d, 0.0)
self.publishParticles()
self.particles = self.commit(self.particles)
self.publishParticles()
self.resultUpdatePub.publish(True)
self.simCompletePub.publish(True)
rospy.sleep(10)
rospy.signal_shutdown("sim complete")
def updateMove(self, parts, d, t):
p = parts[0]
toret = list()
rad = (m.pi * t)/180.0
for p in parts:
theta = p.theta + rad
x = p.x + d*m.cos(p.theta)
y = p.y + d*m.sin(p.theta)
particle = Particle(x, y, theta, p.weight)
toret.append(particle)
return toret
def reweight(self, parts):
zhit = self.config["laser_z_hit"]
zrand = self.config["laser_z_rand"]
lsd = self.getLaserScanData()
increment = lsd.angle_increment
startAngle = lsd.angle_min
newParts = list()
for p in parts:
x, y, theta, q = p.x, p.y, p.theta, p.weight
iswall = self.baseMap.get_cell(x, y)
if m.isnan(iswall):
q = 0.0
elif iswall == 1.0:
q = 0.0
currincrement = 0.0
for r in lsd.ranges:
if q == 0.0:
break
currx = x + r * m.cos(theta + startAngle + currincrement)
curry = y + r * m.sin(theta + startAngle + currincrement)
prob = self.likelihood_field.get_cell(currx, curry)
if m.isnan(prob):
prob = 0.0
q *= zhit * prob + zrand
q += zrand
currincrement += increment
p.weight = q
newParts.append(p)
return self.normalize(newParts)
def normalize(self, parts):
#ALL WEIGHTS GETTING SO SMALL ITS BECOMING 0
weights = [p.weight * 1000 if not m.isnan(p.weight) else 0.0 for p in parts]
normalizer = sum(weights)
if m.isnan(normalizer):
print "GOFFYY!!!!"
else:
weights = [w/normalizer if not m.isnan(w/normalizer) else 0.0 for w in weights]
#why do this?! you don't use it again...
print "TTTTTT", weights.count(0), len(weights)
normalizer = sum(weights)
for i, __ in enumerate(parts):
parts[i].weight = weights[i]
return parts
def resample(self, parts):
res = list()
weights = [p.weight for p in parts]
print "SUMMM", sum(weights)
randPoints = np.random.choice(parts, self.config["num_particles"], replace=True, p=weights)
for p in randPoints:
noise_x = random.gauss(0, self.config["resample_sigma_x"])
noise_y = random.gauss(0, self.config["resample_sigma_y"])
noise_theta = random.gauss(0, self.config["resample_sigma_angle"])
p.x += noise_x
p.y += noise_y
p.theta += noise_theta
res.append(p)
return res
def commit(self, parts):
return self.resample(self.reweight(parts))
def getLaserScanData(self):
self.laserScanFlag = False
while not self.laserScanFlag:
continue
return self.laserScanData
def poseArrayTemplate(self):
pose_array = PoseArray()
pose_array.header.stamp = rospy.Time.now()
pose_array.header.frame_id = 'map'
pose_array.poses =[]
return pose_array
def start(self):
rospy.sleep(5)
rospy.spin()
class Robot():
def __init__(self):
rospy.init_node('robot_node', anonymous = True)
self.map_inited = 0;
self.move_num = 0
self.first_move = True
self.init_map_sub()
self.init_pubs()
rospy.sleep(3) #make sure we create the Map object in init_subs
self.init_config()
random.seed(self.seed)
self.init_particles()
self.publish_particles()
self.init_likelihood_map()
self.start_moves()
if self.move_num >= self.total_moves:
self.handle_shutdown()
rospy.spin()
def handle_shutdown(self):
self.sim_complete.publish(True)
rospy.sleep(3)
rospy.signal_shutdown("Done with Moves")
def init_config(self):
self.config = read_config()
self.num_particles = self.config["num_particles"]
self.map_width = self.myMap.width
self.map_height = self.myMap.height
self.laser_sig_hit = self.config["laser_sigma_hit"]
self.move_list = self.config["move_list"]
self.fm_sigma_x = self.config["first_move_sigma_x"]
self.fm_sigma_y = self.config["first_move_sigma_y"]
self.fm_sigma_ang = self.config["first_move_sigma_angle"]
self.total_moves = len(self.move_list)
self.laser_z_hit = self.config["laser_z_hit"]
self.laser_z_rand = self.config["laser_z_rand"]
self.resamp_sig_x = self.config["resample_sigma_x"]
self.resamp_sig_y = self.config["resample_sigma_y"]
self.resamp_sig_a = self.config["resample_sigma_angle"]
self.turned = False
self.seed = self.config["seed"]
def init_map_sub(self):
self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.handle_map_reply)
self.scan_sub = rospy.Subscriber('/base_scan', LaserScan, self.handle_scan)
def init_pubs(self):
self.particle_cloud_pub = rospy.Publisher('/particlecloud', PoseArray,
queue_size = 10, latch = True)
self.likelihood_pub = rospy.Publisher('/likelihood_field', OccupancyGrid,
queue_size = 10, latch = True)
self.result_pub = rospy.Publisher('/result_update', Bool,
queue_size = 10)
self.sim_complete = rospy.Publisher('/sim_complete', Bool,
queue_size = 10)
def handle_map_reply(self, grid):
if self.map_inited == 0:
self.myMap = Map(grid)
self.likelihood_map = Map(grid)
self.map_inited = 1
def handle_scan(self, scan):
self.scan = scan
self.num_sensors = (scan.angle_max - scan.angle_min) / scan.angle_increment
self.scan_ls = scan.ranges
self.scan_ang_min = scan.angle_min
def init_likelihood_map(self):
occupied_points = self.get_occupied_points()
near_ls = self.get_nearest_neighbors(occupied_points)
it = 0
for x in range (0, self.map_width):
for y in range (0, self.map_height):
val = self.gaussian_likelihood(near_ls[it][0], 0, self.laser_sig_hit)
self.likelihood_map.set_cell(x, y, val)
it+= 1
self.publish_likelihood_map()
def get_occupied_points(self):
occupied_points = []
for x in range ( 0, self.map_width):
for y in range ( 0, self.map_height):
if self.likelihood_map.get_cell(x, y) == 1:
occupied_points.append( [x , y])
return occupied_points
def publish_likelihood_map(self):
message = self.likelihood_map.to_message()
self.likelihood_pub.publish(message)
def gaussian_likelihood(self, x1, x2, sigma):
#alpha = 1 / ( sigma * math.sqrt( 2 * math.pi ))
ex = math.exp(-(x1 - x2)**2 / 2 * sigma**2 )
res = ex
return res
def get_nearest_neighbors(self, occu_pt):
allpts = []
for x in range (0, self.map_width):
for y in range (0, self.map_height):
allpts.append([x,y])
allpts = np.array(allpts)
x = np.array(occu_pt)
kdt = KDTree(x, leaf_size=30, metric='euclidean')
dist, ind = kdt.query(allpts, k=1, return_distance=True)
return dist
def publish_particles(self):
pose_arr = PoseArray()
pose_arr.header.stamp = rospy.Time.now()
pose_arr.header.frame_id = 'map'
pose_arr.poses = []
for p in self.particle_ls:
pose_arr.poses.append(p.pose)
self.particle_cloud_pub.publish(pose_arr)
def start_moves(self):
move_count = 0
while move_count < self.total_moves:
self.move_robot()
self.publish_particles()
self.turned = False
move_count += 1
self.result_pub.publish(True)
def move_robot(self):
angle, dist, steps = self.get_move()
#turn robot
helper_functions.move_function(angle, 0)
p_theta_mov = math.radians(angle)
for p in self.particle_ls:
if self.first_move:
p_theta_mov = random.gauss(p_theta_mov, self.fm_sigma_ang)
p.set_theta( p.theta+p_theta_mov)
p.update_pose()
#move robot
for n in range (0, steps):
helper_functions.move_function(0, dist)
#move particles?
self.move_particles()
self.publish_particles()
self.reweigh_all_particles()
self.publish_particles()
self.resample_particles()
self.publish_particles()
self.publish_particles()
def get_move(self):
self.current_move = self.move_list[self.move_num]
self.move_num += 1
return self.current_move
def resample_particles(self):
pick_ls = []
for p in self.particle_ls:
p_amount = [copy.deepcopy(p)] * int(round(self.num_particles * p.weight))
pick_ls.append(p_amount)
picked_ls_flat = list(itertools.chain(*pick_ls))
new_ls = []
for i in range (0, self.num_particles):
picked = copy.deepcopy(random.choice(picked_ls_flat))
x = random.gauss(picked.x, self.resamp_sig_x)
y = random.gauss(picked.y, self.resamp_sig_y)
#theta = np.random.normal(picked.theta, self.resamp_sig_a, 1)
theta = random.gauss(picked.theta, self.resamp_sig_a)
resampled_p = Particle(x, y, theta, picked.weight)
#picked.update_pose()
new_ls.append(resampled_p)
self.particle_ls = copy.deepcopy(new_ls)
def reweigh_all_particles(self):
Lp = 0
self.laser_ind = 0
for p in self.particle_ls:
p_tot = 0
num_shit_sensors = 0
for s in self.scan_ls:
laser_angle_local = self.scan_ang_min+ self.scan.angle_increment*self.laser_ind
x = s*math.cos(laser_angle_local+p.theta) + p.x
y = s*math.sin(laser_angle_local+p.theta) + p.y
Lp = self.likelihood_map.get_cell(x, y)
if math.isnan(Lp):
num_shit_sensors += 1
Lp = 0
Pz = self.laser_z_hit*Lp + self.laser_z_rand
p_tot += Pz**3
self.laser_ind += 1
if p.x < 0 or p.y < 0 or p.x >= self.map_width or p.y >= self.map_height:
p.weight = 0
elif self.likelihood_map.get_cell(p.x, p.y) >=0.9:
p.weight = 0
elif num_shit_sensors / self.num_sensors > 0.6:
p.weight = 0
else:
p.weight = p.weight *(1/(1+math.exp(-1*p_tot)))
self.normalize_particles()
def normalize_particles(self):
su = 0
for p in self.particle_ls:
su += p.weight
for p in self.particle_ls:
p.weight = p.weight / su
def move_particles(self):
#TODO: list comprehension moved_list = [
ang, dist, steps = self.current_move
angle_rad = math.radians(ang)
#move every particle
for p in self.particle_ls:
x_bar = dist*math.cos(p.theta)
y_bar = dist*math.sin(p.theta)
if self.first_move:
#add noise
angl, x, y = self.handle_first_particle_move(x_bar, y_bar,p.theta)
p.x = p.x + x
p.y = p.y + y
p.update_pose()
else:
p.x = p.x + x_bar
p.y = p.y + y_bar
p.update_pose()
self.first_move = False
def handle_first_particle_move(self, x_bar, y_bar, angle):
mu = 0
sig_x = self.fm_sigma_x
sig_y = self.fm_sigma_y
sig_ang = self.fm_sigma_ang
theta = random.gauss(angle, sig_ang)
x = random.gauss(x_bar, sig_x)
y = random.gauss(y_bar, sig_y)
return theta, x, y
def distance_moved_in_xy(self, d, n, theta):
return x, y
def gaussian(self, x, x2, sigma):
alpha = 1 / ( sigma * math.sqrt( 2 * math.pi ))
ex = math.exp(-(x - x2)**2 / 2 * sigma**2 )
res = alpha * ex
return res
def init_particles(self):
self.particle_ls = []
for i in range (0, self.num_particles):
r_x = random.uniform(0, self.map_width)
r_y = random.uniform(0, self.map_height)
r_theta = random.uniform(0, 2*math.pi)
p = Particle(r_x, r_y, r_theta, 1./self.num_particles)
self.particle_ls.append(p)
class Robot():
def __init__(self):
self.config = read_config()
rospy.init_node('robot')
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.header.frame_id = 'map'
self.pose_array.poses = []
self.move_list = self.config["move_list"]
#Particle update values to use
self.first_sigma_x = self.config["first_move_sigma_x"]
self.first_sigma_y = self.config["first_move_sigma_y"]
self.first_sigma_angle = self.config["first_move_sigma_angle"]
self.resample_sigma_x = self.config["resample_sigma_x"]
self.resample_sigma_y = self.config["resample_sigma_y"]
self.resample_sigma_angle = self.config["resample_sigma_angle"]
self.isFirstMove = True
self.particleList = []
self.posePtr = []
self.tally = 0
laser_sub = rospy.Subscriber(
"/base_scan",
LaserScan,
self.senseCallBack
)
self.likely_pub = rospy.Publisher(
"/likelihood_field",
OccupancyGrid,
queue_size = 10,
latch = True
)
self.pose_pub = rospy.Publisher(
"/particlecloud",
PoseArray,
queue_size = 10
)
self.result_pub = rospy.Publisher(
"/result_update",
Bool,
queue_size = 10
)
self.sim_pub = rospy.Publisher(
"/sim_complete",
Bool,
queue_size = 10
)
map_sub = rospy.Subscriber(
"/map",
OccupancyGrid,
self.mapCallBack
)
rospy.sleep(1)
self.width = self.map.width
self.height = self.map.height
# particle init
i = 0
self.numParticles = self.config["num_particles"]
while(i < self.numParticles):
i += 1
x = random.randint(0, self.width)
y = random.randint(0, self.height)
theta = random.uniform(0, 2*m.pi)
obstacle = self.map.get_cell(x,y)
if(obstacle < 0.5):
pose = get_pose(x,y,theta)
self.pose_array.poses.append(pose)
p = Particle(x,y,theta, 1)
self.particleList.append(p)
else:
i -= 1
self.pose_pub.publish(self.pose_array)
rospy.wait_for_message("/likelihood_field", OccupancyGrid)
self.moveRobot()
self.sim_pub.publish(True)
rospy.is_shutdown()
rospy.spin()
def sigmoid(self, x):
y = (1.0 / (1.0 + m.exp(-x)))
return y
def mapCallBack(self, message):
self.map = Map(message)
self.whocares = Map(message)
self.width = self.map.width
self.height = self.map.height
self.sig = self.config['laser_sigma_hit']
coordinates = []
rospy.sleep(1)
for col in range(0, self.width):
for row in range(0, self.height):
x, y = self.map.cell_position(row, col)
self.cellVal = self.map.get_cell(x,y)
if (self.cellVal > 0.5):
coordinates.append([x,y])
self.kdt = KDTree(coordinates)
for col in range(0, self.width):
for row in range(0, self.height):
dist, ind = self.kdt.query([col,row], k=1)
gauss = (1/(self.sig*m.sqrt(2*m.pi)))*((m.e)**-(((dist)**2)/(2*(self.sig**2))))
self.map.set_cell(col, row, gauss)
self.likely_pub.publish(self.map.to_message())
def senseCallBack(self, laser):
self.lsr = laser
def moveRobot(self):
for t in range(0, len(self.move_list)):
move = self.move_list[t]
#move robot
a = move[0]
d = move[1]
n = move[2]
move_function(a,0)
self.first = True
for s in range(0, n):
move_function(0, d)
self.motion_update(move)
self.first = False
self.isFirstMove = False
self.pose_array.poses = self.posePtr
self.pose_pub.publish(self.pose_array)
self.posePtr = []
self.sensor_update()
self.resample()
self.result_pub.publish(True)
#For one particle
def motion_update(self, mv):
for i in range(0, len(self.particleList)):
part_in = self.particleList[i]
x = part_in.getX()
y = part_in.getY()
theta = part_in.getTheta()
weight = part_in.getWeight()
if (self.isFirstMove):
x = x + random.gauss(0, self.first_sigma_x)
y = y + random.gauss(0, self.first_sigma_y)
theta = theta + random.gauss(0, self.first_sigma_angle)
if (self.first):
theta = theta + m.radians(mv[0])
x = x + mv[1] * m.cos(theta)
y = y + mv[1] * m.sin(theta)
new_pose = get_pose(x,y,theta)
self.posePtr.append(new_pose)
part_in.setX(x)
part_in.setY(y)
part_in.setTheta(theta)
if (m.isnan(self.whocares.get_cell(x,y)) or self.whocares.get_cell(x,y) == 1.0):
part_in.setWeight(0.0)
def sensor_update(self):
z_rand = self.config["laser_z_rand"]
z_hit = self.config["laser_z_hit"]
normalizer = 0
#Laser Scan
for p in range(0, len(self.particleList)):
particle = self.particleList[p]
if(self.map.get_cell(particle.getX(),particle.getY()) == 1):
particle.setWeight(0.0)
ptot = 0
noNAN = 0
for l in range(0,len(self.lsr.ranges)):
scan = self.lsr.ranges[l]
if(scan != self.lsr.range_min and scan != self.lsr.range_max):
angle = self.lsr.angle_min + (self.lsr.angle_increment * l) + particle.getTheta()
x_pos = scan * m.cos(angle) + particle.getX()
y_pos = scan * m.sin(angle) + particle.getY()
lp = self.map.get_cell(x_pos,y_pos)
if (m.isnan(lp)):
noNAN += 1
else:
if (noNAN > (len(self.lsr.ranges)*.75)):
particle.setWeight(0.0)
pz = (z_hit * lp) + z_rand
ptot += (pz**2)
old_weight = particle.getWeight()
new_weight = old_weight * self.sigmoid(ptot)
normalizer += new_weight
new_particle = Particle(particle.getX(),particle.getY(),particle.getTheta(),new_weight)
self.particleList[p] = new_particle
for q in range(0, len(self.particleList)):
particle = self.particleList[q]
weight = particle.getWeight() / normalizer
particle.setWeight(weight)
def resample(self):
good_weights = []
good_particles = []
for w in range(0, len(self.particleList)):
particle = self.particleList[w]
value = self.whocares.get_cell(particle.getX(),particle.getY())
if (not (value == 1.0 or m.isnan(value))):
good_particles.append(self.particleList[w])
good_weights.append(self.particleList[w].getWeight())
# new_particles = np.random.choice(self.particleList, self.numParticles, True, weights)
for p in range(0,len(self.particleList)):
particle = self.particleList[p]
value = self.whocares.get_cell(particle.getX(),particle.getY())
if (value == 1.0 or m.isnan(value)):
dingledangle = np.random.choice(good_particles, 1, True, good_weights)
particle = dingledangle[0]
x = particle.getX() + random.gauss(0,self.resample_sigma_x)
y = particle.getY() + random.gauss(0,self.resample_sigma_y)
theta = particle.getTheta() + random.gauss(0,self.resample_sigma_angle)
new_particle = Particle(x,y,theta,particle.getWeight())
self.particleList[p] = new_particle
class Robot():
def __init__(self):
self.config = read_config()
rospy.init_node("robot")
self.rate = rospy.Rate(1)
self.rate.sleep()
self.result_publisher = rospy.Publisher("/result_update",
Bool,
queue_size=10)
self.complete_publisher = rospy.Publisher("/sim_complete",
Bool,
queue_size=10)
self.poseArray_publisher = rospy.Publisher("/particlecloud",
PoseArray,
queue_size=10)
self.likelihood_publisher = rospy.Publisher("/likelihood_field",
OccupancyGrid,
queue_size=10,
latch=True)
self.map_service = rospy.Subscriber("/map", OccupancyGrid,
self.handle_map_message)
self.laser_service = rospy.Subscriber("/base_scan", LaserScan,
self.handle_laser_message)
self.initialized = False
self.mess = Bool()
self.mess.data = True
self.originX = 0
self.originY = 0
self.width = 0
self.height = 0
self.resolution = 0
self.numPart = self.config['num_particles']
self.laser_sigma_hit = self.config['laser_sigma_hit']
self.laser_z_hit = self.config['laser_z_hit']
self.laser_z_rand = self.config['laser_z_rand']
self.move_list = self.config['move_list']
self.first_move_sigma_x = self.config['first_move_sigma_x']
self.first_move_sigma_y = self.config['first_move_sigma_y']
self.first_move_sigma_angle = self.config['first_move_sigma_angle']
self.resample_sigma_x = self.config['resample_sigma_x']
self.resample_sigma_y = self.config['resample_sigma_y']
self.resample_sigma_angle = self.config['resample_sigma_angle']
self.mapLH = None
self.mapOri = None
self.particleX = []
self.particleY = []
self.particleTheta = []
self.particleWeight = []
self.pose_array = None
self.initializeParticle()
self.move()
rospy.spin()
def get_nearest_obstacle(self, x, y, k=1):
# Check if KDTree is initialized
if not hasattr(self, 'obstacle_tree'):
self._initialize_obstacle_KDTree()
# Query for the given location
dist, ind = self.obstacle_tree.query((x, y), k)
# Transform index to actual locations
obstacles = self.obstacle_tree.get_arrays()[0]
obst = [obstacles[i] for i in ind]
return dist, obst
def handle_map_message(self, message):
if (self.initialized == False):
self.resolution = message.info.resolution
self.origin_x = message.info.origin.position.x
self.origin_y = message.info.origin.position.y
self.width = message.info.width
self.height = message.info.height
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.header.frame_id = 'map'
self.pose_array.poses = []
self.mapOri = Map(message)
self.mapLH = Map(message)
self.initialized = True
def initializeParticle(self):
self.rate.sleep()
for i in range(self.numPart):
x = random.uniform(0, self.width)
y = random.uniform(0, self.height)
while (self.mapOri.get_cell(x, y) != 0):
x = random.uniform(0, self.width)
y = random.uniform(0, self.height)
self.particleX.append(x)
self.particleY.append(y)
self.particleTheta.append(random.uniform(0, 2 * np.pi))
self.particleWeight.append(1 / self.numPart)
self.pose_array.poses.append(
get_pose(self.particleX[i], self.particleY[i],
self.particleTheta[i]))
self.poseArray_publisher.publish(self.pose_array)
obstalces = filter(lambda x: self.mapOri.get_cell(x[0], x[1]) != 0.0,
[(x, y) for x in range(self.width)
for y in range(self.height)])
self.obstacle_tree = KDTree(obstalces, leaf_size=20)
self.mapLH.grid = np.array([[
GaussianPDF(
self.get_nearest_obstacle(x, y)[0][0][0], 0,
self.laser_sigma_hit) for x in range(self.width)
] for y in range(self.height)])
self.likelihood_publisher.publish(self.mapLH.to_message())
def move(self):
first_move = True
for moveStep in self.move_list:
angle = moveStep[0]
dist = moveStep[1]
num = moveStep[2]
print(angle)
print(dist)
print(num)
if (first_move == True):
move_function(angle, 0)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
self.particleTheta[i] = self.particleTheta[i] + (
angle / 180.0 * self.angle_max) + random.gauss(
0, self.first_move_sigma_angle)
self.particleX[i] = (
self.particleX[i] +
random.gauss(0, self.first_move_sigma_x))
self.particleY[i] = (
self.particleY[i] +
random.gauss(0, self.first_move_sigma_y))
particle_poss = self.mapLH.get_cell(
self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i] / wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if (re <= resampleProb[j]):
index = j
break
resampleX.append(self.particleX[index] +
random.gauss(0, self.resample_sigma_x))
resampleY.append(self.particleY[index] +
random.gauss(0, self.resample_sigma_y))
resampleAngle.append(
self.particleTheta[index] +
random.gauss(0, self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(
get_pose(resampleX[i], resampleY[i], resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
for manyTimes in range(num):
move_function(0, dist)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
self.particleX[i] = self.particleX[i] + random.gauss(
0, self.first_move_sigma_x)
self.particleY[i] = self.particleY[i] + random.gauss(
0, self.first_move_sigma_y)
self.particleTheta[
i] = self.particleTheta[i] + random.gauss(
0, self.first_move_sigma_angle)
particle_poss = self.mapLH.get_cell(
self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
else:
self.particleX[i] = self.particleX[i] + math.cos(
self.particleTheta[i]) * dist
self.particleY[i] = self.particleY[i] + math.sin(
self.particleTheta[i]) * dist
index = 0
pTot = 1
for z in self.ranges:
laserAngle = self.angle_min + self.angle_incr * index
index += 1
if (z >= self.range_min
and z <= self.range_max):
xz = self.particleX[i] + math.cos(
self.particleTheta[i] + laserAngle) * z
yz = self.particleY[i] + math.sin(
self.particleTheta[i] + laserAngle) * z
prob = self.mapLH.get_cell(xz, yz)
if (math.isnan(prob)):
prob = self.laser_z_rand
else:
prob = self.laser_z_hit * prob + self.laser_z_rand
pTot *= prob
self.particleWeight[i] = self.particleWeight[i] * (
pTot + 0.001)
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i] / wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if (re <= resampleProb[j]):
index = j
break
resampleX.append(
self.particleX[index] +
random.gauss(0, self.resample_sigma_x))
resampleY.append(
self.particleY[index] +
random.gauss(0, self.resample_sigma_y))
resampleAngle.append(
self.particleTheta[index] +
random.gauss(0, self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(
get_pose(resampleX[i], resampleY[i],
resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
first_move = False
else:
move_function(angle, 0)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
self.particleTheta[i] = self.particleTheta[i] + (
angle / 180.0 * self.angle_max)
particle_poss = self.mapLH.get_cell(
self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i] / wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if (re <= resampleProb[j]):
index = j
break
resampleX.append(self.particleX[index] +
random.gauss(0, self.resample_sigma_x))
resampleY.append(self.particleY[index] +
random.gauss(0, self.resample_sigma_y))
resampleAngle.append(
self.particleTheta[index] +
random.gauss(0, self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(
get_pose(resampleX[i], resampleY[i], resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
for manyTimes in range(num):
move_function(0, dist)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
particle_poss = self.mapLH.get_cell(
self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
else:
self.particleX[i] = (
self.particleX[i] +
math.cos(self.particleTheta[i]) * dist)
self.particleY[i] = (
self.particleY[i] +
math.sin(self.particleTheta[i]) * dist)
index = 0
pTot = 1
for z in self.ranges:
laserAngle = self.angle_min + self.angle_incr * index
index += 1
if (z >= self.range_min
and z <= self.range_max):
xz = self.particleX[i] + math.cos(
self.particleTheta[i] + laserAngle) * z
yz = self.particleY[i] + math.sin(
self.particleTheta[i] + laserAngle) * z
prob = self.mapLH.get_cell(xz, yz)
if (math.isnan(prob)):
prob = self.laser_z_rand
else:
prob = self.laser_z_hit * prob + self.laser_z_rand
pTot *= prob
self.particleWeight[i] = self.particleWeight[i] * (
-pTot + 0.001)
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i] / wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if (re <= resampleProb[j]):
index = j
break
resampleX.append(
self.particleX[index] +
random.gauss(0, self.resample_sigma_x))
resampleY.append(
self.particleY[index] +
random.gauss(0, self.resample_sigma_y))
resampleAngle.append(
self.particleTheta[index] +
random.gauss(0, self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(
get_pose(resampleX[i], resampleY[i],
resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
self.result_publisher.publish(self.mess)
self.complete_publisher.publish(self.mess)
def handle_laser_message(self, message):
self.angle_min = message.angle_min
self.angle_max = message.angle_max
self.angle_incr = message.angle_increment
self.range_min = message.range_min
self.range_max = message.range_max
self.ranges = message.ranges