def publish(self, data): if not self._calib and data.getImuMsgId() == PUB_ID: q = data.getOrientation() roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z]) array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180)) res = Quaternion() res.w = array[0] res.x = array[1] res.y = array[2] res.z = array[3] msg = Imu() msg.header.frame_id = self._frameId msg.header.stamp = rospy.get_rostime() msg.orientation = res msg.linear_acceleration = data.getAcceleration() msg.angular_velocity = data.getVelocity() magMsg = MagneticField() magMsg.header.frame_id = self._frameId magMsg.header.stamp = rospy.get_rostime() magMsg.magnetic_field = data.getMagnetometer() self._pub.publish(msg) self._pubMag.publish(magMsg) elif data.getImuMsgId() == CALIB_ID: x, y, z = data.getValues() msg = imuCalib() if x > self._xMax: self._xMax = x if x < self._xMin: self._xMin = x if y > self._yMax: self._yMax = y if y < self._yMin: self._yMin = y if z > self._zMax: self._zMax = z if z < self._zMin: self._zMin = z msg.x.data = x msg.x.max = self._xMax msg.x.min = self._xMin msg.y.data = y msg.y.max = self._yMax msg.y.min = self._yMin msg.z.data = z msg.z.max = self._zMax msg.z.min = self._zMin self._pubCalib.publish(msg)
def _create_msg(self, data): """ Messages published with ROS have axis from the robot frame : x forward, y ?, z up or down """ msg1 = Imu() msg1_magfield = MagneticField() msg1.header.stamp = rospy.Time.now() msg1.header.frame_id = '/base_link' msg1_magfield.header.stamp = rospy.Time.now() msg1_magfield.header.frame_id = '/base_link' msg1.linear_acceleration.x = -data["IMU1"]["accel_y"] msg1.linear_acceleration.y = -data["IMU1"]["accel_z"] msg1.linear_acceleration.z = data["IMU1"]["accel_x"] msg1.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg1.angular_velocity.x = -data["IMU1"]["gyro_y"] msg1.angular_velocity.y = -data["IMU1"]["gyro_z"] msg1.angular_velocity.z = data["IMU1"]["gyro_x"] msg1.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg1.orientation.w = 0 msg1.orientation.x = 0 msg1.orientation.y = 0 msg1.orientation.z = 0 msg1.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg1_magfield.magnetic_field.x = -data["IMU1"]["mag_y"] msg1_magfield.magnetic_field.y = -data["IMU1"]["mag_z"] msg1_magfield.magnetic_field.z = data["IMU1"]["mag_x"] msg2 = Imu() msg2_magfield = MagneticField() msg2.header.stamp = rospy.Time.now() msg2.header.frame_id = '/base_link' msg2_magfield.header.stamp = rospy.Time.now() msg2_magfield.header.frame_id = '/base_link' msg2.linear_acceleration.x = data["IMU2"]["accel_y"] msg2.linear_acceleration.y = data["IMU2"]["accel_z"] msg2.linear_acceleration.z = data["IMU2"]["accel_x"] msg2.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg2.angular_velocity.x = data["IMU2"]["gyro_y"] msg2.angular_velocity.y = data["IMU2"]["gyro_z"] msg2.angular_velocity.z = data["IMU2"]["gyro_x"] msg2.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg2.orientation.w = 0 msg2.orientation.x = 0 msg2.orientation.y = 0 msg2.orientation.z = 0 msg2.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg2_magfield.magnetic_field.x = data["IMU2"]["mag_y"] msg2_magfield.magnetic_field.y = data["IMU2"]["mag_z"] msg2_magfield.magnetic_field.z = data["IMU2"]["mag_x"] return msg1, msg1_magfield, msg2, msg2_magfield
def publishMag(self, mag, time): '''! Publica os dados de campo magnetico @param accel: float[3] - leituras do magnetometro @param time: float - momento em que a leitura foi realizada ''' msg = MagneticField() msg.magnetic_field.x = mag[0] * 0.000001 msg.magnetic_field.y = mag[1] * 0.000001 msg.magnetic_field.z = mag[2] * 0.000001 msg.magnetic_field_covariance = np.zeros((3, 3), dtype=np.float64) frameId = self.get_parameter( "frame_id").get_parameter_value().string_value msg.header.frame_id = frameId timeSec = float(time) / 1000.0 msg.header.stamp.sec = int(timeSec) msg.header.stamp.nanosec = int((timeSec * 1000000000) % 1000000000) self.publisherMag.publish(msg)
def publish_data(self): data_pub = Imu() data_pub_mag = MagneticField() data_pub.header.stamp = rospy.get_rostime() data_pub_mag.header.stamp = rospy.get_rostime() #convert from g to m/s² data_pub.angular_velocity.x = gyro_xyz[0]* 0.0174532925 #gyros_x data_pub.angular_velocity.y = gyro_xyz[1]* 0.0174532925 #gyros_y data_pub.angular_velocity.z = gyro_xyz[2]* 0.0174532925 #gyros_z data_pub.angular_velocity_covariance[0] = 0 #convert from deg/s to rad/s (pi/180 = 0.0174532925) data_pub.linear_acceleration.x = accel_xyz[0]*9.81 #accel_x data_pub.linear_acceleration.y = accel_xyz[1]*9.81#accel_y data_pub.linear_acceleration.z = accel_xyz[2]*9.81 #accel_z data_pub.linear_acceleration_covariance[0] = 0 #convert from Guass to Tesla data_pub_mag.magnetic_field.x = magn_xyz[0]*0.0001 #accel_x data_pub_mag.magnetic_field.y = magn_xyz[1]*0.0001 #accel_y data_pub_mag.magnetic_field.z = magn_xyz[2]*0.0001 #accel_z data_pub_mag.magnetic_field_covariance[0] = 0 #variance unknown self.Imu_data_pub.publish(data_pub) self.Imu_data_mag.publish(data_pub_mag)
def publish(self, data): q = data.getOrientation() roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z]) # print "Before ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n" array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180)) res = Quaternion() res.w = array[0] res.x = array[1] res.y = array[2] res.z = array[3] roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z]) # print "after ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n" msg = Imu() msg.header.frame_id = self._frameId msg.header.stamp = rospy.get_rostime() msg.orientation = res msg.linear_acceleration = data.getAcceleration() msg.angular_velocity = data.getVelocity() magMsg = MagneticField() magMsg.header.frame_id = self._frameId magMsg.header.stamp = rospy.get_rostime() magMsg.magnetic_field = data.getMagnetometer() self._pub.publish(msg) self._pubMag.publish(magMsg)
def publish_sensor_data(self, sensor_data): acc = np.array(sensor_data.acceleration.data) / 1000.0 * g ang_vel = np.array(sensor_data.angular_vel.data) / 180.0 * np.pi # ang_vel = rotate_v(ang_vel.tolist(), self.rotation) msg = Imu() msg.header.frame_id = self.base_frame_id msg.header.stamp = rospy.Time.now() msg.linear_acceleration = Vector3(*acc) # It happens that sensor_data.quaterion and pose.quaternion are NOT in the same frame q = sensor_data.quaternion.data q = [q[1], q[2], q[3], q[0]] msg.orientation = Quaternion(*q) # msg.orientation = Quaternion(*self.orientation) msg.angular_velocity = Vector3(*ang_vel) msg.linear_acceleration_covariance = self.linear_acceleration_cov msg.orientation_covariance = self.orientation_cov msg.angular_velocity_covariance = self.angular_velocity_cov self.imu_pub.publish(msg) mag = np.array(sensor_data.magnetic.data) * 1e-6 m_msg = MagneticField() m_msg.header = msg.header m_msg.magnetic_field = Vector3(*mag) m_msg.magnetic_field_covariance = self.magnetic_field_cov self.mag_pub.publish(m_msg)
def imu_publisher(): global accl_x, accl_y, accl_z, gyro_x, gyro_y, gyro_z, comp_x, comp_y, comp_z rospy.init_node( 'Imu_mpu9250_node', anonymous=False ) # if anonymous=True is to ensure that node is unique by adding random number in the end r = rospy.Rate(20.0) current_time = rospy.Time.now() curn_time = time.time() last_time = time.time() new_time = 0.0 while not rospy.is_shutdown(): current_time = rospy.Time.now() read_imu_raw_data() imu = Imu() imu.header.stamp = rospy.Time.now() imu.angular_velocity = Vector3(gyro_x, gyro_y, gyro_z) imu.linear_acceleration = Vector3(accl_x, accl_y, accl_z) imu_data_pub.publish(imu) mag = MagneticField() mag.header.stamp = current_time mag.header.frame_id = "imu/mag" mag.magnetic_field = Vector3(comp_x, comp_y, comp_z) imu_mag_pub.publish(mag) r.sleep()
def main(): rospy.init_node("imu") port = rospy.get_param("~port", "GPG3_AD1") sensor = InertialMeasurementUnit(bus=port) pub_imu = rospy.Publisher("~imu", Imu, queue_size=10) pub_temp = rospy.Publisher("~temp", Temperature, queue_size=10) pub_magn = rospy.Publisher("~magnetometer", MagneticField, queue_size=10) br = TransformBroadcaster() msg_imu = Imu() msg_temp = Temperature() msg_magn = MagneticField() hdr = Header(stamp=rospy.Time.now(), frame_id="IMU") rate = rospy.Rate(rospy.get_param('~hz', 30)) while not rospy.is_shutdown(): q = sensor.read_quaternion() # x,y,z,w mag = sensor.read_magnetometer() # micro Tesla (µT) gyro = sensor.read_gyroscope() # deg/second accel = sensor.read_accelerometer() # m/s² temp = sensor.read_temperature() # °C msg_imu.header = hdr hdr.stamp = rospy.Time.now() msg_temp.header = hdr msg_temp.temperature = temp pub_temp.publish(msg_temp) msg_imu.header = hdr msg_imu.linear_acceleration.x = accel[0] msg_imu.linear_acceleration.y = accel[1] msg_imu.linear_acceleration.z = accel[2] msg_imu.angular_velocity.x = math.radians(gyro[0]) msg_imu.angular_velocity.y = math.radians(gyro[1]) msg_imu.angular_velocity.z = math.radians(gyro[2]) msg_imu.orientation.w = q[3] msg_imu.orientation.x = q[0] msg_imu.orientation.y = q[1] msg_imu.orientation.z = q[2] pub_imu.publish(msg_imu) msg_magn.header = hdr msg_magn.magnetic_field.x = mag[0] * 1e-6 msg_magn.magnetic_field.y = mag[1] * 1e-6 msg_magn.magnetic_field.z = mag[2] * 1e-6 pub_magn.publish(msg_magn) transform = TransformStamped(header=Header(stamp=rospy.Time.now(), frame_id="world"), child_frame_id="IMU") transform.transform.rotation = msg_imu.orientation br.sendTransformMessage(transform) rate.sleep()
def get_mag(imu_raw): mag_out = MagneticField() mag_out.header.stamp=rospy.Time.now() mag=Vector3() mag.x = float(imu_raw[4]) mag.y = float(imu_raw[5]) mag.z = float(imu_raw[6]) mag_out.magnetic_field=mag return mag_out
def publish_magnetic_field(index): field = MagneticField() field.header.frame_id = 'world' field.header.stamp = rospy.get_rostime() field.magnetic_field.x = random.uniform(-5, 5) field.magnetic_field.y = random.uniform(-5, 5) field.magnetic_field.z = random.uniform(-5, 5) for i in range(len(field.magnetic_field_covariance)): field.magnetic_field_covariance[i] = random.uniform(-5, 5) sensor_publisher[index].publish(field)
def Vn100Pub(): pub = rospy.Publisher('IMUData', Imu, queue_size=1) pub2 = rospy.Publisher('IMUMag', MagneticField, queue_size=1) # Initialize the node and name it. rospy.init_node('IMUpub') vn = imuthread3.Imuthread(port=rospy.get_param("imu_port"), pause=0.0) vn.start() vn.set_data_freq50() vn.set_qmr_mode() #vn.set_data_freq10() #to see if this fixes my gps drop out problem rospy.sleep(3) msg = Imu() msg2 = MagneticField() count = 0 while not rospy.is_shutdown(): if len(vn.lastreadings)>0: if vn.lastreadings[0] =='VNQMR': msg.header.seq = count msg.header.stamp = rospy.Time.now() msg.header.frame_id = 'imu' msg.orientation.x = float(vn.lastreadings[1]) msg.orientation.y = float(vn.lastreadings[2]) msg.orientation.z = float(vn.lastreadings[3]) msg.orientation.w = float(vn.lastreadings[4]) msg.orientation_covariance = [0,0,0,0,0,0,0,0,0] msg.angular_velocity.x = float(vn.lastreadings[8]) msg.angular_velocity.y = float(vn.lastreadings[9]) msg.angular_velocity.z = float(vn.lastreadings[10]) msg.angular_velocity_covariance = [0,0,0,0,0,0,0,0,0] msg.linear_acceleration.x = float(vn.lastreadings[11]) msg.linear_acceleration.y = float(vn.lastreadings[12]) msg.linear_acceleration.z = float(vn.lastreadings[13]) msg.linear_acceleration_covariance = [0,0,0,0,0,0,0,0,0] msg2.header.seq = count msg2.header.stamp = rospy.Time.now() msg2.header.frame_id = 'imu' msg2.magnetic_field.x = float(vn.lastreadings[5]) msg2.magnetic_field.x = float(vn.lastreadings[6]) msg2.magnetic_field.x = float(vn.lastreadings[7]) msg2.magnetic_field_covariance = [0,0,0,0,0,0,0,0,0] #rospy.loginfo("vn100_pub " + str(msg.header.stamp) + " " + str(msg.orientation.x) + " "+ str(msg.orientation.y) + " "+ str(msg.orientation.z) + " "+ str(msg.orientation.w) + " ") current_pose_euler = tf.transformations.euler_from_quaternion([ msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w ]) pub.publish(msg) pub2.publish(msg2) count += 1 #rospy.sleep(1.0/100.0) vn.kill = True
def callbackImu(msg): # Grab accelerometer and gyro data. imu_msg = Imu() imu_msg.header = msg.header imu_msg.header.frame_id = 'imu_link' q = np.array([msg.quaternion.x, msg.quaternion.y, msg.quaternion.z, msg.quaternion.w]) q = normalize(q) euler = tf.transformations.euler_from_quaternion(q) q = tf.transformations.quaternion_from_euler(-(euler[0] + math.pi / 2), euler[1], euler[2] - math.pi) imu_msg.orientation.x = q[0] imu_msg.orientation.y = q[1] imu_msg.orientation.z = q[2] imu_msg.orientation.w = q[3] euler_new = tf.transformations.euler_from_quaternion(q) rospy.loginfo("euler[0] = %s, euler[1] = %s, euler = [2] = %s", str(euler_new[0]), str(euler_new[1]), str(euler_new[2])) imu_msg.orientation_covariance = [0.9, 0, 0, \ 0, 0.9, 0, \ 0, 0, 0.9] imu_msg.angular_velocity = msg.gyro imu_msg.angular_velocity_covariance = [0.9, 0, 0, \ 0, 0.9, 0, \ 0, 0, 0.9] # Estimate gyro bias. global gyro_readings global MAX_GYRO_READINGS global gyro_bias if len(gyro_readings) < MAX_GYRO_READINGS: gyro_readings.append(imu_msg.angular_velocity.z) elif math.isnan(gyro_bias): gyro_bias = sum(gyro_readings)/MAX_GYRO_READINGS if not math.isnan(gyro_bias): imu_msg.angular_velocity.z -= gyro_bias imu_msg.linear_acceleration = msg.accelerometer imu_msg.linear_acceleration_covariance = [0.90, 0, 0, \ 0, 0.90, 0, \ 0, 0, 0.90] # Grab magnetometer data. mag_msg = MagneticField() mag_msg.header = msg.header mag_msg.magnetic_field = msg.magnetometer # TODO(gbrooks): Add covariance. # Publish sensor data. imu_pub.publish(imu_msg) mag_pub.publish(mag_msg)
def publish_mag_msg(self, msg, **metadata): mag_msg = MagneticField() mag_msg.header.stamp = rospy.Time.now() mag_msg.header.frame_id = 'gps_link' # sbp reports in microteslas, the MagneticField() msg requires field in teslas magscale = 1E-6 mag_msg.magnetic_field.x = msg.mag_x * magscale mag_msg.magnetic_field.y = msg.mag_y * magscale mag_msg.magnetic_field.z = msg.mag_z * magscale mag_msg.magnetic_field_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] #publish to /gps/mag/raw self.pub_mag.publish(mag_msg)
def main(): rospy.init_node('em7180') publisher = rospy.Publisher('imu/mag', MagneticField, queue_size=10) # transformed_publisher = rospy.Publisher('imu/magTransformed', Vector3Stamped, queue_size=10) rate = rospy.Rate(10) em7180 = EM7180_Master(MAG_RATE, ACCEL_RATE, GYRO_RATE, BARO_RATE, Q_RATE_DIVISOR) listener = tf.TransformListener() listener.waitForTransform("em7180_link", "imu_0", rospy.Time(0), rospy.Duration(1.0)) # Start the EM7180 in master mode if not em7180.begin(): print(em7180.getErrorString()) exit(1) while not rospy.is_shutdown(): em7180.checkEventStatus() if em7180.gotError(): print('ERROR: ' + em7180.getErrorString()) exit(1) if em7180.gotMagnetometer(): mx, my, mz = em7180.readMagnetometer() magnetic_vector = Vector3Stamped() magnetic_vector.header.stamp = rospy.Time.now() magnetic_vector.header.frame_id = "em7180_link" magnetic_vector.vector.x = mx magnetic_vector.vector.y = my magnetic_vector.vector.z = mz magnetic_vector_transformed = tf.transformVector3( "imu_0", magnetic_vector) magnetic_field_msg = MagneticField() magnetic_field_msg.header = magnetic_vector_transformed.header magnetic_field_msg.magnetic_field = magnetic_vector_transformed.vector magnetic_field_msg.magnetic_field_covariance = [ 700, 0, 0, 0, 700, 0, 0, 0, 700 ] publisher.publish(magnetic_field_msg) rate.sleep()
def format_mag(self, mag_data): """ Formats magnetometer-message :param mag_data: magnetometer data :return: formated message """ msg = MagneticField() msg.header.frame_id = 'imu_mag' msg.header.stamp = rospy.Time.now() msg.magnetic_field.x = mag_data[0] msg.magnetic_field.y = mag_data[1] msg.magnetic_field.z = mag_data[2] msg.magnetic_field_covariance = self.mag_cv return msg
def publish(self, data): msg = Imu() msg.header.frame_id = self._frameId msg.header.stamp = rospy.get_rostime() msg.orientation = data.getOrientation() msg.linear_acceleration = data.getAcceleration() msg.angular_velocity = data.getVelocity() magMsg = MagneticField() magMsg.header.frame_id = self._frameId magMsg.header.stamp = rospy.get_rostime() magMsg.magnetic_field = data.getMagnetometer() self._pub.publish(msg) self._pubMag.publish(magMsg)
def __init__(self): self.accel_mps2 = [0.0] * 3 self.gyro_rps = [0.0] * 3 self.mag_T = [0.0] * 3 self.rpy_rad = [0.0] * 3 self.timestamp_sec = 0.0 self.linear_accel_covariance = 0.098 * 0.098 self.angular_velocity_covariance = 0.012 * 0.012 self.orientation_covariance = 0.035 * 0.035 self.magnetic_field_covariance = 0.000002 * 0.000002 self.publish_data = False self._MsgData = Imu() self._MsgPub = rospy.Publisher('/segway/feedback/imu', Imu, queue_size=10) self._MsgData.header.frame_id = 'imu_frame' self._seq = 0 self._MagMsgData = MagneticField() self._MagMsgPub = rospy.Publisher('/segway/feedback/mag_feild', MagneticField, queue_size=10) self._MagMsgData.header.frame_id = 'imu_frame' self._Magseq = 0
def __init__(self): self.dynamic_pub = rospy.Publisher("/imu/mag", MagneticField, queue_size=1) self.mag_vals_pub = rospy.Publisher("/comp_mag_values", mag_values, queue_size=1) magval = mag_values() rospy.Subscriber("/imu/mag_raw", MagneticField, self.min_max) self.corrected = [0, 0, 0] self.x_out = 0 self.y_out = 0 self.z_out = 0 rate = rospy.Rate(60) while not rospy.is_shutdown(): mag = MagneticField(header=Header(stamp=rospy.Time.now(), frame_id='base_link'), magnetic_field=Vector3(self.corrected[0], self.corrected[1], self.corrected[2]), magnetic_field_covariance=[ 0.000, 0.0, 0.0, 0.0, 0.000, 0.0, 0.0, 0.0, 0.000 ]) self.dynamic_pub.publish(mag) rate.sleep()
def publisher(): # accel_x_sum = 0 # accel_y_sum = 0 # accel_z_sum = 0 # gyro_x_sum = 0 # gyro_y_sum = 0 # gyro_z_sum = 0 # for(int i = 0; i < 100; i++){ # accel = mpu9250.readAccel() # gyro = mpu9250.readGyro() # accel_x_sum += accel['y'] # accel_y_sum += accel['x'] # accel_z_sum += accel['z'] # gyro_x_sum += gyro['y'] # gyro_y_sum += gyro['x'] # gyro_z_sum += gyro['z'] # } # accel_x_init = accel_x_sum / 100 # accel_y_init = accel_y_sum / 100 # accel_z_init = accel_z_sum / 100 # gyro_x_init = gyro_x_sum / 100 # gyro_y_init = gyro_y_sum / 100 # gyro_z_init = gyro_z_sum / 100 pub_imu = rospy.Publisher('imu/data_raw', Imu, queue_size=5) pub_mag = rospy.Publisher('imu/mag', MagneticField, queue_size=5) rospy.init_node('imu_pub') imu = Imu() magnetic = MagneticField() mpu9250 = MPU9250() # change while not rospy.is_shutdown(): accel = mpu9250.readAccel() gyro = mpu9250.readGyro() mag = mpu9250.readMagnet() imu.header.stamp = rospy.Time.now() imu.header.frame_id = 'base_link' imu.angular_velocity.x = (gyro['y']) * 0.07 * DPS_TO_RADS imu.angular_velocity.y = -(gyro['x']) * 0.07 * DPS_TO_RADS imu.angular_velocity.z = (gyro['z']) * 0.07 * DPS_TO_RADS imu.linear_acceleration.x = (accel['y']) * 9.8 imu.linear_acceleration.y = -(accel['x']) * 9.8 imu.linear_acceleration.z = (accel['z']) * 9.8 #imu.linear_acceleration.x = 0 magnetic.header.stamp = rospy.Time.now() magnetic.header.frame_id = 'base_link' magnetic.magnetic_field.x = mag['y'] magnetic.magnetic_field.y = -mag['x'] magnetic.magnetic_field.z = mag['z'] pub_imu.publish(imu) pub_mag.publish(magnetic) rospy.sleep(0.01)
def publish(self, event): compass_accel = self.compass_accel.read() compass = compass_accel[1] Racc = compass_accel[0] gyro = self.gyro.read() GyroRate = gyro[0] GyroRate = [x * self.DEG_TO_RAD for x in GyroRate] #initial A_odd = [0, 0] AvgRate = [0, 0] A = [0, 0] Rgyro = [0, 0, 0] # normalized Racc_lengh = math.sqrt(Racc[0]**2 + Racc[1]**2 + Racc[2]**2) Racc_normalized = [x / Racc_lengh for x in Racc] A_odd[0] = math.atan2(self.Rest[1], self.Rest[2]) A_odd[1] = math.atan2(self.Rest[0], self.Rest[2]) for i in range(2): AvgRate[i] = (GyroRate[i] + self.GyroRate_odd[i]) / 2 A[i] = A_odd[i] + AvgRate[i] * self.T Rgyro[0] = math.sin(A[1]) / math.sqrt(1 + ((math.cos(A[1]))**2) * ((math.tan(A[0]))**2)) Rgyro[1] = math.sin(A[0]) / math.sqrt(1 + ((math.cos(A[0]))**2) * ((math.tan(A[1]))**2)) if self.Rest[2] > 0: RzGyro_sign = 1 else: RzGyro_sign = -1 Rgyro[2] = RzGyro_sign * math.sqrt(1 - Rgyro[0]**2 - Rgyro[1]**2) for i in range(3): self.Rest[i] = (Racc_normalized[i] + Rgyro[i] * self.wGyro) / (1 + self.wGyro) position_msg = Imu() position_msg.linear_acceleration.x = self.Rest[0] position_msg.linear_acceleration.y = self.Rest[1] position_msg.linear_acceleration.z = self.Rest[2] position_msg.header.frame_id = "cheng" self.pub_position.publish(position_msg) self.GyroRate_odd = GyroRate # Put together a magnetometer message mag_msg = MagneticField() mag_msg.header.stamp = rospy.Time.now() mag_msg.magnetic_field.x = compass[0] mag_msg.magnetic_field.y = compass[1] mag_msg.magnetic_field.z = compass[2] self.pub_mag.publish(mag_msg)
def Broadcast_imu_raw(self,lineParts): partsCount = len(lineParts) if (partsCount == 11): # $IMUR,timestamp,ax,ay,az,gx,gy,gz,mx,my,mz pass try: time_now = rospy.Time.now() frame_id = "imu_base" acc_avel_raw = Imu() mag_raw = MagneticField() acc_avel_raw.header.stamp = time_now acc_avel_raw.header.frame_id = frame_id mag_raw.header.stamp = time_now mag_raw.header.frame_id = frame_id #acc_avel_raw.arduinoTime = int(lineParts[1]) # shpuld be in m/s^2 acc_avel_raw.linear_acceleration.x = float(lineParts[2])*9.81 acc_avel_raw.linear_acceleration.y = float(lineParts[3])*9.81 acc_avel_raw.linear_acceleration.z = float(lineParts[4])*9.81 # should be in rad/sec acc_avel_raw.angular_velocity.x = math.radians(float(lineParts[5])) acc_avel_raw.angular_velocity.y = math.radians(float(lineParts[6])) acc_avel_raw.angular_velocity.z = math.radians(float(lineParts[7])) # shloud be in Teslas # if MPU9250 is used, magnetometer orientation mismatch should be corrected to maintain x from acc and gyro as forward mag_raw.magnetic_field.x = float(lineParts[9])e-7 # x of acc corresponds to y of mag mag_raw.magnetic_field.y = float(lineParts[8])e-7 # y of acc correspond to x of mag mag_raw.magnetic_field.z = -float(lineParts[10])e-7 # z of acc is opposite to z of mag self._acc_vel_pub.publish(acc_avel_raw) self._mag_pub.publish(mag_raw) except: rospy.logwarn("Unexpected error:" + str(sys.exc_info()[0]))
def write_ms25(ms25, i, bag): utime = ms25[i, 0] mag_x = ms25[i, 1] mag_y = ms25[i, 2] mag_z = ms25[i, 3] accel_x = ms25[i, 4] accel_y = ms25[i, 5] accel_z = ms25[i, 6] rot_r = ms25[i, 7] rot_p = ms25[i, 8] rot_h = ms25[i, 9] timestamp = microseconds_to_ros_timestamp(utime) rosimu = Imu() rosimu.header.stamp = timestamp rosimu.angular_velocity.x = float(rot_r) rosimu.angular_velocity.y = float(rot_p) rosimu.angular_velocity.z = float(rot_h) rosimu.linear_acceleration.x = float(accel_x) rosimu.linear_acceleration.y = float(accel_y) rosimu.linear_acceleration.z = float(accel_z) bag.write('/ms25_imu', rosimu, t=timestamp) rosmag = MagneticField() rosmag.header.stamp = timestamp rosmag.magnetic_field.x = mag_x rosmag.magnetic_field.y = mag_y rosmag.magnetic_field.z = mag_z bag.write('/ms25_mag', rosmag, t=timestamp)
def __init__(self, i2c=None): super().__init__('rtf_lis3mdl') if i2c is None: self.i2c = busio.I2C(board.SCL, board.SDA) else: self.i2c = i2c self.lis = adafruit_lis3mdl.LIS3MDL( self.i2c) # 155 Hz, 4 gauss, continuous self.lis.data_rate = adafruit_lis3mdl.Rate.RATE_155_HZ self.timer_mag = self.create_timer(1 / 100, self.callback) self.pub_mag = self.create_publisher(MagneticField, 'magnetic', 10) self.mags_bias = self.declare_parameter('mags_bias', [0., 0., 0.]) frame_id = self.declare_parameter('frame_id', "base_imu_link").value self.mag_msg = MagneticField() self.mag_msg.header.frame_id = frame_id mc = 0.01 self.mag_msg.magnetic_field_covariance = [ mc, 0.0, 0.0, 0.0, mc, 0.0, 0.0, 0.0, mc ] self.calibrate = False
def callback_mag(data): global mag_msg, geo_msg mag_msg = MagneticField() geo_msg.header = mag_msg.header mag_msg = data geo_msg.vector = mag_msg.magnetic_field
def publishCustomMessage(imu_data, seq): imu_message = Imu() mgn_message = MagneticField() imu_message.header.seq = seq imu_message.header.frame_id = "IMU Message" imu_message.header.stamp = rospy.Time.now() mgn_message.header.stamp = rospy.Time.now() mgn_message.header.frame_id = "Magnetic field data" mgn_message.header.seq = seq imu_message.linear_acceleration.x = trimValue(imu_data['aclx']) imu_message.linear_acceleration.y = trimValue(imu_data['acly']) imu_message.linear_acceleration.z = trimValue(imu_data['aclz']) imu_message.orientation.w = imu_data['qw'] imu_message.orientation.x = imu_data['qx'] imu_message.orientation.y = imu_data['qy'] imu_message.orientation.z = imu_data['qz'] imu_message.angular_velocity.x = trimValue(imu_data['gyrx']) imu_message.angular_velocity.y = trimValue(imu_data['gyry']) imu_message.angular_velocity.z = trimValue(imu_data['gyrz']) mgn_message.magnetic_field.x = trimValue(imu_data['magx']) mgn_message.magnetic_field.y = trimValue(imu_data['magy']) mgn_message.magnetic_field.z = trimValue(imu_data['magz']) # Publishing magnetic field data and IMU data imu_publisher = rospy.Publisher('sensor'+'/imu',Imu, queue_size = 5) mgn_publisher = rospy.Publisher('sensor'+'/mag', MagneticField, queue_size = 5) imu_publisher.publish(imu_message) mgn_publisher.publish(mgn_message)
def reset_vars(self): self.imu_msg = Imu() self.imu_msg.orientation_covariance = (-1., ) * 9 self.imu_msg.angular_velocity_covariance = (-1., ) * 9 self.imu_msg.linear_acceleration_covariance = (-1., ) * 9 self.pub_imu = False self.pub_syncout_timeref = False self.raw_gps_msg = NavSatFix() self.pub_raw_gps = False self.pos_gps_msg = NavSatFix() self.pub_pos_gps = False self.vel_msg = TwistStamped() self.pub_vel = False self.mag_msg = MagneticField() self.mag_msg.magnetic_field_covariance = (0, ) * 9 self.pub_mag = False self.temp_msg = Temperature() self.temp_msg.variance = 0. self.pub_temp = False self.press_msg = FluidPressure() self.press_msg.variance = 0. self.pub_press = False self.anin1_msg = UInt16() self.pub_anin1 = False self.anin2_msg = UInt16() self.pub_anin2 = False self.ecef_msg = PointStamped() self.pub_ecef = False self.pub_diag = False
def publish_mag_state_msg(publisher): mag_state_msg = MagneticField() mag_state_msg.header.stamp = rospy.get_rostime() mag_state_msg.magnetic_field.x = mag.GetMagRawData()[0] mag_state_msg.magnetic_field.y = mag.GetMagRawData()[1] mag_state_msg.magnetic_field.z = mag.GetMagRawData()[2] publisher.publish(mag_state_msg)
def pub_data(self, ax, ay, az, gx, gy, gz, mx, my, mz): self.processValues() imu_topic = Imu() mag_topic = MagneticField() # self.rate = rospy.Rate(self.rate_hz) while not rospy.is_shutdown(): self.processValues() imu_topic.header.stamp = rospy.Time.now() imu_topic.header.frame_id = "map" imu_topic.angular_velocity.x = self.gx imu_topic.angular_velocity.y = self.gy imu_topic.angular_velocity.z = self.gz imu_topic.linear_acceleration.x = self.ax imu_topic.linear_acceleration.y = self.ay imu_topic.linear_acceleration.z = self.az self.imu_raw_pub.publish(imu_topic) mag_topic.header.stamp = rospy.Time.now() mag_topic.header.frame_id = "map" mag_topic.magnetic_field.x = self.mx mag_topic.magnetic_field.y = self.my mag_topic.magnetic_field.z = self.mz self.mag_raw_pub.publish(mag_topic) self.rate.sleep()
def publish(self, event): compass_accel = self.compass_accel.read() compass = compass_accel[0:3] accel = compass_accel[3:6] gyro = self.gyro.read() # Put together an IMU message imu_msg = Imu() imu_msg.header.stamp = rospy.Time.now() imu_msg.orientation_covariance[0] = -1 imu_msg.angular_velocity = gyro[0] * DEG_TO_RAD imu_msg.angular_velocity = gyro[1] * DEG_TO_RAD imu_msg.angular_velocity = gyro[2] * DEG_TO_RAD imu_msg.linear_acceleration.x = accel[0] * G imu_msg.linear_acceleration.y = accel[1] * G imu_msg.linear_acceleration.z = accel[2] * G self.pub_imu.publish(imu_msg) # Put together a magnetometer message mag_msg = MagneticField() mag_msg.header.stamp = rospy.Time.now() mag_msg.magnetic_field.x = compass[0] mag_msg.magnetic_field.y = compass[1] mag_msg.magnetic_field.z = compass[2] self.pub_mag.publish(mag_msg)
def _log_data_log2(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" msg = Temperature() # ToDo: it would be better to convert from timestamp to rospy time msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_prefix + "/base_link" # measured in degC msg.temperature = data["baro.temp"] self._pubTemp.publish(msg) # ToDo: it would be better to convert from timestamp to rospy time msg = MagneticField() msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_prefix + "/base_link" # measured in Tesla msg.magnetic_field.x = data["mag.x"] msg.magnetic_field.y = data["mag.y"] msg.magnetic_field.z = data["mag.z"] self._pubMag.publish(msg) msg = Float32() # hPa (=mbar) msg.data = data["baro.pressure"] self._pubPressure.publish(msg) # V msg.data = data["pm.vbat"] self._pubBattery.publish(msg) # Test msg.data = data["test.tval"] self._pubTest.publish(msg)
def __init__(self, _threaded_mode = False): self.data_dir = "/tmp/BHG_DATA" self.csv_filename = "default_data.csv" self.datetimeData = "" self.is_recording = False self.rel_alt = Altitude() self.gps_fix = NavSatFix() self.odom = Odometry() self.imu_mag = MagneticField() self.imu_data = Imu() self.vel_gps = TwistStamped() self.temp_imu = Temperature() self.trigtime = "TrigTimeNotSet" self.arduino_dev = rospy.get_param('~arduino_dev', '/dev/ttyACM0') self.arduino_timeout = rospy.get_param('~arduino_timeout', 2) self.ardunio_ser = '' self.pub = rospy.Publisher('trig_timer', String, queue_size=10) rospy.Subscriber('/directory', String, self.directory_callback) rospy.Subscriber("/record", Bool, self.record_callback) rospy.Subscriber("/mavros/altitude", Altitude, self.alt_cb) # change to /global_position/rel_alt Float64 rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.gps_cb) rospy.Subscriber("/mavros/global_position/local", Odometry, self.odom_cb) rospy.Subscriber("/mavros/imu/mag", MagneticField, self.mag_cb) rospy.Subscriber("/mavros/imu/data", Imu, self.imu_cb) rospy.Subscriber("/mavros/global_position/raw/gps_vel", TwistStamped, self.vel_cb) rospy.Subscriber("/mavros/imu/temperature_imu", Temperature, self.temp_cb)
def do_parse(ext_type, ext_len, ext_data): # 将ext_data 封装成完整的数据帧 ext_data.insert(0, ext_len) ext_data.insert(0, ext_type) ext_data.insert(0, 0xfa) ext_data.insert(0, 0xce) # CE FA 03 18 76 0E 5C 03 F0 FF 88 3C E0 FF A2 00 84 00 00 A2 FF E0 00 84 F4 01 20 03 # 根据数据帧的类型的类型来做对应的解析 0x01 线速度 0x02 电池 if ext_type == 0x03: # 对数据进行拆包 # 温度 temperature = struct.unpack('h', bytearray(ext_data[4:6]))[0] # 加速度 ax = struct.unpack('h', bytearray(ext_data[6:8]))[0] ay = struct.unpack('h', bytearray(ext_data[8:10]))[0] az = struct.unpack('h', bytearray(ext_data[10:12]))[0] # 角速度 gx = struct.unpack('h', bytearray(ext_data[12:14]))[0] gy = struct.unpack('h', bytearray(ext_data[14:16]))[0] gz = struct.unpack('h', bytearray(ext_data[16:18]))[0] # 地磁 mx = struct.unpack('h', bytearray(ext_data[18:20]))[0] my = struct.unpack('h', bytearray(ext_data[20:22]))[0] mz = struct.unpack('h', bytearray(ext_data[22:24]))[0] # 速度 velocity = struct.unpack('h', bytearray(ext_data[24:26]))[0] angular = struct.unpack('h', bytearray(ext_data[26:28]))[0] # 发布陀螺仪的数据 imu = Imu() # 根据芯片手册设定缩放系数 accel_ratio = 1 / 16384.0 imu.linear_acceleration.x = ax * accel_ratio imu.linear_acceleration.y = ay * accel_ratio imu.linear_acceleration.z = az * accel_ratio # °/s 转成弧度/s gyro_ratio = 1 / 65.5 / (180 / 3.1415926) imu.angular_velocity.x = gx * gyro_ratio imu.angular_velocity.y = gy * gyro_ratio imu.angular_velocity.z = gz * gyro_ratio imuPublisher.publish(imu) # 发布地磁的数据 mag = MagneticField() mag.magnetic_field.x = mx mag.magnetic_field.y = my mag.magnetic_field.z = mz magPublisher.publish(mag) # print(velocity,angular) # 将小车当前的线速度和角速度发布出去 twist = Twist() twist.linear.x = velocity / 1000.0 twist.angular.z = angular / 1000.0 velPublisher.publish(twist)
def main(): rospy.init_node('imu_rawdata_publisher', anonymous=True) rate = rospy.Rate(100) # 5hz rospy.loginfo("In main function") pubs_imu = rospy.Publisher('/imu/data_raw', Imu, queue_size=15) pubs_magnetometer = rospy.Publisher('/imu/mag', MagneticField, queue_size=15) imu_read = Imu() magnetometer_read = MagneticField() gyro_x, gyro_y, gyro_z, count = 0, 0, 0, 0 while not rospy.is_shutdown(): mpu = mpu6050(0x68) accel_data = mpu.get_accel_data() gyro_data = mpu.get_gyro_data() imu_read.header.stamp = rospy.Time.now() imu_read.header.frame_id = "body_frame" ### Accelerometer data imu_read.linear_acceleration.x = accel_data['x'] imu_read.linear_acceleration.y = accel_data['y'] imu_read.linear_acceleration.z = accel_data['z'] #### Gyrometer data gyro_x = gyro_data['x'] * (pi / 180) gyro_y = gyro_data['y'] * (pi / 180) gyro_z = gyro_data['z'] * (pi / 180) if abs(gyro_x) < 0.2: imu_read.angular_velocity.x = 0.0 else: imu_read.angular_velocity.x = gyro_x if abs(gyro_y) < 0.2: imu_read.angular_velocity.y = 0.0 else: imu_read.angular_velocity.y = gyro_y if abs(gyro_z) < 0.2: imu_read.angular_velocity.z = 0.0 else: imu_read.angular_velocity.z = gyro_z '''##### Magnetometer data magnetometer_read.header.stamp = rospy.Time.now(); magnetometer_read.header.frame_id = "body_frame"; magnetometer_read.magnetic_field.x = magnetometer_read.magnetic_field.y = magnetometer_read.magnetic_field.z = ''' pubs_imu.publish(imu_read) #pubs_magnetometer.publish(magnetometer_read) rate.sleep() rate.sleep() rospy.loginfo("Node is shutting down")
def _sensor_callback(self, accelerometer, magnometer, gyrometer): if rospy.is_shutdown(): return end_ts = rospy.get_time() for i in range(0, len(accelerometer)): imu = Imu() magnetic = MagneticField() # Calculate timestamp for reading samples_behind = (len(accelerometer) - 1) - i samples_per_sec = len(accelerometer) / 50. stamp = rospy.Time.from_sec(end_ts - samples_behind * samples_per_sec) imu.header.stamp = stamp imu.header.frame_id = "imu" magnetic.header.stamp = stamp imu.linear_acceleration.x = accelerometer[i][0] imu.linear_acceleration.y = accelerometer[i][1] imu.linear_acceleration.z = accelerometer[i][2] imu.linear_acceleration_covariance = self.linear_acceleration_covariance imu.angular_velocity.x = gyrometer[i][0] imu.angular_velocity.y = gyrometer[i][1] imu.angular_velocity.z = gyrometer[i][2] imu.angular_velocity_covariance = self.angular_velocity_covariance magnetic.magnetic_field.x = magnometer[i][0] magnetic.magnetic_field.y = magnometer[i][1] magnetic.magnetic_field.z = magnometer[i][2] magnetic.magnetic_field_covariance = self.magnetic_field_covariance try: self._tf_broadcaster.sendTransform( (0, 0, 0), (0., 0., 0., 1.), imu.header.stamp, "base_link", "imu") self._publisher_imu.publish(imu) self._publisher_magnetic.publish(magnetic) except rospy.ROSException: pass
def msg_template(self): msg_imu = Imu(); msg_mag = MagneticField(); msg_imu.header.frame_id = "robocape"; msg_mag.header.frame_id = "robocape_mag"; msg_imu.linear_acceleration_covariance = (0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1); msg_imu.angular_velocity_covariance = (1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); msg_imu.orientation_covariance = (-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); msg_mag.magnetic_field_covariance = (10.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 10.0); return (msg_imu, msg_mag);
def __init__(self): magx = [1,2,3,4] magy = [1,2,3,4] magz = [1,2,3,4] self.DEVICE_ID = rospy.get_param('~device_id','/dev/serial/by-id/usb-Teensyduino_USB_Serial_1558050-if00') self.mag_pub = rospy.Publisher('imu/mag', MagneticField, queue_size = 1) mag = MagneticField() ser = serial.Serial(self.DEVICE_ID) magn = [0.0,0.0,0.0,0.0] rate = rospy.Rate(70) while not rospy.is_shutdown(): ser.write('r') line = ser.readline() magn = line.split("," , 4) #rospy.loginfo(magn) if magn[0] == 'XYZ': x_raw = float(magn[1]) /100.0 y_raw = float(magn[2]) /100.0 z_raw = float(magn[3]) /100.0 raw = np.array([x_raw, y_raw, z_raw]) scale = np.array([[1.0020497985386805, -0.06914522912368153, 0.11962132522309941], [-0.06914522912368157, 1.0741014035679903, 0.22518913831628748], [0.11962132522309966, 0.22518913831628748, 0.9984945224678732]]) bias = np.array([-0.16542780063806176, -0.04899507311891892, -0.10150383298877998]) corrected = np.dot(raw-bias, scale) mag.header.stamp = rospy.Time.now() mag.header.frame_id = '/base_link' # i.e. '/odom' mag.magnetic_field.x = corrected[0] mag.magnetic_field.y = corrected[1] mag.magnetic_field.z = corrected[2] mag.magnetic_field_covariance=[.01, 0.0, 0.0, 0.0, .01, 0.0, 0.0, 0.0, .01] self.mag_pub.publish(mag) #heading = math.atan2(mag.magnetic_field.y,mag.magnetic_field.x) '''for i in range(3,0,-1): magx[i] = magx[i-1] magx[0] = mag.magnetic_field.x for i in range(3,0,-1): magy[i] = magy[i-1] magy[0] = mag.magnetic_field.y for i in range(3,0,-1): magz[i] = magz[i-1] magz[0] = mag.magnetic_field.z if self.checkEqual(magx) == True: rospy.logerr("MAGX value is static: %f" % magx[0]) if self.checkEqual(magy) == True: rospy.logerr("MAGY value is static: %f" % magy[0]) if self.checkEqual(magz) == True: rospy.logerr("MAGZ value is static: %f" % magz[0])''' rate.sleep()
def __init__(self): self.DEVICE_ID = rospy.get_param('~device_id','/dev/serial/by-id/usb-SparkFun_SFE_9DOF-D21-if00') self.imu_pub = rospy.Publisher('imu/camera_tilt', Imu, queue_size = 1) self.mag_pub = rospy.Publisher("/imu/mag_camera", MagneticField, queue_size=1) self.roll_pub = rospy.Publisher("camera_roll", Float64, queue_size = 1) self.pitch_pub = rospy.Publisher("camera_pitch", Float64, queue_size = 1) self.yaw_pub = rospy.Publisher("camera_yaw", Float64, queue_size = 1) #self.mag_pub = rospy.Publisher('imu/camera_mag', MagneticField, queue_size = 1) head1 = 0 head2 = 0 head3 = 0 head4 = 0 head5 = 0 head6 = 0 imu = Imu() mag = MagneticField() ser = serial.Serial(self.DEVICE_ID) gyro = [0.0,0.0,0.0,0.0] magn = [0.0,0.0,0.0,0.0] quat = [0.0,1.0,0.0,0.0,0.0] rate = rospy.Rate(130) while not rospy.is_shutdown(): line = ser.readline() accel = line.split("," , 4) if accel[0] == 'A': line = ser.readline() gyro = line.split("," , 4) line = ser.readline() magn = line.split("," , 4) line = ser.readline() quat = line.split("," , 5) line = ser.readline() rpy = line.split("," , 4) magneticsphere = ([1.0974375136949293, 0.008593465160122918, 0.0003032211129407881], [0.008593465160122857, 1.1120651652148803, 0.8271491584066613], [0.00030322111294077105, 0.0926004380061327, 0.5424720769663267]) bias = ([-0.021621641870448665], [-0.10661356710756408], [0.4377993330407842]) raw_values = ([float(magn[1])], [float(magn[2])], [float(magn[3])]) shift = ([raw_values[0][0] - bias[0][0]], [raw_values[1][0] - bias[1][0]], [raw_values[2][0] - bias[2][0]]) magneticfield = np.dot(magneticsphere, shift) #rospy.loginfo(magneticfield) imu.header.stamp = rospy.Time.now() imu.header.frame_id = '/base_link' # i.e. '/odom' #pres.child_frame_id = self.child_frame_id # i.e. '/base_footprint' imu.angular_velocity.x = float(gyro[1]) imu.angular_velocity.y = float(gyro[2]) imu.angular_velocity.z = float(gyro[3]) imu.angular_velocity_covariance=[.01, 0.0, 0.0, 0.0, .01, 0.0, 0.0, 0.0, .01] imu.linear_acceleration.x = float(accel[1]) imu.linear_acceleration.y = float(accel[2]) imu.linear_acceleration.z = float(accel[3]) imu.linear_acceleration_covariance = [.01, 0.0, 0.0, 0.0, .01, 0.0, 0.0, 0.0, .01] mag.header.stamp = rospy.Time.now() mag.header.frame_id = '/base_link' # i.e. '/odom' mag.magnetic_field.x = float(magn[1])/100.0 mag.magnetic_field.y = float(magn[2])/100.0 mag.magnetic_field.z = float(magn[3])/100.0 mag.magnetic_field_covariance = [.01, 0.0, 0.0, 0.0, .01, 0.0, 0.0, 0.0, .01] imu.orientation.w = float(quat[1]) imu.orientation.x = float(quat[2]) imu.orientation.y = float(quat[3]) imu.orientation.z = float(quat[4]) imu.orientation_covariance = [.01, 0.0, 0.0, 0.0, .01, 0.0, 0.0, 0.0, .01] #magn[2] = -float(magn[2]) #heading = math.atan2(float(magn[2]),float(magn[1]))*(180/math.pi) #heading = math.atan2(float(magn[2]),float(magn[1])) #if heading < 0: # heading += 360 #rospy.loginfo(head[1]) self.roll_pub.publish(float(rpy[1])) self.pitch_pub.publish(float(rpy[2])) self.pitch_pub.publish(float(rpy[3])) self.imu_pub.publish(imu) self.mag_pub.publish(mag) rate.sleep()
def __init__(self): self.DEVICE_ID = rospy.get_param('~device_id','/dev/serial/by-id/usb-SparkFun_SFE_9DOF-D21-if00') self.imu_pub = rospy.Publisher('imu/camera_tilt', Imu, queue_size = 1) self.mag_pub = rospy.Publisher('imu/camera_mag', MagneticField, queue_size = 1) imu = Imu() mag = MagneticField() ser = serial.Serial(self.DEVICE_ID) gyro = [0.0,0.0,0.0,0.0] magn = [0.0,0.0,0.0,0.0] quat = [0.0,1.0,0.0,0.0,0.0] rate = rospy.Rate(130) while not rospy.is_shutdown(): line = ser.readline() accel = line.split("," , 4) if accel[0] == 'A': line = ser.readline() gyro = line.split("," , 4) line = ser.readline() magn = line.split("," , 4) line = ser.readline() quat = line.split("," , 5) imu.header.stamp = rospy.Time.now() imu.header.frame_id = '/base_link' # i.e. '/odom' #pres.child_frame_id = self.child_frame_id # i.e. '/base_footprint' imu.angular_velocity.x = float(gyro[1]) imu.angular_velocity.y = float(gyro[2]) imu.angular_velocity.z = float(gyro[3]) imu.angular_velocity_covariance=[.001, 0.0, 0.0, 0.0, .001, 0.0, 0.0, 0.0, .001] imu.linear_acceleration.x = float(accel[1]) imu.linear_acceleration.y = float(accel[2]) imu.linear_acceleration.z = float(accel[3]) imu.linear_acceleration_covariance = [.01, 0.0, 0.0, 0.0, .01, 0.0, 0.0, 0.0, .01] '''imu.orientation.w = float(quat[1]) imu.orientation.x = float(quat[2]) imu.orientation.y = float(quat[3]) imu.orientation.z = float(quat[4]) imu.orientation_covariance = [.01, 0.0, 0.0, 0.0, .01, 0.0, 0.0, 0.0, .01] ''' mag.header.stamp = rospy.Time.now() mag.header.frame_id = '/base_link' # i.e. '/odom' #pres.child_frame_id = self.child_frame_id # i.e. '/base_footprint' mag.magnetic_field.x = float(magn[1]) mag.magnetic_field.y = float(magn[2]) mag.magnetic_field.z = float(magn[3]) mag.magnetic_field_covariance=[.001, 0.0, 0.0, 0.0, .001, 0.0, 0.0, 0.0, .001] self.imu_pub.publish(imu) self.mag_pub.publish(mag) rate.sleep()
def spin_once(self): def baroPressureToHeight(value): c1 = 44330.0 c2 = 9.869232667160128300024673081668e-6 c3 = 0.1901975534856 intermediate = 1-math.pow(c2*value, c3) height = c1*intermediate return height # get data data = self.mt.read_measurement() # common header h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id # get data (None if not present) #temp = data.get('Temp') # float orient_data = data.get('Orientation Data') velocity_data = data.get('Velocity') position_data = data.get('Latlon') altitude_data = data.get('Altitude') acc_data = data.get('Acceleration') gyr_data = data.get('Angular Velocity') mag_data = data.get('Magnetic') pressure_data = data.get('Pressure') time_data = data.get('Timestamp') gnss_data = data.get('Gnss PVT') status = data.get('Status') # int # create messages and default values "Imu message supported with Modes 1 & 2" imu_msg = Imu() pub_imu = False "SensorSample message supported with Mode 2" ss_msg = sensorSample() pub_ss = False "Mag message supported with Modes 1 & 2" mag_msg = MagneticField() pub_mag = False "Baro in meters" baro_msg = baroSample() pub_baro = False "GNSS message supported only with MTi-G-7xx devices" "Valid only for modes 1 and 2" gnssPvt_msg = gnssSample() pub_gnssPvt = False #gnssSatinfo_msg = GPSStatus() #pub_gnssSatinfo = False # All filter related outputs "Supported in mode 3" ori_msg = orientationEstimate() pub_ori = False "Supported in mode 3 for MTi-G-7xx devices" vel_msg = velocityEstimate() pub_vel = False "Supported in mode 3 for MTi-G-7xx devices" pos_msg = positionEstimate() pub_pos = False secs = 0 nsecs = 0 if time_data: # first getting the sampleTimeFine time = time_data['SampleTimeFine'] secs = 100e-6*time nsecs = 1e5*time - 1e9*math.floor(secs) if acc_data: if 'Delta v.x' in acc_data: # found delta-v's pub_ss = True ss_msg.internal.imu.dv.x = acc_data['Delta v.x'] ss_msg.internal.imu.dv.y = acc_data['Delta v.y'] ss_msg.internal.imu.dv.z = acc_data['Delta v.z'] elif 'accX' in acc_data: # found acceleration pub_imu = True imu_msg.linear_acceleration.x = acc_data['accX'] imu_msg.linear_acceleration.y = acc_data['accY'] imu_msg.linear_acceleration.z = acc_data['accZ'] else: raise MTException("Unsupported message in XDI_AccelerationGroup.") if gyr_data: if 'Delta q0' in gyr_data: # found delta-q's pub_ss = True ss_msg.internal.imu.dq.w = gyr_data['Delta q0'] ss_msg.internal.imu.dq.x = gyr_data['Delta q1'] ss_msg.internal.imu.dq.y = gyr_data['Delta q2'] ss_msg.internal.imu.dq.z = gyr_data['Delta q3'] elif 'gyrX' in gyr_data: # found rate of turn pub_imu = True imu_msg.angular_velocity.x = gyr_data['gyrX'] imu_msg.angular_velocity.y = gyr_data['gyrY'] imu_msg.angular_velocity.z = gyr_data['gyrZ'] else: raise MTException("Unsupported message in XDI_AngularVelocityGroup.") if mag_data: # magfield ss_msg.internal.mag.x = mag_msg.magnetic_field.x = mag_data['magX'] ss_msg.internal.mag.y = mag_msg.magnetic_field.y = mag_data['magY'] ss_msg.internal.mag.z = mag_msg.magnetic_field.z = mag_data['magZ'] pub_mag = True if pressure_data: pub_baro = True height = baroPressureToHeight(pressure_data['Pressure']) baro_msg.height = ss_msg.internal.baro.height = height if gnss_data: pub_gnssPvt = True gnssPvt_msg.itow = gnss_data['iTOW'] gnssPvt_msg.fix = gnss_data['fix'] gnssPvt_msg.latitude = gnss_data['lat'] gnssPvt_msg.longitude = gnss_data['lon'] gnssPvt_msg.hEll = gnss_data['hEll'] gnssPvt_msg.hMsl = gnss_data['hMsl'] gnssPvt_msg.vel.x = gnss_data['velE'] gnssPvt_msg.vel.y = gnss_data['velN'] gnssPvt_msg.vel.z = -gnss_data['velD'] gnssPvt_msg.hAcc = gnss_data['horzAcc'] gnssPvt_msg.vAcc = gnss_data['vertAcc'] gnssPvt_msg.sAcc = gnss_data['speedAcc'] gnssPvt_msg.pDop = gnss_data['PDOP'] gnssPvt_msg.hDop = gnss_data['HDOP'] gnssPvt_msg.vDop = gnss_data['VDOP'] gnssPvt_msg.numSat = gnss_data['nSat'] gnssPvt_msg.heading = gnss_data['heading'] gnssPvt_msg.headingAcc = gnss_data['headingAcc'] if orient_data: if 'Q0' in orient_data: pub_imu = True imu_msg.orientation.x = orient_data['Q2'] #Q0 imu_msg.orientation.y = -orient_data['Q1'] #Q1 imu_msg.orientation.z = -orient_data['Q0'] #Q2 imu_msg.orientation.w = orient_data['Q3'] #Q3 #print orient_data elif 'Roll' in orient_data: pub_ori = True ori_msg.roll = orient_data['Roll'] ori_msg.pitch = orient_data['Pitch'] ori_msg.yaw = orient_data['Yaw'] else: raise MTException('Unsupported message in XDI_OrientationGroup') if velocity_data: pub_vel = True vel_msg.velE = velocity_data['velX'] vel_msg.velN = velocity_data['velY'] vel_msg.velU = velocity_data['velZ'] if position_data: pub_pos = True pos_msg.latitude = position_data['lat'] pos_msg.longitude = position_data['lon'] if altitude_data: pub_pos = True tempData = altitude_data['ellipsoid'] pos_msg.hEll = tempData[0] #if status is not None: # if status & 0b0001: # self.stest_stat.level = DiagnosticStatus.OK # self.stest_stat.message = "Ok" # else: # self.stest_stat.level = DiagnosticStatus.ERROR # self.stest_stat.message = "Failed" # if status & 0b0010: # self.xkf_stat.level = DiagnosticStatus.OK # self.xkf_stat.message = "Valid" # else: # self.xkf_stat.level = DiagnosticStatus.WARN # self.xkf_stat.message = "Invalid" # if status & 0b0100: # self.gps_stat.level = DiagnosticStatus.OK # self.gps_stat.message = "Ok" # else: # self.gps_stat.level = DiagnosticStatus.WARN # self.gps_stat.message = "No fix" # self.diag_msg.header = h # self.diag_pub.publish(self.diag_msg) # publish available information if pub_imu: imu_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time #imu_msg.header.stamp.secs = secs #imu_msg.header.stamp.nsecs = nsecs self.imu_pub.publish(imu_msg) #if pub_gps: # xgps_msg.header = gps_msg.header = h # self.gps_pub.publish(gps_msg) # self.xgps_pub.publish(xgps_msg) if pub_mag: mag_msg.header = h self.mag_pub.publish(mag_msg) #if pub_temp: # self.temp_pub.publish(temp_msg) if pub_ss: ss_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time ss_msg.header.stamp.secs = secs ss_msg.header.stamp.nsecs = nsecs self.ss_pub.publish(ss_msg) if pub_baro: baro_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time baro_msg.header.stamp.secs = secs baro_msg.header.stamp.nsecs = nsecs self.baro_pub.publish(baro_msg) if pub_gnssPvt: gnssPvt_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time baro_msg.header.stamp.secs = secs baro_msg.header.stamp.nsecs = nsecs self.gnssPvt_pub.publish(gnssPvt_msg) if pub_ori: ori_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time ori_msg.header.stamp.secs = secs ori_msg.header.stamp.nsecs = nsecs self.ori_pub.publish(ori_msg) if pub_vel: vel_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time vel_msg.header.stamp.secs = secs vel_msg.header.stamp.nsecs = nsecs self.vel_pub.publish(vel_msg) if pub_pos: pos_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time pos_msg.header.stamp.secs = secs pos_msg.header.stamp.nsecs = nsecs self.pos_pub.publish(pos_msg)