Advertisement
Guest User

ROS Final Working code

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