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 read_rtk_gps():
s = socket.socket() # Create a socket object
# these are parameters loaded onto the ros parameter servers
host = rospy.get_param('~host', '172.20.10.5')
# these are parameters loaded onto the ros parameter servers
port = rospy.get_param('~port', 3000)
s.connect((host, port))
rospy.init_node('rtk_gps_node')
pub_navsatfix = rospy.Publisher("/rtk_fix", NavSatFix, queue_size=0)
navsatfix = NavSatFix()
while True:
GPS_msgs = s.recv(1024)
msgs = re.split('\r\n', GPS_msgs)
for msg in msgs:
if msg[:6] == "$GPGGA":
print msg
data = msg.split(',')
navsatfix.status = NavSatStatus(status=int(data[6]))
navsatfix.latitude = float(data[2][:2])
navsatfix.longitude = float(data[4][:3])
navsatfix.altitude = float(data[9])
navsatfix.header.stamp = rospy.Time.now()
pub_navsatfix.publish(navsatfix)
def run(self):
while True:
#Reading acceleration data
report = self.session.next()
if self.debug: print(report)
if report["class"] == "TPV":
if "time" in report:
gpsMSG = NavSatFix()
gpsMSG.latitude = report["lat"]
gpsMSG.longitude = report["lon"]
try:
gpsMSG.status = report["status"]
except:
pass
try:
gpsMSG.altitude = report["alt"]
except:
pass
current_time = modf(time.time())
# gpsMSG.header.sec = int(current_time[1])
# gpsMSG.header.nanosec = int(current_time[0] * 1000000000) & 0xffffffff
self.gpsPublisher.publish(gpsMSG)
else:
if self.debug: print("GPS has not locked onto sattelites")
def init_path():
# logitude, latitute, altitute, orientation (+deg)
gpsPub = rospy.Publisher("/fix_path_init", NavSatFix, queue_size=10)
wpSub = rospy.Subscriber("/gps_path_init", Odometry, utm_cb)
navStatus = NavSatStatus()
navStatus.service = 1
navStatus.status = 1
# waypoints hardcoded for now, may want to spec with argument in future
num_wp = gps_waypoints.shape[0]
hdop = 1.5
position_covariance_type = 1
global wp_num
for i in range(0, num_wp):
# send gps waypoint for conversion
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'base_link'
msgOut = NavSatFix()
msgOut.position_covariance[0] = hdop**2
msgOut.position_covariance[4] = hdop**2
msgOut.position_covariance[8] = (2 * hdop)**2
msgOut.longitude = gps_waypoints[i][0]
msgOut.latitude = gps_waypoints[i][1]
msgOut.altitude = gps_waypoints[i][2]
msgOut.position_covariance_type = position_covariance_type
msgOut.status = navStatus
msgOut.header = header
wp_num = i
gpsPub.publish(msgOut)
def 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 talker():
pub = rospy.Publisher('/RTK', Point, queue_size=10)
rospy.init_node('RUN', anonymous=True)
rate = rospy.Rate(100) # 10hz
count = 0
#print("ok")
global sequence_number
global z
while not rospy.is_shutdown():
while True:
line = str(str(s.readline())[0:])
# print("angle is ok")
if line.startswith('$GNGGA'):
#print(line)
a = line.split(',')
RTK = (a[6])
#print(RTK)
record_item = []
sequence_number += 1
msg = pynmea2.parse(line)
lat = msg.latitude
lgi = msg.longitude
navsat = NavSatFix()
navsat.status = 0
navsat.header.seq = 1
navsat.header.stamp = rospy.Time.now()
navsat.position_covariance_type = 2
# navsat.STATUS = 1
navsat.header.frame_id = 'base_link'
navsat.latitude = lat
navsat.longitude = lgi
navsat.altitude = txt()
print(navsat)
utm = Proj(proj='utm', zone=48, ellps='WGS84')
x, y = utm(lgi, lat)
utm_point = Point()
#msg = geom_msg.Pose()
utm_point.x = x
utm_point.y = y
#utm_point.angular.z = txt()
# br = tf.TransformBroadcaster()
# br.sendTransform((x, y, 0),
# tf.transformations.quaternion_from_euler(0, 0, txt()),rospy.Time.now(),"base_link","odom")
#rospy.loginfo(utm_point)
pub.publish(utm_point)
def responseCallback(self, msg):
#############################################################################
gps_msg = NavSatFix()
gps_msg.header = msg.header
gps_msg.status = msg.status
gps_msg.latitude = msg.latitude
gps_msg.longitude = msg.longitude
gps_msg.altitude = msg.altitude
gps_msg.position_covariance_type = msg.position_covariance_type
gps_msg.position_covariance[0] = 1.8
gps_msg.position_covariance[4] = 1.8
gps_msg.position_covariance[8] = 5
self.gpsPub.publish(gps_msg)
def gpsCallback(data):
gps = NavSatFix()
gps.header.seq = data.header.seq
gps.header.stamp = data.header.stamp
if (gps.header.stamp.secs % 10 != 4):
gps.header.frame_id = "gps3_Link"
gps.status = data.status
gps.latitude = data.latitude
gps.longitude = data.longitude
gps.altitude = data.altitude
gps.position_covariance = data.position_covariance
gps.position_covariance_type = data.position_covariance_type
pub = rospy.Publisher('/gps_filtered', NavSatFix, queue_size=10)
rate = rospy.Rate(5) # Hz
pub.publish(gps)
rate.sleep()
else:
gps.header.frame_id = "false_Link"
gps.status = data.status
gps.latitude = 0
gps.longitude = 0
gps.altitude = 0
gps.position_covariance = data.position_covariance
gps.position_covariance_type = data.position_covariance_type
def publish_gps(event):
global origin_lat, origin_lon, first
#It may take a second or two to get good data
#print gpsd.fix.latitude,', ',gpsd.fix.longitude,' Time: ',gpsd.utc
# os.system('clear')
# print
# print ' GPS reading'
# print '----------------------------------------'
# print 'latitude ' , gpsd.fix.latitude
# print 'longitude ' , gpsd.fix.longitude
# print 'time utc ' , gpsd.utc,' + ', gpsd.fix.time
# print 'altitude (m)' , gpsd.fix.altitude
# print 'epx ' , gpsd.fix.epx
# print 'epv ' , gpsd.fix.epv
# print 'ept ' , gpsd.fix.ept
# print 'speed (m/s) ' , gpsd.fix.speed
# print 'climb ' , gpsd.fix.climb
# print 'track ' , gpsd.fix.track
# print 'mode ' , gpsd.fix.mode
# print
# print 'sats ' , gpsd.satellites
navsat = NavSatFix()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "base_link"
navsat.header = header
navsat_status = NavSatStatus()
navsat_status.status = 0
navsat_status.service = 1
navsat.status = navsat_status
navsat.latitude = gpsd.fix.latitude
navsat.longitude = gpsd.fix.longitude
navsat.altitude = gpsd.fix.altitude
navsat.position_covariance_type = 2
navsat.position_covariance = [2.5, 0, 0,
0, 2.5, 0,
0, 0, 2.5]
if not origin_lat and not origin_lon:
origin_lat = gpsd.fix.latitude
origin_lon = gpsd.fix.longitude
# print ('Odometry: ')
# print (x, y)
pub_navsat.publish(navsat)
def timepulse_callback(self, channel):
self.get_logger().info(f"{time.time()} Timepulse trigger")
gps_msg = NavSatFix()
timeref_msg = TimeReference()
msg_hdr = Header()
system_time = self.get_clock().now().to_msg()
msg_hdr.frame_id = 'base_link' # center of the plane
try:
ubx = self.ubp.read()
except IOError:
self.get_logger().warning("GPS disconnected. Attempting to reconnect.")
self.ubp = GPSReader(self.port, self.baud,
self.TIMEOUT, self.UBXONLY)
return
while ubx:
if (ubx.msg_cls + ubx.msg_id) == b"\x01\x07": # NAV_PVT
# <UBX(NAV-PVT, iTOW=16:50:32, year=2015, month=10, day=25, hour=16, min=50, second=48, valid=b'\xf0', tAcc=4294967295, nano=0, fixType=0, flags=b'\x00', flags2=b'$', numSV=0, lon=0, lat=0, height=0, hMSL=-17000, hAcc=4294967295, vAcc=4294967295, velN=0, velE=0, velD=0, gSpeed=0, headMot=0, sAcc=20000, headAcc=18000000, pDOP=9999, reserved1=65034815406080, headVeh=0, magDec=0, magAcc=0)>
msg_hdr.stamp = self._gen_timestamp_from_utc(ubx)
fix_stat = NavSatStatus()
if ubx.fixType == 0:
self.get_logger().warning(f"No fix yet.")
break
fix_stat.service = SERVICE_GPS
gps_msg.status = fix_stat
gps_msg.header = msg_hdr
gps_msg.latitude = float(ubx.lat)/10000000
gps_msg.longitude = float(ubx.lon)/10000000
gps_msg.altitude = float(ubx.height)/1000
timeref_msg.header = msg_hdr
timeref_msg.time_ref = system_time
timeref_msg.source = "GPS"
self.fix_pub.publish(gps_msg)
self.time_pub.publish(timeref_msg)
self.get_logger().info(f"Publishing gps message: ({timeref_msg.header.stamp.sec}.{timeref_msg.header.stamp.nanosec}): ({gps_msg.latitude}, {gps_msg.longitude}, {gps_msg.altitude})")
return
else:
self.get_logger().info(f"Other GPS MSG: {(ubx.msg_cls + ubx.msg_id)}")
ubx = self.ubp.read()
def cmdCB2(self, data):
# Serial read & publish
try:
msg = NavSatFix()
msg.header.seq = data.header.seq
msg.header.stamp = data.header.stamp
msg.header.frame_id = data.header.frame_id
msg.status = data.status
msg.latitude = data.latitude
msg.longitude = data.longitude
msg.altitude = data.altitude
for i in range(9):
msg.position_covariance[i] = data.position_covariance[i] * 10
msg.position_covariance_type = data.position_covariance_type
#rospy.logerr(msg)
self.pub2.publish(msg)
except:
pass
def responseCallback(self, msg):
#############################################################################
gps_msg = NavSatFix()
gps_msg.header = msg.header
gps_msg.status = msg.status
gps_msg.latitude = msg.latitude
gps_msg.longitude = msg.longitude
gps_msg.altitude = msg.altitude
gps_msg.position_covariance_type = msg.position_covariance_type
if msg.status.status == 0: # no rtk
gps_msg.position_covariance[0] = 80
gps_msg.position_covariance[4] = 80
gps_msg.position_covariance[8] = 150
else:
gps_msg.position_covariance[0] = 0.1
gps_msg.position_covariance[4] = 0.1
gps_msg.position_covariance[8] = 0.25
self.gpsPub.publish(gps_msg)
def update_uav_home_pos(self, event):
"""
Update UAV home position when ASV moves
"""
for uav in self._uav_home_proxies.keys():
home_wp = np.array([
self.uav_home_wp[uav].geo.latitude,
self.uav_home_wp[uav].geo.longitude,
self.uav_home_wp[uav].geo.altitude, self.heading
])
if (self._uav_home_offset[uav] == np.ones(4) * float('inf')).all():
if self.global_pose != NavSatFix():
asv_home = np.array([
self.global_pose.latitude, self.global_pose.longitude,
self.global_pose.altitude, 0.0
])
self._uav_home_offset[uav] = home_wp - asv_home
continue
# Update the launchpad position relative to last known pos
heading = (self.heading - self._uav_home_offset[uav][-1])
launchpad_lat = self.global_pose.latitude + (
-1 * self._uav_home_offset[uav][1] * np.sin(heading) +
self._uav_home_offset[uav][0] * np.cos(heading))
launchpad_long = self.global_pose.longitude + (
self._uav_home_offset[uav][1] * np.cos(heading) +
self._uav_home_offset[uav][0] * np.sin(heading))
launchpad_alt = self.global_pose.altitude + self._uav_home_offset[
uav][2]
# Publish the launchpad position and its (asv) heading
launchpad_pose = NavSatFix()
launchpad_pose.header = self.global_pose.header
launchpad_pose.status = self.global_pose.status
launchpad_pose.latitude = launchpad_lat
launchpad_pose.longitude = launchpad_long
launchpad_pose.altitude = launchpad_alt
self._uav_home_pose_pub[uav].publish(launchpad_pose)
self._uav_home_heading_pub[uav].publish(
Float64(self.heading * 180. / np.pi))
launchpad = np.array([launchpad_lat, launchpad_long])
if np.linalg.norm(home_wp[:2] - launchpad) > 3e-06:
self._uav_home_proxies[uav](False, self.heading * 180. / np.pi,
launchpad_lat, launchpad_long,
self._uav_home_offset[uav][2])
def publish(self):
header = Header()
header.stamp = rospy.Time.now()
navStatus = NavSatStatus()
navStatus.status = (self.fixStatus - 1)
navStatus.service = (0b1111)
gpsMsg = NavSatFix()
gpsMsg.header = header
gpsMsg.status = navStatus
gpsMsg.latitude = self.lat
gpsMsg.longitude = self.long
gpsMsg.altitude = self.alt
gpsMsg.position_covariance = self.covariance
gpsMsg.position_covariance_type = self.covariance_type
self.navSatPub.publish(gpsMsg)
self.headingPub.publish(self.heading)
def talker():
ser = serial.Serial("/dev/ttyTHS0", 9600, timeout=1)
sio = io.TextIOWrapper(io.BufferedReader(ser))
pub = rospy.Publisher("gps_data", NavSatFix, queue_size=1)
rospy.init_node("gps")
msg = NavSatFix()
msg.status = NavSatStatus(0, 0x1 | 0x2) # Unaugmented fix, GPS & GLONASS
msg.header.frame_id = "gps_link"
rospy.loginfo("GPS node ready!")
while not rospy.is_shutdown():
try:
line = sio.readline()
if not line:
continue
gpsmsg = pynmea2.parse(line)
if type(gpsmsg) == pynmea2.GGA:
qual = gpsmsg.gps_qual
if qual > 0: # Valid gps fix?
msg.status.status = (qual - 1 if qual <= 2 else 2
) # See sensor_msgs/NavSatStatus
msg.latitude = gpsmsg.latitude
msg.longitude = gpsmsg.longitude
msg.altitude = gpsmsg.altitude
except serial.SerialException as e:
rospy.logerr("Device error: {}".format(e))
break
except pynmea2.ParseError as e:
rospy.logerr("Parse error: {}".format(e))
continue
except UnicodeDecodeError:
continue
msg.header.stamp = rospy.Time.now()
pub.publish(msg)
rospy.sleep(0.01)
def write_rtk_gps(timestamp_ns, T_UTM_S, utm_zone, lat_deg, lon_deg, alt_m,
status, bag):
ros_timestamp = nanoseconds_to_ros_timestamp(timestamp_ns)
ros_status = NavSatStatus()
if status == 'INS_SOLUTION_GOOD':
ros_status.status = NavSatStatus.STATUS_FIX
else:
ros_status.status = NavSatStatus.STATUS_FIX
ros_status.service = NavSatStatus.SERVICE_GPS
fix = NavSatFix()
fix.status = ros_status
fix.latitude = lat_deg
fix.longitude = lon_deg
fix.altitude = alt_m
fix.header.frame_id = "WGS"
fix.header.stamp = ros_timestamp
rospose = Odometry()
rospose.child_frame_id = "S"
rospose.header.frame_id = "UTM"
rospose.header.stamp = ros_timestamp
rospose.pose.pose.position.x = T_UTM_S.getPosition()[0]
rospose.pose.pose.position.y = T_UTM_S.getPosition()[1]
rospose.pose.pose.position.z = T_UTM_S.getPosition()[2]
rospose.pose.pose.orientation.x = T_UTM_S.getRotation().x()
rospose.pose.pose.orientation.y = T_UTM_S.getRotation().y()
rospose.pose.pose.orientation.z = T_UTM_S.getRotation().z()
rospose.pose.pose.orientation.w = T_UTM_S.getRotation().w()
rospose.twist.twist.linear.x = 0.0
rospose.twist.twist.linear.y = 0.0
rospose.twist.twist.linear.z = 0.0
rospose.twist.twist.angular.x = 0.0
rospose.twist.twist.angular.y = 0.0
rospose.twist.twist.angular.z = 0.0
bag.write('/gps_rtk', rospose, t=ros_timestamp)
bag.write('/gps_rtk_fix', fix, t=ros_timestamp)
def write_gps(gps, i, bag):
utime = gps[i, 0]
mode = gps[i, 1]
lat = gps[i, 3]
lng = gps[i, 4]
alt = gps[i, 5]
timestamp = microseconds_to_ros_timestamp(utime)
status = NavSatStatus()
if mode == 0 or mode == 1:
status.status = NavSatStatus.STATUS_NO_FIX
else:
status.status = NavSatStatus.STATUS_FIX
status.service = NavSatStatus.SERVICE_GPS
num_sats = UInt16()
num_sats.data = gps[i, 2]
fix = NavSatFix()
fix.status = status
fix.latitude = np.rad2deg(lat)
fix.longitude = np.rad2deg(lng)
fix.altitude = alt
track = Float64()
track.data = gps[i, 6]
speed = Float64()
speed.data = gps[i, 7]
bag.write('/gps_fix', fix, t=timestamp)
bag.write('/gps_track', track, t=timestamp)
bag.write('/gps_speed', speed, t=timestamp)
def talker():
# Subscribe to Vicon messages
viconTopic = rospy.get_param('topic')
rospy.Subscriber(viconTopic, TransformStamped, callback)
# Start a publisher for the GPS messages
pub = rospy.Publisher('GPS/position', NavSatFix) # FIXME
# Start the node
rospy.init_node('talker')
# Populate the NavSatFix message from the parameter server
statusMsg = NavSatStatus()
statusMsg.status = rospy.get_param('status', -1)
statusMsg.service = rospy.get_param('service', 1)
fixMsg = NavSatFix()
fixMsg.header.stamp = rospy.Time.now()
fixMsg.header.frame_id = "/world"
fixMsg.status = statusMsg
fixMsg.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
#position could be modified with some gaussian noise added to it and then calculate the covariance matrix.
fixMsg.position_covariance_type = rospy.get_param('position_covariance_type', 0);
while not rospy.is_shutdown():
[fixMsg.longitude, fixMsg.latitude, fixMsg.altitude] = c.xyz2gps([parentFrameLong, parentFrameLat, parentFrameAlt], latestViconMsg.transform.translation.x, latestViconMsg.transform.translation.y, latestViconMsg.transform.translation.z, parentFrameAngle)
statusMsg.status = rospy.get_param('status', statusMsg.status)
statusMsg.service = rospy.get_param('service', statusMsg.service)
# put the sigma and calculate the cov matrix here
#rospy.loginfo([fixMsg.longitude, fixMsg.latitude, fixMsg.altitude])
pub.publish(fixMsg)
rospy.sleep(0.1)
def write_gps(gps, i, bag):
utime = gps[i, 0]
mode = gps[i, 1]
lat = gps[i, 3]
lng = gps[i, 4]
alt = gps[i, 5]
timestamp = rospy.Time.from_sec(utime / 1e6)
status = NavSatStatus()
if mode == 0 or mode == 1:
status.status = NavSatStatus.STATUS_NO_FIX
else:
status.status = NavSatStatus.STATUS_FIX
status.service = NavSatStatus.SERVICE_GPS
num_sats = UInt16()
num_sats.data = gps[i, 2]
fix = NavSatFix()
fix.status = status
fix.latitude = np.rad2deg(lat)
fix.longitude = np.rad2deg(lng)
fix.altitude = alt
track = Float64()
track.data = gps[i, 6]
speed = Float64()
speed.data = gps[i, 7]
bag.write('gps_fix', fix, t=timestamp)
bag.write('gps_track', track, t=timestamp)
bag.write('gps_speed', speed, t=timestamp)
# print("package image...")
# img_path = sys.argv[1] + 'images/2012-01-08/lb3/Cam0/'
# img_list = os.listdir(img_path)
# i_img = 0
# for img_name in img_list:
# i_img = i_img + 1
# print(i_img)
# img_cv = cv2.imread(os.path.join(img_path, img_name), -1)
# br = CvBridge()
# img = Image()
# img = br.cv2_to_imgmsg(img_cv, "bgr8")
# img.header.seq = i_img
# img.header.frame_id = 'camImage'
# bag.write('/camera/image', img)
img_path = sys.argv[1] + 'images/2012-01-08/lb3/Cam0/'
img_list = os.listdir(img_path)
img_cv = cv2.imread(os.path.join(img_path, img_list[i]), -1)
img_cv = cv2.resize(img_cv, IMAGEDIM, interpolation=cv2.INTER_AREA)
br = CvBridge()
img = Image()
img = br.cv2_to_imgmsg(img_cv, "bgr8")
def gps_to_navsat(self, gps_list, i):
""" converts gps data to ROS NavSatFix messages
:param gps_list: list, containing gps data
:param i: row counter
:return: fill bag with navsat, track, speed, timestamp
"""
# load data from list
utime = gps_list[i, 0]
mode = gps_list[i, 1]
num_satss = gps_list[i, 2]
lat = gps_list[i, 3]
lng = gps_list[i, 4]
alt = gps_list[i, 5]
track_raw = gps_list[i, 6]
speed_raw = gps_list[i, 7]
# create ros timestamp
timestamp = rospy.Time.from_sec(utime / 1e6)
# get gps and base link
gps_link = self.gps_frame
base_link = self.body_frame
# fill NavSat message
status = NavSatStatus()
if mode == 0 or mode == 1:
status.status = NavSatStatus.STATUS_NO_FIX
else:
status.status = NavSatStatus.STATUS_FIX
status.service = NavSatStatus.SERVICE_GPS
num_sats = UInt16()
num_sats.data = num_satss
navsat = NavSatFix()
navsat.header.stamp = timestamp
navsat.header.frame_id = gps_link
navsat.status = status
navsat.latitude = np.rad2deg(lat)
navsat.longitude = np.rad2deg(lng)
navsat.altitude = alt
navsat.position_covariance = NAVSAT_COVAR
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
track = Float64()
track.data = track_raw
speed = Float64()
speed.data = speed_raw
# create base_link gps_link static transformer
gps_static_transform_stamped = geometry_msgs.msg.TransformStamped()
gps_static_transform_stamped.header.stamp = timestamp
gps_static_transform_stamped.header.frame_id = base_link
gps_static_transform_stamped.child_frame_id = gps_link
gps_static_transform_stamped.transform.translation.x = 0
gps_static_transform_stamped.transform.translation.y = 0.25
gps_static_transform_stamped.transform.translation.z = 0.51
quat = tf.transformations.quaternion_from_euler(0, 0, 0)
gps_static_transform_stamped.transform.rotation.x = quat[0]
gps_static_transform_stamped.transform.rotation.y = quat[1]
gps_static_transform_stamped.transform.rotation.z = quat[2]
gps_static_transform_stamped.transform.rotation.w = quat[3]
# publish static transform
tf_static_msg = tf2_msgs.msg.TFMessage([gps_static_transform_stamped])
return navsat, track, speed, timestamp, tf_static_msg
def navigation_handler(self, data):
""" Rebroadcasts navigation data in the following formats:
1) /odom => /base footprint transform (if enabled, as per REP 105)
2) Odometry message, with parent/child IDs as in #1
3) NavSatFix message, for systems which are knowledgeable about GPS stuff
4) IMU messages
"""
rospy.logdebug("Navigation received")
# If we don't have a fix, don't publish anything...
if self.nav_status.status == NavSatStatus.STATUS_NO_FIX:
return
# UTM conversion
easting, northing = data.bestxyz.easting, data.bestxyz.northing
# Initialize starting point if we haven't yet
# TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid?
# TODO: check INSPVAX sol stat for valid position before accepting
if not self.init and self.zero_start:
self.origin.x = easting
self.origin.y = northing
self.init = True
# Publish origin reference for others to know about
p = Pose()
p.position.x = self.origin.x
p.position.y = self.origin.y
self.pub_origin.publish(p)
# IMU
# TODO: Work out these covariances properly. Logs provide covariances in local frame, not body
imu = Imu()
imu.header.stamp == rospy.Time.now()
imu.header.frame_id = self.base_frame
# Orientation
# orient=PyKDL.Rotation.RPY(RAD(data.roll),RAD(data.pitch),RAD(data.heading)).GetQuaternion()
# imu.orientation = Quaternion(*orient)
imu.orientation.x = data.inspvax.pitch
imu.orientation.y = data.inspvax.roll
imu.orientation.z = data.inspvax.azimuth
imu.orientation.w = 0
IMU_ORIENT_COVAR[0] = POW(2, data.inspvax.pitch_std)
IMU_ORIENT_COVAR[4] = POW(2, data.inspvax.roll_std)
IMU_ORIENT_COVAR[8] = POW(2, data.inspvax.azimuth_std)
imu.orientation_covariance = IMU_ORIENT_COVAR
# Angular rates (rad/s)
# corrimudata log provides instantaneous rates so multiply by IMU rate in Hz
imu.angular_velocity.x = data.corrimudata.pitch_rate * self.imu_rate
imu.angular_velocity.y = data.corrimudata.roll_rate * self.imu_rate
imu.angular_velocity.z = data.corrimudata.yaw_rate * self.imu_rate
imu.angular_velocity_covariance = IMU_VEL_COVAR
# Linear acceleration (m/s^2)
imu.linear_acceleration.x = data.corrimudata.x_accel * self.imu_rate
imu.linear_acceleration.y = data.corrimudata.y_accel * self.imu_rate
imu.linear_acceleration.z = data.corrimudata.z_accel * self.imu_rate
imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
self.pub_imu.publish(imu)
# Odometry
# TODO: Work out these covariances properly
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = self.odom_frame
odom.child_frame_id = self.base_frame
odom.pose.pose.position.x = easting - self.origin.x
odom.pose.pose.position.y = northing - self.origin.y
odom.pose.pose.position.z = data.inspvax.altitude
#odom.pose.pose.orientation = Quaternion(*orient)
odom.pose.pose.orientation = imu.orientation
POSE_COVAR[21] = IMU_ORIENT_COVAR[0]
POSE_COVAR[28] = IMU_ORIENT_COVAR[4]
POSE_COVAR[35] = IMU_ORIENT_COVAR[8]
odom.pose.covariance = POSE_COVAR
# Twist is relative to vehicle frame
odom.twist.twist.linear.x = data.bestxyz.velx
odom.twist.twist.linear.y = data.bestxyz.vely
odom.twist.twist.linear.z = data.bestxyz.velz
TWIST_COVAR[0] = pow(2,data.bestxyz.velx_std)
TWIST_COVAR[7] = pow(2,data.bestxyz.vely_std)
TWIST_COVAR[14] = pow(2,data.bestxyz.velz_std)
odom.twist.twist.angular = imu.angular_velocity
odom.twist.covariance = TWIST_COVAR
self.pub_odom.publish(odom)
#
# Odometry transform (if required)
#
if self.publish_tf:
self.tf_broadcast.sendTransform(
(odom.pose.pose.position.x, odom.pose.pose.position.y,
odom.pose.pose.position.z), odom.pose.pose.orientation, #Quaternion(*orient),
odom.header.stamp,odom.child_frame_id, odom.frame_id)
#
# NavSatFix
# TODO: Work out these covariances properly from DOP
#
navsat = NavSatFix()
navsat.header.stamp = rospy.Time.now()
navsat.header.frame_id = self.odom_frame
navsat.status = self.nav_status
# position, in degrees
navsat.latitude = data.bestpos.latitude
navsat.longitude = data.bestpos.longitude
navsat.altitude = data.bestpos.altitude
NAVSAT_COVAR[0] = pow(2, data.bestpos.lat_std) # in meters
NAVSAT_COVAR[4] = pow(2, data.bestpos.lon_std)
NAVSAT_COVAR[8] = pow(2, data.bestpos.hgt_std)
navsat.position_covariance = NAVSAT_COVAR
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
self.pub_navsatfix.publish(navsat)
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(
None,
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 main(argv):
if len(sys.argv) < 2:
print 'Please specify data directory file'
return 1
if len(sys.argv) < 3:
print 'Please specify output rosbag file'
return 1
print("loading files...")
bag = rosbag.Bag(sys.argv[2], 'w')
gps = np.loadtxt(sys.argv[1] + "sensor_data/2012-01-08/gps.csv",
delimiter=",")
imu_100hz = np.loadtxt(sys.argv[1] +
"sensor_data/2012-01-08/imu_100hz.csv",
delimiter=",")
ral_seq = 0
bap_seq = 0
img_seq = 0
imu_seq = 0
cal = -1
gps_seq = 0
# IMAGE_COUNT = 81169
STREET_VIEW = 113
print("package gps and image...")
print("Packaging GPS and image")
for gps_data in gps:
utime = int(gps_data[0])
mode = int(gps_data[1])
timestamp = rospy.Time.from_sec(utime / 1e6)
lat = float(gps_data[3])
lng = float(gps_data[4])
alt = float(gps_data[5])
status = NavSatStatus()
if mode == 0 or mode == 1:
status.status = NavSatStatus.STATUS_NO_FIX
else:
status.status = NavSatStatus.STATUS_FIX
status.service = NavSatStatus.SERVICE_GPS
num_sats = UInt16()
num_sats.data = float(gps_data[2])
fix = NavSatFix()
fix.header.seq = gps_seq
fix.status = status
fix.latitude = np.rad2deg(lat)
fix.longitude = np.rad2deg(lng)
fix.altitude = np.rad2deg(alt)
track = Float64()
track.data = float(gps_data[6])
speed = Float64()
speed.data = float(gps_data[7])
bag.write('/gps', fix, t=timestamp)
bag.write('/gps_track', track, t=timestamp)
bag.write('/gps_speed', speed, t=timestamp)
# write aerial image
if gps_seq <= MAVIMAGE:
img_path = sys.argv[1] + 'images/2012-01-08/lb3/Cam5/'
img_list = os.listdir(img_path)
img_list.sort()
img_cv = cv2.imread(os.path.join(img_path, img_list[gps_seq]), -1)
img_cv = cv2.resize(img_cv, MAVDIM, interpolation=cv2.INTER_AREA)
# 顺时针旋转90度
trans_img = cv2.transpose(img_cv)
img_cv = cv2.flip(trans_img, 1)
br = CvBridge()
Img = Image()
Img = br.cv2_to_imgmsg(img_cv, "bgr8")
Img.header.seq = int(gps_seq)
print(gps_seq)
Img.header.stamp = timestamp
Img.header.frame_id = 'camera'
bag.write('/image/cam5', Img, t=timestamp)
gps_seq = gps_seq + 1
print('packaging imu...')
for imu_data in imu_100hz:
imu_seq = imu_seq + 1
utime = int(imu_data[0])
timestamp = rospy.Time.from_sec(utime / 1e6)
imu = Imu()
imu.header.seq = imu_seq
imu.header.stamp = timestamp
imu.header.frame_id = '/Imu'
imu.linear_acceleration.x = float(imu_data[5])
imu.linear_acceleration.y = float(imu_data[6])
imu.linear_acceleration.z = float(imu_data[7])
imu.linear_acceleration_covariance = np.zeros(9)
imu.angular_velocity.x = float(imu_data[8])
imu.angular_velocity.y = float(imu_data[9])
imu.angular_velocity.z = float(imu_data[10])
imu.angular_velocity_covariance = np.zeros(9)
imu.orientation.w = float(imu_data[1])
imu.orientation.x = float(imu_data[2])
imu.orientation.y = float(imu_data[3])
imu.orientation.z = float(imu_data[4])
bag.write('/Imu', imu, t=timestamp)
bag.close()
return 0
def 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 handle_gps(self, data, timestamp):
"""
Date gps_Date MM/DD/YYYY or DD/MM/YYYY
Time gps_Time HH:MM:SS.SSS
Lat & Lon gps_Lat,gps_Long Degrees-7
Altitude gps_Alt mm
Altitude MSL gps_AltMSL mm
SIV gps_SIV Count
Fix Type gps_FixType 0-5
Carrier Soln. gps_CarrierSolution 0-2
Ground Speed gps_GroundSpeed mm/s
Heading gps_Heading Degrees-5
PDOP gps_pDOP 10-2 (dimensionless)
Time Of Week gps_iTOW ms
Lat = Latitude
Lon = Longitude
MSL = Metres above Sea Level
SIV = Satellites In View
PDOP = Positional Dilution Of Precision
Fix Type:
0: No
1: Dead Reckoning Only
2: 2D
3: 3D
4: GNSS + Dead Reckoning
5: Time Only
Carrier Solution:
0: No
1: Float Solution
2: Fixed Solution
"""
gps_msg = NavSatFix()
gps_msg.header.frame_id = self.tf_prefix+"world"
gps_msg.header.stamp = timestamp
gps_msg.status = NavSatStatus(status=0, service=1)
gps_msg.latitude = float(data[self.data_headers["GPS"]["Lat"]])
gps_msg.longitude = float(data[self.data_headers["GPS"]["Long"]])
gps_msg.altitude = float(data[self.data_headers["GPS"]["Alt"]])
# COVARIANCE_TYPE_UNKNOWN = 0, COVARIANCE_TYPE_APPROXIMATED = 1
# COVARIANCE_TYPE_DIAGONAL_KNOWN = 2, COVARIANCE_TYPE_KNOWN = 3
gps_msg.position_covariance = [0.0] * 9
gps_msg.position_covariance_type = 0
self.gps_pub.publish(gps_msg)
# Time Reference
time_msg = TimeReference()
time_msg.header.stamp = timestamp
gps_time = datetime.strptime("{} {}".format(data[self.data_headers["GPS"]["Date"]], data[self.data_headers["GPS"]["Time"]]),
"%d/%m/%Y %H:%M:%S.%f")
total_secs = (gps_time - epoch).total_seconds()
time_msg.time_ref.secs = int(total_secs)
time_msg.time_ref.nsecs = total_secs-int(total_secs)
time_msg.source = "GPS"
self.time_ref.publish(time_msg)
return True
def navigation_handler(self, data):
""" Rebroadcasts navigation data in the following formats:
1) /odom => /base footprint transform (if enabled, as per REP 105)
2) Odometry message, with parent/child IDs as in #1
3) NavSatFix message, for systems which are knowledgeable about GPS stuff
4) IMU messages
"""
rospy.logdebug("Navigation received")
# If we don't have a fix, don't publish anything...
# if self.nav_status.status == NavSatStatus.STATUS_NO_FIX:
# print 'no fix'
# return
# Changing from NED from the Applanix to ENU in ROS
# Roll is still roll, since it's WRT to the x axis of the vehicle
# Pitch is -ve since axis goes the other way (+y to right vs left)
# Yaw (or heading) in Applanix is clockwise starting with North
# In ROS it's counterclockwise startin with East
time_stat = TimeSync()
time_stat.ros_time = rospy.Time.now()
time_stat.gps_time = data.td
self.pub_time.publish(time_stat)
t1 = time_stat.ros_time.secs + time_stat.ros_time.nsecs / 1E9
t2 = time_stat.gps_time.time1
# print '{0:6f}'.format(t1 - t2)
orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(-data.pitch),
RAD(90 - data.heading)).GetQuaternion()
# UTM conversion
utm_pos = geodesy.utm.fromLatLong(data.latitude, data.longitude)
# Initialize starting point if we haven't yet
if not self.init and self.zero_start:
self.origin.x = utm_pos.easting
self.origin.y = utm_pos.northing
self.origin.z = data.altitude
self.init = True
origin_param = {
"east": self.origin.x,
"north": self.origin.y,
"alt": self.origin.z,
}
rospy.set_param('/gps_origin', origin_param)
# Publish origin reference for others to know about
p = Pose()
p.position.x = self.origin.x
p.position.y = self.origin.y
p.position.z = self.origin.z
self.pub_origin.publish(p)
#
# Odometry
# TODO: Work out these covariances properly from DOP
#
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = self.odom_frame
odom.child_frame_id = self.base_frame
odom.pose.pose.position.x = utm_pos.easting - self.origin.x
odom.pose.pose.position.y = utm_pos.northing - self.origin.y
odom.pose.pose.position.z = data.altitude - self.origin.z
odom.pose.pose.orientation = Quaternion(*orient)
odom.pose.covariance = POSE_COVAR
# Twist is relative to /reference frame or /vehicle frame and
# NED to ENU conversion is needed here too
odom.twist.twist.linear.x = data.east_vel
odom.twist.twist.linear.y = data.north_vel
odom.twist.twist.linear.z = -data.down_vel
odom.twist.twist.angular.x = RAD(data.ang_rate_long)
odom.twist.twist.angular.y = RAD(-data.ang_rate_trans)
odom.twist.twist.angular.z = RAD(-data.ang_rate_down)
odom.twist.covariance = TWIST_COVAR
self.pub_odom.publish(odom)
t_tf = odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z
q_tf = Quaternion(*orient).x, Quaternion(*orient).y, Quaternion(
*orient).z, Quaternion(*orient).w
#
# Odometry transform (if required)
#
if self.publish_tf:
self.tf_broadcast.sendTransform(t_tf, q_tf, odom.header.stamp,
odom.child_frame_id,
odom.header.frame_id)
#
# NavSatFix
# TODO: Work out these covariances properly from DOP
#
navsat = NavSatFix()
navsat.header.stamp = rospy.Time.now()
navsat.header.frame_id = self.odom_frame
navsat.status = self.nav_status
navsat.latitude = data.latitude
navsat.longitude = data.longitude
navsat.altitude = data.altitude
navsat.position_covariance = NAVSAT_COVAR
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.pub_navsatfix.publish(navsat)
#
# IMU
# TODO: Work out these covariances properly
#
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = self.base_frame
# Orientation
imu.orientation = Quaternion(*orient)
imu.orientation_covariance = IMU_ORIENT_COVAR
# Angular rates
imu.angular_velocity.x = RAD(data.ang_rate_long)
imu.angular_velocity.y = RAD(-data.ang_rate_trans)
imu.angular_velocity.z = RAD(-data.ang_rate_down)
imu.angular_velocity_covariance = IMU_VEL_COVAR
# Linear acceleration
imu.linear_acceleration.x = data.long_accel
imu.linear_acceleration.y = data.trans_accel
imu.linear_acceleration.z = data.down_accel
imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
self.pub_imu.publish(imu)
pass
def navigation_handler(self, data):
""" Rebroadcasts navigation data in the following formats:
1) /odom => /base footprint transform (if enabled, as per REP 105)
2) Odometry message, with parent/child IDs as in #1
3) NavSatFix message, for systems which are knowledgeable about GPS stuff
4) IMU messages
"""
rospy.logdebug("Navigation received")
# If we don't have a fix, don't publish anything...
if self.nav_status.status == NavSatStatus.STATUS_NO_FIX:
return
orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(data.pitch), RAD(data.heading)).GetQuaternion()
# UTM conversion
(zone, easting, northing) = LLtoUTM(23, data.latitude, data.longitude)
# Initialize starting point if we haven't yet
# TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid?
if not self.init and self.zero_start:
self.origin.x = easting
self.origin.y = northing
self.init = True
# Publish origin reference for others to know about
p = Pose()
p.position.x = self.origin.x
p.position.y = self.origin.y
self.pub_origin.publish(p)
#
# Odometry
# TODO: Work out these covariances properly from DOP
#
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = self.odom_frame
odom.child_frame_id = self.base_frame
odom.pose.pose.position.x = easting - self.origin.x
odom.pose.pose.position.y = northing - self.origin.y
odom.pose.pose.position.z = data.altitude
odom.pose.pose.orientation = Quaternion(*orient)
odom.pose.covariance = POSE_COVAR
# Twist is relative to /vehicle frame
odom.twist.twist.linear.x = data.speed
odom.twist.twist.linear.y = 0
odom.twist.twist.linear.z = -data.down_vel
odom.twist.twist.angular.x = RAD(data.ang_rate_long)
odom.twist.twist.angular.y = RAD(-data.ang_rate_trans)
odom.twist.twist.angular.z = RAD(-data.ang_rate_down)
odom.twist.covariance = TWIST_COVAR
self.pub_odom.publish(odom)
#
# Odometry transform (if required)
#
if self.publish_tf:
self.tf_broadcast.sendTransform(
(odom.pose.pose.position.x, odom.pose.pose.position.y,
odom.pose.pose.position.z), Quaternion(*orient),
odom.header.stamp,odom.child_frame_id, odom.frame_id)
#
# NavSatFix
# TODO: Work out these covariances properly from DOP
#
navsat = NavSatFix()
navsat.header.stamp = rospy.Time.now()
navsat.header.frame_id = self.odom_frame
navsat.status = self.nav_status
navsat.latitude = data.latitude
navsat.longitude = data.longitude
navsat.altitude = data.altitude
navsat.position_covariance = NAVSAT_COVAR
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.pub_navsatfix.publish(navsat)
#
# IMU
# TODO: Work out these covariances properly
#
imu = Imu()
imu.header.stamp == rospy.Time.now()
imu.header.frame_id = self.base_frame
# Orientation
imu.orientation = Quaternion(*orient)
imu.orientation_covariance = IMU_ORIENT_COVAR
# Angular rates
imu.angular_velocity.x = RAD(data.ang_rate_long)
imu.angular_velocity.y = RAD(-data.ang_rate_trans)
imu.angular_velocity.y = RAD(-data.ang_rate_down)
imu.angular_velocity_covariance = IMU_VEL_COVAR
# Linear acceleration
imu.linear_acceleration.x = data.long_accel
imu.linear_acceleration.y = data.trans_accel
imu.linear_acceleration.z = data.down_accel
imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
self.pub_imu.publish(imu)
pass
