def checkAndUpdateCover(self, brushfire, pose = None, threshold = 1.): if pose == None: self.readPose = True self.readRobotPose() while self.readPose: pass xx = int(self.robot_pose['x']) yy = int(self.robot_pose['y']) th = self.robot_pose['th'] th_deg = math.degrees(th) else: xx = pose[0] yy = pose[1] th_deg = pose[2] indexes = [] for s in range(self.sensor_number): cover_length = int(self.sensor_range[s] / self.resolution) if self.sensor_shape[s] == 'rectangular': temp = Cffi.rectangularBrushfireCoverageCffi((xx,yy), self.ogm, cover_length, self.sensor_fov[s], th_deg, self.sensor_direction[s]) indexes.extend(temp) elif self.sensor_shape[s] == 'circular': temp = Cffi.circularRayCastCoverageCffi((xx,yy), self.ogm, cover_length, self.sensor_fov[s], th_deg, self.sensor_direction[s]) indexes.extend(temp) else: rospy.loginfo("Error!Sensor's shape not found!") return covered_area = self.coverage[zip(*indexes)] brushfire_area = brushfire[zip(*indexes)] near_obstacles_len = len(np.where(brushfire_area == 2)[0]) if near_obstacles_len == 0: return False old_obstacles_len = len(np.where((brushfire_area == 2) & (covered_area > 80))[0]) perc = old_obstacles_len / near_obstacles_len if perc < threshold: for x,y in indexes: self.coverage[x, y] = 100 * self.sensor_reliability[0] i = int(x + self.ogm_width * y) self.coverage_ogm.data[i] = 100 updated = True else: updated = False return updated
def updateCover(self, pose = None, publish = True, track_specs = True): if pose == None: self.readPose = True self.readRobotPose() while self.readPose: pass xx = int(self.robot_pose['x']) yy = int(self.robot_pose['y']) th = self.robot_pose['th'] th_deg = math.degrees(th) else: xx = pose[0] yy = pose[1] th_deg = pose[2] # Check if robot moved move = ((xx - self.previous_robot_pose['x'])**2 + \ (yy - self.previous_robot_pose['y'])**2 + \ (th_deg - self.previous_robot_pose['th'])**2)**(1/2) # Update coverage only if robot moved if move > 1.0: for s in range(self.sensor_number): cover_length = int(self.sensor_range[s] / self.resolution) if self.sensor_shape[s] == 'rectangular': indexes = Cffi.rectangularBrushfireCoverageCffi((xx,yy), self.ogm, cover_length, self.sensor_fov[s], th_deg, self.sensor_direction[s]) for x,y in indexes: self.coverage[x, y] = 100 * self.sensor_reliability[s] i = int(x + self.ogm_width * y) self.coverage_ogm.data[i] = 100 if track_specs: self.coverage_number_ogm.data[i] += 1 self.coverage_number[x, y] += 1 elif self.sensor_shape[s] == 'circular': indexes = Cffi.circularRayCastCoverageCffi((xx,yy), self.ogm, cover_length, self.sensor_fov[s], th_deg, self.sensor_direction[s]) for x,y in indexes: self.coverage[x, y] = 100 * self.sensor_reliability[s] i = int(x + self.ogm_width * y) self.coverage_ogm.data[i] = 100 if track_specs: self.coverage_number[x, y] += 1 if self.coverage_number[x, y] < 100: self.coverage_number_ogm.data[i] += 1 yaw_between_nodes = math.degrees(math.atan2(yy-y, xx-x)) angle = yaw_between_nodes + th_deg + self.sensor_direction[s] # print('th', th_deg, 'dir', self.sensor_direction[s], 'yaw', yaw_between_nodes, 'angle', angle) if angle > 180: angle -= 360 if angle <= -180: angle += 360 for a in range(len(self.bins)): if angle >= self.bins[a][0] and angle < self.bins[a][1]: self.coverage_angles[x][y][a] += 1 ii = int(x + self.ogm_width * y + self.ogm_height * self.ogm_width * a) self.coverage_angles_ogm.data[ii] += 1 # print(self.bins[a], a, angle) break else: rospy.loginfo("Error!Sensor's shape not found!") return if publish: self.coverage_pub.publish(self.coverage_ogm) if track_specs: self.coverage_number_pub.publish(self.coverage_number_ogm) max_val = np.max(self.coverage_number) rospy.loginfo("Maximum number of coverage look ups of points is {}.".format(max_val)) self.coverage_angles_pub.publish(self.coverage_angles_ogm) rospy.loginfo("Update coverage ogm!") self.previous_robot_pose['x'] = xx self.previous_robot_pose['y'] = yy self.previous_robot_pose['th'] = th_deg return