def __init__(self): rospy.init_node('talker', anonymous=True) pub_droneout = rospy.get_param('~drone_out', '/communication/drone') self.pub = rospy.Publisher(pub_droneout, GPS, queue_size=10) ## change msg type sub_dronein = rospy.get_param('~pose_in', '/positions/drone') rospy.Subscriber(sub_dronein, DronePose, self.pose_callback) hmi_in = rospy.get_param('~hmi_in', '/fmHMI/keyboard') rospy.Subscriber(hmi_in, Char, self.hmi_callback) self.rate = rospy.Rate(5) #5 1hz self.uc = utmconv() # Robolab in UTM # (hemisphere, zone, letter, easting, northing) = uc.geodetic_to_utm (Robolab_ll) self.utm_offset = self.uc.geodetic_to_utm (Robolab_ll[0], Robolab_ll[1]) (self.offset_hemisphere, self.offset_zone, self.offset_letter, self.offset_easting, self.offset_northing) = self.utm_offset print '\nConverted from geodetic to UTM [m]' print ' %d %c %.5fe %.5fn' % (self.offset_zone, self.offset_letter, self.offset_easting, self.offset_northing) self.last_msg_time = 0 self.img_hz = 25 self.utm_pose_in = (0,0) self.height = 20
def __init__(self): rospy.loginfo(rospy.get_name() + ": Start") # static parameters self.update_rate = 200 # set update frequency [Hz] self.count = 0 # status parameters self.position_ok = Int8() self.position_ok = 0 self.last_heard = 0.0 self.last_nmea_sent = 0.0 # get parameters self.output_format = rospy.get_param("~output_format", "nmea") self.serial_device = rospy.get_param("~serial_device", "/dev/gt_pos_robot") self.serial_baudrate = rospy.get_param("~serial_baudrate", 57600) self.robot_id = rospy.get_param("~robot_id", "robot") self.lat_offset = rospy.get_param("~latitude_offset", 55.47038) self.lon_offset = rospy.get_param("~longitude_offset", 10.32966) self.alt_offset = rospy.get_param("~altitude_offset", 0.0) # get topic names self.topic_nmea_from_gt_pos_sub = rospy.get_param("~nmea_from_gt_pos_sub",'/fmData/nmea_from_gt_pos') self.topic_position_ok_pub = rospy.get_param("~position_ok_pub",'/fmData/robot_position_ok') self.topic_gpgga_pub = rospy.get_param("~gpgga_pub",'/fmData/robot_gpgga') # setup publishers self.position_ok_pub = rospy.Publisher(self.topic_position_ok_pub, Int8, queue_size=0) self.gpgga = StringStamped() self.gpgga_pub = rospy.Publisher(self.topic_gpgga_pub, StringStamped, queue_size=0) # configure and open serial device ser_error = False try: self.ser_dev = serial.Serial(self.serial_device, self.serial_baudrate) except Exception as e: rospy.logerr(rospy.get_name() + ": Unable to open serial device: %s" % self.serial_device) ser_error = True if ser_error == False: self.slip = slip_protocol() self.crc = crc16() # instantiate utmconv class self.uc = utmconv() (self.utm_hem, self.utm_z, self.utm_l, self.utm_e, self.utm_n) = self.uc.geodetic_to_utm (self.lat_offset, self.lon_offset) # setup subscription topic callbacks rospy.Subscriber(self.topic_nmea_from_gt_pos_sub, nmea, self.on_nmea_msg) # call updater function self.r = rospy.Rate(self.update_rate) self.updater()
def __init__(self, debug=False, lock_UTM_zone=None): """ Init method Input: optional debug parameter which toogles debug messages Output: none """ self.debug = debug self.geodetic_proj = Proj( '+init=EPSG:4326' ) # EPSG:4326 - WGS 84 / World Geodetic System 1984, used in GPS - https://epsg.io/4326 self.OSM_proj = Proj( '+init=EPSG:3857' ) # EPSG:3857 - WGS 84 / Pseudo-Mercator - https://epsg.io/3857 - OSM projection # Instantiate utmconv class self.uc = utmconv() self.first_UTM_conv = True if lock_UTM_zone != None: self.uc.set_zone_override(lock_UTM_zone)
2015-03-09 KJ Minor update of the license text. 2016-01-16 KJ Corrected a minor problem with the library location reference. """ # import utmconv class from transverse_mercator_py.utm import utmconv from math import pi, cos # define test position test_lat = 55.0000000000 test_lon = 009.0000000000 print 'Test position [deg]:' print ' latitude: %.10f' % (test_lat) print ' longitude: %.10f' % (test_lon) # instantiate utmconv class uc = utmconv() # convert from geodetic to UTM (hemisphere, zone, letter, easting, northing) = uc.geodetic_to_utm(test_lat, test_lon) print '\nConverted from geodetic to UTM [m]' print ' %d %c %.5fe %.5fn' % (zone, letter, easting, northing) # convert back from UTM to geodetic (lat, lon) = uc.utm_to_geodetic(hemisphere, zone, easting, northing) print '\nConverted back from UTM to geodetic [deg]:' print ' latitude: %.10f' % (lat) print ' longitude: %.10f' % (lon) # detrmine conversion position error [m] lat_err = abs(lat - test_lat)
2015-03-09 KJ Minor update of the license text. 2016-01-16 KJ Corrected a minor problem with the library location reference. """ # import utmconv class from transverse_mercator_py.utm import utmconv from math import pi, cos # define test position test_lat = 55.0000000000 test_lon = 009.0000000000 print 'Test position [deg]:' print ' latitude: %.10f' % (test_lat) print ' longitude: %.10f' % (test_lon) # instantiate utmconv class uc = utmconv() # convert from geodetic to UTM (hemisphere, zone, letter, easting, northing) = uc.geodetic_to_utm (test_lat,test_lon) print '\nConverted from geodetic to UTM [m]' print ' %d %c %.5fe %.5fn' % (zone, letter, easting, northing) # convert back from UTM to geodetic (lat, lon) = uc.utm_to_geodetic (hemisphere, zone, easting, northing) print '\nConverted back from UTM to geodetic [deg]:' print ' latitude: %.10f' % (lat) print ' longitude: %.10f' % (lon) # detrmine conversion position error [m] lat_err = abs(lat-test_lat) lon_err = abs(lon-test_lon)