content: #!/usr/bin/env python
  
  
  import cv2
  import sys
  import yaml
  
  import signal
  import numpy as np
  #import utm
  
  
  import matplotlib as mpl
  import matplotlib.cm as cm
  
  import rospy
  
  import argparse
  
  import actionlib
  
  
  from cosmos_msgs.msg import KrigInfo
  from cosmos_msgs.srv import CompareModels
  import kriging_exploration.map_coords
  import std_msgs.msg
  
  import open_nav.msg
  
  from kriging_exploration.data_grid import DataGrid
  from kriging_exploration.map_coords import MapCoords
  from kriging_exploration.visualiser import KrigingVisualiser
  from kriging_exploration.canvas import ViewerCanvas
  from kriging_exploration.topological_map import TopoMap
  from kriging_exploration.exploration import ExplorationPlan
  
  from sensor_msgs.msg import NavSatFix
  
  
  def overlay_image_alpha(img, img_overlay):
      """Overlay img_overlay on top of img at the position specified by
      pos and blend using alpha_mask.
      """
      show_image = img.copy()
      alpha =  img_overlay[:, :, 3] / 255.0   # Alpha mask must contain values 
                                              # within the range [0, 1] 
                                              # and be the same size as img_overlay.
      # Image ranges
      y1, y2 = 0, img.shape[0]
      x1, x2 = 0, img.shape[1]
  
      channels = img.shape[2]
  
      alpha_inv = 1.0 - alpha
  
      for c in range(channels):
          show_image[y1:y2, x1:x2, c] = (alpha * img_overlay[y1:y2, x1:x2, c] + alpha_inv * img[y1:y2, x1:x2, c])
      return show_image
  
  
  class Explorator(KrigingVisualiser):
  
      #_w_shape=[(0, 16), (1, 17), (3, 17), (5, 16), (8, 15), (10, 15), (12, 14), (14, 13), (12, 12), (10, 11), (8, 11), (5, 10), (8, 9), (10, 9), (12, 8), (14, 7), (12, 6), (10, 5), (8, 5), (6, 4), (4, 3), (3, 2), (4, 1), (5, 0), (7, 0)]
      #_w_shape=[(17, 0), (17, 1), (17, 3), (16, 5), (15, 8), (15, 10), (14, 12), (13, 14), (12, 12), (11, 10), (11, 8), (10, 5), (9, 8), (9, 10), (8, 12), (7, 14), (6, 12), (5, 10), (5, 8), (4, 6), (3, 4), (2, 3), (1, 4), (0, 5), (0, 7)]
      #_w_shape=[(17, 0), (17,1), (17, 2), (17, 4), (16, 4), (16, 6), (16, 8), (15, 8), (15, 10), (14, 10), (14, 12), (13, 12), (13, 14), (12, 14), (12, 12), (11, 12), (11, 10), (10, 10), (10, 8), (10, 6), (10, 4), (9, 4), (9, 6), (9, 8), (9, 10), (8, 10), (8, 12), (7, 12), (7, 14), (6, 14), (6, 12), (5, 12), (5, 10), (4, 10), (4, 8), (4, 6), (4, 4), (3, 4), (3, 3), (2, 3), (2, 4), (1,4), (1, 6), (0,6), (1, 8), (0,8), (1, 10), (0, 10), (0, 12), (0, 14)]
      _w_shape=[(17, 0), (16, 1), (14, 6), (12, 11), (10, 14), (8, 9), (5, 14), (3, 11), (2, 6), (0, 3)]
      def __init__(self, lat_deg, lon_deg, zoom, size, args):
          self.targets = []
          self.results =[]
          self.result_counter=0
          self.explodist=0
          self.running = True
          self.last_coord=None
          signal.signal(signal.SIGINT, self.signal_handler)
          self.expid=args.experiment_name
          print "Creating visualiser object"
          super(Explorator, self).__init__(lat_deg, lon_deg, zoom, size)
  
          cv2.namedWindow('explorator')
          cv2.setMouseCallback('explorator', self.click_callback)
  
          self.current_model=-1
          self.draw_mode = 'none'
          self.grid = DataGrid(args.limits_file, args.cell_size)
          self.topo_map= TopoMap(self.grid)
          self.visited_wp=[]
  
          explo_type = args.area_coverage_type
          self.define_exploration_type(explo_type)
          
              
          self.navigating = False
          self.pause_exp = False
          self.exploring = 0
          self.n_inputs = 0
          
          print "NUMBER OF TARGETS:"
          print len(self.explo_plan.targets)        
          
          self.limits_canvas = ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res)
          self.grid_canvas = ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res)
          self.exploration_canvas = ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res)
          self.gps_canvas = ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res)
  
          self.limits_canvas.draw_polygon(self.grid.limits, (0,0,255,128), thickness=1)
          self.grid_canvas.draw_grid(self.grid.cells, args.cell_size, (128,128,128,2), thickness=1)
          
          self.redraw()
  
          self.redraw_kriged=True
          self.redraw_var=True
          self.redraw_devi=True
          
          self.model_canvas=[]
          self.model_legend=[]
          self.kriging_canvas=[]
          self.klegend_canvas=[]
          self.klegend2_canvas=[]
          self.klegend3_canvas=[]
          self.sigma_canvas=[]
          self.sigma2_canvas=[]
          self.model_canvas_names=[]
          
          self.mean_out_canvas = ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res)
          self.mean_out_legend_canvas = ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res)
          self.mean_var_canvas = ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res)
          self.mean_var_legend_canvas = ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res)
          self.mean_dev_canvas = ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res)
          self.mean_dev_legend_canvas = ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res)        
  
          rospy.loginfo("Subscribing to Krig Info")
          rospy.Subscriber("/kriging_data", KrigInfo, self.data_callback)
          rospy.Subscriber("/fix", NavSatFix, self.gps_callback)
          rospy.Subscriber('/penetrometer_scan', std_msgs.msg.String, self.scan_callback)
          self.req_data_pub = rospy.Publisher('/request_scan', std_msgs.msg.String, latch=False, queue_size=1)
  
          rospy.loginfo(" ... Connecting to Open_nav")
          
          self.open_nav_client = actionlib.SimpleActionClient('/open_nav', open_nav.msg.OpenNavAction)
          self.open_nav_client.wait_for_server()
  
          rospy.loginfo(" ... done")
  
  
          tim1 = rospy.Timer(rospy.Duration(0.2), self.drawing_timer_callback)
          tim2 = rospy.Timer(rospy.Duration(0.1), self.control_timer_callback)
          self.refresh()
  
          while(self.running):
              cv2.imshow('explorator', self.show_image)
              k = cv2.waitKey(20) & 0xFF
              self._change_mode(k)
  
          tim1.shutdown()
          tim2.shutdown()
          cv2.destroyAllWindows()       
          sys.exit(0)
  
  
      # EXPLORATION PARAMS HERE!!!!
      def define_exploration_type(self, explo_type):
          self.exploration_strategy=explo_type    
          self.n_goals=10   
          
          if explo_type=='area_split':
              self.grid._split_area(3,3)
              sb=[]
              for i in self.grid.area_splits_coords:
                  (y, x) = self.grid.get_cell_inds_from_coords(i)
                  sb.append((x,y))
              self.explo_plan = ExplorationPlan(self.topo_map, args.initial_waypoint, args.initial_percent, ac_model=explo_type, ac_coords=sb)
          elif explo_type=='random':
              self.explo_plan = ExplorationPlan(self.topo_map, args.initial_waypoint, args.initial_percent)
          elif explo_type=='w_shape':
              self.explo_plan = ExplorationPlan(self.topo_map, args.initial_waypoint, args.initial_percent, ac_model=explo_type, ac_coords=self._w_shape)
          else: #greedy
              self.explo_plan = ExplorationPlan(self.topo_map, args.initial_waypoint, args.initial_percent, exploration_type='greedy', ac_model=explo_type)
  
  
  
  
      def drawing_timer_callback(self, event):
          self.refresh()
  
      def control_timer_callback(self, event):
          if self.navigating:
              if self.open_nav_client.simple_state ==2:
                  print "DONE NAVIGATING"
                  self.navigating = False
                  if self.exploring==1:
                      self.exploring=2
          
          elif self.exploring==2:
              if not self.pause_exp:
                  self.explo_plan.explored_wp.append(self.explo_plan.route.pop(0))
                  info_str='Do_reading'
                  self.req_data_pub.publish(info_str)
                  self.exploring=3
          
          elif self.exploring==4:
              if not self.pause_exp:
                  if len(self.explo_plan.route) >0:
                      gg=self.explo_plan.route[0]
                      self.open_nav_client.cancel_goal()
                      targ = open_nav.msg.OpenNavActionGoal()
          
                      targ.goal.coords.header.stamp=rospy.Time.now()
                      targ.goal.coords.latitude=gg.coord.lat
                      targ.goal.coords.longitude=gg.coord.lon
          
                      print "Going TO: ", gg
                      self.exploring=1
                      self.navigating=True
                      self.open_nav_client.send_goal(targ.goal)
                  else:
                      print "Done Exploring"
                      self.exploring = 0
  #        else:
  #            if self.exploring:
  #                print "waiting for new goal"
              
      def gps_callback(self, data):
          if not np.isnan(data.latitude):
              self.gps_canvas.clear_image()
              gps_coord = MapCoords(data.latitude,data.longitude)            
              self.gps_canvas.draw_coordinate(gps_coord,'black',size=2, thickness=2, alpha=255)
              if self.last_coord:
                  dist = gps_coord - self.last_coord
                  self.explodist+= dist[0]
              self.last_coord=gps_coord
  
  
  
      def data_callback(self, msg):
          point_coord = kriging_exploration.map_coords.coord_from_satnav_fix(msg.coordinates)
          for i in msg.data:
              self.grid.add_data_point(i.model_name, point_coord, i.measurement)
  
          self.vmin, self.vmax = self.grid.get_max_min_vals()
          self.n_models=len(self.grid.models)
          
          for i in self.grid.models:
              if i.name not in self.model_canvas_names:       
                  print i.name
                  self.model_canvas_names.append(i.name)
                  self.model_canvas.append(ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res))
                  self.model_legend.append(ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res))
                  self.kriging_canvas.append(ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res))
                  self.klegend_canvas.append(ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res))
                  self.klegend2_canvas.append(ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res))
                  self.klegend3_canvas.append(ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res))               
                  self.sigma_canvas.append(ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res))
                  self.sigma2_canvas.append(ViewerCanvas(self.base_image.shape, self.satellite.centre, self.satellite.res))
              self.draw_inputs(self.model_canvas_names.index(i.name))
  
          self.n_inputs+=1
          if self.exploring==3:
              if self.n_inputs>3:
                  self.krieg_all_mmodels()
                  rospy.sleep(0.1)
                  self.grid.calculate_mean_grid()
                  rospy.sleep(0.1)
                  self.draw_means()
                  self.draw_mode="means"
                  
                  resp = self.get_errors()
                  self.result_counter+=1
                  d={}
                  d['step']=self.result_counter
                  d['id']=self.expid
                  d['ns']=len(self.explo_plan.targets)
                  d['coord']={}
                  d['coord']['lat']=self.last_coord.lat
                  d['coord']['lon']=self.last_coord.lon
                  d['dist']=float(self.explodist)
                  d['results']={}
                  d['results']['groundtruth']=resp
                  d['results']['var']={}
                  d['results']['var']['mean']={}
                  d['results']['var']['mean']['mean']= float(np.mean(self.grid.mean_variance))
                  d['results']['var']['mean']['max']= float(np.max(self.grid.mean_variance))
                  d['results']['var']['mean']['min']= float(np.min(self.grid.mean_variance))
                  
  #                d['results']['var']['std']['mean']= np.mean(self.grid.mean_deviation)
  #                d['results']['var']['std']['max']= np.max(self.grid.mean_deviation)
  #                d['results']['var']['std']['min']= np.min(self.grid.mean_deviation)
  
                  means=[]
                  maxs=[]
                  mins=[]
                  for i in range(self.n_models):
                      means.append(float(np.mean(self.grid.models[i].variance)))
                      maxs.append(float(np.max(self.grid.models[i].variance)))
                      mins.append(float(np.min(self.grid.models[i].variance)))
                                  
                  d['results']['models']={}
                  d['results']['models']['means']=means
                  d['results']['models']['maxs']=maxs
                  d['results']['models']['mins']=mins
  
  
                  rospy.sleep(0.1)
                  self.results.append(d)
                  if self.exploration_strategy == 'greedy':
                      nwp = len(self.explo_plan.route) + len(self.explo_plan.explored_wp)
                      print nwp, " nodes in plan"
                      if nwp <= self.n_goals:
                          #THIS IS the ONE
                          #self.explo_plan.add_limited_greedy_goal(self.grid.mean_variance, self.last_coord) 
                          
                          self.explo_plan.add_greedy_goal(self.grid.mean_variance)
                  
                          #self.explo_plan.add_montecarlo_goal(self.grid.mean_variance, self.last_coord)
                  
                  
                  #self.draw_mode="deviation"
  #                self.current_model=0
  #                if self.redraw_devi:
  #                    self.draw_all_devs()
                  self.redraw()
                  rospy.sleep(0.1)
              self.exploring=4
  
      def scan_callback(self, msg):
          if msg.data == 'Reading':
              print "GOT READING!!!"
              cx, cy = self.grid.get_cell_inds_from_coords(self.last_coord)
              if cx <0 or cy<0:
                  print "Reading outside the grid"
              else:
                  print 'Reading at: ', cx, cy
                  for i in self.topo_map.waypoints:
                      if (cy,cx) == i.ind:
                          print 'Setting: ', i.name, i.coord, "as Visited"
                          i.visited= True
                          self.visited_wp.append(i)
                          self.grid_canvas.draw_waypoints(self.topo_map.waypoints, (0,255,0,2), thickness=1)
                          self.grid_canvas.draw_waypoints(self.visited_wp, (0,0,255,2), thickness=1)
                          self.redraw()
  
  
      def refresh(self):
          #self.show_image = self.image.copy()
          #self.show_image = cv2.addWeighted(self.gps_canvas.image, 0.7, self.image, 1.0, 0)
          #self.show_image = transparentOverlay(self.image, self.gps_canvas.image)
          self.show_image = overlay_image_alpha(self.image,self.gps_canvas.image)
  
      def redraw(self):
          self.image = cv2.addWeighted(self.grid_canvas.image, 0.5, self.base_image, 1.0, 0)
          self.image = cv2.addWeighted(self.limits_canvas.image, 0.75, self.image, 1.0, 0)
          self.image = cv2.addWeighted(self.exploration_canvas.image, 0.75, self.image, 1.0, 0)
          if self.draw_mode == "inputs" and self.current_model>=0 :
              self.image = cv2.addWeighted(self.model_canvas[self.current_model].image, 0.75, self.image, 1.0, 0)
              self.image = overlay_image_alpha(self.image, self.model_legend[self.current_model].image)
  
          if self.draw_mode == "kriging":# and self.current_model>=0 :
              self.image = cv2.addWeighted(self.kriging_canvas[self.current_model].image, 0.75, self.image, 1.0, 0)
              #self.image = cv2.addWeighted(self.klegend_canvas[self.current_model].image, 1.0, self.image, 1.0, 0)
              self.image = overlay_image_alpha(self.image, self.klegend_canvas[self.current_model].image)
  
          if self.draw_mode == "deviation":# and self.current_model>=0 :
              self.image = cv2.addWeighted(self.sigma_canvas[self.current_model].image, 0.75, self.image, 1.0, 0)
              #self.image = cv2.addWeighted(self.klegend3_canvas[self.current_model].image, 1.0, self.image, 1.0, 0)
              self.image = overlay_image_alpha(self.image, self.klegend3_canvas[self.current_model].image)
              
          if self.draw_mode == "variance":# and self.current_model>=0 :    
              self.image = cv2.addWeighted(self.sigma2_canvas[self.current_model].image, 0.75, self.image, 1.0, 0)
              #self.image = cv2.addWeighted(self.klegend2_canvas[self.current_model].image, 1.0, self.image, 1.0, 0)
              self.image = overlay_image_alpha(self.image, self.klegend2_canvas[self.current_model].image)
          
          if self.draw_mode == "means":
              self.image = cv2.addWeighted(self.mean_dev_canvas.image, 0.75, self.image, 1.0, 0)
              #self.image = cv2.addWeighted(self.klegend2_canvas[self.current_model].image, 1.0, self.image, 1.0, 0)
              self.image = overlay_image_alpha(self.image, self.mean_dev_legend_canvas.image)
              
          
          self.show_image = self.image.copy()
  
              
  
      def click_callback(self, event, x, y, flags, param):
          
          if event == cv2.EVENT_RBUTTONDOWN:
              click_coord = self.satellite._pix2coord(x,y)
              cx, cy = self.grid.get_cell_inds_from_coords(click_coord)
  
              if cx <0 or cy<0:
                  print "click outside the grid"
              else:
                  print cx, cy
                  
              for i in self.topo_map.waypoints:
                  if (cy,cx) == i.ind:
                      print i.name, i.coord.easting, i.coord.northing
                      i.visited= True
                      self.visited_wp.append(i)
                      self.grid_canvas.draw_waypoints(self.topo_map.waypoints, (0,255,0,2), thickness=1)
                      self.grid_canvas.draw_waypoints(self.visited_wp, (0,0,255,2), thickness=1)
                      self.redraw()
  
          if event == cv2.EVENT_LBUTTONDOWN:
              click_coord = self.satellite._pix2coord(x,y)
              cx, cy = self.grid.get_cell_inds_from_coords(click_coord)
  
              if cx <0 or cy<0:
                  print "click outside the grid"
              else:
                  print cx, cy
  
              for i in self.topo_map.waypoints:
                  if (cy,cx) == i.ind:
                      self.open_nav_client.cancel_goal()
                      targ = open_nav.msg.OpenNavActionGoal()
  
                      #goal.goal.goal.header.
                      targ.goal.coords.header.stamp=rospy.Time.now()
                      targ.goal.coords.latitude=i.coord.lat
                      targ.goal.coords.longitude=i.coord.lon
  
                      print targ
                      self.navigating=True
                      self.open_nav_client.send_goal(targ.goal)
                      #self.client.wait_for_result()
                      # Prints out the result of executing the action
                      #ps = self.client.get_result()
                      #print ps
                  
  
  
      def draw_inputs(self, nm):
          
          minv =  self.grid.models[nm].lims[0]
          maxv =  self.grid.models[nm].lims[1]
  
          if (maxv-minv) <=1:
              maxv = maxv + 50
              minv = minv - 50
              
          norm = mpl.colors.Normalize(vmin=minv, vmax=maxv)
          cmap = cm.jet
          colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
  
          self.model_canvas[nm].clear_image()
          self.model_legend[nm].clear_image()
          
          for i in self.grid.models[nm].orig_data:
              cell = self.grid.cells[i.y][i.x]
              a= colmap.to_rgba(int(i.value))                
              b= (int(a[2]*255), int(a[1]*255), int(a[0]*255), int(a[3]*50))
              self.model_canvas[nm].draw_cell(cell, self.grid.cell_size, b, thickness=-1)
              self.model_canvas[nm].put_text(self.grid.models[nm].name)
          
          self.model_legend[nm].put_text(self.grid.models[nm].name)
          self.model_legend[nm].draw_legend(minv, maxv, colmap, title="Kriging")
          
  
  
  
      def draw_krigged(self, nm):
          print "drawing kriging" + str(nm)
  
          minv =  self.grid.models[nm].min_val
          maxv =  self.grid.models[nm].max_val
  
          if (maxv-minv) <=1:
              maxv = maxv + 50
              minv = minv - 50
  
          norm = mpl.colors.Normalize(vmin=minv, vmax=maxv)
          cmap = cm.jet
          colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
  
  
          self.kriging_canvas[nm].clear_image()
          self.klegend_canvas[nm].clear_image()
          for i in range(self.grid.models[nm].shape[0]):
              for j in range(self.grid.models[nm].shape[1]):
                  cell = self.grid.cells[i][j]
                  a= colmap.to_rgba(int(self.grid.models[nm].output[i][j]))
                  b= (int(a[2]*255), int(a[1]*255), int(a[0]*255), int(a[3]*50))    
                  self.kriging_canvas[nm].draw_cell(cell, self.grid.cell_size, b, thickness=-1)
          
          self.klegend_canvas[nm].put_text(self.grid.models[nm].name)
          self.klegend_canvas[nm].draw_legend(minv, maxv, colmap, title="Kriging")
          
          self.redraw()
  
  
      def draw_variance(self, nm):
          print "drawing variance" + str(nm)
          
          minv =  self.grid.models[nm].min_var
          maxv =  self.grid.models[nm].max_var
          
          if (maxv-minv) <=1:
              maxv = maxv + 50
              minv = minv - 50
          
          norm = mpl.colors.Normalize(vmin=minv, vmax= maxv)
          cmap = cm.jet
          colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
  
          self.sigma_canvas[nm].clear_image()
          self.klegend2_canvas[nm].clear_image()
          
          for i in range(self.grid.models[nm].shape[0]):
              for j in range(self.grid.models[nm].shape[1]):
                  cell = self.grid.cells[i][j]
                  a= colmap.to_rgba(int(self.grid.models[nm].variance[i][j]))
                  b= (int(a[2]*255), int(a[1]*255), int(a[0]*255), int(a[3]*50))
                  self.sigma2_canvas[nm].draw_cell(cell, self.grid.cell_size, b, thickness=-1)
  
  
          self.klegend2_canvas[nm].put_text(self.grid.models[nm].name)
          self.klegend2_canvas[nm].draw_legend(minv, maxv, colmap, title="Variance")
          self.redraw()
  
  
  
  
      def draw_means(self):
          print "drawing mean deviation ..."
          
          minv =  self.grid.min_mean_deviation
          maxv =  self.grid.max_mean_deviation
  
          if (maxv-minv) <=1:
              maxv = maxv + 50
              minv = minv - 50        
          
          
          norm = mpl.colors.Normalize(vmin=minv, vmax=maxv)
          cmap = cm.jet
          colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
  
          self.mean_dev_canvas.clear_image()
          self.mean_dev_legend_canvas.clear_image()
          
          for i in range(self.grid.shape[0]):
              for j in range(self.grid.shape[1]):
                  cell = self.grid.cells[i][j]
                  a= colmap.to_rgba(int(self.grid.mean_deviation[i][j]))
                  b= (int(a[2]*255), int(a[1]*255), int(a[0]*255), int(a[3]*50))
                  self.mean_dev_canvas.draw_cell(cell, self.grid.cell_size, b, thickness=-1)
  
  
          #self.mean_dev_legend_canvas.put_text(self.grid.models[nm].name)
          self.mean_dev_legend_canvas.draw_legend(minv, maxv, colmap, title="Mean Deviation")
          
          #self.draw_mode="means"
          self.redraw()
  
  
  
      def draw_deviation(self, nm):
          print "drawing deviation" + str(nm)
          
          minv =  self.grid.models[nm].min_dev
          maxv =  self.grid.models[nm].max_dev
  
          if (maxv-minv) <=1:
              maxv = maxv + 50
              minv = minv - 50        
          
          
          
          norm = mpl.colors.Normalize(vmin=minv, vmax=maxv)
          cmap = cm.jet
          colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
  
          self.sigma_canvas[nm].clear_image()
          self.klegend3_canvas[nm].clear_image()
          
          for i in range(self.grid.models[nm].shape[0]):
              for j in range(self.grid.models[nm].shape[1]):
                  cell = self.grid.cells[i][j]
                  a= colmap.to_rgba(int(self.grid.models[nm].deviation[i][j]))
                  b= (int(a[2]*255), int(a[1]*255), int(a[0]*255), int(a[3]*50))
                  self.sigma_canvas[nm].draw_cell(cell, self.grid.cell_size, b, thickness=-1)
  
  
          self.klegend3_canvas[nm].put_text(self.grid.models[nm].name)
          self.klegend3_canvas[nm].draw_legend(minv, maxv, colmap, title="Deviation")
          self.redraw()
  
  
      def krieg_all_mmodels(self):
          for i in self.grid.models:
              i.do_krigging()
              self.redraw_kriged=True
              self.redraw_var=True
              self.redraw_devi=True
  
      def draw_all_outputs(self):
          for i in self.grid.models:
              self.draw_krigged(self.model_canvas_names.index(i.name))
              self.redraw_kriged=False
  
  
      def draw_all_vars(self):
          for i in self.grid.models:
              self.draw_variance(self.model_canvas_names.index(i.name))
              self.redraw_var=False
      
      def draw_all_devs(self):
          for i in self.grid.models:
              self.draw_deviation(self.model_canvas_names.index(i.name))
              self.redraw_devi=False
          
      
      def _change_mode(self, k):
          if k == 27:
              self.running = False
          elif k == ord('q'):
              self.running = False
          elif k == ord('n'):
              print len(self.grid.models)
          elif k == ord('i'):
              if self.n_models > 0:
                  self.draw_mode="inputs"
                  self.current_model=0
                  self.redraw()
          elif k == ord('d'):
              if self.n_models > 0:
                  self.draw_mode="deviation"
                  self.current_model=0
                  if self.redraw_devi:
                      self.draw_all_devs()
                  self.redraw()                
          elif k == ord('v'):
              if self.n_models > 0:
                  self.draw_mode="variance"
                  self.current_model=0
                  if self.redraw_var:
                      self.draw_all_vars()
                  self.redraw()
          elif k == ord('t'):
              self.krieg_all_mmodels()
              self.grid.calculate_mean_grid()
              if self.n_models > 0:
                  self.draw_all_outputs()
                  self.draw_mode="kriging"
                  self.current_model=0
                  self.redraw()
  
          elif k == ord('k'):
              if self.n_models > 0:
                  self.draw_mode="kriging"
                  self.current_model=0
                  if self.redraw_kriged:
                      self.draw_all_outputs()
                  self.redraw()
  
          elif k == ord('>'):
              self.current_model+=1
              if self.current_model >= self.n_models:
                  self.current_model=0
              self.redraw()
          elif k == ord('<'):
              self.current_model-=1
              if self.current_model < 0:
                  self.current_model=self.n_models-1
              self.redraw()
          elif k == ord('w'):
              self.grid_canvas.draw_waypoints(self.topo_map.waypoints, (0,255,0,2), thickness=1)
              self.grid_canvas.draw_waypoints(self.visited_wp, (0,0,255,2), thickness=1)
              self.redraw()
          elif k == ord('e'):
              self.exploration_canvas.draw_waypoints(self.explo_plan.targets, (255,200,128,255), thickness=3)
              self.exploration_canvas.draw_plan(self.explo_plan.route, 'cyan', thickness=1)
              self.redraw()
              #xnames = [x.name for x in self.explo_plan.route]
              #print xnames
          elif k == ord('g'):
              if len(self.explo_plan.route) >0:
                  gg=self.explo_plan.route[0]
                  self.open_nav_client.cancel_goal()
                  targ = open_nav.msg.OpenNavActionGoal()
      
                  targ.goal.coords.header.stamp=rospy.Time.now()
                  targ.goal.coords.latitude=gg.coord.lat
                  targ.goal.coords.longitude=gg.coord.lon
      
                  print "Going TO: ", gg
                  self.exploring=1
                  self.navigating=True
                  self.open_nav_client.send_goal(targ.goal)
                  self.result_counter=0
                  self.explodist=0
              else:
                  print "Done Exploring"
                  self.exploring = 0
          elif k == ord('y'):
              vwp = []
              for i in self.visited_wp:
                  vwp.append(i.name)
              yml = yaml.safe_dump(vwp, default_flow_style=False)
              fh = open("visited.yaml", "w")
              s_output = str(yml)
              fh.write(s_output)
              fh.close          
          elif k == ord('l'):
              print "loading visited"
              
              with open("visited.yaml", 'r') as f:
                  visited = yaml.load(f)
                  for i in visited:
                      for l in self.topo_map.waypoints:
                          if i == l.name:
                              self.visited_wp.append(l)
                              break
  
          elif k == ord('a'):
              self.grid.calculate_mean_grid()
              self.draw_means()
              self.draw_mode="means"
  
          elif k == ord('p'):    
              self.pause_exp= not self.pause_exp
              
          elif k == ord('c'):
              print self.grid.limits
              print "Area: ", self.grid.calculate_area(self.grid.limits)
              print "Area of Area: ", self.grid.area.area_size
              colours=['magenta','cyan', 'grey','white','red','yellow','green','blue']
              
              nc=0
              for j in self.grid.area_splits:
                  print j.area_size
                  #self.limits_canvas.draw_coordinate(j.centre, 'crimson', size=3, thickness=2)
                  for i in j.limit_lines:
                      #self.limits_canvas.draw_line(i, colours[nc], thickness=1)
                      self.limits_canvas.draw_line(i, 'white', thickness=1)
                  if nc < len(colours)-1:
                      nc+=1
                  else:
                      nc=0
  
              self.redraw()
              
          elif k== ord('r'):
              #diff = (self.grid.models[1].output - self.grid.models[0].output)
              #print np.mean(diff), np.std(diff), diff.dtype
              print self.get_errors()            
  
          elif k== ord('o'):
              print self.results
              outfile = self.expid + '.yaml'
              #print self.data_out
              yml = yaml.safe_dump(self.results, default_flow_style=False)
              fh = open(outfile, "w")
              s_output = str(yml)
              #print s_output
              fh.write(s_output)
              fh.close
  
              
              
  
      def get_errors(self):
          error_chain=[]
          shapeo = self.grid.models[0].output.shape
          
          #print vals
          print "Waiting for Service"
          rospy.wait_for_service('/compare_model')
          compare_serv = rospy.ServiceProxy('/compare_model', CompareModels)
          
          for i in range(self.n_models):
              try:
                  d={}
                  print "going for it ", i
                  vals = np.reshape(self.grid.models[i].output, -1)
                  resp1 = compare_serv('kriging', i, shapeo[0], shapeo[1], vals.tolist())
                  d['name']= self.grid.models[i].name
                  d['type']= 'kriging'
                  d['errors']={}
                  d['errors']['error']=resp1.error
                  d['errors']['mse']=resp1.mse
                  d['errors']['std']=resp1.std
                  d['errors']['var']=resp1.var
                  #print resp1
                  error_chain.append(d)
              except rospy.ServiceException, e:
                  print "Service call failed: %s"%e    
                  
          try:
              d={}
              print "Mean "
              vals = np.reshape(self.grid.mean_output, -1)
              resp1 = compare_serv('mean', 0, shapeo[0], shapeo[1], vals.tolist())
              #print self.grid.mean_output
              d['name']= 'mean'
              d['type']= 'mean'
              d['errors']={}
              d['errors']['error']=resp1.error
              d['errors']['mse']=resp1.mse
              d['errors']['std']=resp1.std
              d['errors']['var']=resp1.var
  
              #print resp1
              error_chain.append(d)
          except rospy.ServiceException, e:
              print "Service call failed: %s"%e        
          
          
          return error_chain
      
      def signal_handler(self, signal, frame):
          self.running = False
          print('You pressed Ctrl+C!')
  
  
  
  
  
  if __name__ == '__main__':
      parser = argparse.ArgumentParser()
      parser.add_argument("--cell_size", type=int, default=10,
                          help="cell size in meters")
      parser.add_argument("--initial_percent", type=float, default=0.05,
                          help="Percentage of cells to be explored on the initial plan") 
      parser.add_argument("--limits_file", type=str, default='limits.coords',
                          help="Percentage of cells to be explored on the initial plan")
      parser.add_argument("--initial_waypoint", type=str, default='WayPoint498',
                          help="Percentage of cells to be explored on the initial plan")
      parser.add_argument("--area_coverage_type", type=str, default='area_split',
                          help="Type of area coverage, random or area_split")
      parser.add_argument("--experiment_name", type=str, default='exp1',
                          help="Experiment ID")
      args = parser.parse_args()
      
      rospy.init_node('kriging_exploration')
      #Explorator(53.261685, -0.527158, 16, 640, args.cell_size)
      
      #Explorator(53.267213, -0.533420, 17, 640, args)  #Football Field
      Explorator(53.261576, -0.526648, 17, 640, args)  #Half cosmos field
      #Explorator(53.261685, -0.525158, 17, 640, args) #COSMOS Field
  
      
  