def to_msg(self): h = Header() h.stamp = self.stamp h.frame_id = earth_frame_id msg = NavSatFix() msg.header = h msg.latitude = math.degrees(self.geopoint.latitude) msg.longitude = math.degrees(self.geopoint.longitude) msg.altitude = self.geopoint.z return msg
def publish_next_goal(self): gps_pub = rospy.Publisher('/gps_goal_fix', NavSatFix, queue_size=10) gps_data = NavSatFix() gps_data.header.frame_id = '/world' self.lat = self.lat + 0.00000004000 gps_data.latitude = self.lat rospy.loginfo('latitude is : ' + str(self.lat)) gps_data.longitude = self.lng rospy.loginfo('longitude is : ' + str(self.lng)) rospy.loginfo('publishing next goal') gps_pub.publish(gps_data)
def save_gps_fix_data(bag, kitti, gps_frame_id, topic): for timestamp, oxts in zip(kitti.timestamps, kitti.oxts): navsatfix_msg = NavSatFix() navsatfix_msg.header.frame_id = gps_frame_id navsatfix_msg.header.stamp = rospy.Time.from_sec( float(timestamp.strftime("%s.%f"))) navsatfix_msg.latitude = oxts.packet.lat navsatfix_msg.longitude = oxts.packet.lon navsatfix_msg.altitude = oxts.packet.alt navsatfix_msg.status.service = 1 bag.write(topic, navsatfix_msg, t=navsatfix_msg.header.stamp)
def publish_llh_msg(self, msg, **metadata): llh_msg = NavSatFix() llh_msg.latitude = msg.lat llh_msg.longitude = msg.lon llh_msg.altitude = msg.height llh_msg.position_covariance_type = 2 llh_msg.position_covariance = [9, 0, 0, 0, 9, 0, 0, 0, 9] # Publish ROS messages self.pub_llh.publish(llh_msg) self.pub_llh_n_sats.publish(Int32(msg.n_sats)) self.pub_llh_fix_mode.publish(Int32(msg.flags))
def CallGNSS_(self, data): # GNSS data # 1Hz self.GNSS_ = data self.puGNSSPosCalF_ = puGNSSPosCal.GNSSPosCal() self.puGNSSPosCalF_.iterPosCal(self.GNSS_, 'WLSGNSS') navfix_ = NavSatFix() llh_ = [] llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_.ecef_) navfix_.header.stamp.secs = self.GNSS_.GNSS_Raws[-1].GNSS_time navfix_.latitude = float(llh_[0]) navfix_.longitude = float(llh_[1]) navfix_.altitude = float(llh_[2]) self.GNSS_Navfix_pub.publish(navfix_) # print 'ecef',self.puGNSSPosCalF_.ecef_,'llh',llh_
def gps_talker(): rospy.loginfo("GPS: Node started.") pub = rospy.Publisher('gps', NavSatFix, queue_size=10) rospy.init_node('gps', anonymous=True) msg = NavSatFix() seq = 0 lastPub = time.monotonic() while not rospy.is_shutdown(): gps.update() currentTime = time.monotonic() # publish every second if currentTime - lastPub >= 1.0: seq += 1 lastPub = currentTime # fill header msg.header.seq = seq msg.header.stamp = rospy.Time.now() msg.header.frame_id = 'world' # log if gps knows its position (fix) if prev_has_fix != gps.has_fix: if gps.has_fix: rospy.loginfo("GPS: has fix") else: rospy.loginfo("GPS: lost fix") prev_has_fix = gps.has_fix if not gps.has_fix: # Try again if no fix msg.status.status = int(0) # publish pub.publish(msg) continue else: msg.status.status = int(1) msg.status.service = int(0) # position msg.latitude = float(gps.latitude) msg.longitude = float(gps.longitude) #!!!!!!using altitude as velocity!!!!! if gps.gps.speed_knots is not None: msg.altitude = float(gps.speed_knots) * 1.852 # publish pub.publish(msg)
def parse_goals(self): waypoints = WaypointsArray() w_list = [] for location in self.goals['Goals']: current = NavSatFix() current.latitude = float(location['lat']) current.longitude = float(location['long']) current.altitude = float(location['elev']) w_list.append(current) waypoints.waypoints = w_list self.waypoint_pub.publish(waypoints)
def publish_next_goal(self,i): self.i = self.i+1 gps_pub = rospy.Publisher('/gps_goal_fix',NavSatFix,queue_size=10) gps_data = NavSatFix() gps_data.header.frame_id='/world' rospy.logerr('number is '+str(self.i)) gps_data.latitude=self.px[self.i] rospy.loginfo('latitude is : '+str(self.px[self.i])) gps_data.longitude=self.py[self.i] rospy.loginfo('longitude is : '+str(self.py[self.i])) rospy.loginfo('publishing next goal') gps_pub.publish(gps_data)
def publish_gps(gps_pub, imu_data, log=False): gps = NavSatFix() gps.header.frame_id = FRAME_ID gps.header.stamp = rospy.Time.now() gps.latitude = imu_data.lat gps.longitude = imu_data.lon gps.altitude = imu_data.alt gps_pub.publish(gps) if log: rospy.loginfo("gps msg published")
def callgnss_ins_navsat(self,data): self.uck_bestPos_ = NavSatFix() self.uck_bestPos_ = data self.ukf_lat_.append(float(self.uck_bestPos_.latitude)) self.ukf_lon_.append(float(self.uck_bestPos_.longitude)) fold = KML.Folder(KML.Placemark(KML.Point(KML.coordinates(str(self.ukf_lon_[0]) + ',' + str(self.ukf_lat_[0]) + ',0')))) for i in range(1, len(self.ukf_lon_)): fold.append(KML.Placemark( KML.Point(KML.coordinates(str(self.ukf_lon_[i]) + ',' + str(self.ukf_lat_[i]) + ',0')))) content = etree.tostring(etree.ElementTree(fold), pretty_print=True) with open('Berkeley_gnss_ins.kml', 'w') as fp: fp.write(content)
def gps_fix_sim(): pub = rospy.Publisher('sensor_msgs/NavSatFix', NavSatFix, queue_size=10) rospy.init_node('gps_sim', anonymous=True) rate = rospy.Rate(2) while not rospy.is_shutdown(): gps_fix = NavSatFix() gps_fix.latitude = 36.6 gps_fix.longitude = -121.1 gps_fix.altitude = 1.0 pub.publish(gps_fix) rate.sleep()
def callback(data): # Set Map Origin # 30.174885, -96.512199 olat = 30.174885 olon = -96.512199 # Odom to lat/lon xg2, yg2 = gc.xy2ll(data.pose.pose.position.x, data.pose.pose.position.y, olat, olon) # build navsat message fake_gps = NavSatFix() fake_gps.header.frame_id = rospy.get_param('~frame_id', 'gps') fake_gps.header.stamp = rospy.Time.now() fake_gps.status.status = 1 fake_gps.status.service = 1 fake_gps.latitude = xg2 fake_gps.longitude = yg2 fake_gps.altitude = 0 #compass = Float(90.0) pub = rospy.Publisher('fake_gps', NavSatFix, queue_size=10) pub.publish(fake_gps) # Odom to UTM utmy, utmx, utmzone = gc.LLtoUTM(xg2, yg2) # build navsat message fake_UTM = NavSatFix() fake_UTM.header.frame_id = rospy.get_param('~frame_id', 'utm') fake_UTM.header.stamp = rospy.Time.now() fake_UTM.status.status = 1 fake_UTM.status.service = 1 fake_UTM.latitude = utmy fake_UTM.longitude = utmx fake_UTM.altitude = 0 #compass = Float(90.0) pub2 = rospy.Publisher('fake_utm', NavSatFix, queue_size=10) pub2.publish(fake_UTM)
def PointCloudCall(self,data): # 20 Hz As this function has highest frequency, good for update self.pointCloud_ = PointCloud2() self.pointCloud_ = data self.graphslam_PointCloud_pub.publish(pointCloud_) # for Graph slam self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS) self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_, 'statManu_Exclusion', self.excluSatLis) # statManu self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal() if((len(self.GNSSNRAWlosDel.GNSS_Raws) > 1) and (self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time != self.GNSSNRAWlosDelTimeFlag)): # only if there is a change, there will conduct the calculation # print 'self.GNSSNRAWlosDel',len(self.GNSSNRAWlosDel.GNSS_Raws) self.puGNSSPosCalF_prop.iterPosCal(self.GNSSNRAWlosDel, 'WLSGNSS') self.GNSSNRAWlosDelTimeFlag = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time navfix_ = NavSatFix() llh_ = [] llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_prop.ecef_) navfix_.header.stamp.secs = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time navfix_.latitude = float(llh_[0]) navfix_.longitude = float(llh_[1]) navfix_.altitude = float(llh_[2]) self.propGNSS_Navfix_pub.publish(navfix_) # for Graph slam graphNavfix_ = NavSatFix() graphNavfix_ = navfix_ graphNavfix_.header = pointCloud_.header self.graphslam_Navfix_pub.publish(graphNavfix_) # for Graph slam geopoint = GeoPointStamped() geopoint.header = graphNavfix_.header # calculate the ENU coordiantes initialLLH enu_ = pugeomath.transformEcefToEnu(self.initialLLH,self.puGNSSPosCalF_prop.ecef_) print 'enu_ ->: ',enu_ geopoint.position.latitude = float(enu_[0]) geopoint.position.longitude = float(enu_[1]) # geopoint.position.altitude = float(llh_[2]) # use this to save the covariance in altitude component geopoint.position.altitude = self.GNSSNRAWlosDel.GNSS_Raws[-1].snr * self.HDOPProp # save the covariance self.graphslam_GeoPoint_pub.publish(geopoint)
def __init__(self): self.latitude_deg = 0.0 self.longitude_deg = 0.0 self.msl_height_m = 0.0 self.el_height_m = 0.0 self.horizontal_stdev = 0.0 self.vertical_stdev = 0.0 self.valid_llh_flag = 0 self.fix_valid_flags = 0 self.fix_type = 0 self.fix_num_space_vehicles = 0 self.hw_sensor_state = 0 self.hw_antenna_state = 0 self.hw_antenna_power = 0 self.hw_valid_hw_flags = 0 self.publish_data = False self._HVMsgData = NavSatFix() self._HVMsgPub = rospy.Publisher('/segway/feedback/gps/fix_3d', NavSatFix, queue_size=10) self._HVMsgData.header.frame_id = '/segway/gps_frame' self._HVMsgData.status.service = NavSatStatus.SERVICE_GPS self._HMsgData = NavSatFix() self._HMsgPub = rospy.Publisher('/segway/feedback/gps/fix_2d', NavSatFix, queue_size=10) self._HMsgData.header.frame_id = '/segway/gps_frame' self._HMsgData.status.service = NavSatStatus.SERVICE_GPS self._seq = 0 self.LAT_LON_FIX_VALID = 0x0001 self.ELLIPSOID_HEIGHT_FIX_VALID = 0x0002 self.MSL_HEIGHT_FIX_VALID = 0x0004 self.HORIZONTAL_ACCURACY_VALID = 0x0008 self.VERTICAL_ACCURACY_VALID = 0x0010
def CreateBag(matfile, bagname, frame_id, navsat_topic="/fix"): '''Creates the actual bag file by successively adding images''' bag = rosbag.Bag(bagname, 'w', compression=rosbag.Compression.BZ2, chunk_threshold=32 * 1024 * 1024) data = loadmat(matfile)['rnv'] rnv = {} for i, col in enumerate(data.dtype.names): rnv[col] = data.item()[i] try: ref = None x, y = [], [] for i, t in enumerate(tqdm(rnv['t'])): # print("Adding %s" % image_name) stamp = rospy.Time.from_sec(t) nav_msg = NavSatFix() nav_msg.header.stamp = stamp nav_msg.header.frame_id = "map" nav_msg.header.seq = i nav_msg.latitude = rnv['lat'][i][0] nav_msg.longitude = rnv['lon'][i][0] nav_msg.altitude = -rnv['depth'][i][0] nav_msg.position_covariance_type = nav_msg.COVARIANCE_TYPE_UNKNOWN transform_msg = TransformStamped() transform_msg.header = copy(nav_msg.header) utm = fromLatLong(nav_msg.latitude, nav_msg.longitude, nav_msg.altitude) if ref is None: ref = utm.toPoint() x.append(utm.toPoint().x - ref.x) y.append(utm.toPoint().y - ref.y) dx = x[-1] - sum(x[-min(20, len(x)):]) / min(20, len(x)) dy = y[-1] - sum(y[-min(20, len(y)):]) / min(20, len(y)) transform_msg.transform.translation.x = x[-1] transform_msg.transform.translation.y = y[-1] transform_msg.transform.translation.z = nav_msg.altitude transform_msg.transform.rotation = Quaternion(*tf_conversions.transformations.quaternion_from_euler(0, 0, math.atan2(dy, dx))) transform_msg.child_frame_id = frame_id tf_msg = tfMessage(transforms=[transform_msg]) odometry_msg = Odometry() odometry_msg.header = copy(transform_msg.header) odometry_msg.child_frame_id = frame_id odometry_msg.pose.pose.position = transform_msg.transform.translation odometry_msg.pose.pose.orientation = transform_msg.transform.rotation bag.write(navsat_topic, nav_msg, stamp) bag.write("/tf", tf_msg, stamp) bag.write("/transform", transform_msg, stamp) bag.write("/odom", odometry_msg, stamp) finally: bag.close()
def callback(self): rate = rospy.Rate(10) ports = [5801,5802,5803] sockets = [] pubs = [self.static_pub, self.rtkStatic_pub, self.rtkDynamic_pub] for i in ports: sock = socket.socket() host = socket.gethostname() sock.connect((host, i)) sockets.append(sock) e2 = 6.69437999014e-3 a = 6378137.0 while not rospy.is_shutdown(): for i in range(len(sockets)): navsat = NavSatFix() ecef_xyz = Point() ecef_pose = Pose() #ecef_stampedPose = PoseStamped() navsat.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = sockets[i].recv(1024) #Split the message msg = msgStr.split() navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) ecef_pose.position = ecef_xyz #ecef_stampedPose.pose = ecef_pose pubs[i].publish(navsat) rate.sleep()
def 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 scooby(): gps1 = NavSatFix() pub = rospy.Publisher("/rtk_new", NavSatFix, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) for i in range(1, 2000): gps1.latitude = 42 gps1.longitude = -71 gps1.altitude = i pub.publish(gps1) print "Publishing data:" print i rate.sleep()
def sensor_data_updated(self, carla_gnss_event): """ Function to transform a received gnss event into a ROS NavSatFix message :param carla_gnss_event: carla gnss event object :type carla_gnss_event: carla.GnssEvent """ navsatfix_msg = NavSatFix() navsatfix_msg.header = self.get_msg_header(use_parent_frame=False) navsatfix_msg.latitude = carla_gnss_event.latitude navsatfix_msg.longitude = carla_gnss_event.longitude navsatfix_msg.altitude = carla_gnss_event.altitude self.publish_ros_message(self.topic_name() + "/fix", navsatfix_msg)
def __init__(self): State.__init__(self, outcomes=['success'], input_keys=['waypoints', 'cur_waypoint_in']) self.odom_topic = rospy.get_param('~odom_topic', default='odom') self.gps_topic = rospy.get_param('~gps_topic', default='fix') self.pose = PoseWithCovarianceStamped() self.last_gps = NavSatFix() self.odom_sub = rospy.Subscriber(self.odom_topic, PoseWithCovarianceStamped, self._odom_cb) self.gps_sub = rospy.Subscriber(self.gps_topic, NavSatFix, self._gps_cb) self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction) self.client.wait_for_server()
def sensor_data_updated(self, carla_gnss_measurement): """ Function to transform a received gnss event into a ROS NavSatFix message :param carla_gnss_measurement: carla gnss measurement object :type carla_gnss_measurement: carla.GnssMeasurement """ navsatfix_msg = NavSatFix() navsatfix_msg.header = self.get_msg_header(timestamp=carla_gnss_measurement.timestamp) navsatfix_msg.latitude = carla_gnss_measurement.latitude navsatfix_msg.longitude = carla_gnss_measurement.longitude navsatfix_msg.altitude = carla_gnss_measurement.altitude self.gnss_publisher.publish(navsatfix_msg)
def talker(self): rospy.Subscriber("fix", NavSatFix, self.callback) pub = rospy.Publisher("navsat/fix", NavSatFix, queue_size=50) rospy.init_node('navsat_republisher', anonymous=True) r = rospy.Rate(10) msg = NavSatFix() while not rospy.is_shutdown(): msg = self.callback_msg msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.frame_id pub.publish(msg) r.sleep()
def testAutoOriginFromNavSatFix(self): rospy.init_node('test_initialize_origin') nsf_pub = rospy.Publisher('gps', NavSatFix, queue_size=2) origin_sub = self.subscribeToOrigin() nsf_msg = NavSatFix() nsf_msg.latitude = swri['latitude'] nsf_msg.longitude = swri['longitude'] nsf_msg.altitude = swri['altitude'] nsf_msg.header.frame_id = "/far_field" r = rospy.Rate(10.0) while not rospy.is_shutdown(): nsf_pub.publish(nsf_msg) r.sleep()
def main(): pub = rospy.Publisher("goal_coords", NavSatFix, queue_size=10) rospy.init_node("goal_pub", anonymous=False) r = rospy.Rate(1) while not rospy.is_shutdown(): msg = NavSatFix() msg.header.frame_id = "gps_frame" msg.longitude = 100 msg.latitude = 100 msg.position_covariance = [1, 0, 0, 0, 1, 0, 0, 0, 1] pub.publish(msg) r.sleep() print(msg)
def publish_gnss(self, sensor_id, data): """ Function to publish gnss data """ msg = NavSatFix() msg.header = self.get_header() msg.header.frame_id = 'gps' msg.latitude = data[0] msg.longitude = data[1] msg.altitude = data[2] msg.status.status = NavSatStatus.STATUS_SBAS_FIX msg.status.service = NavSatStatus.SERVICE_GPS | NavSatStatus.SERVICE_GLONASS | NavSatStatus.SERVICE_COMPASS | NavSatStatus.SERVICE_GALILEO self.publisher_map[sensor_id].publish(msg)
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 publishCurLatLon(self): msg = NavSatFix() msg.header.stamp = rospy.Time(self.curTime) msg.latitude = self.curLat msg.longitude = self.curLon # very small error for now msg.position_covariance = [0] * 9 msg.position_covariance[0] = 5**2 msg.position_covariance[3] = 5**2 rospy.loginfo("Publishing position %f, %f at time %f", self.curLat, self.curLon, self.curTime) self.fixPub.publish(msg)
def callback_gps_position(self, gps_input=NavSatFix()): lon = [48.1374, 48.1555] lat = [11.575, 11.579] lon.append(gps_input.longitude) lat.append(gps_input.latitude) alt = gps_input.altitude print "Longitude: ", gps_input.longitude, "\tLatitude: ", gps_input.latitude, "\tAltitude: ", gps_input.altitude gmap = gmplot.GoogleMapPlotter(48.1374, 11.575, 13) gmap.plot(lon, lat, 'cornflowerblue', edge_width=10) gmap.scatter(lon, lat, 'm', marker=True) gmap.draw("mymap.html")
def _publish_gnss(self, carla_gnss_measurement): """ Function to transform a received gnss event into a ROS NavSatFix message :param carla_gnss_measurement: carla gnss measurement object :type carla_gnss_measurement: carla.GnssMeasurement """ navsatfix_msg = NavSatFix() set_timestamp(navsatfix_msg.header, carla_gnss_measurement.timestamp) navsatfix_msg.header.frame_id = self._gnss_frame navsatfix_msg.latitude = carla_gnss_measurement.latitude navsatfix_msg.longitude = carla_gnss_measurement.longitude navsatfix_msg.altitude = carla_gnss_measurement.altitude self.publish("fix", navsatfix_msg)
def pub_gps(msg_type, msg, bridge): pub = bridge.get_ros_pub("gps", NavSatFix, queue_size=1) fix = NavSatFix() fix.header.stamp = bridge.project_ap_time(msg) fix.header.frame_id = 'base_footprint' fix.latitude = msg.lat / 1e07 fix.longitude = msg.lon / 1e07 # NOTE: absolute (MSL) altitude fix.altitude = msg.alt / 1e03 fix.status.status = NavSatStatus.STATUS_FIX fix.status.service = NavSatStatus.SERVICE_GPS fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN pub.publish(fix)