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 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 PointCloudCall(self,data): # 20 Hz As this function has highest frequency, good for update
pointCloud_ = PointCloud2()
pointCloud_ = data
self.graphslam_PointCloud_pub.publish(pointCloud_) # for Graph slam
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_, 'statManu', self.excluSatLis) # statManu
self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal()
if((len(self.GNSSNRAWlosDel.GNSS_Raws) > 1) and (self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time != self.GNSSNRAWlosDelTimeFlag)): # only if there is a change, there will conduct the calculation
# print 'self.GNSSNRAWlosDel',len(self.GNSSNRAWlosDel.GNSS_Raws)
self.puGNSSPosCalF_prop.iterPosCal(self.GNSSNRAWlosDel, 'WLSGNSS')
self.GNSSNRAWlosDelTimeFlag = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_prop.ecef_)
navfix_.header.stamp.secs = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.propGNSS_Navfix_pub.publish(navfix_)
# for Graph slam
graphNavfix_ = NavSatFix()
graphNavfix_ = navfix_
graphNavfix_.header = pointCloud_.header
self.graphslam_Navfix_pub.publish(graphNavfix_)
# for Graph slam
geopoint = GeoPointStamped()
geopoint.header = graphNavfix_.header
geopoint.position.latitude = float(llh_[0])
geopoint.position.longitude = float(llh_[1])
geopoint.position.altitude = float(llh_[2])
self.graphslam_GeoPoint_pub.publish(geopoint)
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 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 to_msg(self):
h = Header()
h.stamp = self.stamp
h.frame_id = map_frame_id
msg = NavSatFix()
msg.header = h
msg.latitude = math.degrees(self.geopoint.latitude)
msg.longitude = math.degrees(self.geopoint.longitude)
msg.altitude = self.geopoint.z
return 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 sensor_data_updated(self, carla_gnss_event):
"""
Function to transform a received gnss event into a ROS NavSatFix message
:param carla_gnss_event: carla gnss event object
:type carla_gnss_event: carla.GnssEvent
"""
navsatfix_msg = NavSatFix()
navsatfix_msg.header = self.get_msg_header(use_parent_frame=False)
navsatfix_msg.latitude = carla_gnss_event.latitude
navsatfix_msg.longitude = carla_gnss_event.longitude
navsatfix_msg.altitude = carla_gnss_event.altitude
self.publish_ros_message(self.topic_name() + "/fix", navsatfix_msg)
def responseCallback(self, msg):
#############################################################################
gps_msg = NavSatFix()
gps_msg.header = msg.header
gps_msg.status = msg.status
gps_msg.latitude = msg.latitude
gps_msg.longitude = msg.longitude
gps_msg.altitude = msg.altitude
gps_msg.position_covariance_type = msg.position_covariance_type
gps_msg.position_covariance[0] = 1.8
gps_msg.position_covariance[4] = 1.8
gps_msg.position_covariance[8] = 5
self.gpsPub.publish(gps_msg)
def publish_gnss(self, sensor_id, data):
"""
Function to publish gnss data
"""
msg = NavSatFix()
msg.header = self.get_header()
msg.header.frame_id = 'gps'
msg.latitude = data[0]
msg.longitude = data[1]
msg.altitude = data[2]
msg.status.status = NavSatStatus.STATUS_SBAS_FIX
msg.status.service = NavSatStatus.SERVICE_GPS | NavSatStatus.SERVICE_GLONASS | NavSatStatus.SERVICE_COMPASS | NavSatStatus.SERVICE_GALILEO
self.publisher_map[sensor_id].publish(msg)
def sensor_data_updated(self, carla_gnss_measurement):
"""
Function to transform a received gnss event into a ROS NavSatFix message
:param carla_gnss_measurement: carla gnss measurement object
:type carla_gnss_measurement: carla.GnssMeasurement
"""
navsatfix_msg = NavSatFix()
navsatfix_msg.header = self.get_msg_header(timestamp=carla_gnss_measurement.timestamp)
navsatfix_msg.latitude = carla_gnss_measurement.latitude
navsatfix_msg.longitude = carla_gnss_measurement.longitude
navsatfix_msg.altitude = carla_gnss_measurement.altitude
self.gnss_publisher.publish(navsatfix_msg)
def msg_dict_to_NavSatFix(msg_dict):
msg = NavSatFix()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = msg_dict['header']['frame_id']
msg.latitude = msg_dict['latitude']
msg.longitude = msg_dict['longitude']
msg.altitude = msg_dict['altitude']
msg.status.status = msg_dict['status']['status']
msg.status.service = msg_dict['status']['service']
msg.position_covariance = msg_dict['position_covariance']
msg.position_covariance_type = msg_dict['position_covariance_type']
return msg
def PointCloudCall(
self, data
): # 20 Hz As this function has highest frequency, good for update
self.pointCloud_ = PointCloud2()
self.pointCloud_ = data
# self.graphslam_PointCloud_pub.publish(self.pointCloud_) # for Graph slam
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_,
'statManu_ExclusionOpenLoop',
self.excluSatLis) # statManu
self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal()
if ((len(self.GNSSNRAWlosDel.GNSS_Raws) > 1)
and (self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time !=
self.GNSSNRAWlosDelTimeFlag)
): # only if there is a change, there will conduct the calculation
# print 'self.GNSSNRAWlosDel',len(self.GNSSNRAWlosDel.GNSS_Raws)
self.puGNSSPosCalF_prop.iterPosCal(self.GNSSNRAWlosDel, 'WLSGNSS')
self.GNSSNRAWlosDelTimeFlag = self.GNSSNRAWlosDel.GNSS_Raws[
-1].GNSS_time
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_prop.ecef_)
navfix_.header.stamp.secs = self.GNSSNRAWlosDel.GNSS_Raws[
-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.propGNSS_Navfix_pub.publish(navfix_)
# for Graph slam
graphNavfix_ = NavSatFix()
graphNavfix_ = navfix_
graphNavfix_.header = self.pointCloud_.header
self.graphslam_Navfix_pub.publish(graphNavfix_)
# for Graph slam
geopoint = GeoPointStamped()
geopoint.header = graphNavfix_.header
# calculate the ENU coordiantes initialLLH
enu_ = pugeomath.transformEcefToEnu(self.initialLLH,
self.puGNSSPosCalF_prop.ecef_)
print 'enu_ ->: ', enu_
geopoint.position.latitude = float(enu_[0])
geopoint.position.longitude = float(enu_[1])
# geopoint.position.altitude = float(llh_[2]) # use this to save the covariance in altitude component
geopoint.position.altitude = self.GNSSNRAWlosDel.GNSS_Raws[
-1].snr * self.HDOPProp # save the covariance
self.graphslam_GeoPoint_pub.publish(geopoint)
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 subCB(msg_in):
global pub
msg_out = NavSatFix()
msg_out.header = msg_in.header
msg_out.status.status = NavSatStatus.STATUS_FIX # TODO - fix this
msg_out.status.service = NavSatStatus.SERVICE_GPS
msg_out.latitude = msg_in.LLA.x
msg_out.longitude = msg_in.LLA.y
msg_out.altitude = msg_in.LLA.z
msg_out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
msg_out.position_covariance[1] = msg_in.PosUncerainty
pub.publish(msg_out)
def subCB(msg_in):
global pub
msg_out = NavSatFix()
msg_out.header = msg_in.header
msg_out.status.status = NavSatStatus.STATUS_FIX # TODO - fix this
msg_out.status.service = NavSatStatus.SERVICE_GPS
msg_out.latitude = msg_in.LLA.x
msg_out.longitude = msg_in.LLA.y
msg_out.altitude = msg_in.LLA.z
msg_out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
msg_out.position_covariance[1] = msg_in.PosUncerainty
pub.publish(msg_out)
def 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 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 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 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_origin_point(self):
"""
Function to publish origin point
"""
# self.origin_point_publisher = rospy.Publisher(
# '/carla/ego_vehicle/origin_point', NavSatFix, queue_size=1)
lon_0, lat_0 = calibrate_lonlat0(self._global_plan,
self._global_plan_world_coord)
msg = NavSatFix()
msg.header = self.get_header()
msg.header.frame_id = 'gps'
msg.latitude = lat_0
msg.longitude = lon_0
msg.altitude = 0.0
msg.status.status = NavSatStatus.STATUS_SBAS_FIX
msg.status.service = NavSatStatus.SERVICE_GPS | NavSatStatus.SERVICE_GLONASS | NavSatStatus.SERVICE_COMPASS | NavSatStatus.SERVICE_GALILEO
self.origin_point_publisher.publish(msg)
def sub_insCB(msg_in):
global pub_imu
global pub_gps
global msg_imu
msg_imu.header.stamp = msg_in.header.stamp
msg_imu.header.frame_id = msg_in.header.frame_id
# Convert the RPY data from the Vectornav into radians!
roll = (math.pi * msg_in.RPY.x) / 180.0
pitch = (math.pi * msg_in.RPY.y) / 180.0
yaw = (math.pi * msg_in.RPY.z) / 180.0
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
msg_imu.orientation.x = q[0]
msg_imu.orientation.y = q[1]
msg_imu.orientation.z = q[2]
msg_imu.orientation.w = q[3]
pub_imu.publish(msg_imu)
msg_gps = NavSatFix()
msg_gps.header = msg_in.header
msg_gps.status.status = NavSatStatus.STATUS_FIX # TODO - fix this
msg_gps.status.service = NavSatStatus.SERVICE_GPS
msg_gps.latitude = msg_in.LLA.x
msg_gps.longitude = msg_in.LLA.y
msg_gps.altitude = msg_in.LLA.z
msg_gps.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
msg_gps.position_covariance[0] = msg_in.PosUncerainty
pub_gps.publish(msg_gps)
msg_time = TimeReference()
msg_time.header.stamp = msg_in.header.stamp
msg_time.header.frame_id = msg_in.header.frame_id
unix_time = 315964800 + (msg_in.Week * 7 * 24 * 3600) + msg_in.Time
msg_time.time_ref = rospy.Time.from_sec(unix_time)
pub_time.publish(msg_time)
def sub_insCB(msg_in):
global pub_imu
global pub_gps
global msg_imu
msg_imu.header.stamp = msg_in.header.stamp
msg_imu.header.frame_id = msg_in.header.frame_id
# Convert the RPY data from the Vectornav into radians!
roll = (math.pi * msg_in.RPY.x) / 180.0
pitch = (math.pi * msg_in.RPY.y) / 180.0
yaw = (math.pi * msg_in.RPY.z) / 180.0
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
msg_imu.orientation.x = q[0]
msg_imu.orientation.y = q[1]
msg_imu.orientation.z = q[2]
msg_imu.orientation.w = q[3]
pub_imu.publish(msg_imu)
msg_gps = NavSatFix()
msg_gps.header = msg_in.header
msg_gps.status.status = NavSatStatus.STATUS_FIX # TODO - fix this
msg_gps.status.service = NavSatStatus.SERVICE_GPS
msg_gps.latitude = msg_in.LLA.x
msg_gps.longitude = msg_in.LLA.y
msg_gps.altitude = msg_in.LLA.z
msg_gps.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
msg_gps.position_covariance[0] = msg_in.PosUncerainty
pub_gps.publish(msg_gps)
msg_time = TimeReference()
msg_time.header.stamp = msg_in.header.stamp
msg_time.header.frame_id = msg_in.header.frame_id
unix_time = 315964800 + (msg_in.Week * 7 * 24 * 3600) + msg_in.Time
msg_time.time_ref = rospy.Time.from_sec(unix_time)
pub_time.publish(msg_time)
def timer_callback(self, event):
msg = NavSatFix()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "gps"
msg.status.status = NavSatStatus.STATUS_FIX
msg.status.service = NavSatStatus.SERVICE_GPS
# Position in degrees.
msg.latitude = 59.172728
msg.longitude = 10.29502696
# Altitude in metres.
msg.altitude = 3.25
msg.position_covariance[0] = 0
msg.position_covariance[4] = 0
msg.position_covariance[8] = 0
msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
self.publisher.publish(msg)
self.best_pos_a = None
def gpsfix_to_navsatfix(gpsfix_msg):
# Convert gps_common/GPSFix messages to sensor_msgs/NavSatFix messages
navsat_msg = NavSatFix()
navsat_msg.header = gpsfix_msg.header
# Caution: GPSFix has defined some additional status constants, which are
# not defined in NavSatFix.
navsat_msg.status.status = gpsfix_msg.status.status
navsat_msg.status.service = 0
if gpsfix_msg.status.position_source & GPSStatus.SOURCE_GPS:
navsat_msg.status.service = \
navsat_msg.status.service | NavSatStatus.SERVICE_GPS
if gpsfix_msg.status.orientation_source & GPSStatus.SOURCE_MAGNETIC:
navsat_msg.status.service = \
navsat_msg.status.service | NavSatStatus.SERVICE_COMPASS
navsat_msg.latitude = gpsfix_msg.latitude
navsat_msg.longitude = gpsfix_msg.longitude
navsat_msg.altitude = gpsfix_msg.altitude
navsat_msg.position_covariance = gpsfix_msg.position_covariance
navsat_msg.position_covariance_type = gpsfix_msg.position_covariance_type
return navsat_msg
ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_VELECEF,
1)
ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_POSECEF,
1)
ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_RAW, 1)
ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_SFRB, 1)
ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_SVSI, 1)
ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_ALM, 1)
ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_EPH, 1)
ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_TIMEGPS,
5)
ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_CLOCK, 5)
while not rospy.is_shutdown():
msg = ubl.receive_message()
if msg is None:
pass
if msg.name() == "NAV_POSLLH":
outstr = str(msg).split(",")[1:]
outstr = "".join(outstr)
components = search(" Longitude={:d} Latitude={:d} Height={:d}",
outstr)
h = Header()
h.stamp = rospy.Time.now()
navmsg = NavSatFix()
navmsg.header = h
navmsg.latitude = float(components[1]) / 10000000.0
navmsg.longitude = float(components[0]) / 10000000.0
navmsg.altitude = float(components[2]) / 10000000.0
publisher.publish(navmsg)
def spin_once(self):
def baroPressureToHeight(value):
c1 = 44330.0
c2 = 9.869232667160128300024673081668e-6
c3 = 0.1901975534856
intermediate = 1 - math.pow(c2 * value, c3)
height = c1 * intermediate
return height
# get data
data = self.mt.read_measurement()
# common header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
# get data (None if not present)
temp_data = data.get('Temperature') # float
orient_data = data.get('Orientation Data')
velocity_data = data.get('Velocity')
position_data = data.get('Latlon')
altitude_data = data.get('Altitude')
acc_data = data.get('Acceleration')
gyr_data = data.get('Angular Velocity')
mag_data = data.get('Magnetic')
pressure_data = data.get('Pressure')
time_data = data.get('Timestamp')
gnss_data = data.get('GNSS')
# debug the data from the sensor
# print(data)
# print("\n")
# create messages and default values
"Temp message"
temp_msg = Temperature()
pub_temp = False
"Imu message supported with Modes 1 & 2"
imuraw_msg = Imu()
pub_imuraw = False
imuins_msg = Imu()
pub_imuins = False
"SensorSample message supported with Mode 2"
ss_msg = sensorSample()
pub_ss = False
"Mag message supported with Modes 1 & 2"
mag_msg = Vector3Stamped()
pub_mag = False
"Baro in meters"
baro_msg = FluidPressure()
height_msg = baroSample()
pub_baro = False
"GNSS message supported only with MTi-G-7xx devices"
"Valid only for modes 1 and 2"
gnssPvt_msg = gnssSample()
gps1_msg = NavSatFix()
gps2_msg = GPSFix()
pub_gnssPvt = False
gnssSatinfo_msg = GPSStatus()
pub_gnssSatinfo = False
# All filter related outputs
"Supported in mode 3"
ori_msg = orientationEstimate()
pub_ori = False
"Supported in mode 3 for MTi-G-7xx devices"
vel_msg = velocityEstimate()
pub_vel = False
"Supported in mode 3 for MTi-G-7xx devices"
pos_msg = positionEstimate()
pub_pos = False
# first getting the sampleTimeFine
# note if we are not using ros time, the we should replace the header
# with the time supplied by the GNSS unit
if time_data and not self.use_rostime:
time = time_data['SampleTimeFine']
h.stamp.secs = 100e-6 * time
h.stamp.nsecs = 1e5 * time - 1e9 * math.floor(h.stamp.secs)
# temp data
if temp_data:
temp_msg.temperature = temp_data['Temp']
pub_temp = True
# acceleration data
if acc_data:
if 'accX' in acc_data: # found acceleration
pub_imuraw = True
imuraw_msg.linear_acceleration.x = acc_data['accX']
imuraw_msg.linear_acceleration.y = acc_data['accY']
imuraw_msg.linear_acceleration.z = acc_data['accZ']
if 'freeAccX' in acc_data: # found free acceleration
pub_imuins = True
imuins_msg.linear_acceleration.x = acc_data['freeAccX']
imuins_msg.linear_acceleration.y = acc_data['freeAccY']
imuins_msg.linear_acceleration.z = acc_data['freeAccZ']
if 'Delta v.x' in acc_data: # found delta-v's
pub_ss = True
ss_msg.internal.imu.dv.x = acc_data['Delta v.x']
ss_msg.internal.imu.dv.y = acc_data['Delta v.y']
ss_msg.internal.imu.dv.z = acc_data['Delta v.z']
#else:
# raise MTException("Unsupported message in XDI_AccelerationGroup.")
# gyro data
if gyr_data:
if 'gyrX' in gyr_data: # found rate of turn
pub_imuraw = True
imuraw_msg.angular_velocity.x = gyr_data['gyrX']
imuraw_msg.angular_velocity.y = gyr_data['gyrY']
imuraw_msg.angular_velocity.z = gyr_data['gyrZ']
# note we do not force publishing the INS if we do not use the free acceleration
imuins_msg.angular_velocity.x = gyr_data['gyrX']
imuins_msg.angular_velocity.y = gyr_data['gyrY']
imuins_msg.angular_velocity.z = gyr_data['gyrZ']
if 'Delta q0' in gyr_data: # found delta-q's
pub_ss = True
ss_msg.internal.imu.dq.w = gyr_data['Delta q0']
ss_msg.internal.imu.dq.x = gyr_data['Delta q1']
ss_msg.internal.imu.dq.y = gyr_data['Delta q2']
ss_msg.internal.imu.dq.z = gyr_data['Delta q3']
#else:
# raise MTException("Unsupported message in XDI_AngularVelocityGroup.")
# magfield
if mag_data:
ss_msg.internal.mag.x = mag_msg.vector.x = mag_data['magX']
ss_msg.internal.mag.y = mag_msg.vector.y = mag_data['magY']
ss_msg.internal.mag.z = mag_msg.vector.z = mag_data['magZ']
pub_mag = True
if pressure_data:
pub_baro = True
baro_msg.fluid_pressure = pressure_data['Pressure']
height = baroPressureToHeight(pressure_data['Pressure'])
height_msg.height = ss_msg.internal.baro.height = height
# gps fix message
if gnss_data and 'lat' in gnss_data:
pub_gnssPvt = True
# A "3" means that the MTi-G is using the GPS data.
# A "1" means that the MTi-G was using GPS data and is now coasting/dead-reckoning the
# position based on the inertial sensors (the MTi-G is not using GPS data in this mode).
# This is done for 45 seconds, before the MTi-G Mode drops to "0".
# A "0" means that the MTi-G doesn't use GPS data and also that it
# doesn't output position based on the inertial sensors.
if gnss_data['fix'] < 2:
gnssSatinfo_msg.status = NavSatStatus.STATUS_NO_FIX # no fix
gps1_msg.status.status = NavSatStatus.STATUS_NO_FIX # no fix
gps1_msg.status.service = 0
else:
gnssSatinfo_msg.status = NavSatStatus.STATUS_FIX # unaugmented
gps1_msg.status.status = NavSatStatus.STATUS_FIX # unaugmented
gps1_msg.status.service = NavSatStatus.SERVICE_GPS
# lat lon alt
gps1_msg.latitude = gnss_data['lat']
gps1_msg.longitude = gnss_data['lon']
gps1_msg.altitude = gnss_data['hEll']
# covariances
gps1_msg.position_covariance[0] = math.pow(gnss_data['horzAcc'], 2)
gps1_msg.position_covariance[4] = math.pow(gnss_data['horzAcc'], 2)
gps1_msg.position_covariance[8] = math.pow(gnss_data['vertAcc'], 2)
gps1_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
# custom message
gnssPvt_msg.itow = gnss_data['iTOW']
gnssPvt_msg.fix = gnss_data['fix']
gnssPvt_msg.latitude = gnss_data['lat']
gnssPvt_msg.longitude = gnss_data['lon']
gnssPvt_msg.hEll = gnss_data['hEll']
gnssPvt_msg.hMsl = gnss_data['hMsl']
gnssPvt_msg.vel.x = gnss_data['velE']
gnssPvt_msg.vel.y = gnss_data['velN']
gnssPvt_msg.vel.z = gnss_data['velD']
gnssPvt_msg.hAcc = gnss_data['horzAcc']
gnssPvt_msg.vAcc = gnss_data['vertAcc']
gnssPvt_msg.sAcc = gnss_data['speedAcc']
gnssPvt_msg.pDop = gnss_data['PDOP']
gnssPvt_msg.hDop = gnss_data['HDOP']
gnssPvt_msg.vDop = gnss_data['VDOP']
gnssPvt_msg.numSat = gnss_data['nSat']
gnssPvt_msg.heading = gnss_data['heading']
gnssPvt_msg.headingAcc = gnss_data['headingAcc']
if orient_data:
if 'Q0' in orient_data:
pub_imuraw = True
imuraw_msg.orientation.w = orient_data['Q0']
imuraw_msg.orientation.x = orient_data['Q1']
imuraw_msg.orientation.y = orient_data['Q2']
imuraw_msg.orientation.z = orient_data['Q3']
pub_imuins = True
imuins_msg.orientation.w = orient_data['Q0']
imuins_msg.orientation.x = orient_data['Q1']
imuins_msg.orientation.y = orient_data['Q2']
imuins_msg.orientation.z = orient_data['Q3']
elif 'Roll' in orient_data:
pub_ori = True
ori_msg.roll = orient_data['Roll']
ori_msg.pitch = orient_data['Pitch']
ori_msg.yaw = orient_data['Yaw']
else:
raise MTException(
'Unsupported message in XDI_OrientationGroup')
if velocity_data:
pub_vel = True
vel_msg.velE = velocity_data['velX']
vel_msg.velN = velocity_data['velY']
vel_msg.velU = velocity_data['velZ']
if position_data:
pub_pos = True
pos_msg.latitude = position_data['lat']
pos_msg.longitude = position_data['lon']
if altitude_data:
pub_pos = True
tempData = altitude_data['ellipsoid']
pos_msg.hEll = tempData[0]
# publish available information
if pub_imuraw:
imuraw_msg.header = h
self.imuraw_pub.publish(imuraw_msg)
if pub_imuins:
imuins_msg.header = h
self.imuins_pub.publish(imuins_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
if pub_temp:
temp_msg.header = h
self.temp_pub.publish(temp_msg)
if pub_ss:
ss_msg.header = h
self.ss_pub.publish(ss_msg)
if pub_baro:
baro_msg.header = h
height_msg.header = h
self.baro_pub.publish(baro_msg)
self.height_pub.publish(height_msg)
if pub_gnssPvt:
gnssPvt_msg.header = h
gps1_msg.header = h
self.gnssPvt_pub.publish(gnssPvt_msg)
self.gps1_pub.publish(gps1_msg)
#if pub_gnssSatinfo:
# gnssSatinfo_msg.header = h
# self.gnssSatinfo_pub.publish(gnssSatinfo_msg)
if pub_ori:
ori_msg.header = h
self.ori_pub.publish(ori_msg)
if pub_vel:
vel_msg.header = h
self.vel_pub.publish(vel_msg)
if pub_pos:
pos_msg.header = h
self.pos_pub.publish(pos_msg)
def my_handler(channel, data):
global seq
fix_pub = rospy.Publisher('fix', NavSatFix, queue_size=10)
odom_pub = rospy.Publisher('odom', Odometry, queue_size=10)
imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10)
use_odom = True
use_fix = True
use_imu = True
gps_frame = "novatel"
headermsg = Header()
headermsg.seq = seq + 1
headermsg.stamp = rospy.Time.now()
headermsg.frame_id = gps_frame
msg = auv_acfr_nav_t.decode(data)
if use_fix == True:
rospy.logdebug("Publising gps info")
fixmsg = NavSatFix()
fixmsg.header = headermsg
fixmsg.latitude = math.radians(msg.latitude)
fixmsg.longitude = math.radians(msg.longitude)
fixmsg.altitude = msg.altitude
fixmsg.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
fixmsg.position_covariance_type = 0
fix_pub.publish(fixmsg)
if use_imu == True:
rospy.logdebug("Publishing imu")
imumsg = Imu()
q = quaternion_from_euler(
msg.pitch, msg.roll,
-np.unwrap(msg.heading - np.pi / 2)) # TODO check this
imumsg.orientation.x = q[0]
imumsg.orientation.y = q[1]
imumsg.orientation.z = q[2]
imumsg.orientation.w = q[3]
imumsg.angular_velocity.x = msg.pitchRate
imumsg.angular_velocity.y = msg.rollRate
imumsg.angular_velocity.z = -msg.headingRate
imu_pub.publish(imumsg)
if use_odom == True:
br = tf.TransformBroadcaster()
q = quaternion_from_euler(
msg.pitch, msg.roll,
-np.unwrap(msg.heading - np.pi / 2)) # TODO check this
rospy.logdebug("Publishing odom")
odommsg = Odometry()
odommsg.header = headermsg
odommsg.header.frame_id = "odom"
#TODO, USE TRANSFORM
odommsg.child_frame_id = "base_link"
odommsg.pose.pose.position.x = msg.y
odommsg.pose.pose.position.y = msg.x
odommsg.pose.pose.position.z = -msg.altitude
odommsg.pose.pose.orientation.x = q[0]
odommsg.pose.pose.orientation.y = q[1]
odommsg.pose.pose.orientation.z = q[2]
odommsg.pose.pose.orientation.w = q[3]
odommsg.twist.twist.linear.x = msg.vy
odommsg.twist.twist.linear.y = msg.vx
odommsg.twist.twist.linear.z = -msg.vz
odommsg.twist.twist.angular.x = msg.pitchRate
odommsg.twist.twist.angular.y = msg.rollRate
odommsg.twist.twist.angular.z = -msg.headingRate
odom_pub.publish(odommsg)
br.sendTransform((msg.y, msg.x, 0),
tf.transformations.quaternion_from_euler(
msg.pitch, msg.roll,
-np.unwrap(msg.heading - np.pi / 2)),
rospy.Time.now(), "base_link", "odom")
def 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")
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
