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)
