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 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 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 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_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 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 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 __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 __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 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 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 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 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 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 _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 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 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 __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 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 __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 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 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")
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 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 __init__(self):
self._pub_imu = rospy.Publisher(self.IMU_DEST_TOPIC_QUAT, Imu, queue_size=50)
self._pub_mag = rospy.Publisher(self.IMU_DEST_TOPIC_MAG, MagneticField, queue_size=50)
self._current_imu_msg = Imu()
self._current_mag_msg = MagneticField()
self._serial_port = None
self._serial = None
def _send_mag(self, now, x, y, z):
mag_msg = MagneticField()
mag_msg.header.stamp = now
mag_msg.header.frame_id = self._tf_prefix + '/imu_link'
mag_msg.magnetic_field.x = x
mag_msg.magnetic_field.y = y
mag_msg.magnetic_field.z = z
self._mag_pub.publish(mag_msg)
def _publish_current_msg(self):
self._current_imu_msg.header.stamp = rospy.Time.now()
self._current_mag_msg.header.stamp = rospy.Time.now()
self._current_imu_msg.header.frame_id = "imu_link"
self._current_mag_msg.header.frame_id = "imu_link"
self._pub_imu.publish(self._current_imu_msg)
self._current_imu_msg = Imu()
self._pub_mag.publish(self._current_mag_msg)
self._current_mag_msg = MagneticField()
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 imu_pub():
rate = rospy.Rate(100)
no_covariance_matrix = (0, 0, 0, 0, 0, 0, 0, 0, 0)
unknown_matrix = (-1, 0, 0, 0, 0, 0, 0, 0, 0)
unknown_quat = Quaternion(0, 0, 0, 0)
yaw = 0
imu = navio.mpu9250.MPU9250()
if imu.testConnection():
print "Connection OK"
else:
sys.exit("Connection failed")
accel_gyro_pub = rospy.Publisher("imu/data_raw", Imu)
mag_pub = rospy.Publisher("imu/mag", MagneticField)
yaw_pub = rospy.Publisher("yaw", Yaw)
imu.initialize()
rospy.sleep(1.0)
last_time = rospy.get_time()
while not rospy.is_shutdown():
m9a, m9g, m9m = imu.getMotion9()
m9m_corrected = (
m9m[1], m9m[0], -m9m[2]
) # the magnetometer is oriented differently in the package
accel_vec = Vector3(*m9a)
angvel_vec = Vector3(-m9g[0], -m9g[1], m9g[2])
mag_vec = Vector3(*m9m_corrected)
accel_gyro_header = std_msgs.msg.Header()
accel_gyro_header.stamp = rospy.Time.now()
accel_gyro_header.frame_id = "base_link"
mag_header = std_msgs.msg.Header()
mag_header.stamp = rospy.Time.now()
mag_header.frame_id = "base_link"
current_time = rospy.get_time()
yaw -= m9g[2] * (current_time - last_time)
last_time = current_time
accel_gyro_pub.publish(
Imu(accel_gyro_header, unknown_quat, unknown_matrix, angvel_vec,
no_covariance_matrix, accel_vec, no_covariance_matrix))
mag_pub.publish(
MagneticField(mag_header, mag_vec, no_covariance_matrix))
yaw_pub.publish(Yaw(yaw))
rate.sleep()
def unbatchMagneticField(msg):
nbFrames = len(msg.stamps)
for i in range(nbFrames):
m = MagneticField()
m.header.seq = nbFrames * msg.header.seq + i
m.header.frame_id = msg.header.frame_id
m.header.stamp = msg.stamps[i]
m.magnetic_field.x = msg.magnetic_fields[i].x
m.magnetic_field.y = msg.magnetic_fields[i].y
m.magnetic_field.z = msg.magnetic_fields[i].z
yield m
