Advertisement
Guest User

Final Working Code

a guest
Jan 27th, 2020
116
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 3.67 KB | None | 0 0
  1. import rospy
  2. import serial
  3. from std_msgs.msg import Float64
  4. from std_msgs.msg import String
  5. import utm
  6.  
  7.  
  8.    
  9. def _parse_latitude(raw_latitude, direction):
  10.     latitude = float(raw_latitude[0:2]) + float(raw_latitude[2:]) / 60.0
  11.     if direction == 'S':
  12.         latitude = -latitude
  13.     return latitude
  14.  
  15. def _parse_longitude(raw_longitude, direction):
  16.     longitude = float(raw_longitude[0:3]) + float(raw_longitude[3:]) / 60.0
  17.     if direction == 'W':
  18.         longitude = -longitude
  19.     return longitude
  20.  
  21.  
  22.  
  23. if __name__ == '__main__':
  24.     ### Start Rospy and get all the parameters.
  25.     rospy.logdebug("Initializing Minicom")
  26.    
  27.    
  28.     rospy.init_node('serial_connection', anonymous = True)
  29.     serial_port = rospy.get_param('~port','/dev/ttyUSB0')
  30.     serial_baud = rospy.get_param('~baudrate',4800)
  31.    
  32.     ### Read the serial ports with above parameters
  33.     port = serial.Serial(serial_port, serial_baud, timeout=3)
  34.    
  35.     lat_pub = rospy.Publisher("Minicom"+'/lattitude', Float64, queue_size=5)
  36.     lon_pub = rospy.Publisher("Minicom"+'/longitude', Float64, queue_size=5)
  37.     alt_pub = rospy.Publisher("Minicom"+'/altitude', Float64, queue_size=5)
  38.     utm_easting_pub = rospy.Publisher("Minicom"+'/utm_easting', Float64, queue_size=5)
  39.     utm_northing_pub = rospy.Publisher("Minicom"+'/utm_northing', Float64, queue_size=5)
  40.     zone_pub = rospy.Publisher("Minicom"+'/zone', Float64, queue_size=5)
  41.     letter_pub = rospy.Publisher("Minicom"+'/letter', String, queue_size=5)
  42.    
  43.    
  44.     # End the initialization Part
  45.     rospy.logdebug("Initialization Complete")
  46.    
  47.    
  48.    
  49.    
  50.    
  51.     rospy.loginfo("Calculating lattitude, longitude and altitude")
  52.    
  53.    
  54.    
  55.    
  56.     try:
  57.         while not rospy.is_shutdown():
  58.             data = str(port.readline())
  59.            
  60.             if data == '':
  61.                 rospy.logwarn("No data")
  62.                 print("data")
  63.             else:
  64.                
  65.                 if data.startswith('$GPGGA'):
  66.            
  67.                     datalist = data.split(',')
  68.        
  69.                     ### Saving Lattitude, longitude and Altitude in each reading in seperate lists.
  70.                     lattitude = datalist[2]
  71.                     latdirection = datalist[3]
  72.                     finalLat = _parse_latitude(lattitude, latdirection)
  73.                    
  74.                     longitude = datalist[4]
  75.                     londirection = datalist[5]
  76.                     finalLon = _parse_longitude(longitude, londirection)
  77.  
  78.                    
  79.                    
  80.                     altitude = float(datalist[9]) + float(datalist[11])
  81.                    
  82.                     utm_values = utm.from_latlon(finalLat, finalLon)
  83.                     utm_easting = utm_values[0]
  84.                     utm_northing = utm_values[1]
  85.                     utm_zone = utm_values[2]
  86.                     utm_letter = utm_values[3]
  87.                    
  88.                     print("Tejas")
  89.                        
  90.                                                            
  91.                     lat_pub.publish(finalLat)
  92.                     lon_pub.publish(finalLon)
  93.                     alt_pub.publish(altitude)
  94.                     utm_easting_pub.publish(utm_easting)
  95.                     utm_northing_pub.publish(utm_northing)
  96.                     zone_pub.publish(utm_zone)
  97.                     letter_pub.publish(utm_letter)
  98.                
  99.                
  100.                
  101.            
  102.     except rospy.ROSInterruptException:
  103.         port.close()
  104.    
  105.     except serial.serialutil.SerialException:
  106.         rospy.loginfo("Shutting down Minicom Node due to Serial Exception!!!")
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement