Xuan0319
/
DroneControl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add gps log file

Browse Source
protobuf
RangeOfGlitching 3 years ago
parent 17e375a850
commit a5f98d6694
14 changed files with 168 additions and 21 deletions
Show Stats Download Patch File Download Diff File

18
class_model/src/mqttPubMain.py
Unescape Escape View File

@ -42,11 +42,11 @@ def init_dataFormat(cfg:utils.Read_PUB_Config):
rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps)
rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)
else:
logging.debug("msg_format not found")
logger.debug("msg_format not found")
def on_connect(self, userdata, flags, rc):
logger.info("Connected with result code " + str(rc))
logger.debug("Connected with result code " + str(rc))
def on_publish(self, userdata, mid):
logger.debug("pub success")
@ -63,22 +63,24 @@ if __name__ == '__main__':
client = utils.MQTTClient(cfg.MQTTClientNamePub)
# set log
log_format = "%(asctime)s - %(levelname)s - %(message)s"
formatter = logging.Formatter(log_format)
stream_log_format = "%(asctime)s - %(levelname)s - %(message)s"
file_log_format = "%(message)s"
file_formatter = logging.Formatter(file_log_format)
stream_formatter = logging.Formatter(stream_log_format)
stream_handler = logging.StreamHandler()
stream_handler.setFormatter(formatter)
stream_handler.setFormatter(stream_formatter)
stream_handler.setLevel(logging.DEBUG)
file_handler = logging.FileHandler(cfg.logFileName)
file_handler.setFormatter(formatter)
file_handler.setFormatter(file_formatter)
file_handler.setLevel(logging.INFO)
logger = logging.getLogger(__name__)
logger.setLevel(logging.INFO)
logger.addHandler(file_handler)
logger.addHandler(stream_handler)
logger.info(cfg)
logger.debug(cfg)
# Mqtt
client.on_connect = on_connect
@ -98,6 +100,6 @@ if __name__ == '__main__':
rospy.spin()
except BaseException as error:
client.disconnect()
logger.info("End of program")
logger.debug("End of program")
sys.exit()

2
class_model/src/uavlinkPubMain.py
Unescape Escape View File

@ -20,7 +20,7 @@ from sensor_msgs.msg import Range
from geometry_msgs.msg import Vector3
def init_dataFormat(cfg:Read_PUB_Config):
def init_dataFormat(cfg):
Proto_msg_from_ros.sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE)
Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()
Proto_msg_from_ros.rate = rospy.Rate(5)

2
class_model/src/uavlinkSubMain.py
Unescape Escape View File

@ -23,7 +23,7 @@ from geometry_msgs.msg import Vector3
from class_model.msg import FlightInformation
def init_dataFormat(cfg:Read_SUB_Config):
def init_dataFormat(cfg):
Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message()
Proto_msg_to_ros.rate = rospy.Rate(10)
Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.Dron550_ROStopicName_Flight_Information,FlightInformation,queue_size=10)

3
class_model/src/utils/.idea/.gitignore vendored
Unescape Escape View File

@ -0,0 +1,3 @@
# Default ignored files
/shelf/
/workspace.xml

104
class_model/src/utils/.idea/inspectionProfiles/Project_Default.xml
Unescape Escape View File

