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