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 publish_sensor_data(self, sensor_data): acc = np.array(sensor_data.acceleration.data) / 1000.0 * g ang_vel = np.array(sensor_data.angular_vel.data) / 180.0 * np.pi # ang_vel = rotate_v(ang_vel.tolist(), self.rotation) msg = Imu() msg.header.frame_id = self.base_frame_id msg.header.stamp = rospy.Time.now() msg.linear_acceleration = Vector3(*acc) # It happens that sensor_data.quaterion and pose.quaternion are NOT in the same frame q = sensor_data.quaternion.data q = [q[1], q[2], q[3], q[0]] msg.orientation = Quaternion(*q) # msg.orientation = Quaternion(*self.orientation) msg.angular_velocity = Vector3(*ang_vel) msg.linear_acceleration_covariance = self.linear_acceleration_cov msg.orientation_covariance = self.orientation_cov msg.angular_velocity_covariance = self.angular_velocity_cov self.imu_pub.publish(msg) mag = np.array(sensor_data.magnetic.data) * 1e-6 m_msg = MagneticField() m_msg.header = msg.header m_msg.magnetic_field = Vector3(*mag) m_msg.magnetic_field_covariance = self.magnetic_field_cov self.mag_pub.publish(m_msg)
def publish(self, 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 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 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 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 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 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 callbackImu(msg): # Grab accelerometer and gyro data. imu_msg = Imu() imu_msg.header = msg.header imu_msg.header.frame_id = 'imu_link' imu_msg.orientation_covariance = [0.09, 0, 0, \ 0, 0.09, 0, \ 0, 0, 0.09] 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) print(len(gyro_readings)) 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 parse_imu_data(imu_data): rospy.loginfo(imu_data) imu_msg = Imu() mag_msg = MagneticField() mag_field = Vector3() orient_field = Quaternion() omega_dot_field = Vector3() v_dot_field = Vector3() imu_msg.header.stamp = rospy.Time.now() mag_msg.header.stamp = rospy.Time.now() sections = line.split(',') yaw = radians(float(sections[1])) pitch = radians(float(sections[2])) roll = radians(float(sections[3])) w, x, y, z = euler_to_quaternion(yaw, pitch, roll) orient_field.x = x orient_field.y = y orient_field.z = z orient_field.w = w mag_field.x = float(sections[4]) mag_field.y = float(sections[5]) mag_field.z = float(sections[6]) v_dot_field.x = float(sections[7]) v_dot_field.y = float(sections[8]) v_dot_field.z = float(sections[9]) omega_dot_field.x = float(sections[10]) omega_dot_field.y = float(sections[11]) raw_omega_dot_Z = sections[12] omega_dot_field.z = float(raw_omega_dot_Z[:raw_omega_dot_Z.find("*")]) imu_msg.orientation = orient_field imu_msg.angular_velocity = omega_dot_field imu_msg.linear_acceleration = v_dot_field mag_msg.magnetic_field = mag_field return imu_msg, mag_msg
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 run(self): # parse time from log and create rostime instance start_time = datetime.strptime(self.log_filename[4:18], '%Y%m%d_%H%M%S') ros_start_time = rospy.Time(secs=mktime(start_time.timetuple())) bag = rosbag.Bag(self.save_folder + self.bag_name, 'w') SCAN = 0 TIME = 1 WRITE = 2 CONTINUE = -1 STATE = CONTINUE with open(self.save_folder + self.lidar_filename) as lidarData: initialTimeStamp = 0 Laserscan_msg = LaserScan() # generate the constant parts of the scan messages Laserscan_msg.header.frame_id = 'laser' Laserscan_msg.angle_increment = .125 * math.pi / 180 Laserscan_msg.angle_min = -95 * math.pi / 180 Laserscan_msg.angle_max = 95 * math.pi / 180 Laserscan_msg.range_min = .023 Laserscan_msg.range_max = 120. Laserscan_msg.scan_time = 0.05 Laserscan_msg.time_increment = Laserscan_msg.angle_increment / ( 2 * math.pi) * Laserscan_msg.scan_time for line in lidarData: if STATE == SCAN: scan = [float(x) / 1000 for x in line.split(';') ] #convert to meters and float Laserscan_msg.ranges = scan STATE = WRITE elif STATE == TIME: if initialTimeStamp == 0: initialTimeStamp = float(line) / 1000 laser_time = ros_start_time else: laser_time = ros_start_time + rospy.Duration.from_sec( float(line) / 1000 - initialTimeStamp) # add time difference from start Laserscan_msg.header.stamp = laser_time STATE = CONTINUE if STATE == CONTINUE: if re.match('\[timestamp\]', line): STATE = TIME elif re.match('\[scan\]', line): STATE = SCAN elif STATE == WRITE: bag.write('/scan', Laserscan_msg, t=laser_time) STATE = CONTINUE #print('Laser duration: %.3s\n' % (laser_time-ros_start_time)/10**9) with open(self.save_folder + self.log_filename, 'rb') as csvData: excelDialect = csv.excel() excelDialect.skipinitialspace = True reader = csv.DictReader(csvData, dialect=excelDialect) Imu0_msg = Imu() Mag0_msg = MagneticField() Imu1_msg = Imu() Mag1_msg = MagneticField() seq = 0 for line in reader: line = self.fix_csv_type(line) seq += 1 if len(line) != 0: line_time = ros_start_time + rospy.Duration( line['TimeFromStart'] ) # add time difference from start # define IMU0 in imu0_link Imu0_msg.angular_velocity = Vector3( x=line['AngRateX'] / 180, y=line['AngRateY'] / 180, z=line['AngRateZ'] / 180) Imu0_msg.linear_acceleration = Vector3(x=line['AccX'] * G, y=line['AccY'] * G, z=line['AccZ'] * G) Imu0_msg.header.stamp = line_time Imu0_msg.header.frame_id = 'imu0_link' # define MAG0 in imu0_link Mag0_msg.magnetic_field = Vector3( x=line['MagFieldX'] * 0.0001, y=line['MagFieldY'] * 0.0001, z=line['MagFieldZ'] * 0.0001) Mag0_msg.header.stamp = line_time Mag0_msg.header.frame_id = 'imu0_link' # define IMU1 in imu1_link Imu1_msg.angular_velocity = Vector3( x=line['AngRateX_2'] / 180, y=line['AngRateY_2'] / 180, z=line['AngRateZ_2'] / 180) Imu1_msg.linear_acceleration = Vector3( x=line['AccX_2'] * G, y=line['AccY_2'] * G, z=line['AccZ_2'] * G) Imu1_msg.header.stamp = line_time Imu1_msg.header.frame_id = 'imu1_link' # define MAG1 in imu1_link Mag1_msg.magnetic_field = Vector3( x=line['MagFieldX_2'] * 0.0001, y=line['MagFieldY_2'] * 0.0001, z=line['MagFieldZ_2'] * 0.0001) Mag1_msg.header.stamp = line_time Mag1_msg.header.frame_id = 'imu1_link' # write in the bag bag.write('/imu0/data_raw', Imu0_msg, t=line_time) bag.write('/imu0/mag', Mag0_msg, t=line_time) bag.write('/imu1/data_raw', Imu1_msg, t=line_time) bag.write('/imu1/mag', Mag1_msg, t=line_time) print 'Done' bag.close()
def mag_callback(in_msg): out_msg = Mag_msg() out_msg.header = in_msg.header out_msg.magnetic_field = in_msg.vector set_covariance_as_identity(out_msg.magnetic_field_covariance) mag_publisher.publish(out_msg)
def main(): #SETTINGS_FILE = "/home/ubuntu/catkin_ws/src/urc_2018/src/RTIMULib.ini" SETTINGS_FILE = "/home/yonder/2018_URC/src/RTIMULib" s = RTIMU.Settings(SETTINGS_FILE) print(s) imu = RTIMU.RTIMU(s) if (not imu.IMUInit()): print("Failed to init IMU") imu.setGyroEnable(True) imu.setAccelEnable(True) imu.setCompassEnable(False) poll_interval = imu.IMUGetPollInterval() pubIMU = rospy.Publisher('imu/data', Imu, queue_size=10) #pubMag = rospy.Publisher('/imu/magnetic_field', MagneticField, queue_size=10) #pubGPS = rospy.Publisher('/arduino/gps', NavSatFix, queue_size=10) rospy.init_node('imu', anonymous=True) r = rospy.Rate(1000 / imu.IMUGetPollInterval()) while not rospy.is_shutdown(): if imu.IMURead(): data = imu.getIMUData() gyro = data["gyro"] # subtract gravity rot = pyquaternion.Quaternion(axis=[0, 0, 1], degrees=0) q = pyquaternion.Quaternion(data["fusionQPose"][2], data["fusionQPose"][1], data["fusionQPose"][0], data["fusionQPose"][3]) q1 = pyquaternion.Quaternion(data["fusionQPose"][0], data["fusionQPose"][1], data["fusionQPose"][2], data["fusionQPose"][3]) q = q * rot grav = q.rotate([0, 0, -1]) print(grav) #print(np.multiply(grav, 9.80665)) accel = data["accel"] #accel = np.add(accel, grav) acc = Vector3() acc.x = accel[0] * 9.80665 acc.y = accel[1] * -9.80665 acc.z = accel[2] * 9.80665 gyro = Vector3() gyro.x = -data["gyro"][0] gyro.y = data["gyro"][1] gyro.z = data["gyro"][2] msg = MagneticField() msg.header.frame_id = IMU_FRAME_ID mag = Vector3() mag.x = data["compass"][0] / 1000000 mag.y = data["compass"][1] / 1000000 mag.z = data["compass"][2] / 1000000 msg.magnetic_field = mag quat = Quaternion() """ quat.x = data["fusionQPose"][0] quat.y = data["fusionQPose"][1] quat.z = data["fusionQPose"][2] quat.w = data["fusionQPose"][3] """ quat.x = q[0] quat.y = q[1] quat.z = q[2] quat.w = q[3] imu_dat = Imu() imu_dat.header.frame_id = IMU_FRAME_ID imu_dat.header.stamp = rospy.Time.now() imu_dat.linear_acceleration = acc imu_dat.orientation_covariance = [0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1] imu_dat.angular_velocity = gyro imu_dat.angular_velocity_covariance = [ 0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1 ] imu_dat.orientation = quat imu_dat.linear_acceleration_covariance = [ 0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1 ] pubIMU.publish(imu_dat) print(imu_dat) r.sleep()
def publish(self, data): if not self._calib and data.getImuMsgId() == PUB_ID: q = data.getOrientation() roll, pitch, yaw = euler_from_quaternion((q.y, q.z, q.w, q.x)) #array = quaternion_from_euler(yaw + (self._angle * pi / 180), roll, pitch) #array = quaternion_from_euler(yaw + (self._angle * pi / 180), -1 (roll - 180), -1 * pitch) array = quaternion_from_euler((yaw + (self._angle * pi / 180)), roll, -1 * pitch) 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)
acc.x = accel[0] * 9.80665 acc.y = accel[1] * 9.80665 acc.z = accel[2] * 9.80665 gyro = Vector3() gyro.x = -data["gyro"][0] gyro.y = data["gyro"][1] gyro.z = -data["gyro"][2] msg = MagneticField() msg.header.frame_id = IMU_FRAME_ID mag = Vector3() mag.x = data["compass"][0] / 1000000 mag.y = data["compass"][1] / 1000000 mag.z = data["compass"][2] / 1000000 msg.magnetic_field = mag quat = Quaternion() """ quat.x = data["fusionQPose"][0] quat.y = data["fusionQPose"][1] quat.z = data["fusionQPose"][2] quat.w = data["fusionQPose"][3] """ quat.x = q[1] quat.y = q[2] quat.z = q[3] quat.w = q[0] imu_dat = Imu() imu_dat.header.frame_id = IMU_FRAME_ID