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