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 calculate_lat(lat, direction):
- latitude = float(lat[0:2]) + float(lat[2:]) / 60.0
- if direction == 'S':
- latitude = -latitude
- return latitude
- def calculate_lon(lon, direction):
- longitude = float(lon[0:3]) + float(lon[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")
- count = 0
- 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 = calculate_lat(lattitude, latdirection)
- longitude = datalist[4]
- londirection = datalist[5]
- finalLon = calculate_lon(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]
- count = count + 1
- print(count)
- 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