def __init__(self):
rospy.init_node('sensor_sim_gps')
self.pub = rospy.Publisher("/gps_pose", PoseStamped)
self.pubStat = rospy.Publisher("/gps_status", GPSStatus)
# declare messages
self.gps_msg = PoseStamped()
self.gps_stat = GPSStatus()
# Mimic GPS status message
self.gps_stat.satellites_used = 7
self.gps_stat.satellites_visible = 9
self.gps_stat.position_source = 50
#setup subscriber
rospy.Subscriber("base_pose_ground_truth", Odometry, self.callback)
#spin
rospy.spin()
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)
#ROS init
rospy.loginfo("1")
rospy.init_node('arduimu_driver')
imuPub = rospy.Publisher('arduimu/imu', Imu)
gpsPub = rospy.Publisher('arduimu/gps_fix', NavSatFix)
velPub = rospy.Publisher('arduimu/gps_vel', TwistStamped)
statPub = rospy.Publisher('arduimu/gps_status', GPSStatus)
#Init Imu port
arduPort = rospy.get_param('~port', '/dev/ttyUSB0')
arduRate = rospy.get_param('~baud', 57600)
rospy.loginfo("2")
imuData = Imu()
navData = NavSatFix()
gpsVel = TwistStamped()
gpsStat = GPSStatus()
imuData.header.frame_id = "base_arduimu"
navData.header.frame_id = "base_arduimu"
gpsVel.header.frame_id = "base_arduimu"
gpsStat.header.frame_id = "base_arduimu"
rospy.loginfo("3")
parser = ArduimuParser()
try:
rospy.loginfo("4")
dataIn = serial.Serial(port=arduPort, baudrate=arduRate, timeout=.02)
#Read in data
sync0 = '\x00'
sync1 = '\x00'
while not rospy.is_shutdown():
gpsPub = rospy.Publisher('gps_fix', NavSatFix, queue_size=1)
gpsStatPub = rospy.Publisher('gps_status', GPSStatus, queue_size=1)
gpsVelPub = rospy.Publisher('gps_vel',TwistStamped, queue_size=1)
#gpsTimePub = rospy.Publisher('time_reference', TimeReference)
#Init gpsSerial port
GPSport = rospy.get_param('~port','/dev/ttyUSB0')
GPSrate = rospy.get_param('~baud',57600)
frame_id = rospy.get_param('~frame_id','gps')
if frame_id[0] != "/":
frame_id = addTFPrefix(frame_id)
time_ref_source = rospy.get_param('~time_ref_source', frame_id)
navData = NavSatFix()
gpsVel = TwistStamped()
gpstime = TimeReference()
gpsStatus = GPSStatus()
gpstime.source = time_ref_source
navData.header.frame_id = frame_id
gpsVel.header.frame_id = frame_id
GPSLock = False
parser = NovatelParser()
try:
gpsSerial = serial.Serial(port=GPSport,baudrate=GPSrate,timeout=.01)
#Read in gpsSerial data
sync0 = '\x00'; sync1 = '\x00'; sync2 = '\x00';
while not rospy.is_shutdown():
# READ UNTIL SYNC
data = gpsSerial.read(1)
sync2 = sync1; sync1 = sync0; sync0 = data;
import serial
import time
import rospy
import numpy as np # se emplea esta para operar matrices
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Vector3Stamped
from gps_common.msg import GPSStatus, GPSFix
msg = Vector3Stamped()
gps = GPSFix()
gps2 = GPSStatus()
#gps = serial.Serial("/dev/tty_gps", baudrate = 9600)
class GPS(object):
def __init__(self):
self.gps = serial.Serial("/dev/tty_gps", baudrate=9600)
self.line = 0
self.data = 0
#Geometria del elipsoide
self.a = 6378137.0 # radius of the earth in metters
self.b = 6356752.31414
self.e1 = 6.6943800229e-3
self.e2 = 6.73949677548e-3
#radio polar de curvatura = c
self.c = 6399593.626
self.Long_rad = 0