def plotArrows(self, eps_val, pos_clustering, q):
#debugging data/ takes a snap shot of the system at the start and plots an arrow graph of headings and positions and marks the estimated spot
#clusters=KMeans(n_clusters=3,precompute_distances=True).fit(pos_clustering)
#clusters=AffinityPropagation(affinity='euclidean',preference=None,damping=0.5).fit(pos_clustering)
clusters = DBSCAN(eps=eps_val, min_samples=2).fit(pos_clustering)
py.figure()
u, v = self.get_arrow_directions(pos_clustering, eps_val)
colours = np.asarray(
[]) #need to make sure all values in color param in quiver is >0
min_val = np.min(clusters.labels_.astype(float))
if min_val < 0:
colours = np.asarray(
[c + np.abs(min_val) for c in clusters.labels_.astype(float)])
if min_val == 0:
colours = np.asarray(
[c + 1 for c in clusters.labels_.astype(float)])
else:
colours = clusters.labels_.astype(float)
py.quiver(pos_clustering[:, 0], pos_clustering[:, 1], u, v,
colours) #plot quiver graph
mean_point, q_mean = self.get_estimate(pos_clustering, eps_val)
#print(pos_clustering)
py.quiver(mean_point[0],
mean_point[1],
np.cos(getHeading(q)),
np.sin(getHeading(q)),
color='red') #plot mean
py.xlabel('x')
py.ylabel('y')
py.show(block=False)
py.savefig("pose_graph_DBSCAN.png")
self.plotted = True
def update_temperature(self):
temperatures = []
for i in [
(1, 0),
(0, -1),
(-1, 0),
(0, 1),
]:
robot_angle = getHeading(self.pose.orientation)
sensor_pos_x = self.pose.position.x + i[0] * math.cos(
robot_angle) - i[1] * math.sin(robot_angle)
sensor_pos_y = self.pose.position.y + i[1] * math.cos(
robot_angle) + i[0] * math.sin(robot_angle)
pixel_nb = self.converter.RealToOccupancyGrid(
sensor_pos_x, sensor_pos_y)
temperatures.append(self.firemap.data[pixel_nb])
temp = sum(temperature)
nb_measures += 1
avg_temps = (avg_temps * (nb_measures - 1) + 1 * temp / 4) / 100
for i in range(4):
if temp[i] > 50:
nb_burn += 1
if temp[i] > 30:
nb_dangerous += 1
print("Avg temperature:", avg_temps)
def update_particle_cloud(self, scan):
global actualSigma
Rt = numpy.cov(numpy.random.rand(2,2))/10
predictedSigma = actualSigma + Rt
predictedMut = numpy.zeros((2,1))
actualMut = numpy.zeros((2,1))
predictedMut[0][0] = self.particlecloud.poses[0].position.x
predictedMut[1][0] = self.particlecloud.poses[0].position.y
#print(predictedMut)
while any(numpy.absolute(actualMut-predictedMut)>0.2):
#print("I'm in")
#print(predictedMut)
predictedMut[0][0] = self.particlecloud.poses[0].position.x
predictedMut[1][0] = self.particlecloud.poses[0].position.y
#print(predictedMut)
predictedScan = self.createPredictedScan(predictedMut)
Qt = numpy.cov(numpy.random.rand(20,20))/10
theta = getHeading(self.particlecloud.poses[0].orientation)
Ct = self.create_C(predictedMut,predictedScan, theta)
#print(Ct)
zt = self.createActualScan(scan.ranges, scan.range_max)
Kt = numpy.dot(numpy.dot(predictedSigma, numpy.transpose(Ct)), numpy.linalg.inv(numpy.dot(numpy.dot(Ct,predictedSigma),numpy.transpose(Ct))+Qt)) * 0.001
#print(Kt)
I = numpy.identity(2)
actualMut = predictedMut + numpy.dot(Kt,(zt - numpy.dot(Ct,predictedMut)))
actualSigma = numpy.dot((I-numpy.dot(Kt,Ct)),predictedSigma)
#self.particlecloud.poses[0].position.x = actualMut[0][0]
#self.particlecloud.poses[0].position.y = actualMut[1][0]
print(actualMut-predictedMut)
#print("I'm out")
self.particlecloud.poses[0].position.x = actualMut[0][0]
self.particlecloud.poses[0].position.y = actualMut[1][0]
def get_weight(self, scan, pose):
"""
Compute the likelihood weighting for each of a set of particles
:Args:
| scan (sensor_msgs.msg.LaserScan): Current observation
| pose (geometry_msgs.msg.Pose): Particle's estimated location
:Returns:
| (double) likelihood weighting for this particle, given the map
and laser readings
"""
p = 1.0 # Sample weight (not a probability!)
for i, obs_bearing in self.reading_points:
# For each range...
obs_range = scan.ranges[i]
# Laser reports max range as zero, so set it to range_max
if (obs_range <= 0.0):
obs_range = self.scan_range_max
# Compute the range according to the map
map_range = self.calc_map_range(pose.position.x, pose.position.y,
getHeading(pose.orientation) + obs_bearing)
pz = self.predict(obs_range, map_range)
p += pz*pz*pz # Cube probability: reduce low-probability particles
return p
def systematic_resampling(S, W):
# cumulative density function
cdf = []
# keep track of running total
cdf_counter = 0
# create cdf entry for each sample
for (p, w) in S:
cdf.append((p, cdf_counter + w / W))
cdf_counter += w / W
U = random.uniform(0, 1 / len(S))
counter = 0
S_n = PoseArray()
for j in range(0, len(S)):
while U > cdf[counter][1]:
counter += 1
new_particle = Pose()
new_particle.position.x = cdf[counter][0].position.x + random.gauss(
0, 0.1)
new_particle.position.y = cdf[counter][0].position.y + random.gauss(
0, 0.1)
new_particle.orientation = rotateQuaternion(
Quaternion(w=1),
getHeading(cdf[counter][0].orientation) + random.gauss(0, 0.05))
S_n.poses.append(new_particle)
U += (1 / len(S))
return S_n
def rotate2(yaw): #same as rotate but we have defined yaw
rospy.loginfo(yaw)
while True:
try:
data = rospy.wait_for_message("/estimatedsituation", situation, 20)
except:
rospy.logerr(
"Problem getting a map. Check that you have a map_server"
" running: rosrun map_server map_server <mapname> ")
rospy.loginfo(getHeading(data.pose.orientation))
rospy.loginfo(yaw)
if is_small_angle(yaw, getHeading(data.pose.orientation), 0.2):
talker(0, 0)
break
talker(0.3, 0)
def get_weight(self, scan, pose):
"""
Compute the likelihood weighting for each of a set of particles
:Args:
| scan (sensor_msgs.msg.LaserScan): Current observation
| pose (geometry_msgs.msg.Pose): Particle's estimated location
:Returns:
| (double) likelihood weighting for this particle, given the map
and laser readings
"""
p = 1.0 # Sample weight (not a probability!)
for i, obs_bearing in self.reading_points:
# For each range...
obs_range = scan.ranges[i]
# Laser reports max range as zero, so set it to range_max
if (obs_range <= 0.0):
obs_range = self.scan_range_max
# Compute the range according to the map
map_range = self.calc_map_range(
pose.position.x, pose.position.y,
getHeading(pose.orientation) + obs_bearing)
pz = self.predict(obs_range, map_range)
p += pz * pz * pz # Cube probability: reduce low-probability particles
return p
def weighted_zscore(self):
result = Pose()
sin_rotation = 0.0
cos_rotation = 0.0
total = 0.0
for i in range(self.NUMBER_OF_PARTICLES):
total += self.weights[i]
j = 0
for i in self.particlecloud.poses:
result.position.x += i.position.x * self.weights[j] / total
result.position.y += i.position.y * self.weights[j] / total
theta = util.getHeading(i.orientation) * self.weights[j] / total
sin_rotation += math.sin(theta) * self.weights[j] / total
cos_rotation += math.cos(theta) * self.weights[j] / total
j += 1
rotation = math.atan2(sin_rotation, cos_rotation)
result.orientation = util.rotateQuaternion(Quaternion(w=1.0), rotation)
dist = []
for i in self.particlecloud.poses:
x = (i.position.x - result.position.x)**2 + (i.position.y -
result.position.y)**2
dist.append(math.sqrt(x))
dist = np.array(dist)
zscore = stats.zscore(dist)
result = Pose()
sin_rotation = 0.0
cos_rotation = 0.0
total = 0.0
#print(zscore)
for i in range(self.NUMBER_OF_PARTICLES):
if zscore[i] < 0.7 and zscore[i] > -0.7:
result.position.x += self.particlecloud.poses[i].position.x
result.position.y += self.particlecloud.poses[i].position.y
theta = util.getHeading(
self.particlecloud.poses[i].orientation)
sin_rotation += math.sin(theta)
cos_rotation += math.cos(theta)
total += 1.0
print(self.NUMBER_OF_PARTICLES)
print(total)
result.position.x /= total
result.position.y /= total
rotation = math.atan2(sin_rotation / total, cos_rotation / total)
result.orientation = util.rotateQuaternion(Quaternion(w=1.0), rotation)
return result
def navigate(estimated_pose, next_point, scan, kp, kn, maximum_velocity,
positive_border, negative_border):
# Finds best velocity for 1. moving through created path 2. Avoiding obsacles
dif_x = next_point.position.x - estimated_pose.position.x
dif_y = next_point.position.y - estimated_pose.position.y
#where_obstacle(scan)
# differences between current and next position
forcepx, forcepy = positive_force(
dif_x, dif_y, kp,
positive_border) #compute positive foces usuing parameters
rospy.loginfo('positive')
rospy.loginfo(forcepx)
rospy.loginfo(forcepy)
forcepx, forcepy = rotation(
forcepx, forcepy, getHeading(estimated_pose.orientation)
) #Rotate forces to robot coordinate axis from map axis
rospy.loginfo('rotation')
rospy.loginfo(forcepx)
rospy.loginfo(forcepy)
rospy.loginfo(getHeading(estimated_pose.orientation))
# following the path
forcenx, forceny = negative_force(kn, scan, negative_border,
0) #Compute negative forces
rospy.loginfo('negative')
rospy.loginfo(forcenx)
rospy.loginfo(forceny)
# avoiding obstacles
#rospy.loginfo('sum')
#forcex = forcepx + forcenx
#forcey = forcepy + forceny
#rospy.loginfo(forcex)
#rospy.loginfo(forcey)
#vx, vy = compute_velocity(forcex, forcey, maximum_velocity)
force_robot_x = forcepx + forcenx #add them
force_robot_y = forcepy + forceny
#rotation(forcex, forcey, getHeading(estimated_pose.orientation))
rospy.loginfo('final forces')
rospy.loginfo(force_robot_x)
rospy.loginfo(force_robot_y)
rospy.loginfo('speed')
v, w = controller(force_robot_x, force_robot_y, maximum_velocity, 0.2,
0.4) #Conroller compute best velocity
return w, v
def rotate(self, look_at_tile):
tmp_heading = util.getHeading(self.pos, look_at_tile)
if self.heading == tmp_heading:
return False
#self.last_action = 'rotate'
self.heading = tmp_heading
return True
def update_particle_cloud(self, scan):
"""
This should use the supplied laser scan to update the current
particle cloud. i.e. self.particlecloud should be updated.
:Args:
| scan (sensor_msgs.msg.LaserScan): laser scan to use for update
"""
self.scan = scan
movement_particles = []
dtime = rospy.Time.now().to_sec() - self.last_time.to_sec()
self.last_time = rospy.Time.now()
for i in self.particlecloud.poses:
x = i.position.x
y = i.position.y
theta = util.getHeading(i.orientation)
for j in range(4):
p = Pose()
p.position.x = x + random.uniform(-0.2, 0.2)
p.position.y = y + random.uniform(-0.2, 0.2)
p.position.z = 0
thetad = theta + random.uniform(-0.4, 0.4)
p.orientation = util.rotateQuaternion(Quaternion(w=1.0),
thetad)
probn = self.sensor_model.get_weight(scan, p)**2
movement_particles.append((probn, p))
w_avg = 0.0
for x in movement_particles:
w_avg += x[0] / len(movement_particles)
self.w_slow += self.A_SLOW * (w_avg - self.w_slow)
self.w_fast += self.A_FAST * (w_avg - self.w_fast)
particlecloudnew = PoseArray()
weights = np.asarray([i[0] for i in movement_particles]).cumsum()
new_weights = []
for i in range(self.NUMBER_OF_PARTICLES):
j = random.random() * w_avg * len(movement_particles)
index = np.searchsorted(weights, j)
pose = movement_particles[index][1]
weight = movement_particles[index][0]
if random.random() > (self.w_fast / self.w_slow):
pose = self.generate_random_map_pose()
weight = 0.0
particlecloudnew.poses.append(pose)
new_weights.append(weight)
self.particlecloud.poses = particlecloudnew.poses
self.weights = new_weights
def avg_estimate(self, pf):
sumX = 0
sumY = 0
sumOrientation = 0
particles = pf.particlecloud
for i in range (0, len(particles.poses)):
sumX += particles.poses[i].position.x
sumY += particles.poses[i].position.y
heading = getHeading(particles.poses[i].orientation)
sumOrientation += getHeading(particles.poses[i].orientation)
pose = Pose()
pose.position.x = sumX/len(particles.poses)
pose.position.y = sumY/len(particles.poses)
orientation = pf.estimatedpose.pose.pose.orientation
pose.orientation = rotateQuaternion(orientation, sumOrientation/len(particles.poses) - getHeading(orientation))
rospy.loginfo("X: " + str(pose.position.x) +", Y: " + str(pose.position.y))
return pose
def update_particle_cloud(self, scan):
# Update particlecloud, given map and laser scan
particles = self.particlecloud.poses
# Work out weights of particles from sensor readings
weighted_particles = []
for p in particles:
pw = self.sensor_model.get_weight(scan, p)
weighted_particles.append((p, pw))
# Sort the particles by weight in descending order
sorted_weighted_particles = sorted(weighted_particles,
key=lambda p: p[1],
reverse=True)
# Keep the 3/4 of particles with the highest weights for resampling
pred_weighted_particles = sorted_weighted_particles[:int(
(1 - self.REDIST_PERCENTAGE) * self.NUMBER_PARTICLES)]
weight_sum = sum([p[1] for p in pred_weighted_particles])
# Discard and randomly distribute the remaining 1/4 of particles across the map
rand_weighted_particles = self.gen_random_particles(
int(self.REDIST_PERCENTAGE * self.NUMBER_PARTICLES))
# Resample particles according to weights
cdf_aux = 0.0
cdf = []
for (p, w) in pred_weighted_particles:
cdf.append((p, cdf_aux + w / weight_sum))
cdf_aux += w / weight_sum
u = random.uniform(0.0, 1.0 / len(pred_weighted_particles))
i = 0
new_particles = PoseArray()
for j in range(0, len(pred_weighted_particles)):
while (u > cdf[i][1]):
i += 1
new_particle = Pose()
new_particle.position.x = cdf[i][0].position.x + random.gauss(
0.0, 0.1)
new_particle.position.y = cdf[i][0].position.y + random.gauss(
0.0, 0.1)
new_particle.orientation = rotateQuaternion(
Quaternion(w=1.0),
getHeading(cdf[i][0].orientation) + random.gauss(0, 0.05))
new_particles.poses.append(new_particle)
u += (1.0 / len(pred_weighted_particles))
rand_weighted_particles = self.gen_random_particles(
int(self.REDIST_PERCENTAGE * self.NUMBER_PARTICLES))
new_particles.poses += rand_weighted_particles.poses
self.particlecloud = new_particles
def createPredictedScan(self, predictedMut):
predictedLaserScans = numpy.zeros((len(self.sensor_model.reading_points),1))
iter = 0
for i, obs_bearing in self.sensor_model.reading_points:
# ----- Predict the scan according to the map
map_range = self.sensor_model.calc_map_range(predictedMut[0][0], predictedMut[1][0],
getHeading(self.particlecloud.poses[0].orientation) + obs_bearing)
# ----- Laser reports max range as zero, so set it to range_max
if (map_range <= 0.0):
map_range = self.sensor_model.scan_range_max
predictedLaserScans[iter][0] = map_range
iter += 1
return predictedLaserScans
def rotate(trajectory): #Try to rotate a robot to go trhough trajectory
x1 = trajectory.poses[0].pose.position.x
y1 = trajectory.poses[0].pose.position.y
x2 = trajectory.poses[1].pose.position.x
y2 = trajectory.poses[1].pose.position.y
yaw = math.atan2(y2 - y1, x2 - x1) #compute yaw
rospy.loginfo(yaw)
while True:
try:
rospy.loginfo('I am here')
data = rospy.wait_for_message("/estimatedsituation", situation)
except:
rospy.logerr(
"Problem getting a map. Check that you have a map_server"
" running: rosrun map_server map_server <mapname> ")
rospy.loginfo(getHeading(data.pose.orientation))
rospy.loginfo(yaw)
if is_small_angle(yaw, getHeading(data.pose.orientation), 0.2):
talker(0, 0)
break
talker(0.3, 0) #if we are not close, we turn left
def get_arrow_directions(
self, arr, eps_val
): #given the data and the eps_val, gets the headings of each particle and returns the directional data
clustering = DBSCAN(eps=eps_val, min_samples=2).fit(arr)
#clustering=KMeans(n_clusters=3,precompute_distances=True).fit(arr)
#clustering=AffinityPropagation(affinity='euclidean',preference=None,damping=0.5).fit(arr)
largest = self.largest_cluster(clustering.labels_)
points = self.getPoints(largest, clustering.labels_, arr)
q_list = np.asarray([])
for pose in self.particlecloud.poses:
if np.asarray([pose.position.x, pose.position.y]) in points:
q_list = np.append(q_list, np.asarray([pose.orientation]))
radian = np.asarray([getHeading(q) for q in q_list])
u = np.asarray([np.cos(r) for r in radian])
v = np.asarray([np.sin(r) for r in radian])
return u, v
def update_temperature(self):
t0 = time.time()
temperatures = []
for i in [
(self.thermometer_range_m, 0), (0, -self.thermometer_range_m),
(-self.thermometer_range_m, 0), (0, self.thermometer_range_m)
]:
robot_angle = getHeading(self.pose.orientation)
sensor_pos_x = self.pose.position.x + i[0] * math.cos(
robot_angle) - i[1] * math.sin(robot_angle)
sensor_pos_y = self.pose.position.y + i[1] * math.cos(
robot_angle) + i[0] * math.sin(robot_angle)
pixel_nb = self.converter.RealToOccupancyGrid(
sensor_pos_x, sensor_pos_y)
temperatures.append(self.firemap.data[pixel_nb])
array = Int8MultiArray()
array.data = temperatures
self.pub.publish(array)
print(temperatures)
def predict_from_odometry(self, odom):
"""
Adds the estimated motion from odometry readings to each of the
particles in particlecloud.
:Args:
| odom (nav_msgs.msg.Odometry): Recent Odometry data
"""
with self._update_lock:
t = time.time()
x = odom.pose.pose.position.x
y = odom.pose.pose.position.y
new_heading = getHeading(odom.pose.pose.orientation)
# On our first run, the incoming translations may not be equal to
# zero, so set them appropriately
if not self.odom_initialised:
self.prev_odom_x = x
self.prev_odom_y = y
self.prev_odom_heading = new_heading
self.odom_initialised = True
# Find difference between current and previous translations
dif_x = x - self.prev_odom_x
dif_y = y - self.prev_odom_y
dif_heading = new_heading - self.prev_odom_heading
if dif_heading > math.pi:
dif_heading = (math.pi * 2) - dif_heading
if dif_heading < -math.pi:
dif_heading = (math.pi * 2) + dif_heading
# Update previous pure odometry location (i.e. excluding noise)
# with the new translation
self.prev_odom_x = x
self.prev_odom_y = y
self.prev_odom_heading = new_heading
self.last_odom_pose = odom
# Find robot's linear forward/backward motion, given the dif_x and
# dif_y changes and its orientation
distance_travelled = math.sqrt(dif_x*dif_x + dif_y*dif_y)
direction_travelled = math.atan2(dif_y, dif_x)
temp = abs(new_heading - direction_travelled)
if temp < -PI_OVER_TWO or temp > PI_OVER_TWO:
# We are going backwards
distance_travelled = distance_travelled * -1
# Update each particle with change in position (plus noise)
for p in self.particlecloud.poses:
rnd = random.normalvariate(0, 1)
# Rotate particle according to odometry rotation, plus noise
p.orientation = (rotateQuaternion(p.orientation,
dif_heading + rnd * dif_heading * self.ODOM_ROTATION_NOISE))
# Get particle's new orientation
theta = getHeading(p.orientation)
# Find translation in the direction of particle's orientation
travel_x = distance_travelled * math.cos(theta)
travel_y = distance_travelled * math.sin(theta)
p.position.x = (p.position.x + travel_x +
(rnd * travel_x * self.ODOM_TRANSLATION_NOISE))
p.position.y = (p.position.y + travel_y +
(rnd * travel_y * self.ODOM_DRIFT_NOISE))
return time.time() - t
def predict_from_odometry(self, odom):
"""
Adds the estimated motion from odometry readings to each of the
particles in particlecloud.
:Args:
| odom (nav_msgs.msg.Odometry): Recent Odometry data
"""
with self._update_lock:
t = time.time()
x = odom.pose.pose.position.x
y = odom.pose.pose.position.y
new_heading = getHeading(odom.pose.pose.orientation)
# ----- On our first run, the incoming translations may not be equal to
# ----- zero, so set them appropriately
if not self.odom_initialised:
self.prev_odom_x = x
self.prev_odom_y = y
self.prev_odom_heading = new_heading
self.odom_initialised = True
# ----- Find difference between current and previous translations
dif_x = x - self.prev_odom_x
dif_y = y - self.prev_odom_y
dif_heading = new_heading - self.prev_odom_heading
if dif_heading > math.pi:
dif_heading = (math.pi * 2) - dif_heading
if dif_heading < -math.pi:
dif_heading = (math.pi * 2) + dif_heading
# ----- Update previous pure odometry location (i.e. excluding noise)
# ----- with the new translation
self.prev_odom_x = x
self.prev_odom_y = y
self.prev_odom_heading = new_heading
self.last_odom_pose = odom
# ----- Find robot's linear forward/backward motion, given the dif_x and
# ----- dif_y changes and its orientation
distance_travelled = math.sqrt(dif_x * dif_x + dif_y * dif_y)
direction_travelled = math.atan2(dif_y, dif_x)
temp = abs(new_heading - direction_travelled)
if temp < -PI_OVER_TWO or temp > PI_OVER_TWO:
# ----- We are going backwards
distance_travelled = distance_travelled * -1
# ----- Update each particle with change in position (plus noise)
for p in self.particlecloud.poses:
rnd = random.normalvariate(0, 1)
# ----- Rotate particle according to odometry rotation, plus noise
p.orientation = (rotateQuaternion(
p.orientation, dif_heading +
rnd * dif_heading * self.ODOM_ROTATION_NOISE))
# ----- Get particle's new orientation
theta = getHeading(p.orientation)
# ----- Find translation in the direction of particle's orientation
travel_x = distance_travelled * math.cos(theta)
travel_y = distance_travelled * math.sin(theta)
p.position.x = (p.position.x + travel_x +
(rnd * travel_x * self.ODOM_TRANSLATION_NOISE))
p.position.y = (p.position.y + travel_y +
(rnd * travel_y * self.ODOM_DRIFT_NOISE))
return time.time() - t
