def publish(self, data):
if not self._calib and data.getImuMsgId() == PUB_ID:
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
elif data.getImuMsgId() == CALIB_ID:
x, y, z = data.getValues()
msg = imuCalib()
if x > self._xMax:
self._xMax = x
if x < self._xMin:
self._xMin = x
if y > self._yMax:
self._yMax = y
if y < self._yMin:
self._yMin = y
if z > self._zMax:
self._zMax = z
if z < self._zMin:
self._zMin = z
msg.x.data = x
msg.x.max = self._xMax
msg.x.min = self._xMin
msg.y.data = y
msg.y.max = self._yMax
msg.y.min = self._yMin
msg.z.data = z
msg.z.max = self._zMax
msg.z.min = self._zMin
self._pubCalib.publish(msg)
def _create_msg(self, data):
"""
Messages published with ROS have axis from the robot frame : x forward, y ?, z up or down
"""
msg1 = Imu()
msg1_magfield = MagneticField()
msg1.header.stamp = rospy.Time.now()
msg1.header.frame_id = '/base_link'
msg1_magfield.header.stamp = rospy.Time.now()
msg1_magfield.header.frame_id = '/base_link'
msg1.linear_acceleration.x = -data["IMU1"]["accel_y"]
msg1.linear_acceleration.y = -data["IMU1"]["accel_z"]
msg1.linear_acceleration.z = data["IMU1"]["accel_x"]
msg1.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg1.angular_velocity.x = -data["IMU1"]["gyro_y"]
msg1.angular_velocity.y = -data["IMU1"]["gyro_z"]
msg1.angular_velocity.z = data["IMU1"]["gyro_x"]
msg1.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg1.orientation.w = 0
msg1.orientation.x = 0
msg1.orientation.y = 0
msg1.orientation.z = 0
msg1.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg1_magfield.magnetic_field.x = -data["IMU1"]["mag_y"]
msg1_magfield.magnetic_field.y = -data["IMU1"]["mag_z"]
msg1_magfield.magnetic_field.z = data["IMU1"]["mag_x"]
msg2 = Imu()
msg2_magfield = MagneticField()
msg2.header.stamp = rospy.Time.now()
msg2.header.frame_id = '/base_link'
msg2_magfield.header.stamp = rospy.Time.now()
msg2_magfield.header.frame_id = '/base_link'
msg2.linear_acceleration.x = data["IMU2"]["accel_y"]
msg2.linear_acceleration.y = data["IMU2"]["accel_z"]
msg2.linear_acceleration.z = data["IMU2"]["accel_x"]
msg2.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg2.angular_velocity.x = data["IMU2"]["gyro_y"]
msg2.angular_velocity.y = data["IMU2"]["gyro_z"]
msg2.angular_velocity.z = data["IMU2"]["gyro_x"]
msg2.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg2.orientation.w = 0
msg2.orientation.x = 0
msg2.orientation.y = 0
msg2.orientation.z = 0
msg2.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg2_magfield.magnetic_field.x = data["IMU2"]["mag_y"]
msg2_magfield.magnetic_field.y = data["IMU2"]["mag_z"]
msg2_magfield.magnetic_field.z = data["IMU2"]["mag_x"]
return msg1, msg1_magfield, msg2, msg2_magfield
def publishMag(self, mag, time):
'''!
Publica os dados de campo magnetico
@param accel: float[3] - leituras do magnetometro
@param time: float - momento em que a leitura foi realizada
'''
msg = MagneticField()
msg.magnetic_field.x = mag[0] * 0.000001
msg.magnetic_field.y = mag[1] * 0.000001
msg.magnetic_field.z = mag[2] * 0.000001
msg.magnetic_field_covariance = np.zeros((3, 3), dtype=np.float64)
frameId = self.get_parameter(
"frame_id").get_parameter_value().string_value
msg.header.frame_id = frameId
timeSec = float(time) / 1000.0
msg.header.stamp.sec = int(timeSec)
msg.header.stamp.nanosec = int((timeSec * 1000000000) % 1000000000)
self.publisherMag.publish(msg)
def publish_data(self):
data_pub = Imu()
data_pub_mag = MagneticField()
data_pub.header.stamp = rospy.get_rostime()
data_pub_mag.header.stamp = rospy.get_rostime()
#convert from g to m/s²
data_pub.angular_velocity.x = gyro_xyz[0]* 0.0174532925 #gyros_x
data_pub.angular_velocity.y = gyro_xyz[1]* 0.0174532925 #gyros_y
data_pub.angular_velocity.z = gyro_xyz[2]* 0.0174532925 #gyros_z
data_pub.angular_velocity_covariance[0] = 0
#convert from deg/s to rad/s (pi/180 = 0.0174532925)
data_pub.linear_acceleration.x = accel_xyz[0]*9.81 #accel_x
data_pub.linear_acceleration.y = accel_xyz[1]*9.81#accel_y
data_pub.linear_acceleration.z = accel_xyz[2]*9.81 #accel_z
data_pub.linear_acceleration_covariance[0] = 0
#convert from Guass to Tesla
data_pub_mag.magnetic_field.x = magn_xyz[0]*0.0001 #accel_x
data_pub_mag.magnetic_field.y = magn_xyz[1]*0.0001 #accel_y
data_pub_mag.magnetic_field.z = magn_xyz[2]*0.0001 #accel_z
data_pub_mag.magnetic_field_covariance[0] = 0 #variance unknown
self.Imu_data_pub.publish(data_pub)
self.Imu_data_mag.publish(data_pub_mag)
def publish(self, data):
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
# print "Before ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"
array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
# print "after ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
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 imu_publisher():
global accl_x, accl_y, accl_z, gyro_x, gyro_y, gyro_z, comp_x, comp_y, comp_z
rospy.init_node(
'Imu_mpu9250_node', anonymous=False
) # if anonymous=True is to ensure that node is unique by adding random number in the end
r = rospy.Rate(20.0)
current_time = rospy.Time.now()
curn_time = time.time()
last_time = time.time()
new_time = 0.0
while not rospy.is_shutdown():
current_time = rospy.Time.now()
read_imu_raw_data()
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.angular_velocity = Vector3(gyro_x, gyro_y, gyro_z)
imu.linear_acceleration = Vector3(accl_x, accl_y, accl_z)
imu_data_pub.publish(imu)
mag = MagneticField()
mag.header.stamp = current_time
mag.header.frame_id = "imu/mag"
mag.magnetic_field = Vector3(comp_x, comp_y, comp_z)
imu_mag_pub.publish(mag)
r.sleep()
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 get_mag(imu_raw):
mag_out = MagneticField()
mag_out.header.stamp=rospy.Time.now()
mag=Vector3()
mag.x = float(imu_raw[4])
mag.y = float(imu_raw[5])
mag.z = float(imu_raw[6])
mag_out.magnetic_field=mag
return mag_out
def publish_magnetic_field(index):
field = MagneticField()
field.header.frame_id = 'world'
field.header.stamp = rospy.get_rostime()
field.magnetic_field.x = random.uniform(-5, 5)
field.magnetic_field.y = random.uniform(-5, 5)
field.magnetic_field.z = random.uniform(-5, 5)
for i in range(len(field.magnetic_field_covariance)):
field.magnetic_field_covariance[i] = random.uniform(-5, 5)
sensor_publisher[index].publish(field)
def Vn100Pub():
pub = rospy.Publisher('IMUData', Imu, queue_size=1)
pub2 = rospy.Publisher('IMUMag', MagneticField, queue_size=1)
# Initialize the node and name it.
rospy.init_node('IMUpub')
vn = imuthread3.Imuthread(port=rospy.get_param("imu_port"), pause=0.0)
vn.start()
vn.set_data_freq50()
vn.set_qmr_mode()
#vn.set_data_freq10() #to see if this fixes my gps drop out problem
rospy.sleep(3)
msg = Imu()
msg2 = MagneticField()
count = 0
while not rospy.is_shutdown():
if len(vn.lastreadings)>0:
if vn.lastreadings[0] =='VNQMR':
msg.header.seq = count
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'imu'
msg.orientation.x = float(vn.lastreadings[1])
msg.orientation.y = float(vn.lastreadings[2])
msg.orientation.z = float(vn.lastreadings[3])
msg.orientation.w = float(vn.lastreadings[4])
msg.orientation_covariance = [0,0,0,0,0,0,0,0,0]
msg.angular_velocity.x = float(vn.lastreadings[8])
msg.angular_velocity.y = float(vn.lastreadings[9])
msg.angular_velocity.z = float(vn.lastreadings[10])
msg.angular_velocity_covariance = [0,0,0,0,0,0,0,0,0]
msg.linear_acceleration.x = float(vn.lastreadings[11])
msg.linear_acceleration.y = float(vn.lastreadings[12])
msg.linear_acceleration.z = float(vn.lastreadings[13])
msg.linear_acceleration_covariance = [0,0,0,0,0,0,0,0,0]
msg2.header.seq = count
msg2.header.stamp = rospy.Time.now()
msg2.header.frame_id = 'imu'
msg2.magnetic_field.x = float(vn.lastreadings[5])
msg2.magnetic_field.x = float(vn.lastreadings[6])
msg2.magnetic_field.x = float(vn.lastreadings[7])
msg2.magnetic_field_covariance = [0,0,0,0,0,0,0,0,0]
#rospy.loginfo("vn100_pub " + str(msg.header.stamp) + " " + str(msg.orientation.x) + " "+ str(msg.orientation.y) + " "+ str(msg.orientation.z) + " "+ str(msg.orientation.w) + " ")
current_pose_euler = tf.transformations.euler_from_quaternion([
msg.orientation.x,
msg.orientation.y,
msg.orientation.z,
msg.orientation.w
])
pub.publish(msg)
pub2.publish(msg2)
count += 1
#rospy.sleep(1.0/100.0)
vn.kill = True
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 publish_mag_msg(self, msg, **metadata):
mag_msg = MagneticField()
mag_msg.header.stamp = rospy.Time.now()
mag_msg.header.frame_id = 'gps_link'
# sbp reports in microteslas, the MagneticField() msg requires field in teslas
magscale = 1E-6
mag_msg.magnetic_field.x = msg.mag_x * magscale
mag_msg.magnetic_field.y = msg.mag_y * magscale
mag_msg.magnetic_field.z = msg.mag_z * magscale
mag_msg.magnetic_field_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
#publish to /gps/mag/raw
self.pub_mag.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 format_mag(self, mag_data):
"""
Formats magnetometer-message
:param mag_data: magnetometer data
:return: formated message
"""
msg = MagneticField()
msg.header.frame_id = 'imu_mag'
msg.header.stamp = rospy.Time.now()
msg.magnetic_field.x = mag_data[0]
msg.magnetic_field.y = mag_data[1]
msg.magnetic_field.z = mag_data[2]
msg.magnetic_field_covariance = self.mag_cv
return msg
def publish(self, data):
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = data.getOrientation()
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
def __init__(self):
self.accel_mps2 = [0.0] * 3
self.gyro_rps = [0.0] * 3
self.mag_T = [0.0] * 3
self.rpy_rad = [0.0] * 3
self.timestamp_sec = 0.0
self.linear_accel_covariance = 0.098 * 0.098
self.angular_velocity_covariance = 0.012 * 0.012
self.orientation_covariance = 0.035 * 0.035
self.magnetic_field_covariance = 0.000002 * 0.000002
self.publish_data = False
self._MsgData = Imu()
self._MsgPub = rospy.Publisher('/segway/feedback/imu',
Imu,
queue_size=10)
self._MsgData.header.frame_id = 'imu_frame'
self._seq = 0
self._MagMsgData = MagneticField()
self._MagMsgPub = rospy.Publisher('/segway/feedback/mag_feild',
MagneticField,
queue_size=10)
self._MagMsgData.header.frame_id = 'imu_frame'
self._Magseq = 0
def __init__(self):
self.dynamic_pub = rospy.Publisher("/imu/mag",
MagneticField,
queue_size=1)
self.mag_vals_pub = rospy.Publisher("/comp_mag_values",
mag_values,
queue_size=1)
magval = mag_values()
rospy.Subscriber("/imu/mag_raw", MagneticField, self.min_max)
self.corrected = [0, 0, 0]
self.x_out = 0
self.y_out = 0
self.z_out = 0
rate = rospy.Rate(60)
while not rospy.is_shutdown():
mag = MagneticField(header=Header(stamp=rospy.Time.now(),
frame_id='base_link'),
magnetic_field=Vector3(self.corrected[0],
self.corrected[1],
self.corrected[2]),
magnetic_field_covariance=[
0.000, 0.0, 0.0, 0.0, 0.000, 0.0, 0.0, 0.0,
0.000
])
self.dynamic_pub.publish(mag)
rate.sleep()
def publisher():
# accel_x_sum = 0
# accel_y_sum = 0
# accel_z_sum = 0
# gyro_x_sum = 0
# gyro_y_sum = 0
# gyro_z_sum = 0
# for(int i = 0; i < 100; i++){
# accel = mpu9250.readAccel()
# gyro = mpu9250.readGyro()
# accel_x_sum += accel['y']
# accel_y_sum += accel['x']
# accel_z_sum += accel['z']
# gyro_x_sum += gyro['y']
# gyro_y_sum += gyro['x']
# gyro_z_sum += gyro['z']
# }
# accel_x_init = accel_x_sum / 100
# accel_y_init = accel_y_sum / 100
# accel_z_init = accel_z_sum / 100
# gyro_x_init = gyro_x_sum / 100
# gyro_y_init = gyro_y_sum / 100
# gyro_z_init = gyro_z_sum / 100
pub_imu = rospy.Publisher('imu/data_raw', Imu, queue_size=5)
pub_mag = rospy.Publisher('imu/mag', MagneticField, queue_size=5)
rospy.init_node('imu_pub')
imu = Imu()
magnetic = MagneticField()
mpu9250 = MPU9250()
# change
while not rospy.is_shutdown():
accel = mpu9250.readAccel()
gyro = mpu9250.readGyro()
mag = mpu9250.readMagnet()
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = 'base_link'
imu.angular_velocity.x = (gyro['y']) * 0.07 * DPS_TO_RADS
imu.angular_velocity.y = -(gyro['x']) * 0.07 * DPS_TO_RADS
imu.angular_velocity.z = (gyro['z']) * 0.07 * DPS_TO_RADS
imu.linear_acceleration.x = (accel['y']) * 9.8
imu.linear_acceleration.y = -(accel['x']) * 9.8
imu.linear_acceleration.z = (accel['z']) * 9.8
#imu.linear_acceleration.x = 0
magnetic.header.stamp = rospy.Time.now()
magnetic.header.frame_id = 'base_link'
magnetic.magnetic_field.x = mag['y']
magnetic.magnetic_field.y = -mag['x']
magnetic.magnetic_field.z = mag['z']
pub_imu.publish(imu)
pub_mag.publish(magnetic)
rospy.sleep(0.01)
def publish(self, event):
compass_accel = self.compass_accel.read()
compass = compass_accel[1]
Racc = compass_accel[0]
gyro = self.gyro.read()
GyroRate = gyro[0]
GyroRate = [x * self.DEG_TO_RAD for x in GyroRate]
#initial
A_odd = [0, 0]
AvgRate = [0, 0]
A = [0, 0]
Rgyro = [0, 0, 0]
# normalized
Racc_lengh = math.sqrt(Racc[0]**2 + Racc[1]**2 + Racc[2]**2)
Racc_normalized = [x / Racc_lengh for x in Racc]
A_odd[0] = math.atan2(self.Rest[1], self.Rest[2])
A_odd[1] = math.atan2(self.Rest[0], self.Rest[2])
for i in range(2):
AvgRate[i] = (GyroRate[i] + self.GyroRate_odd[i]) / 2
A[i] = A_odd[i] + AvgRate[i] * self.T
Rgyro[0] = math.sin(A[1]) / math.sqrt(1 + ((math.cos(A[1]))**2) *
((math.tan(A[0]))**2))
Rgyro[1] = math.sin(A[0]) / math.sqrt(1 + ((math.cos(A[0]))**2) *
((math.tan(A[1]))**2))
if self.Rest[2] > 0:
RzGyro_sign = 1
else:
RzGyro_sign = -1
Rgyro[2] = RzGyro_sign * math.sqrt(1 - Rgyro[0]**2 - Rgyro[1]**2)
for i in range(3):
self.Rest[i] = (Racc_normalized[i] +
Rgyro[i] * self.wGyro) / (1 + self.wGyro)
position_msg = Imu()
position_msg.linear_acceleration.x = self.Rest[0]
position_msg.linear_acceleration.y = self.Rest[1]
position_msg.linear_acceleration.z = self.Rest[2]
position_msg.header.frame_id = "cheng"
self.pub_position.publish(position_msg)
self.GyroRate_odd = GyroRate
# Put together a magnetometer message
mag_msg = MagneticField()
mag_msg.header.stamp = rospy.Time.now()
mag_msg.magnetic_field.x = compass[0]
mag_msg.magnetic_field.y = compass[1]
mag_msg.magnetic_field.z = compass[2]
self.pub_mag.publish(mag_msg)
def Broadcast_imu_raw(self,lineParts):
partsCount = len(lineParts)
if (partsCount == 11): # $IMUR,timestamp,ax,ay,az,gx,gy,gz,mx,my,mz
pass
try:
time_now = rospy.Time.now()
frame_id = "imu_base"
acc_avel_raw = Imu()
mag_raw = MagneticField()
acc_avel_raw.header.stamp = time_now
acc_avel_raw.header.frame_id = frame_id
mag_raw.header.stamp = time_now
mag_raw.header.frame_id = frame_id
#acc_avel_raw.arduinoTime = int(lineParts[1])
# shpuld be in m/s^2
acc_avel_raw.linear_acceleration.x = float(lineParts[2])*9.81
acc_avel_raw.linear_acceleration.y = float(lineParts[3])*9.81
acc_avel_raw.linear_acceleration.z = float(lineParts[4])*9.81
# should be in rad/sec
acc_avel_raw.angular_velocity.x = math.radians(float(lineParts[5]))
acc_avel_raw.angular_velocity.y = math.radians(float(lineParts[6]))
acc_avel_raw.angular_velocity.z = math.radians(float(lineParts[7]))
# shloud be in Teslas
# if MPU9250 is used, magnetometer orientation mismatch should be corrected to maintain x from acc and gyro as forward
mag_raw.magnetic_field.x = float(lineParts[9])e-7 # x of acc corresponds to y of mag
mag_raw.magnetic_field.y = float(lineParts[8])e-7 # y of acc correspond to x of mag
mag_raw.magnetic_field.z = -float(lineParts[10])e-7 # z of acc is opposite to z of mag
self._acc_vel_pub.publish(acc_avel_raw)
self._mag_pub.publish(mag_raw)
except:
rospy.logwarn("Unexpected error:" + str(sys.exc_info()[0]))
def write_ms25(ms25, i, bag):
utime = ms25[i, 0]
mag_x = ms25[i, 1]
mag_y = ms25[i, 2]
mag_z = ms25[i, 3]
accel_x = ms25[i, 4]
accel_y = ms25[i, 5]
accel_z = ms25[i, 6]
rot_r = ms25[i, 7]
rot_p = ms25[i, 8]
rot_h = ms25[i, 9]
timestamp = microseconds_to_ros_timestamp(utime)
rosimu = Imu()
rosimu.header.stamp = timestamp
rosimu.angular_velocity.x = float(rot_r)
rosimu.angular_velocity.y = float(rot_p)
rosimu.angular_velocity.z = float(rot_h)
rosimu.linear_acceleration.x = float(accel_x)
rosimu.linear_acceleration.y = float(accel_y)
rosimu.linear_acceleration.z = float(accel_z)
bag.write('/ms25_imu', rosimu, t=timestamp)
rosmag = MagneticField()
rosmag.header.stamp = timestamp
rosmag.magnetic_field.x = mag_x
rosmag.magnetic_field.y = mag_y
rosmag.magnetic_field.z = mag_z
bag.write('/ms25_mag', rosmag, t=timestamp)
def __init__(self, i2c=None):
super().__init__('rtf_lis3mdl')
if i2c is None:
self.i2c = busio.I2C(board.SCL, board.SDA)
else:
self.i2c = i2c
self.lis = adafruit_lis3mdl.LIS3MDL(
self.i2c) # 155 Hz, 4 gauss, continuous
self.lis.data_rate = adafruit_lis3mdl.Rate.RATE_155_HZ
self.timer_mag = self.create_timer(1 / 100, self.callback)
self.pub_mag = self.create_publisher(MagneticField, 'magnetic', 10)
self.mags_bias = self.declare_parameter('mags_bias', [0., 0., 0.])
frame_id = self.declare_parameter('frame_id', "base_imu_link").value
self.mag_msg = MagneticField()
self.mag_msg.header.frame_id = frame_id
mc = 0.01
self.mag_msg.magnetic_field_covariance = [
mc, 0.0, 0.0, 0.0, mc, 0.0, 0.0, 0.0, mc
]
self.calibrate = False
def callback_mag(data): global mag_msg, geo_msg mag_msg = MagneticField() geo_msg.header = mag_msg.header mag_msg = data geo_msg.vector = mag_msg.magnetic_field
def publishCustomMessage(imu_data, seq):
imu_message = Imu()
mgn_message = MagneticField()
imu_message.header.seq = seq
imu_message.header.frame_id = "IMU Message"
imu_message.header.stamp = rospy.Time.now()
mgn_message.header.stamp = rospy.Time.now()
mgn_message.header.frame_id = "Magnetic field data"
mgn_message.header.seq = seq
imu_message.linear_acceleration.x = trimValue(imu_data['aclx'])
imu_message.linear_acceleration.y = trimValue(imu_data['acly'])
imu_message.linear_acceleration.z = trimValue(imu_data['aclz'])
imu_message.orientation.w = imu_data['qw']
imu_message.orientation.x = imu_data['qx']
imu_message.orientation.y = imu_data['qy']
imu_message.orientation.z = imu_data['qz']
imu_message.angular_velocity.x = trimValue(imu_data['gyrx'])
imu_message.angular_velocity.y = trimValue(imu_data['gyry'])
imu_message.angular_velocity.z = trimValue(imu_data['gyrz'])
mgn_message.magnetic_field.x = trimValue(imu_data['magx'])
mgn_message.magnetic_field.y = trimValue(imu_data['magy'])
mgn_message.magnetic_field.z = trimValue(imu_data['magz'])
# Publishing magnetic field data and IMU data
imu_publisher = rospy.Publisher('sensor'+'/imu',Imu, queue_size = 5)
mgn_publisher = rospy.Publisher('sensor'+'/mag', MagneticField, queue_size = 5)
imu_publisher.publish(imu_message)
mgn_publisher.publish(mgn_message)
def reset_vars(self):
self.imu_msg = Imu()
self.imu_msg.orientation_covariance = (-1., ) * 9
self.imu_msg.angular_velocity_covariance = (-1., ) * 9
self.imu_msg.linear_acceleration_covariance = (-1., ) * 9
self.pub_imu = False
self.pub_syncout_timeref = False
self.raw_gps_msg = NavSatFix()
self.pub_raw_gps = False
self.pos_gps_msg = NavSatFix()
self.pub_pos_gps = False
self.vel_msg = TwistStamped()
self.pub_vel = False
self.mag_msg = MagneticField()
self.mag_msg.magnetic_field_covariance = (0, ) * 9
self.pub_mag = False
self.temp_msg = Temperature()
self.temp_msg.variance = 0.
self.pub_temp = False
self.press_msg = FluidPressure()
self.press_msg.variance = 0.
self.pub_press = False
self.anin1_msg = UInt16()
self.pub_anin1 = False
self.anin2_msg = UInt16()
self.pub_anin2 = False
self.ecef_msg = PointStamped()
self.pub_ecef = False
self.pub_diag = False
def publish_mag_state_msg(publisher):
mag_state_msg = MagneticField()
mag_state_msg.header.stamp = rospy.get_rostime()
mag_state_msg.magnetic_field.x = mag.GetMagRawData()[0]
mag_state_msg.magnetic_field.y = mag.GetMagRawData()[1]
mag_state_msg.magnetic_field.z = mag.GetMagRawData()[2]
publisher.publish(mag_state_msg)
def pub_data(self, ax, ay, az, gx, gy, gz, mx, my, mz):
self.processValues()
imu_topic = Imu()
mag_topic = MagneticField()
# self.rate = rospy.Rate(self.rate_hz)
while not rospy.is_shutdown():
self.processValues()
imu_topic.header.stamp = rospy.Time.now()
imu_topic.header.frame_id = "map"
imu_topic.angular_velocity.x = self.gx
imu_topic.angular_velocity.y = self.gy
imu_topic.angular_velocity.z = self.gz
imu_topic.linear_acceleration.x = self.ax
imu_topic.linear_acceleration.y = self.ay
imu_topic.linear_acceleration.z = self.az
self.imu_raw_pub.publish(imu_topic)
mag_topic.header.stamp = rospy.Time.now()
mag_topic.header.frame_id = "map"
mag_topic.magnetic_field.x = self.mx
mag_topic.magnetic_field.y = self.my
mag_topic.magnetic_field.z = self.mz
self.mag_raw_pub.publish(mag_topic)
self.rate.sleep()
def publish(self, event):
compass_accel = self.compass_accel.read()
compass = compass_accel[0:3]
accel = compass_accel[3:6]
gyro = self.gyro.read()
# Put together an IMU message
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.orientation_covariance[0] = -1
imu_msg.angular_velocity = gyro[0] * DEG_TO_RAD
imu_msg.angular_velocity = gyro[1] * DEG_TO_RAD
imu_msg.angular_velocity = gyro[2] * DEG_TO_RAD
imu_msg.linear_acceleration.x = accel[0] * G
imu_msg.linear_acceleration.y = accel[1] * G
imu_msg.linear_acceleration.z = accel[2] * G
self.pub_imu.publish(imu_msg)
# Put together a magnetometer message
mag_msg = MagneticField()
mag_msg.header.stamp = rospy.Time.now()
mag_msg.magnetic_field.x = compass[0]
mag_msg.magnetic_field.y = compass[1]
mag_msg.magnetic_field.z = compass[2]
self.pub_mag.publish(mag_msg)
def _log_data_log2(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
msg = Temperature()
# ToDo: it would be better to convert from timestamp to rospy time
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.tf_prefix + "/base_link"
# measured in degC
msg.temperature = data["baro.temp"]
self._pubTemp.publish(msg)
# ToDo: it would be better to convert from timestamp to rospy time
msg = MagneticField()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.tf_prefix + "/base_link"
# measured in Tesla
msg.magnetic_field.x = data["mag.x"]
msg.magnetic_field.y = data["mag.y"]
msg.magnetic_field.z = data["mag.z"]
self._pubMag.publish(msg)
msg = Float32()
# hPa (=mbar)
msg.data = data["baro.pressure"]
self._pubPressure.publish(msg)
# V
msg.data = data["pm.vbat"]
self._pubBattery.publish(msg)
# Test
msg.data = data["test.tval"]
self._pubTest.publish(msg)
def __init__(self, _threaded_mode = False):
self.data_dir = "/tmp/BHG_DATA"
self.csv_filename = "default_data.csv"
self.datetimeData = ""
self.is_recording = False
self.rel_alt = Altitude()
self.gps_fix = NavSatFix()
self.odom = Odometry()
self.imu_mag = MagneticField()
self.imu_data = Imu()
self.vel_gps = TwistStamped()
self.temp_imu = Temperature()
self.trigtime = "TrigTimeNotSet"
self.arduino_dev = rospy.get_param('~arduino_dev', '/dev/ttyACM0')
self.arduino_timeout = rospy.get_param('~arduino_timeout', 2)
self.ardunio_ser = ''
self.pub = rospy.Publisher('trig_timer', String, queue_size=10)
rospy.Subscriber('/directory', String, self.directory_callback)
rospy.Subscriber("/record", Bool, self.record_callback)
rospy.Subscriber("/mavros/altitude", Altitude, self.alt_cb) # change to /global_position/rel_alt Float64
rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.gps_cb)
rospy.Subscriber("/mavros/global_position/local", Odometry, self.odom_cb)
rospy.Subscriber("/mavros/imu/mag", MagneticField, self.mag_cb)
rospy.Subscriber("/mavros/imu/data", Imu, self.imu_cb)
rospy.Subscriber("/mavros/global_position/raw/gps_vel", TwistStamped, self.vel_cb)
rospy.Subscriber("/mavros/imu/temperature_imu", Temperature, self.temp_cb)
def do_parse(ext_type, ext_len, ext_data):
# 将ext_data 封装成完整的数据帧
ext_data.insert(0, ext_len)
ext_data.insert(0, ext_type)
ext_data.insert(0, 0xfa)
ext_data.insert(0, 0xce)
# CE FA 03 18 76 0E 5C 03 F0 FF 88 3C E0 FF A2 00 84 00 00 A2 FF E0 00 84 F4 01 20 03
# 根据数据帧的类型的类型来做对应的解析 0x01 线速度 0x02 电池
if ext_type == 0x03:
# 对数据进行拆包
# 温度
temperature = struct.unpack('h', bytearray(ext_data[4:6]))[0]
# 加速度
ax = struct.unpack('h', bytearray(ext_data[6:8]))[0]
ay = struct.unpack('h', bytearray(ext_data[8:10]))[0]
az = struct.unpack('h', bytearray(ext_data[10:12]))[0]
# 角速度
gx = struct.unpack('h', bytearray(ext_data[12:14]))[0]
gy = struct.unpack('h', bytearray(ext_data[14:16]))[0]
gz = struct.unpack('h', bytearray(ext_data[16:18]))[0]
# 地磁
mx = struct.unpack('h', bytearray(ext_data[18:20]))[0]
my = struct.unpack('h', bytearray(ext_data[20:22]))[0]
mz = struct.unpack('h', bytearray(ext_data[22:24]))[0]
# 速度
velocity = struct.unpack('h', bytearray(ext_data[24:26]))[0]
angular = struct.unpack('h', bytearray(ext_data[26:28]))[0]
# 发布陀螺仪的数据
imu = Imu()
# 根据芯片手册设定缩放系数
accel_ratio = 1 / 16384.0
imu.linear_acceleration.x = ax * accel_ratio
imu.linear_acceleration.y = ay * accel_ratio
imu.linear_acceleration.z = az * accel_ratio
# °/s 转成弧度/s
gyro_ratio = 1 / 65.5 / (180 / 3.1415926)
imu.angular_velocity.x = gx * gyro_ratio
imu.angular_velocity.y = gy * gyro_ratio
imu.angular_velocity.z = gz * gyro_ratio
imuPublisher.publish(imu)
# 发布地磁的数据
mag = MagneticField()
mag.magnetic_field.x = mx
mag.magnetic_field.y = my
mag.magnetic_field.z = mz
magPublisher.publish(mag)
# print(velocity,angular)
# 将小车当前的线速度和角速度发布出去
twist = Twist()
twist.linear.x = velocity / 1000.0
twist.angular.z = angular / 1000.0
velPublisher.publish(twist)
def main():
rospy.init_node('imu_rawdata_publisher', anonymous=True)
rate = rospy.Rate(100) # 5hz
rospy.loginfo("In main function")
pubs_imu = rospy.Publisher('/imu/data_raw', Imu, queue_size=15)
pubs_magnetometer = rospy.Publisher('/imu/mag',
MagneticField,
queue_size=15)
imu_read = Imu()
magnetometer_read = MagneticField()
gyro_x, gyro_y, gyro_z, count = 0, 0, 0, 0
while not rospy.is_shutdown():
mpu = mpu6050(0x68)
accel_data = mpu.get_accel_data()
gyro_data = mpu.get_gyro_data()
imu_read.header.stamp = rospy.Time.now()
imu_read.header.frame_id = "body_frame"
### Accelerometer data
imu_read.linear_acceleration.x = accel_data['x']
imu_read.linear_acceleration.y = accel_data['y']
imu_read.linear_acceleration.z = accel_data['z']
#### Gyrometer data
gyro_x = gyro_data['x'] * (pi / 180)
gyro_y = gyro_data['y'] * (pi / 180)
gyro_z = gyro_data['z'] * (pi / 180)
if abs(gyro_x) < 0.2:
imu_read.angular_velocity.x = 0.0
else:
imu_read.angular_velocity.x = gyro_x
if abs(gyro_y) < 0.2:
imu_read.angular_velocity.y = 0.0
else:
imu_read.angular_velocity.y = gyro_y
if abs(gyro_z) < 0.2:
imu_read.angular_velocity.z = 0.0
else:
imu_read.angular_velocity.z = gyro_z
'''##### Magnetometer data
magnetometer_read.header.stamp = rospy.Time.now();
magnetometer_read.header.frame_id = "body_frame";
magnetometer_read.magnetic_field.x =
magnetometer_read.magnetic_field.y =
magnetometer_read.magnetic_field.z = '''
pubs_imu.publish(imu_read)
#pubs_magnetometer.publish(magnetometer_read)
rate.sleep()
rate.sleep()
rospy.loginfo("Node is shutting down")
def _sensor_callback(self, accelerometer, magnometer, gyrometer):
if rospy.is_shutdown():
return
end_ts = rospy.get_time()
for i in range(0, len(accelerometer)):
imu = Imu()
magnetic = MagneticField()
# Calculate timestamp for reading
samples_behind = (len(accelerometer) - 1) - i
samples_per_sec = len(accelerometer) / 50.
stamp = rospy.Time.from_sec(end_ts -
samples_behind * samples_per_sec)
imu.header.stamp = stamp
imu.header.frame_id = "imu"
magnetic.header.stamp = stamp
imu.linear_acceleration.x = accelerometer[i][0]
imu.linear_acceleration.y = accelerometer[i][1]
imu.linear_acceleration.z = accelerometer[i][2]
imu.linear_acceleration_covariance = self.linear_acceleration_covariance
imu.angular_velocity.x = gyrometer[i][0]
imu.angular_velocity.y = gyrometer[i][1]
imu.angular_velocity.z = gyrometer[i][2]
imu.angular_velocity_covariance = self.angular_velocity_covariance
magnetic.magnetic_field.x = magnometer[i][0]
magnetic.magnetic_field.y = magnometer[i][1]
magnetic.magnetic_field.z = magnometer[i][2]
magnetic.magnetic_field_covariance = self.magnetic_field_covariance
try:
self._tf_broadcaster.sendTransform(
(0, 0, 0), (0., 0., 0., 1.), imu.header.stamp, "base_link",
"imu")
self._publisher_imu.publish(imu)
self._publisher_magnetic.publish(magnetic)
except rospy.ROSException:
pass
def msg_template(self):
msg_imu = Imu();
msg_mag = MagneticField();
msg_imu.header.frame_id = "robocape";
msg_mag.header.frame_id = "robocape_mag";
msg_imu.linear_acceleration_covariance = (0.1, 0.0, 0.0,
0.0, 0.1, 0.0,
0.0, 0.0, 0.1);
msg_imu.angular_velocity_covariance = (1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
msg_imu.orientation_covariance = (-1.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0);
msg_mag.magnetic_field_covariance = (10.0, 0.0, 0.0,
0.0, 10.0, 0.0,
0.0, 0.0, 10.0);
return (msg_imu, msg_mag);
def __init__(self):
magx = [1,2,3,4]
magy = [1,2,3,4]
magz = [1,2,3,4]
self.DEVICE_ID = rospy.get_param('~device_id','/dev/serial/by-id/usb-Teensyduino_USB_Serial_1558050-if00')
self.mag_pub = rospy.Publisher('imu/mag', MagneticField, queue_size = 1)
mag = MagneticField()
ser = serial.Serial(self.DEVICE_ID)
magn = [0.0,0.0,0.0,0.0]
rate = rospy.Rate(70)
while not rospy.is_shutdown():
ser.write('r')
line = ser.readline()
magn = line.split("," , 4)
#rospy.loginfo(magn)
if magn[0] == 'XYZ':
x_raw = float(magn[1]) /100.0
y_raw = float(magn[2]) /100.0
z_raw = float(magn[3]) /100.0
raw = np.array([x_raw, y_raw, z_raw])
scale = np.array([[1.0020497985386805, -0.06914522912368153, 0.11962132522309941],
[-0.06914522912368157, 1.0741014035679903, 0.22518913831628748],
[0.11962132522309966, 0.22518913831628748, 0.9984945224678732]])
bias = np.array([-0.16542780063806176, -0.04899507311891892, -0.10150383298877998])
corrected = np.dot(raw-bias, scale)
mag.header.stamp = rospy.Time.now()
mag.header.frame_id = '/base_link' # i.e. '/odom'
mag.magnetic_field.x = corrected[0]
mag.magnetic_field.y = corrected[1]
mag.magnetic_field.z = corrected[2]
mag.magnetic_field_covariance=[.01, 0.0, 0.0,
0.0, .01, 0.0,
0.0, 0.0, .01]
self.mag_pub.publish(mag)
#heading = math.atan2(mag.magnetic_field.y,mag.magnetic_field.x)
'''for i in range(3,0,-1):
magx[i] = magx[i-1]
magx[0] = mag.magnetic_field.x
for i in range(3,0,-1):
magy[i] = magy[i-1]
magy[0] = mag.magnetic_field.y
for i in range(3,0,-1):
magz[i] = magz[i-1]
magz[0] = mag.magnetic_field.z
if self.checkEqual(magx) == True:
rospy.logerr("MAGX value is static: %f" % magx[0])
if self.checkEqual(magy) == True:
rospy.logerr("MAGY value is static: %f" % magy[0])
if self.checkEqual(magz) == True:
rospy.logerr("MAGZ value is static: %f" % magz[0])'''
rate.sleep()
def __init__(self):
self.DEVICE_ID = rospy.get_param('~device_id','/dev/serial/by-id/usb-SparkFun_SFE_9DOF-D21-if00')
self.imu_pub = rospy.Publisher('imu/camera_tilt', Imu, queue_size = 1)
self.mag_pub = rospy.Publisher("/imu/mag_camera", MagneticField, queue_size=1)
self.roll_pub = rospy.Publisher("camera_roll", Float64, queue_size = 1)
self.pitch_pub = rospy.Publisher("camera_pitch", Float64, queue_size = 1)
self.yaw_pub = rospy.Publisher("camera_yaw", Float64, queue_size = 1)
#self.mag_pub = rospy.Publisher('imu/camera_mag', MagneticField, queue_size = 1)
head1 = 0
head2 = 0
head3 = 0
head4 = 0
head5 = 0
head6 = 0
imu = Imu()
mag = MagneticField()
ser = serial.Serial(self.DEVICE_ID)
gyro = [0.0,0.0,0.0,0.0]
magn = [0.0,0.0,0.0,0.0]
quat = [0.0,1.0,0.0,0.0,0.0]
rate = rospy.Rate(130)
while not rospy.is_shutdown():
line = ser.readline()
accel = line.split("," , 4)
if accel[0] == 'A':
line = ser.readline()
gyro = line.split("," , 4)
line = ser.readline()
magn = line.split("," , 4)
line = ser.readline()
quat = line.split("," , 5)
line = ser.readline()
rpy = line.split("," , 4)
magneticsphere = ([1.0974375136949293, 0.008593465160122918, 0.0003032211129407881],
[0.008593465160122857, 1.1120651652148803, 0.8271491584066613],
[0.00030322111294077105, 0.0926004380061327, 0.5424720769663267])
bias = ([-0.021621641870448665],
[-0.10661356710756408],
[0.4377993330407842])
raw_values = ([float(magn[1])],
[float(magn[2])],
[float(magn[3])])
shift = ([raw_values[0][0] - bias[0][0]],
[raw_values[1][0] - bias[1][0]],
[raw_values[2][0] - bias[2][0]])
magneticfield = np.dot(magneticsphere, shift)
#rospy.loginfo(magneticfield)
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = '/base_link' # i.e. '/odom'
#pres.child_frame_id = self.child_frame_id # i.e. '/base_footprint'
imu.angular_velocity.x = float(gyro[1])
imu.angular_velocity.y = float(gyro[2])
imu.angular_velocity.z = float(gyro[3])
imu.angular_velocity_covariance=[.01, 0.0, 0.0,
0.0, .01, 0.0,
0.0, 0.0, .01]
imu.linear_acceleration.x = float(accel[1])
imu.linear_acceleration.y = float(accel[2])
imu.linear_acceleration.z = float(accel[3])
imu.linear_acceleration_covariance = [.01, 0.0, 0.0,
0.0, .01, 0.0,
0.0, 0.0, .01]
mag.header.stamp = rospy.Time.now()
mag.header.frame_id = '/base_link' # i.e. '/odom'
mag.magnetic_field.x = float(magn[1])/100.0
mag.magnetic_field.y = float(magn[2])/100.0
mag.magnetic_field.z = float(magn[3])/100.0
mag.magnetic_field_covariance = [.01, 0.0, 0.0,
0.0, .01, 0.0,
0.0, 0.0, .01]
imu.orientation.w = float(quat[1])
imu.orientation.x = float(quat[2])
imu.orientation.y = float(quat[3])
imu.orientation.z = float(quat[4])
imu.orientation_covariance = [.01, 0.0, 0.0,
0.0, .01, 0.0,
0.0, 0.0, .01]
#magn[2] = -float(magn[2])
#heading = math.atan2(float(magn[2]),float(magn[1]))*(180/math.pi)
#heading = math.atan2(float(magn[2]),float(magn[1]))
#if heading < 0:
# heading += 360
#rospy.loginfo(head[1])
self.roll_pub.publish(float(rpy[1]))
self.pitch_pub.publish(float(rpy[2]))
self.pitch_pub.publish(float(rpy[3]))
self.imu_pub.publish(imu)
self.mag_pub.publish(mag)
rate.sleep()
def __init__(self):
self.DEVICE_ID = rospy.get_param('~device_id','/dev/serial/by-id/usb-SparkFun_SFE_9DOF-D21-if00')
self.imu_pub = rospy.Publisher('imu/camera_tilt', Imu, queue_size = 1)
self.mag_pub = rospy.Publisher('imu/camera_mag', MagneticField, queue_size = 1)
imu = Imu()
mag = MagneticField()
ser = serial.Serial(self.DEVICE_ID)
gyro = [0.0,0.0,0.0,0.0]
magn = [0.0,0.0,0.0,0.0]
quat = [0.0,1.0,0.0,0.0,0.0]
rate = rospy.Rate(130)
while not rospy.is_shutdown():
line = ser.readline()
accel = line.split("," , 4)
if accel[0] == 'A':
line = ser.readline()
gyro = line.split("," , 4)
line = ser.readline()
magn = line.split("," , 4)
line = ser.readline()
quat = line.split("," , 5)
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = '/base_link' # i.e. '/odom'
#pres.child_frame_id = self.child_frame_id # i.e. '/base_footprint'
imu.angular_velocity.x = float(gyro[1])
imu.angular_velocity.y = float(gyro[2])
imu.angular_velocity.z = float(gyro[3])
imu.angular_velocity_covariance=[.001, 0.0, 0.0,
0.0, .001, 0.0,
0.0, 0.0, .001]
imu.linear_acceleration.x = float(accel[1])
imu.linear_acceleration.y = float(accel[2])
imu.linear_acceleration.z = float(accel[3])
imu.linear_acceleration_covariance = [.01, 0.0, 0.0,
0.0, .01, 0.0,
0.0, 0.0, .01]
'''imu.orientation.w = float(quat[1])
imu.orientation.x = float(quat[2])
imu.orientation.y = float(quat[3])
imu.orientation.z = float(quat[4])
imu.orientation_covariance = [.01, 0.0, 0.0,
0.0, .01, 0.0,
0.0, 0.0, .01] '''
mag.header.stamp = rospy.Time.now()
mag.header.frame_id = '/base_link' # i.e. '/odom'
#pres.child_frame_id = self.child_frame_id # i.e. '/base_footprint'
mag.magnetic_field.x = float(magn[1])
mag.magnetic_field.y = float(magn[2])
mag.magnetic_field.z = float(magn[3])
mag.magnetic_field_covariance=[.001, 0.0, 0.0,
0.0, .001, 0.0,
0.0, 0.0, .001]
self.imu_pub.publish(imu)
self.mag_pub.publish(mag)
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)
