Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- # -*- coding: utf-8 -*-
- """
- Spyder Editor
- This is a temporary script file.
- """
- import rospy
- import serial
- from std_msgs.msg import Float64
- from std_msgs.msg import String, Header
- import utm
- def _parse_latitude(raw_latitude, direction):
- latitude = float(raw_latitude[0:2]) + float(raw_latitude[2:]) / 60.0
- if direction == 'S':
- latitude = -latitude
- return latitude
- def _parse_longitude(raw_longitude, direction):
- longitude = float(raw_longitude[0:3]) + float(raw_longitude[3:]) / 60.0
- if direction == 'W':
- longitude = -longitude
- return longitude
- if __name__ == '__main__':
- ### Start Rospy and get all the parameters.
- rospy.logdebug("Initializing Minicom")
- h = Header()
- #h.stamp = rospy.Time.now()
- rospy.init_node('serial_connection', anonymous = True)
- serial_port = rospy.get_param('~port','/dev/ttyUSB0')
- serial_baud = rospy.get_param('~baudrate',4800)
- ### Read the serial ports with above parameters
- port = serial.Serial(serial_port, serial_baud, timeout=3)
- lat_pub = rospy.Publisher("Minicom"+'/lattitude', Float64, queue_size=5)
- lon_pub = rospy.Publisher("Minicom"+'/longitude', Float64, queue_size=5)
- alt_pub = rospy.Publisher("Minicom"+'/altitude', Float64, queue_size=5)
- utm_easting_pub = rospy.Publisher("Minicom"+'/utm_easting', Float64, queue_size=5)
- utm_northing_pub = rospy.Publisher("Minicom"+'/utm_northing', Float64, queue_size=5)
- zone_pub = rospy.Publisher("Minicom"+'/zone', Float64, queue_size=5)
- letter_pub = rospy.Publisher("Minicom"+'/letter', String, queue_size=5)
- # End the initialization Part
- rospy.logdebug("Initialization Complete")
- rospy.loginfo("Calculating lattitude, longitude and altitude")
- try:
- while not rospy.is_shutdown():
- data = str(port.readline())
- if data == '':
- rospy.logwarn("No data")
- print("data")
- else:
- if data.startswith('$GPGGA'):
- datalist = data.split(',')
- ### Saving Lattitude, longitude and Altitude in each reading in seperate lists.
- lattitude = datalist[2]
- latdirection = datalist[3]
- finalLat = _parse_latitude(lattitude, latdirection)
- longitude = datalist[4]
- londirection = datalist[5]
- finalLon = _parse_longitude(longitude, londirection)
- altitude = float(datalist[9]) + float(datalist[11])
- utm_values = utm.from_latlon(finalLat, finalLon)
- utm_easting = utm_values[0]
- utm_northing = utm_values[1]
- utm_zone = utm_values[2]
- utm_letter = utm_values[3]
- print("Tejas")
- lat_pub.publish(finalLat)
- lon_pub.publish(finalLon)
- alt_pub.publish(altitude)
- utm_easting_pub.publish(utm_easting)
- utm_northing_pub.publish(utm_northing)
- zone_pub.publish(utm_zone)
- letter_pub.publish(utm_letter)
- except rospy.ROSInterruptException:
- port.close()
- except serial.serialutil.SerialException:
- rospy.loginfo("Shutting down Minicom Node due to Serial Exception!!!")
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement