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