@ -0,0 +1,104 @@
<component name="InspectionProjectProfileManager">
<profile version="1.0">
<option name="myName" value="Project Default" />
<inspection_tool class="PyPackageRequirementsInspection" enabled="true" level="WARNING" enabled_by_default="true">
<option name="ignoredPackages">
<value>
<list size="84">
<item index="0" class="java.lang.String" itemvalue="mysqlclient" />
<item index="1" class="java.lang.String" itemvalue="protobuf" />
<item index="2" class="java.lang.String" itemvalue="pywinpty" />
<item index="3" class="java.lang.String" itemvalue="mysql-connector" />
<item index="4" class="java.lang.String" itemvalue="PyYAML" />
<item index="5" class="java.lang.String" itemvalue="atomicwrites" />
<item index="6" class="java.lang.String" itemvalue="QtPy" />
<item index="7" class="java.lang.String" itemvalue="cycler" />
<item index="8" class="java.lang.String" itemvalue="MarkupSafe" />
<item index="9" class="java.lang.String" itemvalue="locket" />
<item index="10" class="java.lang.String" itemvalue="patsy" />
<item index="11" class="java.lang.String" itemvalue="tables" />
<item index="12" class="java.lang.String" itemvalue="appdirs" />
<item index="13" class="java.lang.String" itemvalue="contextlib2" />
<item index="14" class="java.lang.String" itemvalue="conda-verify" />
<item index="15" class="java.lang.String" itemvalue="mccabe" />
<item index="16" class="java.lang.String" itemvalue="pyzmq" />
<item index="17" class="java.lang.String" itemvalue="certifi" />
<item index="18" class="java.lang.String" itemvalue="HeapDict" />
<item index="19" class="java.lang.String" itemvalue="entrypoints" />
<item index="20" class="java.lang.String" itemvalue="mysql" />
<item index="21" class="java.lang.String" itemvalue="anaconda-navigator" />
<item index="22" class="java.lang.String" itemvalue="bkcharts" />
<item index="23" class="java.lang.String" itemvalue="comtypes" />
<item index="24" class="java.lang.String" itemvalue="pywin32" />
<item index="25" class="java.lang.String" itemvalue="clyent" />
<item index="26" class="java.lang.String" itemvalue="navigator-updater" />
<item index="27" class="java.lang.String" itemvalue="terminado" />
<item index="28" class="java.lang.String" itemvalue="wincertstore" />
<item index="29" class="java.lang.String" itemvalue="xlwings" />
<item index="30" class="java.lang.String" itemvalue="win-unicode-console" />
<item index="31" class="java.lang.String" itemvalue="pyodbc" />
<item index="32" class="java.lang.String" itemvalue="boto" />
<item index="33" class="java.lang.String" itemvalue="h5py" />
<item index="34" class="java.lang.String" itemvalue="wrapt" />
<item index="35" class="java.lang.String" itemvalue="menuinst" />
<item index="36" class="java.lang.String" itemvalue="QDarkStyle" />
<item index="37" class="java.lang.String" itemvalue="fastcache" />
<item index="38" class="java.lang.String" itemvalue="anaconda-client" />
<item index="39" class="java.lang.String" itemvalue="cytoolz" />
<item index="40" class="java.lang.String" itemvalue="widgetsnbextension" />
<item index="41" class="java.lang.String" itemvalue="multipledispatch" />
<item index="42" class="java.lang.String" itemvalue="mkl-service" />
<item index="43" class="java.lang.String" itemvalue="scikit-image" />
<item index="44" class="java.lang.String" itemvalue="Bottleneck" />
<item index="45" class="java.lang.String" itemvalue="pathspec" />
<item index="46" class="java.lang.String" itemvalue="brotlipy" />
<item index="47" class="java.lang.String" itemvalue="pycurl" />
<item index="48" class="java.lang.String" itemvalue="json5" />
<item index="49" class="java.lang.String" itemvalue="mypy-extensions" />
<item index="50" class="java.lang.String" itemvalue="pep8" />
<item index="51" class="java.lang.String" itemvalue="pycosat" />
<item index="52" class="java.lang.String" itemvalue="llvmlite" />
<item index="53" class="java.lang.String" itemvalue="numpy" />
<item index="54" class="java.lang.String" itemvalue="simplegeneric" />
<item index="55" class="java.lang.String" itemvalue="mysql-connector-python" />
<item index="56" class="java.lang.String" itemvalue="jdcal" />
<item index="57" class="java.lang.String" itemvalue="mkl-fft" />
<item index="58" class="java.lang.String" itemvalue="jupyter" />
<item index="59" class="java.lang.String" itemvalue="backports.weakref" />
<item index="60" class="java.lang.String" itemvalue="pkginfo" />
<item index="61" class="java.lang.String" itemvalue="pywin32-ctypes" />
<item index="62" class="java.lang.String" itemvalue="conda" />
<item index="63" class="java.lang.String" itemvalue="zict" />
<item index="64" class="java.lang.String" itemvalue="xmltodict" />
<item index="65" class="java.lang.String" itemvalue="sip" />
<item index="66" class="java.lang.String" itemvalue="pyreadline" />
<item index="67" class="java.lang.String" itemvalue="zope.event" />
<item index="68" class="java.lang.String" itemvalue="opencv-python" />
<item index="69" class="java.lang.String" itemvalue="parso" />
<item index="70" class="java.lang.String" itemvalue="pytest" />
<item index="71" class="java.lang.String" itemvalue="black" />
<item index="72" class="java.lang.String" itemvalue="conda-build" />
<item index="73" class="java.lang.String" itemvalue="xlwt" />
<item index="74" class="java.lang.String" itemvalue="et-xmlfile" />
<item index="75" class="java.lang.String" itemvalue="mistune" />
<item index="76" class="java.lang.String" itemvalue="future" />
<item index="77" class="java.lang.String" itemvalue="mpmath" />
<item index="78" class="java.lang.String" itemvalue="statsmodels" />
<item index="79" class="java.lang.String" itemvalue="olefile" />
<item index="80" class="java.lang.String" itemvalue="ply" />
<item index="81" class="java.lang.String" itemvalue="unicodecsv" />
<item index="82" class="java.lang.String" itemvalue="webencodings" />
<item index="83" class="java.lang.String" itemvalue="argh" />
</list>
</value>
</option>
</inspection_tool>
<inspection_tool class="PyPep8NamingInspection" enabled="true" level="WEAK WARNING" enabled_by_default="true">
<option name="ignoredErrors">
<list>
<option value="N803" />
</list>
</option>
</inspection_tool>
</profile>
</component>

6
class_model/src/utils/.idea/inspectionProfiles/profiles_settings.xml
Unescape Escape View File

@ -0,0 +1,6 @@
<component name="InspectionProjectProfileManager">
<settings>
<option name="USE_PROJECT_PROFILE" value="false" />
<version value="1.0" />
</settings>
</component>

