lat = 55.471689
lon = 10.325267
utm_center = utm.from_latlon(lat, lon)
s = Sun(lat, lon)
m = Mission(0.8, 0.8)
local1 = [utm_center[0] - utm1[0], utm_center[1] - utm1[1]]
local2 = [utm_center[0] - utm2[0], utm_center[1] - utm2[1]]
local3 = [utm_center[0] - utm3[0], utm_center[1] - utm3[1]]
local4 = [utm_center[0] - utm4[0], utm_center[1] - utm4[1]]
height = 2.0
east_wall = Boundary(local1[0], local1[1], 0.0, local2[0], local2[1], 0.0,
local1[0], local1[1], height, "wall", s.get_lightsource())
south_window = Boundary(0.3857, -2.1862, 0.5, 1.3943, -1.532, 0.8, 0.3857,
-2.1862, 0.5 + 0.6, "window", s.get_lightsource())
north_wall = Boundary(local2[0], local2[1], 0.0, local3[0], local3[1],
0.0, local2[0], local2[1], height, "wall",
s.get_lightsource())
fake_east_window = Boundary(2.292, 0.424, 0.8, 1.699, 1.342, 0.8, 2.292, 0.424,
0.8 + 0.5, "window", s.get_lightsource())
west_wall = Boundary(local3[0], local3[1], 0.0, local4[0], local4[1], 0.0,
local3[0], local3[1], height, "wall", s.get_lightsource())
fake_west_window = Boundary(-2.4586, -1.0349, 0.8, -1.9872, -2.0232, 0.8,
-2.4586, -1.0349, 0.8 + 0.5, "window",
s.get_lightsource)
south_wall = Boundary(local4[0], local4[1], 0.0, local1[0], local1[1],
0.0, local4[0], local4[1], height, "wall",
s.get_lightsource())
class Planner():
def __init__(self, building_height):
rospy.init_node('offb')
self.height = building_height
self.GPS_ready = False
# Wait for Offboard and drone to be ready
ready = rospy.wait_for_message('/gps_ready_from_offboard', Bool, timeout=rospy.Duration(60*5)) # Wait for offboard to get GPS fix for 5 minutes, because sometimes GPS locks take long to get when a board is booted up the first time
if ready:
nav = NavSatFix()
# And now put the node into a waiting loop to wait for GPS Lock from Offboard
nav = rospy.wait_for_message('mavros/global_position/global', NavSatFix)
self.coordinates = np.array([nav.latitude, nav.longitude])
else:
rospy.loginfo("Never got position confirmation from Offboard")
self.house_result_pub = rospy.Publisher('/house_results_from_overpass_local', BuildingPolygonResult, queue_size=5) # Information about the house
self.camera_stuff_pub = rospy.Publisher('/camera_and_distances', CameraStuff, queue_size=1)
self.impression_keyframes_pub = rospy.Publisher('/impression_poses_from_planner', PoseArray, queue_size=5) # Poses to take one image of each facade of the house
self.inspection_keyframes_pub = rospy.Publisher('/keyframes_poses_from_planner', PoseArray, queue_size=5)
#self.segmented_image_sub = rospy.Subscriber('segmented_image_from_cnn', cnnResult, self.segmented_image_callback, queue_size=5)
self.image_and_rois_sub = rospy.Subscriber('/segmented_image_from_cnn', ImageAndRois, callback=self.image_and_rois_callback)
self.get_tiles_ready = False
self.images_list = []
self.rois_list = []
self.s = Sun(self.coordinates[0], self.coordinates[1])
self.m = Mission(80, 80)
self.model_walls = []
self.model_windows = []
self.fig_thread = Thread(target=self.draw_thread, args=())
self.fig_thread.daemon = True
self.fig = []
self.ax = []
def image_and_rois_callback(self, data):
image = data.img
rois = data.rois
self.images_list.append(image)
self.rois_list.append(rois)
if self.get_tiles_ready == False:
self.get_tiles_ready = True
def send_camera_and_mission_information_to_cnn(self, mission, distances):
''' Format of message is
float64[] aov
float64 focal_length
float64 resolution
float64[] distances_from_walls
'''
c = CameraStuff()
c.aov = [mission.aov[0], mission.aov[1]]
c.focal_length = mission.f
c.resolution = [mission.sensor_resolution[0], mission.sensor_resolution[1]]
c.distances_from_walls = distances
self.camera_stuff_pub.publish(c)
def build_walls(self, polygon_result, local_coords):
rospy.loginfo("Planner recieved information about the house. Building walls for detailed model")
#model_walls = []
for i in range(len(local_coords) - 1):
# Use the local coordinates to build walls
self.model_walls.append(Boundary(local_coords[i][0], local_coords[i][1], 0.0, local_coords[i+1][0], local_coords[i+1][1], 0.0, local_coords[i][0], local_coords[i][1], polygon_result.building_height, "wall", self.s.get_lightsource()))
# Let's also initiate the figure and plot all the walls
self.fig = plt.figure(num=1, clear=True)
self.ax = self.fig.add_subplot(1, 1, 1, projection='3d')
# For loop that goes through all the model walls and plots them
for wall in self.model_walls:
wall.plot(self.ax)
#ground_plane.plot(ax)
self.ax.set_xlabel('X: East')
self.ax.set_xlim(-50, 50)
self.ax.set_ylabel('Y: North')
self.ax.set_ylim(-50, 50)
self.ax.set_zlabel('Z: Up')
self.ax.set_zlim(-20, 20)
axsolarbutton = plt.axes([0.85, 0.25, 0.10, 0.075])
self.solar_button = Button(axsolarbutton, 'Solar Check')
self.m.draw_mission_planes(self.model_walls, self.ax)
axbrushfirebutton = plt.axes([0.85, 0.10, 0.10, 0.075])
self.brushfirebutton = Button(axbrushfirebutton, 'Generate mission')
def put_up_windows(self, facade_image, windows, wall_length, model_wall):
rospy.loginfo("Planner received information about the windows. Putting windows on walls")
# Generate an array that takes the time from now and the next 30 minutes, which we will say is the estimated mission duration
# We assume that the image itself is the plane, which is harsh to assume, but it's the best we've got...
# Based on the size of the image (width, height), we make translation factors for x and y to be able to place the windows on the planes.
# Let's then generate windows in the form that our mission class can understand, taking the bounding boxes of the instance segmentation, aka the rois and make windows out of them
for i in range(len(windows)):
# Remember that the coordinates of the rois (windows) are upside down on the y-axis
# The rois are in the following form : y1,x1, y2,x2
n_xy_tiles = len(model_wall.x)
n_z_tiles = len(model_wall.z) # this is the y coordinate of the image, but the z coordinate in the world-frame
x1 = n_xy_tiles - round((windows[i].roi[1] / facade_image.width) * n_xy_tiles) - 1
y1 = n_z_tiles - round((windows[i].roi[0] / facade_image.height) * n_z_tiles) - 1
x2 = n_xy_tiles - round((windows[i].roi[3] / facade_image.width) * n_xy_tiles) - 1
y2 = n_z_tiles - round((windows[i].roi[2] / facade_image.height) * n_z_tiles) - 1
#rospy.loginfo([x1, y1, x2, y2])
# We are going to find indecies for the plane and just select the ones that the window covers
# TODO: COrrect this method. It's not quite right...
p1 = np.array([model_wall.x[x1], model_wall.y[x1], model_wall.z[y2]])
p2 = np.array([model_wall.x[x2], model_wall.y[x2], model_wall.z[y2]])
p3 = np.array([model_wall.x[x1], model_wall.y[x1], model_wall.z[y1]])
window = Boundary(p1[0], p1[1], p1[2], p2[0], p2[1], p2[2], p3[0], p3[2], p3[2], "window", self.s.get_lightsource())
window.plot(self.ax)
self.model_windows.append(window)
rospy.loginfo(self.s.date.timestamp())
return PoseArray()
def generate_keyframes_for_inspection(self, segmented_image):
# Something someting
rospy.loginfo("I'm in the generate keyframes for inspection function now")
return_poses = PoseArray()
return return_poses
def solar_button_callback(self, event):
rospy.loginfo("Analyzing mission planes for solar reflections for the next 30 minutes")
time_range = range(int(self.s.date.timestamp()), int(self.s.date.timestamp() + 30.0*60.0), 120) # generate a time-instance every 30th second? 30 minutes
for i in range(len(time_range)):
self.s.date = datetime.datetime.fromtimestamp(time_range[i], tz=pytz.timezone("Europe/Copenhagen"))
self.s.cast_on(self.model_windows, self.ax)
self.fig.canvas.draw_idle()
self.m.check_for_reflection(self.s, self.ax)
rospy.loginfo("Done analyzing mission planes for solar reflections for the next 30 minutes")
def brushfirebutton_callback(self, event):
self.m.generate_mission(self.ax)
def draw_thread(self):
thread_rate = rospy.Rate(20) # Hz
plt.show()
while not rospy.is_shutdown():
#self.fig.canvas.draw_idle()
try: # prevent garbage in console output when thread is killed
thread_rate.sleep()
except rospy.ROSInterruptException:
pass