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)