def talker():
print 'in talker'
pub = rospy.Publisher('GPS', NavSatFix)
rospy.init_node('GPStalker')
while not rospy.is_shutdown():
#Assuming that parse will return these values
time.sleep(1)
parse()
msg = NavSatFix()
Fix = NavSatStatus()
Fix.status = GPS.mode
Fix.service = GPS.numSat
msg.header.stamp = rospy.Time.now()
msg.status = Fix
msg.latitude = GPS.lat
msg.longitude = GPS.lon
msg.altitude = GPS.alt
msg.position_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
msg.position_covariance_type = 0
#covariance_type = 0 unknown
# = 1 approximated
# = 2 diagonal known
# = 3 known
pub.publish(msg)
def publishGps(my_data):
global pub_location
latlng = NavSatFix()
data = my_data.split(',')
if data[0] == 'L':
latlng.header.stamp = rospy.Time.now()
latlng.header.frame_id = "world"
latlng.latitude = float(data[1])
latlng.longitude = float(data[2])
latlng.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
print "Location: " + data[0] + ' ' + data[1] + ", " + data[2]
elif data[0] == 'R':
latlng.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
print "Request Release"
pub_location.publish(latlng)
def make_fix(t, data, args):
fix = NavSatFix()
fix.header.stamp = t
fix.header.frame_id = args.frame_id
if data.Q in (1, 2, 4):
# Fix, Float, GPS
fix.status.status = NavSatStatus.STATUS_GBAS_FIX
elif data.Q == 3:
# SBAS
fix.status.status = NavSatStatus.STATUS_SBAS_FIX
elif data.Q == 5:
# Single
fix.status.status = NavSatStatus.STATUS_FIX
else:
fix.status.status = NavSatStatus.STATUS_NO_FIX
fix.latitude = data.lat
fix.longitude = data.lon
fix.altitude = data.height
fix.position_covariance = [
covariance(data.sde),
covariance(data.sdne),
covariance(data.sdeu),
covariance(data.sdne),
covariance(data.sdn),
covariance(data.sdun),
covariance(data.sdeu),
covariance(data.sdun),
covariance(data.sdu),
]
fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN
return fix
def get_solution(self):
try:
receive = self.client.recv(4096)
except socket.error:
raise
if not receive:
raise ValueError("Not received")
# receive example
# 1918 352534.000 35.674540574 139.531064244 94.6605 5 9 3.3592 2.1315 7.6682 -0.8273 1.5609 -2.0968 0.00 0.0
#$6 1:fix, 2:float 3:sbab, 4:dgps, 5:single, 6:ppp
receive_split = receive.split()
if len(receive_split) is not 15:
raise ValueError("Receive data length is not 15")
print receive_split
t = self.gpst2time(receive_split[0], receive_split[1])
latitude = float(receive_split[2])
longtitude = float(receive_split[3])
ret = NavSatFix()
ret.header.stamp = rospy.Time(float(t))
ret.latitude = latitude
ret.longitude = longtitude
ret.position_covariance_type = self.covariance_table[
int(receive_split[5]) - 1]
return ret
def talker():
pub = rospy.Publisher('gps/fix', NavSatFix, queue_size=10)
rospy.init_node('fake_gps', anonymous=True)
pub2 = rospy.Publisher('vel', Twist, queue_size=10)
rate = rospy.Rate(2) # 10hz
loc_index = 0
while not rospy.is_shutdown():
#print(loc_index)
fake_localization = NavSatFix()
fake_vel = Twist()
#fake_vel.frame_id = "/gps"
#fake_localization.header.stamp = time.time()
fake_localization.header.frame_id = '/gps'
fake_localization.status.status = 1
fake_localization.status.service = 1
#print(gps_localizations[loc_index][0], gps_localizations[loc_index][1])
fake_localization.latitude = gps_localizations[loc_index][0]
fake_localization.longitude = gps_localizations[loc_index][1]
print(fake_localization.latitude, fake_localization.longitude)
fake_localization.altitude = 0.0
fake_localization.position_covariance = [
1.3224999999999998, 0.0, 0.0, 0.0, 1.3224999999999998, 0.0, 0.0,
0.0, 5.289999999999999
]
fake_localization.position_covariance_type = 1
#fake_vel.header.frame_id = "/gps"
pub.publish(fake_localization)
pub2.publish(fake_vel)
if (loc_index == len(gps_localizations) - 1):
loc_index = 0
else:
loc_index = loc_index + 1
rate.sleep()
def callback(self): #Publish data at 1 Hz rate = rospy.Rate(1) #Define the RTKRCV server ports corresponding to each of the processing #Modes port = 5801 sock = socket.socket() host = socket.gethostname() sock.connect((host, port)) #WGS84 Parameters e2 = 6.69437999014e-3 a = 6378137.0 #At a rate of 1 hz, create an rtklib message for each socket and publish #the data over while not rospy.is_shutdown(): #Create and RTKLIB and a NavSatFix data structure rtk = rtklib() navsat = NavSatFix() #Set the data structure header time to the current system time navsat.header.stamp = rospy.Time.now() rtk.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = socket.recv(1024) #Split the message msg = msgStr.split() #Save the latitude, longitude and ellipsoid height navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) #Save the position covariance navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN #Compute the radius of curvature in the N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) #Compute and store the ECEF position rtk.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) rtk.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) rtk.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) rtk.delay = float(msg[13]) rtk.ftest = float(msg[14]) #Store the NavSatFix data rtk.state = navsat pubs[i].publish(rtk) rate.sleep()
def init_path():
# logitude, latitute, altitute, orientation (+deg)
gpsPub = rospy.Publisher("/fix_path_init", NavSatFix, queue_size=10)
wpSub = rospy.Subscriber("/gps_path_init", Odometry, utm_cb)
navStatus = NavSatStatus()
navStatus.service = 1
navStatus.status = 1
# waypoints hardcoded for now, may want to spec with argument in future
num_wp = gps_waypoints.shape[0]
hdop = 1.5
position_covariance_type = 1
global wp_num
for i in range(0, num_wp):
# send gps waypoint for conversion
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'base_link'
msgOut = NavSatFix()
msgOut.position_covariance[0] = hdop**2
msgOut.position_covariance[4] = hdop**2
msgOut.position_covariance[8] = (2 * hdop)**2
msgOut.longitude = gps_waypoints[i][0]
msgOut.latitude = gps_waypoints[i][1]
msgOut.altitude = gps_waypoints[i][2]
msgOut.position_covariance_type = position_covariance_type
msgOut.status = navStatus
msgOut.header = header
wp_num = i
gpsPub.publish(msgOut)
def step(self):
stamp = super().step()
if not stamp:
return
if self.__gps_publisher.get_subscription_count() > 0 or \
self.__speed_publisher.get_subscription_count() > 0 or \
self._always_publish:
self._wb_device.enable(self._timestep)
msg = Float32()
msg.data = self._wb_device.getSpeed()
self.__speed_publisher.publish(msg)
if self.__coordinate_system == GPS.WGS84:
msg = NavSatFix()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.latitude = self._wb_device.getValues()[0]
msg.longitude = self._wb_device.getValues()[1]
msg.altitude = self._wb_device.getValues()[2]
msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
msg.status.service = NavSatStatus.SERVICE_GPS
self.__gps_publisher.publish(msg)
else:
msg = PointStamped()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.point.x = self._wb_device.getValues()[0]
msg.point.y = self._wb_device.getValues()[1]
msg.point.z = self._wb_device.getValues()[2]
self.__gps_publisher.publish(msg)
else:
self._wb_device.disable()
def bestpos_handler(self, bestpos):
navsat = NavSatFix()
# TODO: The timestamp here should come from SPAN, not the ROS system time.
navsat.header.stamp = rospy.Time.now()
navsat.header.frame_id = self.odom_frame
# Assume GPS - this isn't exposed
navsat.status.service = NavSatStatus.SERVICE_GPS
position_type_to_status = {
BESTPOS.POSITION_TYPE_NONE: NavSatStatus.STATUS_NO_FIX,
BESTPOS.POSITION_TYPE_FIXED: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_FIXEDHEIGHT: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_FLOATCONV: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_WIDELANE: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_NARROWLANE: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_DOPPLER_VELOCITY: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_SINGLE: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_WAAS: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_PROPAGATED: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_OMNISTAR: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_L1_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_IONOFREE_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_NARROW_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_L1_INT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_WIDE_INT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_NARROW_INT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_RTK_DIRECT_INS: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_SBAS: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PSRSP: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_RTKFLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_RTKFIXED: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_OMNISTAR: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_OMNISTAR_HP: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_OMNISTAR_XP: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_OMNISTAR_HP: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_OMNISTAR_XP: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_PPP: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PPP: NavSatStatus.STATUS_SBAS_FIX,
}
navsat.status.status = position_type_to_status.get(bestpos.position_type, NavSatStatus.STATUS_NO_FIX)
# Position in degrees.
navsat.latitude = bestpos.latitude
navsat.longitude = bestpos.longitude
# Altitude in metres.
navsat.altitude = bestpos.altitude
navsat.position_covariance[0] = pow(2, bestpos.latitude_std)
navsat.position_covariance[4] = pow(2, bestpos.longitude_std)
navsat.position_covariance[8] = pow(2, bestpos.altitude_std)
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
# Ship ito
self.pub_navsatfix.publish(navsat)
def callback(sensorstr):
data = sensorstr.data
if data[0] == 'Z':
data = data.replace("Z","").replace("\n","").replace("\r","")
data_list = data.split(',')
if len(data_list) == 4:
try:
##data_list structure: lat, lon, heading, vel
float_list = [float(i) for i in data_list]
poseGPS = NavSatFix()
poseGPS.header.frame_id = "world"
poseGPS.header.stamp = rospy.Time.now()
poseGPS.status.status = 0
poseGPS.status.service = 1
poseGPS.latitude = float_list[0]
poseGPS.longitude = float_list[1]
poseGPS.altitude = 0.0
poseGPS.position_covariance = [3.24, 0, 0, 0, 3,24, 0, 0, 0, 0]
poseGPS.position_covariance_type = 2
try:
pubGPS.publish(poseGPS)
log = "GPS Pose Data: %f %f Publish at Time: %s" % (float_list[0], float_list[1], rospy.get_time())
except:
log = "poseGPS Publisher Error"
except:
log = "GPS Data Error! Data : %s" % data
rospy.loginfo(log)
rospy.loginfo(log)
def spin(self):
#############################################################################
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
current_time = rospy.get_rostime()
frame_id = 'gps'
latitude = 40.444539
longitude = -79.940777
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
current_time_ref.source = frame_id
current_fix.status.status = NavSatStatus.STATUS_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
current_fix.latitude = latitude
current_fix.longitude = longitude
current_fix.position_covariance[0] = 0.1
current_fix.position_covariance[4] = 0.1
current_fix.position_covariance[8] = 0.25
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
current_fix.altitude = 0.0
self.fix_pub.publish(current_fix)
self.update()
r.sleep()
def main():
rospy.init_node('SensorLocalisationReciever', anonymous=True)
print("Running SensorLocalisationReciever")
# ser = serial.Serial(rospy.get_param('~sensorserialport','/dev/ttyACM0'))
imu_data = Imu()
gps_data = NavSatFix()
imu_data.header.frame_id = 'base_link'
gps_data.header.frame_id = 'map'
rate = rospy.Rate(rospy.get_param("~rate", 100))
imu_pub = rospy.Publisher('/imu/data', Imu, queue_size=10)
gps_pub = rospy.Publisher('/gps/fix', NavSatFix, queue_size=10)
while not rospy.is_shutdown():
# print(x,y)
# data = ser.readline()
# if data[0]=='#':
# data = data[1:]
# linear,angular = data.split('/')[0],data.split('/')[1]
linear = [float(x) for x in linear.split(' ')]
angular = [float(x) for x in angular.split(' ')]
imu_data.orientation.x = 1 ##angular[3]
imu_data.orientation.y = 1 ## angular[4]
imu_data.orientation.z = 1 ##angular[5]
imu_data.linear_acceleration.x = 1 # linear[6]
imu_data.linear_acceleration.y = 1 # linear[7]
imu_data.linear_acceleration.z = 1 #linear[8]
imu_data.angular_velocity_covariance = [
1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0
]
imu_data.linear_acceleration_covariance = [
1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0
]
gps_data.latitude = 1 # linear[0]
gps_data.longitude = 1 #linear[1]
gps_data.position_covariance = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0]
gps_data.position_covariance_type = 3
gps_data.header.stamp = imu_data.header.stamp = rospy.Time.now()
imu_pub.publish(imu_data)
gps_pub.publish(gps_data)
print(linear)
# print(angular)
rate.sleep()
def timer_gps_clk_(self):
self.update_status_()
msg = NavSatFix()
msg.latitude = self.status['location']['latitude']
msg.longitude = self.status['location']['longitude']
msg.position_covariance_type = 0
msg.header.frame_id = 'gps'
self.gps_pub_.publish(msg)
def handle(self):
# self.request is the TCP socket connected to the client
global pub_location
latlng = NavSatFix()
self.data = self.request.recv(1024).strip()
data = self.data.split(',')
if data[0] == 'L':
latlng.header.stamp = rospy.Time.now()
latlng.header.frame_id = "world"
latlng.latitude = float(data[1])
latlng.longitude = float(data[2])
latlng.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
print "Location: " + data[0] + ' ' + data[1] + ", " + data[2]
elif data[0] == 'R':
latlng.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
print "Request Release"
pub_location.publish(latlng)
def parse_gps(self, data):
'''
Given raw data from Jaguar GPS socket, parse and return NavSatFix message.
If bad/incomplete data, return None
'''
# use regex to parse
gpgga_hit = re.search('^\$(GPGGA.+?)\r\n', data)
gprmc_hit = re.search('^\$(GPRMC.+?)\r\n', data)
if gprmc_hit:
# Get estimated heading (not part of standard ROS navsatfix message)
gprmc = gprmc_hit.group(0).split(",")
try:
heading = float(gprmc[8])
except:
heading = float("NaN")
else:
while heading < -180.0:
heading += 360.0
while heading > 180.0:
heading -= 360.0
finally:
self.current_est_heading = heading
if gpgga_hit:
gpgga = gpgga_hit.group(0).split(",")
nav_msg = NavSatFix()
# Set header information
time = gpgga[1]
hrs = float(time[0:1])
mins = float(time[2:3])
secs = float(time[4:5])
nav_msg.header.stamp = rospy.Time.from_sec(hrs * 3600 + mins * 60 + secs)
nav_msg.header.frame_id = "gps"
# Set GPS Status
status = NavSatStatus()
status.status = -1 if int(gpgga[6]) == 0 else 0
nav_msg.status = status
# Set longitude and latitude
lat_str = gpgga[2]
lon_str = gpgga[4]
lat_degs = float(lat_str[:2]) + (float(lat_str[2:]) / 60.0)
lon_degs = float(lon_str[:3]) + (float(lon_str[3:]) / 60.0)
nav_msg.latitude = -1 * lat_degs if gpgga[3] == "S" else lat_degs
nav_msg.longitude = -1 * lon_degs if gpgga[5] == "W" else lon_degs
# Set altitude (Positive is above the WGS 84 ellipsoid)
try:
nav_msg.altitude = float(gpgga[9])
except:
nav_msg.altitude = float("NaN")
# Set covariance type to unknown
nav_msg.position_covariance_type = 0
return nav_msg
else:
return None
def callback_gps(data):
gps_data = NavSatFix()
gps_data.header.frame_id = "map"
gps_data.latitude = data.x
gps_data.longitude = data.y
gps_data.position_covariance = [1e-3, 0, 0, 0, 1e-3, 0, 0, 0, 1e-3]
gps_data.position_covariance_type = 3
gps_data.header.stamp = rospy.Time.now()
pub_gps.publish(gps_data)
def callback_sbp_pos(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_POS_LLH (Sender: %d): %s" % (msg.sender, repr(msg)))
out = NavSatFix()
out.header.frame_id = self.frame_id
out.header.stamp = rospy.Time.now()
out.status.service = NavSatStatus.SERVICE_GPS
out.latitude = msg.lat
out.longitude = msg.lon
out.altitude = msg.height
out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
if msg.flags & 0x03:
self.last_rtk_time = time.time()
self.rtk_fix_mode = msg.flags & 0x03
out.status.status = NavSatStatus.STATUS_GBAS_FIX
# TODO this should probably also include covariance of base fix?
out.position_covariance[0] = self.rtk_h_accuracy**2
out.position_covariance[4] = self.rtk_h_accuracy**2
out.position_covariance[8] = self.rtk_v_accuracy**2
pub = self.pub_rtk_fix
self.last_rtk_pos = msg
# If we are getting this message, RTK is our best fix, so publish this as our best fix.
self.pub_fix.publish(out)
else:
self.last_spp_time = time.time()
out.status.status = NavSatStatus.STATUS_FIX
# TODO hack, piksi should provide these numbers
if self.last_dops:
out.position_covariance[0] = (self.last_dops.hdop * 1E-2)**2
out.position_covariance[4] = (self.last_dops.hdop * 1E-2)**2
out.position_covariance[8] = (self.last_dops.vdop * 1E-2)**2
else:
out.position_covariance[0] = COV_NOT_MEASURED
out.position_covariance[4] = COV_NOT_MEASURED
out.position_covariance[8] = COV_NOT_MEASURED
pub = self.pub_spp_fix
self.last_pos = msg
# Check if SPP is currently our best available fix
if self.rtk_fix_mode <= 0:
self.pub_fix.publish(out)
pub.publish(out)
def publish_llh_msg(self, msg, **metadata):
llh_msg = NavSatFix()
llh_msg.latitude = msg.lat
llh_msg.longitude = msg.lon
llh_msg.altitude = msg.height
llh_msg.position_covariance_type = 2
llh_msg.position_covariance = [9, 0, 0, 0, 9, 0, 0, 0, 9]
# Publish ROS messages
self.pub_llh.publish(llh_msg)
self.pub_llh_n_sats.publish(Int32(msg.n_sats))
self.pub_llh_fix_mode.publish(Int32(msg.flags))
def ExtFix2Fix(data):
fix = NavSatFix()
fix.header = data.header
fix.status.status = data.status.status
fix.status.service = data.status.position_source
fix.latitude = data.latitude
fix.longitude = data.longitude
fix.altitude = data.altitude
fix.position_covariance = data.position_covariance
fix.position_covariance_type = data.position_covariance_type
return fix
def ExtFix2Fix(data):
fix = NavSatFix()
fix.header = data.header
fix.status.status = data.status.status
fix.status.service = data.status.position_source
fix.latitude = data.latitude
fix.longitude = data.longitude
fix.altitude = data.altitude
fix.position_covariance = data.position_covariance
fix.position_covariance_type = data.position_covariance_type
return fix
def callback(self): rate = rospy.Rate(10) ports = [5801,5802,5803] sockets = [] pubs = [self.static_pub, self.rtkStatic_pub, self.rtkDynamic_pub] for i in ports: sock = socket.socket() host = socket.gethostname() sock.connect((host, i)) sockets.append(sock) e2 = 6.69437999014e-3 a = 6378137.0 while not rospy.is_shutdown(): for i in range(len(sockets)): navsat = NavSatFix() ecef_xyz = Point() ecef_pose = Pose() #ecef_stampedPose = PoseStamped() navsat.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = sockets[i].recv(1024) #Split the message msg = msgStr.split() navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) ecef_pose.position = ecef_xyz #ecef_stampedPose.pose = ecef_pose pubs[i].publish(navsat) rate.sleep()
def callback(self): rate = rospy.Rate(10) ports = [5801,5802,5803] sockets = [] pubs = [self.static_pub, self.rtkStatic_pub, self.rtkDynamic_pub] for i in ports: sock = socket.socket() host = socket.gethostname() sock.connect((host, i)) sockets.append(sock) e2 = 6.69437999014e-3 a = 6378137.0 while not rospy.is_shutdown(): for i in range(len(sockets)): navsat = NavSatFix() ecef_xyz = Point() ecef_pose = Pose() #ecef_stampedPose = PoseStamped() navsat.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = sockets[i].recv(1024) #Split the message msg = msgStr.split() navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) ecef_pose.position = ecef_xyz #ecef_stampedPose.pose = ecef_pose pubs[i].publish(navsat) rate.sleep()
def talker():
pub = rospy.Publisher('/RTK', Point, queue_size=10)
rospy.init_node('RUN', anonymous=True)
rate = rospy.Rate(100) # 10hz
count = 0
#print("ok")
global sequence_number
global z
while not rospy.is_shutdown():
while True:
line = str(str(s.readline())[0:])
# print("angle is ok")
if line.startswith('$GNGGA'):
#print(line)
a = line.split(',')
RTK = (a[6])
#print(RTK)
record_item = []
sequence_number += 1
msg = pynmea2.parse(line)
lat = msg.latitude
lgi = msg.longitude
navsat = NavSatFix()
navsat.status = 0
navsat.header.seq = 1
navsat.header.stamp = rospy.Time.now()
navsat.position_covariance_type = 2
# navsat.STATUS = 1
navsat.header.frame_id = 'base_link'
navsat.latitude = lat
navsat.longitude = lgi
navsat.altitude = txt()
print(navsat)
utm = Proj(proj='utm', zone=48, ellps='WGS84')
x, y = utm(lgi, lat)
utm_point = Point()
#msg = geom_msg.Pose()
utm_point.x = x
utm_point.y = y
#utm_point.angular.z = txt()
# br = tf.TransformBroadcaster()
# br.sendTransform((x, y, 0),
# tf.transformations.quaternion_from_euler(0, 0, txt()),rospy.Time.now(),"base_link","odom")
#rospy.loginfo(utm_point)
pub.publish(utm_point)
def CreateBag(matfile, bagname, frame_id, navsat_topic="/fix"):
'''Creates the actual bag file by successively adding images'''
bag = rosbag.Bag(bagname, 'w', compression=rosbag.Compression.BZ2, chunk_threshold=32 * 1024 * 1024)
data = loadmat(matfile)['rnv']
rnv = {}
for i, col in enumerate(data.dtype.names):
rnv[col] = data.item()[i]
try:
ref = None
x, y = [], []
for i, t in enumerate(tqdm(rnv['t'])):
# print("Adding %s" % image_name)
stamp = rospy.Time.from_sec(t)
nav_msg = NavSatFix()
nav_msg.header.stamp = stamp
nav_msg.header.frame_id = "map"
nav_msg.header.seq = i
nav_msg.latitude = rnv['lat'][i][0]
nav_msg.longitude = rnv['lon'][i][0]
nav_msg.altitude = -rnv['depth'][i][0]
nav_msg.position_covariance_type = nav_msg.COVARIANCE_TYPE_UNKNOWN
transform_msg = TransformStamped()
transform_msg.header = copy(nav_msg.header)
utm = fromLatLong(nav_msg.latitude, nav_msg.longitude, nav_msg.altitude)
if ref is None:
ref = utm.toPoint()
x.append(utm.toPoint().x - ref.x)
y.append(utm.toPoint().y - ref.y)
dx = x[-1] - sum(x[-min(20, len(x)):]) / min(20, len(x))
dy = y[-1] - sum(y[-min(20, len(y)):]) / min(20, len(y))
transform_msg.transform.translation.x = x[-1]
transform_msg.transform.translation.y = y[-1]
transform_msg.transform.translation.z = nav_msg.altitude
transform_msg.transform.rotation = Quaternion(*tf_conversions.transformations.quaternion_from_euler(0, 0,
math.atan2(dy, dx)))
transform_msg.child_frame_id = frame_id
tf_msg = tfMessage(transforms=[transform_msg])
odometry_msg = Odometry()
odometry_msg.header = copy(transform_msg.header)
odometry_msg.child_frame_id = frame_id
odometry_msg.pose.pose.position = transform_msg.transform.translation
odometry_msg.pose.pose.orientation = transform_msg.transform.rotation
bag.write(navsat_topic, nav_msg, stamp)
bag.write("/tf", tf_msg, stamp)
bag.write("/transform", transform_msg, stamp)
bag.write("/odom", odometry_msg, stamp)
finally:
bag.close()
def msg_dict_to_NavSatFix(msg_dict):
msg = NavSatFix()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = msg_dict['header']['frame_id']
msg.latitude = msg_dict['latitude']
msg.longitude = msg_dict['longitude']
msg.altitude = msg_dict['altitude']
msg.status.status = msg_dict['status']['status']
msg.status.service = msg_dict['status']['service']
msg.position_covariance = msg_dict['position_covariance']
msg.position_covariance_type = msg_dict['position_covariance_type']
return msg
def responseCallback(self, msg):
#############################################################################
gps_msg = NavSatFix()
gps_msg.header = msg.header
gps_msg.status = msg.status
gps_msg.latitude = msg.latitude
gps_msg.longitude = msg.longitude
gps_msg.altitude = msg.altitude
gps_msg.position_covariance_type = msg.position_covariance_type
gps_msg.position_covariance[0] = 1.8
gps_msg.position_covariance[4] = 1.8
gps_msg.position_covariance[8] = 5
self.gpsPub.publish(gps_msg)
def pub_gps(msg_type, msg, bridge):
pub = bridge.get_ros_pub("gps", NavSatFix, queue_size=1)
fix = NavSatFix()
fix.header.stamp = bridge.project_ap_time(msg)
fix.header.frame_id = 'base_footprint'
fix.latitude = msg.lat / 1e07
fix.longitude = msg.lon / 1e07
# NOTE: absolute (MSL) altitude
fix.altitude = msg.alt / 1e03
fix.status.status = NavSatStatus.STATUS_FIX
fix.status.service = NavSatStatus.SERVICE_GPS
fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
pub.publish(fix)
def pub_gps(msg_type, msg, bridge):
pub = bridge.get_ros_pub("gps", NavSatFix, queue_size=1)
fix = NavSatFix()
fix.header.stamp = bridge.project_ap_time(msg)
fix.header.frame_id = 'base_footprint'
fix.latitude = msg.lat/1e07
fix.longitude = msg.lon/1e07
# NOTE: absolute (MSL) altitude
fix.altitude = msg.alt/1e03
fix.status.status = NavSatStatus.STATUS_FIX
fix.status.service = NavSatStatus.SERVICE_GPS
fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
pub.publish(fix)
def talker():
pub = rospy.Publisher('andro2linux_gps', NavSatFix, queue_size=10)
rospy.init_node('andro2linux_gps_publisher', anonymous=True)
# socket type (UDP = SOCK_DGRAM, TCP = SOCK_STREAM)
s = socket(AF_INET, SOCK_DGRAM)
# port setup
s.bind(('', ECHO_PORT))
# ready!
print('udp echo server ready')
# infinite loop
while 1:
# Waiting for client
data, addr = s.recvfrom(BUFSIZE)
# extract the information from fused provided
splitted_data = str(data).split("\n")
provider = splitted_data[0].split()[1]
if provider == 'fused':
gpsmsg = NavSatFix()
gpsmsg.header.stamp = rospy.Time.now()
gpsmsg.header.frame_id = "gps"
#print(splitted_data)
longitude = float(splitted_data[1].split()[1])
latitude = float(splitted_data[2].split()[1])
altitude = float(splitted_data[3].split()[1])
accuracy = float(splitted_data[4].split()[1])
variance = accuracy * accuracy
hasspeed = splitted_data[5].split()[1] == 'true'
gpsmsg.latitude = latitude
gpsmsg.longitude = longitude
gpsmsg.altitude = altitude
gpsmsg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
gpsmsg.position_covariance = (variance, 0, 0, 0, variance, 0, 0, 0,
variance)
print(gpsmsg)
print("------------------------")
# publish
pub.publish(gpsmsg)
if rospy.is_shutdown():
break
# send back to client
s.sendto(data, addr)
def set_destination(self, destination):
msg = NavSatFix()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = "utm"
msg.latitude = float(destination['latitude'])
msg.longitude = float(destination['longitude'])
msg.altitude = 0
msg.status.status = NavSatStatus.STATUS_GBAS_FIX
msg.status.service = 0
msg.position_covariance = [0] * 9
msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
self._goalfixpub.publish(msg)
ret = {'method': "set_destination", 'msg': "set"}
return ret
def gpsCallback(data):
gps = NavSatFix()
gps.header.seq = data.header.seq
gps.header.stamp = data.header.stamp
if (gps.header.stamp.secs % 10 != 4):
gps.header.frame_id = "gps3_Link"
gps.status = data.status
gps.latitude = data.latitude
gps.longitude = data.longitude
gps.altitude = data.altitude
gps.position_covariance = data.position_covariance
gps.position_covariance_type = data.position_covariance_type
pub = rospy.Publisher('/gps_filtered', NavSatFix, queue_size=10)
rate = rospy.Rate(5) # Hz
pub.publish(gps)
rate.sleep()
else:
gps.header.frame_id = "false_Link"
gps.status = data.status
gps.latitude = 0
gps.longitude = 0
gps.altitude = 0
gps.position_covariance = data.position_covariance
gps.position_covariance_type = data.position_covariance_type
def publish_gps(event):
global origin_lat, origin_lon, first
#It may take a second or two to get good data
#print gpsd.fix.latitude,', ',gpsd.fix.longitude,' Time: ',gpsd.utc
# os.system('clear')
# print
# print ' GPS reading'
# print '----------------------------------------'
# print 'latitude ' , gpsd.fix.latitude
# print 'longitude ' , gpsd.fix.longitude
# print 'time utc ' , gpsd.utc,' + ', gpsd.fix.time
# print 'altitude (m)' , gpsd.fix.altitude
# print 'epx ' , gpsd.fix.epx
# print 'epv ' , gpsd.fix.epv
# print 'ept ' , gpsd.fix.ept
# print 'speed (m/s) ' , gpsd.fix.speed
# print 'climb ' , gpsd.fix.climb
# print 'track ' , gpsd.fix.track
# print 'mode ' , gpsd.fix.mode
# print
# print 'sats ' , gpsd.satellites
navsat = NavSatFix()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "base_link"
navsat.header = header
navsat_status = NavSatStatus()
navsat_status.status = 0
navsat_status.service = 1
navsat.status = navsat_status
navsat.latitude = gpsd.fix.latitude
navsat.longitude = gpsd.fix.longitude
navsat.altitude = gpsd.fix.altitude
navsat.position_covariance_type = 2
navsat.position_covariance = [2.5, 0, 0,
0, 2.5, 0,
0, 0, 2.5]
if not origin_lat and not origin_lon:
origin_lat = gpsd.fix.latitude
origin_lon = gpsd.fix.longitude
# print ('Odometry: ')
# print (x, y)
pub_navsat.publish(navsat)
def save_gps_bag(frame_id, seq, seconds, nanoseconds, status, service, latitude, longitude, altitude, position_covariance, position_covariance_type): ros_gps = NavSatFix() ros_gps.header.seq = seq ros_gps.header.stamp.secs = seconds ros_gps.header.stamp.nsecs = nanoseconds ros_gps.header.frame_id = frame_id gps_GGA_topic = "/gps/fix" ros_gps.status.status = status ros_gps.status.service = service ros_gps.latitude = latitude ros_gps.longitude = longitude ros_gps.altitude = altitude ros_gps.position_covariance = position_covariance ros_gps.position_covariance_type = position_covariance_type bag.write(gps_GGA_topic, ros_gps, ros_gps.header.stamp)
def publish(self, data):
msg = NavSatFix()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.latitude = data.getLat()
msg.longitude = data.getLng()
msg.altitude = data.getMeters()
if data.getFix() == 1:
msg.status.status = 0
else:
msg.status.status = -1
msg.position_covariance_type = 1
msg.position_covariance[0] = data.getHDOP() * data.getHDOP()
msg.position_covariance[4] = data.getHDOP() * data.getHDOP()
msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP()
msg.status.service = 1
self._pub.publish(msg)
def subCB(msg_in):
global pub
msg_out = NavSatFix()
msg_out.header = msg_in.header
msg_out.status.status = NavSatStatus.STATUS_FIX # TODO - fix this
msg_out.status.service = NavSatStatus.SERVICE_GPS
msg_out.latitude = msg_in.LLA.x
msg_out.longitude = msg_in.LLA.y
msg_out.altitude = msg_in.LLA.z
msg_out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
msg_out.position_covariance[1] = msg_in.PosUncerainty
pub.publish(msg_out)
def subCB(msg_in):
global pub
msg_out = NavSatFix()
msg_out.header = msg_in.header
msg_out.status.status = NavSatStatus.STATUS_FIX # TODO - fix this
msg_out.status.service = NavSatStatus.SERVICE_GPS
msg_out.latitude = msg_in.LLA.x
msg_out.longitude = msg_in.LLA.y
msg_out.altitude = msg_in.LLA.z
msg_out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
msg_out.position_covariance[1] = msg_in.PosUncerainty
pub.publish(msg_out)
def _gps_pub(self):
if self.info_list[2] == '':
rospy.loginfo("[%s] No data receiving ... Try go outside?" %
(self.node_name))
return
# info format:
# GPGGA UTC_time latitude N/S longitude E/W state number_of_sat HDOP altitude others
fix = NavSatFix()
fix.header.seq = self.seq
# Use received data
#fix.header.stamp = ToTimeFormat(self.info_list[1])
# Use rospy.Time.now
fix.header.stamp = rospy.Time.now()
fix.header.frame_id = 'gps_link'
lat = float(Sexagesimal2Decimal(self.info_list[2]))
longi = float(Sexagesimal2Decimal(self.info_list[4]))
if self.info_list[3] == 'S':
fix.latitude = -lat
else:
fix.latitude = lat
if self.info_list[5] == 'W':
fix.longitude = -longi
else:
fix.longitude = longi
rospy.loginfo(
"[%s] %s %s %s %s" %
(self.node_name, self.info_list[3], lat, self.info_list[5], longi))
# Covariance matrix
# Refer to nmea_navsat_driver
try:
hdop = float(self.info_list[8])
except ValueError:
pass
fix.position_covariance[0] = hdop**2
fix.position_covariance[4] = hdop**2
fix.position_covariance[8] = (2 * hdop)**2
fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
fix.altitude = float(self.info_list[9]) + float(self.info_list[11])
self.pub_fix.publish(fix)
self.info = ""
self.info_list = None
self.seq += 1
def do_fix():
global path, gps_err_x, gps_err_y
rate = rospy.Rate(1)
while not rospy.is_shutdown():
gps_err_x, gps_err_y = random.choice(GPS_ERR_V,
p=GPS_ERR_P[param_gps_err],
size=(2))
gps_err = sqrt(gps_err_x**2 + gps_err_y**2)
with lock:
now = rospy.Time.now()
x, y = pos_x + gps_err_x, pos_y + gps_err_y
m = NavSatFix()
m.header.stamp = now
m.header.frame_id = base_frame
m.status.status = 0
m.status.service = 1
m.latitude, m.longitude = utm.to_latlon(x, y, 30, northern=True)
m.altitude = 0
m.position_covariance = [(2 * gps_err)**2, 0.0, 0.0, 0.0,
(2 * gps_err)**2, 0.0, 0.0, 0.0, 10.0]
m.position_covariance_type = 3
n = PoseStamped()
n.header.stamp = now
n.header.frame_id = map_frame
n.pose.position = Point(x - pos_x0, y - pos_y0, 0)
q = quaternion_from_euler(0, 0, orientation)
n.pose.orientation = Quaternion(*q)
path.poses.append(n)
path.poses = path.poses[-param_path_length:]
path.header.stamp = now
if param_publish_fix:
pub_fix.publish(m)
if param_publish_path:
pub_path.publish(path)
rate.sleep()
def responseCallback(self, msg):
#############################################################################
gps_msg = NavSatFix()
gps_msg.header = msg.header
gps_msg.status = msg.status
gps_msg.latitude = msg.latitude
gps_msg.longitude = msg.longitude
gps_msg.altitude = msg.altitude
gps_msg.position_covariance_type = msg.position_covariance_type
if msg.status.status == 0: # no rtk
gps_msg.position_covariance[0] = 80
gps_msg.position_covariance[4] = 80
gps_msg.position_covariance[8] = 150
else:
gps_msg.position_covariance[0] = 0.1
gps_msg.position_covariance[4] = 0.1
gps_msg.position_covariance[8] = 0.25
self.gpsPub.publish(gps_msg)
def cmdCB2(self, data):
# Serial read & publish
try:
msg = NavSatFix()
msg.header.seq = data.header.seq
msg.header.stamp = data.header.stamp
msg.header.frame_id = data.header.frame_id
msg.status = data.status
msg.latitude = data.latitude
msg.longitude = data.longitude
msg.altitude = data.altitude
for i in range(9):
msg.position_covariance[i] = data.position_covariance[i] * 10
msg.position_covariance_type = data.position_covariance_type
#rospy.logerr(msg)
self.pub2.publish(msg)
except:
pass
def publish(self):
header = Header()
header.stamp = rospy.Time.now()
navStatus = NavSatStatus()
navStatus.status = (self.fixStatus - 1)
navStatus.service = (0b1111)
gpsMsg = NavSatFix()
gpsMsg.header = header
gpsMsg.status = navStatus
gpsMsg.latitude = self.lat
gpsMsg.longitude = self.long
gpsMsg.altitude = self.alt
gpsMsg.position_covariance = self.covariance
gpsMsg.position_covariance_type = self.covariance_type
self.navSatPub.publish(gpsMsg)
self.headingPub.publish(self.heading)
def sub_insCB(msg_in):
global pub_imu
global pub_gps
global msg_imu
msg_imu.header.stamp = msg_in.header.stamp
msg_imu.header.frame_id = msg_in.header.frame_id
# Convert the RPY data from the Vectornav into radians!
roll = (math.pi * msg_in.RPY.x) / 180.0
pitch = (math.pi * msg_in.RPY.y) / 180.0
yaw = (math.pi * msg_in.RPY.z) / 180.0
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
msg_imu.orientation.x = q[0]
msg_imu.orientation.y = q[1]
msg_imu.orientation.z = q[2]
msg_imu.orientation.w = q[3]
pub_imu.publish(msg_imu)
msg_gps = NavSatFix()
msg_gps.header = msg_in.header
msg_gps.status.status = NavSatStatus.STATUS_FIX # TODO - fix this
msg_gps.status.service = NavSatStatus.SERVICE_GPS
msg_gps.latitude = msg_in.LLA.x
msg_gps.longitude = msg_in.LLA.y
msg_gps.altitude = msg_in.LLA.z
msg_gps.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
msg_gps.position_covariance[0] = msg_in.PosUncerainty
pub_gps.publish(msg_gps)
msg_time = TimeReference()
msg_time.header.stamp = msg_in.header.stamp
msg_time.header.frame_id = msg_in.header.frame_id
unix_time = 315964800 + (msg_in.Week * 7 * 24 * 3600) + msg_in.Time
msg_time.time_ref = rospy.Time.from_sec(unix_time)
pub_time.publish(msg_time)
def gps2navsat(filename, bag):
with open(filename, 'r') as file:
try:
while True:
data = struct.unpack('qddd', file.read(8*4))
time = data[0]
local = data[1:]
lat_lon_el_theta = struct.unpack('dddd', file.read(8*4))
gps_cov = struct.unpack('dddddddddddddddd', file.read(8*16))
if abs(lat_lon_el_theta[0]) < 1e-1:
continue
navsat = NavSatFix()
navsat.header.frame_id = 'gps'
navsat.header.stamp = rospy.Time.from_sec(time * 1e-6)
navsat.status.status = NavSatStatus.STATUS_FIX
navsat.status.service = NavSatStatus.SERVICE_GPS
navsat.latitude = lat_lon_el_theta[0]
navsat.longitude = lat_lon_el_theta[1]
navsat.altitude = lat_lon_el_theta[2]
navsat.position_covariance = numpy.array(gps_cov).reshape(4, 4)[:3, :3].flatten().tolist()
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN
bag.write('/gps/fix', navsat, navsat.header.stamp)
geopoint = GeoPointStamped()
geopoint.header = navsat.header
geopoint.position.latitude = lat_lon_el_theta[0]
geopoint.position.longitude = lat_lon_el_theta[1]
geopoint.position.altitude = lat_lon_el_theta[2]
bag.write('/gps/geopoint', geopoint, geopoint.header.stamp)
except:
print 'done'
def talker():
# Subscribe to Vicon messages
viconTopic = rospy.get_param('topic')
rospy.Subscriber(viconTopic, TransformStamped, callback)
# Start a publisher for the GPS messages
pub = rospy.Publisher('GPS/position', NavSatFix) # FIXME
# Start the node
rospy.init_node('talker')
# Populate the NavSatFix message from the parameter server
statusMsg = NavSatStatus()
statusMsg.status = rospy.get_param('status', -1)
statusMsg.service = rospy.get_param('service', 1)
fixMsg = NavSatFix()
fixMsg.header.stamp = rospy.Time.now()
fixMsg.header.frame_id = "/world"
fixMsg.status = statusMsg
fixMsg.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
#position could be modified with some gaussian noise added to it and then calculate the covariance matrix.
fixMsg.position_covariance_type = rospy.get_param('position_covariance_type', 0);
while not rospy.is_shutdown():
[fixMsg.longitude, fixMsg.latitude, fixMsg.altitude] = c.xyz2gps([parentFrameLong, parentFrameLat, parentFrameAlt], latestViconMsg.transform.translation.x, latestViconMsg.transform.translation.y, latestViconMsg.transform.translation.z, parentFrameAngle)
statusMsg.status = rospy.get_param('status', statusMsg.status)
statusMsg.service = rospy.get_param('service', statusMsg.service)
# put the sigma and calculate the cov matrix here
#rospy.loginfo([fixMsg.longitude, fixMsg.latitude, fixMsg.altitude])
pub.publish(fixMsg)
rospy.sleep(0.1)
def publish(self, data):
now = int(round(time.time() * 1000))
msg = NavSatFix()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.latitude = data.getLat()
msg.longitude = data.getLng()
msg.altitude = data.getMeters()
cur_fix = data.getFix()
#print cur_fix
if (cur_fix != self._old_fix):
msg.status.status = 0
self._old_fix = cur_fix
self._wd = now
elif (now - self._wd) < GPS_WD_TIMEOUT:
msg.status.status = 0
else:
msg.status.status = -1
msg.position_covariance_type = 1
msg.position_covariance[0] = data.getHDOP() * data.getHDOP()
msg.position_covariance[4] = data.getHDOP() * data.getHDOP()
msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP()
msg.status.service = 1
self._pub.publish(msg)
def navigation_handler(self, data):
""" Rebroadcasts navigation data in the following formats:
1) /odom => /base footprint transform (if enabled, as per REP 105)
2) Odometry message, with parent/child IDs as in #1
3) NavSatFix message, for systems which are knowledgeable about GPS stuff
4) IMU messages
"""
rospy.logdebug("Navigation received")
# If we don't have a fix, don't publish anything...
if self.nav_status.status == NavSatStatus.STATUS_NO_FIX:
return
orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(data.pitch), RAD(data.heading)).GetQuaternion()
# UTM conversion
(zone, easting, northing) = LLtoUTM(23, data.latitude, data.longitude)
# Initialize starting point if we haven't yet
# TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid?
if not self.init and self.zero_start:
self.origin.x = easting
self.origin.y = northing
self.init = True
# Publish origin reference for others to know about
p = Pose()
p.position.x = self.origin.x
p.position.y = self.origin.y
self.pub_origin.publish(p)
#
# Odometry
# TODO: Work out these covariances properly from DOP
#
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = self.odom_frame
odom.child_frame_id = self.base_frame
odom.pose.pose.position.x = easting - self.origin.x
odom.pose.pose.position.y = northing - self.origin.y
odom.pose.pose.position.z = data.altitude
odom.pose.pose.orientation = Quaternion(*orient)
odom.pose.covariance = POSE_COVAR
# Twist is relative to /vehicle frame
odom.twist.twist.linear.x = data.speed
odom.twist.twist.linear.y = 0
odom.twist.twist.linear.z = -data.down_vel
odom.twist.twist.angular.x = RAD(data.ang_rate_long)
odom.twist.twist.angular.y = RAD(-data.ang_rate_trans)
odom.twist.twist.angular.z = RAD(-data.ang_rate_down)
odom.twist.covariance = TWIST_COVAR
self.pub_odom.publish(odom)
#
# Odometry transform (if required)
#
if self.publish_tf:
self.tf_broadcast.sendTransform(
(odom.pose.pose.position.x, odom.pose.pose.position.y,
odom.pose.pose.position.z), Quaternion(*orient),
odom.header.stamp,odom.child_frame_id, odom.frame_id)
#
# NavSatFix
# TODO: Work out these covariances properly from DOP
#
navsat = NavSatFix()
navsat.header.stamp = rospy.Time.now()
navsat.header.frame_id = self.odom_frame
navsat.status = self.nav_status
navsat.latitude = data.latitude
navsat.longitude = data.longitude
navsat.altitude = data.altitude
navsat.position_covariance = NAVSAT_COVAR
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.pub_navsatfix.publish(navsat)
#
# IMU
# TODO: Work out these covariances properly
#
imu = Imu()
imu.header.stamp == rospy.Time.now()
imu.header.frame_id = self.base_frame
# Orientation
imu.orientation = Quaternion(*orient)
imu.orientation_covariance = IMU_ORIENT_COVAR
# Angular rates
imu.angular_velocity.x = RAD(data.ang_rate_long)
imu.angular_velocity.y = RAD(-data.ang_rate_trans)
imu.angular_velocity.y = RAD(-data.ang_rate_down)
imu.angular_velocity_covariance = IMU_VEL_COVAR
# Linear acceleration
imu.linear_acceleration.x = data.long_accel
imu.linear_acceleration.y = data.trans_accel
imu.linear_acceleration.z = data.down_accel
imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
self.pub_imu.publish(imu)
pass
def add_sentence(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. \
Sentence was: %s" % nmea_string)
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" %
nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_heading = Imu()
current_heading.header.stamp = current_time
current_heading.header.frame_id = 'base_footprint'
current_direction = String() # For testing
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
# Add capture/publishing heading info
if not self.use_RMC and 'HDT' in parsed_sentence:
#rospy.loginfo("HDT!")
data = parsed_sentence['HDT']
tempHeading = data['true_heading']
ccHeading = (2 * math.pi) - tempHeading
q = tf.transformations.quaternion_from_euler(0,0,ccHeading)
current_heading.orientation.x = q[0]
current_heading.orientation.y = q[1]
current_heading.orientation.z = q[2]
current_heading.orientation.w = q[3]
#current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
#if (current_heading.data < .3927): current_direction.data = "N"
#elif (current_heading.data < 1.178): current_direction.data = "NE"
#elif (current_heading.data < 1.9635): current_direction.data = "E"
#elif (current_heading.data < 2.74889): current_direction.data = "SE"
#elif (current_heading.data < 3.53429): current_direction.data = "S"
#elif (current_heading.data < 4.31969): current_direction.data = "SW"
#elif (current_heading.data < 5.10509): current_direction.data = "W"
#elif (current_heading.data < 5.89048): current_direction.data = "NW"
#else: current_direction.data = "N"
self.heading_pub.publish(current_heading)
#self.direction_pub.publish(current_direction)
#self.time_ref_pub.publish(current_time_ref)
elif 'GGA' in parsed_sentence:
#rospy.loginfo("GGA!")
data = parsed_sentence['GGA']
gps_qual = data['fix_type']
if gps_qual == 0:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
elif gps_qual == 1:
current_fix.status.status = NavSatStatus.STATUS_FIX
elif gps_qual == 2:
current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX
elif gps_qual in (4, 5):
current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
current_fix.header.stamp = current_time
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
hdop = data['hdop']
current_fix.position_covariance[0] = hdop**2
current_fix.position_covariance[4] = hdop**2
current_fix.position_covariance[8] = (2*hdop)**2 # FIXME
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
self.fix_pub.publish(current_fix)
self.time_ref_pub.publish(current_time_ref)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
# Only publish a fix from RMC if the use_RMC flag is set.
if self.use_RMC:
if data['fix_valid']:
current_fix.status.status = NavSatStatus.STATUS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
self.fix_pub.publish(current_fix)
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide it.
if data['fix_valid']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
else:
return False
gpstime.header.stamp = gpsVel.header.stamp
gpstime.time_ref = convertNMEATimeToROS(fields[1])
longitude = float(fields[5][0:3]) + float(fields[5][3:])/60
if fields[6] == 'W':
longitude = -longitude
latitude = float(fields[3][0:2]) + float(fields[3][2:])/60
if fields[4] == 'S':
latitude = -latitude
#publish data
navData.latitude = latitude
navData.longitude = longitude
navData.altitude = float('NaN')
navData.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
gpspub.publish(navData)
gpstimePub.publish(gpstime)
else:
pass
#print data
else:
#Use GGA
#No /vel output from just GGA
if 'GGA' in fields[0]:
gps_quality = int(fields[6])
if gps_quality == 0:
navData.status.status = NavSatStatus.STATUS_NO_FIX
elif gps_quality == 1:
navData.status.status = NavSatStatus.STATUS_FIX
elif gps_quality == 2:
def gpsCallback(data):
gps_reading = marshal.loads(data.data)
current_time = rospy.Time.now()
frame_id = "gps_link"
time_ref_msg = TimeReference()
time_ref_msg.header.stamp = current_time
time_ref_msg.header.frame_id = frame_id
# if 'timestamp' in gps_reading:
# timestamp = gps_reading['timestamp']
# timestamp_s = datetime.time(
# hour=int(timestamp[0:2]),
# minute=int(timestamp[3:5]),
# second=int(timestamp[6:8]),
# microsecond=int(timestamp[9:]))
# time_ref_msg.time_ref = rospy.Time.from_sec(timestamp_s.second)
# time_ref_msg.source = "gps_time"
# else:
# time_ref_msg.source = frame_id
time_ref_msg.source = frame_id
time_ref_pub.publish(time_ref_msg)
nav_msg = NavSatFix()
nav_msg.header.stamp = current_time
nav_msg.header.frame_id = frame_id
gps_qual = gps_reading['qual']
if gps_qual == 1:
nav_msg.status.status = NavSatStatus.STATUS_FIX
elif gps_qual == 2:
nav_msg.status.status = NavSatStatus.STATUS_SBAS_FIX
elif gps_qual in (4, 5):
nav_msg.status.status = NavSatStatus.STATUS_GBAS_FIX
elif gps_qual == 9:
nav_msg.status.status = NavSatStatus.STATUS_SBAS_FIX
else:
nav_msg.status.status = NavSatStatus.STATUS_NO_FIX
nav_msg.status.service = NavSatStatus.SERVICE_GPS
nav_msg.latitude = gps_reading['latitude']
nav_msg.longitude = gps_reading['longitude']
# nav_msg.altitude = float('NaN')
nav_msg.altitude = 0 # EKF Not outputing when using NaN?
nav_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
navsatfix_pub.publish(nav_msg)
vel_msg = TwistStamped()
vel_msg.header.stamp = current_time
vel_msg.header.frame_id = frame_id
vel_msg.twist.linear.x = gps_reading['speed_over_ground'] * math.sin(gps_reading['course_over_ground'])
vel_msg.twist.linear.y = gps_reading['speed_over_ground'] * math.cos(gps_reading['course_over_ground'])
vel_pub.publish(vel_msg)
marker_msg = Marker()
marker_msg.header = nav_msg.header
marker_msg.action = 0 # ADD
marker_msg.type = 0 # ARROW
marker_msg.scale.x = 1
marker_msg.scale.y = 0.1
marker_msg.scale.z = 0.1
marker_msg.color.a = 1.0
marker_msg.color.r = 0.0;
marker_msg.color.g = 0.0;
marker_msg.color.b = 1.0;
marker_msg.pose.position.x = 0
marker_msg.pose.position.y = 0
quat = tf.transformations.quaternion_from_euler(0, 0, 0)
marker_msg.pose.orientation.x = quat[0]
marker_msg.pose.orientation.y = quat[1]
marker_msg.pose.orientation.z = quat[2]
marker_msg.pose.orientation.w = quat[3]
marker_pub.publish(marker_msg)
def add_sentence(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if 'B' in parsed_sentence:
data = parsed_sentence['B']
# Only publish a fix from RMC if the use_RMC flag is set.
if True:
current_fix.status.status = NavSatStatus.STATUS_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
current_fix.latitude = data['latitude']
current_fix.longitude = data['longitude']
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide it.
if True:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
else:
return False
def add_sentence(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(
nmea_string)
if not parsed_sentence:
rospy.logdebug(
"Failed to parse NMEA sentence. Sentence was: %s" %
nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if not self.use_RMC and 'GGA' in parsed_sentence:
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
data = parsed_sentence['GGA']
fix_type = data['fix_type']
if not (fix_type in self.gps_qualities):
fix_type = -1
gps_qual = self.gps_qualities[fix_type]
default_epe = gps_qual[0]
current_fix.status.status = gps_qual[1]
current_fix.position_covariance_type = gps_qual[2]
if gps_qual > 0:
self.valid_fix = True
else:
self.valid_fix = False
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
# use default epe std_dev unless we've received a GST sentence with
# epes
if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
self.lon_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
self.lat_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
self.alt_std_dev = default_epe * 2
hdop = data['hdop']
current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2
current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2
current_fix.position_covariance[8] = (
2 * hdop * self.alt_std_dev) ** 2 # FIXME
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.last_valid_fix_time = current_time_ref
self.time_ref_pub.publish(current_time_ref)
elif not self.use_RMC and 'VTG' in parsed_sentence:
data = parsed_sentence['VTG']
# Only report VTG data when you've received a valid GGA fix as
# well.
if self.valid_fix:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
# Only publish a fix from RMC if the use_RMC flag is set.
if self.use_RMC:
if data['fix_valid']:
current_fix.status.status = NavSatStatus.STATUS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide
# it.
if data['fix_valid']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
elif 'GST' in parsed_sentence:
data = parsed_sentence['GST']
# Use receiver-provided error estimate if available
self.using_receiver_epe = True
self.lon_std_dev = data['lon_std_dev']
self.lat_std_dev = data['lat_std_dev']
self.alt_std_dev = data['alt_std_dev']
elif 'HDT' in parsed_sentence:
data = parsed_sentence['HDT']
if data['heading']:
current_heading = QuaternionStamped()
current_heading.header.stamp = current_time
current_heading.header.frame_id = frame_id
q = quaternion_from_euler(0, 0, math.radians(data['heading']))
current_heading.quaternion.x = q[0]
current_heading.quaternion.y = q[1]
current_heading.quaternion.z = q[2]
current_heading.quaternion.w = q[3]
self.heading_pub.publish(current_heading)
else:
return False
def add_sentence(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libjavad_navsat_driver.parser.parse_nmea_sentence(nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if not self.use_RMC and 'GGA' in parsed_sentence:
data = parsed_sentence['GGA']
gps_qual = data['fix_type']
if gps_qual == 0:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
elif gps_qual == 1:
current_fix.status.status = NavSatStatus.STATUS_FIX
elif gps_qual == 2:
current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX
elif gps_qual in (4, 5):
current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
current_fix.header.stamp = current_time
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
hdop = data['hdop']
current_fix.position_covariance[0] = hdop ** 2
current_fix.position_covariance[4] = hdop ** 2
current_fix.position_covariance[8] = (2 * hdop) ** 2 # FIXME
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
# Altitude is above ellipsoid, so adjust for mean-sea-level
#altitude = data['altitude'] + data['mean_sea_level']
altitude = data['altitude']
current_fix.altitude = altitude
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
# Only publish a fix from RMC if the use_RMC flag is set.
if self.use_RMC:
if data['fix_valid']:
current_fix.status.status = NavSatStatus.STATUS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide it.
if data['fix_valid']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
current_vel.twist.angular.z = data['true_course']
self.vel_pub.publish(current_vel)
else:
return False
def talker():
pubGPS = rospy.Publisher('poseGPS', NavSatFix, queue_size=1000)
pubIMU = rospy.Publisher('imuBoat', Imu, queue_size=1000)
pubTwist = rospy.Publisher('twistGPS', TwistWithCovarianceStamped, queue_size=1000)
rospy.init_node('imugpspublisher', anonymous=True)
rate = rospy.Rate(100) # 100hz
while not rospy.is_shutdown():
data = ser.readline()
if data[0] == 'Z':
data = data.replace("Z","").replace("\n","").replace("\r","")
data_list = data.split(',')
if len(data_list) == 4:
try:
##data_list structure: lat, lon, heading, vel
float_list = [float(i) for i in data_list]
poseGPS = NavSatFix()
poseGPS.header.frame_id = "world"
poseGPS.header.stamp = rospy.Time.now()
poseGPS.status.status = 0
poseGPS.status.service = 1
poseGPS.latitude = float_list[0]
poseGPS.longitude = float_list[1]
poseGPS.altitude = 0.0
poseGPS.position_covariance = [3.24, 0, 0, 0, 3,24, 0, 0, 0, 0]
poseGPS.position_covariance_type = 2
pubGPS.publish(poseGPS)
twistGPS = TwistWithCovarianceStamped()
twistGPS.header = poseGPS.header
twistGPS.twist.twist.linear.x = float_list[3]*knots*math.cos(latest_yaw)
twistGPS.twist.twist.linear.y = float_list[3]*knots*math.sin(latest_yaw)
twistGPS.twist.twist.linear.z = 0.0
##angular data not used here
twistGPS.twist.covariance = [0.01, 0.01, 0, 0, 0, 0]
pubTwist.publish(twistGPS)
log = "GPS Data: %f %f %f %f Publish at Time: %s" % (float_list[0], float_list[1], float_list[2], float_list[3], rospy.get_time())
except: log = "GPS Data Error! Data : %s" % data
else:
log = "GPS Data Error! Data : %s" % data
rospy.loginfo(log)
elif data[0] == 'Y':
data = data.replace("Y","").replace("\n","").replace("\r","")
data_list = data.split(',')
if len(data_list) == 9:
try:
##data_list structure: accel, magni, gyro
float_list = [float(i) for i in data_list]
imuData = Imu()
imuData.header.frame_id = "base_link"
imuData.header.stamp = rospy.Time.now()
latest_yaw = float_list[3]
##data form in yaw, pitch, roll
quat = tf.transformations.quaternion_from_euler(float_list[3], float_list[4], float_list[5], 'rzyx')
imuData.orientation.x = quat[0]
imuData.orientation.y = quat[1]
imuData.orientation.z = quat[2]
imuData.orientation.w = quat[3]
imuData.angular_velocity.x = math.radians(float_list[6]*gyro_scale)
imuData.angular_velocity.y = math.radians(-float_list[7]*gyro_scale)
imuData.angular_velocity.z = math.radians(-float_list[8]*gyro_scale)
imuData.linear_acceleration.x = float_list[0]*accel_scale
imuData.linear_acceleration.y = -float_list[1]*accel_scale
imuData.linear_acceleration.z = -float_list[2]*accel_scale
imuData.orientation_covariance = [1.5838e-6, 0, 0, 0, 1.49402e-6, 0, 0, 0, 1.88934e-6]
imuData.angular_velocity_covariance = [7.84113e-7, 0, 0, 0, 5.89609e-7, 0, 0, 0, 6.20293e-7]
imuData.linear_acceleration_covariance = [9.8492e-4, 0, 0, 0, 7.10809e-4, 0, 0, 0, 1.42516e-3]
pubIMU.publish(imuData)
log = "IMU Data: %f %f %f %f %f %f %f %f %f Publish at Time: %s" \
% (imuData.linear_acceleration.x, imuData.linear_acceleration.y, imuData.linear_acceleration.z,
float_list[3], float_list[4], float_list[5],
imuData.angular_velocity.x, imuData.angular_velocity.y, imuData.angular_velocity.z, rospy.get_time())
except: log = "IMU Data Error! Data : %s" % data
else: log = "Data Error! Data : %s" % data
rospy.loginfo(log)
else:
log = "Data Error or Message: %s" % data
rospy.loginfo(log)
rate.sleep()
def navigation_handler(self, data):
""" Rebroadcasts navigation data in the following formats:
1) /odom => /base footprint transform (if enabled, as per REP 105)
2) Odometry message, with parent/child IDs as in #1
3) NavSatFix message, for systems which are knowledgeable about GPS stuff
4) IMU messages
"""
rospy.logdebug("Navigation received")
# If we don't have a fix, don't publish anything...
if self.nav_status.status == NavSatStatus.STATUS_NO_FIX:
return
# UTM conversion
easting, northing = data.bestxyz.easting, data.bestxyz.northing
# Initialize starting point if we haven't yet
# TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid?
# TODO: check INSPVAX sol stat for valid position before accepting
if not self.init and self.zero_start:
self.origin.x = easting
self.origin.y = northing
self.init = True
# Publish origin reference for others to know about
p = Pose()
p.position.x = self.origin.x
p.position.y = self.origin.y
self.pub_origin.publish(p)
# IMU
# TODO: Work out these covariances properly. Logs provide covariances in local frame, not body
imu = Imu()
imu.header.stamp == rospy.Time.now()
imu.header.frame_id = self.base_frame
# Orientation
# orient=PyKDL.Rotation.RPY(RAD(data.roll),RAD(data.pitch),RAD(data.heading)).GetQuaternion()
# imu.orientation = Quaternion(*orient)
imu.orientation.x = data.inspvax.pitch
imu.orientation.y = data.inspvax.roll
imu.orientation.z = data.inspvax.azimuth
imu.orientation.w = 0
IMU_ORIENT_COVAR[0] = POW(2, data.inspvax.pitch_std)
IMU_ORIENT_COVAR[4] = POW(2, data.inspvax.roll_std)
IMU_ORIENT_COVAR[8] = POW(2, data.inspvax.azimuth_std)
imu.orientation_covariance = IMU_ORIENT_COVAR
# Angular rates (rad/s)
# corrimudata log provides instantaneous rates so multiply by IMU rate in Hz
imu.angular_velocity.x = data.corrimudata.pitch_rate * self.imu_rate
imu.angular_velocity.y = data.corrimudata.roll_rate * self.imu_rate
imu.angular_velocity.z = data.corrimudata.yaw_rate * self.imu_rate
imu.angular_velocity_covariance = IMU_VEL_COVAR
# Linear acceleration (m/s^2)
imu.linear_acceleration.x = data.corrimudata.x_accel * self.imu_rate
imu.linear_acceleration.y = data.corrimudata.y_accel * self.imu_rate
imu.linear_acceleration.z = data.corrimudata.z_accel * self.imu_rate
imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
self.pub_imu.publish(imu)
# Odometry
# TODO: Work out these covariances properly
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = self.odom_frame
odom.child_frame_id = self.base_frame
odom.pose.pose.position.x = easting - self.origin.x
odom.pose.pose.position.y = northing - self.origin.y
odom.pose.pose.position.z = data.inspvax.altitude
#odom.pose.pose.orientation = Quaternion(*orient)
odom.pose.pose.orientation = imu.orientation
POSE_COVAR[21] = IMU_ORIENT_COVAR[0]
POSE_COVAR[28] = IMU_ORIENT_COVAR[4]
POSE_COVAR[35] = IMU_ORIENT_COVAR[8]
odom.pose.covariance = POSE_COVAR
# Twist is relative to vehicle frame
odom.twist.twist.linear.x = data.bestxyz.velx
odom.twist.twist.linear.y = data.bestxyz.vely
odom.twist.twist.linear.z = data.bestxyz.velz
TWIST_COVAR[0] = pow(2,data.bestxyz.velx_std)
TWIST_COVAR[7] = pow(2,data.bestxyz.vely_std)
TWIST_COVAR[14] = pow(2,data.bestxyz.velz_std)
odom.twist.twist.angular = imu.angular_velocity
odom.twist.covariance = TWIST_COVAR
self.pub_odom.publish(odom)
#
# Odometry transform (if required)
#
if self.publish_tf:
self.tf_broadcast.sendTransform(
(odom.pose.pose.position.x, odom.pose.pose.position.y,
odom.pose.pose.position.z), odom.pose.pose.orientation, #Quaternion(*orient),
odom.header.stamp,odom.child_frame_id, odom.frame_id)
#
# NavSatFix
# TODO: Work out these covariances properly from DOP
#
navsat = NavSatFix()
navsat.header.stamp = rospy.Time.now()
navsat.header.frame_id = self.odom_frame
navsat.status = self.nav_status
# position, in degrees
navsat.latitude = data.bestpos.latitude
navsat.longitude = data.bestpos.longitude
navsat.altitude = data.bestpos.altitude
NAVSAT_COVAR[0] = pow(2, data.bestpos.lat_std) # in meters
NAVSAT_COVAR[4] = pow(2, data.bestpos.lon_std)
NAVSAT_COVAR[8] = pow(2, data.bestpos.hgt_std)
navsat.position_covariance = NAVSAT_COVAR
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
self.pub_navsatfix.publish(navsat)
