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)