4
class_model/src/utils/.idea/misc.xml
Unescape Escape View File

@ -0,0 +1,4 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.8 (base)" project-jdk-type="Python SDK" />
</project>

8
class_model/src/utils/.idea/modules.xml
Unescape Escape View File

@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/utils.iml" filepath="$PROJECT_DIR$/.idea/utils.iml" />
</modules>
</component>
</project>

8
class_model/src/utils/.idea/utils.iml
Unescape Escape View File

@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager">
<content url="file://$MODULE_DIR$" />
<orderEntry type="inheritedJdk" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
</module>

6
class_model/src/utils/.idea/vcs.xml
Unescape Escape View File

@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="$PROJECT_DIR$/../../.." vcs="Git" />
</component>
</project>

2
class_model/src/utils/mqttConfig_PUB.yml
Unescape Escape View File

@ -19,5 +19,5 @@ ROS:
ROStopicName_Flight_Information: Dron650_Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: pub.log
logFileName: GPSDrone1.log
UAVLINK: "None"

12
class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py
Unescape Escape View File

@ -1,5 +1,8 @@
import orjson
import time
import logging
logger = logging.getLogger(__name__)
class Proto_msg_from_ros:
#Protobuf
@ -88,14 +91,17 @@ class Json_msg_from_ros:
dataGpsUpdate = {"heading": heading}
cls.GPS_Data.update(dataGpsUpdate)
dataJsonFormate = orjson.dumps(cls.GPS_Data)
cls.log()
cls.mqtt_Pub(message=dataJsonFormate, topics=cls.Flight_Information_topicToMqtt)
@classmethod
def callBack_fly_formation(cls, Formation):
Formation_data = {"velocity": Formation.velocity, "type": Formation.type}
flyFormationMsg = orjson.dumps(Formation_data)
cls.mqtt_Pub(message=flyFormationMsg, topics=cls.Fly_Formation_topicToMqtt, qos=cls.Fly_Formation_topicToMqtt_QOS)
print(flyFormationMsg)
@classmethod
def mqtt_Pub(cls, message:str, topics:str, waitForAck:bool=False, qos:int=0):
mid = cls.client.publish(topics, message, qos)[1]
@ -104,3 +110,7 @@ class Json_msg_from_ros:
print("wait for ack")
time.sleep(0.25)
cls.client.topic_ack.remove(mid)
@classmethod
def log(cls):
unit = 1e7
logger.info("{},{},{},{}".format(cls.GPS_Data["lat"]/1e7, cls.GPS_Data["lon"]/1e7, cls.GPS_Data["alt"]/100, cls.GPS_Data["heading"]/100))

3
class_model/src/utils/proto_uavlink_sub_data_from_ros.py
Unescape Escape View File

@ -2,7 +2,7 @@ import orjson
import time
import logging
logger = logging.getLogger("__UAVLINKSUBPUB__")
logger = logging.getLogger(__name__)
class Proto_msg_from_ros:
#Protobuf
@ -33,6 +33,7 @@ class Proto_msg_from_ros:
cls.flight_information_msg.heading = Compass.data
flightInformationMsg = cls.flight_information_msg.SerializeToString()
cls.sel.write(cls.f2_code + flightInformationMsg + cls.noEcho_code)
logger.info()
cls.rate.sleep()

11
class_model/src/utils/readConfig.py
Unescape Escape View File

@ -21,13 +21,13 @@ class Config:
optval=ecfg[opt]
#verify parameter type
if type(optval) != self.options[opts][opt][0]:
raise Exception("Parameter {} has wrong type".format(self.opt))
raise Exception("Parameter {} has wrong type".format(opt))
#create attributes on the fly
setattr(self,opt,optval)
else:
if self.options[opts][opt][1]:
raise Exception("Missing mandatory parameter {}".format(self.opt))
raise Exception("Missing mandatory parameter {}".format(opt))
else:
setattr(self,opt,None)
@ -99,7 +99,6 @@ class Read_SUB_Config(Config):
"ROSClientNameSub": (str,True),
"Dron550_ROStopicName_Flight_Information": (str,False),
"Dron380_ROStopicName_Flight_Information": (str,False),
"Dron380_ROStopicName_Flight_Information": (str,False),
"Dron650_ROStopicName_Flight_Information": (str,False),
"Dron888_ROStopicName_Flight_Information": (str,False),
"Dron555_ROStopicName_Flight_Information": (str,False),
@ -112,10 +111,6 @@ class Read_SUB_Config(Config):
"uav_id": (str,False),
"baudrate": (int,False),
"ttyport": (str,False)}}
self.setAttribute()
def __str__(self):
return super().__str__()
class Read_CMD_Config(Config):
def setAttribute(self):
@ -146,7 +141,7 @@ class Read_CMD_Config(Config):
return super().__str__()
if __name__ == "__main__":
cfg=Read_CMD_Config("mqttConfig_CMD.yml")
cfg=Read_CMD_Config("uavlinkConfig_SUB.yml")
print(cfg)

Write Preview
Loading…
Cancel
Save