def objects_location_cb(self, data): # rospy.loginfo(data) for item in data.pose_list: # Build a PoseStamped as input to transformPose obj_pose_stamped = PoseStamped() obj_pose_stamped.header = data.header obj_pose_stamped.header.stamp = data.header.stamp # Time stamp needed for the transforms obj_pose_stamped.pose = item.pose obj_pose = self.tf1_listener.transformPose("utm", obj_pose_stamped) (obj_latitude,obj_longitude) = UTMtoLL(23, obj_pose.pose.position.y, obj_pose.pose.position.x, self.zone) # 23 is WGS-84. c_id = self.robot_name + item.description.data # rospy.loginfo("Recieved a target of type: %s and sending with ID of: %s" % (item.description.data,c_id)) try: self.takserver.send(mkcot.mkcot(cot_identity="neutral", cot_stale = 1, cot_type="a-n-G-I-c", # TODO find a beter cot type and icon cot_how="m-g", cot_callsign=item.description.data, cot_id= c_id, # team_name=self.my_team_name, team_name='yellow', team_role=self.my_team_role, cot_lat=obj_latitude, cot_lon=obj_longitude )) except: rospy.logdebug("Read Cot failed: %s" % (sys.exc_info()[0]))
def robot_pose_to_tak(self): # Get current position in global frame crnt_pose = self.tf1_listener.lookupTransform('utm', self.baselink_frame, rospy.Time(0)) (crnt_latitude,crnt_longitude) = UTMtoLL(23, crnt_pose[0][1], crnt_pose[0][0], self.zone) # 23 is WGS-84. #(crnt_latitude,crnt_longitude) = (41.39081900138365, -73.9531831888787) my_cot = mkcot.mkcot(cot_identity="friend", cot_stale = 0.1, cot_type="a-f-G-M-F-Q", cot_how="m-g", cot_callsign=self.my_callsign, cot_id=self.my_uid, team_name=self.my_team_name, team_role=self.my_team_role, cot_lat=crnt_latitude, cot_lon=crnt_longitude ) self.takserver.send( my_cot)
def setInitialPose(self): # Make sure we have the initial pose rospy.loginfo("Waiting for initial pose") # Get the initial pose from the robot rospy.loginfo("Initial Pose from /odometry/gps recieved") rospy.loginfo("Establishing initial position wait 10 seconds. Do not move the robot.") easts=[] nrths=[] time.sleep(2) for i in range(5): #TODO move back to 10 utm_c=self.current_utm_odom easts.append(utm_c[1]) #TODO add check for empty array nrths.append(utm_c[2]) rospy.sleep(1.0) easting = sum(easts) / float(len(easts)) northing = sum(nrths) / float(len(nrths)) self.initial_pose = deepcopy(self.current_pose_map) self.initial_utm = [utm_c[0],easting,northing] (latti,longi)=UTMtoLL(23, self.initial_utm[2], self.initial_utm[1], self.initial_utm[0]) rospy.loginfo("Initial UTM at (%.4f, %.4f), Initial Lat/Long is: (%.7f, %.7f)" % (easting, northing,latti,longi) )
def robot_pose_to_tak(self): # Get current position in global frame crnt_pose = self.tf1_listener.lookupTransform('utm', self.baselink_frame, rospy.Time(0)) (crnt_latitude, crnt_longitude) = UTMtoLL(23, crnt_pose[0][1], crnt_pose[0][0], self.zone) # 23 is WGS-84. # Send the current position to the TAK Server #rospy.loginfo("latlong: %.7f,%.7f baselinkg is: %s"%(crnt_latitude,crnt_longitude, self.baselink_frame)) my_cot = mkcot.mkcot(cot_identity="friend", cot_stale=1, cot_type="a-f-G-M-F-Q", cot_how="m-g", cot_callsign=self.my_callsign, cot_id=self.my_uid, team_name=self.my_team_name, team_role=self.my_team_role, cot_lat=crnt_latitude, cot_lon=crnt_longitude) self.takserver.send(my_cot)
def robot_location_cb(self, data): # rospy.loginfo(data) # Build a PoseStamped as input to transformPose robot_pose_stamped = PoseStamped() robot_pose_stamped.header = data.header robot_pose_stamped.header.stamp = data.header.stamp # Time stamp needed for the transforms robot_pose_stamped.pose = data.pose.pose robot_pose = self.tf1_listener.transformPose("utm", robot_pose_stamped) (crnt_latitude,crnt_longitude) = UTMtoLL(23, robot_pose.pose.position.y, robot_pose.pose.position.x, self.zone) # 23 is WGS-84. try: my_cot = mkcot.mkcot(cot_identity="friend", cot_stale = 0.1, cot_type="a-f-G-M-F-Q", cot_how="m-g", cot_callsign=self.my_callsign, cot_id=self.my_uid, team_name=self.my_team_name, team_role=self.my_team_role, cot_lat=crnt_latitude, cot_lon=crnt_longitude ) self.takserver.send( my_cot) except: rospy.logdebug("Read Cot failed: %s" % (sys.exc_info()[0]))
def grnd_object_cb(self, data): for item in data.markers: if item.ns in self.target_list: obj_pose_stamped = PoseStamped() obj_pose_stamped.header = item.header obj_pose_stamped.header.stamp = rospy.Time.now() obj_pose_stamped.pose = item.pose obj_pose = self.tf1_listener.transformPose( "utm", obj_pose_stamped) (obj_latitude, obj_longitude) = UTMtoLL(23, obj_pose.pose.position.y, obj_pose.pose.position.x, self.zone) # 23 is WGS-84. self.takserver.send( mkcot.mkcot(cot_identity="neutral", cot_stale=1, cot_type="a-f-G-M-F-Q", cot_how="m-g", cot_callsign=item.ns, cot_id="ugv_object", team_name="Green", team_role="Team Member", cot_lat=obj_latitude, cot_lon=obj_longitude))
def object_location_cb(self, data): for detection in data.detections: for result in detection.results: if result.id in self.target_list: obj_pose_stamped = PoseStamped() obj_pose_stamped.header = detection.header obj_pose_stamped.pose = result.pose.pose obj_pose_utm = self.tf1_listener.transformPose( "utm", obj_pose_stamped) (obj_latitude, obj_longitude) = UTMtoLL(23, obj_pose_utm.pose.position.y, obj_pose_utm.pose.position.x, self.zone) # 23 is WGS-84. self.takserver.send( mkcot.mkcot(cot_identity="neutral", cot_stale=1, cot_type="a-f-G-M-F-Q", cot_how="m-g", cot_callsign=result.id, cot_id="object", team_name="detector", team_role="obj detector", cot_lat=obj_latitude, cot_lon=obj_longitude))
def apply_model(model, tile_dim, image_path, output_path, **kwargs): tile_path = kwargs.pop('tile_path', None) prob_thresh = kwargs.pop('prob_thresh', 0.0) calc_salience = kwargs.pop('calc_salience', True) tile_stride = kwargs.pop('tile_stride', None) hdr_path = kwargs.pop('hdr_path', None) hdr_suffix = kwargs.pop('hdr_suffix', None) verbose = kwargs.pop('verbose', False) if isdir(image_path): image_files = glob(pathjoin(image_path, '*' + image_ext)) else: image_files = [image_path] hdr_files = {} if hdr_path: for imagef in image_files: imgid = filename2flightid(imagef) hdrfiles = glob( pathjoin(hdr_path, imgid + '*' + hdr_suffix + '*.hdr')) msgtup = (imgid, hdr_path, hdr_suffix) if len(hdrfiles) == 0: warn('no matching hdr for %s in %s with suffix %s' % msgtup) return hdrf = hdrfiles[0] if len(hdrfiles) > 1: msg = 'multiple .hdr files for %s in %s with suffix %s' % msgtup msg += '(using %s)' % hdrf warn(msg) imgmap = mapinfo(openimg(hdrf.replace('.hdr', ''), hdrf=hdrf), astype=dict) imgmap['rotation'] = -imgmap['rotation'] hdr_files[imgid] = imgmap for imagef in image_files: imgid = filename2flightid(imagef) imgmap = hdr_files.get(imgid, None) image_test = imread_image(imagef) if calc_salience: print('Computing salience map for image id {}'.format(imgid)) salience_uls = collect_salience_uls(image_test, tile_dim, tile_stride=tile_stride) salience_tile_uls = gen_salience_tiles(image_test, salience_uls, tile_dim) salience_out = predict_tiles(image_test, salience_tile_uls, len(salience_uls), verbose=verbose) output_prefix = '_'.join([imgid, 'salience']) plot_pred_images(salience_out, mapinfo=imgmap, output_dir=output_dir, output_prefix=output_prefix, mask_zero=False) if tile_path is None: continue tile_files = collect_tile_files(imgid, tile_path) if len(tile_files) != 0: file_tile_uls = gen_file_tiles(tile_files) pred_out = predict_tiles(image_test, file_tile_uls, len(tile_files), verbose=verbose) if plot_predictions: plot_pred_images(pred_out, mapinfo=imgmap, output_dir=output_dir, output_prefix=imgid, mask_zero=True) # geolocate detections with .hdr files if imgmap: from LatLongUTMconversion import UTMtoLL zone, hemi = imgmap['zone'], imgmap['hemi'] zonealpha = zone + ('N' if hemi == 'North' else 'M') tile_out = pred_out['tile_preds'] keep_mask = float32(tile_out[:, -2]) >= prob_thresh tile_keep = tile_out[keep_mask, :] tile_center = np.float32( tile_keep[:, [1, 2]]) + (tile_shape[0] // 2) line, samp = tile_center.T utmx, utmy = sl2xy(samp, line, mapinfo=imgmap) lats, lons = UTMtoLL(23, utmy, utmx, zonealpha) preds, probs = tile_keep[:, -1], tile_keep[:, -2] csvf = pathjoin(output_dir, imgid + '_preds%d.csv' % int(prob_thresh)) outcsv = [] for i in range(tile_keep.shape[0]): entryi = [ imgid, '%d' % samp[i], '%d' % line[i], '%18.4f' % utmx[i], '%18.4f' % utmy[i], zone, hemi, lats[i], lons[i], preds[i], probs[i] ] outcsv.append(', '.join( map(lambda v: str(v).strip(), entryi))) with (open(csvf, 'w')) as fid: fid.write('\n'.join(outcsv) + '\n') print('saved', csvf)