def talker(): print 'in talker' pub = rospy.Publisher('GPS', NavSatFix) rospy.init_node('GPStalker') while not rospy.is_shutdown(): #Assuming that parse will return these values time.sleep(1) parse() msg = NavSatFix() Fix = NavSatStatus() Fix.status = GPS.mode Fix.service = GPS.numSat msg.header.stamp = rospy.Time.now() msg.status = Fix msg.latitude = GPS.lat msg.longitude = GPS.lon msg.altitude = GPS.alt msg.position_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) msg.position_covariance_type = 0 #covariance_type = 0 unknown # = 1 approximated # = 2 diagonal known # = 3 known pub.publish(msg)
def parse_gps(self, data): ''' Given raw data from Jaguar GPS socket, parse and return NavSatFix message. If bad/incomplete data, return None ''' # use regex to parse gpgga_hit = re.search('^\$(GPGGA.+?)\r\n', data) gprmc_hit = re.search('^\$(GPRMC.+?)\r\n', data) if gprmc_hit: # Get estimated heading (not part of standard ROS navsatfix message) gprmc = gprmc_hit.group(0).split(",") try: heading = float(gprmc[8]) except: heading = float("NaN") else: while heading < -180.0: heading += 360.0 while heading > 180.0: heading -= 360.0 finally: self.current_est_heading = heading if gpgga_hit: gpgga = gpgga_hit.group(0).split(",") nav_msg = NavSatFix() # Set header information time = gpgga[1] hrs = float(time[0:1]) mins = float(time[2:3]) secs = float(time[4:5]) nav_msg.header.stamp = rospy.Time.from_sec(hrs * 3600 + mins * 60 + secs) nav_msg.header.frame_id = "gps" # Set GPS Status status = NavSatStatus() status.status = -1 if int(gpgga[6]) == 0 else 0 nav_msg.status = status # Set longitude and latitude lat_str = gpgga[2] lon_str = gpgga[4] lat_degs = float(lat_str[:2]) + (float(lat_str[2:]) / 60.0) lon_degs = float(lon_str[:3]) + (float(lon_str[3:]) / 60.0) nav_msg.latitude = -1 * lat_degs if gpgga[3] == "S" else lat_degs nav_msg.longitude = -1 * lon_degs if gpgga[5] == "W" else lon_degs # Set altitude (Positive is above the WGS 84 ellipsoid) try: nav_msg.altitude = float(gpgga[9]) except: nav_msg.altitude = float("NaN") # Set covariance type to unknown nav_msg.position_covariance_type = 0 return nav_msg else: return None
def gps_sensor(): # initialize node, transform listener, and rate of tf lookup rospy.init_node("gps_sensor", anonymous=True) tfBuffer = tf2_ros.Buffer() gps_sensor = tf2_ros.TransformListener(tfBuffer) rate = rospy.Rate(1.0) # create publisher objects for two topics, establish publish rate pub_ship = rospy.Publisher("zed_f9p/base/fix", sensor_msgs.msg.NavSatFix, queue_size=10) pub_UAV = rospy.Publisher("zed_f9p/rover/fix", sensor_msgs.msg.NavSatFix, queue_size=10) # look up the NED to ship and NED to UAV transforms while not rospy.is_shutdown(): try: tship = tfBuffer.lookup_transform("NED", "boat", rospy.Time(0)) tUAV = tfBuffer.lookup_transform("NED", "UAV", rospy.Time(0)) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rate.sleep() continue trans_ship = tship.transform.translation trans_UAV = tUAV.transform.translation # create the object for the ship which will be published and the tuple with converted NED to lat, long, alt numbers NSF_ship = NavSatFix() ship_lat_long_alt_coords = convert_NED_latlong(rospy.get_param("gps_sensor/rlat"), rospy.get_param("gps_sensor/rlong"), rospy.get_param("gps_sensor/ralt"),\ trans_ship.x, trans_ship.y, trans_ship.z) # fill in the NavSatFix object with the correct numbers NSF_ship.latitude = ship_lat_long_alt_coords[0] NSF_ship.longitude = ship_lat_long_alt_coords[1] NSF_ship.altitude = ship_lat_long_alt_coords[2] # create the object for the ship which will be published and the tuple with converted NED to lat, long, alt numbers NSF_UAV = NavSatFix() UAV_lat_long_alt_coords = convert_NED_latlong(rospy.get_param("gps_sensor/rlat"), rospy.get_param("gps_sensor/rlong"), rospy.get_param("gps_sensor/ralt"),\ trans_UAV.x, trans_UAV.y, trans_UAV.z) # fill in the NavSatFix object with the correct numbers NSF_UAV.latitude = UAV_lat_long_alt_coords[0] NSF_UAV.longitude = UAV_lat_long_alt_coords[1] NSF_UAV.altitude = UAV_lat_long_alt_coords[2] # publish to the two topics # rospy.loginfo(NSF_ship) pub_ship.publish(NSF_ship) # rospy.loginfo(NSF_UAV) pub_UAV.publish(NSF_UAV) # wait at publishing rate rate.sleep()
def callback(sensorstr): data = sensorstr.data if data[0] == 'Z': data = data.replace("Z","").replace("\n","").replace("\r","") data_list = data.split(',') if len(data_list) == 4: try: ##data_list structure: lat, lon, heading, vel float_list = [float(i) for i in data_list] poseGPS = NavSatFix() poseGPS.header.frame_id = "world" poseGPS.header.stamp = rospy.Time.now() poseGPS.status.status = 0 poseGPS.status.service = 1 poseGPS.latitude = float_list[0] poseGPS.longitude = float_list[1] poseGPS.altitude = 0.0 poseGPS.position_covariance = [3.24, 0, 0, 0, 3,24, 0, 0, 0, 0] poseGPS.position_covariance_type = 2 try: pubGPS.publish(poseGPS) log = "GPS Pose Data: %f %f Publish at Time: %s" % (float_list[0], float_list[1], rospy.get_time()) except: log = "poseGPS Publisher Error" except: log = "GPS Data Error! Data : %s" % data rospy.loginfo(log) rospy.loginfo(log)
def get_and_pub_packet(self): data, addr = self.sock.recvfrom(8308) line = data[206:278] my_fix = NavSatFix() my_fix.header.stamp = rospy.Time.now() latitude_degrees = float(line[16:18]) latitude_minutes = float(line[18:25]) latitude = self.decimal_degrees(degs=latitude_degrees, mins=latitude_minutes) if (line[26] == 'S'): latitude = -1 * latitude longitude_degrees = float(line[28:31]) longitude_minutes = float(line[31:38]) longitude = self.decimal_degrees(degs=longitude_degrees, mins=longitude_minutes) if (line[39] == 'W'): longitude = -1 * longitude my_fix.latitude = latitude my_fix.longitude = longitude my_fix.altitude = 0.0 self.fix_pub.publish(my_fix)
def publishGPS(): ser = serial.Serial(port='/dev/ttyACM0', baudrate=4800, timeout=5) pub = rospy.Publisher('/tunnelGPS_fix', NavSatFix, queue_size=1024) rospy.init_node('tunnel_gps_node', anonymous=True) rate = rospy.Rate(1) # 1Hz NavStat_msg = NavSatFix() while not rospy.is_shutdown(): line_raw = ser.readline() line = line_raw.split(',') if line[0] == "GPGGA": NavStat_msg.time = line[1] if line[2] == '': print "No GPS Signal" else: if line[3] == 'N': NavStat_msg.latitude = float(line[2]) else: NavStat_msg.latitude = - float(line[2]) if line[5] == 'E': NavStat_msg.longitude = float(line[4]) else: NavStat_msg.longitude = - float(line[4]) NavStat_msg.altitude = line[9] pub.publish(NavStat_msg) print "Publishing: lat = " + NavStat_msg.latitude + " long = " + NavStat_msg.longitude + " alt = " + NavStat_msg.altitude
def PointCloudCall(self,data): # 20 Hz As this function has highest frequency, good for update pointCloud_ = PointCloud2() pointCloud_ = data self.graphslam_PointCloud_pub.publish(pointCloud_) # for Graph slam self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS) self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_, 'statManu', self.excluSatLis) # statManu self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal() if((len(self.GNSSNRAWlosDel.GNSS_Raws) > 1) and (self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time != self.GNSSNRAWlosDelTimeFlag)): # only if there is a change, there will conduct the calculation # print 'self.GNSSNRAWlosDel',len(self.GNSSNRAWlosDel.GNSS_Raws) self.puGNSSPosCalF_prop.iterPosCal(self.GNSSNRAWlosDel, 'WLSGNSS') self.GNSSNRAWlosDelTimeFlag = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time navfix_ = NavSatFix() llh_ = [] llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_prop.ecef_) navfix_.header.stamp.secs = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time navfix_.latitude = float(llh_[0]) navfix_.longitude = float(llh_[1]) navfix_.altitude = float(llh_[2]) self.propGNSS_Navfix_pub.publish(navfix_) # for Graph slam graphNavfix_ = NavSatFix() graphNavfix_ = navfix_ graphNavfix_.header = pointCloud_.header self.graphslam_Navfix_pub.publish(graphNavfix_) # for Graph slam geopoint = GeoPointStamped() geopoint.header = graphNavfix_.header geopoint.position.latitude = float(llh_[0]) geopoint.position.longitude = float(llh_[1]) geopoint.position.altitude = float(llh_[2]) self.graphslam_GeoPoint_pub.publish(geopoint)
def gps_talker(): pub = rospy.Publisher('navsat', NavSatFix, queue_size=10) rospy.init_node('gps_talker', anonymous=True) msg = NavSatFix() seq = 0 while not rospy.is_shutdown(): line = s.readline() data = pynmea2.parse(line) if line[1:5] == 'GPRM': seq += 1 msg.header.seq = seq msg.header.stamp = rospy.Time.now() msg.header.frame_id = 'world' if data.status == 'A': msg.status.status = int(1) else: msg.status.status = int(0) msg.status.service = int(0) msg.latitude = float(data.lat) msg.longitude = float(data.lon) # data.true_course=142.46 # data.spd_over_grnd if line[1:5] == 'GPGG': seq += 1 msg.header.seq = seq msg.latitude = float(data.lat) msg.longitude = float(data.lon) msg.altitude = float(data.altitude) * 3.28084 pub.publish(msg)
def rtkgps(portname, baudrate): pub = rospy.Publisher('rtk_fix', NavSatFix, queue_size=1) rospy.init_node('rtk_gps_node', anonymous=True) rate = rospy.Rate(10) ser = serial.Serial(portname, baudrate, timeout=2) while True: line = ser.readline() if line.startswith('$GNGGA'): ls = line.split(",") lat = ls[2] lat1 = ls[2].strip() latf = float(lat1) latfinal = latf / 100 longitude = ls[4] longitude1 = ls[4].strip() longitudef = float(longitude1) longitudefinal = longitudef / 100 altd = ls[9].strip() altdf = float(altd) print latfinal print longitudefinal print altdf rtk_fix = NavSatFix() #include header and time rtk_fix.latitude = latfinal rtk_fix.longitude = longitudefinal rtk_fix.altitude = altdf pub.publish(rtk_fix)
def on_message(client, userdata, msg): global pub global last_lat global last_lon y = json.loads(msg.payload) navsat = NavSatFix() navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = 'gps' navsat.status.status = 0 navsat.status.service = 1 latitude = y["latitude"] longitude = y["longitude"] navsat.latitude = float(latitude[:2]) + float(latitude[2:]) / 60 navsat.longitude = float(longitude[:3]) + float(longitude[3:]) / 60 if y["lat"] == "S": navsat.latitude = -1 * navsat.latitude if y["lon"] == "W": navsat.longitude = -1 * navsat.longitude navsat.altitude = y["altitude"] if last_lat != latitude or last_lon != longitude: print("lat:" + navsat.latitude) print("lon:" + navsat.longitude) last_lat = latitude last_lon = longitude pub.publish(navsat)
def __init__(self): rospy.init_node('add_test_points') self.waypoint_pub = rospy.Publisher('/waypoints', WaypointsArray, queue_size=10, latch=True) f = open ("points.txt", "r") p_array = [] for line in f: l = NavSatFix() items = line.split(",") l.latitude = (float) (items[0]) l.longitude = (float) (items[1]) l.altitude = (float) (items[2].split("\n")[0]) p_array.append(l) msg = WaypointsArray() msg.waypoints = p_array #for i in range(100): self.waypoint_pub.publish(msg) #rospy.sleep(.1) #print msg print "done" rospy.spin()
def _on_publish_pose(self, msg): try: point, quat = self._tfl.lookupTransform(self._world_frame, self._robot_frame, rospy.Time(0)) gps_msg = NavSatFix() # result.header.frame_id = self._world_frame gps_msg.status.status = 0 gps_msg.status.service = 1 gps_msg.header.stamp = rospy.Time.now() utm_point = utm.UTMPoint(point[0], point[1], point[2], zone=self.UTM_ZONE, band=self.UTM_BAND) latlon = utm_point.toMsg() gps_msg.latitude = latlon.latitude gps_msg.longitude = latlon.longitude gps_msg.altitude = latlon.altitude imu_msg = Imu() imu_msg.header.stamp = rospy.Time.now() imu_msg.header.frame_id = self._world_frame roll, pitch, yaw = euler_from_quaternion(quat) quat = quaternion_from_euler(roll, pitch, (yaw * self.ORI_INVERT) + self.ORI_OFFSET) imu_msg.orientation.x = quat[0] imu_msg.orientation.y = quat[1] imu_msg.orientation.z = quat[2] imu_msg.orientation.w = quat[3] #print "EULER", math.degrees(roll), math.degrees(pitch), math.degrees(yaw), "mit offset:", math.degrees(yaw + self.ORI_OFFSET) self._pub_gps.publish(gps_msg) self._pub_imu.publish(imu_msg) self._print_transform_error = True except tf.LookupException as le: if self._print_transform_error: self._print_transform_error = False rospy.logwarn("Error while looup transform: %s" % le) except Exception as ee: if self._print_transform_error: self._print_transform_error = False rospy.logwarn("Error while transform: %s" % ee)
def got_position(xx, yy, aa, stamp): global EP fix = NavSatFix() fix.header.stamp = stamp fix.header.frame_id = "sim_gps" fix.status.status = 0 fix.status.service = 1 utm_e = base_xx + base_utm_e + xx + CONFIG['SIM_ERROR'] * cos( (yy + time()) / EP) utm_n = base_yy + base_utm_n + yy + CONFIG['SIM_ERROR'] * cos( (utm_e / 3.14) / EP) (lon, lat) = conv(utm_e, utm_n, inverse=True) fix.latitude = lat fix.longitude = lon fix.altitude = 1.3 #print "got_position: %f %f" % (lat, lon) gps.publish(fix) pos.publish(Pose2D(utm_e, utm_n, aa)) tfBr.sendTransform((utm_e - base_xx, utm_n - base_yy, 0), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/map", "/gps")
def talker(): pub = rospy.Publisher('gps/fix', NavSatFix, queue_size=10) rospy.init_node('fake_gps', anonymous=True) pub2 = rospy.Publisher('vel', Twist, queue_size=10) rate = rospy.Rate(2) # 10hz loc_index = 0 while not rospy.is_shutdown(): #print(loc_index) fake_localization = NavSatFix() fake_vel = Twist() #fake_vel.frame_id = "/gps" #fake_localization.header.stamp = time.time() fake_localization.header.frame_id = '/gps' fake_localization.status.status = 1 fake_localization.status.service = 1 #print(gps_localizations[loc_index][0], gps_localizations[loc_index][1]) fake_localization.latitude = gps_localizations[loc_index][0] fake_localization.longitude = gps_localizations[loc_index][1] print(fake_localization.latitude, fake_localization.longitude) fake_localization.altitude = 0.0 fake_localization.position_covariance = [ 1.3224999999999998, 0.0, 0.0, 0.0, 1.3224999999999998, 0.0, 0.0, 0.0, 5.289999999999999 ] fake_localization.position_covariance_type = 1 #fake_vel.header.frame_id = "/gps" pub.publish(fake_localization) pub2.publish(fake_vel) if (loc_index == len(gps_localizations) - 1): loc_index = 0 else: loc_index = loc_index + 1 rate.sleep()
def step(self): stamp = super().step() if not stamp: return if self.__gps_publisher.get_subscription_count() > 0 or \ self.__speed_publisher.get_subscription_count() > 0 or \ self._always_publish: self._wb_device.enable(self._timestep) msg = Float32() msg.data = self._wb_device.getSpeed() self.__speed_publisher.publish(msg) if self.__coordinate_system == GPS.WGS84: msg = NavSatFix() msg.header.stamp = stamp msg.header.frame_id = self._frame_id msg.latitude = self._wb_device.getValues()[0] msg.longitude = self._wb_device.getValues()[1] msg.altitude = self._wb_device.getValues()[2] msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN msg.status.service = NavSatStatus.SERVICE_GPS self.__gps_publisher.publish(msg) else: msg = PointStamped() msg.header.stamp = stamp msg.header.frame_id = self._frame_id msg.point.x = self._wb_device.getValues()[0] msg.point.y = self._wb_device.getValues()[1] msg.point.z = self._wb_device.getValues()[2] self.__gps_publisher.publish(msg) else: self._wb_device.disable()
def spin(self): ############################################################################# r = rospy.Rate(self.rate) while not rospy.is_shutdown(): current_time = rospy.get_rostime() frame_id = 'gps' latitude = 40.444539 longitude = -79.940777 current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id current_time_ref.source = frame_id current_fix.status.status = NavSatStatus.STATUS_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.latitude = latitude current_fix.longitude = longitude current_fix.position_covariance[0] = 0.1 current_fix.position_covariance[4] = 0.1 current_fix.position_covariance[8] = 0.25 current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED current_fix.altitude = 0.0 self.fix_pub.publish(current_fix) self.update() r.sleep()
def run(self): while True: #Reading acceleration data report = self.session.next() if self.debug: print(report) if report["class"] == "TPV": if "time" in report: gpsMSG = NavSatFix() gpsMSG.latitude = report["lat"] gpsMSG.longitude = report["lon"] try: gpsMSG.status = report["status"] except: pass try: gpsMSG.altitude = report["alt"] except: pass current_time = modf(time.time()) # gpsMSG.header.sec = int(current_time[1]) # gpsMSG.header.nanosec = int(current_time[0] * 1000000000) & 0xffffffff self.gpsPublisher.publish(gpsMSG) else: if self.debug: print("GPS has not locked onto sattelites")
def navToPylon(self): self.towerRequest.publish(True) while not (self.received): self.rate.sleep() if len(self.pylonList) > 0: utmDronePos = utm.from_latlon(self.dronePos.latitude, self.dronePos.longitude) nearidx, nextWP = self.findNearestPylon(utmDronePos) orientation, wpOffN, wpOffE = self.calcOrientation(nearidx) utmNxtWP = utm.from_latlon(nextWP.lat, nextWP.lon) nxtWPAdjN = utmNxtWP[0] + wpOffN nxtWPAdjE = utmNxtWP[1] + wpOffE WPADJ = utm.to_latlon(nxtWPAdjN, nxtWPAdjE, utmNxtWP[2], utmNxtWP[3]) print("Waypoint to pylon is:", WPADJ) wpTarget = NavSatFix() wpTarget.latitude = WPADJ[0] wpTarget.longitude = WPADJ[1] wpTarget.altitude = 25.0 self.wpPub.publish(wpTarget) print("coordSent") self.coordSent = True tmp = NavSatFix() else: # print(self.dronePos) print("Warning: no pylons found! loitering...") self.sendState(False)
def callback(self): #Publish data at 1 Hz rate = rospy.Rate(1) #Define the RTKRCV server ports corresponding to each of the processing #Modes port = 5801 sock = socket.socket() host = socket.gethostname() sock.connect((host, port)) #WGS84 Parameters e2 = 6.69437999014e-3 a = 6378137.0 #At a rate of 1 hz, create an rtklib message for each socket and publish #the data over while not rospy.is_shutdown(): #Create and RTKLIB and a NavSatFix data structure rtk = rtklib() navsat = NavSatFix() #Set the data structure header time to the current system time navsat.header.stamp = rospy.Time.now() rtk.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = socket.recv(1024) #Split the message msg = msgStr.split() #Save the latitude, longitude and ellipsoid height navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) #Save the position covariance navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN #Compute the radius of curvature in the N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) #Compute and store the ECEF position rtk.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) rtk.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) rtk.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) rtk.delay = float(msg[13]) rtk.ftest = float(msg[14]) #Store the NavSatFix data rtk.state = navsat pubs[i].publish(rtk) rate.sleep()
def init_path(): # logitude, latitute, altitute, orientation (+deg) gpsPub = rospy.Publisher("/fix_path_init", NavSatFix, queue_size=10) wpSub = rospy.Subscriber("/gps_path_init", Odometry, utm_cb) navStatus = NavSatStatus() navStatus.service = 1 navStatus.status = 1 # waypoints hardcoded for now, may want to spec with argument in future num_wp = gps_waypoints.shape[0] hdop = 1.5 position_covariance_type = 1 global wp_num for i in range(0, num_wp): # send gps waypoint for conversion header = Header() header.stamp = rospy.Time.now() header.frame_id = 'base_link' msgOut = NavSatFix() msgOut.position_covariance[0] = hdop**2 msgOut.position_covariance[4] = hdop**2 msgOut.position_covariance[8] = (2 * hdop)**2 msgOut.longitude = gps_waypoints[i][0] msgOut.latitude = gps_waypoints[i][1] msgOut.altitude = gps_waypoints[i][2] msgOut.position_covariance_type = position_covariance_type msgOut.status = navStatus msgOut.header = header wp_num = i gpsPub.publish(msgOut)
def make_fix(t, data, args): fix = NavSatFix() fix.header.stamp = t fix.header.frame_id = args.frame_id if data.Q in (1, 2, 4): # Fix, Float, GPS fix.status.status = NavSatStatus.STATUS_GBAS_FIX elif data.Q == 3: # SBAS fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif data.Q == 5: # Single fix.status.status = NavSatStatus.STATUS_FIX else: fix.status.status = NavSatStatus.STATUS_NO_FIX fix.latitude = data.lat fix.longitude = data.lon fix.altitude = data.height fix.position_covariance = [ covariance(data.sde), covariance(data.sdne), covariance(data.sdeu), covariance(data.sdne), covariance(data.sdn), covariance(data.sdun), covariance(data.sdeu), covariance(data.sdun), covariance(data.sdu), ] fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN return fix
def read_rtk_gps(): s = socket.socket() # Create a socket object # these are parameters loaded onto the ros parameter servers host = rospy.get_param('~host', '172.20.10.5') # these are parameters loaded onto the ros parameter servers port = rospy.get_param('~port', 3000) s.connect((host, port)) rospy.init_node('rtk_gps_node') pub_navsatfix = rospy.Publisher("/rtk_fix", NavSatFix, queue_size=0) navsatfix = NavSatFix() while True: GPS_msgs = s.recv(1024) msgs = re.split('\r\n', GPS_msgs) for msg in msgs: if msg[:6] == "$GPGGA": print msg data = msg.split(',') navsatfix.status = NavSatStatus(status=int(data[6])) navsatfix.latitude = float(data[2][:2]) navsatfix.longitude = float(data[4][:3]) navsatfix.altitude = float(data[9]) navsatfix.header.stamp = rospy.Time.now() pub_navsatfix.publish(navsatfix)
def rtk_gps_node(): # queue sine set up to large avoid message lost from buffer # define pub publish to rtk_fix rostopic pub = rospy.Publisher('rtk_fix', NavSatFix, queue_size=100000) rospy.init_node('rtk_gps_node', anonymous=True) # 100hz for more serial message to PC rate = rospy.Rate(100) # getting serial message from /dev/ttyACM0 baud rate as 115200 ser = serial.Serial('/dev/ttyACM0', '115200') navs = NavSatFix() header = Header() while not rospy.is_shutdown(): readinstring = ser.readline() oristr = readinstring.split(',') # when the serial message title as GNGGA, stor in navs(NavaSatFix) structure if oristr[0] == "$GNGGA": if oristr[3] == 'N': navs.latitude = float(float(oristr[2]) / 100) + float(float(oristr[2]) % 100) / 60 elif oristr[3] == 'S': navs.latitude = -(float(float(oristr[2]) / 100) + float(float(oristr[2]) % 100) / 60) if oristr[5] == 'E': navs.longitude = float(float(oristr[4]) / 100) + float( float(oristr[4]) % 100) / 60 elif oristr[5] == 'W': navs.longitude = -(float(float(oristr[4]) / 100) + float(float(oristr[4]) % 100) / 60) navs.altitude = float(oristr[9]) #publish navs to rtk_fix pub.publish(navs) print(readinstring) rate.sleep()
def bestpos_handler(self, bestpos): navsat = NavSatFix() # TODO: The timestamp here should come from SPAN, not the ROS system time. navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame # Assume GPS - this isn't exposed navsat.status.service = NavSatStatus.SERVICE_GPS position_type_to_status = { BESTPOS.POSITION_TYPE_NONE: NavSatStatus.STATUS_NO_FIX, BESTPOS.POSITION_TYPE_FIXED: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_FIXEDHEIGHT: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_FLOATCONV: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_WIDELANE: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_NARROWLANE: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_DOPPLER_VELOCITY: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_SINGLE: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_WAAS: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_PROPAGATED: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_OMNISTAR: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_L1_FLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_IONOFREE_FLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_NARROW_FLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_L1_INT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_WIDE_INT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_NARROW_INT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_RTK_DIRECT_INS: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_SBAS: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_INS_PSRSP: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_RTKFLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_RTKFIXED: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_OMNISTAR: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_OMNISTAR_HP: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_OMNISTAR_XP: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_OMNISTAR_HP: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_OMNISTAR_XP: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_PPP: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_INS_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_INS_PPP: NavSatStatus.STATUS_SBAS_FIX, } navsat.status.status = position_type_to_status.get(bestpos.position_type, NavSatStatus.STATUS_NO_FIX) # Position in degrees. navsat.latitude = bestpos.latitude navsat.longitude = bestpos.longitude # Altitude in metres. navsat.altitude = bestpos.altitude navsat.position_covariance[0] = pow(2, bestpos.latitude_std) navsat.position_covariance[4] = pow(2, bestpos.longitude_std) navsat.position_covariance[8] = pow(2, bestpos.altitude_std) navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN # Ship ito self.pub_navsatfix.publish(navsat)
def got_position(xx, yy, aa, stamp): global EP fix = NavSatFix() fix.header.stamp = stamp fix.header.frame_id = "sim_gps" fix.status.status = 0 fix.status.service = 1 utm_e = base_xx + base_utm_e + xx + CONFIG["SIM_ERROR"] * cos((yy + time()) / EP) utm_n = base_yy + base_utm_n + yy + CONFIG["SIM_ERROR"] * cos((utm_e / 3.14) / EP) (lon, lat) = conv(utm_e, utm_n, inverse=True) fix.latitude = lat fix.longitude = lon fix.altitude = 1.3 # print "got_position: %f %f" % (lat, lon) gps.publish(fix) pos.publish(Pose2D(utm_e, utm_n, aa)) tfBr.sendTransform( (utm_e - base_xx, utm_n - base_yy, 0), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/map", "/gps", )
def publish_navsatfix(msg, num): global i_navsatfix if msg.name() != 'NAV_HPPOSLLH': print "error: invalid type of message!!!!" return navsatfix = NavSatFix() navsatfix.header.stamp = rospy.Time.now() if num == 1: navsatfix.header.frame_id = "/left_gnss" elif num == 2: navsatfix.header.frame_id = "/right_gnss" navsatfix.header.seq = i_navsatfix[num - 1] navsatfix.latitude = msg.Latitude * 1e-7 + msg.latHp * 1e-9 navsatfix.longitude = msg.Longitude * 1e-7 + msg.lonHp * 1e-9 navsatfix.altitude = msg.height * 1e-3 + msg.heightHp * 1e-4 navsatfix.position_covariance = [(msg.hAcc * 1e-4) / math.sqrt(2.), 0, 0, 0, (msg.hAcc * 1e-4) / math.sqrt(2.), 0, 0, 0, (msg.vAcc * 1e-4)] navsatfix.status.status = fix_status[num - 1] if num == 1: pub_leftnavsatfix.publish(navsatfix) elif num == 2: pub_rightnavsatfix.publish(navsatfix) i_navsatfix[num - 1] += 1
def wecar_callback(wecar_msg): navsat_msg = NavSatFix() navsat_msg.header.frame_id = 'map' navsat_msg.header.stamp = rospy.Time.now() navsat_msg.latitude = wecar_msg.latitude navsat_msg.longitude = wecar_msg.longitude navsat_msg.altitude = wecar_msg.altitude wecar_pub.publish(navsat_msg)
def publish_active_way_point(self): if self.active_directions: if len(self.current_way_points) > 0: way_point_msg = NavSatFix() way_point_msg.longitude = self.current_way_points[0][0] way_point_msg.latitude = self.current_way_points[0][1] way_point_msg.altitude = self.current_way_points[0][2] self.ros_pub_gps_way_point.publish(way_point_msg)
def publish_gps(self): if self.drone.positionGPS!=None: gps_data = NavSatFix() gps_data.latitude = self.drone.positionGPS[0] gps_data.longitude = self.drone.positionGPS[1]#maybe reversed? gps_data.altitude = self.drone.positionGPS[2] gps_data.header.frame_id = self.unique_id self.gps_pub.publish(gps_data)
def onPositionChange(self, latitude, longitude, altitude): fix = NavSatFix() fix.header.stamp = rospy.Time.now() fix.header.frame_id = "odom" fix.latitude = latitude fix.longitude = longitude fix.altitude = altitude global_pose_pub.publish(fix)
def gps(): print utm.conversion.R import roslib; roslib.load_manifest('ardrone_control') import rospy; global rospy from sensor_msgs.msg import NavSatFix # rospy.init_node('test') gps = GPS() #print gps m = NavSatFix() m.latitude = 45.0 m.altitude = utm.conversion.R m.longitude = 20.0 gps.set_zero(m) gps.measure(m) print gps.get_state() print gps.Z m.latitude = 45.1 m.altitude = utm.conversion.R m.longitude = 20.0 gps.measure(m) print gps.get_state() print gps.Z gps.set_zero_yaw( 30 * pi/180 ) gps.measure(m) print gps.get_state() print gps.get_measurement() print gps.Z for i in range(10000): gps.measure(m) gps.Broadcast() rospy.spin()
def publish_gps(gps_pub, imu_data): gps = NavSatFix() gps.header.frame_id = FRAME_ID gps.header.stamp = rospy.Time.now() gps.latitude = imu_data.lat gps.longitude = imu_data.lon gps.altitude = imu_data.alt gps_pub.publish(gps)
def publish_gps(self, data): G = NavSatFix() gps_fmt = 'c3f' read_data = struct.unpack(gps_fmt, data) G.latitude = read_data[1] G.longitude = read_data[2] G.altitude = read_data[3] self.gps_pub.publish(G) rospy.loginfo("Published GPS MSG")
def navsat_callback(gps_msg): navsat_msg = NavSatFix() navsat_msg.header.frame_id = 'map' navsat_msg.latitude = gps_msg.latitude navsat_msg.altitude = gps_msg.altitude navsat_msg.longitude = gps_msg.longitude navsat_msg.header.stamp = rospy.Time.now() navsat_msg.status.status = 1 navsat_pub.publish(navsat_msg)
def default(self, ci='unused'): gps = NavSatFix() gps.header = self.get_ros_header() gps.latitude = self.data['x'] gps.longitude = self.data['y'] gps.altitude = self.data['z'] self.publish(gps)
def gps(): # If using I2C, we'll create an I2C interface to talk to using default pins i2c = board.I2C() # Create a GPS module instance. gps = adafruit_gps.GPS_GtopI2C(i2c, debug=False) # Use I2C interface # Turn on the basic GGA and RMC info (what you typically want) gps.send_command(b"PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0") # Set update rate to once a second (1hz) gps.send_command(b"PMTK220,1000") rospy.loginfo("Initializing gps publisher") gps_pub = rospy.Publisher('/gps', NavSatFix, queue_size=5) rospy.loginfo("Publishing NavSatFix at: " + gps_pub.resolved_name) rospy.init_node('gps_node') rate = rospy.Rate(1) # 50hz while not rospy.is_shutdown(): gps.update() if gps.has_fix: nav = NavSatFix() nav.header.stamp = rospy.Time.now() nav.header.frame_id = 'Adafruit Mini GPS PA1010D' nav.latitude = gps.latitude nav.longitude = gps.longitude nav.altitude = gps.altitude_m rospy.loginfo("=" * 40) rospy.loginfo("Latitude: {0:.6f} degrees".format(gps.latitude)) rospy.loginfo("Longitude: {0:.6f} degrees".format(gps.longitude)) rospy.loginfo("Fix quality: {}".format(gps.fix_quality)) # Some attributes beyond latitude, longitude and timestamp are optional # and might not be present. Check if they're None before trying to use! if gps.satellites is not None: rospy.loginfo("# satellites: {}".format(gps.satellites)) if gps.altitude_m is not None: rospy.loginfo("Altitude: {} meters".format(gps.altitude_m)) if gps.speed_knots is not None: rospy.loginfo("Speed: {} knots".format(gps.speed_knots)) if gps.track_angle_deg is not None: rospy.loginfo("Track angle: {} degrees".format(gps.track_angle_deg)) if gps.horizontal_dilution is not None: rospy.loginfo("Horizontal dilution: {}".format(gps.horizontal_dilution)) if gps.height_geoid is not None: rospy.loginfo("Height geoid: {} meters".format(gps.height_geoid)) gps_pub.publish(nav) else: # Try again if we don't have a fix yet. rospy.loginfo("Waiting for fix...") rate.sleep()
def publishWaypoint(self): msg = NavSatFix() msg.longitude = self.longitude msg.latitude = self.latitude msg.altitude = self.height outstr = "Drone is publishing waypoint: objective is at (%f|%f|%f)" % ( self.longitude, self.latitude, self.height) rospy.loginfo(outstr) self.pub.publish(msg)
def getMsg(): pub_navsat = rospy.Publisher('/rtklib/rover', NavSatFix) pub_pose = rospy.Publisher('/rtklib/pose', PoseStamped) #Initialize the RTKLIB ROS node rospy.init_node('rtklib_messages', anonymous=True) #Define the publishing frequency of the node rate = rospy.Rate(10) #Create a socket sock = socket.socket() #Get the address of the local host host = socket.gethostname() #Connect to the RTKRCV server that is bound to port xxxx port = 5801 sock.connect((host,port)) e2 = 6.69437999014e-3 a = 6378137.0 while not rospy.is_shutdown(): navsat = NavSatFix() ecef_xyz = Point() ecef_pose = Pose() ecef_stampedPose = PoseStamped() ecef_stampedPose = navsat.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = sock.recv(1024) #Split the message msg = msgStr.split() navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) ecef_pose.position = ecef_xyz ecef_stampedPose.pose = ecef_pose pub_navsat.publish(navsat) pub_pose.publish(ecef_stampedPose) rate.sleep()
def callback_sbp_pos(self, msg, **metadata): if self.debug: rospy.loginfo("Received SBP_MSG_POS_LLH (Sender: %d): %s" % (msg.sender, repr(msg))) out = NavSatFix() out.header.frame_id = self.frame_id out.header.stamp = rospy.Time.now() out.status.service = NavSatStatus.SERVICE_GPS out.latitude = msg.lat out.longitude = msg.lon out.altitude = msg.height out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED if msg.flags & 0x03: self.last_rtk_time = time.time() self.rtk_fix_mode = msg.flags & 0x03 out.status.status = NavSatStatus.STATUS_GBAS_FIX # TODO this should probably also include covariance of base fix? out.position_covariance[0] = self.rtk_h_accuracy**2 out.position_covariance[4] = self.rtk_h_accuracy**2 out.position_covariance[8] = self.rtk_v_accuracy**2 pub = self.pub_rtk_fix self.last_rtk_pos = msg # If we are getting this message, RTK is our best fix, so publish this as our best fix. self.pub_fix.publish(out) else: self.last_spp_time = time.time() out.status.status = NavSatStatus.STATUS_FIX # TODO hack, piksi should provide these numbers if self.last_dops: out.position_covariance[0] = (self.last_dops.hdop * 1E-2)**2 out.position_covariance[4] = (self.last_dops.hdop * 1E-2)**2 out.position_covariance[8] = (self.last_dops.vdop * 1E-2)**2 else: out.position_covariance[0] = COV_NOT_MEASURED out.position_covariance[4] = COV_NOT_MEASURED out.position_covariance[8] = COV_NOT_MEASURED pub = self.pub_spp_fix self.last_pos = msg # Check if SPP is currently our best available fix if self.rtk_fix_mode <= 0: self.pub_fix.publish(out) pub.publish(out)
def recieve_gps(self, data ): msg = NavSatFix() msg.latitude = data.vector.x msg.longitude = data.vector.y msg.altitude = data.vector.z msg.header.stamp = data.header.stamp msg.header.frame_id = data.header.frame_id self.publisher['gps'].publish(msg)
def default(self, ci='unused'): """ Publish the data of the gps sensor as a custom ROS NavSatFix message """ gps = NavSatFix() gps.header = self.get_ros_header() # TODO ros.org/doc/api/sensor_msgs/html/msg/NavSatFix.html gps.latitude = self.data['x'] gps.longitude = self.data['y'] gps.altitude = self.data['z'] self.publish(pose)
def ExtFix2Fix(data): fix = NavSatFix() fix.header = data.header fix.status.status = data.status.status fix.status.service = data.status.position_source fix.latitude = data.latitude fix.longitude = data.longitude fix.altitude = data.altitude fix.position_covariance = data.position_covariance fix.position_covariance_type = data.position_covariance_type return fix
def callback(self): rate = rospy.Rate(10) ports = [5801,5802,5803] sockets = [] pubs = [self.static_pub, self.rtkStatic_pub, self.rtkDynamic_pub] for i in ports: sock = socket.socket() host = socket.gethostname() sock.connect((host, i)) sockets.append(sock) e2 = 6.69437999014e-3 a = 6378137.0 while not rospy.is_shutdown(): for i in range(len(sockets)): navsat = NavSatFix() ecef_xyz = Point() ecef_pose = Pose() #ecef_stampedPose = PoseStamped() navsat.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = sockets[i].recv(1024) #Split the message msg = msgStr.split() navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) ecef_pose.position = ecef_xyz #ecef_stampedPose.pose = ecef_pose pubs[i].publish(navsat) rate.sleep()
def gps_fix_sim(): pub=rospy.Publisher('sensor_msgs/NavSatFix', NavSatFix, queue_size=10) rospy.init_node('gps_sim', anonymous=True) rate = rospy.Rate(2) while not rospy.is_shutdown(): gps_fix = NavSatFix() gps_fix.latitude = 36.6 gps_fix.longitude = -121.1 gps_fix.altitude = 1.0 pub.publish(gps_fix) rate.sleep()
def pub_gps(msg_type, msg, bridge): pub = bridge.get_ros_pub("gps", NavSatFix, queue_size=1) fix = NavSatFix() fix.header.stamp = bridge.project_ap_time(msg) fix.header.frame_id = 'base_footprint' fix.latitude = msg.lat/1e07 fix.longitude = msg.lon/1e07 # NOTE: absolute (MSL) altitude fix.altitude = msg.alt/1e03 fix.status.status = NavSatStatus.STATUS_FIX fix.status.service = NavSatStatus.SERVICE_GPS fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN pub.publish(fix)
def testAutoOriginFromNavSatFix(self): rospy.init_node('test_initialize_origin') nsf_pub = rospy.Publisher('fix', NavSatFix, queue_size=2) origin_sub = self.subscribeToOrigin() self.test_stamp = True nsf_msg = NavSatFix() nsf_msg.latitude = swri['latitude'] nsf_msg.longitude = swri['longitude'] nsf_msg.altitude = swri['altitude'] nsf_msg.header.frame_id = "/far_field" nsf_msg.header.stamp = msg_stamp r = rospy.Rate(10.0) while not rospy.is_shutdown(): nsf_pub.publish(nsf_msg) r.sleep()
def test_telemetry_serializer(self): """Tests telemetry serializer.""" # Set up test data. navsat = NavSatFix() navsat.latitude = 38.149 navsat.longitude = -76.432 navsat.altitude = 30.48 compass = Float64(90.0) json = serializers.TelemetrySerializer.from_msg(navsat, compass) altitude_msl = serializers.meters_to_feet(navsat.altitude) # Compare. self.assertEqual(json["latitude"], navsat.latitude) self.assertEqual(json["longitude"], navsat.longitude) self.assertEqual(json["altitude_msl"], altitude_msl) self.assertEqual(json["uas_heading"], compass.data)
def publish(self, data): msg = NavSatFix() msg.header.frame_id = self._frameId msg.header.stamp = rospy.get_rostime() msg.latitude = data.getLat() msg.longitude = data.getLng() msg.altitude = data.getMeters() if data.getFix() == 1: msg.status.status = 0 else: msg.status.status = -1 msg.position_covariance_type = 1 msg.position_covariance[0] = data.getHDOP() * data.getHDOP() msg.position_covariance[4] = data.getHDOP() * data.getHDOP() msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP() msg.status.service = 1 self._pub.publish(msg)
def subCB(msg_in): global pub msg_out = NavSatFix() msg_out.header = msg_in.header msg_out.status.status = NavSatStatus.STATUS_FIX # TODO - fix this msg_out.status.service = NavSatStatus.SERVICE_GPS msg_out.latitude = msg_in.LLA.x msg_out.longitude = msg_in.LLA.y msg_out.altitude = msg_in.LLA.z msg_out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED msg_out.position_covariance[1] = msg_in.PosUncerainty pub.publish(msg_out)
def sub_insCB(msg_in): global pub_imu global pub_gps global msg_imu msg_imu.header.stamp = msg_in.header.stamp msg_imu.header.frame_id = msg_in.header.frame_id # Convert the RPY data from the Vectornav into radians! roll = (math.pi * msg_in.RPY.x) / 180.0 pitch = (math.pi * msg_in.RPY.y) / 180.0 yaw = (math.pi * msg_in.RPY.z) / 180.0 q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) msg_imu.orientation.x = q[0] msg_imu.orientation.y = q[1] msg_imu.orientation.z = q[2] msg_imu.orientation.w = q[3] pub_imu.publish(msg_imu) msg_gps = NavSatFix() msg_gps.header = msg_in.header msg_gps.status.status = NavSatStatus.STATUS_FIX # TODO - fix this msg_gps.status.service = NavSatStatus.SERVICE_GPS msg_gps.latitude = msg_in.LLA.x msg_gps.longitude = msg_in.LLA.y msg_gps.altitude = msg_in.LLA.z msg_gps.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED msg_gps.position_covariance[0] = msg_in.PosUncerainty pub_gps.publish(msg_gps) msg_time = TimeReference() msg_time.header.stamp = msg_in.header.stamp msg_time.header.frame_id = msg_in.header.frame_id unix_time = 315964800 + (msg_in.Week * 7 * 24 * 3600) + msg_in.Time msg_time.time_ref = rospy.Time.from_sec(unix_time) pub_time.publish(msg_time)
def talker(): pub = rospy.Publisher("/gps/fix", NavSatFix, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) while not rospy.is_shutdown(): #gps config gpsd.next() os.system('clear') gps_data = NavSatFix() #status of gps # gps_data.status.status = 0 # gps_data.status.service = 1 #time gps_data.header.stamp.secs = round(time.time()) #position gps_data.latitude = gpsd.fix.latitude gps_data.longitude = gpsd.fix.longitude gps_data.altitude = 0 # gps_data.position_covariance_type = 0 #publication ros pub.publish(gps_data) rate.sleep()
def gps2navsat(filename, bag): with open(filename, 'r') as file: try: while True: data = struct.unpack('qddd', file.read(8*4)) time = data[0] local = data[1:] lat_lon_el_theta = struct.unpack('dddd', file.read(8*4)) gps_cov = struct.unpack('dddddddddddddddd', file.read(8*16)) if abs(lat_lon_el_theta[0]) < 1e-1: continue navsat = NavSatFix() navsat.header.frame_id = 'gps' navsat.header.stamp = rospy.Time.from_sec(time * 1e-6) navsat.status.status = NavSatStatus.STATUS_FIX navsat.status.service = NavSatStatus.SERVICE_GPS navsat.latitude = lat_lon_el_theta[0] navsat.longitude = lat_lon_el_theta[1] navsat.altitude = lat_lon_el_theta[2] navsat.position_covariance = numpy.array(gps_cov).reshape(4, 4)[:3, :3].flatten().tolist() navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN bag.write('/gps/fix', navsat, navsat.header.stamp) geopoint = GeoPointStamped() geopoint.header = navsat.header geopoint.position.latitude = lat_lon_el_theta[0] geopoint.position.longitude = lat_lon_el_theta[1] geopoint.position.altitude = lat_lon_el_theta[2] bag.write('/gps/geopoint', geopoint, geopoint.header.stamp) except: print 'done'
def publish(self, data): now = int(round(time.time() * 1000)) msg = NavSatFix() msg.header.frame_id = self._frameId msg.header.stamp = rospy.get_rostime() msg.latitude = data.getLat() msg.longitude = data.getLng() msg.altitude = data.getMeters() cur_fix = data.getFix() #print cur_fix if (cur_fix != self._old_fix): msg.status.status = 0 self._old_fix = cur_fix self._wd = now elif (now - self._wd) < GPS_WD_TIMEOUT: msg.status.status = 0 else: msg.status.status = -1 msg.position_covariance_type = 1 msg.position_covariance[0] = data.getHDOP() * data.getHDOP() msg.position_covariance[4] = data.getHDOP() * data.getHDOP() msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP() msg.status.service = 1 self._pub.publish(msg)
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if 'B' in parsed_sentence: data = parsed_sentence['B'] # Only publish a fix from RMC if the use_RMC flag is set. if True: current_fix.status.status = NavSatStatus.STATUS_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.latitude = data['latitude'] current_fix.longitude = data['longitude'] current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if True: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) else: return False
def navigation_handler(self, data): """ Rebroadcasts navigation data in the following formats: 1) /odom => /base footprint transform (if enabled, as per REP 105) 2) Odometry message, with parent/child IDs as in #1 3) NavSatFix message, for systems which are knowledgeable about GPS stuff 4) IMU messages """ rospy.logdebug("Navigation received") # If we don't have a fix, don't publish anything... if self.nav_status.status == NavSatStatus.STATUS_NO_FIX: return orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(data.pitch), RAD(data.heading)).GetQuaternion() # UTM conversion (zone, easting, northing) = LLtoUTM(23, data.latitude, data.longitude) # Initialize starting point if we haven't yet # TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid? if not self.init and self.zero_start: self.origin.x = easting self.origin.y = northing self.init = True # Publish origin reference for others to know about p = Pose() p.position.x = self.origin.x p.position.y = self.origin.y self.pub_origin.publish(p) # # Odometry # TODO: Work out these covariances properly from DOP # odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = self.odom_frame odom.child_frame_id = self.base_frame odom.pose.pose.position.x = easting - self.origin.x odom.pose.pose.position.y = northing - self.origin.y odom.pose.pose.position.z = data.altitude odom.pose.pose.orientation = Quaternion(*orient) odom.pose.covariance = POSE_COVAR # Twist is relative to /vehicle frame odom.twist.twist.linear.x = data.speed odom.twist.twist.linear.y = 0 odom.twist.twist.linear.z = -data.down_vel odom.twist.twist.angular.x = RAD(data.ang_rate_long) odom.twist.twist.angular.y = RAD(-data.ang_rate_trans) odom.twist.twist.angular.z = RAD(-data.ang_rate_down) odom.twist.covariance = TWIST_COVAR self.pub_odom.publish(odom) # # Odometry transform (if required) # if self.publish_tf: self.tf_broadcast.sendTransform( (odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z), Quaternion(*orient), odom.header.stamp,odom.child_frame_id, odom.frame_id) # # NavSatFix # TODO: Work out these covariances properly from DOP # navsat = NavSatFix() navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame navsat.status = self.nav_status navsat.latitude = data.latitude navsat.longitude = data.longitude navsat.altitude = data.altitude navsat.position_covariance = NAVSAT_COVAR navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self.pub_navsatfix.publish(navsat) # # IMU # TODO: Work out these covariances properly # imu = Imu() imu.header.stamp == rospy.Time.now() imu.header.frame_id = self.base_frame # Orientation imu.orientation = Quaternion(*orient) imu.orientation_covariance = IMU_ORIENT_COVAR # Angular rates imu.angular_velocity.x = RAD(data.ang_rate_long) imu.angular_velocity.y = RAD(-data.ang_rate_trans) imu.angular_velocity.y = RAD(-data.ang_rate_down) imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration imu.linear_acceleration.x = data.long_accel imu.linear_acceleration.y = data.trans_accel imu.linear_acceleration.z = data.down_accel imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu) pass
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libjavad_navsat_driver.parser.parse_nmea_sentence(nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude hdop = data['hdop'] current_fix.position_covariance[0] = hdop ** 2 current_fix.position_covariance[4] = hdop ** 2 current_fix.position_covariance[8] = (2 * hdop) ** 2 # FIXME current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level #altitude = data['altitude'] + data['mean_sea_level'] altitude = data['altitude'] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.time_ref_pub.publish(current_time_ref) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) current_vel.twist.angular.z = data['true_course'] self.vel_pub.publish(current_vel) else: return False
def talker(): ser.flush() pub = rospy.Publisher('gps', NavSatFix, queue_size=100) pub_n = rospy.Publisher('comb', comb, queue_size=100) rospy.init_node('gps_node', anonymous=True) rate = rospy.Rate(100) # 10hz GPSmsg = NavSatFix() combMsg = comb() seq = 0 while not rospy.is_shutdown(): if (ser.read() != '$'): continue line0 = ser.readline() cs = line0[-4:-2] cs1 = int(cs, 16) cs2 = checksum(line0) print("cs1,cs2", cs1, cs2) if (cs1 != cs2): continue line = line0.replace("GPFPD,", "") line = line.replace("\r\n", "") if ("\x00" in line): continue if (not string.find('*', line)): continue line = line.replace("*", ",") words = string.split(line, ",") GPSWeek = string.atoi(words[0]) GPSTime = string.atof(words[1]) Heading = string.atof(words[2]) Pitch = string.atof(words[3]) Roll = string.atof(words[4]) Lattitude = string.atof(words[5]) Longitude = string.atof(words[6]) Altitude = string.atof(words[7]) Ve = string.atof(words[8]) Vn = string.atof(words[9]) Vu = string.atof(words[10]) Baseline = string.atof(words[11]) NSV1 = string.atoi(words[12]) NSV2 = string.atoi(words[13]) Status = words[14] combMsg.GPSWeek = GPSWeek combMsg.GPSTime = GPSTime combMsg.Heading = Heading combMsg.Pitch = Pitch combMsg.Roll = Roll combMsg.Lattitude = Lattitude combMsg.Longitude = Longitude combMsg.Altitude = Altitude combMsg.Ve = Ve combMsg.Vn = Vn combMsg.Vu = Vu combMsg.Baseline = Baseline combMsg.NSV1 = NSV1 combMsg.NSV2 = NSV2 combMsg.Status = Status GPSmsg.latitude = Lattitude GPSmsg.longitude = Longitude GPSmsg.altitude = Altitude #GPSmsg.status.status = Status GPSmsg.header.stamp = rospy.Time.now() GPSmsg.header.frame_id = 'gps_node' GPSmsg.header.seq = seq #rospy.loginfo(hello_str) pub.publish(GPSmsg) print("pub GPS") pub_n.publish(combMsg) print("pub comb") seq = seq + 1 rate.sleep()
def talker(): pubGPS = rospy.Publisher('poseGPS', NavSatFix, queue_size=1000) pubIMU = rospy.Publisher('imuBoat', Imu, queue_size=1000) pubTwist = rospy.Publisher('twistGPS', TwistWithCovarianceStamped, queue_size=1000) rospy.init_node('imugpspublisher', anonymous=True) rate = rospy.Rate(100) # 100hz while not rospy.is_shutdown(): data = ser.readline() if data[0] == 'Z': data = data.replace("Z","").replace("\n","").replace("\r","") data_list = data.split(',') if len(data_list) == 4: try: ##data_list structure: lat, lon, heading, vel float_list = [float(i) for i in data_list] poseGPS = NavSatFix() poseGPS.header.frame_id = "world" poseGPS.header.stamp = rospy.Time.now() poseGPS.status.status = 0 poseGPS.status.service = 1 poseGPS.latitude = float_list[0] poseGPS.longitude = float_list[1] poseGPS.altitude = 0.0 poseGPS.position_covariance = [3.24, 0, 0, 0, 3,24, 0, 0, 0, 0] poseGPS.position_covariance_type = 2 pubGPS.publish(poseGPS) twistGPS = TwistWithCovarianceStamped() twistGPS.header = poseGPS.header twistGPS.twist.twist.linear.x = float_list[3]*knots*math.cos(latest_yaw) twistGPS.twist.twist.linear.y = float_list[3]*knots*math.sin(latest_yaw) twistGPS.twist.twist.linear.z = 0.0 ##angular data not used here twistGPS.twist.covariance = [0.01, 0.01, 0, 0, 0, 0] pubTwist.publish(twistGPS) log = "GPS Data: %f %f %f %f Publish at Time: %s" % (float_list[0], float_list[1], float_list[2], float_list[3], rospy.get_time()) except: log = "GPS Data Error! Data : %s" % data else: log = "GPS Data Error! Data : %s" % data rospy.loginfo(log) elif data[0] == 'Y': data = data.replace("Y","").replace("\n","").replace("\r","") data_list = data.split(',') if len(data_list) == 9: try: ##data_list structure: accel, magni, gyro float_list = [float(i) for i in data_list] imuData = Imu() imuData.header.frame_id = "base_link" imuData.header.stamp = rospy.Time.now() latest_yaw = float_list[3] ##data form in yaw, pitch, roll quat = tf.transformations.quaternion_from_euler(float_list[3], float_list[4], float_list[5], 'rzyx') imuData.orientation.x = quat[0] imuData.orientation.y = quat[1] imuData.orientation.z = quat[2] imuData.orientation.w = quat[3] imuData.angular_velocity.x = math.radians(float_list[6]*gyro_scale) imuData.angular_velocity.y = math.radians(-float_list[7]*gyro_scale) imuData.angular_velocity.z = math.radians(-float_list[8]*gyro_scale) imuData.linear_acceleration.x = float_list[0]*accel_scale imuData.linear_acceleration.y = -float_list[1]*accel_scale imuData.linear_acceleration.z = -float_list[2]*accel_scale imuData.orientation_covariance = [1.5838e-6, 0, 0, 0, 1.49402e-6, 0, 0, 0, 1.88934e-6] imuData.angular_velocity_covariance = [7.84113e-7, 0, 0, 0, 5.89609e-7, 0, 0, 0, 6.20293e-7] imuData.linear_acceleration_covariance = [9.8492e-4, 0, 0, 0, 7.10809e-4, 0, 0, 0, 1.42516e-3] pubIMU.publish(imuData) log = "IMU Data: %f %f %f %f %f %f %f %f %f Publish at Time: %s" \ % (imuData.linear_acceleration.x, imuData.linear_acceleration.y, imuData.linear_acceleration.z, float_list[3], float_list[4], float_list[5], imuData.angular_velocity.x, imuData.angular_velocity.y, imuData.angular_velocity.z, rospy.get_time()) except: log = "IMU Data Error! Data : %s" % data else: log = "Data Error! Data : %s" % data rospy.loginfo(log) else: log = "Data Error or Message: %s" % data rospy.loginfo(log) rate.sleep()