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 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 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 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 _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 = (10.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0,
0.0, 10.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 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 magnetometer():
i2c = board.I2C() # uses board.SCL and board.SDA
sensor = adafruit_lis3mdl.LIS3MDL(i2c)
rospy.loginfo("Initializing mag publisher")
mag_pub = rospy.Publisher('/mag', MagneticField, queue_size=5)
rospy.loginfo("Publishing MagneticField at: " + mag_pub.resolved_name)
rospy.init_node('mag_node')
rate = rospy.Rate(10) # 50hz
while not rospy.is_shutdown():
mag_x, mag_y, mag_z = sensor.magnetic
mag_x, mag_y, mag_z = calibrate(mag_x, mag_y, mag_z)
rospy.loginfo('X:{0:10.2f}, Y:{1:10.2f}, Z:{2:10.2f} uT'.format(mag_x, mag_y, mag_z))
rospy.loginfo("")
mag = MagneticField()
mag.header.stamp = rospy.Time.now()
mag.header.frame_id = 'LIS3MDL Triple-axis Magnetometer'
mag.magnetic_field_covariance = 0
mag.magnetic_field = create_vector3(mag_x, mag_y, mag_z)
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 publishIMUSensorData(pitch, roll, yaw, angVelx, angVely, angVelz, linAccx, linAccy, linAccz, temp, press, alt,
magX, magY, magZ):
# Initialize node
rospy.init_node('em7180', anonymous=False)
# Publisher
imuSensorPublisher = rospy.Publisher('sensors/imus/em7180', Ximu, queue_size=10)
magneticFieldPublisher = rospy.Publisher('imu/mag', MagneticField, queue_size=10)
rate = rospy.Rate(10)
# Initilize objects
magneticVector = MagneticField()
theXimu = Ximu()
# Make a struct that contains the IMU data
imuMsg = theXimu.imu
# Make a struct that contains the Temperature data
tempMsg = theXimu.temperature
# Make a struct that contains the Pressure data
pressMsg = theXimu.pressure
# Make a struct that contains the Altitude data
altMsg = theXimu.altitude
# Set Temperature variables
tempMsg.header.stamp = rospy.Time.now()
tempMsg.temperature = temp
tempMsg.variance = 0
# Set Pressure variables
pressMsg.header.stamp = rospy.Time.now()
pressMsg.fluid_pressure = press
pressMsg.variance = 0
# Set Altitude variables
altMsg = alt
# Covariance matricies
# imuMsg.orientation_covariance=[0,0,0,0,0,0,0,0,0] # Place in the covariance matrix here
# imuMsg.angular_velocity_covariance=[0,0,0,0,0,0,0,0,0] # Place in the covariance matrix here
# imuMsg.linear_acceleration_covariance=[0,0,0,0,0,0,0,0,0] # Same here
imuMsg.orientation_covariance[0] = -1
imuMsg.angular_velocity_covariance[0] = -1
imuMsg.linear_acceleration_covariance[0] = -1
# Place sensor data from IMU to message
imuMsg.header.stamp = rospy.Time.now()
imuMsg.linear_acceleration.x = linAccx
imuMsg.linear_acceleration.y = linAccy
imuMsg.linear_acceleration.z = linAccz
imuMsg.angular_velocity.x = angVelx
imuMsg.angular_velocity.y = angVely
imuMsg.angular_velocity.z = angVelz
imuMsg.orientation.x = roll
imuMsg.orientation.y = pitch
imuMsg.orientation.z = yaw
# Compile custom message
theXimu.imu = imuMsg
theXimu.temperature = tempMsg
theXimu.pressure = pressMsg
theXimu.altitude = altMsg
# Magnetic field vector
magneticVector.header.stamp = rospy.Time.now()
magneticVector.header.frame_id = "magnetometer_link"
magneticVector.magnetic_field.x = magX
magneticVector.magnetic_field.y = magY
magneticVector.magnetic_field.z = magZ
magneticVector.magnetic_field_covariance = [700, 0, 0, 0, 700, 0, 0, 0, 700]
imuSensorPublisher.publish(theXimu)
magneticFieldPublisher.publish(magneticVector)
# Info to ros_console and screen
rospy.loginfo("Publishing sensor data from IMU")
# Sleep in order to maintain the rate
rate.sleep()
#read the data - call the get imu measurement data
readback = openimu_wrp.readimu()
#publish the data m/s^2 and convert deg/s to rad/s
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.frame_id = frame_id
imu_msg.header.seq = seq
imu_msg.orientation_covariance[0] = -1
imu_msg.linear_acceleration.x = readback[1]
imu_msg.linear_acceleration.y = readback[2]
imu_msg.linear_acceleration.z = readback[3]
imu_msg.linear_acceleration_covariance[0] = -1
imu_msg.angular_velocity.x = readback[4] * convert_rads
imu_msg.angular_velocity.y = readback[5] * convert_rads
imu_msg.angular_velocity.z = readback[6] * convert_rads
imu_msg.angular_velocity_covariance[0] = -1
pub_imu.publish(imu_msg)
# Publish magnetometer data - convert Gauss to Tesla
mag_msg.header.stamp = imu_msg.header.stamp
mag_msg.header.frame_id = frame_id
mag_msg.header.seq = seq
mag_msg.magnetic_field.x = readback[7] * convert_tesla
mag_msg.magnetic_field.y = readback[8] * convert_tesla
mag_msg.magnetic_field.z = readback[9] * convert_tesla
mag_msg.magnetic_field_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
pub_mag.publish(mag_msg)
seq = seq + 1
rate.sleep()
openimu_wrp.close() # exit
# so set all covariances the same.
imuMsg.orientation_covariance = [0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025]
# Angular velocity covariance estimation:
# Observed gyro noise: 4 counts => 0.28 degrees/sec
# nonlinearity spec: 0.2% of full scale => 8 degrees/sec = 0.14 rad/sec
# Choosing the larger (0.14) as std dev, variance = 0.14^2 ~= 0.02
imuMsg.angular_velocity_covariance = [0.02, 0, 0, 0, 0.02, 0, 0, 0, 0.02]
# linear acceleration covariance estimation:
# observed acceleration noise: 5 counts => 20milli-G's ~= 0.2m/s^2
# nonliniarity spec: 0.5% of full scale => 0.2m/s^2
# Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
imuMsg.linear_acceleration_covariance = [0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.04]
magMsg.magnetic_field_covariance = [0.0, 0, 0, 0, 0.0, 0, 0, 0, 0.0]
default_port = '/dev/ttyUSB0'
port = rospy.get_param('~port', default_port)
#read calibration parameters
port = rospy.get_param('~port', default_port)
#accelerometer
accel_x_min = rospy.get_param('~accel_x_min', -250.0)
accel_x_max = rospy.get_param('~accel_x_max', 250.0)
accel_y_min = rospy.get_param('~accel_y_min', -250.0)
accel_y_max = rospy.get_param('~accel_y_max', 250.0)
accel_z_min = rospy.get_param('~accel_z_min', -250.0)
accel_z_max = rospy.get_param('~accel_z_max', 250.0)
tempMsg.variance = 0
# Set Altitude variables
altMsg = altitude
# publish message
temp_pub.publish(tempMsg)
pressure_pub.publish(pressMsg)
alt_pub.publish(altMsg)
if em7180.gotMagnetometer():
mx, my, mz = em7180.readMagnetometer()
# Magnetic field vector
magneticVector = MagneticField()
magneticVector.header.stamp = rospy.Time.now()
magneticVector.header.frame_id = "imu_link"
magneticVector.magnetic_field.x = mx
magneticVector.magnetic_field.y = my
magneticVector.magnetic_field.z = mz
magneticVector.magnetic_field_covariance = [2, 0, 0, 0, 2, 0, 0, 0, 4]
# publish message
mag_pub.publish(magneticVector)
# Info to ros_console and screen
#rospy.loginfo("Publishing sensor data from IMU")
rate.sleep()
def parse_imu(self, data):
# {
# 'acc': [0.19589658081531525, -0.18210914731025696, 9.45071792602539],
# 'gyro': [0.008880757726728916, 0.012528236024081707, -0.021537888795137405],
# 'mag': [-0.00010609201126499102, -0.00013743649469688535, -6.562667840626091e-05],
# 'quat': [0.31689611077308655, -0.01368665136396885, 0.002746865153312683, -0.9483575224876404],
# 'rpy': [-2.795783042907715, -1.3877670764923096, -143.03599548339844],
# 'ts': 6539060
# }
msg = Imu()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.frame
msg.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
mag_msg = MagneticField()
mag_msg.header.stamp = rospy.Time.now()
mag_msg.header.frame_id = self.frame
mag_msg.magnetic_field_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
try:
msg.angular_velocity.x = data['gyro'][0]
msg.angular_velocity.y = -data['gyro'][1]
msg.angular_velocity.z = -data['gyro'][2]
msg.angular_velocity_covariance[0] = 0.02
msg.angular_velocity_covariance[4] = 0.02
msg.angular_velocity_covariance[8] = 0.02
msg.linear_acceleration.x = -data['acc'][0]
msg.linear_acceleration.y = data['acc'][1]
msg.linear_acceleration.z = data['acc'][2]
msg.linear_acceleration_covariance[0] = 0.04
msg.linear_acceleration_covariance[4] = 0.04
msg.linear_acceleration_covariance[8] = 0.04
q = [
data['quat'][0], data['quat'][1], data['quat'][2],
data['quat'][3]
]
msg.orientation.x = q[1]
msg.orientation.y = q[2]
msg.orientation.z = q[3]
msg.orientation.w = q[0]
msg.orientation_covariance[0] = 0.0025
msg.orientation_covariance[4] = 0.0025
msg.orientation_covariance[8] = 0.0025
mag_msg.magnetic_field.x = data['mag'][0]
mag_msg.magnetic_field.y = -data['mag'][1]
mag_msg.magnetic_field.z = -data['mag'][2]
mag_msg.magnetic_field_covariance[0] = 0.0
mag_msg.magnetic_field_covariance[4] = 0.0
mag_msg.magnetic_field_covariance[8] = 0.0
except KeyError as e:
rospy.logwarn('Unable to IMU parse message ' + str(e))
print(data)
except ValueError as e:
rospy.logwarn('Unable to IMU parse message ' + str(e))
print(data)
else:
# No exception
self.imu_pub.publish(msg)
self.magnetic_field_pub.publish(mag_msg)
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 get_sensor_data(self):
"""Read IMU data from the sensor, parse and publish."""
# Initialize ROS msgs
imu_raw_msg = Imu()
imu_msg = Imu()
mag_msg = MagneticField()
temp_msg = Temperature()
# read from sensor
buf = self.con.receive(registers.BNO055_ACCEL_DATA_X_LSB_ADDR, 45)
# Publish raw data
imu_raw_msg.header.stamp = self.node.get_clock().now().to_msg()
imu_raw_msg.header.frame_id = self.param.frame_id.value
# TODO: do headers need sequence counters now?
# imu_raw_msg.header.seq = seq
# TODO: make this an option to publish?
imu_raw_msg.orientation_covariance = [
self.param.variance_orientation.value[0], 0.0, 0.0, 0.0,
self.param.variance_orientation.value[1], 0.0, 0.0, 0.0,
self.param.variance_orientation.value[2]
]
imu_raw_msg.linear_acceleration.x = \
self.unpackBytesToFloat(buf[0], buf[1]) / self.param.acc_factor.value
imu_raw_msg.linear_acceleration.y = \
self.unpackBytesToFloat(buf[2], buf[3]) / self.param.acc_factor.value
imu_raw_msg.linear_acceleration.z = \
self.unpackBytesToFloat(buf[4], buf[5]) / self.param.acc_factor.value
imu_raw_msg.linear_acceleration_covariance = [
self.param.variance_acc.value[0], 0.0, 0.0, 0.0,
self.param.variance_acc.value[1], 0.0, 0.0, 0.0,
self.param.variance_acc.value[2]
]
imu_raw_msg.angular_velocity.x = \
self.unpackBytesToFloat(buf[12], buf[13]) / self.param.gyr_factor.value
imu_raw_msg.angular_velocity.y = \
self.unpackBytesToFloat(buf[14], buf[15]) / self.param.gyr_factor.value
imu_raw_msg.angular_velocity.z = \
self.unpackBytesToFloat(buf[16], buf[17]) / self.param.gyr_factor.value
imu_raw_msg.angular_velocity_covariance = [
self.param.variance_angular_vel.value[0], 0.0, 0.0, 0.0,
self.param.variance_angular_vel.value[1], 0.0, 0.0, 0.0,
self.param.variance_angular_vel.value[2]
]
# node.get_logger().info('Publishing imu message')
self.pub_imu_raw.publish(imu_raw_msg)
# TODO: make this an option to publish?
# Publish filtered data
imu_msg.header.stamp = self.node.get_clock().now().to_msg()
imu_msg.header.frame_id = self.param.frame_id.value
q = Quaternion()
# imu_msg.header.seq = seq
q.w = self.unpackBytesToFloat(buf[24], buf[25])
q.x = self.unpackBytesToFloat(buf[26], buf[27])
q.y = self.unpackBytesToFloat(buf[28], buf[29])
q.z = self.unpackBytesToFloat(buf[30], buf[31])
# TODO(flynneva): replace with standard normalize() function
# normalize
norm = sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w)
imu_msg.orientation.x = q.x / norm
imu_msg.orientation.y = q.y / norm
imu_msg.orientation.z = q.z / norm
imu_msg.orientation.w = q.w / norm
imu_msg.orientation_covariance = imu_raw_msg.orientation_covariance
imu_msg.linear_acceleration.x = \
self.unpackBytesToFloat(buf[32], buf[33]) / self.param.acc_factor.value
imu_msg.linear_acceleration.y = \
self.unpackBytesToFloat(buf[34], buf[35]) / self.param.acc_factor.value
imu_msg.linear_acceleration.z = \
self.unpackBytesToFloat(buf[36], buf[37]) / self.param.acc_factor.value
imu_msg.linear_acceleration_covariance = imu_raw_msg.linear_acceleration_covariance
imu_msg.angular_velocity.x = \
self.unpackBytesToFloat(buf[12], buf[13]) / self.param.gyr_factor.value
imu_msg.angular_velocity.y = \
self.unpackBytesToFloat(buf[14], buf[15]) / self.param.gyr_factor.value
imu_msg.angular_velocity.z = \
self.unpackBytesToFloat(buf[16], buf[17]) / self.param.gyr_factor.value
imu_msg.angular_velocity_covariance = imu_raw_msg.angular_velocity_covariance
self.pub_imu.publish(imu_msg)
# Publish magnetometer data
mag_msg.header.stamp = self.node.get_clock().now().to_msg()
mag_msg.header.frame_id = self.param.frame_id.value
# mag_msg.header.seq = seq
mag_msg.magnetic_field.x = \
self.unpackBytesToFloat(buf[6], buf[7]) / self.param.mag_factor.value
mag_msg.magnetic_field.y = \
self.unpackBytesToFloat(buf[8], buf[9]) / self.param.mag_factor.value
mag_msg.magnetic_field.z = \
self.unpackBytesToFloat(buf[10], buf[11]) / self.param.mag_factor.value
mag_msg.magnetic_field_covariance = [
self.param.variance_mag.value[0], 0.0, 0.0, 0.0,
self.param.variance_mag.value[1], 0.0, 0.0, 0.0,
self.param.variance_mag.value[2]
]
self.pub_mag.publish(mag_msg)
# Publish temperature
temp_msg.header.stamp = self.node.get_clock().now().to_msg()
temp_msg.header.frame_id = self.param.frame_id.value
# temp_msg.header.seq = seq
temp_msg.temperature = float(buf[44])
self.pub_temp.publish(temp_msg)
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()
odom = Odometry() odom.header.frame_id = frame_id odom.child_frame_id = child_frame_id odom.pose.covariance = cov_pose odom.twist.covariance = cov_twist # Define static IMU information imu = Imu() imu.header.frame_id = frame_id imu.orientation_covariance = orientation_cov imu.angular_velocity_covariance = ang_vel_cov # Define static magnetic information mag = MagneticField() mag.header.frame_id = frame_id mag.magnetic_field_covariance = mag_field_cov # Define flags is_initialized = False is_calibrated = False theta_start = 0 # Calibration samples = 0 acc_bias = [0, 0, 0] gyro_bias = [0, 0, 0] SamplesNumber = 1000 acc_threshold = 0.2 # Define velocity commands lin_comm = 0
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):
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()
directionAccuracyMsg.data = rotation_accuracy
directionAccuracyPub.publish(directionAccuracyMsg)
# Publish the mag data #
magMsg.header.seq = seq
magMsg.header.stamp = rospy.Time.now() - rospy.Duration(
mag_data_delay / 1000000)
magMsg.header.frame_id = "base_link"
# Magnetometer data (in micro-Teslas):
magMsg.magnetic_field.x = magX / 1000000 # Convert to Teslas
magMsg.magnetic_field.y = magY / 1000000
magMsg.magnetic_field.z = magZ / 1000000
if mag_accuracy == 0:
magMsg.magnetic_field_covariance = [
-1, 0.00, 0.00, 0.00, -1, 0.00, 0.00, 0.00, -1
]
else:
magMsg.magnetic_field_covariance = [
0.01, 0.00, 0.00, 0.00, 0.01, 0.00, 0.00, 0.00, 0.01
]
magPub.publish(magMsg)
# Publish the gyro and accel data #
imuMsg.header.seq = seq
imuMsg.header.stamp = rospy.Time.now() - rospy.Duration(
gyro_data_delay / 1000000)
imuMsg.header.frame_id = "base_link"
for i in range(3):
eul.append(Data[i]/32768.0 * math.pi)
# 磁场
if byte_temp[1] == 0x54:
magnetic_field_x = Data[0]
magnetic_field_y = Data[1]
magnetic_field_z = Data[2]
imu_msg.orientation = eul_to_qua(eul)
imu_msg.linear_acceleration.x = linear_acceleration_x
imu_msg.linear_acceleration.y = linear_acceleration_y
imu_msg.linear_acceleration.z = linear_acceleration_z
imu_msg.angular_velocity.x = angular_velocity_x
imu_msg.angular_velocity.y = angular_velocity_y
imu_msg.angular_velocity.z = angular_velocity_z
imu_msg.angular_velocity_covariance = cov_angular_velocity
imu_msg.linear_acceleration_covariance = cov_linear_acceleration
imu_pub.publish(imu_msg)
mag_msg.magnetic_field.x = magnetic_field_x
mag_msg.magnetic_field.y = magnetic_field_y
mag_msg.magnetic_field.z = magnetic_field_z
mag_msg.magnetic_field_covariance = cov_magnetic_field
mag_pub.publish(mag_msg)
time.sleep(0.001)
