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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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)
from sensor_msgs.msg import NavSatFix import rospy, time rospy.init_node('gps_faker') nav_pub=rospy.Publisher('fix', NavSatFix, queue_size=10) lat=38.405144 lon=-110.792672 while not rospy.is_shutdown(): data=NavSatFix() data.latitude=lat#38.405144 data.longitude=lon#-110.792672 #lat+=0.0001 #lon+=0.0001 nav_pub.publish(data) time.sleep(1)
def add_sentence(self, message, frame_id, timestamp=None, sentence_type="NMEA", ubx_message_definitions=None): """Public method to provide a new sentence to the driver. Args: message (str): NMEA or UBX 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. sentence_type (str, optional): NMEA or UBX ubx_message_definitions (str, optional): Message definitions for UBX sentences Return: bool: True if the NMEA string is successfully processed, False if there is an error. """ if not "UBX" == sentence_type: if not check_nmea_checksum(message): rospy.logwarn( "Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(message)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( message) if not parsed_sentence: rospy.logdebug( "Failed to parse NMEA sentence. Sentence was: %s" % message) return False elif "UBX" == sentence_type: parsed_sentence = libnmea_navsat_driver.parser_ubx.parse_ubx_sentence( message, ubx_message_definitions) if not parsed_sentence: rospy.logdebug( "Failed to parse UBX sentence. Sentence was: %s" % message) 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) if self.use_extended_nmea_messages: info = NavSatInfo() info.header.stamp = current_time info.header.frame_id = frame_id info.num_satellites = data['num_satellites'] info.last_dgps_update = data['age_dgps'] self.age_dgps = data['age_dgps'] info.hdop = data['hdop'] self.info_pub.publish(info) 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 'GRS' in parsed_sentence and self.use_extended_nmea_messages: data = parsed_sentence['GRS'] msg = NavSatGRS() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.utc_time = data['utc_time'] msg.mode = data['mode'] msg.residual_01 = data['residual_01'] msg.residual_02 = data['residual_02'] msg.residual_03 = data['residual_03'] msg.residual_04 = data['residual_04'] msg.residual_05 = data['residual_05'] msg.residual_06 = data['residual_06'] msg.residual_07 = data['residual_07'] msg.residual_08 = data['residual_08'] msg.residual_09 = data['residual_09'] msg.residual_10 = data['residual_10'] msg.residual_11 = data['residual_11'] # The following two attributes have been introduced with NMEA 4.11 msg.system_id = data['system_id'] msg.signal_id = data['signal_id'] self.grs_pub.publish(msg) elif 'GST' in parsed_sentence and self.use_extended_nmea_messages: 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'] msg = NavSatGST() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.utc_time = data['utc_time'] msg.ranges_std_dev = data['ranges_std_dev'] msg.semi_major_ellipse_std_dev = data['semi_major_ellipse_std_dev'] msg.semi_minor_ellipse_std_dev = data['semi_minor_ellipse_std_dev'] msg.semi_major_orientation = data['semi_major_orientation'] msg.lat_std_dev = data['lat_std_dev'] msg.lon_std_dev = data['lon_std_dev'] msg.alt_std_dev = data['alt_std_dev'] self.gst_pub.publish(msg) 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 self.use_trimble_messages and 'PTNL,VHD' in parsed_sentence: data = parsed_sentence['PTNL,VHD'] msg = NavSatTrimbleHeading() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.azimuth = data['azimuth'] msg.azimuth_rate = data['azimuth_rate'] msg.vertical_angle = data['vertical_angle'] msg.vertical_angle_rate = data['vertical_angle_rate'] msg.range = data['range'] msg.range_rate = data['range_rate'] # explanation of the status codes in message definition msg.status = data['fix_type'] msg.num_satellites = data['num_satellites'] msg.pdop = data['pdop'] self.trimble_heading_pub.publish(msg) elif self.use_trimble_messages and 'PTNL,AVR' in parsed_sentence: data = parsed_sentence['PTNL,AVR'] msg = NavSatTrimbleMovingBase() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.yaw = data['yaw'] if self.ccw_heading: msg.yaw = -msg.yaw msg.yaw += self.heading_offset # wrap yaw angle to [-pi, pi) msg.yaw = (msg.yaw + math.pi) % (2 * math.pi) - math.pi msg.tilt = data['tilt'] msg.roll = data['roll'] msg.range = data['range'] # explanation of the status codes in message definition msg.status = data['fix_type'] msg.pdop = data['pdop'] msg.num_satellites = data['num_satellites'] self.trimble_moving_base_pub.publish(msg) # An dual antenna system can only measure tow out of the # three angles tilt, roll, yaw. Yaw is always measured. # Assuming all not measured angles are zero. if math.isnan(msg.tilt): msg.tilt = 0. if math.isnan(msg.roll): msg.roll = 0. msg2 = Imu() msg2.header.stamp = current_time msg2.header.frame_id = frame_id q = quaternion_from_euler(msg.tilt, msg.roll, msg.yaw) msg2.orientation.x = q[0] msg2.orientation.y = q[1] msg2.orientation.z = q[2] msg2.orientation.w = q[3] # Calculate the orientation error estimate based on the position # error estimates and the baseline. Very conservative by taking the # largest possible error at both ends in both directions. Set high # error if there is no proper vector fix (msg.status is not 3). if msg.status == 3: total_position_error = math.sqrt(self.lat_std_dev**2 + self.lon_std_dev**2) angular_error_estimate = math.atan2(total_position_error * 2, msg.range) angular_error_cov = angular_error_estimate**2 else: angular_error_cov = 1 msg2.orientation_covariance = [ angular_error_cov, 0.0, 0.0, 0.0, angular_error_cov, 0.0, 0.0, 0.0, angular_error_cov ] msg2.angular_velocity_covariance = [ -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] msg2.linear_acceleration_covariance = [ -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] self.trimble_moving_base_imu_pub.publish(msg2) # Documentation source: # https://www.u-blox.com/sites/default/files/u-blox_ZED-F9P_InterfaceDescription_%28UBX-18010854%29.pdf # Some comments have been directly copied from the documentation elif self.use_ublox_messages and 'PUBX,00' in parsed_sentence: data = parsed_sentence['PUBX,00'] msg = NavSatUbloxPubxPosition() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.utc_time = rospy.Time(data['utc_time'][0], data['utc_time'][1]) msg.latitude = data['latitude'] if data['latitude_direction'] == 'S': msg.latitude = -msg.latitude msg.longitude = data['longitude'] if data['longitude_direction'] == 'W': msg.longitude = -msg.longitude msg.altitude = data['altitude'] msg.nav_stat = data['nav_stat'] msg.h_acc = data['h_acc'] msg.v_acc = data['v_acc'] msg.sog = data['sog'] / 3.6 msg.cog = data['cog'] / 180.0 * math.pi msg.v_vel = data['v_vel'] msg.diff_age = data['diff_age'] msg.hdop = data['hdop'] msg.vdop = data['vdop'] msg.tdop = data['tdop'] msg.num_svs = data['num_svs'] msg.dr = data['dr'] self.ublox_pubx_position_pub.publish(msg) # Documentation source: # https://www.u-blox.com/sites/default/files/u-blox_ZED-F9P_InterfaceDescription_%28UBX-18010854%29.pdf # Some comments have been directly copied from the documentation elif self.use_ublox_messages and 'UBX-NAV-RELPOSNED' in parsed_sentence: data = parsed_sentence['UBX-NAV-RELPOSNED'] msg = NavSatUbloxRelPos() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.tow = data['iTOW'] * 0.001 msg.n = data['relPosN'] * 0.01 + data['relPosHPN'] * 0.0001 msg.e = data['relPosE'] * 0.01 + data['relPosHPE'] * 0.0001 msg.d = data['relPosD'] * 0.01 + data['relPosHPD'] * 0.0001 msg.length = data['relPosLength'] * 0.01 + data[ 'relPosHPLength'] * 0.0001 msg.heading = data['relPosHeading'] * 0.00001 / 180.0 * math.pi if self.ccw_heading: msg.heading = -msg.heading # only add offset if heading is valid - invalid heading should always be 0.0 data['flags'] = format(data['flags'], '032b') if 1 == int(data['flags'][23], 2): msg.heading += self.heading_offset # wrap yaw angle to [-pi, pi) msg.heading = (msg.heading + math.pi) % (2 * math.pi) - math.pi msg.acc_n = data['accN'] * 0.0001 msg.acc_e = data['accE'] * 0.0001 msg.acc_d = data['accD'] * 0.0001 msg.acc_length = data['accLength'] * 0.0001 msg.acc_heading = data['accHeading'] * 0.00001 / 180.0 * math.pi msg.rel_pos_heading_valid = int(data['flags'][23], 2) msg.ref_obs_miss = int(data['flags'][24], 2) msg.ref_pos_miss = int(data['flags'][25], 2) msg.is_moving = int(data['flags'][26], 2) msg.carr_soln = int(data['flags'][27:29], 2) msg.rel_pos_valid = int(data['flags'][29], 2) msg.diff_soln = int(data['flags'][30], 2) msg.gnss_fix_ok = int(data['flags'][31], 2) # accuracy estimates in non RTK fixed modes shouldn't be relied on if msg.carr_soln == 2 and msg.rel_pos_valid == 1: self.lat_std_dev = msg.acc_n self.lon_std_dev = msg.acc_e self.alt_std_dev = msg.acc_d elif msg.carr_soln == 1 and msg.rel_pos_valid == 1: self.lat_std_dev = self.default_epe_quality5 self.lon_std_dev = self.default_epe_quality5 self.alt_std_dev = self.default_epe_quality5 else: self.lat_std_dev = self.default_epe_quality1 self.lon_std_dev = self.default_epe_quality1 self.alt_std_dev = self.default_epe_quality1 self.ublox_relpos_pub.publish(msg) # report erroneous messages if msg.carr_soln != 0: if msg.ref_obs_miss == 1 \ or msg.ref_pos_miss == 1 \ or msg.rel_pos_valid == 0 \ or msg.gnss_fix_ok == 0: rospy.logerr( "GNSS receiver failed to calculate GNSS fix despite available correction data. " "Consider lowering the frequency, receiver load might be too high." ) return False # publish odometry and IMU messages for fusion in case message is fine if msg.carr_soln == 0: if msg.diff_soln == 1: rospy.logwarn_throttle( 10, "Unable to calculate RTK solution.") return False msg2 = Odometry() msg2.header.stamp = current_time msg2.header.frame_id = frame_id # follow enu conventions msg2.pose.pose.position.x = msg.e msg2.pose.pose.position.y = msg.n msg2.pose.pose.position.z = -msg.d msg2.pose.covariance[0] = self.lon_std_dev**2 msg2.pose.covariance[7] = self.lat_std_dev**2 msg2.pose.covariance[14] = self.alt_std_dev**2 # output heading once in moving base mode if msg.is_moving == 1: # take the already inverted heading from above and not the raw # value q = quaternion_from_euler(0., 0., msg.heading) msg2.pose.pose.orientation.x == q[0] msg2.pose.pose.orientation.y == q[1] msg2.pose.pose.orientation.z == q[2] msg2.pose.pose.orientation.w == q[3] # degrade estimated when not in RTK fixed mode msg2.pose.covariance[21] = msg.acc_heading**2 + (2 - msg.carr_soln) msg2.pose.covariance[28] = msg.acc_heading**2 + (2 - msg.carr_soln) msg2.pose.covariance[35] = msg.acc_heading**2 + (2 - msg.carr_soln) else: msg2.pose.covariance[21] = -1.0 msg2.pose.covariance[28] = -1.0 msg2.pose.covariance[35] = -1.0 self.ublox_relpos_odom_pub.publish(msg2) # Publish IMU message if in moving base mode if msg.is_moving == 1: msg3 = Imu() msg3.header.stamp = current_time msg3.header.frame_id = frame_id xyzw = quaternion_from_euler(0.0, 0.0, msg.heading) msg3.orientation.x = xyzw[0] msg3.orientation.y = xyzw[1] msg3.orientation.z = xyzw[2] msg3.orientation.w = xyzw[3] if msg.carr_soln == 2: angular_error_cov = msg.acc_heading**2 else: angular_error_cov = msg.acc_heading**2 + (2 - msg.carr_soln) msg3.orientation_covariance = [ angular_error_cov, 0.0, 0.0, 0.0, angular_error_cov, 0.0, 0.0, 0.0, angular_error_cov ] msg3.angular_velocity_covariance = [ -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] msg3.linear_acceleration_covariance = [ -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] self.ublox_relpos_imu_pub.publish(msg3) elif self.use_ublox_messages and 'UBX-NAV-GEOFENCE' in parsed_sentence: data = parsed_sentence['UBX-NAV-GEOFENCE'] msg = NavSatUbloxGeoFence() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.tow = data['iTOW'] * 0.001 msg.status = data['status'] msg.num_fences = data['numFences'] msg.comb_state = data['combState'] msg.status = data['status'] # The original message is variable length, so all fences are # initialized with "deactivated" state. msg.fence1 = NavSatUbloxGeoFence.DEACTIVATED msg.fence2 = NavSatUbloxGeoFence.DEACTIVATED msg.fence3 = NavSatUbloxGeoFence.DEACTIVATED msg.fence4 = NavSatUbloxGeoFence.DEACTIVATED try: msg.fence1 = data['state'][0] msg.fence2 = data['state'][1] msg.fence3 = data['state'][2] msg.fence4 = data['state'][3] except KeyError: pass self.ublox_geofence_pub.publish(msg) elif self.use_ublox_messages and 'UBX-NAV-PVT' in parsed_sentence: data = parsed_sentence['UBX-NAV-PVT'] msg = NavSatUbloxPositionVelocityTime() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.tow = data['iTOW'] * 0.001 msg.year = data['year'] msg.month = data['month'] msg.day = data['day'] msg.hour = data['hour'] msg.min = data['min'] msg.sec = data['sec'] bitfield_valid = format(data['valid'], '08b') msg.valid_date = int(bitfield_valid[7], 2) msg.valid_time = int(bitfield_valid[6], 2) msg.fully_resolved = int(bitfield_valid[5], 2) msg.valid_mag = int(bitfield_valid[4], 2) msg.t_acc = data['tAcc'] / 1.e9 msg.nano = data['nano'] msg.fix_type = data['fixType'] bitfield_flags = format(data['flags'], '08b') msg.gnss_fix_ok = int(bitfield_flags[7], 2) msg.diff_soln = int(bitfield_flags[6], 2) msg.psm_state = int(bitfield_flags[3:6], 2) msg.head_veh_valid = int(bitfield_flags[2], 2) msg.carr_soln = int(bitfield_flags[0:2], 2) bitfield_flags2 = format(data['flags2'], '08b') msg.confirmed_avai = int(bitfield_flags2[2], 2) msg.confirmed_date = int(bitfield_flags2[1], 2) msg.confirmed_time = int(bitfield_flags2[0], 2) msg.num_sv = data['numSV'] msg.lon = data['lon'] / 1.e7 msg.lat = data['lat'] / 1.e7 msg.height = data['height'] / 1.e3 msg.h_msl = data['hMSL'] / 1.e3 msg.h_acc = data['hAcc'] / 1.e3 msg.v_acc = data['vAcc'] / 1.e3 msg.vel_n = data['velN'] / 1.e3 msg.vel_e = data['velE'] / 1.e3 msg.vel_d = data['velD'] / 1.e3 msg.g_speed = data['gSpeed'] / 1.e3 msg.head_mot = data['headMot'] / 1.e5 / 180.0 * math.pi msg.s_acc = data['sAcc'] / 1.e3 msg.head_acc = data['headAcc'] / 1.e5 / 180.0 * math.pi msg.p_dop = data['pDOP'] / 1.e2 msg.head_veh = data['headVeh'] / 1.e5 / 180.0 * math.pi msg.mag_dec = data['magDec'] / 1.e2 / 180.0 * math.pi msg.mag_acc = data['magAcc'] / 1.e2 / 180.0 * math.pi self.ublox_position_velocity_time_pub.publish(msg) elif self.use_ublox_messages and 'UBX-RAW' in parsed_sentence: data = parsed_sentence['UBX-RAW'] msg = String() # transmitting binary data (bytes) directly is not possible anymore # with ROS and Python3, therefore base64 encoding is applied msg.data = base64.b64encode(data['raw']).decode('ascii') self.ublox_raw.publish(msg) else: return False
def navigation_handler(self, data): """ Rebroadcasts navigation data in the following formats: 1) /odom => /base footprint transform (if enabled, as per REP 105) 2) Odometry message, with parent/child IDs as in #1 3) NavSatFix message, for systems which are knowledgeable about GPS stuff 4) IMU messages """ rospy.logdebug("Navigation received") # If we don't have a fix, don't publish anything... if self.nav_status.status == NavSatStatus.STATUS_NO_FIX: return # UTM conversion easting, northing = data.bestxyz.easting, data.bestxyz.northing # Initialize starting point if we haven't yet # TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid? # TODO: check INSPVAX sol stat for valid position before accepting if not self.init and self.zero_start: self.origin.x = easting self.origin.y = northing self.init = True # Publish origin reference for others to know about p = Pose() p.position.x = self.origin.x p.position.y = self.origin.y self.pub_origin.publish(p) # IMU # TODO: Work out these covariances properly. Logs provide covariances in local frame, not body imu = Imu() imu.header.stamp == rospy.Time.now() imu.header.frame_id = self.base_frame # Orientation # orient=PyKDL.Rotation.RPY(RAD(data.roll),RAD(data.pitch),RAD(data.heading)).GetQuaternion() # imu.orientation = Quaternion(*orient) imu.orientation.x = data.inspvax.pitch imu.orientation.y = data.inspvax.roll imu.orientation.z = data.inspvax.azimuth imu.orientation.w = 0 IMU_ORIENT_COVAR[0] = POW(2, data.inspvax.pitch_std) IMU_ORIENT_COVAR[4] = POW(2, data.inspvax.roll_std) IMU_ORIENT_COVAR[8] = POW(2, data.inspvax.azimuth_std) imu.orientation_covariance = IMU_ORIENT_COVAR # Angular rates (rad/s) # corrimudata log provides instantaneous rates so multiply by IMU rate in Hz imu.angular_velocity.x = data.corrimudata.pitch_rate * self.imu_rate imu.angular_velocity.y = data.corrimudata.roll_rate * self.imu_rate imu.angular_velocity.z = data.corrimudata.yaw_rate * self.imu_rate imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration (m/s^2) imu.linear_acceleration.x = data.corrimudata.x_accel * self.imu_rate imu.linear_acceleration.y = data.corrimudata.y_accel * self.imu_rate imu.linear_acceleration.z = data.corrimudata.z_accel * self.imu_rate imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu) # Odometry # TODO: Work out these covariances properly odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = self.odom_frame odom.child_frame_id = self.base_frame odom.pose.pose.position.x = easting - self.origin.x odom.pose.pose.position.y = northing - self.origin.y odom.pose.pose.position.z = data.inspvax.altitude #odom.pose.pose.orientation = Quaternion(*orient) odom.pose.pose.orientation = imu.orientation POSE_COVAR[21] = IMU_ORIENT_COVAR[0] POSE_COVAR[28] = IMU_ORIENT_COVAR[4] POSE_COVAR[35] = IMU_ORIENT_COVAR[8] odom.pose.covariance = POSE_COVAR # Twist is relative to vehicle frame odom.twist.twist.linear.x = data.bestxyz.velx odom.twist.twist.linear.y = data.bestxyz.vely odom.twist.twist.linear.z = data.bestxyz.velz TWIST_COVAR[0] = pow(2, data.bestxyz.velx_std) TWIST_COVAR[7] = pow(2, data.bestxyz.vely_std) TWIST_COVAR[14] = pow(2, data.bestxyz.velz_std) odom.twist.twist.angular = imu.angular_velocity odom.twist.covariance = TWIST_COVAR self.pub_odom.publish(odom) # # Odometry transform (if required) # if self.publish_tf: self.tf_broadcast.sendTransform( (odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z), odom.pose.pose.orientation, #Quaternion(*orient), odom.header.stamp, odom.child_frame_id, odom.frame_id) # # NavSatFix # TODO: Work out these covariances properly from DOP # navsat = NavSatFix() navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame navsat.status = self.nav_status # position, in degrees navsat.latitude = data.bestpos.latitude navsat.longitude = data.bestpos.longitude navsat.altitude = data.bestpos.altitude NAVSAT_COVAR[0] = pow(2, data.bestpos.lat_std) # in meters NAVSAT_COVAR[4] = pow(2, data.bestpos.lon_std) NAVSAT_COVAR[8] = pow(2, data.bestpos.hgt_std) navsat.position_covariance = NAVSAT_COVAR navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN self.pub_navsatfix.publish(navsat)
def 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.roll, msg.pitch, msg.heading) 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.rollRate imumsg.angular_velocity.y = msg.pitchRate imumsg.angular_velocity.z = msg.headingRate imu_pub.publish(imumsg) if use_odom == True: br = tf.TransformBroadcaster() q = quaternion_from_euler(msg.roll, msg.pitch, msg.heading) 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.x odommsg.pose.pose.position.y = msg.y 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.vx odommsg.twist.twist.linear.y = msg.vy odommsg.twist.twist.linear.z = msg.vz odommsg.twist.twist.angular.x = msg.rollRate odommsg.twist.twist.angular.y = msg.pitchRate odommsg.twist.twist.angular.z = msg.headingRate odom_pub.publish(odommsg) br.sendTransform( (msg.x, msg.y, 0), tf.transformations.quaternion_from_euler(msg.roll, msg.pitch, msg.heading), rospy.Time.now(), "base_link", "odom")
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_pose_utm = GpsLocal() current_pose_utm.header.stamp = current_time current_pose_utm.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: self.seq = self.seq + 1 current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] gps_qual = data['fix_type'] try: # Unpack the fix params for this quality value current_fix.status.status, self.default_epe, self.fix_type = self.fix_mappings[ gps_qual] except KeyError: current_fix.status.status = NavSatStatus.STATUS_NO_FIX self.fix_type = 'Unknown' if gps_qual == 0: current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self.using_receiver_epe = False self.invalid_cnt += 1 current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude # use default epe std_dev unless we've received a GST sentence with epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = self.default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = self.default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = self.default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2 current_fix.position_covariance[8] = (hdop * self.alt_std_dev)**2 # FIXME # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not math.isnan(latitude) and not math.isnan(longitude) and ( gps_qual == 5 or gps_qual == 4): UTMNorthing, UTMEasting = LLtoUTM(latitude, longitude)[0:2] current_pose_utm.position.x = UTMEasting current_pose_utm.position.y = UTMNorthing current_pose_utm.position.z = altitude # Pose x/y/z covariance is whatever we decided h & v covariance is. # Here is it the same as for ECEF coordinates current_pose_utm.covariance[0] = (hdop * self.lon_std_dev)**2 current_pose_utm.covariance[4] = (hdop * self.lat_std_dev)**2 current_pose_utm.covariance[8] = (hdop * self.alt_std_dev)**2 current_pose_utm.rtk_fix = True if gps_qual == 4 else False self.local_pub.publish(current_pose_utm) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) if (self.diag_pub_time < rospy.get_time()): self.diag_pub_time += 1 diag_arr = DiagnosticArray() diag_arr.header.stamp = rospy.get_rostime() diag_arr.header.frame_id = frame_id diag_msg = DiagnosticStatus() diag_msg.name = 'GPS_status' diag_msg.hardware_id = 'GPS' diag_msg.level = DiagnosticStatus.OK diag_msg.message = 'Received GGA Fix' diag_msg.values.append( KeyValue('Sequence number', str(self.seq))) diag_msg.values.append( KeyValue('Invalid fix count', str(self.invalid_cnt))) diag_msg.values.append(KeyValue('Latitude', str(latitude))) diag_msg.values.append(KeyValue('Longitude', str(longitude))) diag_msg.values.append(KeyValue('Altitude', str(altitude))) diag_msg.values.append(KeyValue('GPS quality', str(gps_qual))) diag_msg.values.append(KeyValue('Fix type', self.fix_type)) diag_msg.values.append( KeyValue('Number of satellites', str(data['num_satellites']))) diag_msg.values.append( KeyValue('Receiver providing accuracy', str(self.using_receiver_epe))) diag_msg.values.append(KeyValue('Hdop', str(hdop))) diag_msg.values.append( KeyValue('Latitude std dev', str(hdop * self.lat_std_dev))) diag_msg.values.append( KeyValue('Longitude std dev', str(hdop * self.lon_std_dev))) diag_msg.values.append( KeyValue('Altitude std dev', str(hdop * self.alt_std_dev))) diag_arr.status.append(diag_msg) self.diag_pub.publish(diag_arr) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] else: return False
data_bytes = data_bytes + rec_str # print(count) #print('当前数据接收总字节数:'+str(len(data_bytes))+' 本次接收字节数:'+str(len(rec_str))) #print(str(datetime.now()),':',binascii.b2a_hex(rec_str)) serialPort = '/dev/ttyACM0' # 串口 baudRate = 9600 # 波特率 is_exit = False data_bytes = bytearray() if __name__ == '__main__': rospy.init_node("gps_sensor", anonymous=True) position_pub = rospy.Publisher('position', NavSatFix, queue_size=10) msg_position = NavSatFix() msg_position.latitude = 0 msg_position.longitude = 0 lat = 0 lon = 0 vx = 0 vy = 0 #打开串口 mSerial = SerialPort(serialPort, baudRate) #开始数据读取线程 t1 = threading.Thread(target=mSerial.read_data) t1.setDaemon(True) t1.start() while not rospy.is_shutdown(): #主线程:对读取的串口数据进行处理
def ros_pub(topic, data): global pub_location, gps_data_save, pub_service_mode print("topic::", topic) print("data::", data, type(data)) print("==============================") latlng = NavSatFix() service_mode = UInt8() if (topic.find("/pending") > 1): service_mode.data = ServiceMode.Pending pub_service_mode.publish(service_mode) callDriveService(DriveMode.lock, "") elif (topic.find("/gps_summit") > 1): print("data::gps_summit", data) latlng.header.stamp = rospy.Time.now() latlng.header.frame_id = "world" latlng.latitude = float(data['od_data']['origin']['lat']) latlng.longitude = float(data['od_data']['origin']['lng']) # latlng.latitude = float(35.6479613028) # latlng.longitude = float(134.032092814) latlng.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN print(":::gps_summit:::latlng::", latlng) gps_data_save = data pub_location.publish(latlng) service_mode.data = ServiceMode.Auto pub_service_mode.publish(service_mode) elif (topic.find("/gps_confirm") > 1): latlng.header.stamp = rospy.Time.now() latlng.header.frame_id = "world" latlng.latitude = float(gps_data_save['od_data']['destination']['lat']) latlng.longitude = float( gps_data_save['od_data']['destination']['lng']) # latlng.latitude = float(35.6478800523) # latlng.longitude = float(134.032039782) latlng.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN print(":::gps_confirm:::latlng::", latlng) pub_location.publish(latlng) service_mode.data = ServiceMode.Auto pub_service_mode.publish(service_mode) elif (topic.find("/riding") > 1): print("data::riding", data) service_mode.data = ServiceMode.Manual pub_service_mode.publish(service_mode) callDriveService(DriveMode.release, "") elif (topic.find("/following") > 1): print("data::following", data) service_mode.data = ServiceMode.Following pub_service_mode.publish(service_mode) callDriveService(DriveMode.auto_mode, "tag_cmd_vel") elif (topic.find("/customize") > 1): service_mode.data = ServiceMode.Pending pub_service_mode.publish(service_mode) print("data::customize", data) # Setting device control # print(data['light_color']) get_data = data['light_color'] get_data = list(get_data.replace("#", "")) # print("get_data",get_data) r_data = int("0x" + get_data[0] + get_data[1], 16) g_data = int("0x" + get_data[2] + get_data[3], 16) b_data = int("0x" + get_data[4] + get_data[5], 16) # print("r_data",r_data,g_data,b_data) if (data['hvac_active'] == True): msg_sended = Bool() msg_sended.data = True pub_hvac_front.publish(msg_sended) pub_hvac_back.publish(msg_sended) elif (data['hvac_active'] == False): msg_sended = Bool() msg_sended.data = False pub_hvac_front.publish(msg_sended) pub_hvac_back.publish(msg_sended) if (data['light_active'] == True): msg_sended = Bool() msg_sended.data = True ColorRGBA_msg = ColorRGBA() ColorRGBA_msg.r = r_data ColorRGBA_msg.g = g_data ColorRGBA_msg.b = b_data ColorRGBA_msg.a = 0 pub_light_color.publish(ColorRGBA_msg) elif (data['light_active'] == False): msg_sended = Bool() msg_sended.data = False ColorRGBA_msg = ColorRGBA() ColorRGBA_msg.r = 0 ColorRGBA_msg.g = 0 ColorRGBA_msg.b = 0 ColorRGBA_msg.a = 0 pub_light_color.publish(ColorRGBA_msg) # 000 -> close else: print("no get setting topic")
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 nmea_depth_udp(): # Init node rospy.init_node("nmea_depth_udp", anonymous=True) rospy.loginfo("[nmea_depth_udp] Initializing node...") rate = rospy.Rate(10) # 10 Hz # Parameters udp_addr = rospy.get_param('~address', '') udp_port = rospy.get_param('~port', 12021) # Random palindrome port device_frame_id = rospy.get_param('~frame_id', "") # Start UDP socket (defaults to any IP and port 12021) udp_in.bind((udp_addr, udp_port)) # Publishers sentence_pub = rospy.Publisher("%s/nmea_sentence" % device_frame_id, Sentence, queue_size=10) position_pub = rospy.Publisher("%s/fix" % device_frame_id, NavSatFix, queue_size=10) vel_pub = rospy.Publisher("%s/vel" % device_frame_id, TwistStamped, queue_size=10) timeref_pub = rospy.Publisher("%s/time_reference" % device_frame_id, TimeReference, queue_size=10) depth_below_trans_pub = rospy.Publisher("%s/depth/below_transducer" % device_frame_id, DepthBelowTransducer, queue_size=10) depth_water_pub = rospy.Publisher("%s/depth/water" % device_frame_id, DepthOfWater, queue_size=10) rospy.loginfo("[nmea_depth_udp] Initialization done.") rospy.loginfo("[nmea_depth_udp] Published topics:") rospy.loginfo("[nmea_depth_udp] Sentence:\t\t\t%s/nmea_sentence" % device_frame_id) rospy.loginfo("[nmea_depth_udp] GPS Fix:\t\t\t%s/fix" % device_frame_id) rospy.loginfo("[nmea_depth_udp] GPS Velocity:\t\t%s/vel" % device_frame_id) rospy.loginfo("[nmea_depth_udp] Time Reference:\t\t%s/time_reference" % device_frame_id) rospy.loginfo("[nmea_depth_udp] Depth of Water:\t\t%s/depth/water" % device_frame_id) rospy.loginfo( "[nmea_depth_udp] Depth below transducer:\t%s/depth/below_transducer" % device_frame_id) # Run node while not rospy.is_shutdown(): try: nmea_in, _ = udp_in.recvfrom(1024) except socket.error: pass ros_now = rospy.Time().now() nmea_parts = nmea_in.strip().split(',') if len(nmea_parts): try: # GPS Fix position if nmea_parts[0] == '$GPGGA' and len(nmea_parts) >= 10: latitude = int( nmea_parts[2][0:2]) + float(nmea_parts[2][2:]) / 60.0 if nmea_parts[3] == 'S': latitude = -latitude longitude = int( nmea_parts[4][0:3]) + float(nmea_parts[4][3:]) / 60.0 if nmea_parts[5] == 'W': longitude = -longitude # altitude = float(nmea_parts[9]) nsf = NavSatFix() nsf.header.stamp = ros_now nsf.header.frame_id = device_frame_id nsf.latitude = latitude nsf.longitude = longitude # nsf.altitude = altitude position_pub.publish(nsf) # Velocity if nmea_parts[0] == '$GPVTG' and len(nmea_parts) >= 9: vel = TwistStamped() vel.header.frame_id = device_frame_id vel.header.stamp = ros_now vel.twist.linear.x = float( nmea_parts[7]) / 3.6 # Km/h to m/s vel_pub.publish(vel) # Time reference (GPST) if nmea_parts[0] == '$GPZDA' and len(nmea_parts) >= 5: tref = TimeReference() tref.header.frame_id = device_frame_id tref.header.stamp = ros_now hour = int(nmea_parts[1][0:2]) minute = int(nmea_parts[1][2:4]) second = int(nmea_parts[1][4:6]) try: ms = int(float(nmea_parts[1][6:]) * 1000000) except ValueError: ms = 0 day = int(nmea_parts[2]) month = int(nmea_parts[3]) year = int(nmea_parts[4]) zda = datetime.datetime(year, month, day, hour, minute, second, ms) tref.time_ref = rospy.Time( calendar.timegm(zda.timetuple()), zda.microsecond * 1000) tref.source = device_frame_id timeref_pub.publish(tref) # Depth (DBT - Depth Below Transducer) if nmea_parts[0] == '$SDDBT' and len(nmea_parts) >= 7: d = DepthBelowTransducer() d.header.frame_id = device_frame_id d.header.stamp = ros_now try: d.feet = float(nmea_parts[1]) # In feet except ValueError: pass try: d.meters = float(nmea_parts[3]) # In meters except ValueError: pass try: d.fathoms = float(nmea_parts[5]) # In fathoms except ValueError: pass depth_below_trans_pub.publish(d) # Depth (DPT - DePTh of water) if nmea_parts[0] == '$SDDPT' and len(nmea_parts) >= 4: d = DepthOfWater() d.header.frame_id = device_frame_id d.header.stamp = ros_now try: d.depth = float(nmea_parts[1]) # In meters except ValueError: pass try: d.offset = float(nmea_parts[2]) # In meters except ValueError: pass try: d.range = float(nmea_parts[3]) except ValueError: pass depth_water_pub.publish(d) #### Other possible parsings (from Heck's provided logs) # GPGLL - Geographic Latitude and Longitude (legacy sentence, same info as contained in GPGGA) # GPGSA - Gps dillution of position and active SAtellites # GPGSV - Gps Satellites in View # GPRMC - Recommendec Minimum navigation type C (includes latitude, longitude, speed in knots, date... all info already available on other messages) # SDMTW - Mean Temperature of Water # SDVHW - Velocity and Heading in Water (Water speed in knots/kilometers-per-hour and heading in magnetic degrees) # SDHDG - magnetic HeaDinG (in degrees, with deviation and variation) except ValueError: pass # NMEA Sentence (published regardless of content) sentence_msg = Sentence() sentence_msg.header.frame_id = device_frame_id sentence_msg.header.stamp = ros_now sentence_msg.sentence = nmea_in sentence_pub.publish(sentence_msg) # Node sleeps for 10 Hz rate.sleep()
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 bag = rosbag.Bag(sys.argv[2], 'w') ral = csv.reader(open(sys.argv[1] + "/LogFiles_tmp/RawAccel.csv")) rgo = csv.reader(open(sys.argv[1] + "/LogFiles_tmp/RawGyro.csv")) gps = csv.reader(open(sys.argv[1] + "/LogFiles_tmp/OnboardGPS.csv")) gta = csv.reader(open(sys.argv[1] + "/LogFiles_tmp/GroundTruthAGM.csv")) gtl = csv.reader(open(sys.argv[1] + "/LogFiles_tmp/GroundTruthAGL.csv")) bap = csv.reader(open(sys.argv[1] + "/LogFiles_tmp/BarometricPressure.csv")) onp = csv.reader(open(sys.argv[1] + "/LogFiles_tmp/OnboardPose.csv")) ori = csv.reader(open(sys.argv[1] + "/LogFiles_tmp/RawOrien.csv")) pos = csv.reader(open(sys.argv[1] + "/LogFiles_tmp/OnboardPose.csv")) ral_seq = 0 bap_seq = 0 img_seq = 0 pos_seq = 0 cal = -1 gps_seq = 0 # IMAGE_COUNT = 81169 STREET_VIEW = 113 print("packageing Imu for OnboardPose.csv ...") for pos_data in pos: pos_seq = pos_seq + 1 utime = int(pos_data[0]) timestamp = rospy.Time.from_sec(utime/1e6) imu = Imu() imu.header.seq = pos_seq imu.header.stamp = timestamp imu.header.frame_id = '/Imu' imu.linear_acceleration.x = float(pos_data[4]) imu.linear_acceleration.y = float(pos_data[5]) imu.linear_acceleration.z = float(pos_data[6]) imu.linear_acceleration_covariance = np.zeros(9) imu.angular_velocity.x = float(pos_data[1]) imu.angular_velocity.y = float(pos_data[2]) imu.angular_velocity.z = float(pos_data[3]) imu.angular_velocity_covariance = np.zeros(9) imu.orientation.w = float(pos_data[14]) imu.orientation.x = float(pos_data[15]) imu.orientation.y = float(pos_data[16]) imu.orientation.z = float(pos_data[17]) bag.write('/Imu', imu, t=timestamp) pos_seq = pos_seq + 1 imu.header.seq = pos_seq bag.write('/Imu', imu, t=timestamp) # if cal < 0: # Caminfo = CameraInfo() # cam_data = np.load(sys.argv[1] + '/calibration_data.npz') # Caminfo.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] # Caminfo.P = [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0] # Caminfo.D = np.asarray(cam_data['distCoeff']).reshape(-1) # Caminfo.K = np.asarray(cam_data['intrinsic_matrix']).reshape(-1) # Caminfo.binning_x = 1 # Caminfo.binning_y = 1 # img_cv_h, img_cv_w = img_cv.shape[:2] # Caminfo.width = img_cv_w # Caminfo.height = img_cv_h # Caminfo.distortion_model = 'plumb_bob' # bag.write('/camera/camera_info', Caminfo, t=timestamp) # cal = 0 print("Package groundtruthAGM...") for gta_data in gta: Igt = Int8MultiArray() Igt.data = [int(gta_data[2]), int(gta_data[3]), int(gta_data[4])] bag.write('/groundtruth/sv_id', Igt) print("Package groundtruthIMAGE...") for stv_seq in range(STREET_VIEW): # print(stv_seq) img_cv = cv2.imread(sys.argv[1] + "/Street View Images/left-" + '{0:03d}'.format(stv_seq+1) + ".jpg", 1) br = CvBridge() Img = Image() Img = br.cv2_to_imgmsg(img_cv, "bgr8") Img.header.seq = stv_seq # Img.header.stamp = timestamp Img.header.frame_id = 'streetview' bag.write('/groundtruth/image', Img) for bap_data in bap: bap_seq = bap_seq + 1 bar = Barometer() bar.altitude = float(bap_data[2]) bar.pressure = float(bap_data[1]) bar.temperature = float(bap_data[3]) bar.header.seq - int(bap_seq) bar.header.frame_id = 'BarometricPressure' bag.write('/barometric_pressure', bar) print("Packaging GPS and cam_info") for gps_data in gps: imgid = int(gps_data[1]) utime = int(gps_data[0]) timestamp = rospy.Time.from_sec(utime / 1e6) header = Header() header.seq = imgid header.stamp = timestamp header.frame_id = 'gps' Gps = NavSatFix() Gps.header = header Gps.status.service = 1 Gps.latitude = float(gps_data[2]) Gps.longitude = float(gps_data[3]) Gps.altitude = float(gps_data[4]) bag.write('/gps', Gps, t=timestamp) if imgid <= MAVIMAGE and imgid % 3 == 0: # write aerial image img_seq = img_seq+1 img_cv = cv2.imread(sys.argv[1] + "/MAV Images/" + '{0:05d}'.format(int(imgid)) + ".jpg", 1) img_cv = cv2.resize(img_cv, MAVDIM, interpolation=cv2.INTER_AREA) cv2.imshow('1', img_cv) br = CvBridge() Img = Image() Img = br.cv2_to_imgmsg(img_cv, "bgr8") Img.header.seq = int(img_seq) print(imgid) Img.header.stamp = timestamp Img.header.frame_id = 'camera' bag.write('/camera/image', Img, t=timestamp) bag.close() return 0
def code(self): micro = 1000000 acc_l = False gyro = False orien = False quat = Quaternion() imuMsg = Imu() gpsMsg = NavSatFix() magMsg = MagneticField() #rate = rospy.Rate(1000) last = rospy.Time.now().to_sec() imuMsg.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] imuMsg.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0.02] imuMsg.linear_acceleration_covariance = [ 0.2, 0, 0, 0, 0.2, 0, 0, 0, 0.2 ] gpsMsg.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] while not rospy.is_shutdown(): data_pix = ser1.read() if data_pix == ">": data_pix = ser1.readline() data_list = data_pix.split(",") #a=len(data_list) #print(a) #print(data_list) tipo = float(data_list[0]) xx = float(data_list[2]) yy = float(data_list[3]) zz = float(data_list[4]) if tipo == 1: #ACCELEROMETER (m/s^2 - X,Y,Z) acc = True elif tipo == 2: #MAGNETIC_FIELD (uT - X,Y,Z) mag = True magMsg.header.stamp = rospy.Time.now() magMsg.header.frame_id = 'base_link' magMsg.magnetic_field.x = xx / micro magMsg.magnetic_field.y = yy / micro magMsg.magnetic_field.z = zz / micro self.pub_mag.publish(magMsg) elif tipo == 3: #ORIENTATION (Yaw, Pitch, Roll) orien = True # orientation to ENU Right Hand roll = -zz #*3.14159/180 pitch = yy #*3.18159/180 yaw = -xx + 90 #*3.14159/180 if yaw < -180: yaw = yaw + 360 #q=tf.transformations.quaternion_from_euler(roll*3.14159/180,pitch*3.18159/180,yaw*3.14159/180) #imuMsg.orientation = Quaternion(*q) imuMsg.orientation.x = roll #magnetometer imuMsg.orientation.y = pitch imuMsg.orientation.z = yaw imuMsg.orientation.w = 0 #q=tf.transformations.quaternion_from_euler(zz*3.14159/180,yy*3.18159/180,xx*3.14159/180)''' #quat.x = q[0] #magnetometer #quat.y = q[1] #quat.z = q[2] #quat.w = q[3] elif tipo == 4: #GYROSCOPE (rad/sec - X,Y,Z) gyro = True imuMsg.angular_velocity.x = xx #gyro imuMsg.angular_velocity.y = yy imuMsg.angular_velocity.z = zz elif tipo == 5: #LIGHT (SI lux) lux = True elif tipo == 6: #PRESSURE (hPa millibar) press = True elif tipo == 7: #DEVICE TEMPERATURE (C) temp = True elif tipo == 8: #PROXIMITY (Centimeters or 1,0) prox = True elif tipo == 9: #GRAVITY (m/s^2 - X,Y,Z) grav = True elif tipo == 10: #LINEAR_ACCELERATION (m/s^2 - X,Y,Z) acc_l = True imuMsg.linear_acceleration.x = xx # tripe axis accelerator meter imuMsg.linear_acceleration.y = yy imuMsg.linear_acceleration.z = zz elif tipo == 11: #ROTATION_VECTOR (Degrees - X,Y,Z) rot = True elif tipo == 12: #RELATIVE_HUMIDITY (%) hum = True elif tipo == 13: #AMBIENT_TEMPERATURE (C) amb_temp = True elif tipo == 14: #MAGNETIC_FIELD_UNCALIBRATED (uT - X,Y,Z) mag_u = True elif tipo == 15: #GAME_ROTATION_VECTOR (Degrees - X,Y,Z) game_rot = True elif tipo == 16: #GYROSCOPE_UNCALIBRATED (rad/sec - X,Y,Z) gyro_u = True elif tipo == 17: #SIGNIFICANT_MOTION (1,0) sig_mot = True elif tipo == 97: #AUDIO (Vol.)} audio = True elif tipo == 98: #GPS1 (Lat., Long., Alt.) gps1 = True gpsMsg.header.stamp = rospy.Time.now() gpsMsg.header.frame_id = 'base_link' gpsMsg.latitude = xx gpsMsg.longitude = yy gpsMsg.altitude = zz gpsMsg.position_covariance_type = 0 gpsMsg.status.service = NavSatStatus.SERVICE_GPS gpsMsg.status.status = NavSatStatus.STATUS_SBAS_FIX #SOLO SAT--STATUS_GBAS_FIX ANTENA TIERRA self.pub_navsatfix.publish(gpsMsg) elif tipo == 99: #GPS2 (Bearing, Speed, Date/Time) gps2 = True #print('tipo=',tipo,'x=',xx,'y=',yy,'z=',zz,rospy.Time.now().to_sec()-last) #rate.sleep() ## until the msg is complete if orien == True and acc_l == True and gyro == True: imuMsg.header.stamp = rospy.Time.now() imuMsg.header.frame_id = 'base_link' self.pub_imu.publish(imuMsg) acc_l = False gyro = False orien = False
def main(args): if len(sys.argv) < 2: print 'Please specify gps file' return 1 if len(sys.argv) < 3: print 'Please specify output rosbag file' return 1 gps = np.loadtxt(sys.argv[1], delimiter=",") utimes = gps[:, 0] modes = gps[:, 1] num_satss = gps[:, 2] lats = gps[:, 3] lngs = gps[:, 4] alts = gps[:, 5] tracks = gps[:, 6] speeds = gps[:, 7] bag = rosbag.Bag(sys.argv[2], 'w') try: for i, utime in enumerate(utimes): timestamp = rospy.Time.from_sec(utime / 1e6) status = NavSatStatus() if modes[i] == 0 or modes[i] == 1: status.status = NavSatStatus.STATUS_NO_FIX else: status.status = NavSatStatus.STATUS_FIX status.service = NavSatStatus.SERVICE_GPS num_sats = UInt16() num_sats.data = num_satss[i] fix = NavSatFix() fix.status = status fix.latitude = np.rad2deg(lats[i]) fix.longitude = np.rad2deg(lngs[i]) fix.altitude = alts[i] track = Float64() track.data = tracks[i] speed = Float64() speed.data = speeds[i] bag.write('fix', fix, t=timestamp) bag.write('track', track, t=timestamp) bag.write('speed', speed, t=timestamp) finally: bag.close() return 0
def posmv_nmea_listener(): position_pub = rospy.Publisher('/base/position', NavSatFix, queue_size=10) timeref_pub = rospy.Publisher('/base/time_reference', TimeReference, queue_size=10) orientation_pub = rospy.Publisher('/base/orientation', NavEulerStamped, queue_size=10) rospy.init_node('posmv_nmea') input_type = rospy.get_param('/posmv_nmea/input_type') input_address = rospy.get_param('/posmv_nmea/input', '') input_speed = rospy.get_param('/posmv_nmea/input_speed', 0) input_port = int(rospy.get_param('/posmv_nmea/input_port', 0)) output_port = int(rospy.get_param('/posmv_nmea/output', 0)) output_address = rospy.get_param('/posmv_nmea/output_address', '<broadcast>') if input_type == 'serial': serial_in = serial.Serial(input_address, int(input_speed)) else: udp_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) udp_in.bind(('', input_port)) if output_port > 0: udp_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) udp_out.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) else: udp_out = None timestamp = datetime.datetime.utcfromtimestamp( rospy.Time.now().to_time()).isoformat() bag = rosbag.Bag( 'nodes/posmv_nmea_' + ('-'.join(timestamp.split(':'))) + '.bag', 'w', rosbag.Compression.BZ2) while not rospy.is_shutdown(): if input_type == 'serial': nmea_in = serial_in.readline() #print nmea_in if udp_out is not None: udp_out.sendto(nmea_in, (output_address, output_port)) else: nmea_in, addr = udp_in.recvfrom(1024) #print addr, nmea_in now = rospy.get_rostime() nmea_parts = nmea_in.strip().split(',') if len(nmea_parts): #print nmea_parts try: if nmea_parts[0] == '$GPZDA' and len(nmea_parts) >= 5: tref = TimeReference() tref.header.stamp = now hour = int(nmea_parts[1][0:2]) minute = int(nmea_parts[1][2:4]) second = int(nmea_parts[1][4:6]) ms = int(float(nmea_parts[1][6:]) * 1000000) day = int(nmea_parts[2]) month = int(nmea_parts[3]) year = int(nmea_parts[4]) zda = datetime.datetime(year, month, day, hour, minute, second, ms) tref.time_ref = rospy.Time( calendar.timegm(zda.timetuple()), zda.microsecond * 1000) tref.source = 'posmv' timeref_pub.publish(tref) bag.write('/posmv_nmea/time_reference', tref) if nmea_parts[0] == '$GPGGA' and len(nmea_parts) >= 10: latitude = int( nmea_parts[2][0:2]) + float(nmea_parts[2][2:]) / 60.0 if nmea_parts[3] == 'S': latitude = -latitude longitude = int( nmea_parts[4][0:3]) + float(nmea_parts[4][3:]) / 60.0 if nmea_parts[5] == 'W': longitude = -longitude altitude = float(nmea_parts[9]) nsf = NavSatFix() nsf.header.stamp = now nsf.header.frame_id = 'posmv_operator' nsf.latitude = latitude nsf.longitude = longitude nsf.altitude = altitude position_pub.publish(nsf) bag.write('/posmv_nmea/position', nsf) if nmea_parts[0] == '$GPHDT' and len(nmea_parts) >= 2: heading = float(nmea_parts[1]) nes = NavEulerStamped() nes.header.stamp = now nes.header.frame_id = 'posmv_operator' nes.orientation.heading = heading orientation_pub.publish(nes) bag.write('/posmv_nmea/orientation', nes) except ValueError: pass bag.close()
break if msg.name() == "NAV_POSLLH": msgstrl = str(msg).split(",")[1:] longitude = float(msgstrl[0].split('=')[-1]) / 10000000 latitude = float(msgstrl[1].split('=')[-1]) / 10000000 height = float(msgstrl[2].split('=')[-1]) / 1000 hMSL = float(msgstrl[3].split('=')[-1]) / 1000 hACC = float(msgstrl[4].split('=')[-1]) / 1000 vACC = float(msgstrl[5].split('=')[-1]) / 1000 navmsg = NavSatFix() navmsg.header.stamp = rospy.Time.now() navmsg.header.frame_id = 'gps' navmsg.header.seq = count count += 1 navmsg.latitude = latitude navmsg.longitude = longitude navmsg.altitude = height navmsg.position_covariance_type = navmsg.COVARIANCE_TYPE_KNOWN navmsg.position_covariance = [ hACC, hACC, 0, hACC, hACC, 0, 0, 0, vACC ] if valid_gps == 0: navmsg.status.status = -1 else: navmsg.status.status = 0 navmsg.status.service = 1 pub.publish(navmsg) if msg.name() == "NAV_STATUS": msgstrl = str(msg).split(",")[1:2]
if __name__ == "__main__": rospy.init_node("fake_waterlinked", log_level=rospy.INFO) heading_offset = rospy.get_param( "~heading" ) # heading is given by waterlinked GPS in degrees CW from North master_datum = rospy.get_param( "~datum") # Master located (latitude, longitude) lat, lon, alt = master_datum + [0.0] if len( master_datum) < 3 else master_datum master_gps = rospy.Publisher('gps_datum', NavSatFix, queue_size=5) master_msg = NavSatFix() master_msg.altitude = alt master_msg.longitude = lon master_msg.latitude = lat master_msg.status = NavSatStatus() master_msg.status.status = master_msg.status.STATUS_FIX master_msg.status.service = master_msg.status.SERVICE_GPS master_msg.position_covariance_type = master_msg.COVARIANCE_TYPE_UNKNOWN master_msg.position_covariance = [-1, 0, 0, 0, 0, 0, 0, 0, 0] pose_pub = rospy.Publisher('waterlinked/pose_with_cov_stamped', PoseWithCovarianceStamped, queue_size=5) buff = Buffer() TransformListener(buff) rospy.Subscriber("mavros/global_position/global", NavSatFix, handle_gps, (pose_pub, buff)) map_to_waterlink = TransformStamped( Header(0, rospy.Time.now(), 'map'), 'waterlinked', Transform(
gpstime.header.stamp = gpsVel.header.stamp gpstime.time_ref = convertNMEATimeToROS( fields[1]) longitude = float( fields[5][0:3]) + float(fields[5][3:]) / 60 if fields[6] == 'W': longitude = -longitude latitude = float( fields[3][0:2]) + float(fields[3][2:]) / 60 if fields[4] == 'S': latitude = -latitude #publish data navData.latitude = latitude navData.longitude = longitude navData.altitude = float('NaN') navData.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN gpspub.publish(navData) gpstimePub.publish(gpstime) else: pass #print data else: #Use GGA #No /vel output from just GGA if 'GGA' in fields[0]: gps_quality = int(fields[6]) if gps_quality == 0: navData.status.status = NavSatStatus.STATUS_NO_FIX
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) # print parsed_sentence if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude hdop = data['hdop'] current_fix.position_covariance[0] = hdop**2 current_fix.position_covariance[4] = hdop**2 current_fix.position_covariance[8] = (2 * hdop)**2 # FIXME current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] 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 = 0. # 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) else: return False
depth_pub = rospy.Publisher('/depth', Vector3Stamped, queue_size=1) img_pub = rospy.Publisher('/stereo/right/image_raw', Image, queue_size=1) rec_pub = rospy.Publisher('/recording', Bool, queue_size=1) log_pub = rospy.Publisher('/logs', String, queue_size=1) log_options = ['Starting logs to /media/data/logs.bag', 'Running all the logs', 'Finishing logs /media/data/logs.bag', 'Shutting down'] # log_options = ['Running all the logs', 'Shutting down', 'Starting up', 'Shutting down', 'Starting up'] count = 0 log_idx = 0 bridge = CvBridge() while not rospy.is_shutdown(): # Publish lat,lon navmsg = NavSatFix() navmsg.latitude = np.random.uniform(-33.0,-32.0) navmsg.longitude = np.random.uniform(151.0,152.0) navmsg.altitude = np.random.uniform(-5.0,0.0) navpub.publish(navmsg) # Publish battery state batmsg = BatteryState() batmsg.voltage = np.random.uniform(14.5,15.5) batmsg.current = np.random.uniform(0.0, 2.0) batmsg.capacity = np.random.uniform(0.0, 1.0) battery_pub.publish(batmsg) # Depth vmsg = Vector3Stamped() vmsg.header.frame_id = 'header' vmsg.header.seq = count
def gps_waypoint_cb(self, data): nav = NavSatFix() nav.latitude = data.x nav.longitude = data.y self.gps_translator_pub.publish(nav)
def fake_auto_demo(): rospy.init_node('autonomous_demo23') #Requirements pub1 = rospy.Publisher('/gps/fix', NavSatFix, queue_size=10) pub2 = rospy.Publisher('/imu/data', Imu, queue_size=10) pub3 = rospy.Publisher('/odometry/wheel', Odometry, queue_size=10) pub4 = rospy.Publisher('/gps_waypoint_handler/status/gps/fix', String, queue_size=10) pub5 = rospy.Publisher('/pass_gate_topic', String, queue_size=10) #Autonomous movement #pub5 = rospy.Publisher('/move_base/status',GoalStatusArray,queue_size=10) #Image Detect pub6 = rospy.Publisher('/artag_detect_topic', String, queue_size=10) #Image Reach pub7 = rospy.Publisher('/artag_reach_topic', String, queue_size=10) pub8 = rospy.Publisher('/done_app_topic', String, queue_size=10) rate = rospy.Rate(10) # 10hz count = 0 while not rospy.is_shutdown(): print("0: All sensors are good.") print("1: All sensors except encoder are good.") print("2: Damaged Sensors") print("3: Got Waypoint") print("4: Did not get any waypoint") print("5: Reached to way point") print("6: Did not Reached to way point") print("7: Detected AR Tag") print("8: Did not detect AR Tag") print("9: Reached AR Tag") print("10: Did not reached AR Tag") imuMsg = Imu() imuMsg.orientation.x = 5 imuMsg.orientation.y = 5 gpsMsg = NavSatFix() gpsMsg.latitude = 5 gpsMsg.longitude = 5 encoderMsg = Odometry() encoderMsg.pose.pose.position.x = 5 encoderMsg.pose.pose.position.y = 5 wpStatusMsgLow = GoalStatus() wpStatusMsgLow.status = 3 wpStatusArray = [] wpStatusArray.append(wpStatusMsgLow) wpStatusMsg = GoalStatusArray() wpStatusMsg.status_list = wpStatusArray userInput = raw_input() if userInput == "0": #All sensors are good. pub1.publish(gpsMsg) pub2.publish(imuMsg) pub3.publish(encoderMsg) elif userInput == "1": # All sensors except encoder are good. pub1.publish(gpsMsg) pub2.publish(imuMsg) #pub3.publish("0") elif userInput == "2": #Damaged Sensors pub1.publish(gpsMsg) #pub2.publish("0") pub3.publish(encoderMsg) elif userInput == "3": # Got Waypoint pub4.publish("1") elif userInput == "4": #Did not get any waypoint pub4.publish("0") elif userInput == "5": #Reached to way point #pub5.publish(wpStatusMsg) pub4.publish("2") #elif userInput == "6": #Did not reached to way point #pub5.publish("0") elif userInput == "7": #Detected Image pub6.publish("1") #pub5.publish("1") elif userInput == "8": #Did not Detect Image pub6.publish("0") elif userInput == "9": #Reached Image pub7.publish("1") elif userInput == "10": #Did not reached image pub7.publish("0") elif userInput == "11": pub5.publish("1") elif userInput == "12": pub8.publish("1") else: print("Invalid entry") rate.sleep()
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] if gps_qual > 0: self.valid_fix = True else: self.valid_fix = False current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with # epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2 current_fix.position_covariance[8] = ( 2 * hdop * self.alt_std_dev) ** 2 # FIXME self.fix_pub.publish(current_fix) if not (math.isnan(data['utc_time'][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
#!/usr/bin/python # -*-coding: utf-8 -*- import serial import threading import binascii from datetime import datetime import struct import csv import time import rospy from sensor_msgs.msg import NavSatFix if __name__ == '__main__': try: pos_pub = rospy.Publisher('position', NavSatFix, queue_size=10) rospy.init_node("sensor_ggps", anonymous=True) position = NavSatFix() rate = rospy.Rate(10) while not rospy.is_shutdown(): position.latitude = 29.89877 position.longitude = 121.527707 pos_pub.publish(position) rate.sleep() except rospy.ROSInterruptException: pass
def publish_gps_way_point(self, lon, lat, altitude=0): way_point_msg = NavSatFix() way_point_msg.longitude = lon way_point_msg.latitude = lat way_point_msg.altitude = altitude self.ros_pub_gps_way_point.publish(way_point_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" % nmea_string) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_heading = Imu() current_heading.header.stamp = current_time current_heading.header.frame_id = 'base_footprint' current_direction = String() # For testing current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id # Add capture/publishing heading info if not self.use_RMC and 'HDT' in parsed_sentence: #rospy.loginfo("HDT!") data = parsed_sentence['HDT'] tempHeading = data['true_heading'] ccHeading = (2 * math.pi) - tempHeading q = tf.transformations.quaternion_from_euler(0, 0, ccHeading) current_heading.orientation.x = q[0] current_heading.orientation.y = q[1] current_heading.orientation.z = q[2] current_heading.orientation.w = q[3] #current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) #if (current_heading.data < .3927): current_direction.data = "N" #elif (current_heading.data < 1.178): current_direction.data = "NE" #elif (current_heading.data < 1.9635): current_direction.data = "E" #elif (current_heading.data < 2.74889): current_direction.data = "SE" #elif (current_heading.data < 3.53429): current_direction.data = "S" #elif (current_heading.data < 4.31969): current_direction.data = "SW" #elif (current_heading.data < 5.10509): current_direction.data = "W" #elif (current_heading.data < 5.89048): current_direction.data = "NW" #else: current_direction.data = "N" self.heading_pub.publish(current_heading) #self.direction_pub.publish(current_direction) #self.time_ref_pub.publish(current_time_ref) elif 'GGA' in parsed_sentence: #rospy.loginfo("GGA!") data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude hdop = data['hdop'] current_fix.position_covariance[0] = hdop**2 current_fix.position_covariance[4] = hdop**2 current_fix.position_covariance[8] = (2 * hdop)**2 # FIXME current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.fix_pub.publish(current_fix) self.time_ref_pub.publish(current_time_ref) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.fix_pub.publish(current_fix) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) else: return False
def ais_listener(logdir=None): position_pub = rospy.Publisher('/base/position', NavSatFix, queue_size=10) timeref_pub = rospy.Publisher('/base/time_reference', TimeReference, queue_size=10) ais_pub = rospy.Publisher('/base/ais/contacts', Contact, queue_size=10) ais_raw_pub = rospy.Publisher('/base/ais/raw', Heartbeat, queue_size=10) rospy.init_node('ais') input_type = rospy.get_param('/ais/input_type') input_address = rospy.get_param('/ais/input', '') input_speed = rospy.get_param('/ais/input_speed', 0) input_port = int(rospy.get_param('/ais/input_port', 0)) output_port = int(rospy.get_param('/ais/output', 0)) output_address = rospy.get_param('/ais/output_address', '<broadcast>') ais_decoder = ais.decoder.AISDecoder() if logdir is not None: logfile = file( logdir + '.'.join(datetime.datetime.utcnow().isoformat().split(':')) + '_ais.log', 'w') else: logfile = None if input_type == 'serial': serial_in = serial.Serial(input_address, int(input_speed)) else: udp_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) udp_in.bind(('', input_port)) if output_port > 0: udp_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) udp_out.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) else: udp_out = None while not rospy.is_shutdown(): if input_type == 'serial': nmea_ins = (serial_in.readline(), ) #print nmea_in if udp_out is not None: udp_out.sendto(nmea_in, (output_address, output_port)) else: nmea_in, addr = udp_in.recvfrom(2048) #print addr, nmea_in nmea_ins = nmea_in.split('\n') now = rospy.get_rostime() for nmea_in in nmea_ins: if logfile is not None: logfile.write(datetime.datetime.utcnow().isoformat() + ',' + nmea_in + '\n') if nmea_in.startswith('!AIVDM'): ais_decoder.addNMEA(nmea_in.strip()) msgs = ais_decoder.popMessages() for m in msgs: if m['type'] in (1, 2, 3, 18, 19): #position reports c = Contact() c.header.stamp = now c.mmsi = m['mmsi'] if 'shipname' in ais_decoder.mmsi_db[m['mmsi']]: c.name = ais_decoder.mmsi_db[m['mmsi']]['shipname'] if 'callsign' in ais_decoder.mmsi_db[m['mmsi']]: c.callsign = ais_decoder.mmsi_db[ m['mmsi']]['callsign'] c.position.latitude = m['lat'] c.position.longitude = m['lon'] if m['course'] is not None: c.cog = math.radians(m['course']) else: c.cog = -1 if m['speed'] is not None: c.sog = m['speed'] * 0.514444 #knots to m/s else: c.sog = -1 if m['heading'] is not None: c.heading = math.radians(m['heading']) else: c.heading = -1 if 'to_bow' in ais_decoder.mmsi_db[m['mmsi']]: c.dimension_to_bow = ais_decoder.mmsi_db[ m['mmsi']]['to_bow'] if 'to_stern' in ais_decoder.mmsi_db[m['mmsi']]: c.dimension_to_stern = ais_decoder.mmsi_db[ m['mmsi']]['to_stern'] if 'to_port' in ais_decoder.mmsi_db[m['mmsi']]: c.dimension_to_port = ais_decoder.mmsi_db[ m['mmsi']]['to_port'] if 'to_starboard' in ais_decoder.mmsi_db[m['mmsi']]: c.dimension_to_stbd = ais_decoder.mmsi_db[ m['mmsi']]['to_starboard'] ais_pub.publish(c) raw = Heartbeat() for k, v in m.iteritems(): raw.values.append(KeyValue(k, str(v))) ais_raw_pub.publish(raw) else: nmea_parts = nmea_in.strip().split(',') if len(nmea_parts): #print nmea_parts try: if nmea_parts[0][3:] == 'ZDA' and len(nmea_parts) >= 5: tref = TimeReference() tref.header.stamp = now hour = int(nmea_parts[1][0:2]) minute = int(nmea_parts[1][2:4]) second = int(nmea_parts[1][4:6]) ms = int(float(nmea_parts[1][6:]) * 1000000) day = int(nmea_parts[2]) month = int(nmea_parts[3]) year = int(nmea_parts[4]) zda = datetime.datetime(year, month, day, hour, minute, second, ms) tref.time_ref = rospy.Time( calendar.timegm(zda.timetuple()), zda.microsecond * 1000) tref.source = 'ais' timeref_pub.publish(tref) if nmea_parts[0][3:] == 'GGA' and len( nmea_parts) >= 10: latitude = int(nmea_parts[2][0:2]) + float( nmea_parts[2][2:]) / 60.0 if nmea_parts[3] == 'S': latitude = -latitude longitude = int(nmea_parts[4][0:3]) + float( nmea_parts[4][3:]) / 60.0 if nmea_parts[5] == 'W': longitude = -longitude altitude = float(nmea_parts[9]) nsf = NavSatFix() nsf.header.stamp = now nsf.header.frame_id = 'mobile_lab' nsf.latitude = latitude nsf.longitude = longitude nsf.altitude = altitude position_pub.publish(nsf) if nmea_parts[0][3:] == 'HDT' and len(nmea_parts) >= 2: heading = float(nmea_parts[1]) nes = NavEulerStamped() nes.header.stamp = now nes.header.frame_id = 'mobile_lab' nes.orientation.heading = heading orientation_pub.publish(nes) except ValueError: pass
def handler(self, channel, data): """The LCM callback. Receives the LCM messages, publishes ROS messages. Args: channel (str): The channel it is received on. data (type): Description of parameter `data`. Returns: type: Description of returned object. """ # Header Message headermsg = Header() headermsg.seq = self.seq + 1 headermsg.stamp = rospy.Time.now() headermsg.frame_id = self.gps_frame_ # GPS msg = auv_acfr_nav_t.decode(data) if self.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 self.fix_pub_.publish(fixmsg) if self.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 self.imu_pub_.publish(imumsg) if self.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 self.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")
#!/usr/bin/env python from sensor_msgs.msg import NavSatFix import rospy import random import time print "Starting location faker" rospy.init_node('location_faker', anonymous=True) publisher = rospy.Publisher("drone/position", NavSatFix, queue_size=10) while True: fix = NavSatFix() fix.latitude = 55.1 + random.randint(1, 10000) / 30000. fix.longitude = 10.4 + random.randint(1, 10000) / 30000. time.sleep(1) publisher.publish(fix)
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 #TODO if msg.flags & 0x07: self.last_rtk_time = time.time() self.rtk_fix_mode = msg.flags & 0x07 out.status.status = NavSatStatus.STATUS_GBAS_FIX #TODO # TODO this should probably also include covariance of base fix? out.position_covariance[ 0] = msg.h_accuracy**2 #self.rtk_h_accuracy ** 2 out.position_covariance[ 4] = msg.h_accuracy**2 #self.rtk_h_accuracy ** 2 out.position_covariance[ 8] = msg.v_accuracy**2 #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 main(): # Load File folder_path = '/home/dank-engine/3d_ws/json2_merger/' path = list(pathlib.Path(folder_path).glob('*.json')) id = 0 k = 0 num = 20 arr_rx = np.zeros(num) arr_ry = np.zeros(num) arr_lx = np.zeros(num) arr_ly = np.zeros(num) arr_cx = np.zeros(num) arr_cy = np.zeros(num) for i in range(100, 552): filename = folder_path + '{i:0{width}}'.format(i=i + 1, width=4) + '.json' with open(filename, 'r') as f: print(filename) data = json.load(f) # GPS Topic gps_coor = GeoPointStamped() gps_map = NavSatFix() gps_pub = rospy.Publisher('/gps', GeoPointStamped, queue_size=10) gps_map_pub = rospy.Publisher('/gps/fix', NavSatFix, queue_size=10) #IMU Topic imu_coor = Imu() imu_pub = rospy.Publisher('/imu', Imu, queue_size=10) #PointCloud Topic cloud = PointCloud2() pc_data = rospy.Publisher('/velodyne_points', PointCloud2, queue_size=32) rospy.init_node('converter') # Publish GPS Data in ROS gps_coor.header.frame_id = "gps" gps_coor.position.latitude = data["INS_SWIFT"]["Latitude"] gps_coor.position.longitude = data["INS_SWIFT"]["Longitude"] gps_coor.position.altitude = float('nan') gps_map.header.frame_id = "base_link" gps_map.latitude = data["INS_SWIFT"]["Latitude"] gps_map.longitude = data["INS_SWIFT"]["Longitude"] gps_map.altitude = data["INS_SWIFT"]["Altitude"] gps_pub.publish(gps_coor) gps_map_pub.publish(gps_map) # Publish IMU Data in ROS roll = data["INS_SWIFT"]["Roll"] pitch = data["INS_SWIFT"]["Pitch"] yaw = data["INS_SWIFT"]["Yaw"] quaternion = tf.transformations.quaternion_from_euler( roll, pitch, yaw) imu_coor.header.frame_id = "imu" imu_coor.header.stamp = rospy.get_rostime() imu_coor.orientation.x = quaternion[0] imu_coor.orientation.y = quaternion[1] imu_coor.orientation.z = quaternion[2] imu_coor.orientation.w = quaternion[3] imu_coor.orientation_covariance = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] imu_pub.publish(imu_coor) # Publish Point Cloud Data point = [] scan = data["PointCloud"] for i in range(len(scan)): if (scan[i]["x"] != None): xyzi = (scan[i]["x"], scan[i]["y"], scan[i]["z"], scan[i]["intensity"]) point.append(xyzi) else: xyzi = (np.nan, np.nan, np.nan, 0) point.append(xyzi) cloud = xyzi_array_to_pointcloud2(point, rospy.get_rostime(), "velodyne") pc_data.publish(cloud) lane_mark = rospy.Publisher('/marker_lane', Marker, queue_size=100) marker_l = Marker() marker_l.header.frame_id = "base_link" marker_l.type = marker_l.TEXT_VIEW_FACING marker_l.action = marker_l.ADD marker_l.scale.x = 5 marker_l.scale.y = 5 marker_l.scale.z = 5 marker_l.color.a = 1.0 marker_l.color.r = 1.0 marker_l.color.g = 1.0 marker_l.color.b = 0.0 marker_l.pose.position.x = 0 marker_l.pose.position.y = 30 marker_l.pose.position.z = 1 marker_l.pose.orientation.w = 1.0 marker_l.text = "Lanes: " + str( data["Lanes"]) + '\n' + "Width: " + str( data["Width"]) + '\n' + "Slope_Hoizontal: " + str( data["Slope"]["Slope_Hoizontal"]) marker_l.id = 0 lane_mark.publish(marker_l) try: r = rospy.Rate(1) # 10hz lane_r = data["Right_Marker"] lane_l = data["Left_Marker"] lane_c = data["Center_Marker"] publisher = rospy.Publisher('/drive_region', MarkerArray, queue_size=100) arr_rx[k % num] = lane_r["x"] arr_ry[k % num] = lane_r["y"] arr_lx[k % num] = lane_l["x"] arr_ly[k % num] = lane_l["y"] arr_cx[k % num] = lane_c["x"] arr_cy[k % num] = lane_c["y"] # if k == 10: markerArray = MarkerArray() marker = Marker() marker.header.frame_id = "base_link" marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 2 marker.scale.y = 2 marker.scale.z = 2 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = np.average(arr_rx) marker.pose.position.y = np.average(arr_ry) marker.pose.position.z = 0 marker1 = Marker() marker1.header.frame_id = "base_link" marker1.type = marker.SPHERE marker1.action = marker.ADD marker1.scale.x = 2 marker1.scale.y = 2 marker1.scale.z = 2 marker1.color.a = 1.0 marker1.color.r = 1.0 marker1.color.g = 1.0 marker1.color.b = 0.0 marker1.pose.orientation.w = 1.0 marker1.pose.position.x = np.average(arr_lx) marker1.pose.position.y = np.average(arr_ly) marker1.pose.position.z = 0 marker2 = Marker() marker2.header.frame_id = "base_link" marker2.type = marker.SPHERE marker2.action = marker.ADD marker2.scale.x = 2 marker2.scale.y = 2 marker2.scale.z = 2 marker2.color.a = 1.0 marker2.color.r = 0.0 marker2.color.g = 0.0 marker2.color.b = 1.0 marker2.pose.orientation.w = 1.0 marker2.pose.position.x = np.average(arr_cx) marker2.pose.position.y = np.average(arr_cy) marker2.pose.position.z = 0 markerArray.markers.append(marker) markerArray.markers.append(marker1) markerArray.markers.append(marker2) # Renumber the marker IDs for m in markerArray.markers: m.id = id id += 1 # Publish the MarkerArray publisher.publish(markerArray) # k = 0 k = k + 1 r.sleep() except KeyError: r = rospy.Rate(1) # 10hz r.sleep()
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: #rospy.loginfo('GGA') data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude #hdop = data['hdop'] #current_fix.position_covariance[0] = hdop ** 2 #current_fix.position_covariance[4] = hdop ** 2 #current_fix.position_covariance[8] = (2 * hdop) ** 2 # FIXME #current_fix.position_covariance_type = \ # NavSatFix.COVARIANCE_TYPE_APPROXIMATED # covariances diagonals as rospy arguments current_fix.position_covariance[0] = self.gps_covariance_pos[0] current_fix.position_covariance[4] = self.gps_covariance_pos[1] current_fix.position_covariance[8] = self.gps_covariance_pos[2] current_fix.position_covariance_type =\ NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec(int(int(timestamp.to_sec())/(60*60))*(60*60)+data['utc_time']) self.time_ref_pub.publish(current_time_ref) if self.use_GPS_time: self.gps_time = current_time_ref.time_ref current_fix.header.stamp = current_time_ref.time_ref current_fix.header.stamp = current_fix.header.stamp+rospy.Duration(self.time_delay) #print(timestamp.to_sec()) 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'] #rospy.loginfo('RMC') # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] track_made_good_degrees_true = data['track_made_good_degrees_true'] track_made_good_degrees_magnetic = data['track_made_good_degrees_magnetic'] speed = data['speed'] SPEED_OVER_GROUND = data['speed_over_ground'] if not math.isnan(track_made_good_degrees_true): DIRECTION_REFERENCE = "True" COURSE_OVER_GROUND = track_made_good_degrees_true elif not math.isnan(track_made_good_degrees_magnetic): DIRECTION_REFERENCE = "Magnetic" COURSE_OVER_GROUND = track_made_good_degrees_magnetic else: DIRECTION_REFERENCE = "Null" COURSE_OVER_GROUND = float(0) SPEED_OVER_GROUND = float('NaN') current_vtg = Course_Speed() current_vtg.header.stamp = current_time current_vtg.header.frame_id = frame_id current_vtg.DIRECTION_REFERENCE = DIRECTION_REFERENCE current_vtg.COURSE_OVER_GROUND = COURSE_OVER_GROUND current_vtg.SPEED_OVER_GROUND = SPEED_OVER_GROUND self.vtg_pub.publish(current_vtg) #rospy.loginfo(track_made_good_degrees_magnetic) elif 'HDT' in parsed_sentence: #rospy.loginfo('HDT') data = parsed_sentence['HDT'] heading_degrees = data['heading_degrees'] current_hdt = GPHDT() current_hdt.header.stamp = current_time current_hdt.header.frame_id = frame_id current_hdt.HEADING_DEGREES = heading_degrees if self.use_GPS_time and not self.gps_time==None: current_hdt.header.stamp = self.gps_time-rospy.Duration(0,1000000) elif self.use_GPS_time: current_hdt.header.stamp.secs = 0 current_hdt.header.stamp = current_hdt.header.stamp+rospy.Duration(self.time_delay)+rospy.Duration(self.time_delay_heading) #print(current_hdt.header.stamp.to_sec()) self.hdt_pub.publish(current_hdt) else: rospy.loginfo('Not valid') return False