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
