def __init__(self):
super().__init__('gps')
# setup the GPS
self.declare_parameter('gps_baud', self.BAUDRATE)
self.declare_parameter('gps_port', self.PORT)
self.baud = self.get_parameter('gps_baud').value
self.port = self.get_parameter('gps_port').value
self.ubp = GPSReader(self.port, self.baud,
self.TIMEOUT, self.UBXONLY)
if not self.ubp.connect():
# TODO: raise custom error
self.get_logger().error(f"GPS Failed to connect on given port. Aborting")
return
self.ubp.config_timepulse_badly()
self.ubp.config_msg(self.ON)
# setup the ROS
self.declare_parameter('gps_top', 'gps')
self.declare_parameter('time_top', 'gps_time')
pub_top = self.get_parameter('gps_top').value
time_top = self.get_parameter('time_top').value
self.fix_pub = self.create_publisher(NavSatFix, pub_top, 10)
self.time_pub = self.create_publisher(TimeReference, time_top, 10)
# setup the gpio
GPIO.setmode(GPIO.BCM)
GPIO.setup(26, GPIO.IN, GPIO.PUD_UP)
GPIO.add_event_detect(26, GPIO.RISING,
callback=self.timepulse_callback, bouncetime=50)
def main(protocol):
try:
print("\nPython %s\n" % sys.version)
print("IoT Hub Client for Python")
hub_manager = HubManager(protocol)
print("Starting the GPS locator module using protocol %s..." %
hub_manager.client_protocol)
print(
"This module is now waiting for messages and will indefinitely. Press Ctrl-C to exit. "
)
#start reading gps data...
myGPSReader = GPSReader(GPSDeviceTtyAC)
latestFixedTime = None
while True:
latestFixedPoint = myGPSReader.getLatestFixedGPSPoint()
if latestFixedPoint is not None and latestFixedPoint.sentenceTimestamp != latestFixedTime:
#new GPS point than previous one, send to iot hub
jsonPayload = latestFixedPoint.buildJsonPayload(DEVICE_ID)
print("Payload of new GPS data is: " + jsonPayload)
msg = IoTHubMessage(bytearray(jsonPayload, 'utf8'))
#in order to use IoT Hub message routing on body, we have to setup the content type and encoding
set_content_result = msg.set_content_encoding_system_property(
"utf-8")
set_content_type_result = msg.set_content_type_system_property(
"application/json")
if set_content_result != 0:
print("set_content_encoding_system_property FAILED")
if set_content_type_result != 0:
print("set_content_type_system_property FAILED")
hub_manager.forward_event_to_output("gps", msg, 0)
latestFixedTime = latestFixedPoint.sentenceTimestamp
else:
print(str(datetime.now()) + ": Not fixed yet....")
time.sleep(1)
except IoTHubError as iothub_error:
print("Unexpected error %s from IoTHub" % iothub_error)
return
except KeyboardInterrupt:
print("IoTHubModuleClient sample stopped")
def timepulse_callback(self, channel):
self.get_logger().info(f"{time.time()} Timepulse trigger")
gps_msg = NavSatFix()
timeref_msg = TimeReference()
msg_hdr = Header()
system_time = self.get_clock().now().to_msg()
msg_hdr.frame_id = 'base_link' # center of the plane
try:
ubx = self.ubp.read()
except IOError:
self.get_logger().warning("GPS disconnected. Attempting to reconnect.")
self.ubp = GPSReader(self.port, self.baud,
self.TIMEOUT, self.UBXONLY)
return
while ubx:
if (ubx.msg_cls + ubx.msg_id) == b"\x01\x07": # NAV_PVT
# <UBX(NAV-PVT, iTOW=16:50:32, year=2015, month=10, day=25, hour=16, min=50, second=48, valid=b'\xf0', tAcc=4294967295, nano=0, fixType=0, flags=b'\x00', flags2=b'$', numSV=0, lon=0, lat=0, height=0, hMSL=-17000, hAcc=4294967295, vAcc=4294967295, velN=0, velE=0, velD=0, gSpeed=0, headMot=0, sAcc=20000, headAcc=18000000, pDOP=9999, reserved1=65034815406080, headVeh=0, magDec=0, magAcc=0)>
msg_hdr.stamp = self._gen_timestamp_from_utc(ubx)
fix_stat = NavSatStatus()
if ubx.fixType == 0:
self.get_logger().warning(f"No fix yet.")
break
fix_stat.service = SERVICE_GPS
gps_msg.status = fix_stat
gps_msg.header = msg_hdr
gps_msg.latitude = float(ubx.lat)/10000000
gps_msg.longitude = float(ubx.lon)/10000000
gps_msg.altitude = float(ubx.height)/1000
timeref_msg.header = msg_hdr
timeref_msg.time_ref = system_time
timeref_msg.source = "GPS"
self.fix_pub.publish(gps_msg)
self.time_pub.publish(timeref_msg)
self.get_logger().info(f"Publishing gps message: ({timeref_msg.header.stamp.sec}.{timeref_msg.header.stamp.nanosec}): ({gps_msg.latitude}, {gps_msg.longitude}, {gps_msg.altitude})")
return
else:
self.get_logger().info(f"Other GPS MSG: {(ubx.msg_cls + ubx.msg_id)}")
ubx = self.ubp.read()
def __init__(self):
"""Initializes the DataCollector."""
super(DataCollector, self).__init__()
obd2readerClass = getattr(obd2reader, CONFIGURATION["obd2reader"])
self._obd2reader = obd2readerClass(CONFIGURATION["device"], CONFIGURATION["speed"])
self._obd2reader.open_connection()
self._gpsreader = GPSReader(CONFIGURATION["gpsdPort"])
def __init__(self, display):
"""Initializes the DataCollector.
:param Display display: Display that can be used to view messages.
"""
super(DataCollector, self).__init__()
obd2readerClass = getattr(obd2reader, CONFIGURATION["obd2reader"])
self._obd2reader = obd2readerClass(CONFIGURATION["device"],
CONFIGURATION["speed"], display)
self._obd2reader.open_connection()
self._gpsreader = GPSReader()
class Gps(Node):
ON = b"\x00\x01\x01\x01\x00\x00"
OFF = b"\x00\x00\x00\x00\x00\x00"
PORT = "/dev/ttyACM0"
BAUDRATE = 9600
TIMEOUT = 1
UBXONLY = False
def __init__(self):
super().__init__('gps')
# setup the GPS
self.declare_parameter('gps_baud', self.BAUDRATE)
self.declare_parameter('gps_port', self.PORT)
self.baud = self.get_parameter('gps_baud').value
self.port = self.get_parameter('gps_port').value
self.ubp = GPSReader(self.port, self.baud,
self.TIMEOUT, self.UBXONLY)
if not self.ubp.connect():
# TODO: raise custom error
self.get_logger().error(f"GPS Failed to connect on given port. Aborting")
return
self.ubp.config_timepulse_badly()
self.ubp.config_msg(self.ON)
# setup the ROS
self.declare_parameter('gps_top', 'gps')
self.declare_parameter('time_top', 'gps_time')
pub_top = self.get_parameter('gps_top').value
time_top = self.get_parameter('time_top').value
self.fix_pub = self.create_publisher(NavSatFix, pub_top, 10)
self.time_pub = self.create_publisher(TimeReference, time_top, 10)
# setup the gpio
GPIO.setmode(GPIO.BCM)
GPIO.setup(26, GPIO.IN, GPIO.PUD_UP)
GPIO.add_event_detect(26, GPIO.RISING,
callback=self.timepulse_callback, bouncetime=50)
def timepulse_callback(self, channel):
self.get_logger().info(f"{time.time()} Timepulse trigger")
gps_msg = NavSatFix()
timeref_msg = TimeReference()
msg_hdr = Header()
system_time = self.get_clock().now().to_msg()
msg_hdr.frame_id = 'base_link' # center of the plane
try:
ubx = self.ubp.read()
except IOError:
self.get_logger().warning("GPS disconnected. Attempting to reconnect.")
self.ubp = GPSReader(self.port, self.baud,
self.TIMEOUT, self.UBXONLY)
return
while ubx:
if (ubx.msg_cls + ubx.msg_id) == b"\x01\x07": # NAV_PVT
# <UBX(NAV-PVT, iTOW=16:50:32, year=2015, month=10, day=25, hour=16, min=50, second=48, valid=b'\xf0', tAcc=4294967295, nano=0, fixType=0, flags=b'\x00', flags2=b'$', numSV=0, lon=0, lat=0, height=0, hMSL=-17000, hAcc=4294967295, vAcc=4294967295, velN=0, velE=0, velD=0, gSpeed=0, headMot=0, sAcc=20000, headAcc=18000000, pDOP=9999, reserved1=65034815406080, headVeh=0, magDec=0, magAcc=0)>
msg_hdr.stamp = self._gen_timestamp_from_utc(ubx)
fix_stat = NavSatStatus()
if ubx.fixType == 0:
self.get_logger().warning(f"No fix yet.")
break
fix_stat.service = SERVICE_GPS
gps_msg.status = fix_stat
gps_msg.header = msg_hdr
gps_msg.latitude = float(ubx.lat)/10000000
gps_msg.longitude = float(ubx.lon)/10000000
gps_msg.altitude = float(ubx.height)/1000
timeref_msg.header = msg_hdr
timeref_msg.time_ref = system_time
timeref_msg.source = "GPS"
self.fix_pub.publish(gps_msg)
self.time_pub.publish(timeref_msg)
self.get_logger().info(f"Publishing gps message: ({timeref_msg.header.stamp.sec}.{timeref_msg.header.stamp.nanosec}): ({gps_msg.latitude}, {gps_msg.longitude}, {gps_msg.altitude})")
return
else:
self.get_logger().info(f"Other GPS MSG: {(ubx.msg_cls + ubx.msg_id)}")
ubx = self.ubp.read()
def _gen_timestamp_from_utc(self, ubx):
second = ubx.second
usecond = ubx.nano/1000
subtract_sec = False
if usecond < 0:
usecond = 1000000 + usecond
subtract_sec = True
utc = datetime.datetime(ubx.year, ubx.month, ubx.day, ubx.hour, ubx.min, second, int(usecond))
gps_time = utc.timestamp()
if subtract_sec:
gps_time -= 1
gps_stamp = self.get_clock().now().to_msg()
gps_stamp.sec = int(gps_time)
gps_stamp.nanosec = int((gps_time - int(gps_time))*1000000000)
return gps_stamp
class DataCollector(object):
"""The DataCollector collects and stores all data.
The stored data includes:
- Time
- GPS data
- OBD2 data
"""
def __init__(self):
"""Initializes the DataCollector."""
super(DataCollector, self).__init__()
obd2readerClass = getattr(obd2reader, CONFIGURATION["obd2reader"])
self._obd2reader = obd2readerClass(CONFIGURATION["device"], CONFIGURATION["speed"])
self._obd2reader.open_connection()
self._gpsreader = GPSReader(CONFIGURATION["gpsdPort"])
def shutdown(self):
"""Shuts down the DataCollector and all of its Threads."""
self._obd2reader.shutdown()
self._gpsreader.shutdown()
def write_data_log(self, logFileName, nbrOfOBDFrames, messagesPerTimestamp):
"""Stores an data log file to the given location.
:param String logFileName: Path to store the log file.
:param Integer nbrOfOBDFrames: Number of OBD frames to be stored inside the log.
This parameter does NOT affect the number of time
stamps or GPS reports.
:param Integer messagesPerTimestamp: Number of OBD2 frames share the same time stamp.
:return: Returns the number of bytes stored in the log.
:rtype: Integer
"""
## create the log file
logf = file(logFileName, "ab")
## performance optimization
## reduces the __getattr__ calls for the major instances
write = logf.write
gpsreader = self._gpsreader
obd2reader = self._obd2reader
## number of bytes written
bytes = 0
## collect the requested number of OBD2 frames
for idx in xrange(0, nbrOfOBDFrames, messagesPerTimestamp):
## time stamp
timestamp = "#TIME__%s\n" % _getCurrentTime()
write(timestamp)
bytes += len(timestamp)
## read the number of frames requested
try:
logEntries = obd2reader.read_frames(nbrOfOBDFrames)
except:
obd2reader.reconnect()
continue
## store the frames in the log
logEntries = "\n".join(logEntries)
write("%s\n" % logEntries)
bytes += len(logEntries) + 1
## append the gpslog entries, when there are any
gpsreports = ["#GPS__%s\n" % entry for entry in gpsreader.get_current_gps_entries()]
## no GPS report collected since last run
if not gpsreports:
continue
## store the reports in the log
gpsreports = "\n".join([str(r) for r in gpsreports])
write("%s\n" % gpsreports)
bytes += len(gpsreports) + 1
# close the log file
logf.close()
## return the number of bytes written
return bytes
from gpsreader import GPSReader
import time
import json
from datetime import datetime
#start reading gps data...
myGPSReader = GPSReader("/dev/ttyACM0")
latestFixedTime = None
while True:
latestFixedPoint = myGPSReader.getLatestFixedGPSPoint()
if latestFixedPoint is not None and latestFixedPoint.sentenceTimestamp != latestFixedTime:
#new GPS point than previous one, send to iot hub
jsonPayload = latestFixedPoint.buildJsonPayload("test_device")
print("Payload of new GPS data is: " + jsonPayload)
latestFixedTime = latestFixedPoint.sentenceTimestamp
print("++++++++++++++++++++")
else:
print(str(datetime.now()) + ": Not fixed yet....")
time.sleep(1)
