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 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 got_position(xx, yy, aa, stamp): global EP fix = NavSatFix() fix.header.stamp = stamp fix.header.frame_id = "sim_gps" fix.status.status = 0 fix.status.service = 1 utm_e = base_xx + base_utm_e + xx + CONFIG["SIM_ERROR"] * cos((yy + time()) / EP) utm_n = base_yy + base_utm_n + yy + CONFIG["SIM_ERROR"] * cos((utm_e / 3.14) / EP) (lon, lat) = conv(utm_e, utm_n, inverse=True) fix.latitude = lat fix.longitude = lon fix.altitude = 1.3 # print "got_position: %f %f" % (lat, lon) gps.publish(fix) pos.publish(Pose2D(utm_e, utm_n, aa)) tfBr.sendTransform( (utm_e - base_xx, utm_n - base_yy, 0), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/map", "/gps", )
def 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 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 talker(): UDP_IP = "192.168.0.107" UDP_PORT = 9002 sock = socket.socket(socket.AF_INET, # Internet socket.SOCK_DGRAM) # UDP sock.bind((UDP_IP, UDP_PORT)) pub = rospy.Publisher('gps/fix', NavSatFix, queue_size=10) rospy.init_node('test_relay', anonymous=True) rate = rospy.Rate(100) # 10hz while not rospy.is_shutdown(): data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes print "received message:", data data = re.sub("[A-Za-z]","",str(data)) result = str(data).split(":") print result rospy.loginfo(result) msg = NavSatFix() msg.header = Header() msg.latitude = float(result[1]) msg.longitude = float(result[2]) #rospy.loginfo(msg) pub.publish(msg)
def publish_gps(self): if self.drone.positionGPS!=None: gps_data = NavSatFix() gps_data.latitude = self.drone.positionGPS[0] gps_data.longitude = self.drone.positionGPS[1]#maybe reversed? gps_data.altitude = self.drone.positionGPS[2] gps_data.header.frame_id = self.unique_id self.gps_pub.publish(gps_data)
def joy_callback(msg): if msg.buttons[12] == 1: gps_goal = NavSatFix() gps_goal.latitude = 33.726526 gps_goal.longitude = -83.299984 gps_put.publish(gps_goal) if msg.buttons[14] == 1: client.cancel_all_goals()
def gps(): print utm.conversion.R import roslib; roslib.load_manifest('ardrone_control') import rospy; global rospy from sensor_msgs.msg import NavSatFix # rospy.init_node('test') gps = GPS() #print gps m = NavSatFix() m.latitude = 45.0 m.altitude = utm.conversion.R m.longitude = 20.0 gps.set_zero(m) gps.measure(m) print gps.get_state() print gps.Z m.latitude = 45.1 m.altitude = utm.conversion.R m.longitude = 20.0 gps.measure(m) print gps.get_state() print gps.Z gps.set_zero_yaw( 30 * pi/180 ) gps.measure(m) print gps.get_state() print gps.get_measurement() print gps.Z for i in range(10000): gps.measure(m) gps.Broadcast() rospy.spin()
def default(self, ci='unused'): gps = NavSatFix() gps.header = self.get_ros_header() gps.latitude = self.data['x'] gps.longitude = self.data['y'] gps.altitude = self.data['z'] self.publish(gps)
def getMsg(): pub_navsat = rospy.Publisher('/rtklib/rover', NavSatFix) pub_pose = rospy.Publisher('/rtklib/pose', PoseStamped) #Initialize the RTKLIB ROS node rospy.init_node('rtklib_messages', anonymous=True) #Define the publishing frequency of the node rate = rospy.Rate(10) #Create a socket sock = socket.socket() #Get the address of the local host host = socket.gethostname() #Connect to the RTKRCV server that is bound to port xxxx port = 5801 sock.connect((host,port)) e2 = 6.69437999014e-3 a = 6378137.0 while not rospy.is_shutdown(): navsat = NavSatFix() ecef_xyz = Point() ecef_pose = Pose() ecef_stampedPose = PoseStamped() ecef_stampedPose = navsat.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = sock.recv(1024) #Split the message msg = msgStr.split() navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) ecef_pose.position = ecef_xyz ecef_stampedPose.pose = ecef_pose pub_navsat.publish(navsat) pub_pose.publish(ecef_stampedPose) rate.sleep()
def 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_sbp_pos(self, msg, **metadata): if self.debug: rospy.loginfo("Received SBP_MSG_POS_LLH (Sender: %d): %s" % (msg.sender, repr(msg))) out = NavSatFix() out.header.frame_id = self.frame_id out.header.stamp = rospy.Time.now() out.status.service = NavSatStatus.SERVICE_GPS out.latitude = msg.lat out.longitude = msg.lon out.altitude = msg.height out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED if msg.flags & 0x03: self.last_rtk_time = time.time() self.rtk_fix_mode = msg.flags & 0x03 out.status.status = NavSatStatus.STATUS_GBAS_FIX # TODO this should probably also include covariance of base fix? out.position_covariance[0] = self.rtk_h_accuracy**2 out.position_covariance[4] = self.rtk_h_accuracy**2 out.position_covariance[8] = self.rtk_v_accuracy**2 pub = self.pub_rtk_fix self.last_rtk_pos = msg # If we are getting this message, RTK is our best fix, so publish this as our best fix. self.pub_fix.publish(out) else: self.last_spp_time = time.time() out.status.status = NavSatStatus.STATUS_FIX # TODO hack, piksi should provide these numbers if self.last_dops: out.position_covariance[0] = (self.last_dops.hdop * 1E-2)**2 out.position_covariance[4] = (self.last_dops.hdop * 1E-2)**2 out.position_covariance[8] = (self.last_dops.vdop * 1E-2)**2 else: out.position_covariance[0] = COV_NOT_MEASURED out.position_covariance[4] = COV_NOT_MEASURED out.position_covariance[8] = COV_NOT_MEASURED pub = self.pub_spp_fix self.last_pos = msg # Check if SPP is currently our best available fix if self.rtk_fix_mode <= 0: self.pub_fix.publish(out) pub.publish(out)
def recieve_gps(self, data ): msg = NavSatFix() msg.latitude = data.vector.x msg.longitude = data.vector.y msg.altitude = data.vector.z msg.header.stamp = data.header.stamp msg.header.frame_id = data.header.frame_id self.publisher['gps'].publish(msg)
def 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 address2geo(address): gurl = 'https://maps.googleapis.com/maps/api/geocode/xml?address='+address res = urllib.urlopen(gurl).read() ans = NavSatFix() i1 = res.find('lat') i2 = res.find('/lat',i1) ans.latitude = float(res[i1+4:i2-1]) i1 = res.find('lng') i2 = res.find('/lng',i1) ans.longitude = float(res[i1+4:i2-1]) return ans
def default(self, ci='unused'): """ Publish the data of the gps sensor as a custom ROS NavSatFix message """ gps = NavSatFix() gps.header = self.get_ros_header() # TODO ros.org/doc/api/sensor_msgs/html/msg/NavSatFix.html gps.latitude = self.data['x'] gps.longitude = self.data['y'] gps.altitude = self.data['z'] self.publish(pose)
def callback(self): rate = rospy.Rate(10) ports = [5801,5802,5803] sockets = [] pubs = [self.static_pub, self.rtkStatic_pub, self.rtkDynamic_pub] for i in ports: sock = socket.socket() host = socket.gethostname() sock.connect((host, i)) sockets.append(sock) e2 = 6.69437999014e-3 a = 6378137.0 while not rospy.is_shutdown(): for i in range(len(sockets)): navsat = NavSatFix() ecef_xyz = Point() ecef_pose = Pose() #ecef_stampedPose = PoseStamped() navsat.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = sockets[i].recv(1024) #Split the message msg = msgStr.split() navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) ecef_pose.position = ecef_xyz #ecef_stampedPose.pose = ecef_pose pubs[i].publish(navsat) rate.sleep()
def gps_fix_sim(): pub=rospy.Publisher('sensor_msgs/NavSatFix', NavSatFix, queue_size=10) rospy.init_node('gps_sim', anonymous=True) rate = rospy.Rate(2) while not rospy.is_shutdown(): gps_fix = NavSatFix() gps_fix.latitude = 36.6 gps_fix.longitude = -121.1 gps_fix.altitude = 1.0 pub.publish(gps_fix) rate.sleep()
def pub_gps(msg_type, msg, bridge): pub = bridge.get_ros_pub("gps", NavSatFix, queue_size=1) fix = NavSatFix() fix.header.stamp = bridge.project_ap_time(msg) fix.header.frame_id = 'base_footprint' fix.latitude = msg.lat/1e07 fix.longitude = msg.lon/1e07 # NOTE: absolute (MSL) altitude fix.altitude = msg.alt/1e03 fix.status.status = NavSatStatus.STATUS_FIX fix.status.service = NavSatStatus.SERVICE_GPS fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN pub.publish(fix)
def convert_gps_to_pose(self, coords): # create NavSatFix msg nav_msg = NavSatFix() nav_msg.header.stamp = rospy.get_rostime() nav_msg.header.frame_id = 'odom' # x is longitude, y is latitude nav_msg.longitude = coords['x'] nav_msg.latitude = coords['y'] # convert using /latlong_goal_node/AddLatLongGoal service # returns PoseStamped response = self.latlong_service(nav_msg) rospy.loginfo("Converted NavSatFix to PoseStamped -> (%f, %f)" % (response.goal_xy.pose.position.x, response.goal_xy.pose.position.y)) return response.goal_xy
def calc_lat_long(init_pos,distance,brng): ''' * calculate the latitude and longitude given distance and bearing * distnace input in [m] brng in [rad] ''' R = 6371.0 * 1000 p2 = NavSatFix() lat1 = init_pos.latitude*PI/180.0 lon1 = init_pos.longitude*PI/180.0 p2.latitude = (asin( sin(lat1)*cos(distance/R) + cos(lat1)*sin(distance/R)*cos(brng))) p2.longitude = (lon1 + atan2(sin(brng)*sin(distance/R)*cos(lat1),cos(distance/R)-sin(lat1)*sin(p2.latitude)))*180 / PI p2.latitude = p2.latitude *180 / PI return p2
def testAutoOriginFromNavSatFix(self): rospy.init_node('test_initialize_origin') nsf_pub = rospy.Publisher('fix', NavSatFix, queue_size=2) origin_sub = self.subscribeToOrigin() self.test_stamp = True nsf_msg = NavSatFix() nsf_msg.latitude = swri['latitude'] nsf_msg.longitude = swri['longitude'] nsf_msg.altitude = swri['altitude'] nsf_msg.header.frame_id = "/far_field" nsf_msg.header.stamp = msg_stamp r = rospy.Rate(10.0) while not rospy.is_shutdown(): nsf_pub.publish(nsf_msg) r.sleep()
def test_telemetry_serializer(self): """Tests telemetry serializer.""" # Set up test data. navsat = NavSatFix() navsat.latitude = 38.149 navsat.longitude = -76.432 navsat.altitude = 30.48 compass = Float64(90.0) json = serializers.TelemetrySerializer.from_msg(navsat, compass) altitude_msl = serializers.meters_to_feet(navsat.altitude) # Compare. self.assertEqual(json["latitude"], navsat.latitude) self.assertEqual(json["longitude"], navsat.longitude) self.assertEqual(json["altitude_msl"], altitude_msl) self.assertEqual(json["uas_heading"], compass.data)
def publish(self, data): msg = NavSatFix() msg.header.frame_id = self._frameId msg.header.stamp = rospy.get_rostime() msg.latitude = data.getLat() msg.longitude = data.getLng() msg.altitude = data.getMeters() if data.getFix() == 1: msg.status.status = 0 else: msg.status.status = -1 msg.position_covariance_type = 1 msg.position_covariance[0] = data.getHDOP() * data.getHDOP() msg.position_covariance[4] = data.getHDOP() * data.getHDOP() msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP() msg.status.service = 1 self._pub.publish(msg)
def subCB(msg_in): global pub msg_out = NavSatFix() msg_out.header = msg_in.header msg_out.status.status = NavSatStatus.STATUS_FIX # TODO - fix this msg_out.status.service = NavSatStatus.SERVICE_GPS msg_out.latitude = msg_in.LLA.x msg_out.longitude = msg_in.LLA.y msg_out.altitude = msg_in.LLA.z msg_out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED msg_out.position_covariance[1] = msg_in.PosUncerainty pub.publish(msg_out)
def get_directions_service(req): p = xy2geo(init_pos,req.start) orig = "%f,%f"%(p.latitude,p.longitude) p = xy2geo(init_pos,req.goal) dest = "%f,%f"%(p.latitude,p.longitude) print "From ", orig print "To ", dest print "--------" direction_list,status = get_directions(orig,dest) #print status res = Path() for wp in direction_list: point = NavSatFix() point.latitude = float(wp.start_location[0]) point.longitude = float(wp.start_location[1]) pos = geo2xy(init_pos,point) res.poses.append(pos) return res
def convert_to_pose(self, x, y): if not self.latlong_service: rospy.loginfo("Waiting for AddLatLongGoal service...") rospy.wait_for_service('/latlong_goal_node/AddLatlongGoal') rospy.loginfo("AddLatLongGoal service responded") self.latlong_service = rospy.ServiceProxy('/latlong_goal_node/AddLatlongGoal', AddLatlongGoal) # create NavSatFix msg nav_msg = NavSatFix() nav_msg.header.stamp = rospy.get_rostime() nav_msg.header.frame_id = 'odom' # x is longitude, y is latitude nav_msg.longitude = float(x) nav_msg.latitude = float(y) # convert using /latlong_goal_node/AddLatLongGoal service # returns PoseStamped response = self.latlong_service(nav_msg) rospy.loginfo("Converted NavSatFix to PoseStamped -> (%f, %f)" % (response.goal_xy.pose.position.x, response.goal_xy.pose.position.y)) return response.goal_xy.pose.position.x, response.goal_xy.pose.position.y
def test_telemetry_serializer(self): """Tests telemetry serializer.""" # Set up test data. navsat = NavSatFix() navsat.latitude = 38.149 navsat.longitude = -76.432 altitude = Altitude() altitude.amsl = 30.48 pose_stamped = PoseStamped() pose_stamped.pose.orientation.w = 1.0 data = serializers.TelemetrySerializer.from_msg(navsat, altitude, pose_stamped) altitude_msl = serializers.meters_to_feet(altitude.amsl) # Compare. self.assertEqual(data["latitude"], navsat.latitude) self.assertEqual(data["longitude"], navsat.longitude) self.assertEqual(data["altitude_msl"], altitude_msl) self.assertEqual(data["uas_heading"], 90.0)
def receiveGpsOverUdp(): pub = rospy.Publisher('/bozobox/gps/fix', NavSatFix) rospy.init_node('gps_over_udp_receiver') while not rospy.is_shutdown(): data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes #print "received message: >"+data+'<' dataArray = data.split(','); fix = NavSatFix() fix.header.stamp.secs = int(dataArray[0]) fix.header.stamp.nsecs = int(dataArray[1]) fix.latitude = float(dataArray[2]) fix.longitude = float(dataArray[3]) fix.status.status = int(dataArray[4]) print fix.latitude print fix.longitude print fix.status.status pub.publish(fix)
def time_callback(self, event): self.precord = self.gps_coord self.northang = self.northang + (self.cmd.angular.z) if self.northang > self.two_pi: self.northang = self.northang - self.two_pi if self.northang < -self.two_pi: self.northang = self.northang + self.two_pi northing_dif = (self.cmd.linear.x * 0.5) * math.cos(self.northang) easting_dif = (self.cmd.linear.x * 0.5) * math.sin(self.northang) if northing_dif != 0.0 or easting_dif != 0: self.gps_coord = self.gps_coord._get_rel_point( easting_dif, northing_dif) #print self.cmd.linear.x, self.cmd.angular.z #print self.northang, northing_dif, easting_dif #self.gps_coord.northing, self.gps_coord.easting # print self.gps_coord fix = NavSatFix() fix.latitude = self.gps_coord.lat fix.longitude = self.gps_coord.lon self.pub.publish(fix)
def send_goals(self): frame = self.web.page().currentFrame() goals = frame.documentElement().evaluateJavaScript("get_goals()") print(goals) if goals and ROS: goals_msg = GPSList() for goal in goals: fix = NavSatFix() fix.latitude = goal['lat'] fix.longitude = goal['lng'] fix.altitude = 9.3 fix.header.frame_id = '/map' fix.header.stamp = rospy.Time.now() fix.header.seq = self.seq fix.position_covariance_type = 1 self.seq += 1 goals_msg.goals.append(fix) print(goals_msg) self.goal_pub.publish(goals_msg)
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)
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 timer_callback(self, event): msg = NavSatFix() msg.header = Header() msg.header.stamp = rospy.Time.now() msg.header.frame_id = "gps" msg.status.status = NavSatStatus.STATUS_FIX msg.status.service = NavSatStatus.SERVICE_GPS # Position in degrees. msg.latitude = 59.172728 msg.longitude = 10.29502696 # Altitude in metres. msg.altitude = 3.25 msg.position_covariance[0] = 0 msg.position_covariance[4] = 0 msg.position_covariance[8] = 0 msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN self.publisher.publish(msg) self.best_pos_a = None
def publish(self): header = Header() header.stamp = rospy.Time.now() header.frame_id = "base_link" 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 if self.covariance: gpsMsg.position_covariance = self.covariance gpsMsg.position_covariance_type = self.covariance_type self.navSatPub.publish(gpsMsg) self.speedPub.publish(self.speed) self.headingPub.publish(self.heading)
def pub_gps_callback(self, event): """ This method is a callback of a timer. This publishes gps data """ gps = NavSatFix() gps.header.stamp = rospy.Time.now() gps.header.frame_id = self.robot_frame_id gps.status.status = NavSatStatus.STATUS_FIX; gps.status.service = NavSatStatus.SERVICE_GPS; gps.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN; gps.position_covariance[0] = self.gps_position_covariance[0] gps.position_covariance[4] = self.gps_position_covariance[1] gps.position_covariance[8] = self.gps_position_covariance[2] # Extract NED referenced pose north = (self.odom.pose.pose.position.y + np.random.normal(self.gps_drift[0], self.gps_position_covariance_gen[0])) east = (self.odom.pose.pose.position.x + np.random.normal(self.gps_drift[1], self.gps_position_covariance_gen[1])) # Convert coordinates lat, lon, h = self.ned.ned2geodetic(np.array([north, east, 0])) gps.latitude = lat gps.longitude = lon # Publish self.pub_gps.publish(gps)
def gps_Pos(): global source, exitFlag pos_Glob = NavSatFix() pub_Global = rospy.Publisher("/dGPS/Global", NavSatFix, queue_size=2) for msg, metadata in source.filter(SBP_MSG_POS_LLH): timestring = metadata['time'] #print "%.10f, %.10f" % (msg.lat, msg.lon) unix_Time, nano_Secs = convert_Timestamp(timestring) pos_Glob.status.status = msg.flags pos_Glob.status.service = msg.n_sats pos_Glob.header.stamp.secs = unix_Time pos_Glob.header.stamp.nsecs = nano_Secs pos_Glob.latitude = msg.lat pos_Glob.longitude = msg.lon pos_Glob.altitude = msg.height if exitFlag == False: pub_Global.publish(pos_Glob) else: sys.exit()
def transmitGPS(): ''' This function sets up the ROS node, serial connection and NMEA Parser. It then repeatedly checks for new data received on the serial line and converts this data to GPS coordinates if available. The frequency of checks depends on the global variable ROS_REFRESH_RATE. ''' gps_interface = SerialInterface("/dev/serial0", 9600, 0) msg = NavSatFix() pub = rospy.Publisher('ant_gps', NavSatFix, queue_size=10) rospy.init_node('antenna_gps', anonymous=True) rate = rospy.Rate(ROS_REFRESH_RATE) parser = NMEAParser('NMEA_0183') while not rospy.is_shutdown(): try: received_data = gps_interface.readSerialInput() # Get serial data [latitude, longitude] = parser.getGPSLocation(received_data) # Parse data and set message fields msg.latitude = latitude # Publish coordinates msg.longitude = longitude pub.publish(msg) rate.sleep() except: rospy.loginfo("NO GPS FIX") rate.sleep() pass
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 objective_node(): # node should update objective when notified by server state_objective_latitude = -37.8136 state_objective_longitude = 144.9631 # publish objective objective_publisher = rospy.Publisher("/core_rover/current_objective", NavSatFix, queue_size=1) rospy.init_node("objective_node") publish_rate = rospy.Rate(2) while not rospy.is_shutdown(): if is_ready(): rospy.loginfo("Publishing objective.") msg = NavSatFix() msg.header.stamp = rospy.get_rostime() msg.latitude = state_objective_latitude msg.longitude = state_objective_longitude objective_publisher.publish(msg) else: rospy.loginfo("Not publishing objective.") publish_rate.sleep()
def gpsfix_to_navsatfix(gpsfix_msg): # Convert gps_common/GPSFix messages to sensor_msgs/NavSatFix messages navsat_msg = NavSatFix() navsat_msg.header = gpsfix_msg.header # Caution: GPSFix has defined some additional status constants, which are # not defined in NavSatFix. navsat_msg.status.status = gpsfix_msg.status.status navsat_msg.status.service = 0 if gpsfix_msg.status.position_source & GPSStatus.SOURCE_GPS: navsat_msg.status.service = \ navsat_msg.status.service | NavSatStatus.SERVICE_GPS if gpsfix_msg.status.orientation_source & GPSStatus.SOURCE_MAGNETIC: navsat_msg.status.service = \ navsat_msg.status.service | NavSatStatus.SERVICE_COMPASS navsat_msg.latitude = gpsfix_msg.latitude navsat_msg.longitude = gpsfix_msg.longitude navsat_msg.altitude = gpsfix_msg.altitude navsat_msg.position_covariance = gpsfix_msg.position_covariance navsat_msg.position_covariance_type = gpsfix_msg.position_covariance_type return navsat_msg
def encoder_callback(encoder_msg): global gps_updated global ekf, state_space_init, yaw_init, elapsed_time, time_init, last_time, avg_x, avg_y global fitlered_lat, filtered_lon global seq, sec, nsec gps_lock.acquire() gps_meas = deepcopy(gps_meas_msg) gps_valid = copy(gps_updated) gps_updated = False gps_lock.release() imu_lock.acquire() imu_meas = deepcopy(imu_meas_msg) imu_lock.release() enc_updated = True abso_time = rospy.Time.now() time = abso_time.secs + abso_time.nsecs / 1000000000.0 purdue_fountain = (40.428642, -86.913776) if time_init == 0: time_init = rospy.Time.now() last_time = rospy.Time.now() if gps_valid: # y is north, x is east lat_lon = (gps_meas.latitude, gps_meas.longitude) x = (lat_lon[1] - purdue_fountain[1]) * 111139 # in meters y = (lat_lon[0] - purdue_fountain[0]) * 111139 # in meters #print("GPS LAT: " + str(float(lat_lon[0]))) #print("GPS LON: " + str(float(lat_lon[1]))) # Find IMU Heading (True North) quaternion = (imu_meas.orientation.x, imu_meas.orientation.y, imu_meas.orientation.z, imu_meas.orientation.w) angles = tf.transformations.euler_from_quaternion(quaternion) yaw = angles[2] imuHeading = yaw elapsed_dur = rospy.Time.now() - time_init elapsed_time = elapsed_dur.secs * 1000 + float( elapsed_dur.nsecs) / 1000000.0 if (gps_valid and not state_space_init): ekf.x[1] = (40.429155 - purdue_fountain[0]) * 111139 ekf.x[0] = (-86.912950 - purdue_fountain[1]) * 111139 ekf.x[2] = 2.094 yaw_init = yaw state_space_init = True imuHeading = (yaw - yaw_init) + 2.094 ############# Mike takes the wheel ###### theta_l = int(encoder_msg.data[2:4]) theta_r = int(encoder_msg.data[5:7]) if gps_valid: ekf.update_gps_cov(gps_meas.status.status) ekf.update_encoder_cov(theta_r, theta_l) ekf.update_imu_cov(0.01 + 0.005 * elapsed_time) if (gps_valid and state_space_init): timestep_dur = rospy.Time.now() - last_time timestep = timestep_dur.secs * 1000 + float( timestep_dur.nsecs) / 1000000.0 timestep = timestep / 1000 if (timestep <= 0): timestep = 0.2 #print("GPS Timestep: " + str(timestep)) last_time = rospy.Time.now() ekf.step(timestep, x, y, imuHeading, theta_l, theta_r) elif (state_space_init): timestep_dur = rospy.Time.now() - last_time timestep = timestep_dur.secs * 1000 + float( timestep_dur.nsecs) / 1000000.0 timestep = timestep / 1000 #print("Timestep: " + str(timestep)) last_time = rospy.Time.now() ekf.step(timestep, imuHeading, theta_l, theta_r) if (state_space_init): #print("FILETERED OUTPUT:") #print("GPS X (meters): " + str(float(ekf.x[0]))) #print("GPS Y (meters): " + str(float(ekf.x[1]))) #print("IMU Heading (radians): " + str(float(ekf.x[2]))) #print("Encoder Theta R (ticks): " + str(float(ekf.x[3]))) #print("Encoder Theta L (ticks): " + str(float(ekf.x[4]))) #print("\n") filtered_nsf = NavSatFix() filtered_nsf.longitude = ekf.x[0] / 111139 + purdue_fountain[1] filtered_nsf.latitude = ekf.x[1] / 111139 + purdue_fountain[0] seq = seq + 1 filtered_nsf.header.seq = seq filtered_nsf.header.stamp = gps_meas.header.stamp filtered_nsf.header.frame_id = gps_meas.header.frame_id #print("Filtered Lat: " + str(float(filtered_nsf.latitude))) #print("Filtered Lon: " + str(float(filtered_nsf.longitude))) r.publish(filtered_nsf) filtered_imu = deepcopy(imu_meas) filtered_imu.orientation.x = imuHeading filtered_imu.orientation.y = ekf.x[2] filtered_imu.orientation.z = yaw_init r2.publish(filtered_imu)
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return None parsed_sentence = enway_reach_rs_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return None if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude if self.covarianceMatrix and isinstance( self.covarianceMatrix, list) and len( self.covarianceMatrix) == 9: for i in range(9): current_fix.position_covariance[i] = self.covarianceMatrix[ i] current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_KNOWN else: hdop = data['hdop'] current_fix.position_covariance[0] = hdop**2 current_fix.position_covariance[4] = hdop**2 current_fix.position_covariance[8] = (2 * hdop)**2 # FIXME current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) return current_fix elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) return current_fix else: return None
def imu_publisher(UDP_IP,UDP_PORT,BUFFER_SIZE=1024,debug = False): GYRO_X_OFFSET = 0.0 GYRO_Y_OFFSET = 0.0 GYRO_Z_OFFSET = 0.0 pub_freq = 10 iter_count = 0 num_callibration_itrs = 60 gps_pub = rospy.Publisher('phone/fix', NavSatFix, queue_size=50) imu_pub = rospy.Publisher('phone/imu', Imu, queue_size=50) mag_pub = rospy.Publisher('phone/magnetic_field', MagneticField, queue_size=50) rospy.init_node('imu_publisher', anonymous=True) rate = rospy.Rate(pub_freq) rospy.loginfo("waiting for device...") sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) connected = False while not rospy.is_shutdown(): if not connected: try: sock.bind((UDP_IP, UDP_PORT)) connected = True rospy.loginfo("Device Connected @ {}:{}".format(UDP_IP,UDP_PORT)) except: rospy.loginfo("{0} UDP link failed. Retrying every second...".format(UDP_IP)) time.sleep(1) continue data,_ = sock.recvfrom(1024) line = data.split(',') if len(line) == 12: #received complete packet acceleration_x = float(line[0]) acceleration_y = float(line[1]) acceleration_z = float(line[2]) magnetic_x = float(line[3]) magnetic_y = float(line[4]) magnetic_z = float(line[5]) gyro_x = float(line[6]) gyro_y = float(line[7]) gyro_z = float(line[8]) lat = float(line[-3]) long = float(line[-2]) alt = float(line[-1][:-1]) if iter_count < num_callibration_itrs: GYRO_X_OFFSET += gyro_x GYRO_Y_OFFSET += gyro_y GYRO_Z_OFFSET += gyro_z iter_count += 1 elif iter_count == num_callibration_itrs and num_callibration_itrs != 0: GYRO_X_OFFSET /= num_callibration_itrs GYRO_Y_OFFSET /= num_callibration_itrs GYRO_Z_OFFSET /= num_callibration_itrs rospy.loginfo("finished callibrating yaw") iter_count += 1 #Publish Ros Imu message else: gyro_x -= GYRO_X_OFFSET gyro_y -= GYRO_Y_OFFSET gyro_z -= GYRO_Z_OFFSET imu_msg = Imu() imu_msg.header.stamp = rospy.Time.now() imu_msg.header.frame_id = 'phone_link' imu_msg.angular_velocity.x = gyro_x imu_msg.angular_velocity.y = gyro_y imu_msg.angular_velocity.z = gyro_z imu_msg.linear_acceleration.x = acceleration_x imu_msg.linear_acceleration.y = acceleration_y imu_msg.linear_acceleration.z = acceleration_z imu_msg.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] imu_msg.angular_velocity_covariance[0] = -1 imu_msg.linear_acceleration_covariance[0] = -1 imu_pub.publish(imu_msg) mag_msg = MagneticField() mag_msg.header.stamp = rospy.Time.now() mag_msg.header.frame_id = 'phone_link' mag_msg.magnetic_field.x = magnetic_x mag_msg.magnetic_field.y = magnetic_y mag_msg.magnetic_field.z = magnetic_z mag_pub.publish(mag_msg) gps_msg = NavSatFix() gps_msg.header.stamp = rospy.Time.now() gps_msg.header.frame_id = 'phone_link' gps_msg.latitude = lat gps_msg.longitude = long gps_msg.altitude = alt gps_pub.publish(gps_msg) rate.sleep() else: rospy.loginfo("received incomplete TCP packet from android IMU") continue
def spin_once(self): def baroPressureToHeight(value): c1 = 44330.0 c2 = 9.869232667160128300024673081668e-6 c3 = 0.1901975534856 intermediate = 1 - math.pow(c2 * value, c3) height = c1 * intermediate return height # get data data = self.mt.read_measurement() # common header h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id # get data (None if not present) temp_data = data.get('Temperature') # float orient_data = data.get('Orientation Data') velocity_data = data.get('Velocity') position_data = data.get('Latlon') altitude_data = data.get('Altitude') acc_data = data.get('Acceleration') gyr_data = data.get('Angular Velocity') mag_data = data.get('Magnetic') pressure_data = data.get('Pressure') time_data = data.get('Timestamp') gnss_data = data.get('GNSS') # debug the data from the sensor # print(data) # print("\n") # create messages and default values "Temp message" temp_msg = Temperature() pub_temp = False "Imu message supported with Modes 1 & 2" imuraw_msg = Imu() pub_imuraw = False imuins_msg = Imu() pub_imuins = False "SensorSample message supported with Mode 2" ss_msg = sensorSample() pub_ss = False "Mag message supported with Modes 1 & 2" mag_msg = Vector3Stamped() pub_mag = False "Baro in meters" baro_msg = FluidPressure() height_msg = baroSample() pub_baro = False "GNSS message supported only with MTi-G-7xx devices" "Valid only for modes 1 and 2" gnssPvt_msg = gnssSample() gps1_msg = NavSatFix() gps2_msg = GPSFix() pub_gnssPvt = False gnssSatinfo_msg = GPSStatus() pub_gnssSatinfo = False # All filter related outputs "Supported in mode 3" ori_msg = orientationEstimate() pub_ori = False "Supported in mode 3 for MTi-G-7xx devices" vel_msg = velocityEstimate() pub_vel = False "Supported in mode 3 for MTi-G-7xx devices" pos_msg = positionEstimate() pub_pos = False # first getting the sampleTimeFine # note if we are not using ros time, the we should replace the header # with the time supplied by the GNSS unit if time_data and not self.use_rostime: time = time_data['SampleTimeFine'] h.stamp.secs = 100e-6 * time h.stamp.nsecs = 1e5 * time - 1e9 * math.floor(h.stamp.secs) # temp data if temp_data: temp_msg.temperature = temp_data['Temp'] pub_temp = True # acceleration data if acc_data: if 'accX' in acc_data: # found acceleration pub_imuraw = True imuraw_msg.linear_acceleration.x = acc_data['accX'] imuraw_msg.linear_acceleration.y = acc_data['accY'] imuraw_msg.linear_acceleration.z = acc_data['accZ'] if 'freeAccX' in acc_data: # found free acceleration pub_imuins = True imuins_msg.linear_acceleration.x = acc_data['freeAccX'] imuins_msg.linear_acceleration.y = acc_data['freeAccY'] imuins_msg.linear_acceleration.z = acc_data['freeAccZ'] if 'Delta v.x' in acc_data: # found delta-v's pub_ss = True ss_msg.internal.imu.dv.x = acc_data['Delta v.x'] ss_msg.internal.imu.dv.y = acc_data['Delta v.y'] ss_msg.internal.imu.dv.z = acc_data['Delta v.z'] #else: # raise MTException("Unsupported message in XDI_AccelerationGroup.") # gyro data if gyr_data: if 'gyrX' in gyr_data: # found rate of turn pub_imuraw = True imuraw_msg.angular_velocity.x = gyr_data['gyrX'] imuraw_msg.angular_velocity.y = gyr_data['gyrY'] imuraw_msg.angular_velocity.z = gyr_data['gyrZ'] # note we do not force publishing the INS if we do not use the free acceleration imuins_msg.angular_velocity.x = gyr_data['gyrX'] imuins_msg.angular_velocity.y = gyr_data['gyrY'] imuins_msg.angular_velocity.z = gyr_data['gyrZ'] if 'Delta q0' in gyr_data: # found delta-q's pub_ss = True ss_msg.internal.imu.dq.w = gyr_data['Delta q0'] ss_msg.internal.imu.dq.x = gyr_data['Delta q1'] ss_msg.internal.imu.dq.y = gyr_data['Delta q2'] ss_msg.internal.imu.dq.z = gyr_data['Delta q3'] #else: # raise MTException("Unsupported message in XDI_AngularVelocityGroup.") # magfield if mag_data: ss_msg.internal.mag.x = mag_msg.vector.x = mag_data['magX'] ss_msg.internal.mag.y = mag_msg.vector.y = mag_data['magY'] ss_msg.internal.mag.z = mag_msg.vector.z = mag_data['magZ'] pub_mag = True if pressure_data: pub_baro = True baro_msg.fluid_pressure = pressure_data['Pressure'] height = baroPressureToHeight(pressure_data['Pressure']) height_msg.height = ss_msg.internal.baro.height = height # gps fix message if gnss_data and 'lat' in gnss_data: pub_gnssPvt = True # A "3" means that the MTi-G is using the GPS data. # A "1" means that the MTi-G was using GPS data and is now coasting/dead-reckoning the # position based on the inertial sensors (the MTi-G is not using GPS data in this mode). # This is done for 45 seconds, before the MTi-G Mode drops to "0". # A "0" means that the MTi-G doesn't use GPS data and also that it # doesn't output position based on the inertial sensors. if gnss_data['fix'] < 2: gnssSatinfo_msg.status = NavSatStatus.STATUS_NO_FIX # no fix gps1_msg.status.status = NavSatStatus.STATUS_NO_FIX # no fix gps1_msg.status.service = 0 else: gnssSatinfo_msg.status = NavSatStatus.STATUS_FIX # unaugmented gps1_msg.status.status = NavSatStatus.STATUS_FIX # unaugmented gps1_msg.status.service = NavSatStatus.SERVICE_GPS # lat lon alt gps1_msg.latitude = gnss_data['lat'] gps1_msg.longitude = gnss_data['lon'] gps1_msg.altitude = gnss_data['hEll'] # covariances gps1_msg.position_covariance[0] = math.pow(gnss_data['horzAcc'], 2) gps1_msg.position_covariance[4] = math.pow(gnss_data['horzAcc'], 2) gps1_msg.position_covariance[8] = math.pow(gnss_data['vertAcc'], 2) gps1_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN # custom message gnssPvt_msg.itow = gnss_data['iTOW'] gnssPvt_msg.fix = gnss_data['fix'] gnssPvt_msg.latitude = gnss_data['lat'] gnssPvt_msg.longitude = gnss_data['lon'] gnssPvt_msg.hEll = gnss_data['hEll'] gnssPvt_msg.hMsl = gnss_data['hMsl'] gnssPvt_msg.vel.x = gnss_data['velE'] gnssPvt_msg.vel.y = gnss_data['velN'] gnssPvt_msg.vel.z = gnss_data['velD'] gnssPvt_msg.hAcc = gnss_data['horzAcc'] gnssPvt_msg.vAcc = gnss_data['vertAcc'] gnssPvt_msg.sAcc = gnss_data['speedAcc'] gnssPvt_msg.pDop = gnss_data['PDOP'] gnssPvt_msg.hDop = gnss_data['HDOP'] gnssPvt_msg.vDop = gnss_data['VDOP'] gnssPvt_msg.numSat = gnss_data['nSat'] gnssPvt_msg.heading = gnss_data['heading'] gnssPvt_msg.headingAcc = gnss_data['headingAcc'] if orient_data: if 'Q0' in orient_data: pub_imuraw = True imuraw_msg.orientation.w = orient_data['Q0'] imuraw_msg.orientation.x = orient_data['Q1'] imuraw_msg.orientation.y = orient_data['Q2'] imuraw_msg.orientation.z = orient_data['Q3'] pub_imuins = True imuins_msg.orientation.w = orient_data['Q0'] imuins_msg.orientation.x = orient_data['Q1'] imuins_msg.orientation.y = orient_data['Q2'] imuins_msg.orientation.z = orient_data['Q3'] elif 'Roll' in orient_data: pub_ori = True ori_msg.roll = orient_data['Roll'] ori_msg.pitch = orient_data['Pitch'] ori_msg.yaw = orient_data['Yaw'] else: raise MTException( 'Unsupported message in XDI_OrientationGroup') if velocity_data: pub_vel = True vel_msg.velE = velocity_data['velX'] vel_msg.velN = velocity_data['velY'] vel_msg.velU = velocity_data['velZ'] if position_data: pub_pos = True pos_msg.latitude = position_data['lat'] pos_msg.longitude = position_data['lon'] if altitude_data: pub_pos = True tempData = altitude_data['ellipsoid'] pos_msg.hEll = tempData[0] # publish available information if pub_imuraw: imuraw_msg.header = h self.imuraw_pub.publish(imuraw_msg) if pub_imuins: imuins_msg.header = h self.imuins_pub.publish(imuins_msg) if pub_mag: mag_msg.header = h self.mag_pub.publish(mag_msg) if pub_temp: temp_msg.header = h self.temp_pub.publish(temp_msg) if pub_ss: ss_msg.header = h self.ss_pub.publish(ss_msg) if pub_baro: baro_msg.header = h height_msg.header = h self.baro_pub.publish(baro_msg) self.height_pub.publish(height_msg) if pub_gnssPvt: gnssPvt_msg.header = h gps1_msg.header = h self.gnssPvt_pub.publish(gnssPvt_msg) self.gps1_pub.publish(gps1_msg) #if pub_gnssSatinfo: # gnssSatinfo_msg.header = h # self.gnssSatinfo_pub.publish(gnssSatinfo_msg) if pub_ori: ori_msg.header = h self.ori_pub.publish(ori_msg) if pub_vel: vel_msg.header = h self.vel_pub.publish(vel_msg) if pub_pos: pos_msg.header = h self.pos_pub.publish(pos_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 not self.use_RMC and 'GGA' in parsed_sentence: data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX elif gps_qual == 9: # Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9 # http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/ current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude hdop = data['hdop'] current_fix.position_covariance[0] = hdop**2 current_fix.position_covariance[4] = hdop**2 current_fix.position_covariance[8] = (2 * hdop)**2 # FIXME current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel_global = Vector3Stamped() current_vel_global.header.stamp = current_time current_vel_global.header.frame_id = frame_id current_vel_global.vector.x = data['speed'] * \ math.sin(data['true_course']) current_vel_global.vector.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel_global) current_vel_local = TwistStamped() current_vel_local.header.stamp = current_time current_vel_local.header.frame_id = frame_id current_vel_local.twist.linear.x = data['speed'] self.vel_pub2.publish(current_vel_local) elif 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading_north']: self.hdt_pub.publish(data['heading_north']) elif 'ROT' in parsed_sentence: data = parsed_sentence['ROT'] current_rot = TwistStamped() current_rot.header.stamp = current_time current_rot.header.frame_id = frame_id current_rot.twist.angular.z = data['rate'] * 3.14 / 180 * 60 self.rot_pub.publish(current_rot) elif 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] current_speed = TwistStamped() current_speed.header.stamp = current_time current_speed.header.frame_id = frame_id current_speed.twist.linear.x = data['speed_kph'] / 360 * 1000 self.speed_pub.publish(current_speed) elif 'AVR' in parsed_sentence: data = parsed_sentence['AVR'] current_pyr = TwistStamped() current_pyr.header.stamp = current_timecur current_pyr.header.frame_id = frame_id current_pyr.twist.angular.z = data['yaw'] current_pyr.twist.angular.y = data['pitch'] current_pyr.twist.angular.x = data['roll'] if data['fix_type'] > 0: self.pyr_pub.publish(current_pyr) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False rospy.logdebug(parsed_sentence) if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id self.extend_fix.header.stamp = current_time self.extend_fix.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0] current_fix.status.status = gps_qual[1] current_fix.position_covariance_type = gps_qual[2] if gps_qual > 0: self.valid_fix = True else: self.valid_fix = False current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with # epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2 current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev)**2 # FIXME self.fix_pub.publish(current_fix) # set extend fix self.extend_fix.status.header.stamp = current_time self.extend_fix.status.header.frame_id = frame_id self.extend_fix.status.status = gps_qual[1] self.extend_fix.status.satellites_used = data['num_satellites'] self.extend_fix.status.motion_source = GPSStatus.SOURCE_GPS self.extend_fix.status.orientation_source = GPSStatus.SOURCE_GPS self.extend_fix.status.position_source = GPSStatus.SOURCE_GPS self.extend_fix.latitude = current_fix.latitude self.extend_fix.longitude = current_fix.longitude self.extend_fix.altitude = current_fix.altitude self.extend_fix.position_covariance = current_fix.position_covariance self.position_covariance_type = current_fix.position_covariance_type if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.last_valid_fix_time = current_time_ref self.time_ref_pub.publish(current_time_ref) self.extend_fix.time = current_time_ref.time_ref.to_sec() elif not self.use_RMC and 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] # Only report VTG data when you've received a valid GGA fix as # well. if self.valid_fix: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) self.extend_fix.track = data['true_course'] self.extend_fix.speed = data['speed'] self.extend_fix_pub.publish(self.extend_fix) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide # it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) self.extend_fix.track = data['true_course'] elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] elif 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading']: current_heading = QuaternionStamped() current_heading.header.stamp = current_time current_heading.header.frame_id = frame_id q = quaternion_from_euler(0, 0, math.radians(data['heading'])) current_heading.quaternion.x = q[0] current_heading.quaternion.y = q[1] current_heading.quaternion.z = q[2] current_heading.quaternion.w = q[3] self.heading_pub.publish(current_heading) elif 'GSA' in parsed_sentence: data = parsed_sentence['GSA'] self.star_use_gps = [ data['sate_id1'], data['sate_id2'], data['sate_id3'], data['sate_id4'], data['sate_id5'], data['sate_id6'], data['sate_id7'], data['sate_id8'], data['sate_id9'], data['sate_id10'], data['sate_id11'], data['sate_id12'] ] self.star_use_gps = filter(lambda star: star != 0, self.star_use_gps) self.extend_fix.pdop = data['pdop'] self.extend_fix.hdop = data['hdop'] self.extend_fix.vdop = data['vdop'] elif 'BDGSA' in parsed_sentence: data = parsed_sentence['BDGSA'] self.star_use_bd = [ data['sate_id1'], data['sate_id2'], data['sate_id3'], data['sate_id4'], data['sate_id5'], data['sate_id6'], data['sate_id7'], data['sate_id8'], data['sate_id9'], data['sate_id10'], data['sate_id11'], data['sate_id12'] ] self.star_use_bd = filter(lambda star: star != 0, self.star_use_bd) self.extend_fix.pdop = data['pdop'] self.extend_fix.hdop = data['hdop'] self.extend_fix.vdop = data['vdop'] self.extend_fix.status.satellite_used_prn = self.star_use_gps + self.star_use_bd elif 'GSV' in parsed_sentence: data = parsed_sentence['GSV'] if data['index'] == 1: self.star_map_gps = [] self.star_map_gps.append({ 'id': data['id_satellites1'], 'elevation': data['elevation_satellites1'], 'azimuth': data['azimuth_satellites1'], 'snr': data['snr1'] }) self.star_map_gps.append({ 'id': data['id_satellites2'], 'elevation': data['elevation_satellites2'], 'azimuth': data['azimuth_satellites2'], 'snr': data['snr2'] }) self.star_map_gps.append({ 'id': data['id_satellites3'], 'elevation': data['elevation_satellites3'], 'azimuth': data['azimuth_satellites3'], 'snr': data['snr3'] }) self.star_map_gps.append({ 'id': data['id_satellites4'], 'elevation': data['elevation_satellites4'], 'azimuth': data['azimuth_satellites4'], 'snr': data['snr4'] }) self.star_map_gps = filter(lambda star: star['id'] != 0, self.star_map_gps) elif 'BDGSV' in parsed_sentence: data = parsed_sentence['BDGSV'] if data['index'] == 1: self.star_map_bd = [] self.star_map_bd.append({ 'id': data['id_satellites1'], 'elevation': data['elevation_satellites1'], 'azimuth': data['azimuth_satellites1'], 'snr': data['snr1'] }) self.star_map_bd.append({ 'id': data['id_satellites2'], 'elevation': data['elevation_satellites2'], 'azimuth': data['azimuth_satellites2'], 'snr': data['snr2'] }) self.star_map_bd.append({ 'id': data['id_satellites3'], 'elevation': data['elevation_satellites3'], 'azimuth': data['azimuth_satellites3'], 'snr': data['snr3'] }) self.star_map_bd.append({ 'id': data['id_satellites4'], 'elevation': data['elevation_satellites4'], 'azimuth': data['azimuth_satellites4'], 'snr': data['snr4'] }) self.star_map_bd = filter(lambda star: star['id'] != 0, self.star_map_bd) self.star_map = self.star_map_gps + self.star_map_bd if data['length'] == data['index']: self.extend_fix.status.satellites_visible = len(self.star_map) self.extend_fix.status.satellite_visible_prn = [ star['id'] for star in self.star_map ] self.extend_fix.status.satellite_visible_snr = [ star['snr'] for star in self.star_map ] self.extend_fix.status.satellite_visible_azimuth = [ star['azimuth'] for star in self.star_map ] self.extend_fix.status.satellite_visible_z = [ star['elevation'] for star in self.star_map ] else: return False
# Execute path # ... print "Mission completed" return True if __name__ == "__main__": print "Starting Path Planner Node" rospy.init_node('path_planner_node', anonymous=True) nav_sat_msg = NavSatFix() nav_sat_msg.latitude = 10.40 nav_sat_msg.longitude = 10.50 waypoint_msg = waypoints() waypoint_msg.path = [waypoint_msg, waypoint_msg, waypoint_msg] pub = rospy.Publisher('drone_logic/mission_path', waypoints, queue_size=10) rate = rospy.Rate(1) # 10hz while not rospy.is_shutdown(): pub.publish(waypoint_msg) rate.sleep() # planner_object = path_planner() # # planner_object.get_map_data()
def my_handler(channel, data): global seq fix_pub = rospy.Publisher('fix', NavSatFix, queue_size=10) odom_pub = rospy.Publisher('odom', Odometry, queue_size=10) imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10) use_odom = True use_fix = True use_imu = True gps_frame = "novatel" headermsg = Header() headermsg.seq = seq + 1 headermsg.stamp = rospy.Time.now() headermsg.frame_id = gps_frame msg = auv_acfr_nav_t.decode(data) if use_fix == True: rospy.logdebug("Publising gps info") fixmsg = NavSatFix() fixmsg.header = headermsg fixmsg.latitude = math.radians(msg.latitude) fixmsg.longitude = math.radians(msg.longitude) fixmsg.altitude = msg.altitude fixmsg.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] fixmsg.position_covariance_type = 0 fix_pub.publish(fixmsg) if use_imu == True: rospy.logdebug("Publishing imu") imumsg = Imu() q = quaternion_from_euler( msg.pitch, msg.roll, -np.unwrap(msg.heading - np.pi / 2)) # TODO check this imumsg.orientation.x = q[0] imumsg.orientation.y = q[1] imumsg.orientation.z = q[2] imumsg.orientation.w = q[3] imumsg.angular_velocity.x = msg.pitchRate imumsg.angular_velocity.y = msg.rollRate imumsg.angular_velocity.z = -msg.headingRate imu_pub.publish(imumsg) if use_odom == True: br = tf.TransformBroadcaster() q = quaternion_from_euler( msg.pitch, msg.roll, -np.unwrap(msg.heading - np.pi / 2)) # TODO check this rospy.logdebug("Publishing odom") odommsg = Odometry() odommsg.header = headermsg odommsg.header.frame_id = "odom" #TODO, USE TRANSFORM odommsg.child_frame_id = "base_link" odommsg.pose.pose.position.x = msg.y odommsg.pose.pose.position.y = msg.x odommsg.pose.pose.position.z = -msg.altitude odommsg.pose.pose.orientation.x = q[0] odommsg.pose.pose.orientation.y = q[1] odommsg.pose.pose.orientation.z = q[2] odommsg.pose.pose.orientation.w = q[3] odommsg.twist.twist.linear.x = msg.vy odommsg.twist.twist.linear.y = msg.vx odommsg.twist.twist.linear.z = -msg.vz odommsg.twist.twist.angular.x = msg.pitchRate odommsg.twist.twist.angular.y = msg.rollRate odommsg.twist.twist.angular.z = -msg.headingRate odom_pub.publish(odommsg) br.sendTransform((msg.y, msg.x, 0), tf.transformations.quaternion_from_euler( msg.pitch, msg.roll, -np.unwrap(msg.heading - np.pi / 2)), rospy.Time.now(), "base_link", "odom")
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 run(self): global MOTOR_SENSORS pub = rospy.Publisher('/motor_sensors', Float32MultiArray, queue_size=10) pubUltr1 = rospy.Publisher('/ultrasonic_sensors1', Float32MultiArray, queue_size=10) pubUltr2 = rospy.Publisher('/ultrasonic_sensors2', Float32MultiArray, queue_size=10) pubGPS = rospy.Publisher('/GPS_coordinates', NavSatFix, queue_size=10) pubIMUraw = rospy.Publisher('imu/data_raw', Imu, queue_size=10) pubIMUmagn = rospy.Publisher('imu/mag', MagneticField, queue_size=10) #rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz vect = Float32MultiArray() vect.layout.dim.append(MultiArrayDimension()) vect.layout.dim[0].label = "height" vect.layout.dim[0].size = 4 vect.layout.dim[0].stride = 4 #Ultrasonic1 publisher rateU = rospy.Rate(10) # 10hz vectU = Float32MultiArray() vectU.layout.dim.append(MultiArrayDimension()) vectU.layout.dim[0].label = "height" vectU.layout.dim[0].size = 3 vectU.layout.dim[0].stride = 3 #Ultrasonic1 publisher rateU2 = rospy.Rate(10) # 10hz vectU2 = Float32MultiArray() vectU2.layout.dim.append(MultiArrayDimension()) vectU2.layout.dim[0].label = "height" vectU2.layout.dim[0].size = 3 vectU2.layout.dim[0].stride = 3 #GPS publisher rateGPS = rospy.Rate(10) vectGPS = NavSatFix() vectGPS.header.frame_id = "base_link" vectGPS.status.status = 1 vectGPS.status.service = 1 vectGPS.altitude = 0.0 vectGPS.position_covariance[0] = 1.0 vectGPS.position_covariance[4] = 1.0 vectGPS.position_covariance[8] = 1.0 vectGPS.position_covariance_type = 0 #0=Unknown #IMU raw data publisher rateIMUraw = rospy.Rate(10) vectIMUraw = Imu() vectIMUraw.header.frame_id = "imu_link" vectIMUraw.orientation.x = 0.0 vectIMUraw.orientation.y = 0.0 vectIMUraw.orientation.z = 0.0 vectIMUraw.orientation.w = 0.0 vectIMUraw.orientation_covariance[0] = 0.0 vectIMUraw.orientation_covariance[4] = 0.0 vectIMUraw.orientation_covariance[8] = 0.0 vectIMUraw.angular_velocity_covariance[0] = (3.40)*(10**(-6)) vectIMUraw.angular_velocity_covariance[4] = (7.96)*(10**(-7)) vectIMUraw.angular_velocity_covariance[8] = (1.69)*(10**(-5)) vectIMUraw.linear_acceleration_covariance[0] = (4.63)*(10**(-4)) vectIMUraw.linear_acceleration_covariance[4] = (8.79)*(10**(-4)) vectIMUraw.linear_acceleration_covariance[8] = (2.56)*(10**(-4)) #IMU magnetic field publisher rateIMUmagn = rospy.Rate(10) vectIMUmagn = MagneticField() vectIMUmagn.header.frame_id = "imu_link" vectIMUmagn.magnetic_field_covariance[0] = 0.0 vectIMUmagn.magnetic_field_covariance[4] = 0.0 vectIMUmagn.magnetic_field_covariance[8] = 0.0 while not rospy.is_shutdown(): current_time = rospy.Time.now() MOTOR_SENSORS.MUT.acquire() vect.data = [MOTOR_SENSORS.steering_angle, MOTOR_SENSORS.batt_level, MOTOR_SENSORS.motor_speed_L, MOTOR_SENSORS.motor_speed_R] MOTOR_SENSORS.MUT.release() pub.publish(vect) rate.sleep() ULTRASONIC_SENSORS1.MUT.acquire() vectU.data = [ULTRASONIC_SENSORS1.frontLeftUltr, ULTRASONIC_SENSORS1.frontRightUltr, ULTRASONIC_SENSORS1.rearCentralUltr] ULTRASONIC_SENSORS1.MUT.release() pubUltr1.publish(vectU) rateU.sleep() ULTRASONIC_SENSORS2.MUT.acquire() vectU2.data = [ULTRASONIC_SENSORS2.rearLeftUltr, ULTRASONIC_SENSORS2.rearRightUltr, ULTRASONIC_SENSORS2.frontCentralUltr] ULTRASONIC_SENSORS2.MUT.release() pubUltr2.publish(vectU2) rateU2.sleep() GPS.MUT.acquire() vectGPS.latitude = GPS.latitude vectGPS.longitude = GPS.longitude GPS.MUT.release() pubGPS.publish(vectGPS) rateGPS.sleep() IMU.MUT.acquire() vectIMUraw.header.stamp = current_time vectIMUraw.angular_velocity.x = IMU.x_rotation vectIMUraw.angular_velocity.y = IMU.y_rotation vectIMUraw.angular_velocity.z = IMU.z_rotation vectIMUraw.linear_acceleration.x = IMU.x_acceleration vectIMUraw.linear_acceleration.y = IMU.y_acceleration vectIMUraw.linear_acceleration.z = IMU.z_acceleration IMU.MUT.release() pubIMUraw.publish(vectIMUraw) rateIMUraw.sleep() IMU.MUT.acquire() vectIMUmagn.header.stamp = current_time vectIMUmagn.magnetic_field.x = IMU.x_magneto vectIMUmagn.magnetic_field.y = IMU.y_magneto vectIMUmagn.magnetic_field.z = IMU.z_magneto IMU.MUT.release() pubIMUmagn.publish(vectIMUmagn) rateIMUmagn.sleep()
def talker(): global global_g, deg_rad, Heading_HPD ser.flush() pub_m = rospy.Publisher('/imu/data_raw', Imu, queue_size=100) pub = rospy.Publisher('comb', comb, queue_size=100) pub_n = rospy.Publisher('comb_now', comb, queue_size=100) pub_g = rospy.Publisher('gps', comb, queue_size=100) pub_o = rospy.Publisher('/gps/fix', NavSatFix, queue_size=100) rospy.init_node('comb_node', anonymous=True) rate = rospy.Rate(100) # 10hz imuMsg = Imu() combMsg = comb() gpsMsg = comb() gpsmeanMsg = NavSatFix() seq = 0 while not rospy.is_shutdown(): # $GTIMU,GPSWeek,GPSTime,GyroX,GyroY,GyroZ,AccX,AccY,AccZ,Tpr*cs<CR><LF> if ser.read() != '$': continue line0 = ser.readline() print line0 cs = line0[-4:-2] print cs for s_tmp in line0[6:-8]: if s_tmp.isalpha(): continue try: cs1 = int(cs, 16) except: continue cs2 = checksum(line0) print("cs1,cs2", cs1, cs2) if cs1 != cs2: continue if line0[0:5] == "GTIMU": line = line0.replace("GTIMU,", "") line = line.replace("\r\n", "") if "\x00" in line: continue if not string.find('*', line): continue line = line.replace("*", ",") words = string.split(line, ",") if len(words) != 10: continue GyroX = string.atof(words[2]) GyroY = string.atof(words[3]) GyroZ = string.atof(words[4]) AccX = string.atof(words[5]) AccY = string.atof(words[6]) AccZ = string.atof(words[7]) imuMsg.linear_acceleration.x = AccX * global_g imuMsg.linear_acceleration.y = AccY * global_g imuMsg.linear_acceleration.z = AccZ * global_g imuMsg.linear_acceleration_covariance[0] = 0.0001 imuMsg.linear_acceleration_covariance[4] = 0.0001 imuMsg.linear_acceleration_covariance[8] = 0.0001 imuMsg.angular_velocity.x = GyroX * deg_rad imuMsg.angular_velocity.y = GyroY * deg_rad imuMsg.angular_velocity.z = GyroZ * deg_rad imuMsg.angular_velocity_covariance[0] = 0.0001 imuMsg.angular_velocity_covariance[4] = 0.0001 imuMsg.angular_velocity_covariance[8] = 0.0001 q = quaternion_from_euler(0, 0, 0) imuMsg.orientation.x = q[0] imuMsg.orientation.y = q[1] imuMsg.orientation.z = q[2] imuMsg.orientation.w = q[3] imuMsg.header.stamp = rospy.Time.now() imuMsg.header.frame_id = 'imu' imuMsg.header.seq = seq seq = seq + 1 pub_m.publish(imuMsg) rate.sleep() elif line0[0:5] == "GPFPD": line = line0.replace("GPFPD,", "") line = line.replace("\r\n", "") if ("\x00" in line): continue if (not string.find('*', line)): continue line = line.replace("*", ",") words = string.split(line, ",") if len(words) != 16: continue GPSWeek = string.atoi(words[0]) GPSTime = string.atof(words[1]) Heading = string.atof(words[2]) Pitch = string.atof(words[3]) Roll = string.atof(words[4]) Latitude = string.atof(words[5]) Longitude = string.atof(words[6]) Altitude = string.atof(words[7]) Ve = string.atof(words[8]) Vn = string.atof(words[9]) Vu = string.atof(words[10]) Baseline = string.atof(words[11]) NSV1 = string.atoi(words[12]) NSV2 = string.atoi(words[13]) Status = words[14] if Status == '03': Vne = math.sqrt(Vn * Vn + Ve * Ve) if (Vne > 0.3 and Ve < 0): Heading = math.acos(Vn / Vne) else: Heading = 2 * 3.141592658 - math.acos(Vn / Vne) combMsg.GPSWeek = GPSWeek combMsg.GPSTime = GPSTime combMsg.Heading = Heading combMsg.Pitch = Pitch combMsg.Roll = Roll combMsg.Latitude = Latitude combMsg.Longitude = Longitude combMsg.Altitude = Altitude combMsg.Ve = Ve combMsg.Vn = Vn combMsg.Vu = Vu combMsg.Baseline = Baseline combMsg.NSV1 = NSV1 combMsg.NSV2 = NSV2 combMsg.Status = Status combMsg.header.stamp = rospy.Time.now() combMsg.header.frame_id = 'gps' combMsg.header.seq = seq pub_n.publish(combMsg) print("pub comb_now") if Status == '03' or Status == '04' or Status == '05' or Status == '08' or Status == '0B': pub.publish(combMsg) print("pub comb") seq += 1 rate.sleep() #$GPHPD, GPSWeek, GPSTime, Heading, Pitch, Track, Latitude, Longitude, Altitude, Ve , Vn, Vu,Baseline, NSV1, NSV2*cs<CR><LF> elif line0[0:5] == "GPHPD": line = line0.replace("GPHPD,", "") line = line.replace("\r\n", "") if ("\x00" in line): continue if (not string.find('*', line)): continue line = line.replace("*", ",") words = string.split(line, ",") if len(words) != 16: continue Status = words[14] GPSWeek = string.atoi(words[0]) GPSTime = string.atof(words[1]) Heading_HPD = string.atof(words[2]) Heading_HPD = 360 - Heading_HPD Pitch = string.atof(words[3]) Track = string.atof(words[4]) Latitude = string.atof(words[5]) Longitude = string.atof(words[6]) Altitude = string.atof(words[7]) Ve = string.atof(words[8]) Vn = string.atof(words[9]) Vu = string.atof(words[10]) Baseline = string.atof(words[11]) NSV1 = string.atoi(words[12]) NSV2 = string.atoi(words[13]) gpsMsg.GPSWeek = GPSWeek gpsMsg.GPSTime = GPSTime gpsMsg.Heading = Heading_HPD gpsMsg.Pitch = Pitch gpsMsg.Roll = Track gpsMsg.Latitude = Latitude gpsMsg.Longitude = Longitude gpsMsg.Altitude = Altitude gpsMsg.Ve = Ve gpsMsg.Vn = Vn gpsMsg.Vu = Vu gpsMsg.Baseline = Baseline gpsMsg.NSV1 = NSV1 gpsMsg.NSV2 = NSV2 gpsMsg.Status = Status gpsMsg.header.stamp = rospy.Time.now() gpsMsg.header.frame_id = 'gps' gpsMsg.header.seq = seq pub_g.publish(gpsMsg) print("pub gps") gpsmeanMsg.header.stamp = rospy.Time.now() gpsmeanMsg.header.frame_id = 'gps' gpsmeanMsg.header.seq = seq if Status == '05': gpsmeanMsg.status.status = 2 covariance_xyz = 0.05**2 elif Status == '03': gpsmeanMsg.status.status = 0 covariance_xyz = 5**2 else: gpsmeanMsg.status.status = -1 covariance_xyz = 99999 gpsmeanMsg.status.service = 1 gpsmeanMsg.latitude = Latitude gpsmeanMsg.longitude = Longitude gpsmeanMsg.altitude = Latitude gpsmeanMsg.position_covariance = [ covariance_xyz, 0, 0, 0, covariance_xyz, 0, 0, 0, covariance_xyz ] gpsmeanMsg.position_covariance_type = 3 pub_o.publish(gpsmeanMsg) print("pub gps_mean") # try: # utm_pos = geodesy.utm.fromLatLong(inspvax.latitude, inspvax.longitude) # except ValueError: # # Probably coordinates out of range for UTM conversion. # return # # gpsmeanMsg.pose.pose.position.x = utm_pos.easting # gpsmeanMsg.pose.pose.position.y = utm_pos.northing # gpsmeanMsg.pose.pose.position.z = Altitude # gpsmeanMsg.pose.pose.orientation.x = 1 # gpsmeanMsg.pose.pose.orientation.y = 0 # gpsmeanMsg.pose.pose.orientation.z = 0 # gpsmeanMsg.pose.pose.orientation.w = 0 # gpsmeanMsg.pose.covariance = { # 2, 0, 0, 0, 0, 0, # 0, 2, 0, 0, 0, 0, # 0, 0, 25, 0, 0, 0, # 0, 0, 0, 99999, 0, 0, # 0, 0, 0, 0, 99999, 0, # 0, 0, 0, 0, 0, 99999} # # gpsmeanMsg.header.frame_id = 'base_footprint' # gpsmeanMsg.header.stamp = gps_time # # if Status == '0A' or Status == '0B': # pub_o.publish(gpsmeanMsg) # print("pub gps_mean") seq += 1 rate.sleep() else: continue
def add_sentence(self, nmea_string, frame_id, timestamp=None): """Public method to provide a new NMEA sentence to the driver. Args: nmea_string (str): NMEA sentence in string form. frame_id (str): TF frame ID of the GPS receiver. timestamp(rospy.Time, optional): Time the sentence was received. If timestamp is not specified, the current time is used. Returns: bool: True if the NMEA string is successfully processed, False if there is an error. """ if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id if not self.use_GNSS_time: current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] if self.use_GNSS_time: if math.isnan(data['utc_time'][0]): rospy.logwarn("Time in the NMEA sentence is NOT valid") return False current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1]) fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0] current_fix.status.status = gps_qual[1] current_fix.position_covariance_type = gps_qual[2] self.valid_fix = (fix_type > 0) current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with # epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2 current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev)**2 # FIXME self.fix_pub.publish(current_fix) if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time): current_time_ref.time_ref = rospy.Time(data['utc_time'][0], data['utc_time'][1]) self.last_valid_fix_time = current_time_ref self.time_ref_pub.publish(current_time_ref) elif not self.use_RMC and 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] # Only report VTG data when you've received a valid GGA fix as # well. if self.valid_fix: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * math.sin( data['true_course']) current_vel.twist.linear.y = data['speed'] * math.cos( data['true_course']) self.vel_pub.publish(current_vel) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] if self.use_GNSS_time: if math.isnan(data['utc_time'][0]): rospy.logwarn("Time in the NMEA sentence is NOT valid") return False current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1]) # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time): current_time_ref.time_ref = rospy.Time( data['utc_time'][0], data['utc_time'][1]) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide # it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] elif 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading']: current_heading = QuaternionStamped() current_heading.header.stamp = current_time current_heading.header.frame_id = frame_id q = quaternion_from_euler(0, 0, math.radians(data['heading'])) current_heading.quaternion.x = q[0] current_heading.quaternion.y = q[1] current_heading.quaternion.z = q[2] current_heading.quaternion.w = q[3] self.heading_pub.publish(current_heading) else: return False
def main(args): #prepare the listener global socket socket.connect("tcp://" + ip + ":" + port) socket.setsockopt_string(zmq.SUBSCRIBE, topic) #setup some opencv windows cv2.namedWindow("Listener Left Camera") cv2.namedWindow("Listener Right Camera") #Start the main loop while True: message_in = socket.recv(10 * 1024) #For some reason I have to make a deep copy of the message. Otherwise, when, the second time I received the message I got a truncated message error. This solves it so I did not worry about it anymore. message = copy.deepcopy(message_in) #Deserialization or unmarshalling #We use message[4:] because we know the first four bytes are #"777 " and we only want to give the data to the parser (not the topic name sd.ParseFromString(message[4:]) #Getting the left image cv_left_image = np.zeros( (sd.left_image.height, sd.left_image.width, 3), np.uint8) cv_left_image.data = sd.left_image.data #Getting the right image cv_right_image = np.zeros( (sd.right_image.height, sd.right_image.width, 3), np.uint8) cv_right_image.data = sd.right_image.data #Getting the point cloud point_cloud_msg = PointCloud2() point_cloud_msg.height = sd.point_cloud.height point_cloud_msg.width = sd.point_cloud.width for field in sd.point_cloud.fields: point_field = PointField() point_field.name = field.name point_field.offset = field.offset point_field.datatype = field.datatype point_field.count = field.count point_cloud_msg.fields.append(point_field) point_cloud_msg.is_bigendian = sd.point_cloud.is_bigendian point_cloud_msg.is_bigendian = sd.point_cloud.is_bigendian point_cloud_msg.point_step = sd.point_cloud.point_step point_cloud_msg.row_step = sd.point_cloud.row_step point_cloud_msg.data = sd.point_cloud.data #Getting the NavSatFix nav_sat_fix = NavSatFix() nav_sat_fix.latitude = sd.nav_sat_fix.latitude nav_sat_fix.longitude = sd.nav_sat_fix.longitude nav_sat_fix.altitude = sd.nav_sat_fix.altitude nav_sat_fix.status.status = sd.nav_sat_fix.status.status nav_sat_fix.status.service = sd.nav_sat_fix.status.service #Getting the Odometry odometry = Odometry() #and then we can copy every field ... no need in this example #--------------------------------- #Visualizing the received data #--------------------------------- print("Received new message with stamp:\n" + str(sd.header.stamp)) print( "First 10 points x,y,z and rgb (packed in float32) values (just for debug)" ) count = 0 for p in pc2.read_points(point_cloud_msg, field_names=("x", "y", "z", "rgb"), skip_nans=True): print " x : %f y: %f z: %f rgb: %f" % (p[0], p[1], p[2], p[3]) count = count + 1 if count > 10: break print("GPS data:") print("Latitude =" + str(nav_sat_fix.latitude)) print("Longitude =" + str(nav_sat_fix.longitude)) print("Altitude =" + str(nav_sat_fix.altitude)) print("Status =" + str(nav_sat_fix.status.status)) print("Service =" + str(nav_sat_fix.status.service)) print("Odometry data (only position for example):") print("odom.pose.pose.position.x =" + str(sd.odometry.pose.pose.position.x)) print("odom.pose.pose.position.y =" + str(sd.odometry.pose.pose.position.y)) print("odom.pose.pose.position.z =" + str(sd.odometry.pose.pose.position.z)) cv2.imshow("Listener Left Camera", cv_left_image) cv2.imshow("Listener Right Camera", cv_right_image) cv2.waitKey(30)
import rospy from sensor_msgs.msg import NavSatFix my = NavSatFix() my.latitude = 41 my.longitude = -70 my.altitude = 2 print my.latitude
def onPositionChange(self, latitude, longitude, altitude): print("TEST") global INITIAL_LATITUDE global INITIAL_LONGITUDE global LATITUDE_STDEV global LONGITUDE_STDEV global DECI_DEGREE_TO_METER global device_serial_number global ENABLE_PUBLISH global warming_time global pub_frequency global CONTROL_STDEV global warming_queue fix = NavSatFix() fix.header.stamp = rospy.Time.now() fix.header.frame_id = "map" fix.latitude = latitude fix.longitude = longitude fix.altitude = altitude global_pose_pub.publish(fix) if (ENABLE_PUBLISH == 0): # Store in the warming_queue and compute queue_len = len(warming_queue) warming_queue.append((longitude, latitude)) if (queue_len >= warming_time * pub_frequency): warming_queue.pop(0) long_sum = 0.0 lati_sum = 0.0 long_var = 0.0 lati_var = 0.0 for i in range(queue_len): long_sum += warming_queue[i][0] lati_sum += warming_queue[i][1] long_var += warming_queue[i][0]**2 lati_var += warming_queue[i][1]**2 # rospy.logdebug("Current longitude variance: [%f]", long_var ) # rospy.logdebug("Current latitude variance: [%f]", lati_var ) long_average = long_sum / queue_len lati_average = lati_sum / queue_len long_var = (long_var / queue_len - long_average**2) * DECI_DEGREE_TO_METER**2 lati_var = (lati_var / queue_len - lati_average**2) * DECI_DEGREE_TO_METER**2 rospy.logdebug("Current average longitude: [%.6f]", long_average) rospy.logdebug("Current average latitude: [%.6f]", lati_average) #rospy.logdebug("Current longitude variance: [%f]", long_var ) #rospy.logdebug("Current latitude variance: [%f]", lati_var ) #rospy.logdebug("Initial longitude stdev: [%f]", np.sqrt(long_var + lati_var)*DECI_DEGREE_TO_METER ) if (np.sqrt(long_var + lati_var) < CONTROL_STDEV): ENABLE_PUBLISH = 1 LONGITUDE_STDEV = np.sqrt(long_var) LATITUDE_STDEV = np.sqrt(lati_var) INITIAL_LONGITUDE = long_average INITIAL_LATITUDE = lati_average rospy.loginfo("GNSS [%s] stdev within control value.", device_serial_number) rospy.loginfo("Initial longitude: [%.6f]", long_average) rospy.loginfo("Initial latitude: [%.6f]", lati_average) rospy.loginfo("Initial longitude stdev: [%f]", LONGITUDE_STDEV) rospy.loginfo("Initial latitude stdev: [%f]", LATITUDE_STDEV) else: rospy.logdebug("Current average longitude: [%f]", long_average) rospy.logdebug("Current average latitude: [%f]", lati_average) rospy.logdebug("Current position stdev in meter: [%f]", np.sqrt(long_var + lati_var)) if (ENABLE_PUBLISH == 1): # Converting from decimal degree into meter and into the map frame. ############################################################# # TODO: take in the orientation difference of map and north?? ############################################################# gps_e, gps_n, gps_u = pm.geodetic2enu(latitude, longitude, 0, INITIAL_LATITUDE, INITIAL_LONGITUDE, 0) MOVE_X = gps_n MOVE_Y = -gps_e #MOVE_X = (longitude - INITIAL_LONGITUDE)*DECI_DEGREE_TO_METER #MOVE_Y = (latitude - INITIAL_LATITUDE)*DECI_DEGREE_TO_METER # MOVE_Z = pose = PoseWithCovarianceStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "map" pose.pose.pose.position.x = MOVE_X pose.pose.pose.position.y = MOVE_Y # When processing, double the covariance pose.pose.covariance[0] = LATITUDE_STDEV * 2 pose.pose.covariance[7] = LONGITUDE_STDEV * 2 local_pose_pub.publish(pose)
ros_gps_vel = Vector3Stamped() ros_gps.status.service = ros_gps.status.SERVICE_GPS ros_gps.header.frame_id = "odom" ros_gps_vel.header.frame_id = "gps" rospy.init_node("airsim_gps_node") pos_pub = rospy.Publisher('airsim/gps/fix', NavSatFix, queue_size=10) vel_pub = rospy.Publisher('airsim/gps/velocity', Vector3Stamped, queue_size=10) r = rospy.Rate(10) while not rospy.is_shutdown(): airsim_gps = client.getGpsData(gps_name="Gps", vehicle_name="") ros_gps.altitude = -1 * airsim_gps.gnss.geo_point.altitude ros_gps.longitude = -1 * airsim_gps.gnss.geo_point.longitude ros_gps.latitude = airsim_gps.gnss.geo_point.latitude ros_gps.header.stamp = rospy.Time.from_sec(airsim_gps.time_stamp / 1e9) ros_gps_vel.header.stamp = rospy.Time.from_sec(airsim_gps.time_stamp / 1e9) ros_gps_vel.vector.x = airsim_gps.gnss.velocity.x_val ros_gps_vel.vector.y = -1 * airsim_gps.gnss.velocity.y_val ros_gps_vel.vector.z = -1 * airsim_gps.gnss.velocity.z_val pos_pub.publish(ros_gps) vel_pub.publish(ros_gps_vel) r.sleep()
if data[0] == '$GPGGA': if data[2] == '': print 'poor gps signal' else: if data[3] == 'N': msg.latitude = data[2] #convert gps latitude data to degree msg.latitude = float( msg.latitude[0:2]) + float(msg.latitude[2:]) / 60 else: msg.latitude = data[2] msg.latitude = -(float(msg.latitude[0:2]) + float(msg.latitude[2:]) / 60) if data[5] == 'W': msg.longitude = data[4] msg.longitude = -(float(msg.longitude[1:3]) + float(msg.longitude[3:]) / 60) else: msg.longitude = data[4] msg.longitude = float( msg.longitude[1:3]) + float(msg.longitude[3:]) / 60 msg.altitude = float(data[9]) #NMEA system. ddmm.mmmm ->degree as input of utm -> utm x,y lc.publish("gpsdata", msg.encode()) pub.publish(msg) else: print 'Waiting for GPGGA signal' rate.sleep()
def bestpos_handler(self, bestpos): navsat = NavSatFix() # TODO: The timestamp here should come from SPAN, not the ROS system time. navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame # Assume GPS - this isn't exposed navsat.status.service = NavSatStatus.SERVICE_GPS position_type_to_status = { BESTPOS.POSITION_TYPE_NONE: NavSatStatus.STATUS_NO_FIX, BESTPOS.POSITION_TYPE_FIXED: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_FIXEDHEIGHT: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_FLOATCONV: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_WIDELANE: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_NARROWLANE: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_DOPPLER_VELOCITY: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_SINGLE: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_WAAS: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_PROPAGATED: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_OMNISTAR: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_L1_FLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_IONOFREE_FLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_NARROW_FLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_L1_INT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_WIDE_INT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_NARROW_INT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_RTK_DIRECT_INS: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_SBAS: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_INS_PSRSP: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_RTKFLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_RTKFIXED: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_OMNISTAR: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_OMNISTAR_HP: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_OMNISTAR_XP: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_OMNISTAR_HP: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_OMNISTAR_XP: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_PPP: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_INS_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_INS_PPP: NavSatStatus.STATUS_SBAS_FIX, } navsat.status.status = position_type_to_status.get( bestpos.position_type, NavSatStatus.STATUS_NO_FIX) # Position in degrees. navsat.latitude = bestpos.latitude navsat.longitude = bestpos.longitude # Altitude in metres. navsat.altitude = bestpos.altitude navsat.position_covariance[0] = pow(2, bestpos.latitude_std) navsat.position_covariance[4] = pow(2, bestpos.longitude_std) navsat.position_covariance[8] = pow(2, bestpos.altitude_std) navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN # Ship ito self.pub_navsatfix.publish(navsat)