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 publishGps(my_data): global pub_location latlng = NavSatFix() data = my_data.split(',') if data[0] == 'L': latlng.header.stamp = rospy.Time.now() latlng.header.frame_id = "world" latlng.latitude = float(data[1]) latlng.longitude = float(data[2]) latlng.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN print "Location: " + data[0] + ' ' + data[1] + ", " + data[2] elif data[0] == 'R': latlng.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED print "Request Release" pub_location.publish(latlng)
def make_fix(t, data, args): fix = NavSatFix() fix.header.stamp = t fix.header.frame_id = args.frame_id if data.Q in (1, 2, 4): # Fix, Float, GPS fix.status.status = NavSatStatus.STATUS_GBAS_FIX elif data.Q == 3: # SBAS fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif data.Q == 5: # Single fix.status.status = NavSatStatus.STATUS_FIX else: fix.status.status = NavSatStatus.STATUS_NO_FIX fix.latitude = data.lat fix.longitude = data.lon fix.altitude = data.height fix.position_covariance = [ covariance(data.sde), covariance(data.sdne), covariance(data.sdeu), covariance(data.sdne), covariance(data.sdn), covariance(data.sdun), covariance(data.sdeu), covariance(data.sdun), covariance(data.sdu), ] fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN return fix
def get_solution(self): try: receive = self.client.recv(4096) except socket.error: raise if not receive: raise ValueError("Not received") # receive example # 1918 352534.000 35.674540574 139.531064244 94.6605 5 9 3.3592 2.1315 7.6682 -0.8273 1.5609 -2.0968 0.00 0.0 #$6 1:fix, 2:float 3:sbab, 4:dgps, 5:single, 6:ppp receive_split = receive.split() if len(receive_split) is not 15: raise ValueError("Receive data length is not 15") print receive_split t = self.gpst2time(receive_split[0], receive_split[1]) latitude = float(receive_split[2]) longtitude = float(receive_split[3]) ret = NavSatFix() ret.header.stamp = rospy.Time(float(t)) ret.latitude = latitude ret.longitude = longtitude ret.position_covariance_type = self.covariance_table[ int(receive_split[5]) - 1] return ret
def talker(): pub = rospy.Publisher('gps/fix', NavSatFix, queue_size=10) rospy.init_node('fake_gps', anonymous=True) pub2 = rospy.Publisher('vel', Twist, queue_size=10) rate = rospy.Rate(2) # 10hz loc_index = 0 while not rospy.is_shutdown(): #print(loc_index) fake_localization = NavSatFix() fake_vel = Twist() #fake_vel.frame_id = "/gps" #fake_localization.header.stamp = time.time() fake_localization.header.frame_id = '/gps' fake_localization.status.status = 1 fake_localization.status.service = 1 #print(gps_localizations[loc_index][0], gps_localizations[loc_index][1]) fake_localization.latitude = gps_localizations[loc_index][0] fake_localization.longitude = gps_localizations[loc_index][1] print(fake_localization.latitude, fake_localization.longitude) fake_localization.altitude = 0.0 fake_localization.position_covariance = [ 1.3224999999999998, 0.0, 0.0, 0.0, 1.3224999999999998, 0.0, 0.0, 0.0, 5.289999999999999 ] fake_localization.position_covariance_type = 1 #fake_vel.header.frame_id = "/gps" pub.publish(fake_localization) pub2.publish(fake_vel) if (loc_index == len(gps_localizations) - 1): loc_index = 0 else: loc_index = loc_index + 1 rate.sleep()
def callback(self): #Publish data at 1 Hz rate = rospy.Rate(1) #Define the RTKRCV server ports corresponding to each of the processing #Modes port = 5801 sock = socket.socket() host = socket.gethostname() sock.connect((host, port)) #WGS84 Parameters e2 = 6.69437999014e-3 a = 6378137.0 #At a rate of 1 hz, create an rtklib message for each socket and publish #the data over while not rospy.is_shutdown(): #Create and RTKLIB and a NavSatFix data structure rtk = rtklib() navsat = NavSatFix() #Set the data structure header time to the current system time navsat.header.stamp = rospy.Time.now() rtk.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = socket.recv(1024) #Split the message msg = msgStr.split() #Save the latitude, longitude and ellipsoid height navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) #Save the position covariance navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN #Compute the radius of curvature in the N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) #Compute and store the ECEF position rtk.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) rtk.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) rtk.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) rtk.delay = float(msg[13]) rtk.ftest = float(msg[14]) #Store the NavSatFix data rtk.state = navsat pubs[i].publish(rtk) rate.sleep()
def init_path(): # logitude, latitute, altitute, orientation (+deg) gpsPub = rospy.Publisher("/fix_path_init", NavSatFix, queue_size=10) wpSub = rospy.Subscriber("/gps_path_init", Odometry, utm_cb) navStatus = NavSatStatus() navStatus.service = 1 navStatus.status = 1 # waypoints hardcoded for now, may want to spec with argument in future num_wp = gps_waypoints.shape[0] hdop = 1.5 position_covariance_type = 1 global wp_num for i in range(0, num_wp): # send gps waypoint for conversion header = Header() header.stamp = rospy.Time.now() header.frame_id = 'base_link' msgOut = NavSatFix() msgOut.position_covariance[0] = hdop**2 msgOut.position_covariance[4] = hdop**2 msgOut.position_covariance[8] = (2 * hdop)**2 msgOut.longitude = gps_waypoints[i][0] msgOut.latitude = gps_waypoints[i][1] msgOut.altitude = gps_waypoints[i][2] msgOut.position_covariance_type = position_covariance_type msgOut.status = navStatus msgOut.header = header wp_num = i gpsPub.publish(msgOut)
def step(self): stamp = super().step() if not stamp: return if self.__gps_publisher.get_subscription_count() > 0 or \ self.__speed_publisher.get_subscription_count() > 0 or \ self._always_publish: self._wb_device.enable(self._timestep) msg = Float32() msg.data = self._wb_device.getSpeed() self.__speed_publisher.publish(msg) if self.__coordinate_system == GPS.WGS84: msg = NavSatFix() msg.header.stamp = stamp msg.header.frame_id = self._frame_id msg.latitude = self._wb_device.getValues()[0] msg.longitude = self._wb_device.getValues()[1] msg.altitude = self._wb_device.getValues()[2] msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN msg.status.service = NavSatStatus.SERVICE_GPS self.__gps_publisher.publish(msg) else: msg = PointStamped() msg.header.stamp = stamp msg.header.frame_id = self._frame_id msg.point.x = self._wb_device.getValues()[0] msg.point.y = self._wb_device.getValues()[1] msg.point.z = self._wb_device.getValues()[2] self.__gps_publisher.publish(msg) else: self._wb_device.disable()
def bestpos_handler(self, bestpos): navsat = NavSatFix() # TODO: The timestamp here should come from SPAN, not the ROS system time. navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame # Assume GPS - this isn't exposed navsat.status.service = NavSatStatus.SERVICE_GPS position_type_to_status = { BESTPOS.POSITION_TYPE_NONE: NavSatStatus.STATUS_NO_FIX, BESTPOS.POSITION_TYPE_FIXED: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_FIXEDHEIGHT: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_FLOATCONV: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_WIDELANE: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_NARROWLANE: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_DOPPLER_VELOCITY: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_SINGLE: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_WAAS: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_PROPAGATED: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_OMNISTAR: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_L1_FLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_IONOFREE_FLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_NARROW_FLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_L1_INT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_WIDE_INT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_NARROW_INT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_RTK_DIRECT_INS: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_SBAS: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_INS_PSRSP: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_RTKFLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_RTKFIXED: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_OMNISTAR: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_OMNISTAR_HP: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_OMNISTAR_XP: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_OMNISTAR_HP: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_OMNISTAR_XP: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_PPP: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_INS_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_INS_PPP: NavSatStatus.STATUS_SBAS_FIX, } navsat.status.status = position_type_to_status.get(bestpos.position_type, NavSatStatus.STATUS_NO_FIX) # Position in degrees. navsat.latitude = bestpos.latitude navsat.longitude = bestpos.longitude # Altitude in metres. navsat.altitude = bestpos.altitude navsat.position_covariance[0] = pow(2, bestpos.latitude_std) navsat.position_covariance[4] = pow(2, bestpos.longitude_std) navsat.position_covariance[8] = pow(2, bestpos.altitude_std) navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN # Ship ito self.pub_navsatfix.publish(navsat)
def callback(sensorstr): data = sensorstr.data if data[0] == 'Z': data = data.replace("Z","").replace("\n","").replace("\r","") data_list = data.split(',') if len(data_list) == 4: try: ##data_list structure: lat, lon, heading, vel float_list = [float(i) for i in data_list] poseGPS = NavSatFix() poseGPS.header.frame_id = "world" poseGPS.header.stamp = rospy.Time.now() poseGPS.status.status = 0 poseGPS.status.service = 1 poseGPS.latitude = float_list[0] poseGPS.longitude = float_list[1] poseGPS.altitude = 0.0 poseGPS.position_covariance = [3.24, 0, 0, 0, 3,24, 0, 0, 0, 0] poseGPS.position_covariance_type = 2 try: pubGPS.publish(poseGPS) log = "GPS Pose Data: %f %f Publish at Time: %s" % (float_list[0], float_list[1], rospy.get_time()) except: log = "poseGPS Publisher Error" except: log = "GPS Data Error! Data : %s" % data rospy.loginfo(log) rospy.loginfo(log)
def 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 main(): rospy.init_node('SensorLocalisationReciever', anonymous=True) print("Running SensorLocalisationReciever") # ser = serial.Serial(rospy.get_param('~sensorserialport','/dev/ttyACM0')) imu_data = Imu() gps_data = NavSatFix() imu_data.header.frame_id = 'base_link' gps_data.header.frame_id = 'map' rate = rospy.Rate(rospy.get_param("~rate", 100)) imu_pub = rospy.Publisher('/imu/data', Imu, queue_size=10) gps_pub = rospy.Publisher('/gps/fix', NavSatFix, queue_size=10) while not rospy.is_shutdown(): # print(x,y) # data = ser.readline() # if data[0]=='#': # data = data[1:] # linear,angular = data.split('/')[0],data.split('/')[1] linear = [float(x) for x in linear.split(' ')] angular = [float(x) for x in angular.split(' ')] imu_data.orientation.x = 1 ##angular[3] imu_data.orientation.y = 1 ## angular[4] imu_data.orientation.z = 1 ##angular[5] imu_data.linear_acceleration.x = 1 # linear[6] imu_data.linear_acceleration.y = 1 # linear[7] imu_data.linear_acceleration.z = 1 #linear[8] imu_data.angular_velocity_covariance = [ 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0 ] imu_data.linear_acceleration_covariance = [ 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0 ] gps_data.latitude = 1 # linear[0] gps_data.longitude = 1 #linear[1] gps_data.position_covariance = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0] gps_data.position_covariance_type = 3 gps_data.header.stamp = imu_data.header.stamp = rospy.Time.now() imu_pub.publish(imu_data) gps_pub.publish(gps_data) print(linear) # print(angular) rate.sleep()
def timer_gps_clk_(self): self.update_status_() msg = NavSatFix() msg.latitude = self.status['location']['latitude'] msg.longitude = self.status['location']['longitude'] msg.position_covariance_type = 0 msg.header.frame_id = 'gps' self.gps_pub_.publish(msg)
def handle(self): # self.request is the TCP socket connected to the client global pub_location latlng = NavSatFix() self.data = self.request.recv(1024).strip() data = self.data.split(',') if data[0] == 'L': latlng.header.stamp = rospy.Time.now() latlng.header.frame_id = "world" latlng.latitude = float(data[1]) latlng.longitude = float(data[2]) latlng.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN print "Location: " + data[0] + ' ' + data[1] + ", " + data[2] elif data[0] == 'R': latlng.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED print "Request Release" pub_location.publish(latlng)
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 callback_gps(data): gps_data = NavSatFix() gps_data.header.frame_id = "map" gps_data.latitude = data.x gps_data.longitude = data.y gps_data.position_covariance = [1e-3, 0, 0, 0, 1e-3, 0, 0, 0, 1e-3] gps_data.position_covariance_type = 3 gps_data.header.stamp = rospy.Time.now() pub_gps.publish(gps_data)
def callback_sbp_pos(self, msg, **metadata): if self.debug: rospy.loginfo("Received SBP_MSG_POS_LLH (Sender: %d): %s" % (msg.sender, repr(msg))) out = NavSatFix() out.header.frame_id = self.frame_id out.header.stamp = rospy.Time.now() out.status.service = NavSatStatus.SERVICE_GPS out.latitude = msg.lat out.longitude = msg.lon out.altitude = msg.height out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED if msg.flags & 0x03: self.last_rtk_time = time.time() self.rtk_fix_mode = msg.flags & 0x03 out.status.status = NavSatStatus.STATUS_GBAS_FIX # TODO this should probably also include covariance of base fix? out.position_covariance[0] = self.rtk_h_accuracy**2 out.position_covariance[4] = self.rtk_h_accuracy**2 out.position_covariance[8] = self.rtk_v_accuracy**2 pub = self.pub_rtk_fix self.last_rtk_pos = msg # If we are getting this message, RTK is our best fix, so publish this as our best fix. self.pub_fix.publish(out) else: self.last_spp_time = time.time() out.status.status = NavSatStatus.STATUS_FIX # TODO hack, piksi should provide these numbers if self.last_dops: out.position_covariance[0] = (self.last_dops.hdop * 1E-2)**2 out.position_covariance[4] = (self.last_dops.hdop * 1E-2)**2 out.position_covariance[8] = (self.last_dops.vdop * 1E-2)**2 else: out.position_covariance[0] = COV_NOT_MEASURED out.position_covariance[4] = COV_NOT_MEASURED out.position_covariance[8] = COV_NOT_MEASURED pub = self.pub_spp_fix self.last_pos = msg # Check if SPP is currently our best available fix if self.rtk_fix_mode <= 0: self.pub_fix.publish(out) pub.publish(out)
def 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 ExtFix2Fix(data): fix = NavSatFix() fix.header = data.header fix.status.status = data.status.status fix.status.service = data.status.position_source fix.latitude = data.latitude fix.longitude = data.longitude fix.altitude = data.altitude fix.position_covariance = data.position_covariance fix.position_covariance_type = data.position_covariance_type return fix
def ExtFix2Fix(data): fix = NavSatFix() fix.header = data.header fix.status.status = data.status.status fix.status.service = data.status.position_source fix.latitude = data.latitude fix.longitude = data.longitude fix.altitude = data.altitude fix.position_covariance = data.position_covariance fix.position_covariance_type = data.position_covariance_type return fix
def callback(self): rate = rospy.Rate(10) ports = [5801,5802,5803] sockets = [] pubs = [self.static_pub, self.rtkStatic_pub, self.rtkDynamic_pub] for i in ports: sock = socket.socket() host = socket.gethostname() sock.connect((host, i)) sockets.append(sock) e2 = 6.69437999014e-3 a = 6378137.0 while not rospy.is_shutdown(): for i in range(len(sockets)): navsat = NavSatFix() ecef_xyz = Point() ecef_pose = Pose() #ecef_stampedPose = PoseStamped() navsat.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = sockets[i].recv(1024) #Split the message msg = msgStr.split() navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) ecef_pose.position = ecef_xyz #ecef_stampedPose.pose = ecef_pose pubs[i].publish(navsat) rate.sleep()
def 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 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 msg_dict_to_NavSatFix(msg_dict): msg = NavSatFix() msg.header = Header() msg.header.stamp = rospy.Time.now() msg.header.frame_id = msg_dict['header']['frame_id'] msg.latitude = msg_dict['latitude'] msg.longitude = msg_dict['longitude'] msg.altitude = msg_dict['altitude'] msg.status.status = msg_dict['status']['status'] msg.status.service = msg_dict['status']['service'] msg.position_covariance = msg_dict['position_covariance'] msg.position_covariance_type = msg_dict['position_covariance_type'] return 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 pub_gps(msg_type, msg, bridge): pub = bridge.get_ros_pub("gps", NavSatFix, queue_size=1) fix = NavSatFix() fix.header.stamp = bridge.project_ap_time(msg) fix.header.frame_id = 'base_footprint' fix.latitude = msg.lat / 1e07 fix.longitude = msg.lon / 1e07 # NOTE: absolute (MSL) altitude fix.altitude = msg.alt / 1e03 fix.status.status = NavSatStatus.STATUS_FIX fix.status.service = NavSatStatus.SERVICE_GPS fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN pub.publish(fix)
def pub_gps(msg_type, msg, bridge): pub = bridge.get_ros_pub("gps", NavSatFix, queue_size=1) fix = NavSatFix() fix.header.stamp = bridge.project_ap_time(msg) fix.header.frame_id = 'base_footprint' fix.latitude = msg.lat/1e07 fix.longitude = msg.lon/1e07 # NOTE: absolute (MSL) altitude fix.altitude = msg.alt/1e03 fix.status.status = NavSatStatus.STATUS_FIX fix.status.service = NavSatStatus.SERVICE_GPS fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN pub.publish(fix)
def talker(): pub = rospy.Publisher('andro2linux_gps', NavSatFix, queue_size=10) rospy.init_node('andro2linux_gps_publisher', anonymous=True) # socket type (UDP = SOCK_DGRAM, TCP = SOCK_STREAM) s = socket(AF_INET, SOCK_DGRAM) # port setup s.bind(('', ECHO_PORT)) # ready! print('udp echo server ready') # infinite loop while 1: # Waiting for client data, addr = s.recvfrom(BUFSIZE) # extract the information from fused provided splitted_data = str(data).split("\n") provider = splitted_data[0].split()[1] if provider == 'fused': gpsmsg = NavSatFix() gpsmsg.header.stamp = rospy.Time.now() gpsmsg.header.frame_id = "gps" #print(splitted_data) longitude = float(splitted_data[1].split()[1]) latitude = float(splitted_data[2].split()[1]) altitude = float(splitted_data[3].split()[1]) accuracy = float(splitted_data[4].split()[1]) variance = accuracy * accuracy hasspeed = splitted_data[5].split()[1] == 'true' gpsmsg.latitude = latitude gpsmsg.longitude = longitude gpsmsg.altitude = altitude gpsmsg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED gpsmsg.position_covariance = (variance, 0, 0, 0, variance, 0, 0, 0, variance) print(gpsmsg) print("------------------------") # publish pub.publish(gpsmsg) if rospy.is_shutdown(): break # send back to client s.sendto(data, addr)
def set_destination(self, destination): msg = NavSatFix() msg.header.stamp = rospy.get_rostime() msg.header.frame_id = "utm" msg.latitude = float(destination['latitude']) msg.longitude = float(destination['longitude']) msg.altitude = 0 msg.status.status = NavSatStatus.STATUS_GBAS_FIX msg.status.service = 0 msg.position_covariance = [0] * 9 msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self._goalfixpub.publish(msg) ret = {'method': "set_destination", 'msg': "set"} return ret
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 save_gps_bag(frame_id, seq, seconds, nanoseconds, status, service, latitude, longitude, altitude, position_covariance, position_covariance_type): ros_gps = NavSatFix() ros_gps.header.seq = seq ros_gps.header.stamp.secs = seconds ros_gps.header.stamp.nsecs = nanoseconds ros_gps.header.frame_id = frame_id gps_GGA_topic = "/gps/fix" ros_gps.status.status = status ros_gps.status.service = service ros_gps.latitude = latitude ros_gps.longitude = longitude ros_gps.altitude = altitude ros_gps.position_covariance = position_covariance ros_gps.position_covariance_type = position_covariance_type bag.write(gps_GGA_topic, ros_gps, ros_gps.header.stamp)
def publish(self, data): msg = NavSatFix() msg.header.frame_id = self._frameId msg.header.stamp = rospy.get_rostime() msg.latitude = data.getLat() msg.longitude = data.getLng() msg.altitude = data.getMeters() if data.getFix() == 1: msg.status.status = 0 else: msg.status.status = -1 msg.position_covariance_type = 1 msg.position_covariance[0] = data.getHDOP() * data.getHDOP() msg.position_covariance[4] = data.getHDOP() * data.getHDOP() msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP() msg.status.service = 1 self._pub.publish(msg)
def subCB(msg_in): global pub msg_out = NavSatFix() msg_out.header = msg_in.header msg_out.status.status = NavSatStatus.STATUS_FIX # TODO - fix this msg_out.status.service = NavSatStatus.SERVICE_GPS msg_out.latitude = msg_in.LLA.x msg_out.longitude = msg_in.LLA.y msg_out.altitude = msg_in.LLA.z msg_out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED msg_out.position_covariance[1] = msg_in.PosUncerainty pub.publish(msg_out)
def subCB(msg_in): global pub msg_out = NavSatFix() msg_out.header = msg_in.header msg_out.status.status = NavSatStatus.STATUS_FIX # TODO - fix this msg_out.status.service = NavSatStatus.SERVICE_GPS msg_out.latitude = msg_in.LLA.x msg_out.longitude = msg_in.LLA.y msg_out.altitude = msg_in.LLA.z msg_out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED msg_out.position_covariance[1] = msg_in.PosUncerainty pub.publish(msg_out)
def _gps_pub(self): if self.info_list[2] == '': rospy.loginfo("[%s] No data receiving ... Try go outside?" % (self.node_name)) return # info format: # GPGGA UTC_time latitude N/S longitude E/W state number_of_sat HDOP altitude others fix = NavSatFix() fix.header.seq = self.seq # Use received data #fix.header.stamp = ToTimeFormat(self.info_list[1]) # Use rospy.Time.now fix.header.stamp = rospy.Time.now() fix.header.frame_id = 'gps_link' lat = float(Sexagesimal2Decimal(self.info_list[2])) longi = float(Sexagesimal2Decimal(self.info_list[4])) if self.info_list[3] == 'S': fix.latitude = -lat else: fix.latitude = lat if self.info_list[5] == 'W': fix.longitude = -longi else: fix.longitude = longi rospy.loginfo( "[%s] %s %s %s %s" % (self.node_name, self.info_list[3], lat, self.info_list[5], longi)) # Covariance matrix # Refer to nmea_navsat_driver try: hdop = float(self.info_list[8]) except ValueError: pass fix.position_covariance[0] = hdop**2 fix.position_covariance[4] = hdop**2 fix.position_covariance[8] = (2 * hdop)**2 fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED fix.altitude = float(self.info_list[9]) + float(self.info_list[11]) self.pub_fix.publish(fix) self.info = "" self.info_list = None self.seq += 1
def do_fix(): global path, gps_err_x, gps_err_y rate = rospy.Rate(1) while not rospy.is_shutdown(): gps_err_x, gps_err_y = random.choice(GPS_ERR_V, p=GPS_ERR_P[param_gps_err], size=(2)) gps_err = sqrt(gps_err_x**2 + gps_err_y**2) with lock: now = rospy.Time.now() x, y = pos_x + gps_err_x, pos_y + gps_err_y m = NavSatFix() m.header.stamp = now m.header.frame_id = base_frame m.status.status = 0 m.status.service = 1 m.latitude, m.longitude = utm.to_latlon(x, y, 30, northern=True) m.altitude = 0 m.position_covariance = [(2 * gps_err)**2, 0.0, 0.0, 0.0, (2 * gps_err)**2, 0.0, 0.0, 0.0, 10.0] m.position_covariance_type = 3 n = PoseStamped() n.header.stamp = now n.header.frame_id = map_frame n.pose.position = Point(x - pos_x0, y - pos_y0, 0) q = quaternion_from_euler(0, 0, orientation) n.pose.orientation = Quaternion(*q) path.poses.append(n) path.poses = path.poses[-param_path_length:] path.header.stamp = now if param_publish_fix: pub_fix.publish(m) if param_publish_path: pub_path.publish(path) rate.sleep()
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 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 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 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 gps2navsat(filename, bag): with open(filename, 'r') as file: try: while True: data = struct.unpack('qddd', file.read(8*4)) time = data[0] local = data[1:] lat_lon_el_theta = struct.unpack('dddd', file.read(8*4)) gps_cov = struct.unpack('dddddddddddddddd', file.read(8*16)) if abs(lat_lon_el_theta[0]) < 1e-1: continue navsat = NavSatFix() navsat.header.frame_id = 'gps' navsat.header.stamp = rospy.Time.from_sec(time * 1e-6) navsat.status.status = NavSatStatus.STATUS_FIX navsat.status.service = NavSatStatus.SERVICE_GPS navsat.latitude = lat_lon_el_theta[0] navsat.longitude = lat_lon_el_theta[1] navsat.altitude = lat_lon_el_theta[2] navsat.position_covariance = numpy.array(gps_cov).reshape(4, 4)[:3, :3].flatten().tolist() navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN bag.write('/gps/fix', navsat, navsat.header.stamp) geopoint = GeoPointStamped() geopoint.header = navsat.header geopoint.position.latitude = lat_lon_el_theta[0] geopoint.position.longitude = lat_lon_el_theta[1] geopoint.position.altitude = lat_lon_el_theta[2] bag.write('/gps/geopoint', geopoint, geopoint.header.stamp) except: print 'done'
def 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 publish(self, data): now = int(round(time.time() * 1000)) msg = NavSatFix() msg.header.frame_id = self._frameId msg.header.stamp = rospy.get_rostime() msg.latitude = data.getLat() msg.longitude = data.getLng() msg.altitude = data.getMeters() cur_fix = data.getFix() #print cur_fix if (cur_fix != self._old_fix): msg.status.status = 0 self._old_fix = cur_fix self._wd = now elif (now - self._wd) < GPS_WD_TIMEOUT: msg.status.status = 0 else: msg.status.status = -1 msg.position_covariance_type = 1 msg.position_covariance[0] = data.getHDOP() * data.getHDOP() msg.position_covariance[4] = data.getHDOP() * data.getHDOP() msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP() msg.status.service = 1 self._pub.publish(msg)
def navigation_handler(self, data): """ Rebroadcasts navigation data in the following formats: 1) /odom => /base footprint transform (if enabled, as per REP 105) 2) Odometry message, with parent/child IDs as in #1 3) NavSatFix message, for systems which are knowledgeable about GPS stuff 4) IMU messages """ rospy.logdebug("Navigation received") # If we don't have a fix, don't publish anything... if self.nav_status.status == NavSatStatus.STATUS_NO_FIX: return orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(data.pitch), RAD(data.heading)).GetQuaternion() # UTM conversion (zone, easting, northing) = LLtoUTM(23, data.latitude, data.longitude) # Initialize starting point if we haven't yet # TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid? if not self.init and self.zero_start: self.origin.x = easting self.origin.y = northing self.init = True # Publish origin reference for others to know about p = Pose() p.position.x = self.origin.x p.position.y = self.origin.y self.pub_origin.publish(p) # # Odometry # TODO: Work out these covariances properly from DOP # odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = self.odom_frame odom.child_frame_id = self.base_frame odom.pose.pose.position.x = easting - self.origin.x odom.pose.pose.position.y = northing - self.origin.y odom.pose.pose.position.z = data.altitude odom.pose.pose.orientation = Quaternion(*orient) odom.pose.covariance = POSE_COVAR # Twist is relative to /vehicle frame odom.twist.twist.linear.x = data.speed odom.twist.twist.linear.y = 0 odom.twist.twist.linear.z = -data.down_vel odom.twist.twist.angular.x = RAD(data.ang_rate_long) odom.twist.twist.angular.y = RAD(-data.ang_rate_trans) odom.twist.twist.angular.z = RAD(-data.ang_rate_down) odom.twist.covariance = TWIST_COVAR self.pub_odom.publish(odom) # # Odometry transform (if required) # if self.publish_tf: self.tf_broadcast.sendTransform( (odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z), Quaternion(*orient), odom.header.stamp,odom.child_frame_id, odom.frame_id) # # NavSatFix # TODO: Work out these covariances properly from DOP # navsat = NavSatFix() navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame navsat.status = self.nav_status navsat.latitude = data.latitude navsat.longitude = data.longitude navsat.altitude = data.altitude navsat.position_covariance = NAVSAT_COVAR navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self.pub_navsatfix.publish(navsat) # # IMU # TODO: Work out these covariances properly # imu = Imu() imu.header.stamp == rospy.Time.now() imu.header.frame_id = self.base_frame # Orientation imu.orientation = Quaternion(*orient) imu.orientation_covariance = IMU_ORIENT_COVAR # Angular rates imu.angular_velocity.x = RAD(data.ang_rate_long) imu.angular_velocity.y = RAD(-data.ang_rate_trans) imu.angular_velocity.y = RAD(-data.ang_rate_down) imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration imu.linear_acceleration.x = data.long_accel imu.linear_acceleration.y = data.trans_accel imu.linear_acceleration.z = data.down_accel imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu) pass
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. \ Sentence was: %s" % 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
gpstime.header.stamp = gpsVel.header.stamp gpstime.time_ref = convertNMEATimeToROS(fields[1]) longitude = float(fields[5][0:3]) + float(fields[5][3:])/60 if fields[6] == 'W': longitude = -longitude latitude = float(fields[3][0:2]) + float(fields[3][2:])/60 if fields[4] == 'S': latitude = -latitude #publish data navData.latitude = latitude navData.longitude = longitude navData.altitude = float('NaN') navData.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN gpspub.publish(navData) gpstimePub.publish(gpstime) else: pass #print data else: #Use GGA #No /vel output from just GGA if 'GGA' in fields[0]: gps_quality = int(fields[6]) if gps_quality == 0: navData.status.status = NavSatStatus.STATUS_NO_FIX elif gps_quality == 1: navData.status.status = NavSatStatus.STATUS_FIX elif gps_quality == 2:
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
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 = libjavad_navsat_driver.parser.parse_nmea_sentence(nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude hdop = data['hdop'] current_fix.position_covariance[0] = hdop ** 2 current_fix.position_covariance[4] = hdop ** 2 current_fix.position_covariance[8] = (2 * hdop) ** 2 # FIXME current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level #altitude = data['altitude'] + data['mean_sea_level'] altitude = data['altitude'] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.time_ref_pub.publish(current_time_ref) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) current_vel.twist.angular.z = data['true_course'] self.vel_pub.publish(current_vel) else: return False
def talker(): pubGPS = rospy.Publisher('poseGPS', NavSatFix, queue_size=1000) pubIMU = rospy.Publisher('imuBoat', Imu, queue_size=1000) pubTwist = rospy.Publisher('twistGPS', TwistWithCovarianceStamped, queue_size=1000) rospy.init_node('imugpspublisher', anonymous=True) rate = rospy.Rate(100) # 100hz while not rospy.is_shutdown(): data = ser.readline() if data[0] == 'Z': data = data.replace("Z","").replace("\n","").replace("\r","") data_list = data.split(',') if len(data_list) == 4: try: ##data_list structure: lat, lon, heading, vel float_list = [float(i) for i in data_list] poseGPS = NavSatFix() poseGPS.header.frame_id = "world" poseGPS.header.stamp = rospy.Time.now() poseGPS.status.status = 0 poseGPS.status.service = 1 poseGPS.latitude = float_list[0] poseGPS.longitude = float_list[1] poseGPS.altitude = 0.0 poseGPS.position_covariance = [3.24, 0, 0, 0, 3,24, 0, 0, 0, 0] poseGPS.position_covariance_type = 2 pubGPS.publish(poseGPS) twistGPS = TwistWithCovarianceStamped() twistGPS.header = poseGPS.header twistGPS.twist.twist.linear.x = float_list[3]*knots*math.cos(latest_yaw) twistGPS.twist.twist.linear.y = float_list[3]*knots*math.sin(latest_yaw) twistGPS.twist.twist.linear.z = 0.0 ##angular data not used here twistGPS.twist.covariance = [0.01, 0.01, 0, 0, 0, 0] pubTwist.publish(twistGPS) log = "GPS Data: %f %f %f %f Publish at Time: %s" % (float_list[0], float_list[1], float_list[2], float_list[3], rospy.get_time()) except: log = "GPS Data Error! Data : %s" % data else: log = "GPS Data Error! Data : %s" % data rospy.loginfo(log) elif data[0] == 'Y': data = data.replace("Y","").replace("\n","").replace("\r","") data_list = data.split(',') if len(data_list) == 9: try: ##data_list structure: accel, magni, gyro float_list = [float(i) for i in data_list] imuData = Imu() imuData.header.frame_id = "base_link" imuData.header.stamp = rospy.Time.now() latest_yaw = float_list[3] ##data form in yaw, pitch, roll quat = tf.transformations.quaternion_from_euler(float_list[3], float_list[4], float_list[5], 'rzyx') imuData.orientation.x = quat[0] imuData.orientation.y = quat[1] imuData.orientation.z = quat[2] imuData.orientation.w = quat[3] imuData.angular_velocity.x = math.radians(float_list[6]*gyro_scale) imuData.angular_velocity.y = math.radians(-float_list[7]*gyro_scale) imuData.angular_velocity.z = math.radians(-float_list[8]*gyro_scale) imuData.linear_acceleration.x = float_list[0]*accel_scale imuData.linear_acceleration.y = -float_list[1]*accel_scale imuData.linear_acceleration.z = -float_list[2]*accel_scale imuData.orientation_covariance = [1.5838e-6, 0, 0, 0, 1.49402e-6, 0, 0, 0, 1.88934e-6] imuData.angular_velocity_covariance = [7.84113e-7, 0, 0, 0, 5.89609e-7, 0, 0, 0, 6.20293e-7] imuData.linear_acceleration_covariance = [9.8492e-4, 0, 0, 0, 7.10809e-4, 0, 0, 0, 1.42516e-3] pubIMU.publish(imuData) log = "IMU Data: %f %f %f %f %f %f %f %f %f Publish at Time: %s" \ % (imuData.linear_acceleration.x, imuData.linear_acceleration.y, imuData.linear_acceleration.z, float_list[3], float_list[4], float_list[5], imuData.angular_velocity.x, imuData.angular_velocity.y, imuData.angular_velocity.z, rospy.get_time()) except: log = "IMU Data Error! Data : %s" % data else: log = "Data Error! Data : %s" % data rospy.loginfo(log) else: log = "Data Error or Message: %s" % data rospy.loginfo(log) rate.sleep()
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)