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 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 publish_navsatfix(msg, num):
global i_navsatfix
if msg.name() != 'NAV_HPPOSLLH':
print "error: invalid type of message!!!!"
return
navsatfix = NavSatFix()
navsatfix.header.stamp = rospy.Time.now()
if num == 1:
navsatfix.header.frame_id = "/left_gnss"
elif num == 2:
navsatfix.header.frame_id = "/right_gnss"
navsatfix.header.seq = i_navsatfix[num - 1]
navsatfix.latitude = msg.Latitude * 1e-7 + msg.latHp * 1e-9
navsatfix.longitude = msg.Longitude * 1e-7 + msg.lonHp * 1e-9
navsatfix.altitude = msg.height * 1e-3 + msg.heightHp * 1e-4
navsatfix.position_covariance = [(msg.hAcc * 1e-4) / math.sqrt(2.), 0, 0,
0, (msg.hAcc * 1e-4) / math.sqrt(2.), 0,
0, 0, (msg.vAcc * 1e-4)]
navsatfix.status.status = fix_status[num - 1]
if num == 1:
pub_leftnavsatfix.publish(navsatfix)
elif num == 2:
pub_rightnavsatfix.publish(navsatfix)
i_navsatfix[num - 1] += 1
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 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 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 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 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 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 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 publishCurLatLon(self):
msg = NavSatFix()
msg.header.stamp = rospy.Time(self.curTime)
msg.latitude = self.curLat
msg.longitude = self.curLon
# very small error for now
msg.position_covariance = [0] * 9
msg.position_covariance[0] = 5**2
msg.position_covariance[3] = 5**2
rospy.loginfo("Publishing position %f, %f at time %f", self.curLat,
self.curLon, self.curTime)
self.fixPub.publish(msg)
def main():
pub = rospy.Publisher("goal_coords", NavSatFix, queue_size=10)
rospy.init_node("goal_pub", anonymous=False)
r = rospy.Rate(1)
while not rospy.is_shutdown():
msg = NavSatFix()
msg.header.frame_id = "gps_frame"
msg.longitude = 100
msg.latitude = 100
msg.position_covariance = [1, 0, 0, 0, 1, 0, 0, 0, 1]
pub.publish(msg)
r.sleep()
print(msg)
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 ubicacion(self):
rospy.init_node("GPS_DATA")
pub1 = rospy.Publisher('/gps/odom', Odometry, queue_size=10)
pub2 = rospy.Publisher('/gps/data', NavSatFix, queue_size=10)
pub3 = rospy.Publisher('/gps/str', String, queue_size=10)
gps = serial.Serial(self.port, baudrate=self.baud, timeout=0.1)
if gps.isOpen():
rate = rospy.Rate(100) # 100hz
while not rospy.is_shutdown():
try:
self.GPS_read(gps)
self.LLH2GPS(self.Latitude, self.Longitude)
except:
pass
# PUBLISH DATA
gps_msg = NavSatFix()
gps_msg.header.stamp = rospy.get_rostime()
gps_msg.header.frame_id = "gps_data"
gps_msg.status.status = -1
if self.quality == 1:
gps_msg.status.status = 0
elif self.quality == 4:
gps_msg.status.status = 2
elif self.quality == 5:
gps_msg.status.status = 1
gps_msg.latitude = self.Latitude
gps_msg.longitude = self.Longitude
gps_msg.altitude = self.altitude
gps_msg.position_covariance = np.array([
self.eLat, 0.0, 0.0, 0.0, self.eLon, 0.0, 0.0, 0.0,
self.eAlt
])
gps_odm = Odometry()
gps_odm.header.stamp = rospy.get_rostime()
gps_odm.header.frame_id = "gps_odom_data"
gps_odm.child_frame_id = "UTM_coordenates"
gps_odm.pose.pose.position.x = self.x
gps_odm.pose.pose.position.y = self.y
gps_odm.pose.pose.position.z = self.altitude
gps_odm.pose.pose.orientation.z = self.COG
gps_odm.twist.twist.linear.x = self.speed_m_s
pub1.publish(gps_odm)
pub2.publish(gps_msg)
pub3.publish(self.line)
rate.sleep()
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 main():
ser = serial.Serial(port, 9600)
pub_nfs = rospy.Publisher('gps_nfs', NavSatFix, queue_size=10)
pub_odo = rospy.Publisher('gps_odo', Odometry, queue_size=10)
rospy.init_node('internal_gps', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
gpsdat = ser.readline()
if gpsdat[0:6] == "$GPGGA":
msg = pynmea2.parse(gpsdat)
r_msg = NavSatFix()
r_msg.latitude = msg.latitude
r_msg.longitude = msg.longitude
r_msg.altitude = msg.altitude
c = msg.gps_qual
r_msg.position_covariance = \
[c,0,0, 0,c,0, 0,0,c]
pub_nfs.publish(r_msg)
o_msg = Odometry()
o_msg.header.frame_id = "gps_frame"
o_msg.pose.pose.position.x = msg.latitude
o_msg.pose.pose.position.y = msg.longitude
o_msg.pose.pose.position.z = msg.altitude
o_msg.pose.pose.orientation.x = 1
o_msg.pose.pose.orientation.y = 0
o_msg.pose.pose.orientation.z = 0
o_msg.pose.pose.orientation.w = 0
h = 99999
o_msg.pose.covariance = [
c, 0, 0, 0, 0, 0, 0, c, 0, 0, 0, 0, 0, 0, c, 0, 0, 0, 0, 0, 0,
h, 0, 0, 0, 0, 0, 0, h, 0, 0, 0, 0, 0, 0, h
]
o_msg.twist.twist.linear.x = 0
o_msg.twist.twist.linear.y = 0
o_msg.twist.twist.linear.z = 0
o_msg.twist.twist.angular.x = 0
o_msg.twist.twist.angular.y = 0
o_msg.twist.twist.angular.z = 0
o_msg.twist.covariance = [
h, 0, 0, 0, 0, 0, 0, h, 0, 0, 0, 0, 0, 0, h, 0, 0, 0, 0, 0, 0,
h, 0, 0, 0, 0, 0, 0, h, 0, 0, 0, 0, 0, 0, h
]
print(r_msg)
pub_odo.publish(o_msg)
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 parse_gps(self, data):
# {
# u'status':
# {
# u'pDOP': 99.99,
# u'positionLatLonAlt': [u'0.000000000', u'0.000000000', u'0.000000000'],
# u'satellites_dB': u'15,',
# u'gga': u'',
# u'vAcc': 3750111.232,
# u'position': [u'6378137.0000', u'0.0000', u'0.0000'],
# u'estAccuracy': 4294967.295, u'heading': 0,
# u'headingAccuracy': 180,
# u'hAcc': 4294967.295
# },
# u'ts': 77168472,
# u'general':
# {
# u'gpsFix': u'no fix/invalid',
# u'mode': u'rover'
# }
# }
fix = NavSatFix()
fix.header.stamp = rospy.Time.now()
fix.header.frame_id = self.frame
fix.status.service = NavSatStatus.SERVICE_GPS
fix.position_covariance = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
try:
fix.latitude = float(data['status']['positionLatLonAlt'][0])
fix.longitude = float(data['status']['positionLatLonAlt'][1])
fix.altitude = float(data['status']['positionLatLonAlt'][2])
fix.position_covariance[0] = float(data['status']['hAcc'])
fix.position_covariance[4] = float(data['status']['hAcc'])
fix.position_covariance[8] = float(data['status']['vAcc'])
if data['general']['gpsFix'] == 'no fix/invalid':
fix.status.status = NavSatStatus.STATUS_NO_FIX
else:
fix.status.status = NavSatStatus.STATUS_FIX
except KeyError as e:
rospy.logwarn('Unable to GNSS parse message ' + str(e))
print(data)
else:
self.nav_sat_fix_pub.publish(fix)
def parse_novatelGPS(gpsString):
# ----- parse the data string from fields to variables -----
solutionStatus = gpsString[0] # Solution status
positionType = gpsString[1] # Position type
latitude = gpsString[2] # Latitude
longitude = gpsString[3] # Longitude
heightMSL = gpsString[4] # Height above mean sea level
undulation = gpsString[5] # Undulation
datumID = gpsString[6] # Datum ID
latitudeDev = gpsString[7] # Latitude standard deviation
longitudeDev = gpsString[8] # Longitude standard deviation
heightDev = gpsString[9] # Height standard deviation
baseStationID = gpsString[10] # Base station ID
differentialAge = gpsString[11] # Differential age
solutionAge = gpsString[12] # Solution age in seconds
observationsTracked = gpsString[13] # Number of observations tracked
usedL1Ranges = gpsString[
14] # Number of satellite solutions used in computation
aboveMaskL1Ranges = gpsString[
15] # Number of GPS and GLONASS L1 ranges above the RTK mask angle
aboveMaskL2Ranges = gpsString[
16] # Number of GPS and GLONASS L2 ranges above the RTK mask angle
# ----- Format the gps data into ros msg -----
fix_msg = NavSatFix()
fix_msg.header.stamp = rospy.get_rostime()
fix_msg.header.frame_id = 'cns5000_frame'
fix_msg.latitude = float(latitude)
fix_msg.longitude = float(longitude)
fix_msg.altitude = float(heightMSL)
fix_msg.position_covariance = [
0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 999.0
]
fix_msg.position_covariance_type = 1
#fix_msg.position_covariance_type = 0
#fix_msg.position_covariance = 0
#fix_msg.status = 0 # Fix
# TODO add a check for a valid position. Currently all data is marked as valid
# TODO use the checksum to check the message
#fix_msg.status = -1 # No Fix
#print "Solution Status:", solutionStatus
return fix_msg
def publish_gps_point(self, latitude, longitude, height, variance, status,
pub_navsatfix, pub_pose, pub_point, pub_transform):
# Navsatfix message.
navsatfix_msg = NavSatFix()
navsatfix_msg.header.stamp = rospy.Time.now()
navsatfix_msg.header.frame_id = self.navsatfix_frame_id
navsatfix_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
navsatfix_msg.status.service = NavSatStatus.SERVICE_GPS
navsatfix_msg.latitude = latitude
navsatfix_msg.longitude = longitude
navsatfix_msg.altitude = height
navsatfix_msg.status.status = status
navsatfix_msg.position_covariance = [
variance[0], 0, 0, 0, variance[1], 0, 0, 0, variance[2]
]
# Local Enu coordinate.
(east, north, up) = self.geodetic_to_enu(latitude, longitude, height)
# Pose message.
pose_msg = PoseWithCovarianceStamped()
pose_msg.header.stamp = navsatfix_msg.header.stamp
pose_msg.header.frame_id = self.enu_frame_id
pose_msg.pose = self.enu_to_pose_msg(east, north, up, variance)
# Point message.
point_msg = PointStamped()
point_msg.header.stamp = navsatfix_msg.header.stamp
point_msg.header.frame_id = self.enu_frame_id
point_msg.point = self.enu_to_point_msg(east, north, up)
# Transform message.
transform_msg = TransformStamped()
transform_msg.header.stamp = navsatfix_msg.header.stamp
transform_msg.header.frame_id = self.enu_frame_id
transform_msg.child_frame_id = self.transform_child_frame_id
transform_msg.transform = self.enu_to_transform_msg(east, north, up)
# Publish.
pub_navsatfix.publish(navsatfix_msg)
pub_pose.publish(pose_msg)
pub_point.publish(point_msg)
pub_transform.publish(transform_msg)
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 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 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 navigation_handler(self, data):
""" Rebroadcasts navigation data in the following formats:
1) /odom => /base footprint transform (if enabled, as per REP 105)
2) Odometry message, with parent/child IDs as in #1
3) NavSatFix message, for systems which are knowledgeable about GPS stuff
4) IMU messages
"""
rospy.logdebug("Navigation received")
# If we don't have a fix, don't publish anything...
if self.nav_status.status == NavSatStatus.STATUS_NO_FIX:
return
# UTM conversion
easting, northing = data.bestxyz.easting, data.bestxyz.northing
# Initialize starting point if we haven't yet
# TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid?
# TODO: check INSPVAX sol stat for valid position before accepting
if not self.init and self.zero_start:
self.origin.x = easting
self.origin.y = northing
self.init = True
# Publish origin reference for others to know about
p = Pose()
p.position.x = self.origin.x
p.position.y = self.origin.y
self.pub_origin.publish(p)
# IMU
# TODO: Work out these covariances properly. Logs provide covariances in local frame, not body
imu = Imu()
imu.header.stamp == rospy.Time.now()
imu.header.frame_id = self.base_frame
# Orientation
# orient=PyKDL.Rotation.RPY(RAD(data.roll),RAD(data.pitch),RAD(data.heading)).GetQuaternion()
# imu.orientation = Quaternion(*orient)
imu.orientation.x = data.inspvax.pitch
imu.orientation.y = data.inspvax.roll
imu.orientation.z = data.inspvax.azimuth
imu.orientation.w = 0
IMU_ORIENT_COVAR[0] = POW(2, data.inspvax.pitch_std)
IMU_ORIENT_COVAR[4] = POW(2, data.inspvax.roll_std)
IMU_ORIENT_COVAR[8] = POW(2, data.inspvax.azimuth_std)
imu.orientation_covariance = IMU_ORIENT_COVAR
# Angular rates (rad/s)
# corrimudata log provides instantaneous rates so multiply by IMU rate in Hz
imu.angular_velocity.x = data.corrimudata.pitch_rate * self.imu_rate
imu.angular_velocity.y = data.corrimudata.roll_rate * self.imu_rate
imu.angular_velocity.z = data.corrimudata.yaw_rate * self.imu_rate
imu.angular_velocity_covariance = IMU_VEL_COVAR
# Linear acceleration (m/s^2)
imu.linear_acceleration.x = data.corrimudata.x_accel * self.imu_rate
imu.linear_acceleration.y = data.corrimudata.y_accel * self.imu_rate
imu.linear_acceleration.z = data.corrimudata.z_accel * self.imu_rate
imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
self.pub_imu.publish(imu)
# Odometry
# TODO: Work out these covariances properly
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = self.odom_frame
odom.child_frame_id = self.base_frame
odom.pose.pose.position.x = easting - self.origin.x
odom.pose.pose.position.y = northing - self.origin.y
odom.pose.pose.position.z = data.inspvax.altitude
#odom.pose.pose.orientation = Quaternion(*orient)
odom.pose.pose.orientation = imu.orientation
POSE_COVAR[21] = IMU_ORIENT_COVAR[0]
POSE_COVAR[28] = IMU_ORIENT_COVAR[4]
POSE_COVAR[35] = IMU_ORIENT_COVAR[8]
odom.pose.covariance = POSE_COVAR
# Twist is relative to vehicle frame
odom.twist.twist.linear.x = data.bestxyz.velx
odom.twist.twist.linear.y = data.bestxyz.vely
odom.twist.twist.linear.z = data.bestxyz.velz
TWIST_COVAR[0] = pow(2,data.bestxyz.velx_std)
TWIST_COVAR[7] = pow(2,data.bestxyz.vely_std)
TWIST_COVAR[14] = pow(2,data.bestxyz.velz_std)
odom.twist.twist.angular = imu.angular_velocity
odom.twist.covariance = TWIST_COVAR
self.pub_odom.publish(odom)
#
# Odometry transform (if required)
#
if self.publish_tf:
self.tf_broadcast.sendTransform(
(odom.pose.pose.position.x, odom.pose.pose.position.y,
odom.pose.pose.position.z), odom.pose.pose.orientation, #Quaternion(*orient),
odom.header.stamp,odom.child_frame_id, odom.frame_id)
#
# NavSatFix
# TODO: Work out these covariances properly from DOP
#
navsat = NavSatFix()
navsat.header.stamp = rospy.Time.now()
navsat.header.frame_id = self.odom_frame
navsat.status = self.nav_status
# position, in degrees
navsat.latitude = data.bestpos.latitude
navsat.longitude = data.bestpos.longitude
navsat.altitude = data.bestpos.altitude
NAVSAT_COVAR[0] = pow(2, data.bestpos.lat_std) # in meters
NAVSAT_COVAR[4] = pow(2, data.bestpos.lon_std)
NAVSAT_COVAR[8] = pow(2, data.bestpos.hgt_std)
navsat.position_covariance = NAVSAT_COVAR
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
self.pub_navsatfix.publish(navsat)
def my_handler(channel, data):
global seq
fix_pub = rospy.Publisher('fix', NavSatFix, queue_size=10)
odom_pub = rospy.Publisher('odom', Odometry, queue_size=10)
imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10)
use_odom = True
use_fix = True
use_imu = True
gps_frame = "novatel"
headermsg = Header()
headermsg.seq = seq + 1
headermsg.stamp = rospy.Time.now()
headermsg.frame_id = gps_frame
msg = auv_acfr_nav_t.decode(data)
if use_fix == True:
rospy.logdebug("Publising gps info")
fixmsg = NavSatFix()
fixmsg.header = headermsg
fixmsg.latitude = math.radians(msg.latitude)
fixmsg.longitude = math.radians(msg.longitude)
fixmsg.altitude = msg.altitude
fixmsg.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
fixmsg.position_covariance_type = 0
fix_pub.publish(fixmsg)
if use_imu == True:
rospy.logdebug("Publishing imu")
imumsg = Imu()
q = quaternion_from_euler(
msg.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 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 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 code(self):
micro = 1000000
acc_l = False
gyro = False
orien = False
quat = Quaternion()
imuMsg = Imu()
gpsMsg = NavSatFix()
magMsg = MagneticField()
#rate = rospy.Rate(1000)
last = rospy.Time.now().to_sec()
imuMsg.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
imuMsg.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0.02]
imuMsg.linear_acceleration_covariance = [
0.2, 0, 0, 0, 0.2, 0, 0, 0, 0.2
]
gpsMsg.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
while not rospy.is_shutdown():
data_pix = ser1.read()
if data_pix == ">":
data_pix = ser1.readline()
data_list = data_pix.split(",")
#a=len(data_list)
#print(a)
#print(data_list)
tipo = float(data_list[0])
xx = float(data_list[2])
yy = float(data_list[3])
zz = float(data_list[4])
if tipo == 1: #ACCELEROMETER (m/s^2 - X,Y,Z)
acc = True
elif tipo == 2: #MAGNETIC_FIELD (uT - X,Y,Z)
mag = True
magMsg.header.stamp = rospy.Time.now()
magMsg.header.frame_id = 'base_link'
magMsg.magnetic_field.x = xx / micro
magMsg.magnetic_field.y = yy / micro
magMsg.magnetic_field.z = zz / micro
self.pub_mag.publish(magMsg)
elif tipo == 3: #ORIENTATION (Yaw, Pitch, Roll)
orien = True
# orientation to ENU Right Hand
roll = -zz #*3.14159/180
pitch = yy #*3.18159/180
yaw = -xx + 90 #*3.14159/180
if yaw < -180:
yaw = yaw + 360
#q=tf.transformations.quaternion_from_euler(roll*3.14159/180,pitch*3.18159/180,yaw*3.14159/180)
#imuMsg.orientation = Quaternion(*q)
imuMsg.orientation.x = roll #magnetometer
imuMsg.orientation.y = pitch
imuMsg.orientation.z = yaw
imuMsg.orientation.w = 0
#q=tf.transformations.quaternion_from_euler(zz*3.14159/180,yy*3.18159/180,xx*3.14159/180)'''
#quat.x = q[0] #magnetometer
#quat.y = q[1]
#quat.z = q[2]
#quat.w = q[3]
elif tipo == 4: #GYROSCOPE (rad/sec - X,Y,Z)
gyro = True
imuMsg.angular_velocity.x = xx #gyro
imuMsg.angular_velocity.y = yy
imuMsg.angular_velocity.z = zz
elif tipo == 5: #LIGHT (SI lux)
lux = True
elif tipo == 6: #PRESSURE (hPa millibar)
press = True
elif tipo == 7: #DEVICE TEMPERATURE (C)
temp = True
elif tipo == 8: #PROXIMITY (Centimeters or 1,0)
prox = True
elif tipo == 9: #GRAVITY (m/s^2 - X,Y,Z)
grav = True
elif tipo == 10: #LINEAR_ACCELERATION (m/s^2 - X,Y,Z)
acc_l = True
imuMsg.linear_acceleration.x = xx # tripe axis accelerator meter
imuMsg.linear_acceleration.y = yy
imuMsg.linear_acceleration.z = zz
elif tipo == 11: #ROTATION_VECTOR (Degrees - X,Y,Z)
rot = True
elif tipo == 12: #RELATIVE_HUMIDITY (%)
hum = True
elif tipo == 13: #AMBIENT_TEMPERATURE (C)
amb_temp = True
elif tipo == 14: #MAGNETIC_FIELD_UNCALIBRATED (uT - X,Y,Z)
mag_u = True
elif tipo == 15: #GAME_ROTATION_VECTOR (Degrees - X,Y,Z)
game_rot = True
elif tipo == 16: #GYROSCOPE_UNCALIBRATED (rad/sec - X,Y,Z)
gyro_u = True
elif tipo == 17: #SIGNIFICANT_MOTION (1,0)
sig_mot = True
elif tipo == 97: #AUDIO (Vol.)}
audio = True
elif tipo == 98: #GPS1 (Lat., Long., Alt.)
gps1 = True
gpsMsg.header.stamp = rospy.Time.now()
gpsMsg.header.frame_id = 'base_link'
gpsMsg.latitude = xx
gpsMsg.longitude = yy
gpsMsg.altitude = zz
gpsMsg.position_covariance_type = 0
gpsMsg.status.service = NavSatStatus.SERVICE_GPS
gpsMsg.status.status = NavSatStatus.STATUS_SBAS_FIX #SOLO SAT--STATUS_GBAS_FIX ANTENA TIERRA
self.pub_navsatfix.publish(gpsMsg)
elif tipo == 99: #GPS2 (Bearing, Speed, Date/Time)
gps2 = True
#print('tipo=',tipo,'x=',xx,'y=',yy,'z=',zz,rospy.Time.now().to_sec()-last)
#rate.sleep()
## until the msg is complete
if orien == True and acc_l == True and gyro == True:
imuMsg.header.stamp = rospy.Time.now()
imuMsg.header.frame_id = 'base_link'
self.pub_imu.publish(imuMsg)
acc_l = False
gyro = False
orien = False
def _publish_navsatfix(pub, msg):
"""
Converts a MSG_POS_LLH SBP message to a NavSatFix message and publishes
it on the provided publisher pub. Most of this code is borrowed from
https://github.com/ethz-asl/ethz_piksi_ros/tree/master/piksi_multi_rtk_ros
"""
# Copyright (c) 2017, Autonomous Systems Lab & Robotic Systems Lab,
# ETH Zurich
# All rights reserved.
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are
# met:
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# 3. Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
# names of its contributors may be used to endorse or promote products
# derived from this software without specific prior written permission.
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
# IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
# PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
# CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
# EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
# PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
# PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
# NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
# Convert standard deviation in mm to variance in m^2
h_var = (msg.h_accuracy / 1000)**2
v_var = (msg.h_accuracy / 1000)**2
# Lower 3 bits define fix mode.
fix_mode = msg.flags & 0b111
if fix_mode == 0: # Invalid mode
navsat_status = NavSatStatus.STATUS_NO_FIX
elif fix_mode == 1:
navsat_status = NavSatStatus.STATUS_FIX
elif fix_mode == 2: # Not implemented
navsat_status = NavSatStatus.STATUS_NO_FIX
elif fix_mode == 3:
navsat_status = NavSatStatus.STATUS_GBAS_FIX
elif fix_mode == 4:
navsat_status = NavSatStatus.STATUS_GBAS_FIX
elif fix_mode == 5: # Not implemented
navsat_status = NavSatStatus.STATUS_NO_FIX
elif fix_mode == 6:
navsat_status = NavSatStatus.STATUS_SBAS_FIX
else: # Undefined modes
navsat_status = NavSatStatus.STATUS_NO_FIX
# Again, (almost) all copied from
# http://github.com/ethz-asl/ethz_piksi_ros/tree/master/piksi_multi_rtk_ros
navsatfix = NavSatFix()
navsatfix.header.stamp = rospy.Time.now()
navsatfix.header.frame_id = 'gps'
navsatfix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
navsatfix.status.service = NavSatStatus.SERVICE_GPS
navsatfix.latitude = msg.lat
navsatfix.longitude = msg.lon
navsatfix.altitude = msg.height
navsatfix.status.status = navsat_status
navsatfix.position_covariance = [
h_var,
0,
0, # This I came up with
0,
h_var,
0, # myself. ethz_piksi_ros
0,
0,
v_var
] # uses a constant cov-matrix
# Publish to ROS topic
pub.publish(navsatfix)
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 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:
# print 'no fix'
# return
# Changing from NED from the Applanix to ENU in ROS
# Roll is still roll, since it's WRT to the x axis of the vehicle
# Pitch is -ve since axis goes the other way (+y to right vs left)
# Yaw (or heading) in Applanix is clockwise starting with North
# In ROS it's counterclockwise startin with East
time_stat = TimeSync()
time_stat.ros_time = rospy.Time.now()
time_stat.gps_time = data.td
self.pub_time.publish(time_stat)
t1 = time_stat.ros_time.secs + time_stat.ros_time.nsecs / 1E9
t2 = time_stat.gps_time.time1
# print '{0:6f}'.format(t1 - t2)
orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(-data.pitch),
RAD(90 - data.heading)).GetQuaternion()
# UTM conversion
utm_pos = geodesy.utm.fromLatLong(data.latitude, data.longitude)
# Initialize starting point if we haven't yet
if not self.init and self.zero_start:
self.origin.x = utm_pos.easting
self.origin.y = utm_pos.northing
self.origin.z = data.altitude
self.init = True
origin_param = {
"east": self.origin.x,
"north": self.origin.y,
"alt": self.origin.z,
}
rospy.set_param('/gps_origin', origin_param)
# Publish origin reference for others to know about
p = Pose()
p.position.x = self.origin.x
p.position.y = self.origin.y
p.position.z = self.origin.z
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 = utm_pos.easting - self.origin.x
odom.pose.pose.position.y = utm_pos.northing - self.origin.y
odom.pose.pose.position.z = data.altitude - self.origin.z
odom.pose.pose.orientation = Quaternion(*orient)
odom.pose.covariance = POSE_COVAR
# Twist is relative to /reference frame or /vehicle frame and
# NED to ENU conversion is needed here too
odom.twist.twist.linear.x = data.east_vel
odom.twist.twist.linear.y = data.north_vel
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)
t_tf = odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z
q_tf = Quaternion(*orient).x, Quaternion(*orient).y, Quaternion(
*orient).z, Quaternion(*orient).w
#
# Odometry transform (if required)
#
if self.publish_tf:
self.tf_broadcast.sendTransform(t_tf, q_tf, odom.header.stamp,
odom.child_frame_id,
odom.header.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.z = 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
