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): r = rospy.Rate(self.freq) while not rospy.is_shutdown(): t = TimeReference() t.source = str(self.freq) t.header.stamp = rospy.Time.now() self.pub.publish(t) r.sleep()
def callback(self, data): ''' Callback function, <data> is the depth image ''' try: time1 = time.time() try: frame = self.bridge.imgmsg_to_cv2(data, desired_encoding="passthrough") except CvBridgeError as err: print err return mog2_res = self.mog2.run(False, frame.astype(np.float32)) if mog2_res is None: return mask1 = cv2.morphologyEx(mog2_res.copy(), cv2.MORPH_OPEN, self.open_kernel) check_sum = np.sum(mask1 > 0) if not check_sum or check_sum == np.sum(frame > 0): return _, contours, _ = cv2.findContours(mask1, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) cont_ind = np.argmax([cv2.contourArea(contour) for contour in contours]) final_mask = np.zeros_like(mask1) cv2.drawContours(final_mask, contours, cont_ind, 1, -1) #cv2.imshow('test',mask1*255) #cv2.waitKey(10) frame = frame * final_mask scores_exist,_ = self.used_classifier.run_testing(frame, online=True) #DEBUGGING #cv2.imshow('test',(frame%256).astype(np.uint8)) #cv2.waitKey(10) time2 = time.time() self.time.append(np.mean(self.time[-9:]+[(time2-time1)])) if (self.used_classifier.recognized_classes is not None and len(self.used_classifier.recognized_classes)>0 and scores_exist): self.image_pub.publish(self.bridge.cv2_to_imgmsg( self.used_classifier.frames_preproc.hand_img, "passthrough")) msg = TimeReference() try: msg.source = self.used_classifier.train_classes[ self.used_classifier.recognized_classes[-1]] except TypeError: msg.source = self.used_classifier.recognized_classes[-1].name msg.time_ref = Time() msg.time_ref.secs = int(1/float(self.time[-1]) if self.time[-1] else 0) self.class_pub.publish(msg) self.skel_pub.publish(self.bridge.cv2_to_imgmsg( np.atleast_2d(np.array(self.used_classifier. frames_preproc.skeleton.skeleton,np.int32)))) except Exception as e: exc_type, exc_value, exc_traceback = sys.exc_info() traceback.print_exception(exc_type, exc_value, exc_traceback, limit=10, file=sys.stdout)
def run(self): r = rospy.Rate(self.freq) while not rospy.is_shutdown(): t = TimeReference() t.source = str(self.freq) t.header.stamp = rospy.Time.now() if rospy.get_param("RL_agent_active", True): self.pub.publish(t) r.sleep()
def callback_sbp_gps_time(self, msg, **metadata): if self.debug: rospy.loginfo("Received SBP_MSG_GPS_TIME (Sender: %d): %s" % (msg.sender, repr(msg))) out = TimeReference() out.header.frame_id = self.frame_id out.header.stamp = rospy.Time.now() out.time_ref = ros_time_from_sbp_time(msg) out.source = "gps" self.pub_time.publish(out)
def posmv_listener(): position_pub = rospy.Publisher('/posmv/position', NavSatFix, queue_size=10) timeref_pub = rospy.Publisher('/posmv/time_reference', TimeReference, queue_size=10) orientation_pub = rospy.Publisher('/posmv/orientation', NavEulerStamped, queue_size=10) rospy.init_node('posmv') pos = posmv.Posmv() gps_week = None gps_utc_offset = None timestamp = datetime.datetime.utcfromtimestamp( rospy.Time.now().to_time()).isoformat() bag = rosbag.Bag( 'nodes/posmv_' + ('-'.join(timestamp.split(':'))) + '.bag', 'w', rosbag.Compression.BZ2) while not rospy.is_shutdown(): data = pos.read((1, 3)) #print data for d in data: if d['group_id'] == 1: now = rospy.get_rostime() pos_now = decode_time(d, gps_week, gps_utc_offset) if pos_now is not None: tref = TimeReference() tref.header.stamp = now tref.time_ref = rospy.Time( calendar.timegm(pos_now.timetuple()), pos_now.microsecond * 1000) tref.source = 'posmv' timeref_pub.publish(tref) bag.write('/posmv/time_reference', tref) nsf = NavSatFix() nsf.header.stamp = now nsf.header.frame_id = 'posmv' nsf.latitude = d['latitude'] nsf.longitude = d['longitude'] nsf.altitude = d['altitude'] position_pub.publish(nsf) bag.write('/posmv/position', nsf) nes = NavEulerStamped() nes.header.stamp = now nes.header.frame_id = 'posmv' nes.orientation.roll = d['vessel_roll'] nes.orientation.pitch = d['vessel_pitch'] nes.orientation.heading = d['vessel_heading'] orientation_pub.publish(nes) bag.write('/posmv/orientation', nes) if d['group_id'] == 3: gps_week = d['gps_utc_week_number'] gps_utc_offset = d['gps_utc_time_offset']
def publish_time_ref(secs, nsecs, source): """Publish a time reference.""" # Doesn't follow the standard publishing pattern since several time # refs could be published simultaneously if self.time_ref_pub is None: self.time_ref_pub = rospy.Publisher( '~time_reference', TimeReference, queue_size=10) time_ref_msg = TimeReference() time_ref_msg.header = self.h time_ref_msg.time_ref.secs = secs time_ref_msg.time_ref.nsecs = nsecs time_ref_msg.source = source self.time_ref_pub.publish(time_ref_msg)
def publish_time_ref(secs, nsecs, source): """Publish a time reference.""" # Doesn't follow the standard publishing pattern since several time # refs could be published simultaneously if self.time_ref_pub is None: self.time_ref_pub = rospy.Publisher( 'time_reference', TimeReference, queue_size=10) time_ref_msg = TimeReference() time_ref_msg.header = self.h time_ref_msg.time_ref.secs = secs time_ref_msg.time_ref.nsecs = nsecs time_ref_msg.source = source self.time_ref_pub.publish(time_ref_msg)
def ImageCallbackResize(Imagedata): ResizedImage = TimeReference() img = np.array(list(map(ord, list(Imagedata.data)))).astype("uint8") img = np.reshape(img[::-1], (Imagedata.height, Imagedata.width)) img = resize(img, (128, 128), preserve_range=True, mode='constant', anti_aliasing=True) img = np.reshape(img, (1, 16384)).astype("uint8") img_byte = img.tobytes() ResizedImage.source = base64.b64encode(img_byte) ResizedImage.header = Imagedata.header pub.publish(ResizedImage)
def __init__(self, **kwargs): self.system_name = socket.gethostname() rospy.init_node('timesync_node', anonymous=True) # Parameters self.timeref_topic = rospy.get_param("~topic", "walltime/time_reference") self.mode = rospy.get_param("~mode", "server") self.tolerance = rospy.get_param( "~tolerance", 2) # Tolerance in seconds for considering as "synchronized" # Publishers self.timeref_pub = rospy.Publisher(self.timeref_topic, TimeReference, queue_size=10) self.status_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=10) # Subscribers rospy.Subscriber(self.timeref_topic, TimeReference, callback=self.timeref_callback) # Messages self.timeref_msg = TimeReference() self.diag_status = DiagnosticStatus() self.last_update = 0 # Rate self.rate = rospy.Rate(0.5) # Once every 2 secs
def __init__(self): # Our publishers self.fix_pub = rospy.Publisher('tcpfix', NavSatFix, queue_size=1) self.vel_pub = rospy.Publisher('tcpvel', TwistStamped, queue_size=1) self.timeref_pub = rospy.Publisher('tcptime', TimeReference, queue_size=1) # Frame of references we should publish in self.frame_timeref = rospy.get_param('~frame_timeref', 'gps') self.frame_gps = rospy.get_param('~frame_gps', 'gps') self.use_rostime = rospy.get_param('~use_rostime', True) self.use_rmc = rospy.get_param('~use_rmc', False) # Publish NMEA messages, debug mode self.publish_nmea = rospy.get_param('~publish_nmea', False) if self.publish_nmea: self.nmea_pub = rospy.Publisher('~nmea', String, queue_size=1) #TODO: to be completed self.use_mag_ori = rospy.get_param('~use_magnetic_orientation', False) # Flags for what information we have self.has_fix = False self.has_std = False self.has_vel = False self.has_timeref = False # Blank messages we create self.msg_fix = NavSatFix() self.msg_vel = TwistStamped() self.msg_timeref = TimeReference()
def __init__(self): self.no_decbbox = 0 print("[INFO] Initializing ROS...") rospy.init_node('Detection') print("[INFO] Loading modules...") self.yolo = Load_Yolo_model() self.bridge = CvBridge() print("[INFO] Loading config...") # Create local variables self.timer = TimeReference() print("[INFO] Initialize ROS publishers...") # Create Topics to publish self.boxes_pub = rospy.Publisher("/Tracker/Detection/Boxes", Detection2DArray, queue_size=1) self.timer_pub = rospy.Publisher("/Tracker/Timer", TimeReference, queue_size=1) print("[INFO] Initialize ROS Subscribers...") rospy.Subscriber("/zed2/zed_node/left/image_rect_color/compressed", CompressedImage, self.callback, queue_size=400) # Create subscriptions print("[INFO] Loading complete")
def __init__(self): rospy.init_node('nmea_to_navsat_converter') # Get parameters self.nmea_sub = rospy.get_param("~nmea_sub", '/fmLib/nmea_from_gps') self.fix_pub = rospy.get_param("~fix_pub", 'fix') self.vel_pub = rospy.get_param("~vel_pub", 'vel') self.time_pub = rospy.get_param("~time_pub", 'time_reference') # Set up topics self.gpspub = rospy.Publisher(self.fix_pub, NavSatFix) self.gpsVelPub = rospy.Publisher(self.vel_pub, TwistStamped) self.gpstimePub = rospy.Publisher(self.time_pub, TimeReference) self.status_sub = rospy.Subscriber(self.nmea_sub, nmea, self.onNmea) # Set up frame self.frame_id = rospy.get_param('~frame_id', 'gps') if self.frame_id[0] != "/": self.frame_id = self.addTFPrefix(self.frame_id) self.time_ref_source = rospy.get_param('~time_ref_source', self.frame_id) self.useRMC = rospy.get_param('~useRMC', False) #useRMC == True -> generate info from RMC+GSA #useRMC == False -> generate info from GGA self.navData = NavSatFix() self.gpsVel = TwistStamped() self.gpstime = TimeReference() self.gpstime.source = self.time_ref_source self.navData.header.frame_id = self.frame_id self.gpsVel.header.frame_id = self.frame_id self.GPSLock = False
def __init__(self): print("[INFO] Initializing ROS...") rospy.init_node('Detection') print("[INFO] Loading modules...") self.yolo = Load_Yolo_model() self.bridge = CvBridge() print("[INFO] Loading config...") self.timer = TimeReference() print("[INFO] Initialize ROS publishers...") self.boxes_pub = rospy.Publisher("/Tracker/Detection/Boxes", Detection2DArray, queue_size=1) self.timer_pub = rospy.Publisher("/Tracker/Timer", TimeReference, queue_size=1) print("[INFO] Initialize ROS Subscribers...") rospy.Subscriber("/zed2/zed_node/left/image_rect_color/compressed", CompressedImage, self.callback_sub, queue_size=1) print("[INFO] Loading complete") self.init_time_delay = 0 self.callback()
def timepulse_callback(self, channel): self.get_logger().info(f"{time.time()} Timepulse trigger") gps_msg = NavSatFix() timeref_msg = TimeReference() msg_hdr = Header() system_time = self.get_clock().now().to_msg() msg_hdr.frame_id = 'base_link' # center of the plane try: ubx = self.ubp.read() except IOError: self.get_logger().warning("GPS disconnected. Attempting to reconnect.") self.ubp = GPSReader(self.port, self.baud, self.TIMEOUT, self.UBXONLY) return while ubx: if (ubx.msg_cls + ubx.msg_id) == b"\x01\x07": # NAV_PVT # <UBX(NAV-PVT, iTOW=16:50:32, year=2015, month=10, day=25, hour=16, min=50, second=48, valid=b'\xf0', tAcc=4294967295, nano=0, fixType=0, flags=b'\x00', flags2=b'$', numSV=0, lon=0, lat=0, height=0, hMSL=-17000, hAcc=4294967295, vAcc=4294967295, velN=0, velE=0, velD=0, gSpeed=0, headMot=0, sAcc=20000, headAcc=18000000, pDOP=9999, reserved1=65034815406080, headVeh=0, magDec=0, magAcc=0)> msg_hdr.stamp = self._gen_timestamp_from_utc(ubx) fix_stat = NavSatStatus() if ubx.fixType == 0: self.get_logger().warning(f"No fix yet.") break fix_stat.service = SERVICE_GPS gps_msg.status = fix_stat gps_msg.header = msg_hdr gps_msg.latitude = float(ubx.lat)/10000000 gps_msg.longitude = float(ubx.lon)/10000000 gps_msg.altitude = float(ubx.height)/1000 timeref_msg.header = msg_hdr timeref_msg.time_ref = system_time timeref_msg.source = "GPS" self.fix_pub.publish(gps_msg) self.time_pub.publish(timeref_msg) self.get_logger().info(f"Publishing gps message: ({timeref_msg.header.stamp.sec}.{timeref_msg.header.stamp.nanosec}): ({gps_msg.latitude}, {gps_msg.longitude}, {gps_msg.altitude})") return else: self.get_logger().info(f"Other GPS MSG: {(ubx.msg_cls + ubx.msg_id)}") ubx = self.ubp.read()
def __init__(self): # ------------------------------------------------------------------------------ # init values self.text_buffer = ' ' self.parser = anavs_parser.ANAVSParserUBX() self.odometry_msg = Odometry() self.rtk_groundtruth = odom() self.nav_msg = NavSatFix() self.gnss_time_msg = TimeReference() self.odom_local = Odometry() self.tcp_ip = rospy.get_param('/anavs_rtk_static_node/rtk_module_ip') #self.tcp_ip = rospy.get_param('/anavs_rtk_static_node/rtk_module_ip', "localhost") # dummy_receiver (PAD_solution) #self.tcp_ip = rospy.get_param('/anavs_rtk_static_node/rtk_module_ip', "192.168.42.1") # tum-nav #self.tcp_ip = rospy.get_param('/anavs_rtk_static_node/rtk_module_ip', "192.168.20.13") # dlr-kn: Columbus (pw: #LocoExplo#) #self.tcp_ip = rospy.get_param('/anavs_rtk_static_node/rtk_module_ip', "192.168.20.63") # dlr-kn: Vespucci (pw: #LocoExplo#) # ------------------------------------------------------------------------------ # create publisher, subscriber and node handle self.pub_odometry = rospy.Publisher('rtk_odometry_static', odom, queue_size=10) self.pub_nav = rospy.Publisher('gnss_nav_static', NavSatFix, queue_size=10) self.pub_time = rospy.Publisher('gnss_time_static', TimeReference, queue_size=10) rospy.init_node('anavs_rtk_static_node', anonymous=True) # ------------------------------------------------------------------------------ # create connection self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.tcp_socket.connect((self.tcp_ip, TCP_PORT)) self.tcp_socket.setblocking(0) print "Connected to RTK processing module %s:%d" % (self.tcp_ip, TCP_PORT) # ------------------------------------------------------------------------------ # main loop rate = rospy.Rate(10) while not rospy.is_shutdown(): readable, writable, exceptional = select.select([self.tcp_socket], [], [self.tcp_socket], 1) if self.tcp_socket in readable: self.text_buffer += self.tcp_socket.recv(4096) if self.parse_data_ubx(): # TODO: Remove 1 from the statement if self.parser.code == 4 or self.parser.code == 5 or self.parser.code == 1: ## 1 added just for test current_time = rospy.Time.now() self.build_odometry_msg(current_time, self.parser.code) self.build_nav_msg(current_time) self.build_time(current_time) rate.sleep()
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 main(args): model = YOLO(**vars(args)) node.create_subscription(Image, 'usb_cam/image_raw', callback) pub = node.create_publisher(TimeReference, 'people_info') msg = TimeReference() while rclpy.ok(): # for _ in range(5): rclpy.spin_once(node) try: result = detect_image(model, args.classes_path) print(result) except: result = {'objects': []} print('no people') msg.header = header msg.source = json.dumps(result) pub.publish(msg) model.close_session() node.destroy_node() rclpy.shutdown()
def process_line(self, nmea_string): # Check if valid message if not check_nmea_checksum(nmea_string): rospy.logwarn( "Received a sentence with an invalid checksum. Sentence was: %s" % repr(nmea_string)) return # Else we are good, lets try to process this message parsed_sentence = reach_ros_node.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logwarn("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return # We have a good message!! # Lets send it to the correct method to get processed self.parse_GGA(parsed_sentence) self.parse_GST(parsed_sentence) self.parse_VTG(parsed_sentence) self.parse_RMC(parsed_sentence) # Special care to parse the time reference # This can come from either the GGA or RMC self.parse_time(parsed_sentence) # Now that we are done with processing messages # Lets publish what we have! if self.has_fix and self.has_std: self.fix_pub.publish(self.msg_fix) self.msg_fix = NavSatFix() self.has_fix = False self.has_std = False if self.has_vel: self.vel_pub.publish(self.msg_vel) self.msg_vel = TwistStamped() self.has_vel = False if self.has_timeref: self.timeref_pub.publish(self.msg_timeref) self.msg_timeref = TimeReference() self.has_timeref = False
def __init__(self): # Our publishers self.fix_pub = rospy.Publisher('tcpfix', NavSatFix, queue_size=1) self.vel_pub = rospy.Publisher('tcpvel', TwistStamped, queue_size=1) self.timeref_pub = rospy.Publisher('tcptime', TimeReference, queue_size=1) # Frame of references we should publish in self.frame_timeref = rospy.get_param('~frame_timeref', 'gps') self.frame_gps = rospy.get_param('~frame_gps', 'gps') self.use_rostime = rospy.get_param('~use_rostime', True) self.use_rmc = rospy.get_param('~use_rmc', False) # Flags for what information we have self.has_fix = False self.has_std = False self.has_vel = False self.has_timeref = False # Blank messages we create self.msg_fix = NavSatFix() self.msg_vel = TwistStamped() self.msg_timeref = TimeReference()
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
if __name__ == "__main__": #init publisher rospy.init_node('http_gps_driver') gpspub = rospy.Publisher('fix', NavSatFix) gpsVelPub = rospy.Publisher('vel',TwistStamped) gpstimePub = rospy.Publisher('time_reference', TimeReference) #Init GPS port frame_id = rospy.get_param('~frame_id','gps') if frame_id[0] != "/": frame_id = addTFPrefix(frame_id) time_ref_source = rospy.get_param('~time_ref_source', frame_id) navData = NavSatFix() gpsVel = TwistStamped() gpstime = TimeReference() gpstime.source = time_ref_source navData.header.frame_id = frame_id gpsVel.header.frame_id = frame_id GPSLock = False try: #Read in GPS while not rospy.is_shutdown(): #read GPS line req = urllib2.Request('http://192.168.3.16/prog/show?position') response = urllib2.urlopen(req) data = response.read() rospy.sleep(0.5) timeNow = rospy.get_rostime()
rospy.init_node('novatel_gps_driver') gpsPub = rospy.Publisher('gps_fix', NavSatFix, queue_size=1) gpsStatPub = rospy.Publisher('gps_status', GPSStatus, queue_size=1) gpsVelPub = rospy.Publisher('gps_vel',TwistStamped, queue_size=1) #gpsTimePub = rospy.Publisher('time_reference', TimeReference) #Init gpsSerial port GPSport = rospy.get_param('~port','/dev/ttyUSB0') GPSrate = rospy.get_param('~baud',57600) frame_id = rospy.get_param('~frame_id','gps') if frame_id[0] != "/": frame_id = addTFPrefix(frame_id) time_ref_source = rospy.get_param('~time_ref_source', frame_id) navData = NavSatFix() gpsVel = TwistStamped() gpstime = TimeReference() gpsStatus = GPSStatus() gpstime.source = time_ref_source navData.header.frame_id = frame_id gpsVel.header.frame_id = frame_id GPSLock = False parser = NovatelParser() try: gpsSerial = serial.Serial(port=GPSport,baudrate=GPSrate,timeout=.01) #Read in gpsSerial data sync0 = '\x00'; sync1 = '\x00'; sync2 = '\x00'; while not rospy.is_shutdown(): # READ UNTIL SYNC data = gpsSerial.read(1)
def __init__(self): # Movement spesifications self.overlap = rospy.get_param('/cam_shutter/overlap') rospy.logwarn('The Overlap parm selected was %.2f' % self.overlap) # Camera spesifications self.cam_ref = rospy.get_param('/cam_shutter/Cam_ref') self.Focal_len = float(rospy.get_param('/cam_shutter/Focal_len')) self.Sensor_dim_h = float(rospy.get_param('/cam_shutter/Sensor_dim_h')) self.Sensor_dim_w = float(rospy.get_param('/cam_shutter/Sensor_dim_w')) self.Pixel_size = float(rospy.get_param('/cam_shutter/Pixel_size')) self.Depth_filed = float(rospy.get_param('/cam_shutter/Depth_filed')) rospy.Subscriber('/ekf', Odometry, self.EKF_calback) rospy.Subscriber('/echoes', MultiEchoLaserScan, self.Lidar_calback) rospy.Subscriber("/ctrl_flag", KeyValue, self.mode_callbaback) rospy.Subscriber('/WL', Float32, self.Wl_callbaback) rospy.Subscriber('/WR', Float32, self.Wr_callbaback) self.photo_cont = 0 self.t_photo = 0.0 self.t_source = "" self.dk = 0.0 self.r = 3.0 self.mode = 0 self.flag_k = True self.x1 = 0.0 self.y1 = 0.0 self.wl = 0.0 self.wr = 0.0 rate = rospy.Rate(100) # 100hz pub = rospy.Publisher('/cam_time', TimeReference, queue_size=10) pub_flag = rospy.Publisher('/cam_shut', Bool, queue_size=10) d = self.Cam_calculation(self.r) d_ = d * (1 - self.overlap) flag = False while not rospy.is_shutdown(): print(flag) if (self.dk >= d_) and (self.mode == 2): flag = True if (self.wl == 0.0) and (self.wr == 0.0): print('photo') os.system('echo 1 > /sys/class/gpio/gpio38/value') # take picture t_photo = TimeReference() t_photo.header.seq = self.photo_cont t_photo.header.stamp = rospy.Time.now() t_photo.header.frame_id = self.cam_ref t_photo.time_ref = self.t_photo t_photo.source = self.t_source pub.publish(t_photo) self.photo_cont = self.photo_cont + 1 self.flag_k = True self.dk = 0 d = self.Cam_calculation(self.r) d_ = d * (1 - self.overlap) flag = False os.system('echo 0 > /sys/class/gpio/gpio38/value') print(self.dk, d_) pub_flag.publish(flag) rate.sleep()
port = rospy.get_param('~port', 3333) global_frame_id = rospy.get_param('~global_frame_id', '/map') odom_frame_id = rospy.get_param('~odom_frame_id', '/odom') base_frame_id = rospy.get_param('~base_frame_id', '/base_link') publish_tf = rospy.get_param('~publish_tf', True) # Quality parameters accept_quality = rospy.get_param('~quality','1,2,4,5,6') accept_quality = [int(x) for x in accept_quality.split(',')] accept_num_sats = int(rospy.get_param('~min_sat', 5)) accept_ratio = float(rospy.get_param('~min_ratio', 1.0)) enu = PoseStamped() enu.header.frame_id = '/map' time_ref_source = rospy.get_param('~time_ref_source', global_frame_id) gpstime = TimeReference() gpstime.source = time_ref_source trans_corr = (0.0, 0.0, 0.0) rtk = rtk() try: sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) sock.connect((host, port)) file = sock.makefile() while not rospy.is_shutdown(): data = file.readline() time_now = rospy.get_rostime() fields = re.split('\s+', data) if fields[0] == '%': continue assert len(fields) is 16
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 = GPS() 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 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] if data['fix_valid']: self.speed = data['speed'] if not math.isnan(data['true_course']): self.ground_course = data['true_course'] 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.fix = True current_fix.NumSat = data['num_satellites'] # 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.covariance = 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'] current_fix.altitude = altitude current_fix.speed = self.speed current_fix.ground_course = self.ground_course 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: current_fix.fix = True current_fix.NumSat = data['num_satellites'] # 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 if data['fix_valid']: current_fix.speed = data['speed'] if not math.isnan(data['true_course']): current_fix.ground_course = data['true_course'] else: current_fix.ground_course = self.ground_course 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) else: return False
def ais_listener(logdir=None): rospy.init_node('ais') position_pub = rospy.Publisher('project11/ais/position',NavSatFix,queue_size=10) timeref_pub = rospy.Publisher('project11/ais/time_reference',TimeReference,queue_size=10) orientation_pub = rospy.Publisher('project11/ais/orientation', Imu, queue_size=10) velocity_pub = rospy.Publisher('project11/ais/velocity', TwistWithCovarianceStamped, queue_size=10) ais_pub = rospy.Publisher('project11/ais/contact',Contact,queue_size=10) ais_raw_pub = rospy.Publisher('project11/ais/raw',Heartbeat,queue_size=10) input_type = rospy.get_param('~input_type') input_address = rospy.get_param('~input','') input_speed = rospy.get_param('~input_speed',0) input_port = int(rospy.get_param('~input_port',0)) output_port = int(rospy.get_param('~output',0)) output_address = rospy.get_param('~output_address','<broadcast>') frame_id = rospy.get_param("~frame_id",'ais') ais_decoder = ais.decoder.AISDecoder() if logdir is not None: logfile = open(logdir+'ais_'+'.'.join(datetime.datetime.utcnow().isoformat().split(':'))+'.log','w') else: logfile = None if input_type == 'serial': serial_in = serial.Serial(input_address, int(input_speed)) else: udp_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) udp_in.settimeout(0.1) udp_in.bind(('',input_port)) if output_port > 0: udp_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) udp_out.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) else: udp_out = None while not rospy.is_shutdown(): if input_type == 'serial': nmea_ins = (serial_in.readline(),) #print( nmea_ins) if udp_out is not None: udp_out.sendto(nmea_in, (output_address,output_port)) else: try: nmea_in,addr = udp_in.recvfrom(2048) #print addr, nmea_in nmea_ins = nmea_in.decode('utf-8').split('\n') except socket.timeout: nmea_ins = None now = rospy.get_rostime() if nmea_ins is not None: for nmea_in_b in nmea_ins: try: nmea_in = nmea_in_b.decode('utf-8') except AttributeError: nmea_in = nmea_in_b #print(nmea_in) if logfile is not None: logfile.write(datetime.datetime.utcnow().isoformat()+','+nmea_in+'\n') if nmea_in.startswith('!AIVDM'): #print(nmea_in) ais_decoder.addNMEA(nmea_in.strip()) msgs = ais_decoder.popMessages() #print(msgs) for m in msgs: if m['type'] in (1,2,3,18,19): #position reports c = Contact() c.header.stamp = now c.header.frame_id = frame_id c.mmsi = m['mmsi'] if 'shipname' in ais_decoder.mmsi_db[m['mmsi']]: c.name = ais_decoder.mmsi_db[m['mmsi']]['shipname'] if 'callsign' in ais_decoder.mmsi_db[m['mmsi']]: c.callsign = ais_decoder.mmsi_db[m['mmsi']]['callsign'] c.position.latitude = m['lat'] c.position.longitude = m['lon'] if m['course'] is not None: c.cog = math.radians(m['course']) else: c.cog = -1 if m['speed'] is not None: c.sog = m['speed']*0.514444 #knots to m/s else: c.sog = -1 if m['heading'] is not None: c.heading = math.radians(m['heading']) else: c.heading = -1 if 'to_bow' in ais_decoder.mmsi_db[m['mmsi']]: c.dimension_to_bow = ais_decoder.mmsi_db[m['mmsi']]['to_bow'] if 'to_stern' in ais_decoder.mmsi_db[m['mmsi']]: c.dimension_to_stern = ais_decoder.mmsi_db[m['mmsi']]['to_stern'] if 'to_port' in ais_decoder.mmsi_db[m['mmsi']]: c.dimension_to_port = ais_decoder.mmsi_db[m['mmsi']]['to_port'] if 'to_starboard' in ais_decoder.mmsi_db[m['mmsi']]: c.dimension_to_stbd = ais_decoder.mmsi_db[m['mmsi']]['to_starboard'] ais_pub.publish(c) raw = Heartbeat() for k,v in m.items(): raw.values.append(KeyValue(k,str(v))) ais_raw_pub.publish(raw) else: nmea_parts = nmea_in.strip().split(',') if len(nmea_parts): #print nmea_parts try: if nmea_parts[0][3:] == 'ZDA' and len(nmea_parts) >= 5: tref = TimeReference() tref.header.stamp = now tref.header.frame_id = frame_id hour = int(nmea_parts[1][0:2]) minute = int(nmea_parts[1][2:4]) second = int(nmea_parts[1][4:6]) ms = int(float(nmea_parts[1][6:])*1000000) day = int(nmea_parts[2]) month = int(nmea_parts[3]) year = int(nmea_parts[4]) zda = datetime.datetime(year,month,day,hour,minute,second,ms) tref.time_ref = rospy.Time(calendar.timegm(zda.timetuple()),zda.microsecond*1000) tref.source = 'ais' timeref_pub.publish(tref) if nmea_parts[0][3:] == 'GGA' and len(nmea_parts) >= 10: latitude = int(nmea_parts[2][0:2])+float(nmea_parts[2][2:])/60.0 if nmea_parts[3] == 'S': latitude = -latitude longitude = int(nmea_parts[4][0:3])+float(nmea_parts[4][3:])/60.0 if nmea_parts[5] == 'W': longitude = -longitude altitude = float(nmea_parts[9]) nsf = NavSatFix() nsf.header.stamp = now nsf.header.frame_id = frame_id nsf.latitude = latitude nsf.longitude = longitude nsf.altitude = altitude position_pub.publish(nsf) if nmea_parts[0][3:] == 'HDT' and len(nmea_parts) >= 2: heading = float(nmea_parts[1]) imu = Imu() imu.header.stamp = now imu.header.frame_id = frame_id q = tf.transformations.quaternion_from_euler(math.radians(90.0-heading), 0, 0, 'rzyx') imu.orientation.x = q[0] imu.orientation.y = q[1] imu.orientation.z = q[2] imu.orientation.w = q[3] orientation_pub.publish(imu) except ValueError: 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 = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False rospy.logdebug(parsed_sentence) 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 self.extend_fix.header.stamp = current_time self.extend_fix.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: current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0] current_fix.status.status = gps_qual[1] current_fix.position_covariance_type = gps_qual[2] if gps_qual > 0: self.valid_fix = True else: self.valid_fix = False 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 # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with # epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2 current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev)**2 # FIXME self.fix_pub.publish(current_fix) # set extend fix self.extend_fix.status.header.stamp = current_time self.extend_fix.status.header.frame_id = frame_id self.extend_fix.status.status = gps_qual[1] self.extend_fix.status.satellites_used = data['num_satellites'] self.extend_fix.status.motion_source = GPSStatus.SOURCE_GPS self.extend_fix.status.orientation_source = GPSStatus.SOURCE_GPS self.extend_fix.status.position_source = GPSStatus.SOURCE_GPS self.extend_fix.latitude = current_fix.latitude self.extend_fix.longitude = current_fix.longitude self.extend_fix.altitude = current_fix.altitude self.extend_fix.position_covariance = current_fix.position_covariance self.position_covariance_type = current_fix.position_covariance_type if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.last_valid_fix_time = current_time_ref self.time_ref_pub.publish(current_time_ref) self.extend_fix.time = current_time_ref.time_ref.to_sec() elif not self.use_RMC and 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] # Only report VTG data when you've received a valid GGA fix as # well. if self.valid_fix: 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) self.extend_fix.track = data['true_course'] self.extend_fix.speed = data['speed'] self.extend_fix_pub.publish(self.extend_fix) 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']) self.vel_pub.publish(current_vel) self.extend_fix.track = data['true_course'] elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] elif 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading']: current_heading = QuaternionStamped() current_heading.header.stamp = current_time current_heading.header.frame_id = frame_id q = quaternion_from_euler(0, 0, math.radians(data['heading'])) current_heading.quaternion.x = q[0] current_heading.quaternion.y = q[1] current_heading.quaternion.z = q[2] current_heading.quaternion.w = q[3] self.heading_pub.publish(current_heading) elif 'GSA' in parsed_sentence: data = parsed_sentence['GSA'] self.star_use_gps = [ data['sate_id1'], data['sate_id2'], data['sate_id3'], data['sate_id4'], data['sate_id5'], data['sate_id6'], data['sate_id7'], data['sate_id8'], data['sate_id9'], data['sate_id10'], data['sate_id11'], data['sate_id12'] ] self.star_use_gps = filter(lambda star: star != 0, self.star_use_gps) self.extend_fix.pdop = data['pdop'] self.extend_fix.hdop = data['hdop'] self.extend_fix.vdop = data['vdop'] elif 'BDGSA' in parsed_sentence: data = parsed_sentence['BDGSA'] self.star_use_bd = [ data['sate_id1'], data['sate_id2'], data['sate_id3'], data['sate_id4'], data['sate_id5'], data['sate_id6'], data['sate_id7'], data['sate_id8'], data['sate_id9'], data['sate_id10'], data['sate_id11'], data['sate_id12'] ] self.star_use_bd = filter(lambda star: star != 0, self.star_use_bd) self.extend_fix.pdop = data['pdop'] self.extend_fix.hdop = data['hdop'] self.extend_fix.vdop = data['vdop'] self.extend_fix.status.satellite_used_prn = self.star_use_gps + self.star_use_bd elif 'GSV' in parsed_sentence: data = parsed_sentence['GSV'] if data['index'] == 1: self.star_map_gps = [] self.star_map_gps.append({ 'id': data['id_satellites1'], 'elevation': data['elevation_satellites1'], 'azimuth': data['azimuth_satellites1'], 'snr': data['snr1'] }) self.star_map_gps.append({ 'id': data['id_satellites2'], 'elevation': data['elevation_satellites2'], 'azimuth': data['azimuth_satellites2'], 'snr': data['snr2'] }) self.star_map_gps.append({ 'id': data['id_satellites3'], 'elevation': data['elevation_satellites3'], 'azimuth': data['azimuth_satellites3'], 'snr': data['snr3'] }) self.star_map_gps.append({ 'id': data['id_satellites4'], 'elevation': data['elevation_satellites4'], 'azimuth': data['azimuth_satellites4'], 'snr': data['snr4'] }) self.star_map_gps = filter(lambda star: star['id'] != 0, self.star_map_gps) elif 'BDGSV' in parsed_sentence: data = parsed_sentence['BDGSV'] if data['index'] == 1: self.star_map_bd = [] self.star_map_bd.append({ 'id': data['id_satellites1'], 'elevation': data['elevation_satellites1'], 'azimuth': data['azimuth_satellites1'], 'snr': data['snr1'] }) self.star_map_bd.append({ 'id': data['id_satellites2'], 'elevation': data['elevation_satellites2'], 'azimuth': data['azimuth_satellites2'], 'snr': data['snr2'] }) self.star_map_bd.append({ 'id': data['id_satellites3'], 'elevation': data['elevation_satellites3'], 'azimuth': data['azimuth_satellites3'], 'snr': data['snr3'] }) self.star_map_bd.append({ 'id': data['id_satellites4'], 'elevation': data['elevation_satellites4'], 'azimuth': data['azimuth_satellites4'], 'snr': data['snr4'] }) self.star_map_bd = filter(lambda star: star['id'] != 0, self.star_map_bd) self.star_map = self.star_map_gps + self.star_map_bd if data['length'] == data['index']: self.extend_fix.status.satellites_visible = len(self.star_map) self.extend_fix.status.satellite_visible_prn = [ star['id'] for star in self.star_map ] self.extend_fix.status.satellite_visible_snr = [ star['snr'] for star in self.star_map ] self.extend_fix.status.satellite_visible_azimuth = [ star['azimuth'] for star in self.star_map ] self.extend_fix.status.satellite_visible_z = [ star['elevation'] for star in self.star_map ] else: return False
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 gpsfix = self.gpsfix gpsfix.header.stamp = current_time gpsfix.header.frame_id = 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 elif gps_qual == 9: # Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9 # http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/ current_fix.status.status = NavSatStatus.STATUS_SBAS_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'] 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) gpsfix.hdop = data['hdop'] gpsfix.time = data['utc_time'] 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']) self.vel_pub.publish(current_vel) gpsfix.speed = data['speed'] gpsfix.track = math.degrees(data['true_course']) elif 'GSV' in parsed_sentence: data = parsed_sentence['GSV'] msg = Gpgsv() msg.header.stamp = rospy.Time.now() msg.n_msgs = data['n_msgs'] msg.msg_number = data['msg_number'] msg.n_satellites = data['n_satellites'] gpsfix.status.satellites_visible = msg.n_satellites if data['msg_number'] == 1: gpsfix.status.satellite_visible_prn = [] gpsfix.status.satellite_visible_z = [] gpsfix.status.satellite_visible_azimuth = [] gpsfix.status.satellite_visible_snr = [] for i in range(0, 4): try: sat = GpgsvSatellite(data['prn%d' % i], data['elevation%d' % i], data['azimuth%d' % i], data['snr%d' % i]) msg.satellites.append(sat) gpsfix.status.satellite_visible_prn.append(data['prn%d' % i]) gpsfix.status.satellite_visible_z.append( data['elevation%d' % i]) gpsfix.status.satellite_visible_azimuth.append( data['azimuth%d' % i]) gpsfix.status.satellite_visible_snr.append(data['snr%d' % i]) except: pass self.sat_pub.publish(msg) elif 'GSA' in parsed_sentence: data = parsed_sentence['GSA'] gpsfix.pdop = data['pdop'] gpsfix.hdop = data['hdop'] gpsfix.vdop = data['vdop'] gpsfix.status.satellites_used = len(data['prns']) gpsfix.status.satellite_used_prn = data['prns'] else: return False if ('RMC' in parsed_sentence and self.use_RMC) or ('GGA' in parsed_sentence and not self.use_RMC): gpsfix.latitude = current_fix.latitude gpsfix.longitude = current_fix.longitude gpsfix.altitude = current_fix.altitude gpsfix.status.status = current_fix.status.status gpsfix.status.position_source = gpsfix.status.orientation_source = gpsfix.status.motion_source = current_fix.status.service self.gpsfix_pub.publish(gpsfix)
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. Sentence was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.seq = self.seq self.seq += 1 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: current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0]; current_fix.status.status = gps_qual[1] current_fix.position_covariance_type = gps_qual[2]; 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 # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2 current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev) ** 2 # FIXME self.fix_pub.publish(current_fix) # Publish GPGGA msgs gpgga_msg = Gpgga() gpgga_msg.header.stamp = current_time gpgga_msg.header.frame_id = frame_id gpgga_msg.message_id = data['sent_id'] gpgga_msg.utc_seconds = data['utc_time'] gpgga_msg.lat = latitude gpgga_msg.lon = longitude gpgga_msg.lat_dir = data['latitude_direction'] gpgga_msg.lon_dir = data['longitude_direction'] gpgga_msg.gps_qual = data['fix_type'] gpgga_msg.num_sats = data['num_satellites'] gpgga_msg.hdop = hdop gpgga_msg.alt = data['altitude'] gpgga_msg.altitude_units = data['altitude_units'] gpgga_msg.undulation = data['mean_sea_level'] gpgga_msg.undulation_units = data['mean_sea_level_units'] gpgga_msg.diff_age = data['diff_age']*10 gpgga_msg.station_id = data['station_id'] self.gpgga_pub.publish(gpgga_msg) 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']) self.vel_pub.publish(current_vel) elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] else: return False
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. Sentence 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_pose_utm = GpsLocal() current_pose_utm.header.stamp = current_time current_pose_utm.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: self.seq = self.seq + 1 current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] gps_qual = data['fix_type'] try: # Unpack the fix params for this quality value current_fix.status.status, self.default_epe, self.fix_type = self.fix_mappings[ gps_qual] except KeyError: current_fix.status.status = NavSatStatus.STATUS_NO_FIX self.fix_type = 'Unknown' if gps_qual == 0: current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self.using_receiver_epe = False self.invalid_cnt += 1 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 # use default epe std_dev unless we've received a GST sentence with epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = self.default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = self.default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = self.default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2 current_fix.position_covariance[8] = (hdop * self.alt_std_dev)**2 # FIXME # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not math.isnan(latitude) and not math.isnan(longitude) and ( gps_qual == 5 or gps_qual == 4): UTMNorthing, UTMEasting = LLtoUTM(latitude, longitude)[0:2] current_pose_utm.position.x = UTMEasting current_pose_utm.position.y = UTMNorthing current_pose_utm.position.z = altitude # Pose x/y/z covariance is whatever we decided h & v covariance is. # Here is it the same as for ECEF coordinates current_pose_utm.covariance[0] = (hdop * self.lon_std_dev)**2 current_pose_utm.covariance[4] = (hdop * self.lat_std_dev)**2 current_pose_utm.covariance[8] = (hdop * self.alt_std_dev)**2 current_pose_utm.rtk_fix = True if gps_qual == 4 else False self.local_pub.publish(current_pose_utm) 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) if (self.diag_pub_time < rospy.get_time()): self.diag_pub_time += 1 diag_arr = DiagnosticArray() diag_arr.header.stamp = rospy.get_rostime() diag_arr.header.frame_id = frame_id diag_msg = DiagnosticStatus() diag_msg.name = 'GPS_status' diag_msg.hardware_id = 'GPS' diag_msg.level = DiagnosticStatus.OK diag_msg.message = 'Received GGA Fix' diag_msg.values.append( KeyValue('Sequence number', str(self.seq))) diag_msg.values.append( KeyValue('Invalid fix count', str(self.invalid_cnt))) diag_msg.values.append(KeyValue('Latitude', str(latitude))) diag_msg.values.append(KeyValue('Longitude', str(longitude))) diag_msg.values.append(KeyValue('Altitude', str(altitude))) diag_msg.values.append(KeyValue('GPS quality', str(gps_qual))) diag_msg.values.append(KeyValue('Fix type', self.fix_type)) diag_msg.values.append( KeyValue('Number of satellites', str(data['num_satellites']))) diag_msg.values.append( KeyValue('Receiver providing accuracy', str(self.using_receiver_epe))) diag_msg.values.append(KeyValue('Hdop', str(hdop))) diag_msg.values.append( KeyValue('Latitude std dev', str(hdop * self.lat_std_dev))) diag_msg.values.append( KeyValue('Longitude std dev', str(hdop * self.lon_std_dev))) diag_msg.values.append( KeyValue('Altitude std dev', str(hdop * self.alt_std_dev))) diag_arr.status.append(diag_msg) self.diag_pub.publish(diag_arr) 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']) self.vel_pub.publish(current_vel) elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] else: return False
gpsVelPub = rospy.Publisher('vel',TwistStamped) gpstimePub = rospy.Publisher('time_reference', TimeReference) #Init GPS port GPSport = rospy.get_param('~port','/dev/ttyUSB0') GPSrate = rospy.get_param('~baud',4800) frame_id = rospy.get_param('~frame_id','gps') if frame_id[0] != "/": frame_id = addTFPrefix(frame_id) time_ref_source = rospy.get_param('~time_ref_source', frame_id) useRMC = rospy.get_param('~useRMC', False) #useRMC == True -> generate info from RMC+GSA #useRMC == False -> generate info from GGA navData = NavSatFix() gpsVel = TwistStamped() gpstime = TimeReference() gpstime.source = time_ref_source navData.header.frame_id = frame_id gpsVel.header.frame_id = frame_id GPSLock = False try: GPS = serial.Serial(port=GPSport, baudrate=GPSrate, timeout=2) #Read in GPS while not rospy.is_shutdown(): #read GPS line data = GPS.readline() if not check_checksum(data): rospy.logerr("Received a sentence with an invalid checksum. Sentence was: %s" % data) continue
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 = GPS() 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 "RMC" in parsed_sentence: data = parsed_sentence["RMC"] if data["fix_valid"]: self.speed = data["speed"] if not math.isnan(data["true_course"]): self.ground_course = data["true_course"] 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.fix = True current_fix.NumSat = 4 # 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.covariance = 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"] current_fix.altitude = altitude current_fix.speed = self.speed current_fix.ground_course = self.ground_course 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: current_fix.fix = True current_fix.NumSat = 4 # 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 if data["fix_valid"]: current_fix.speed = data["speed"] if not math.isnan(data["true_course"]): current_fix.ground_course = data["true_course"] else: current_fix.ground_course = self.ground_course 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) else: return False
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 None parsed_sentence = enway_reach_rs_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return None 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 if self.covarianceMatrix and isinstance( self.covarianceMatrix, list) and len( self.covarianceMatrix) == 9: for i in range(9): current_fix.position_covariance[i] = self.covarianceMatrix[ i] current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_KNOWN else: 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'] 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) return current_fix 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']) self.vel_pub.publish(current_vel) return current_fix else: return None
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" % 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. Sentence 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_heading = Imu() current_heading.header.stamp = current_time current_heading.header.frame_id = 'base_footprint' current_direction = String() # For testing 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 # Add capture/publishing heading info if not self.use_RMC and 'HDT' in parsed_sentence: #rospy.loginfo("HDT!") data = parsed_sentence['HDT'] tempHeading = data['true_heading'] ccHeading = (2 * math.pi) - tempHeading q = tf.transformations.quaternion_from_euler(0,0,ccHeading) current_heading.orientation.x = q[0] current_heading.orientation.y = q[1] current_heading.orientation.z = q[2] current_heading.orientation.w = q[3] #current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) #if (current_heading.data < .3927): current_direction.data = "N" #elif (current_heading.data < 1.178): current_direction.data = "NE" #elif (current_heading.data < 1.9635): current_direction.data = "E" #elif (current_heading.data < 2.74889): current_direction.data = "SE" #elif (current_heading.data < 3.53429): current_direction.data = "S" #elif (current_heading.data < 4.31969): current_direction.data = "SW" #elif (current_heading.data < 5.10509): current_direction.data = "W" #elif (current_heading.data < 5.89048): current_direction.data = "NW" #else: current_direction.data = "N" self.heading_pub.publish(current_heading) #self.direction_pub.publish(current_direction) #self.time_ref_pub.publish(current_time_ref) elif 'GGA' in parsed_sentence: #rospy.loginfo("GGA!") 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'] current_fix.altitude = altitude current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.fix_pub.publish(current_fix) 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 current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.fix_pub.publish(current_fix) 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']) self.vel_pub.publish(current_vel) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): """Public method to provide a new NMEA sentence to the driver. Args: nmea_string (str): NMEA sentence in string form. frame_id (str): TF frame ID of the GPS receiver. timestamp(rospy.Time, optional): Time the sentence was received. If timestamp is not specified, the current time is used. Returns: bool: True if the NMEA string is successfully processed, False if there is an error. """ 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. Sentence 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 if not self.use_GNSS_time: 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: current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] if self.use_GNSS_time: if math.isnan(data['utc_time'][0]): rospy.logwarn("Time in the NMEA sentence is NOT valid") return False current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1]) fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0] current_fix.status.status = gps_qual[1] current_fix.position_covariance_type = gps_qual[2] self.valid_fix = (fix_type > 0) 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 # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with # epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2 current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev)**2 # FIXME self.fix_pub.publish(current_fix) if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time): current_time_ref.time_ref = rospy.Time(data['utc_time'][0], data['utc_time'][1]) self.last_valid_fix_time = current_time_ref self.time_ref_pub.publish(current_time_ref) elif not self.use_RMC and 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] # Only report VTG data when you've received a valid GGA fix as # well. if self.valid_fix: 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) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] if self.use_GNSS_time: if math.isnan(data['utc_time'][0]): rospy.logwarn("Time in the NMEA sentence is NOT valid") return False current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1]) # 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'][0]) or self.use_GNSS_time): current_time_ref.time_ref = rospy.Time( data['utc_time'][0], data['utc_time'][1]) 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']) self.vel_pub.publish(current_vel) elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] elif 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading']: current_heading = QuaternionStamped() current_heading.header.stamp = current_time current_heading.header.frame_id = frame_id q = quaternion_from_euler(0, 0, math.radians(data['heading'])) current_heading.quaternion.x = q[0] current_heading.quaternion.y = q[1] current_heading.quaternion.z = q[2] current_heading.quaternion.w = q[3] self.heading_pub.publish(current_heading) else: return False
def posmv_nmea_listener(): position_pub = rospy.Publisher('/base/position', NavSatFix, queue_size=10) timeref_pub = rospy.Publisher('/base/time_reference', TimeReference, queue_size=10) orientation_pub = rospy.Publisher('/base/orientation', NavEulerStamped, queue_size=10) rospy.init_node('posmv_nmea') input_type = rospy.get_param('/posmv_nmea/input_type') input_address = rospy.get_param('/posmv_nmea/input', '') input_speed = rospy.get_param('/posmv_nmea/input_speed', 0) input_port = int(rospy.get_param('/posmv_nmea/input_port', 0)) output_port = int(rospy.get_param('/posmv_nmea/output', 0)) output_address = rospy.get_param('/posmv_nmea/output_address', '<broadcast>') if input_type == 'serial': serial_in = serial.Serial(input_address, int(input_speed)) else: udp_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) udp_in.bind(('', input_port)) if output_port > 0: udp_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) udp_out.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) else: udp_out = None timestamp = datetime.datetime.utcfromtimestamp( rospy.Time.now().to_time()).isoformat() bag = rosbag.Bag( 'nodes/posmv_nmea_' + ('-'.join(timestamp.split(':'))) + '.bag', 'w', rosbag.Compression.BZ2) while not rospy.is_shutdown(): if input_type == 'serial': nmea_in = serial_in.readline() #print nmea_in if udp_out is not None: udp_out.sendto(nmea_in, (output_address, output_port)) else: nmea_in, addr = udp_in.recvfrom(1024) #print addr, nmea_in now = rospy.get_rostime() nmea_parts = nmea_in.strip().split(',') if len(nmea_parts): #print nmea_parts try: if nmea_parts[0] == '$GPZDA' and len(nmea_parts) >= 5: tref = TimeReference() tref.header.stamp = now hour = int(nmea_parts[1][0:2]) minute = int(nmea_parts[1][2:4]) second = int(nmea_parts[1][4:6]) ms = int(float(nmea_parts[1][6:]) * 1000000) day = int(nmea_parts[2]) month = int(nmea_parts[3]) year = int(nmea_parts[4]) zda = datetime.datetime(year, month, day, hour, minute, second, ms) tref.time_ref = rospy.Time( calendar.timegm(zda.timetuple()), zda.microsecond * 1000) tref.source = 'posmv' timeref_pub.publish(tref) bag.write('/posmv_nmea/time_reference', tref) if nmea_parts[0] == '$GPGGA' and len(nmea_parts) >= 10: latitude = int( nmea_parts[2][0:2]) + float(nmea_parts[2][2:]) / 60.0 if nmea_parts[3] == 'S': latitude = -latitude longitude = int( nmea_parts[4][0:3]) + float(nmea_parts[4][3:]) / 60.0 if nmea_parts[5] == 'W': longitude = -longitude altitude = float(nmea_parts[9]) nsf = NavSatFix() nsf.header.stamp = now nsf.header.frame_id = 'posmv_operator' nsf.latitude = latitude nsf.longitude = longitude nsf.altitude = altitude position_pub.publish(nsf) bag.write('/posmv_nmea/position', nsf) if nmea_parts[0] == '$GPHDT' and len(nmea_parts) >= 2: heading = float(nmea_parts[1]) nes = NavEulerStamped() nes.header.stamp = now nes.header.frame_id = 'posmv_operator' nes.orientation.heading = heading orientation_pub.publish(nes) bag.write('/posmv_nmea/orientation', nes) except ValueError: pass bag.close()
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. Sentence 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: current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0] current_fix.status.status = gps_qual[1] current_fix.position_covariance_type = gps_qual[2] if gps_qual > 0: self.valid_fix = True else: self.valid_fix = False 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 # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with # epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2 current_fix.position_covariance[8] = ( 2 * hdop * self.alt_std_dev) ** 2 # FIXME 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.last_valid_fix_time = current_time_ref self.time_ref_pub.publish(current_time_ref) elif not self.use_RMC and 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] # Only report VTG data when you've received a valid GGA fix as # well. if self.valid_fix: 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) 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']) self.vel_pub.publish(current_vel) elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] elif 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading']: current_heading = QuaternionStamped() current_heading.header.stamp = current_time current_heading.header.frame_id = frame_id q = quaternion_from_euler(0, 0, math.radians(data['heading'])) current_heading.quaternion.x = q[0] current_heading.quaternion.y = q[1] current_heading.quaternion.z = q[2] current_heading.quaternion.w = q[3] self.heading_pub.publish(current_heading) else: return False
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 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 elif gps_qual == 9: # Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9 # http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/ current_fix.status.status = NavSatStatus.STATUS_SBAS_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'] 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_global = Vector3Stamped() current_vel_global.header.stamp = current_time current_vel_global.header.frame_id = frame_id current_vel_global.vector.x = data['speed'] * \ math.sin(data['true_course']) current_vel_global.vector.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel_global) current_vel_local = TwistStamped() current_vel_local.header.stamp = current_time current_vel_local.header.frame_id = frame_id current_vel_local.twist.linear.x = data['speed'] self.vel_pub2.publish(current_vel_local) elif 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading_north']: self.hdt_pub.publish(data['heading_north']) elif 'ROT' in parsed_sentence: data = parsed_sentence['ROT'] current_rot = TwistStamped() current_rot.header.stamp = current_time current_rot.header.frame_id = frame_id current_rot.twist.angular.z = data['rate'] * 3.14 / 180 * 60 self.rot_pub.publish(current_rot) elif 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] current_speed = TwistStamped() current_speed.header.stamp = current_time current_speed.header.frame_id = frame_id current_speed.twist.linear.x = data['speed_kph'] / 360 * 1000 self.speed_pub.publish(current_speed) elif 'AVR' in parsed_sentence: data = parsed_sentence['AVR'] current_pyr = TwistStamped() current_pyr.header.stamp = current_timecur current_pyr.header.frame_id = frame_id current_pyr.twist.angular.z = data['yaw'] current_pyr.twist.angular.y = data['pitch'] current_pyr.twist.angular.x = data['roll'] if data['fix_type'] > 0: self.pyr_pub.publish(current_pyr) else: return False
def gpsCallback(data): gps_reading = marshal.loads(data.data) current_time = rospy.Time.now() frame_id = "gps_link" time_ref_msg = TimeReference() time_ref_msg.header.stamp = current_time time_ref_msg.header.frame_id = frame_id # if 'timestamp' in gps_reading: # timestamp = gps_reading['timestamp'] # timestamp_s = datetime.time( # hour=int(timestamp[0:2]), # minute=int(timestamp[3:5]), # second=int(timestamp[6:8]), # microsecond=int(timestamp[9:])) # time_ref_msg.time_ref = rospy.Time.from_sec(timestamp_s.second) # time_ref_msg.source = "gps_time" # else: # time_ref_msg.source = frame_id time_ref_msg.source = frame_id time_ref_pub.publish(time_ref_msg) nav_msg = NavSatFix() nav_msg.header.stamp = current_time nav_msg.header.frame_id = frame_id gps_qual = gps_reading['qual'] if gps_qual == 1: nav_msg.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: nav_msg.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): nav_msg.status.status = NavSatStatus.STATUS_GBAS_FIX elif gps_qual == 9: nav_msg.status.status = NavSatStatus.STATUS_SBAS_FIX else: nav_msg.status.status = NavSatStatus.STATUS_NO_FIX nav_msg.status.service = NavSatStatus.SERVICE_GPS nav_msg.latitude = gps_reading['latitude'] nav_msg.longitude = gps_reading['longitude'] # nav_msg.altitude = float('NaN') nav_msg.altitude = 0 # EKF Not outputing when using NaN? nav_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN navsatfix_pub.publish(nav_msg) vel_msg = TwistStamped() vel_msg.header.stamp = current_time vel_msg.header.frame_id = frame_id vel_msg.twist.linear.x = gps_reading['speed_over_ground'] * math.sin(gps_reading['course_over_ground']) vel_msg.twist.linear.y = gps_reading['speed_over_ground'] * math.cos(gps_reading['course_over_ground']) vel_pub.publish(vel_msg) marker_msg = Marker() marker_msg.header = nav_msg.header marker_msg.action = 0 # ADD marker_msg.type = 0 # ARROW marker_msg.scale.x = 1 marker_msg.scale.y = 0.1 marker_msg.scale.z = 0.1 marker_msg.color.a = 1.0 marker_msg.color.r = 0.0; marker_msg.color.g = 0.0; marker_msg.color.b = 1.0; marker_msg.pose.position.x = 0 marker_msg.pose.position.y = 0 quat = tf.transformations.quaternion_from_euler(0, 0, 0) marker_msg.pose.orientation.x = quat[0] marker_msg.pose.orientation.y = quat[1] marker_msg.pose.orientation.z = quat[2] marker_msg.pose.orientation.w = quat[3] marker_pub.publish(marker_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