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():