def publish_sensor_data(self, sensor_data):
acc = np.array(sensor_data.acceleration.data) / 1000.0 * g
ang_vel = np.array(sensor_data.angular_vel.data) / 180.0 * np.pi
# ang_vel = rotate_v(ang_vel.tolist(), self.rotation)
msg = Imu()
msg.header.frame_id = self.base_frame_id
msg.header.stamp = rospy.Time.now()
msg.linear_acceleration = Vector3(*acc)
# It happens that sensor_data.quaterion and pose.quaternion are NOT in the same frame
q = sensor_data.quaternion.data
q = [q[1], q[2], q[3], q[0]]
msg.orientation = Quaternion(*q)
# msg.orientation = Quaternion(*self.orientation)
msg.angular_velocity = Vector3(*ang_vel)
msg.linear_acceleration_covariance = self.linear_acceleration_cov
msg.orientation_covariance = self.orientation_cov
msg.angular_velocity_covariance = self.angular_velocity_cov
self.imu_pub.publish(msg)
mag = np.array(sensor_data.magnetic.data) * 1e-6
m_msg = MagneticField()
m_msg.header = msg.header
m_msg.magnetic_field = Vector3(*mag)
m_msg.magnetic_field_covariance = self.magnetic_field_cov
self.mag_pub.publish(m_msg)
def main():
rospy.init_node('HMR2300_Publisher', anonymous = True)
Mag_pub = rospy.Publisher('/imu/mag', MagneticField, queue_size=10)
rate = rospy.Rate(2)
mag_msg = MagneticField()
mag_msg.header = Header()
mag_msg.header.frame_id = '/Magnet'
while not rospy.is_shutdown():
Yaw, Mag_X, Mag_Y, Mag_Z, secs, nsecs = receive()
mag_msg.header.stamp.secs = secs
mag_msg.header.stamp.nsecs = nsecs
mag_msg.magnetic_field.x = 0
mag_msg.magnetic_field.y = 0
mag_msg.magnetic_field.z = Yaw
mag_msg.magnetic_field_covariance[0] = Mag_X
mag_msg.magnetic_field_covariance[1] = Mag_Y
mag_msg.magnetic_field_covariance[2] = Mag_Z
Mag_pub.publish(mag_msg)
#print mag_msg
rate.sleep()
rospy.spin()
def main():
rospy.init_node("imu")
port = rospy.get_param("~port", "GPG3_AD1")
sensor = InertialMeasurementUnit(bus=port)
pub_imu = rospy.Publisher("~imu", Imu, queue_size=10)
pub_temp = rospy.Publisher("~temp", Temperature, queue_size=10)
pub_magn = rospy.Publisher("~magnetometer", MagneticField, queue_size=10)
br = TransformBroadcaster()
msg_imu = Imu()
msg_temp = Temperature()
msg_magn = MagneticField()
hdr = Header(stamp=rospy.Time.now(), frame_id="IMU")
rate = rospy.Rate(rospy.get_param('~hz', 30))
while not rospy.is_shutdown():
q = sensor.read_quaternion() # x,y,z,w
mag = sensor.read_magnetometer() # micro Tesla (µT)
gyro = sensor.read_gyroscope() # deg/second
accel = sensor.read_accelerometer() # m/s²
temp = sensor.read_temperature() # °C
msg_imu.header = hdr
hdr.stamp = rospy.Time.now()
msg_temp.header = hdr
msg_temp.temperature = temp
pub_temp.publish(msg_temp)
msg_imu.header = hdr
msg_imu.linear_acceleration.x = accel[0]
msg_imu.linear_acceleration.y = accel[1]
msg_imu.linear_acceleration.z = accel[2]
msg_imu.angular_velocity.x = math.radians(gyro[0])
msg_imu.angular_velocity.y = math.radians(gyro[1])
msg_imu.angular_velocity.z = math.radians(gyro[2])
msg_imu.orientation.w = q[3]
msg_imu.orientation.x = q[0]
msg_imu.orientation.y = q[1]
msg_imu.orientation.z = q[2]
pub_imu.publish(msg_imu)
msg_magn.header = hdr
msg_magn.magnetic_field.x = mag[0] * 1e-6
msg_magn.magnetic_field.y = mag[1] * 1e-6
msg_magn.magnetic_field.z = mag[2] * 1e-6
pub_magn.publish(msg_magn)
transform = TransformStamped(header=Header(stamp=rospy.Time.now(),
frame_id="world"),
child_frame_id="IMU")
transform.transform.rotation = msg_imu.orientation
br.sendTransformMessage(transform)
rate.sleep()
def callbackImu(msg):
# Grab accelerometer and gyro data.
imu_msg = Imu()
imu_msg.header = msg.header
imu_msg.header.frame_id = 'imu_link'
q = np.array([msg.quaternion.x, msg.quaternion.y, msg.quaternion.z, msg.quaternion.w])
q = normalize(q)
euler = tf.transformations.euler_from_quaternion(q)
q = tf.transformations.quaternion_from_euler(-(euler[0] + math.pi / 2), euler[1], euler[2] - math.pi)
imu_msg.orientation.x = q[0]
imu_msg.orientation.y = q[1]
imu_msg.orientation.z = q[2]
imu_msg.orientation.w = q[3]
euler_new = tf.transformations.euler_from_quaternion(q)
rospy.loginfo("euler[0] = %s, euler[1] = %s, euler = [2] = %s",
str(euler_new[0]), str(euler_new[1]), str(euler_new[2]))
imu_msg.orientation_covariance = [0.9, 0, 0, \
0, 0.9, 0, \
0, 0, 0.9]
imu_msg.angular_velocity = msg.gyro
imu_msg.angular_velocity_covariance = [0.9, 0, 0, \
0, 0.9, 0, \
0, 0, 0.9]
# Estimate gyro bias.
global gyro_readings
global MAX_GYRO_READINGS
global gyro_bias
if len(gyro_readings) < MAX_GYRO_READINGS:
gyro_readings.append(imu_msg.angular_velocity.z)
elif math.isnan(gyro_bias):
gyro_bias = sum(gyro_readings)/MAX_GYRO_READINGS
if not math.isnan(gyro_bias):
imu_msg.angular_velocity.z -= gyro_bias
imu_msg.linear_acceleration = msg.accelerometer
imu_msg.linear_acceleration_covariance = [0.90, 0, 0, \
0, 0.90, 0, \
0, 0, 0.90]
# Grab magnetometer data.
mag_msg = MagneticField()
mag_msg.header = msg.header
mag_msg.magnetic_field = msg.magnetometer
# TODO(gbrooks): Add covariance.
# Publish sensor data.
imu_pub.publish(imu_msg)
mag_pub.publish(mag_msg)
def main():
rospy.init_node('em7180')
publisher = rospy.Publisher('imu/mag', MagneticField, queue_size=10)
# transformed_publisher = rospy.Publisher('imu/magTransformed', Vector3Stamped, queue_size=10)
rate = rospy.Rate(10)
em7180 = EM7180_Master(MAG_RATE, ACCEL_RATE, GYRO_RATE, BARO_RATE,
Q_RATE_DIVISOR)
listener = tf.TransformListener()
listener.waitForTransform("em7180_link", "imu_0", rospy.Time(0),
rospy.Duration(1.0))
# Start the EM7180 in master mode
if not em7180.begin():
print(em7180.getErrorString())
exit(1)
while not rospy.is_shutdown():
em7180.checkEventStatus()
if em7180.gotError():
print('ERROR: ' + em7180.getErrorString())
exit(1)
if em7180.gotMagnetometer():
mx, my, mz = em7180.readMagnetometer()
magnetic_vector = Vector3Stamped()
magnetic_vector.header.stamp = rospy.Time.now()
magnetic_vector.header.frame_id = "em7180_link"
magnetic_vector.vector.x = mx
magnetic_vector.vector.y = my
magnetic_vector.vector.z = mz
magnetic_vector_transformed = tf.transformVector3(
"imu_0", magnetic_vector)
magnetic_field_msg = MagneticField()
magnetic_field_msg.header = magnetic_vector_transformed.header
magnetic_field_msg.magnetic_field = magnetic_vector_transformed.vector
magnetic_field_msg.magnetic_field_covariance = [
700, 0, 0, 0, 700, 0, 0, 0, 700
]
publisher.publish(magnetic_field_msg)
rate.sleep()
def magResponseCallback(self, msg):
#############################################################################
mag_msg = MagneticField()
mag_msg.header = msg.header
mag_msg.header.frame_id = "imu_link"
mag_msg.magnetic_field.x = msg.vector.x
mag_msg.magnetic_field.y = msg.vector.y
mag_msg.magnetic_field.z = 0.0
mag_msg.magnetic_field_covariance[0] = 0.1
mag_msg.magnetic_field_covariance[4] = 0.1
mag_msg.magnetic_field_covariance[8] = 0.1
self.magPub.publish(mag_msg)
def callbackImu(msg):
# Grab accelerometer and gyro data.
imu_msg = Imu()
imu_msg.header = msg.header
imu_msg.header.frame_id = 'imu_link'
imu_msg.orientation_covariance = [0.09, 0, 0, \
0, 0.09, 0, \
0, 0, 0.09]
imu_msg.angular_velocity = msg.gyro
imu_msg.angular_velocity_covariance = [0.9, 0, 0, \
0, 0.9, 0, \
0, 0, 0.9]
# Estimate gyro bias.
global gyro_readings
global MAX_GYRO_READINGS
global gyro_bias
if len(gyro_readings) < MAX_GYRO_READINGS:
gyro_readings.append(imu_msg.angular_velocity.z)
print(len(gyro_readings))
elif math.isnan(gyro_bias):
gyro_bias = sum(gyro_readings) / MAX_GYRO_READINGS
if not math.isnan(gyro_bias):
imu_msg.angular_velocity.z -= gyro_bias
imu_msg.linear_acceleration = msg.accelerometer
imu_msg.linear_acceleration_covariance = [0.90, 0, 0, \
0, 0.90, 0, \
0, 0, 0.90]
# Grab magnetometer data.
mag_msg = MagneticField()
mag_msg.header = msg.header
mag_msg.magnetic_field = msg.magnetometer
# TODO(gbrooks): Add covariance.
# Publish sensor data.
imu_pub.publish(imu_msg)
mag_pub.publish(mag_msg)
def mag_callback(in_msg):
out_msg = Mag_msg()
out_msg.header = in_msg.header
out_msg.magnetic_field = in_msg.vector
set_covariance_as_identity(out_msg.magnetic_field_covariance)
mag_publisher.publish(out_msg)
def main():
parser = argparse.ArgumentParser(
description="Convert an LCM log to a ROS bag (mono/stereo images only)."
)
parser.add_argument('--input', help='Input LCM log.')
parser.add_argument('--namespace', help='camera namespace')
parser.add_argument('--cam_yml',
help='Image calibration YAML file from ROS calibrator')
parser.add_argument('--visualise', help='Visualise image', default=False)
args = parser.parse_args()
print "Converting images in %s to ROS bag file..." % (args.input)
log = lcm.EventLog(args.input, 'r')
bag = rosbag.Bag(args.input + '.converted.bag',
mode='w',
compression='lz4')
bridge = CvBridge()
# # Read in YAML files.
with open(args.cam_yml) as f:
config = yaml.load(f)
lcm_channels = ['MICROSTRAIN_RAW', 'XTION']
try:
count = 0
with click.progressbar(length=log.size()) as bar:
for event in log:
event_stamp = rospy.Time().from_sec(
float(event.timestamp) / 1e6)
if event.channel == 'XTION':
lcm_msg = rgbd_t.decode(event.data)
rgb_img = cv2.imdecode(
np.fromstring(lcm_msg.rgb, np.uint8), cv2.IMREAD_COLOR)
rgb_img = cv2.cvtColor(rgb_img, cv2.COLOR_RGB2BGR)
if args.visualise:
cv2.imshow('RGB', rgb_img)
cv2.waitKey(10)
depth_img = np.fromstring(zlib.decompress(lcm_msg.depth),
np.uint16).reshape(
lcm_msg.height,
lcm_msg.width)
rgb_ros_msg = bridge.cv2_to_imgmsg(rgb_img, 'bgr8')
rgb_ros_msg.header.seq = event.eventnum
secs_float = float(lcm_msg.utime) / 1e6
nsecs_float = (secs_float - np.floor(secs_float)) * 1e9
rgb_ros_msg.header.stamp.secs = int(secs_float)
rgb_ros_msg.header.stamp.nsecs = int(nsecs_float)
rgb_ros_msg.header.frame_id = args.namespace
depth_ros_msg = bridge.cv2_to_imgmsg(depth_img)
depth_ros_msg.header.seq = event.eventnum
depth_ros_msg.header = rgb_ros_msg.header
camera_info = CameraInfo()
camera_info.header = rgb_ros_msg.header
camera_info.height = rgb_img.shape[0]
camera_info.width = rgb_img.shape[1]
camera_info.distortion_model = 'plumb_bob'
camera_info.K = config["K"]
bag.write(args.namespace + '/rgb/image_raw', rgb_ros_msg,
event_stamp)
bag.write(args.namespace + '/depth/image_rect',
depth_ros_msg, event_stamp)
bag.write(args.namespace + '/camera_info', camera_info,
event_stamp)
if event.channel == 'MICROSTRAIN_RAW':
lcm_msg = raw_t.decode(event.data)
imu_ros_msg = Imu()
secs_float = float(lcm_msg.utime) / 1e9
nsecs_float = (secs_float - np.floor(secs_float)) * 1e9
imu_ros_msg.header.seq = event.eventnum
imu_ros_msg.header.stamp.secs = int(secs_float)
imu_ros_msg.header.stamp.nsecs = int(nsecs_float)
imu_ros_msg.header.frame_id = "imu"
imu_ros_msg.linear_acceleration.x = lcm_msg.accel[0]
imu_ros_msg.linear_acceleration.y = lcm_msg.accel[1]
imu_ros_msg.linear_acceleration.z = lcm_msg.accel[2]
imu_ros_msg.angular_velocity.x = lcm_msg.gyro[0]
imu_ros_msg.angular_velocity.y = lcm_msg.gyro[1]
imu_ros_msg.angular_velocity.z = lcm_msg.gyro[2]
# Store magnetometer in magnetometer msg
mag_msg = MagneticField()
mag_msg.header = imu_ros_msg.header
mag_msg.magnetic_field.x = lcm_msg.mag[0]
mag_msg.magnetic_field.y = lcm_msg.mag[1]
mag_msg.magnetic_field.z = lcm_msg.mag[2]
# Ignore pressure for now
# lcm_msg.pressure
bag.write('imu_raw', imu_ros_msg, event_stamp)
bag.write('imu_mag', mag_msg, event_stamp)
bar.update(event.__sizeof__() + event.data.__sizeof__())
finally:
log.close()
bag.close()
print("Done.")
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.get('Temp') # 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 PVT')
status = data.get('Status') # int
# create messages and default values
"Imu message supported with Modes 1 & 2"
imu_msg = Imu()
pub_imu = False
"SensorSample message supported with Mode 2"
ss_msg = sensorSample()
pub_ss = False
"Mag message supported with Modes 1 & 2"
mag_msg = MagneticField()
pub_mag = False
"Baro in meters"
baro_msg = baroSample()
pub_baro = False
"GNSS message supported only with MTi-G-7xx devices"
"Valid only for modes 1 and 2"
gnssPvt_msg = gnssSample()
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
secs = 0
nsecs = 0
if time_data:
# first getting the sampleTimeFine
time = time_data['SampleTimeFine']
secs = 100e-6*time
nsecs = 1e5*time - 1e9*math.floor(secs)
if acc_data:
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']
elif 'accX' in acc_data: # found acceleration
pub_imu = True
imu_msg.linear_acceleration.x = acc_data['accX']
imu_msg.linear_acceleration.y = acc_data['accY']
imu_msg.linear_acceleration.z = acc_data['accZ']
else:
raise MTException("Unsupported message in XDI_AccelerationGroup.")
if gyr_data:
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']
elif 'gyrX' in gyr_data: # found rate of turn
pub_imu = True
imu_msg.angular_velocity.x = gyr_data['gyrX']
imu_msg.angular_velocity.y = gyr_data['gyrY']
imu_msg.angular_velocity.z = gyr_data['gyrZ']
else:
raise MTException("Unsupported message in XDI_AngularVelocityGroup.")
if mag_data:
# magfield
ss_msg.internal.mag.x = mag_msg.magnetic_field.x = mag_data['magX']
ss_msg.internal.mag.y = mag_msg.magnetic_field.y = mag_data['magY']
ss_msg.internal.mag.z = mag_msg.magnetic_field.z = mag_data['magZ']
pub_mag = True
if pressure_data:
pub_baro = True
height = baroPressureToHeight(pressure_data['Pressure'])
baro_msg.height = ss_msg.internal.baro.height = height
if gnss_data:
pub_gnssPvt = True
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_imu = True
imu_msg.orientation.x = orient_data['Q2'] #Q0
imu_msg.orientation.y = -orient_data['Q1'] #Q1
imu_msg.orientation.z = -orient_data['Q0'] #Q2
imu_msg.orientation.w = orient_data['Q3'] #Q3
#print orient_data
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]
#if status is not None:
# if status & 0b0001:
# self.stest_stat.level = DiagnosticStatus.OK
# self.stest_stat.message = "Ok"
# else:
# self.stest_stat.level = DiagnosticStatus.ERROR
# self.stest_stat.message = "Failed"
# if status & 0b0010:
# self.xkf_stat.level = DiagnosticStatus.OK
# self.xkf_stat.message = "Valid"
# else:
# self.xkf_stat.level = DiagnosticStatus.WARN
# self.xkf_stat.message = "Invalid"
# if status & 0b0100:
# self.gps_stat.level = DiagnosticStatus.OK
# self.gps_stat.message = "Ok"
# else:
# self.gps_stat.level = DiagnosticStatus.WARN
# self.gps_stat.message = "No fix"
# self.diag_msg.header = h
# self.diag_pub.publish(self.diag_msg)
# publish available information
if pub_imu:
imu_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
#imu_msg.header.stamp.secs = secs
#imu_msg.header.stamp.nsecs = nsecs
self.imu_pub.publish(imu_msg)
#if pub_gps:
# xgps_msg.header = gps_msg.header = h
# self.gps_pub.publish(gps_msg)
# self.xgps_pub.publish(xgps_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
#if pub_temp:
# self.temp_pub.publish(temp_msg)
if pub_ss:
ss_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
ss_msg.header.stamp.secs = secs
ss_msg.header.stamp.nsecs = nsecs
self.ss_pub.publish(ss_msg)
if pub_baro:
baro_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
baro_msg.header.stamp.secs = secs
baro_msg.header.stamp.nsecs = nsecs
self.baro_pub.publish(baro_msg)
if pub_gnssPvt:
gnssPvt_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
baro_msg.header.stamp.secs = secs
baro_msg.header.stamp.nsecs = nsecs
self.gnssPvt_pub.publish(gnssPvt_msg)
if pub_ori:
ori_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
ori_msg.header.stamp.secs = secs
ori_msg.header.stamp.nsecs = nsecs
self.ori_pub.publish(ori_msg)
if pub_vel:
vel_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
vel_msg.header.stamp.secs = secs
vel_msg.header.stamp.nsecs = nsecs
self.vel_pub.publish(vel_msg)
if pub_pos:
pos_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
pos_msg.header.stamp.secs = secs
pos_msg.header.stamp.nsecs = nsecs
self.pos_pub.publish(pos_msg)
xsensVel.x = struct.unpack(
'!f', orientation_hex[38:46].decode("hex"))[0]
xsensVel.y = struct.unpack(
'!f', orientation_hex[46:54].decode("hex"))[0]
xsensVel.z = struct.unpack(
'!f', orientation_hex[54:62].decode("hex"))[0]
imuData.angular_velocity.x = struct.unpack(
'!f', orientation_hex[68:76].decode("hex"))[0]
imuData.angular_velocity.y = struct.unpack(
'!f', orientation_hex[76:84].decode("hex"))[0]
imuData.angular_velocity.z = struct.unpack(
'!f', orientation_hex[84:92].decode("hex"))[0]
magData.header = header
magData.magnetic_field.x = struct.unpack(
'!f', orientation_hex[98:106].decode("hex"))[0]
magData.magnetic_field.y = struct.unpack(
'!f', orientation_hex[106:114].decode("hex"))[0]
magData.magnetic_field.z = struct.unpack(
'!f', orientation_hex[114:122].decode("hex"))[0]
temperature.header = header
temperature.temperature = struct.unpack(
'!f', orientation_hex[128:136].decode("hex"))[0]
#print(temperature.temperature)
ImuPub.publish(imuData)
xsensVelPub.publish(xsensVel)
tempPub.publish(temperature)
magPub.publish(magData)
magMsg.magnetic_field.y = float(words[10]) / 1000.0
magMsg.magnetic_field.z = float(words[11]) / 1000.0
else:
magMsg.magnetic_field.x = float('nan')
magMsg.magnetic_field.y = float('nan')
magMsg.magnetic_field.z = float('nan')
q = quaternion_from_euler(roll, pitch, yaw)
imuMsg.orientation.x = q[0]
imuMsg.orientation.y = q[1]
imuMsg.orientation.z = q[2]
imuMsg.orientation.w = q[3]
imuMsg.header.stamp = rospy.Time.now()
imuMsg.header.frame_id = 'imu_link'
imuMsg.header.seq = seq
magMsg.header = imuMsg.header
poseMsg.header = imuMsg.header
poseMsg.pose.position.x = poseMsg.pose.position.y = poseMsg.pose.position.z = 0.0
poseMsg.pose.orientation = imuMsg.orientation
seq = seq + 1
pub.publish(imuMsg)
magpub.publish(magMsg)
posepub.publish(poseMsg)
# br.sendTransform((0,0,0), tf.transformations.quaternion_from_euler(roll,pitch,yaw), imuMsg.header.stamp, 'imu_link','imu_frame')
if (diag_pub_time < rospy.get_time()):
diag_pub_time += 1
diag_arr = DiagnosticArray()
diag_arr.header.stamp = rospy.get_rostime()
diag_arr.header.frame_id = '1'
diag_msg = DiagnosticStatus()
diag_msg.name = 'Razor_Imu'
# imu.read_acc()
# imu.read_temp()
# imu.read_mag()
# print "Accelerometer: ", imu.accelerometer_data
# print "Gyroscope: ", imu.gyroscope_data
# print "Temperature: ", imu.temperature
# print "Magnetometer: ", imu.magnetometer_data
# time.sleep(0.1)
h=Header()
h.stamp=rospy.Time.now()
m9a, m9g, m9m = imu.getMotion9()
imu_msg = Imu()
imu_msg.header=h
imu_msg.angular_velocity.x=m9g[0]
imu_msg.angular_velocity.y=m9g[1]
imu_msg.angular_velocity.z=m9g[2]
imu_msg.linear_acceleration.x=m9a[0]
imu_msg.linear_acceleration.y=m9a[1]
imu_msg.linear_acceleration.z=m9a[2]
imu_pub.publish(imu_msg)
mag_msg= MagneticField()
mag_msg.header=h
mag_msg.magnetic_field.x=m9m[0]
mag_msg.magnetic_field.y=m9m[1]
mag_msg.magnetic_field.z=m9m[2]
mag_pub.publish(mag_msg)
rate.sleep()
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.get('Temp') # 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 PVT')
status = data.get('Status') # int
# create messages and default values
"Imu message supported with Modes 1 & 2"
imu_msg = Imu()
pub_imu = False
"SensorSample message supported with Mode 2"
ss_msg = sensorSample()
pub_ss = False
"Mag message supported with Modes 1 & 2"
mag_msg = MagneticField()
pub_mag = False
"Baro in meters"
baro_msg = baroSample()
pub_baro = False
"GNSS message supported only with MTi-G-7xx devices"
"Valid only for modes 1 and 2"
gnssPvt_msg = gnssSample()
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
secs = 0
nsecs = 0
if time_data:
# first getting the sampleTimeFine
time = time_data['SampleTimeFine']
secs = 100e-6 * time
nsecs = 1e5 * time - 1e9 * math.floor(secs)
if acc_data:
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']
elif 'accX' in acc_data: # found acceleration
pub_imu = True
imu_msg.linear_acceleration.x = acc_data['accX']
imu_msg.linear_acceleration.y = acc_data['accY']
imu_msg.linear_acceleration.z = acc_data['accZ']
else:
raise MTException(
"Unsupported message in XDI_AccelerationGroup.")
if gyr_data:
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']
elif 'gyrX' in gyr_data: # found rate of turn
pub_imu = True
imu_msg.angular_velocity.x = gyr_data['gyrX']
imu_msg.angular_velocity.y = gyr_data['gyrY']
imu_msg.angular_velocity.z = gyr_data['gyrZ']
else:
raise MTException(
"Unsupported message in XDI_AngularVelocityGroup.")
if mag_data:
# magfield
ss_msg.internal.mag.x = mag_msg.magnetic_field.x = mag_data['magX']
ss_msg.internal.mag.y = mag_msg.magnetic_field.y = mag_data['magY']
ss_msg.internal.mag.z = mag_msg.magnetic_field.z = mag_data['magZ']
pub_mag = True
if pressure_data:
pub_baro = True
height = baroPressureToHeight(pressure_data['Pressure'])
baro_msg.height = ss_msg.internal.baro.height = height
if gnss_data:
pub_gnssPvt = True
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_imu = True
imu_msg.orientation.x = orient_data['Q2'] #Q0
imu_msg.orientation.y = -orient_data['Q1'] #Q1
imu_msg.orientation.z = -orient_data['Q0'] #Q2
imu_msg.orientation.w = orient_data['Q3'] #Q3
#print orient_data
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]
#if status is not None:
# if status & 0b0001:
# self.stest_stat.level = DiagnosticStatus.OK
# self.stest_stat.message = "Ok"
# else:
# self.stest_stat.level = DiagnosticStatus.ERROR
# self.stest_stat.message = "Failed"
# if status & 0b0010:
# self.xkf_stat.level = DiagnosticStatus.OK
# self.xkf_stat.message = "Valid"
# else:
# self.xkf_stat.level = DiagnosticStatus.WARN
# self.xkf_stat.message = "Invalid"
# if status & 0b0100:
# self.gps_stat.level = DiagnosticStatus.OK
# self.gps_stat.message = "Ok"
# else:
# self.gps_stat.level = DiagnosticStatus.WARN
# self.gps_stat.message = "No fix"
# self.diag_msg.header = h
# self.diag_pub.publish(self.diag_msg)
# publish available information
if pub_imu:
imu_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
#imu_msg.header.stamp.secs = secs
#imu_msg.header.stamp.nsecs = nsecs
self.imu_pub.publish(imu_msg)
#if pub_gps:
# xgps_msg.header = gps_msg.header = h
# self.gps_pub.publish(gps_msg)
# self.xgps_pub.publish(xgps_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
#if pub_temp:
# self.temp_pub.publish(temp_msg)
if pub_ss:
ss_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
ss_msg.header.stamp.secs = secs
ss_msg.header.stamp.nsecs = nsecs
self.ss_pub.publish(ss_msg)
if pub_baro:
baro_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
baro_msg.header.stamp.secs = secs
baro_msg.header.stamp.nsecs = nsecs
self.baro_pub.publish(baro_msg)
if pub_gnssPvt:
gnssPvt_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
baro_msg.header.stamp.secs = secs
baro_msg.header.stamp.nsecs = nsecs
self.gnssPvt_pub.publish(gnssPvt_msg)
if pub_ori:
ori_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
ori_msg.header.stamp.secs = secs
ori_msg.header.stamp.nsecs = nsecs
self.ori_pub.publish(ori_msg)
if pub_vel:
vel_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
vel_msg.header.stamp.secs = secs
vel_msg.header.stamp.nsecs = nsecs
self.vel_pub.publish(vel_msg)
if pub_pos:
pos_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
pos_msg.header.stamp.secs = secs
pos_msg.header.stamp.nsecs = nsecs
self.pos_pub.publish(pos_msg)
