def talker(): print 'in talker' pub = rospy.Publisher('GPS', NavSatFix) rospy.init_node('GPStalker') while not rospy.is_shutdown(): #Assuming that parse will return these values time.sleep(1) parse() msg = NavSatFix() Fix = NavSatStatus() Fix.status = GPS.mode Fix.service = GPS.numSat msg.header.stamp = rospy.Time.now() msg.status = Fix msg.latitude = GPS.lat msg.longitude = GPS.lon msg.altitude = GPS.alt msg.position_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) msg.position_covariance_type = 0 #covariance_type = 0 unknown # = 1 approximated # = 2 diagonal known # = 3 known pub.publish(msg)
def read_rtk_gps(): s = socket.socket() # Create a socket object # these are parameters loaded onto the ros parameter servers host = rospy.get_param('~host', '172.20.10.5') # these are parameters loaded onto the ros parameter servers port = rospy.get_param('~port', 3000) s.connect((host, port)) rospy.init_node('rtk_gps_node') pub_navsatfix = rospy.Publisher("/rtk_fix", NavSatFix, queue_size=0) navsatfix = NavSatFix() while True: GPS_msgs = s.recv(1024) msgs = re.split('\r\n', GPS_msgs) for msg in msgs: if msg[:6] == "$GPGGA": print msg data = msg.split(',') navsatfix.status = NavSatStatus(status=int(data[6])) navsatfix.latitude = float(data[2][:2]) navsatfix.longitude = float(data[4][:3]) navsatfix.altitude = float(data[9]) navsatfix.header.stamp = rospy.Time.now() pub_navsatfix.publish(navsatfix)
def run(self): while True: #Reading acceleration data report = self.session.next() if self.debug: print(report) if report["class"] == "TPV": if "time" in report: gpsMSG = NavSatFix() gpsMSG.latitude = report["lat"] gpsMSG.longitude = report["lon"] try: gpsMSG.status = report["status"] except: pass try: gpsMSG.altitude = report["alt"] except: pass current_time = modf(time.time()) # gpsMSG.header.sec = int(current_time[1]) # gpsMSG.header.nanosec = int(current_time[0] * 1000000000) & 0xffffffff self.gpsPublisher.publish(gpsMSG) else: if self.debug: print("GPS has not locked onto sattelites")
def init_path(): # logitude, latitute, altitute, orientation (+deg) gpsPub = rospy.Publisher("/fix_path_init", NavSatFix, queue_size=10) wpSub = rospy.Subscriber("/gps_path_init", Odometry, utm_cb) navStatus = NavSatStatus() navStatus.service = 1 navStatus.status = 1 # waypoints hardcoded for now, may want to spec with argument in future num_wp = gps_waypoints.shape[0] hdop = 1.5 position_covariance_type = 1 global wp_num for i in range(0, num_wp): # send gps waypoint for conversion header = Header() header.stamp = rospy.Time.now() header.frame_id = 'base_link' msgOut = NavSatFix() msgOut.position_covariance[0] = hdop**2 msgOut.position_covariance[4] = hdop**2 msgOut.position_covariance[8] = (2 * hdop)**2 msgOut.longitude = gps_waypoints[i][0] msgOut.latitude = gps_waypoints[i][1] msgOut.altitude = gps_waypoints[i][2] msgOut.position_covariance_type = position_covariance_type msgOut.status = navStatus msgOut.header = header wp_num = i gpsPub.publish(msgOut)
def parse_gps(self, data): ''' Given raw data from Jaguar GPS socket, parse and return NavSatFix message. If bad/incomplete data, return None ''' # use regex to parse gpgga_hit = re.search('^\$(GPGGA.+?)\r\n', data) gprmc_hit = re.search('^\$(GPRMC.+?)\r\n', data) if gprmc_hit: # Get estimated heading (not part of standard ROS navsatfix message) gprmc = gprmc_hit.group(0).split(",") try: heading = float(gprmc[8]) except: heading = float("NaN") else: while heading < -180.0: heading += 360.0 while heading > 180.0: heading -= 360.0 finally: self.current_est_heading = heading if gpgga_hit: gpgga = gpgga_hit.group(0).split(",") nav_msg = NavSatFix() # Set header information time = gpgga[1] hrs = float(time[0:1]) mins = float(time[2:3]) secs = float(time[4:5]) nav_msg.header.stamp = rospy.Time.from_sec(hrs * 3600 + mins * 60 + secs) nav_msg.header.frame_id = "gps" # Set GPS Status status = NavSatStatus() status.status = -1 if int(gpgga[6]) == 0 else 0 nav_msg.status = status # Set longitude and latitude lat_str = gpgga[2] lon_str = gpgga[4] lat_degs = float(lat_str[:2]) + (float(lat_str[2:]) / 60.0) lon_degs = float(lon_str[:3]) + (float(lon_str[3:]) / 60.0) nav_msg.latitude = -1 * lat_degs if gpgga[3] == "S" else lat_degs nav_msg.longitude = -1 * lon_degs if gpgga[5] == "W" else lon_degs # Set altitude (Positive is above the WGS 84 ellipsoid) try: nav_msg.altitude = float(gpgga[9]) except: nav_msg.altitude = float("NaN") # Set covariance type to unknown nav_msg.position_covariance_type = 0 return nav_msg else: return None
def talker(): pub = rospy.Publisher('/RTK', Point, queue_size=10) rospy.init_node('RUN', anonymous=True) rate = rospy.Rate(100) # 10hz count = 0 #print("ok") global sequence_number global z while not rospy.is_shutdown(): while True: line = str(str(s.readline())[0:]) # print("angle is ok") if line.startswith('$GNGGA'): #print(line) a = line.split(',') RTK = (a[6]) #print(RTK) record_item = [] sequence_number += 1 msg = pynmea2.parse(line) lat = msg.latitude lgi = msg.longitude navsat = NavSatFix() navsat.status = 0 navsat.header.seq = 1 navsat.header.stamp = rospy.Time.now() navsat.position_covariance_type = 2 # navsat.STATUS = 1 navsat.header.frame_id = 'base_link' navsat.latitude = lat navsat.longitude = lgi navsat.altitude = txt() print(navsat) utm = Proj(proj='utm', zone=48, ellps='WGS84') x, y = utm(lgi, lat) utm_point = Point() #msg = geom_msg.Pose() utm_point.x = x utm_point.y = y #utm_point.angular.z = txt() # br = tf.TransformBroadcaster() # br.sendTransform((x, y, 0), # tf.transformations.quaternion_from_euler(0, 0, txt()),rospy.Time.now(),"base_link","odom") #rospy.loginfo(utm_point) pub.publish(utm_point)
def responseCallback(self, msg): ############################################################################# gps_msg = NavSatFix() gps_msg.header = msg.header gps_msg.status = msg.status gps_msg.latitude = msg.latitude gps_msg.longitude = msg.longitude gps_msg.altitude = msg.altitude gps_msg.position_covariance_type = msg.position_covariance_type gps_msg.position_covariance[0] = 1.8 gps_msg.position_covariance[4] = 1.8 gps_msg.position_covariance[8] = 5 self.gpsPub.publish(gps_msg)
def gpsCallback(data): gps = NavSatFix() gps.header.seq = data.header.seq gps.header.stamp = data.header.stamp if (gps.header.stamp.secs % 10 != 4): gps.header.frame_id = "gps3_Link" gps.status = data.status gps.latitude = data.latitude gps.longitude = data.longitude gps.altitude = data.altitude gps.position_covariance = data.position_covariance gps.position_covariance_type = data.position_covariance_type pub = rospy.Publisher('/gps_filtered', NavSatFix, queue_size=10) rate = rospy.Rate(5) # Hz pub.publish(gps) rate.sleep() else: gps.header.frame_id = "false_Link" gps.status = data.status gps.latitude = 0 gps.longitude = 0 gps.altitude = 0 gps.position_covariance = data.position_covariance gps.position_covariance_type = data.position_covariance_type
def publish_gps(event): global origin_lat, origin_lon, first #It may take a second or two to get good data #print gpsd.fix.latitude,', ',gpsd.fix.longitude,' Time: ',gpsd.utc # os.system('clear') # print # print ' GPS reading' # print '----------------------------------------' # print 'latitude ' , gpsd.fix.latitude # print 'longitude ' , gpsd.fix.longitude # print 'time utc ' , gpsd.utc,' + ', gpsd.fix.time # print 'altitude (m)' , gpsd.fix.altitude # print 'epx ' , gpsd.fix.epx # print 'epv ' , gpsd.fix.epv # print 'ept ' , gpsd.fix.ept # print 'speed (m/s) ' , gpsd.fix.speed # print 'climb ' , gpsd.fix.climb # print 'track ' , gpsd.fix.track # print 'mode ' , gpsd.fix.mode # print # print 'sats ' , gpsd.satellites navsat = NavSatFix() header = Header() header.stamp = rospy.Time.now() header.frame_id = "base_link" navsat.header = header navsat_status = NavSatStatus() navsat_status.status = 0 navsat_status.service = 1 navsat.status = navsat_status navsat.latitude = gpsd.fix.latitude navsat.longitude = gpsd.fix.longitude navsat.altitude = gpsd.fix.altitude navsat.position_covariance_type = 2 navsat.position_covariance = [2.5, 0, 0, 0, 2.5, 0, 0, 0, 2.5] if not origin_lat and not origin_lon: origin_lat = gpsd.fix.latitude origin_lon = gpsd.fix.longitude # print ('Odometry: ') # print (x, y) pub_navsat.publish(navsat)
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 cmdCB2(self, data): # Serial read & publish try: msg = NavSatFix() msg.header.seq = data.header.seq msg.header.stamp = data.header.stamp msg.header.frame_id = data.header.frame_id msg.status = data.status msg.latitude = data.latitude msg.longitude = data.longitude msg.altitude = data.altitude for i in range(9): msg.position_covariance[i] = data.position_covariance[i] * 10 msg.position_covariance_type = data.position_covariance_type #rospy.logerr(msg) self.pub2.publish(msg) except: pass
def responseCallback(self, msg): ############################################################################# gps_msg = NavSatFix() gps_msg.header = msg.header gps_msg.status = msg.status gps_msg.latitude = msg.latitude gps_msg.longitude = msg.longitude gps_msg.altitude = msg.altitude gps_msg.position_covariance_type = msg.position_covariance_type if msg.status.status == 0: # no rtk gps_msg.position_covariance[0] = 80 gps_msg.position_covariance[4] = 80 gps_msg.position_covariance[8] = 150 else: gps_msg.position_covariance[0] = 0.1 gps_msg.position_covariance[4] = 0.1 gps_msg.position_covariance[8] = 0.25 self.gpsPub.publish(gps_msg)
def update_uav_home_pos(self, event): """ Update UAV home position when ASV moves """ for uav in self._uav_home_proxies.keys(): home_wp = np.array([ self.uav_home_wp[uav].geo.latitude, self.uav_home_wp[uav].geo.longitude, self.uav_home_wp[uav].geo.altitude, self.heading ]) if (self._uav_home_offset[uav] == np.ones(4) * float('inf')).all(): if self.global_pose != NavSatFix(): asv_home = np.array([ self.global_pose.latitude, self.global_pose.longitude, self.global_pose.altitude, 0.0 ]) self._uav_home_offset[uav] = home_wp - asv_home continue # Update the launchpad position relative to last known pos heading = (self.heading - self._uav_home_offset[uav][-1]) launchpad_lat = self.global_pose.latitude + ( -1 * self._uav_home_offset[uav][1] * np.sin(heading) + self._uav_home_offset[uav][0] * np.cos(heading)) launchpad_long = self.global_pose.longitude + ( self._uav_home_offset[uav][1] * np.cos(heading) + self._uav_home_offset[uav][0] * np.sin(heading)) launchpad_alt = self.global_pose.altitude + self._uav_home_offset[ uav][2] # Publish the launchpad position and its (asv) heading launchpad_pose = NavSatFix() launchpad_pose.header = self.global_pose.header launchpad_pose.status = self.global_pose.status launchpad_pose.latitude = launchpad_lat launchpad_pose.longitude = launchpad_long launchpad_pose.altitude = launchpad_alt self._uav_home_pose_pub[uav].publish(launchpad_pose) self._uav_home_heading_pub[uav].publish( Float64(self.heading * 180. / np.pi)) launchpad = np.array([launchpad_lat, launchpad_long]) if np.linalg.norm(home_wp[:2] - launchpad) > 3e-06: self._uav_home_proxies[uav](False, self.heading * 180. / np.pi, launchpad_lat, launchpad_long, self._uav_home_offset[uav][2])
def publish(self): header = Header() header.stamp = rospy.Time.now() navStatus = NavSatStatus() navStatus.status = (self.fixStatus - 1) navStatus.service = (0b1111) gpsMsg = NavSatFix() gpsMsg.header = header gpsMsg.status = navStatus gpsMsg.latitude = self.lat gpsMsg.longitude = self.long gpsMsg.altitude = self.alt gpsMsg.position_covariance = self.covariance gpsMsg.position_covariance_type = self.covariance_type self.navSatPub.publish(gpsMsg) self.headingPub.publish(self.heading)
def talker(): ser = serial.Serial("/dev/ttyTHS0", 9600, timeout=1) sio = io.TextIOWrapper(io.BufferedReader(ser)) pub = rospy.Publisher("gps_data", NavSatFix, queue_size=1) rospy.init_node("gps") msg = NavSatFix() msg.status = NavSatStatus(0, 0x1 | 0x2) # Unaugmented fix, GPS & GLONASS msg.header.frame_id = "gps_link" rospy.loginfo("GPS node ready!") while not rospy.is_shutdown(): try: line = sio.readline() if not line: continue gpsmsg = pynmea2.parse(line) if type(gpsmsg) == pynmea2.GGA: qual = gpsmsg.gps_qual if qual > 0: # Valid gps fix? msg.status.status = (qual - 1 if qual <= 2 else 2 ) # See sensor_msgs/NavSatStatus msg.latitude = gpsmsg.latitude msg.longitude = gpsmsg.longitude msg.altitude = gpsmsg.altitude except serial.SerialException as e: rospy.logerr("Device error: {}".format(e)) break except pynmea2.ParseError as e: rospy.logerr("Parse error: {}".format(e)) continue except UnicodeDecodeError: continue msg.header.stamp = rospy.Time.now() pub.publish(msg) rospy.sleep(0.01)
def write_rtk_gps(timestamp_ns, T_UTM_S, utm_zone, lat_deg, lon_deg, alt_m, status, bag): ros_timestamp = nanoseconds_to_ros_timestamp(timestamp_ns) ros_status = NavSatStatus() if status == 'INS_SOLUTION_GOOD': ros_status.status = NavSatStatus.STATUS_FIX else: ros_status.status = NavSatStatus.STATUS_FIX ros_status.service = NavSatStatus.SERVICE_GPS fix = NavSatFix() fix.status = ros_status fix.latitude = lat_deg fix.longitude = lon_deg fix.altitude = alt_m fix.header.frame_id = "WGS" fix.header.stamp = ros_timestamp rospose = Odometry() rospose.child_frame_id = "S" rospose.header.frame_id = "UTM" rospose.header.stamp = ros_timestamp rospose.pose.pose.position.x = T_UTM_S.getPosition()[0] rospose.pose.pose.position.y = T_UTM_S.getPosition()[1] rospose.pose.pose.position.z = T_UTM_S.getPosition()[2] rospose.pose.pose.orientation.x = T_UTM_S.getRotation().x() rospose.pose.pose.orientation.y = T_UTM_S.getRotation().y() rospose.pose.pose.orientation.z = T_UTM_S.getRotation().z() rospose.pose.pose.orientation.w = T_UTM_S.getRotation().w() rospose.twist.twist.linear.x = 0.0 rospose.twist.twist.linear.y = 0.0 rospose.twist.twist.linear.z = 0.0 rospose.twist.twist.angular.x = 0.0 rospose.twist.twist.angular.y = 0.0 rospose.twist.twist.angular.z = 0.0 bag.write('/gps_rtk', rospose, t=ros_timestamp) bag.write('/gps_rtk_fix', fix, t=ros_timestamp)
def write_gps(gps, i, bag): utime = gps[i, 0] mode = gps[i, 1] lat = gps[i, 3] lng = gps[i, 4] alt = gps[i, 5] timestamp = microseconds_to_ros_timestamp(utime) status = NavSatStatus() if mode == 0 or mode == 1: status.status = NavSatStatus.STATUS_NO_FIX else: status.status = NavSatStatus.STATUS_FIX status.service = NavSatStatus.SERVICE_GPS num_sats = UInt16() num_sats.data = gps[i, 2] fix = NavSatFix() fix.status = status fix.latitude = np.rad2deg(lat) fix.longitude = np.rad2deg(lng) fix.altitude = alt track = Float64() track.data = gps[i, 6] speed = Float64() speed.data = gps[i, 7] bag.write('/gps_fix', fix, t=timestamp) bag.write('/gps_track', track, t=timestamp) bag.write('/gps_speed', speed, t=timestamp)
def talker(): # Subscribe to Vicon messages viconTopic = rospy.get_param('topic') rospy.Subscriber(viconTopic, TransformStamped, callback) # Start a publisher for the GPS messages pub = rospy.Publisher('GPS/position', NavSatFix) # FIXME # Start the node rospy.init_node('talker') # Populate the NavSatFix message from the parameter server statusMsg = NavSatStatus() statusMsg.status = rospy.get_param('status', -1) statusMsg.service = rospy.get_param('service', 1) fixMsg = NavSatFix() fixMsg.header.stamp = rospy.Time.now() fixMsg.header.frame_id = "/world" fixMsg.status = statusMsg fixMsg.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] #position could be modified with some gaussian noise added to it and then calculate the covariance matrix. fixMsg.position_covariance_type = rospy.get_param('position_covariance_type', 0); while not rospy.is_shutdown(): [fixMsg.longitude, fixMsg.latitude, fixMsg.altitude] = c.xyz2gps([parentFrameLong, parentFrameLat, parentFrameAlt], latestViconMsg.transform.translation.x, latestViconMsg.transform.translation.y, latestViconMsg.transform.translation.z, parentFrameAngle) statusMsg.status = rospy.get_param('status', statusMsg.status) statusMsg.service = rospy.get_param('service', statusMsg.service) # put the sigma and calculate the cov matrix here #rospy.loginfo([fixMsg.longitude, fixMsg.latitude, fixMsg.altitude]) pub.publish(fixMsg) rospy.sleep(0.1)
def write_gps(gps, i, bag): utime = gps[i, 0] mode = gps[i, 1] lat = gps[i, 3] lng = gps[i, 4] alt = gps[i, 5] timestamp = rospy.Time.from_sec(utime / 1e6) status = NavSatStatus() if mode == 0 or mode == 1: status.status = NavSatStatus.STATUS_NO_FIX else: status.status = NavSatStatus.STATUS_FIX status.service = NavSatStatus.SERVICE_GPS num_sats = UInt16() num_sats.data = gps[i, 2] fix = NavSatFix() fix.status = status fix.latitude = np.rad2deg(lat) fix.longitude = np.rad2deg(lng) fix.altitude = alt track = Float64() track.data = gps[i, 6] speed = Float64() speed.data = gps[i, 7] bag.write('gps_fix', fix, t=timestamp) bag.write('gps_track', track, t=timestamp) bag.write('gps_speed', speed, t=timestamp) # print("package image...") # img_path = sys.argv[1] + 'images/2012-01-08/lb3/Cam0/' # img_list = os.listdir(img_path) # i_img = 0 # for img_name in img_list: # i_img = i_img + 1 # print(i_img) # img_cv = cv2.imread(os.path.join(img_path, img_name), -1) # br = CvBridge() # img = Image() # img = br.cv2_to_imgmsg(img_cv, "bgr8") # img.header.seq = i_img # img.header.frame_id = 'camImage' # bag.write('/camera/image', img) img_path = sys.argv[1] + 'images/2012-01-08/lb3/Cam0/' img_list = os.listdir(img_path) img_cv = cv2.imread(os.path.join(img_path, img_list[i]), -1) img_cv = cv2.resize(img_cv, IMAGEDIM, interpolation=cv2.INTER_AREA) br = CvBridge() img = Image() img = br.cv2_to_imgmsg(img_cv, "bgr8")
def gps_to_navsat(self, gps_list, i): """ converts gps data to ROS NavSatFix messages :param gps_list: list, containing gps data :param i: row counter :return: fill bag with navsat, track, speed, timestamp """ # load data from list utime = gps_list[i, 0] mode = gps_list[i, 1] num_satss = gps_list[i, 2] lat = gps_list[i, 3] lng = gps_list[i, 4] alt = gps_list[i, 5] track_raw = gps_list[i, 6] speed_raw = gps_list[i, 7] # create ros timestamp timestamp = rospy.Time.from_sec(utime / 1e6) # get gps and base link gps_link = self.gps_frame base_link = self.body_frame # fill NavSat message status = NavSatStatus() if mode == 0 or mode == 1: status.status = NavSatStatus.STATUS_NO_FIX else: status.status = NavSatStatus.STATUS_FIX status.service = NavSatStatus.SERVICE_GPS num_sats = UInt16() num_sats.data = num_satss navsat = NavSatFix() navsat.header.stamp = timestamp navsat.header.frame_id = gps_link navsat.status = status navsat.latitude = np.rad2deg(lat) navsat.longitude = np.rad2deg(lng) navsat.altitude = alt navsat.position_covariance = NAVSAT_COVAR navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED track = Float64() track.data = track_raw speed = Float64() speed.data = speed_raw # create base_link gps_link static transformer gps_static_transform_stamped = geometry_msgs.msg.TransformStamped() gps_static_transform_stamped.header.stamp = timestamp gps_static_transform_stamped.header.frame_id = base_link gps_static_transform_stamped.child_frame_id = gps_link gps_static_transform_stamped.transform.translation.x = 0 gps_static_transform_stamped.transform.translation.y = 0.25 gps_static_transform_stamped.transform.translation.z = 0.51 quat = tf.transformations.quaternion_from_euler(0, 0, 0) gps_static_transform_stamped.transform.rotation.x = quat[0] gps_static_transform_stamped.transform.rotation.y = quat[1] gps_static_transform_stamped.transform.rotation.z = quat[2] gps_static_transform_stamped.transform.rotation.w = quat[3] # publish static transform tf_static_msg = tf2_msgs.msg.TFMessage([gps_static_transform_stamped]) return navsat, track, speed, timestamp, tf_static_msg
def navigation_handler(self, data): """ Rebroadcasts navigation data in the following formats: 1) /odom => /base footprint transform (if enabled, as per REP 105) 2) Odometry message, with parent/child IDs as in #1 3) NavSatFix message, for systems which are knowledgeable about GPS stuff 4) IMU messages """ rospy.logdebug("Navigation received") # If we don't have a fix, don't publish anything... if self.nav_status.status == NavSatStatus.STATUS_NO_FIX: return # UTM conversion easting, northing = data.bestxyz.easting, data.bestxyz.northing # Initialize starting point if we haven't yet # TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid? # TODO: check INSPVAX sol stat for valid position before accepting if not self.init and self.zero_start: self.origin.x = easting self.origin.y = northing self.init = True # Publish origin reference for others to know about p = Pose() p.position.x = self.origin.x p.position.y = self.origin.y self.pub_origin.publish(p) # IMU # TODO: Work out these covariances properly. Logs provide covariances in local frame, not body imu = Imu() imu.header.stamp == rospy.Time.now() imu.header.frame_id = self.base_frame # Orientation # orient=PyKDL.Rotation.RPY(RAD(data.roll),RAD(data.pitch),RAD(data.heading)).GetQuaternion() # imu.orientation = Quaternion(*orient) imu.orientation.x = data.inspvax.pitch imu.orientation.y = data.inspvax.roll imu.orientation.z = data.inspvax.azimuth imu.orientation.w = 0 IMU_ORIENT_COVAR[0] = POW(2, data.inspvax.pitch_std) IMU_ORIENT_COVAR[4] = POW(2, data.inspvax.roll_std) IMU_ORIENT_COVAR[8] = POW(2, data.inspvax.azimuth_std) imu.orientation_covariance = IMU_ORIENT_COVAR # Angular rates (rad/s) # corrimudata log provides instantaneous rates so multiply by IMU rate in Hz imu.angular_velocity.x = data.corrimudata.pitch_rate * self.imu_rate imu.angular_velocity.y = data.corrimudata.roll_rate * self.imu_rate imu.angular_velocity.z = data.corrimudata.yaw_rate * self.imu_rate imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration (m/s^2) imu.linear_acceleration.x = data.corrimudata.x_accel * self.imu_rate imu.linear_acceleration.y = data.corrimudata.y_accel * self.imu_rate imu.linear_acceleration.z = data.corrimudata.z_accel * self.imu_rate imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu) # Odometry # TODO: Work out these covariances properly odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = self.odom_frame odom.child_frame_id = self.base_frame odom.pose.pose.position.x = easting - self.origin.x odom.pose.pose.position.y = northing - self.origin.y odom.pose.pose.position.z = data.inspvax.altitude #odom.pose.pose.orientation = Quaternion(*orient) odom.pose.pose.orientation = imu.orientation POSE_COVAR[21] = IMU_ORIENT_COVAR[0] POSE_COVAR[28] = IMU_ORIENT_COVAR[4] POSE_COVAR[35] = IMU_ORIENT_COVAR[8] odom.pose.covariance = POSE_COVAR # Twist is relative to vehicle frame odom.twist.twist.linear.x = data.bestxyz.velx odom.twist.twist.linear.y = data.bestxyz.vely odom.twist.twist.linear.z = data.bestxyz.velz TWIST_COVAR[0] = pow(2,data.bestxyz.velx_std) TWIST_COVAR[7] = pow(2,data.bestxyz.vely_std) TWIST_COVAR[14] = pow(2,data.bestxyz.velz_std) odom.twist.twist.angular = imu.angular_velocity odom.twist.covariance = TWIST_COVAR self.pub_odom.publish(odom) # # Odometry transform (if required) # if self.publish_tf: self.tf_broadcast.sendTransform( (odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z), odom.pose.pose.orientation, #Quaternion(*orient), odom.header.stamp,odom.child_frame_id, odom.frame_id) # # NavSatFix # TODO: Work out these covariances properly from DOP # navsat = NavSatFix() navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame navsat.status = self.nav_status # position, in degrees navsat.latitude = data.bestpos.latitude navsat.longitude = data.bestpos.longitude navsat.altitude = data.bestpos.altitude NAVSAT_COVAR[0] = pow(2, data.bestpos.lat_std) # in meters NAVSAT_COVAR[4] = pow(2, data.bestpos.lon_std) NAVSAT_COVAR[8] = pow(2, data.bestpos.hgt_std) navsat.position_covariance = NAVSAT_COVAR navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN self.pub_navsatfix.publish(navsat)
if __name__ == "__main__": rospy.init_node("fake_waterlinked", log_level=rospy.INFO) heading_offset = rospy.get_param( "~heading" ) # heading is given by waterlinked GPS in degrees CW from North master_datum = rospy.get_param( "~datum") # Master located (latitude, longitude) lat, lon, alt = master_datum + [0.0] if len( master_datum) < 3 else master_datum master_gps = rospy.Publisher('gps_datum', NavSatFix, queue_size=5) master_msg = NavSatFix() master_msg.altitude = alt master_msg.longitude = lon master_msg.latitude = lat master_msg.status = NavSatStatus() master_msg.status.status = master_msg.status.STATUS_FIX master_msg.status.service = master_msg.status.SERVICE_GPS master_msg.position_covariance_type = master_msg.COVARIANCE_TYPE_UNKNOWN master_msg.position_covariance = [-1, 0, 0, 0, 0, 0, 0, 0, 0] pose_pub = rospy.Publisher('waterlinked/pose_with_cov_stamped', PoseWithCovarianceStamped, queue_size=5) buff = Buffer() TransformListener(buff) rospy.Subscriber("mavros/global_position/global", NavSatFix, handle_gps, (pose_pub, buff)) map_to_waterlink = TransformStamped( Header(0, rospy.Time.now(), 'map'), 'waterlinked', Transform( None,
def navigation_handler(self, data): """ Rebroadcasts navigation data in the following formats: 1) /odom => /base footprint transform (if enabled, as per REP 105) 2) Odometry message, with parent/child IDs as in #1 3) NavSatFix message, for systems which are knowledgeable about GPS stuff 4) IMU messages """ rospy.logdebug("Navigation received") # If we don't have a fix, don't publish anything... if self.nav_status.status == NavSatStatus.STATUS_NO_FIX: return # UTM conversion easting, northing = data.bestxyz.easting, data.bestxyz.northing # Initialize starting point if we haven't yet # TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid? # TODO: check INSPVAX sol stat for valid position before accepting if not self.init and self.zero_start: self.origin.x = easting self.origin.y = northing self.init = True # Publish origin reference for others to know about p = Pose() p.position.x = self.origin.x p.position.y = self.origin.y self.pub_origin.publish(p) # IMU # TODO: Work out these covariances properly. Logs provide covariances in local frame, not body imu = Imu() imu.header.stamp == rospy.Time.now() imu.header.frame_id = self.base_frame # Orientation # orient=PyKDL.Rotation.RPY(RAD(data.roll),RAD(data.pitch),RAD(data.heading)).GetQuaternion() # imu.orientation = Quaternion(*orient) imu.orientation.x = data.inspvax.pitch imu.orientation.y = data.inspvax.roll imu.orientation.z = data.inspvax.azimuth imu.orientation.w = 0 IMU_ORIENT_COVAR[0] = POW(2, data.inspvax.pitch_std) IMU_ORIENT_COVAR[4] = POW(2, data.inspvax.roll_std) IMU_ORIENT_COVAR[8] = POW(2, data.inspvax.azimuth_std) imu.orientation_covariance = IMU_ORIENT_COVAR # Angular rates (rad/s) # corrimudata log provides instantaneous rates so multiply by IMU rate in Hz imu.angular_velocity.x = data.corrimudata.pitch_rate * self.imu_rate imu.angular_velocity.y = data.corrimudata.roll_rate * self.imu_rate imu.angular_velocity.z = data.corrimudata.yaw_rate * self.imu_rate imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration (m/s^2) imu.linear_acceleration.x = data.corrimudata.x_accel * self.imu_rate imu.linear_acceleration.y = data.corrimudata.y_accel * self.imu_rate imu.linear_acceleration.z = data.corrimudata.z_accel * self.imu_rate imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu) # Odometry # TODO: Work out these covariances properly odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = self.odom_frame odom.child_frame_id = self.base_frame odom.pose.pose.position.x = easting - self.origin.x odom.pose.pose.position.y = northing - self.origin.y odom.pose.pose.position.z = data.inspvax.altitude #odom.pose.pose.orientation = Quaternion(*orient) odom.pose.pose.orientation = imu.orientation POSE_COVAR[21] = IMU_ORIENT_COVAR[0] POSE_COVAR[28] = IMU_ORIENT_COVAR[4] POSE_COVAR[35] = IMU_ORIENT_COVAR[8] odom.pose.covariance = POSE_COVAR # Twist is relative to vehicle frame odom.twist.twist.linear.x = data.bestxyz.velx odom.twist.twist.linear.y = data.bestxyz.vely odom.twist.twist.linear.z = data.bestxyz.velz TWIST_COVAR[0] = pow(2, data.bestxyz.velx_std) TWIST_COVAR[7] = pow(2, data.bestxyz.vely_std) TWIST_COVAR[14] = pow(2, data.bestxyz.velz_std) odom.twist.twist.angular = imu.angular_velocity odom.twist.covariance = TWIST_COVAR self.pub_odom.publish(odom) # # Odometry transform (if required) # if self.publish_tf: self.tf_broadcast.sendTransform( (odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z), odom.pose.pose.orientation, #Quaternion(*orient), odom.header.stamp, odom.child_frame_id, odom.frame_id) # # NavSatFix # TODO: Work out these covariances properly from DOP # navsat = NavSatFix() navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame navsat.status = self.nav_status # position, in degrees navsat.latitude = data.bestpos.latitude navsat.longitude = data.bestpos.longitude navsat.altitude = data.bestpos.altitude NAVSAT_COVAR[0] = pow(2, data.bestpos.lat_std) # in meters NAVSAT_COVAR[4] = pow(2, data.bestpos.lon_std) NAVSAT_COVAR[8] = pow(2, data.bestpos.hgt_std) navsat.position_covariance = NAVSAT_COVAR navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN self.pub_navsatfix.publish(navsat)
def main(argv): if len(sys.argv) < 2: print 'Please specify data directory file' return 1 if len(sys.argv) < 3: print 'Please specify output rosbag file' return 1 print("loading files...") bag = rosbag.Bag(sys.argv[2], 'w') gps = np.loadtxt(sys.argv[1] + "sensor_data/2012-01-08/gps.csv", delimiter=",") imu_100hz = np.loadtxt(sys.argv[1] + "sensor_data/2012-01-08/imu_100hz.csv", delimiter=",") ral_seq = 0 bap_seq = 0 img_seq = 0 imu_seq = 0 cal = -1 gps_seq = 0 # IMAGE_COUNT = 81169 STREET_VIEW = 113 print("package gps and image...") print("Packaging GPS and image") for gps_data in gps: utime = int(gps_data[0]) mode = int(gps_data[1]) timestamp = rospy.Time.from_sec(utime / 1e6) lat = float(gps_data[3]) lng = float(gps_data[4]) alt = float(gps_data[5]) status = NavSatStatus() if mode == 0 or mode == 1: status.status = NavSatStatus.STATUS_NO_FIX else: status.status = NavSatStatus.STATUS_FIX status.service = NavSatStatus.SERVICE_GPS num_sats = UInt16() num_sats.data = float(gps_data[2]) fix = NavSatFix() fix.header.seq = gps_seq fix.status = status fix.latitude = np.rad2deg(lat) fix.longitude = np.rad2deg(lng) fix.altitude = np.rad2deg(alt) track = Float64() track.data = float(gps_data[6]) speed = Float64() speed.data = float(gps_data[7]) bag.write('/gps', fix, t=timestamp) bag.write('/gps_track', track, t=timestamp) bag.write('/gps_speed', speed, t=timestamp) # write aerial image if gps_seq <= MAVIMAGE: img_path = sys.argv[1] + 'images/2012-01-08/lb3/Cam5/' img_list = os.listdir(img_path) img_list.sort() img_cv = cv2.imread(os.path.join(img_path, img_list[gps_seq]), -1) img_cv = cv2.resize(img_cv, MAVDIM, interpolation=cv2.INTER_AREA) # 顺时针旋转90度 trans_img = cv2.transpose(img_cv) img_cv = cv2.flip(trans_img, 1) br = CvBridge() Img = Image() Img = br.cv2_to_imgmsg(img_cv, "bgr8") Img.header.seq = int(gps_seq) print(gps_seq) Img.header.stamp = timestamp Img.header.frame_id = 'camera' bag.write('/image/cam5', Img, t=timestamp) gps_seq = gps_seq + 1 print('packaging imu...') for imu_data in imu_100hz: imu_seq = imu_seq + 1 utime = int(imu_data[0]) timestamp = rospy.Time.from_sec(utime / 1e6) imu = Imu() imu.header.seq = imu_seq imu.header.stamp = timestamp imu.header.frame_id = '/Imu' imu.linear_acceleration.x = float(imu_data[5]) imu.linear_acceleration.y = float(imu_data[6]) imu.linear_acceleration.z = float(imu_data[7]) imu.linear_acceleration_covariance = np.zeros(9) imu.angular_velocity.x = float(imu_data[8]) imu.angular_velocity.y = float(imu_data[9]) imu.angular_velocity.z = float(imu_data[10]) imu.angular_velocity_covariance = np.zeros(9) imu.orientation.w = float(imu_data[1]) imu.orientation.x = float(imu_data[2]) imu.orientation.y = float(imu_data[3]) imu.orientation.z = float(imu_data[4]) bag.write('/Imu', imu, t=timestamp) bag.close() return 0
def main(args): if len(sys.argv) < 2: print 'Please specify gps file' return 1 if len(sys.argv) < 3: print 'Please specify output rosbag file' return 1 gps = np.loadtxt(sys.argv[1], delimiter=",") utimes = gps[:, 0] modes = gps[:, 1] num_satss = gps[:, 2] lats = gps[:, 3] lngs = gps[:, 4] alts = gps[:, 5] tracks = gps[:, 6] speeds = gps[:, 7] bag = rosbag.Bag(sys.argv[2], 'w') try: for i, utime in enumerate(utimes): timestamp = rospy.Time.from_sec(utime / 1e6) status = NavSatStatus() if modes[i] == 0 or modes[i] == 1: status.status = NavSatStatus.STATUS_NO_FIX else: status.status = NavSatStatus.STATUS_FIX status.service = NavSatStatus.SERVICE_GPS num_sats = UInt16() num_sats.data = num_satss[i] fix = NavSatFix() fix.status = status fix.latitude = np.rad2deg(lats[i]) fix.longitude = np.rad2deg(lngs[i]) fix.altitude = alts[i] track = Float64() track.data = tracks[i] speed = Float64() speed.data = speeds[i] bag.write('fix', fix, t=timestamp) bag.write('track', track, t=timestamp) bag.write('speed', speed, t=timestamp) finally: bag.close() return 0
def handle_gps(self, data, timestamp): """ Date gps_Date MM/DD/YYYY or DD/MM/YYYY Time gps_Time HH:MM:SS.SSS Lat & Lon gps_Lat,gps_Long Degrees-7 Altitude gps_Alt mm Altitude MSL gps_AltMSL mm SIV gps_SIV Count Fix Type gps_FixType 0-5 Carrier Soln. gps_CarrierSolution 0-2 Ground Speed gps_GroundSpeed mm/s Heading gps_Heading Degrees-5 PDOP gps_pDOP 10-2 (dimensionless) Time Of Week gps_iTOW ms Lat = Latitude Lon = Longitude MSL = Metres above Sea Level SIV = Satellites In View PDOP = Positional Dilution Of Precision Fix Type: 0: No 1: Dead Reckoning Only 2: 2D 3: 3D 4: GNSS + Dead Reckoning 5: Time Only Carrier Solution: 0: No 1: Float Solution 2: Fixed Solution """ gps_msg = NavSatFix() gps_msg.header.frame_id = self.tf_prefix+"world" gps_msg.header.stamp = timestamp gps_msg.status = NavSatStatus(status=0, service=1) gps_msg.latitude = float(data[self.data_headers["GPS"]["Lat"]]) gps_msg.longitude = float(data[self.data_headers["GPS"]["Long"]]) gps_msg.altitude = float(data[self.data_headers["GPS"]["Alt"]]) # COVARIANCE_TYPE_UNKNOWN = 0, COVARIANCE_TYPE_APPROXIMATED = 1 # COVARIANCE_TYPE_DIAGONAL_KNOWN = 2, COVARIANCE_TYPE_KNOWN = 3 gps_msg.position_covariance = [0.0] * 9 gps_msg.position_covariance_type = 0 self.gps_pub.publish(gps_msg) # Time Reference time_msg = TimeReference() time_msg.header.stamp = timestamp gps_time = datetime.strptime("{} {}".format(data[self.data_headers["GPS"]["Date"]], data[self.data_headers["GPS"]["Time"]]), "%d/%m/%Y %H:%M:%S.%f") total_secs = (gps_time - epoch).total_seconds() time_msg.time_ref.secs = int(total_secs) time_msg.time_ref.nsecs = total_secs-int(total_secs) time_msg.source = "GPS" self.time_ref.publish(time_msg) return True
def navigation_handler(self, data): """ Rebroadcasts navigation data in the following formats: 1) /odom => /base footprint transform (if enabled, as per REP 105) 2) Odometry message, with parent/child IDs as in #1 3) NavSatFix message, for systems which are knowledgeable about GPS stuff 4) IMU messages """ rospy.logdebug("Navigation received") # If we don't have a fix, don't publish anything... # if self.nav_status.status == NavSatStatus.STATUS_NO_FIX: # print 'no fix' # return # Changing from NED from the Applanix to ENU in ROS # Roll is still roll, since it's WRT to the x axis of the vehicle # Pitch is -ve since axis goes the other way (+y to right vs left) # Yaw (or heading) in Applanix is clockwise starting with North # In ROS it's counterclockwise startin with East time_stat = TimeSync() time_stat.ros_time = rospy.Time.now() time_stat.gps_time = data.td self.pub_time.publish(time_stat) t1 = time_stat.ros_time.secs + time_stat.ros_time.nsecs / 1E9 t2 = time_stat.gps_time.time1 # print '{0:6f}'.format(t1 - t2) orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(-data.pitch), RAD(90 - data.heading)).GetQuaternion() # UTM conversion utm_pos = geodesy.utm.fromLatLong(data.latitude, data.longitude) # Initialize starting point if we haven't yet if not self.init and self.zero_start: self.origin.x = utm_pos.easting self.origin.y = utm_pos.northing self.origin.z = data.altitude self.init = True origin_param = { "east": self.origin.x, "north": self.origin.y, "alt": self.origin.z, } rospy.set_param('/gps_origin', origin_param) # Publish origin reference for others to know about p = Pose() p.position.x = self.origin.x p.position.y = self.origin.y p.position.z = self.origin.z self.pub_origin.publish(p) # # Odometry # TODO: Work out these covariances properly from DOP # odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = self.odom_frame odom.child_frame_id = self.base_frame odom.pose.pose.position.x = utm_pos.easting - self.origin.x odom.pose.pose.position.y = utm_pos.northing - self.origin.y odom.pose.pose.position.z = data.altitude - self.origin.z odom.pose.pose.orientation = Quaternion(*orient) odom.pose.covariance = POSE_COVAR # Twist is relative to /reference frame or /vehicle frame and # NED to ENU conversion is needed here too odom.twist.twist.linear.x = data.east_vel odom.twist.twist.linear.y = data.north_vel odom.twist.twist.linear.z = -data.down_vel odom.twist.twist.angular.x = RAD(data.ang_rate_long) odom.twist.twist.angular.y = RAD(-data.ang_rate_trans) odom.twist.twist.angular.z = RAD(-data.ang_rate_down) odom.twist.covariance = TWIST_COVAR self.pub_odom.publish(odom) t_tf = odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z q_tf = Quaternion(*orient).x, Quaternion(*orient).y, Quaternion( *orient).z, Quaternion(*orient).w # # Odometry transform (if required) # if self.publish_tf: self.tf_broadcast.sendTransform(t_tf, q_tf, odom.header.stamp, odom.child_frame_id, odom.header.frame_id) # # NavSatFix # TODO: Work out these covariances properly from DOP # navsat = NavSatFix() navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame navsat.status = self.nav_status navsat.latitude = data.latitude navsat.longitude = data.longitude navsat.altitude = data.altitude navsat.position_covariance = NAVSAT_COVAR navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self.pub_navsatfix.publish(navsat) # # IMU # TODO: Work out these covariances properly # imu = Imu() imu.header.stamp = rospy.Time.now() imu.header.frame_id = self.base_frame # Orientation imu.orientation = Quaternion(*orient) imu.orientation_covariance = IMU_ORIENT_COVAR # Angular rates imu.angular_velocity.x = RAD(data.ang_rate_long) imu.angular_velocity.y = RAD(-data.ang_rate_trans) imu.angular_velocity.z = RAD(-data.ang_rate_down) imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration imu.linear_acceleration.x = data.long_accel imu.linear_acceleration.y = data.trans_accel imu.linear_acceleration.z = data.down_accel imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu) pass
def navigation_handler(self, data): """ Rebroadcasts navigation data in the following formats: 1) /odom => /base footprint transform (if enabled, as per REP 105) 2) Odometry message, with parent/child IDs as in #1 3) NavSatFix message, for systems which are knowledgeable about GPS stuff 4) IMU messages """ rospy.logdebug("Navigation received") # If we don't have a fix, don't publish anything... if self.nav_status.status == NavSatStatus.STATUS_NO_FIX: return orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(data.pitch), RAD(data.heading)).GetQuaternion() # UTM conversion (zone, easting, northing) = LLtoUTM(23, data.latitude, data.longitude) # Initialize starting point if we haven't yet # TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid? if not self.init and self.zero_start: self.origin.x = easting self.origin.y = northing self.init = True # Publish origin reference for others to know about p = Pose() p.position.x = self.origin.x p.position.y = self.origin.y self.pub_origin.publish(p) # # Odometry # TODO: Work out these covariances properly from DOP # odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = self.odom_frame odom.child_frame_id = self.base_frame odom.pose.pose.position.x = easting - self.origin.x odom.pose.pose.position.y = northing - self.origin.y odom.pose.pose.position.z = data.altitude odom.pose.pose.orientation = Quaternion(*orient) odom.pose.covariance = POSE_COVAR # Twist is relative to /vehicle frame odom.twist.twist.linear.x = data.speed odom.twist.twist.linear.y = 0 odom.twist.twist.linear.z = -data.down_vel odom.twist.twist.angular.x = RAD(data.ang_rate_long) odom.twist.twist.angular.y = RAD(-data.ang_rate_trans) odom.twist.twist.angular.z = RAD(-data.ang_rate_down) odom.twist.covariance = TWIST_COVAR self.pub_odom.publish(odom) # # Odometry transform (if required) # if self.publish_tf: self.tf_broadcast.sendTransform( (odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z), Quaternion(*orient), odom.header.stamp,odom.child_frame_id, odom.frame_id) # # NavSatFix # TODO: Work out these covariances properly from DOP # navsat = NavSatFix() navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame navsat.status = self.nav_status navsat.latitude = data.latitude navsat.longitude = data.longitude navsat.altitude = data.altitude navsat.position_covariance = NAVSAT_COVAR navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self.pub_navsatfix.publish(navsat) # # IMU # TODO: Work out these covariances properly # imu = Imu() imu.header.stamp == rospy.Time.now() imu.header.frame_id = self.base_frame # Orientation imu.orientation = Quaternion(*orient) imu.orientation_covariance = IMU_ORIENT_COVAR # Angular rates imu.angular_velocity.x = RAD(data.ang_rate_long) imu.angular_velocity.y = RAD(-data.ang_rate_trans) imu.angular_velocity.y = RAD(-data.ang_rate_down) imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration imu.linear_acceleration.x = data.long_accel imu.linear_acceleration.y = data.trans_accel imu.linear_acceleration.z = data.down_accel imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu) pass