def takserver_start(self):
rospy.loginfo(
"============ Connecting to TAK Server at %s:%s ===============" %
(self.tak_ip, self.tak_port))
rospy.loginfo(
"==== If the code appears to freeze at this point, then it is likely the server is not reachable ====="
)
try:
tasksock = self.takserver.open(self.tak_ip, self.tak_port)
self.takserver.flush()
except:
rospy.logerr("Failed to connect to the TAK Server")
exit()
# Send a ping and check for answer
connect_xml = (mkcot.mkcot(
cot_type="t",
cot_how="h-g-i-g-o",
cot_callsign=self.my_callsign,
cot_id=self.my_uid,
team_name=self.my_team_name,
team_role=self.my_team_role)).decode('utf-8')
my_xml = parseString(str(connect_xml.replace("\n", ""))).toprettyxml()
tree = ET.fromstring(my_xml)
xml_callsign = tree.find("./detail/contact").attrib['callsign']
rospy.loginfo(
"SUCCESSFUL Conection with the server. The server believes my callsign is: %s"
% (xml_callsign))
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 iterate_and_send(points: list, waits: list, tak_server: str, tak_port: int,
cot_type: str, callsign: str, uid: str, cot_identity: str,
cot_dimension: str, cot_stale: int):
"""
Iterates through list of coordinate points and wait times and sends them to a TAKServer
Parameters
----------
points : list
list of coordinate points
waits : list
list of times in seconds to wait between locations
tak_server : str
the address of the target TAKServer
tak_port : int
the port for the target TAKServer
cot_type : str
cot type string e.g a-f-G-U-C
cot_identity : str
Cot identity e.g friend
cot_dimension : str
Cot dimension e.g land-unit
cot_stale : int
Time in minuets for the object to become stale in ATAK
callsign : str
the callsign of the object
uid : str
the uid for the object
"""
takserver = takcot()
takserver.open(tak_server, tak_port)
locator = 0
takserver.flush()
for location in points:
takserver.send(
mkcot.mkcot(cot_identity=str(cot_identity),
cot_stale=int(cot_stale),
cot_dimension=str(cot_dimension),
cot_typesuffix=str(cot_type),
cot_callsign=str(callsign),
cot_id=str(uid),
cot_lat=round(location[0], 5),
cot_lon=round(location[1], 5)))
takserver.flush()
try:
time.sleep(waits[locator])
except IndexError:
break
locator += 1
takserver.close()
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 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 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 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))
print("Not a valid user, select 0-" + str(len(users)))
user = ""
continue
except:
user = ""
continue
return (user)
# Open the server connection
takserver = takcot()
testsock = takserver.open(TAK_IP, TAK_PORT)
# Connect- now have to connect with a callsign
takserver.send(
mkcot.mkcot(cot_type="t", cot_how="h-g-i-g-o", cot_callsign=my_call))
sleep(1)
takserver.flush() # flush the xmls the server sends
# Main loop
while True:
user = get_user('From')
print("User " + users[user][0] + " selected")
# Setup the sending user
my_call = users[user][0]
my_uid = users[user][1]
my_team = users[user][2]
print(my_uid)
print(my_call)
def onReceive(packet): # called when a packet arrives
packetType = list(packet['decoded'].keys())[0]
#print(f"{timestamp() packet}") # prints incoming packet to STDOUT
if packetType == 'user':
meshId = packet['from']
meshUserId = packet['decoded']['user']['id']
meshLongName = packet['decoded']['user']['longName']
cotId = meshLongName + "-" + str(uuid.uuid1())[-12:]
users.update({
meshId: {
'meshUserId': meshUserId,
'meshLongName': meshLongName,
'cotId': cotId
}
})
print(f"Received User Packet from {meshLongName}")
print(f"Current Users: {users}")
print()
if packetType == 'position':
meshId = packet['from']
if meshId in users:
meshLongName = users[meshId]['meshLongName']
print(f"Received Position Packet from {meshLongName}")
if 'latitude' in packet['decoded'][
'position'] and 'longitude' in packet['decoded'][
'position']:
lat = packet['decoded']['position']['latitude']
lon = packet['decoded']['position']['longitude']
if lon < 0:
lon = -lon
alt = 0
if 'altitude' in packet['decoded']['position']:
alt = packet['decoded']['position']['altitude']
cotId = users[meshId]['cotId']
print(f"Sending PLI CoT Lat: {lat} Lon: {lon} Alt: {alt}")
takserver.send(
mkcot.mkcot(cot_identity="friend",
cot_stale=10,
cot_dimension="land-unit",
cot_typesuffix="E-C-T",
cot_lat=lat,
cot_lon=-lon,
cot_hae=alt,
cot_id=cotId,
team_name="Red",
cot_callsign=meshLongName))
time.sleep(1)
print("read the response")
print(takserver.read())
print()
if packetType == 'data':
meshId = packet['from']
if meshId in users:
meshLongName = users[meshId]['meshLongName']
cotId = users[meshId]['cotId']
print(f"Received Data Packet from {meshLongName}")
if 'typ' in packet['decoded']['data']:
dataType = packet['decoded']['data']['typ']
if dataType == 'CLEAR_TEXT':
target_msg = packet['decoded']['data']['text']
# Messages have a unique uid- critical
event_uid = "GeoChat." + cotId + "." + "All Chat Rooms" + "." + str(
uuid.uuid4())
print(f"Message: {target_msg}")
cot_xml = mkcot.mkcot(
tgt_call="All Chat Rooms",
tgt_uid="All Chat Rooms",
tgt_msg=target_msg
# can be set as needed
,
cot_type="b-t-f",
cot_how="h-g-i-g-o",
cot_typesuffix=""
# takpak defaults are OK
,
cot_identity="",
cot_dimension="",
cot_id=event_uid
# this differentiates the CoT as a message
,
sender_uid=cotId,
cot_callsign=meshLongName)
takserver.flush() # flush the xmls the server sends
takserver.send(cot_xml)
print()
else:
meshLongName = packet['from']
print(f"Received Data Packet from {meshLongName}")
def ping():
print("send ping")
takserver.send(mkcot.mkcot(cot_id=my_uid + "-ping", cot_type="t-x-c-t"))
print("read the ping response")
print(takserver.read())
print()
# Now open server
print("Opening TAK Server")
testsock = takserver.open(TAK_IP)
#print("open return is:")
#print(testsock)
#connect_xml=cot_xml
print()
print("send a connect")
takserver.flush() # flush the xmls the server sends
#print(takserver.read()) # read all the server CoT's, will send last + the connct
# send the connect string, server does not echo
takserver.send(mkcot.mkcot(cot_type="t", cot_how="h-g-i-g-o"))
#print("read the Connect response")
#print(takserver.read()) # read all the server CoT's, will send last + the connct
print("Flush the server response")
takserver.flush() # flush the xmls the server sends
time.sleep(1)
pingThread = Thread(target=pingTimer)
pingThread.start()
users = {}
interface = meshtastic.StreamInterface()
#pub.subscribe(onNodeUpdate, "meshtastic.node.updated")
pub.subscribe(onReceive, "meshtastic.receive")
# Always need to close out the connection
# good practice to include reading anything the server pushed
def callback(packet):
global aprs_reports
global aprs_reportsmax
global lastcycle
global taksock
#if PAUSE:
# return()
#logger.debug("Callback: " + str(packet))
#logger.setLevel(logging.WARNING)
logger.setLevel(DEFAULT_LEVEL)
try:
aprs_atoms = aprslib.parse(packet)
aprs_format = aprs_atoms["format"]
logger.debug("APRS format: " + str(aprs_format))
if (aprs_format == "beacon"):
#print("beacon skipped:" + str(packet))
aprs_source = ""
pass # Skip the rest
return
else:
#print(aprs_atoms)
try:
aprs_source = aprs_atoms["from"]
except:
logger.debug("No APRS From")
aprs_source = ""
try:
aprs_lat = str(round(aprs_atoms["latitude"], 5))
aprs_lon = str(round(aprs_atoms["longitude"], 5))
except:
logger.debug(aprs_source + " No Lat/Lon")
aprs_lat = ""
aprs_lon = ""
next
try:
aprs_alt = round(aprs_atoms["altitude"], 0)
if (aprs_alt < 0):
aprs_alt = 0
aprs_alt = str(aprs_alt)
except:
#print("No Altitude")
aprs_alt = "9999999.0"
# placeholders
#aprs_speed = 0
#aprs_course = 0
except (aprslib.ParseError, aprslib.UnknownFormat) as exp:
logger.debug("APRS parse failed:" + str(packet))
aprs_source = ""
pass
return
except:
logger.debug("APRS parse failed other error:" + str(packet))
aprs_source = ""
pass
return
#logger.setLevel(logging.DEBUG)
logger.setLevel(DEFAULT_LEVEL)
# Now try to make the CoT -------------------------------------------
try:
if aprs_source and aprs_lat and aprs_lon:
# see if it's a real user
#if aprs_source.startswith("K"):
# aprs_source = "KM4BA"
if usercheck and aprs_source in (x[0] for x in users):
logger.debug("User " + aprs_source + " found in users list")
for i in range(0, len(users)):
logger.debug("checking: " + users[i][0] + " " +
users[i][1] + " " + users[i][2])
if users[i][0] == aprs_source:
aprs_uid = users[i][1]
aprs_team = users[i][2]
aprs_icon = ""
aprs_point = False
logger.debug("Match: " + aprs_source + " " + aprs_uid +
" " + aprs_team)
break
else:
#logger.debug("User: "******" not found")
pass
logger.debug("User: "******" " + aprs_uid + " " +
aprs_team + " ---------")
else:
# not a real user, so set an icon instead
#aprs_uid=aprs_source + str(uuid.uuid1())
aprs_uid = aprs_source
#aprs_team="Cyan"
#aprs_source=""
aprs_team = ""
aprs_icon = 'f7f71666-8b28-4b57-9fbb-e38e61d33b79/Google/placemark_circle.png'
aprs_point = True
logger.debug("station: " + aprs_source + " " + aprs_uid + " " +
aprs_team + " ---------")
#cot_xml = injectCoT.inject_cot(aprs_source, aprs_lat, aprs_lon, aprs_alt)
try:
# create the CoT from the info
cot_xml = mkcot.mkcot(
cot_callsign=aprs_source,
cot_id=aprs_uid,
team_name=aprs_team,
cot_lat=aprs_lat,
cot_lon=aprs_lon,
cot_hae=aprs_alt,
cot_stale=10,
cot_how="h-g-i-g-o"
#, cot_type="a" # not needed, default
,
cot_identity="friend",
cot_dimension="land-unit",
cot_typesuffix="E-C-V",
iconpath=aprs_icon,
cot_point=aprs_point)
except:
logger.debug("mkcot failed")
else:
logger.info("Not useful APRS: " + packet)
pass
except:
logger.debug("CoT creation failed: " + packet)
cot_xml = ""
return
if cot_xml:
try:
if logger.level <= logging.DEBUG:
my_xml = cot_xml.decode('utf-8')
my_xml = parseString(str(my_xml.replace("\n", "")))
xml_pretty_str = my_xml.toprettyxml()
logger.debug("CoT: " + aprs_source)
logger.debug("CoT XML is: " + xml_pretty_str)
#if not aprs_point:
# logger.warning("CoT XML is: " + xml_pretty_str)
except:
cot_xml = ""
logger.debug("XML parse failed")
return
#logger.setLevel(logging.DEBUG)
if cot_xml:
try:
#simulate = True
flushtimeout = 0.05
# flush any pending server responses (not needed?)
#if simulate:
# logger.debug("Simulated flush")
#else:
# takserver.flush(readtimeout=flushtimeout)
# send the CoT string, server does not echo
if simulate:
logger.info("Simulated Send")
else:
try:
takserver.send(cot_xml)
if DEFAULT_LEVEL >= logging.WARNING:
# Slow things down a bit if not printing to console
#print("sleep for a bit")
time.sleep(0.1)
except socket.timeout:
logger.error("Socket Timeout- send failed")
raise
except KeyboardInterrupt:
#self.cleanup()
raise
except:
logger.error("Send failed")
raise
#return 1
# flush any server responses
if simulate:
logger.debug("Simulated flush")
else:
takserver.flush(readtimeout=flushtimeout)
# Now log the report by type
#if sleeptime > 0.1:
if True and aprs_source and aprs_lat and aprs_lon:
if aprs_point:
logger.info(
"Station: " + aprs_source + " Lat:" + aprs_lat +
" Lon:" + aprs_lon + " Alt:" + aprs_alt
#+ " Speed:" + aprs_speed + " Course:" + aprs_course
+ " Counter: " + str(aprs_reports))
else:
logger.warning(
"User: "******" Lat:" + aprs_lat +
" Lon:" + aprs_lon + " Alt:" + aprs_alt
#+ " Speed:" + aprs_speed + " Course:" + aprs_course
+ " Counter: " + str(aprs_reports))
#logger.warning(cot_xml)
else:
logger.debug("APRS not useful")
except:
logger.warning("APRS CoT push to server failed")
raise
#return 1
# Increment the report count
aprs_reports += 1
#logger.debug("End of callback processing -----------------------------------")
if aprs_reports > aprs_reportsmax:
try:
#print("aprs_reports:" + str(aprs_reports))
cycletime = int(time.time() - lastcycle)
lastcycle = time.time()
logger.warning(
"Close and Reopen the TAK connection " +
str(aprs_reports - 1) + " reports "
#+ time.strftime("%d/%m/%y %H:%M:%S ", time.gmtime())
)
#print( "Close and Reopen the TAK connection " )
#print( str(aprs_reportsmax / cycletime ) + " Reports / sec")
logger.warning(
str(round(aprs_reportsmax / cycletime, 2)) + " Reports / sec "
#+ aprs_reportsmax + " reports"
)
except:
logger.debug("Reopen Stat Calc failed")
try:
takserver.close()
time.sleep(0.5)
logger.debug(server + " Closed")
except:
logger.warning(server + " close failed")
pass
logger.debug("Opening TAK Server " + server + " " + TAK_IP + ":" +
str(TAK_PORT))
try:
# Now open server
try:
testsock = takserver.open(TAK_IP, TAK_PORT)
except:
logger.warning(server + " reopen failed")
logger.debug("send a takserver connect")
try:
takserver.flush(
) # flush the xmls the server sends (should not be any)
# send the connect string, server does not echo
takserver.send(
mkcot.mkcot(cot_type="t",
cot_how="h-g-i-g-o",
cot_callsign=my_callsign))
logger.warning(
"Server " + server +
" reconnected -------------------------------------")
aprs_reports = 1
except:
logger.warning("Server " + server + " connect failed")
except:
logger.warning(server + " Reopen failed")
my_uid = my_uid + "-" + str(uuid.uuid1())
#my_uid = bytes("APRS_inject-" + my_uid,"UTF-8")
my_uid = ("APRS_inject-" + my_uid)
#print(my_uid)
taksock = "" # this is a global
# Setup a callsign for the main process
my_callsign = "APRS-" + my_uid[-6:]
logger.debug("Callsign: " + str(my_callsign))
# substantiate the class
takserver = takcot()
connect_xml = mkcot.mkcot(cot_type="t",
cot_how="h-g-i-g-o",
cot_callsign=my_callsign)
my_xml = connect_xml.decode('utf-8')
my_xml = parseString(str(my_xml.replace("\n", "")))
xml_pretty_str = my_xml.toprettyxml()
logger.debug("Connect XML is: " + xml_pretty_str)
lastcycle = time.time()
while True:
try:
# Now open server
logger.debug("Opening TAK Server " + server +
"-------------------------------------------------")
# Now open server
print("Opening TAK Server")
testsock = takserver.open(TAK_IP, TAK_PORT)
#print("open return is:")
#print(testsock)
#connect_xml=cot_xml
print()
print("send a connect")
takserver.flush() # flush the xmls the server sends
#print(takserver.read()) # read all the server CoT's, will send last + the connct
# send the connect string, server does not echo
takserver.send(mkcot.mkcot(cot_type="t", cot_how="h-g-i-g-o"))
#print("read the Connect response")
#print(takserver.read()) # read all the server CoT's, will send last + the connct
print("Flush the server response")
takserver.flush() # flush the xmls the server sends
time.sleep(1)
#for i in range(10):
while True:
delta_lat = math.sin(math.radians(degrees)) / 4
delta_lon = math.cos(math.radians(degrees)) / 4
#print(str(degrees) + " " + str(delta_lat) + " " + str(delta_lon))
# substantiate the class
takserver = takcot()
# Now open server
print("Opening TAK Server")
testsock = takserver.open(TAK_IP)
#print("open return is:")
#print(testsock)
print()
print("send a connect")
takserver.flush() # flush the xmls the server sends
#print(takserver.read()) # read all the server CoT's, will send last + the connct
connect_xml = mkcot.mkcot(cot_type="t", cot_how="h-g-i-g-o")
print("Connect XML is:")
print(connect_xml)
# send the connect string, server does not echo
takserver.send(mkcot.mkcot(cot_type="t", cot_how="h-g-i-g-o"))
# send the connect string, server does not echo
#time.sleep(5)
print("read the Connect response")
print(
takserver.read()) # read all the server CoT's, will send last + the connct
#print("Flush the server response")
#takserver.flush() # flush the xmls the server sends
#time.sleep(3)
# Now open server
logger.debug("Opening TAK Server " + server +
"----------------------------------------------------")
try:
testsock = takserver.open(TAK_IP, TAK_PORT)
except:
logger.error("takserver open failed")
exit()
logger.debug("send a takserver connect")
try:
takserver.flush() # flush the xmls the server sends (should not be any)
except:
logger.error("takserver flush failed")
connect_xml = mkcot.mkcot(cot_type="t", cot_how="h-g-i-g-o")
#connect_xml = mkcot.mkcot(cot_type="t", cot_how="h-g-i-g-o",cot_platform="linux",cot_os="29")
my_xml = connect_xml.decode('utf-8')
my_xml = parseString(str(my_xml.replace("\n", "")))
xml_pretty_str = my_xml.toprettyxml()
logger.debug("Connect XML is: " + xml_pretty_str)
# send the connect string, server does not echo
try:
takserver.send(mkcot.mkcot(cot_type="t", cot_how="h-g-i-g-o"))
except:
logger.error("takserver connect send failed")
exit()
node_name)
rospy.loginfo(
"==== If the code appears to freeze at this point, then it is likely the server is not reachable ====="
)
takserver = takcot() #TODO add a timeout and exit condition
try:
tasksock = takserver.open(tak_ip, tak_port)
takserver.flush()
except:
rospy.logerr("Failed to connect to the TAK Server")
exit()
# Send a ping and check for answer
connect_xml = (mkcot.mkcot(cot_type="t",
cot_how="h-g-i-g-o",
cot_callsign=my_callsign,
cot_id=my_uid,
team_name=my_team_name,
team_role=my_team_role)).decode('utf-8')
my_xml = parseString(str(connect_xml.replace("\n", ""))).toprettyxml()
tree = ET.fromstring(my_xml)
xml_callsign = tree.find("./detail/contact").attrib['callsign']
rospy.loginfo(
"SUCCESSFUL Conection with the server. The server believes my callsign is: %s"
% (xml_callsign))
# Setup and run the main while loop
count = 0 # used to keep connection alive
loop_hz = 5
rate = rospy.Rate(
loop_hz) # The rate of the loop is no faster than then the readtimeout.
while not rospy.is_shutdown():
