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 rawimu_handler(self, rawimus): imu = Imu() imu.header.stamp = rospy.Time.now() imu.header.frame_id = self.base_frame # Populate orientation field with one from inspvax message. imu.orientation = Quaternion(*self.orientation) imu.orientation_covariance = self.orientation_covariance # Angular rates (rad/s) # corrimudata log provides instantaneous rates so multiply by IMU rate in Hz imu.angular_velocity.x = rawimus.x_gyro * 0.1 / (3600.0 * 256.0) * self.imu_rate imu.angular_velocity.y = -rawimus.my_gyro * 0.1 / ( 3600.0 * 256.0) * self.imu_rate imu.angular_velocity.z = rawimus.z_gyro * 0.1 / (3600.0 * 256.0) * self.imu_rate imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration (m/s^2) imu.linear_acceleration.x = rawimus.x_accel * 0.05 / 2**15 * self.imu_rate imu.linear_acceleration.y = rawimus.my_accel * 0.05 / 2**15 * self.imu_rate imu.linear_acceleration.z = rawimus.z_accel * 0.05 / 2**15 * self.imu_rate imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu) pass
def corrimudata_handler(self, corrimudata): # TODO: Work out these covariances properly. Logs provide covariances in local frame, not body imu = Imu() imu.header.stamp = rospy.Time.now() # imu.header.frame_id = self.base_frame imu.header.frame_id = 'imu_link' # Populate orientation field with one from inspvax message. imu.orientation = Quaternion(*self.orientation) imu.orientation_covariance = self.orientation_covariance # Angular rates (rad/s) # corrimudata log provides instantaneous rates so multiply by IMU rate in Hz imu.angular_velocity.x = corrimudata.pitch_rate * self.imu_rate imu.angular_velocity.y = corrimudata.roll_rate * self.imu_rate imu.angular_velocity.z = corrimudata.yaw_rate * self.imu_rate imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration (m/s^2) imu.linear_acceleration.x = corrimudata.x_accel * self.imu_rate imu.linear_acceleration.y = corrimudata.y_accel * self.imu_rate imu.linear_acceleration.z = corrimudata.z_accel * self.imu_rate + 9.7803 imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu)
def fake_imu(): pub = rospy.Publisher('IMU_ned', Imu, queue_size=10) rospy.init_node('test_convertimu', anonymous=True) rate = rospy.Rate(0.5) # 10hz while not rospy.is_shutdown(): imu_message=Imu() imu_message.header.stamp=rospy.Time.now() imu_message.header.frame_id="IMU" imu_message.orientation.x=1 imu_message.orientation.y=2 imu_message.orientation.z=3 imu_message.orientation.w=4 imu_message.orientation_covariance=[-1,0,0,0,-1,0,0,0,-1] imu_message.linear_acceleration.x=6 imu_message.linear_acceleration.y=7 imu_message.linear_acceleration.z=8 imu_message.linear_acceleration_covariance=[1e6, 0,0,0,1e6,0,0,0,1e6] imu_message.angular_velocity.x=10 imu_message.angular_velocity.y=11 imu_message.angular_velocity.z=12 imu_message.angular_velocity_covariance=[1e6, 0,0,0,1e6,0,0,0,1e6] pub.publish(imu_message) rate.sleep()
def Publish(self): imu_data = self._hal_proxy.GetImu() imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self._frame_id imu_msg.header = h imu_msg.orientation_covariance = (-1., )*9 imu_msg.linear_acceleration_covariance = (-1., )*9 imu_msg.angular_velocity_covariance = (-1., )*9 imu_msg.orientation.w = imu_data['orientation']['w'] imu_msg.orientation.x = imu_data['orientation']['x'] imu_msg.orientation.y = imu_data['orientation']['y'] imu_msg.orientation.z = imu_data['orientation']['z'] imu_msg.linear_acceleration.x = imu_data['linear_accel']['x'] imu_msg.linear_acceleration.y = imu_data['linear_accel']['y'] imu_msg.linear_acceleration.z = imu_data['linear_accel']['z'] imu_msg.angular_velocity.x = imu_data['angular_velocity']['x'] imu_msg.angular_velocity.y = imu_data['angular_velocity']['y'] imu_msg.angular_velocity.z = imu_data['angular_velocity']['z'] # Read the x, y, z and heading self._publisher.publish(imu_msg)
def publish(self, theta, omega): msg = Imu() msg.header = self.header msg.angular_velocity.x = omega[0]*1 msg.angular_velocity.y = omega[1]*1 msg.angular_velocity.z = omega[2]*1 msg.orientation = self.thetaToOrientation(theta) grav = np.array([0,0,self.gravity], dtype=np.float64) r = self.thetaToRotation(theta) grav = r.apply(grav, True) pureAcceleration = self.accel - grav msg.linear_acceleration.x = pureAcceleration[0]*1 msg.linear_acceleration.y = pureAcceleration[1]*1 msg.linear_acceleration.z = pureAcceleration[2]*1 cov = np.zeros(9, dtype=np.float64) msg.angular_velocity_covariance = cov msg.orientation_covariance = cov msg.linear_acceleration_covariance = cov self.publisher.publish(msg)
def unpackIMU(self, data): imu_msg = Imu() #Unpack Header imu_msg.header = self.unpackIMUHeader(data[0:19]) #Unpack Orientation Message quat = self.bytestr_to_array(data[19:19 + (4*8)]) imu_msg.orientation.x = quat[0] imu_msg.orientation.y = quat[1] imu_msg.orientation.z = quat[2] imu_msg.orientation.w = quat[3] #Unpack Orientation Covariance imu_msg.orientation_covariance = list(self.bytestr_to_array(data[55:(55 + (9*8))])) #Unpack Angular Velocity ang = self.bytestr_to_array(data[127: 127 + (3*8)]) imu_msg.angular_velocity.x = ang[0] imu_msg.angular_velocity.y = ang[1] imu_msg.angular_velocity.z = ang[2] #Unpack Angular Velocity Covariance imu_msg.angular_velocity_covariance = list(self.bytestr_to_array(data[155:(155 + (9*8))])) #Unpack Linear Acceleration lin = self.bytestr_to_array(data[227: 227 + (3*8)]) imu_msg.linear_acceleration.x = lin[0] imu_msg.linear_acceleration.y = lin[1] imu_msg.linear_acceleration.z = lin[2] #Unpack Linear Acceleration Covariance imu_msg.linear_acceleration_covariance = list(self.bytestr_to_array(data[255:(255 + (9*8))])) return imu_msg
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 cb_all_data(self, acceleration, magnetic_field, angular_velocity, euler_angle, quaternion, linear_acceleration, gravity_vector, temperature, calibration_status): msg = Imu() msg.header.stamp = rospy.Time.now() msg.header.frame_id = "base_imu_link" msg.orientation.x = quaternion[1] / 16383.0 msg.orientation.y = quaternion[2] / 16383.0 msg.orientation.z = quaternion[3] / 16383.0 msg.orientation.w = quaternion[0] / 16383.0 # Observed orientation variance: 0.0 (10k samples) # Magnometer heading accuracy is +-2.5 deg => 0.088 rad # With heading accuracy as std dev, variance = 0.088^2 = 0.008 msg.orientation_covariance = [0.008, 0, 0, 0, 0.008, 0, 0, 0, 0.008] msg.angular_velocity.x = angular_velocity[0] / 16.0 * pi / 180 msg.angular_velocity.y = angular_velocity[1] / 16.0 * pi / 180 msg.angular_velocity.z = angular_velocity[2] / 16.0 * pi / 180 # Observed angular velocity variance: 0.006223 (10k samples), => round up to 0.02 msg.angular_velocity_covariance = [0.02, 0, 0, 0, 0.02, 0, 0, 0, 0.02] msg.linear_acceleration.x = acceleration[0] / 100.0 msg.linear_acceleration.y = acceleration[1] / 100.0 msg.linear_acceleration.z = acceleration[2] / 100.0 # Observed linear acceleration variance: 0.001532 (10k samples) # Calculation for variance taken from razor imu: # nonliniarity spec: 1% of full scale (+-2G) => 0.2m/s^2 # Choosing 0.2 as std dev, variance = 0.2^2 = 0.04 msg.linear_acceleration_covariance = [ 0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.04 ] self.pub_imu.publish(msg)
def createMessage(self): xQ, yQ, zQ, wQ = self.toQuaternion(self.degrees) deltaAngle = math.radians(float(self.degrees - self.lastDegrees)) deltaTime = float(self.lastMeasurementTime - time()) wz = deltaAngle / deltaTime self.lastMeasurementTime = time() self.lastDegrees = self.degrees data = Imu() data.header.stamp = rospy.Time.now() data.header.frame_id = "imu_link" data.header.seq = self.seq data.orientation.w = wQ data.orientation.x = xQ data.orientation.y = yQ data.orientation.z = zQ data.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] data.linear_acceleration.x = 0.0 data.linear_acceleration.y = 0.0 data.linear_acceleration.z = 9.8 data.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] data.angular_velocity.x = 0.0 data.angular_velocity.y = 0.0 data.angular_velocity.z = wz data.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] return data
def loop(self): while not rospy.is_shutdown(): line = self.port.readline() chunks = line.split(":") if chunks[0] == "!QUAT": readings = chunks[1].split(",") if len(readings) == 10: imu_msg = Imu() imu_msg.header.stamp = rospy.Time.now() imu_msg.header.frame_id = 'imu' imu_msg.orientation.x = float(readings[0]) imu_msg.orientation.y = float(readings[1]) imu_msg.orientation.z = float(readings[2]) imu_msg.orientation.w = float(readings[3]) imu_msg.orientation_covariance = list(zeros((3,3)).flatten()) imu_msg.angular_velocity.x = float(readings[4]) imu_msg.angular_velocity.y = float(readings[5]) imu_msg.angular_velocity.z = float(readings[6]) imu_msg.angular_velocity_covariance = list(0.1*diagflat(ones((3,1))).flatten()) imu_msg.linear_acceleration.x = float(readings[7]) imu_msg.linear_acceleration.y = float(readings[8]) imu_msg.linear_acceleration.z = float(readings[9]) imu_msg.linear_acceleration_covariance = list(0.1*diagflat(ones((3,1))).flatten()) self.pub.publish(imu_msg) quaternion = (imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w) tf_br.sendTransform(translation = (0,0, 0), rotation = quaternion,time = rospy.Time.now(),child = 'imu',parent = 'world') else: rospy.logerr("Did not get a valid IMU packet, got %s", line) else: rospy.loginfo("Did not receive IMU data, instead got %s", line)
def main(): rospy.init_node('accelerometer', anonymous=False) pub = rospy.Publisher("imu", Imu, queue_size=10) rospy.loginfo('MMA8451 3DOF Accelerometer Publishing to IMU') i2c = busio.I2C(board.SCL, board.SDA) sensor = adafruit_lsm6ds.LSM6DSOX(i2c) imu_msg = Imu() imu_msg.linear_acceleration_covariance = [ 0, 0, 0, 0, 0, 0, 0, 0, 0 ] imu_msg.angular_velocity_covariance = [ 0, 0, 0, 0, 0, 0, 0, 0, 0 ] while not rospy.is_shutdown(): x, y, z = sensor.acceleration imu_msg.linear_acceleration.x = x imu_msg.linear_acceleration.y = y imu_msg.linear_acceleration.z = z pub.publish(imu_msg) rospy.sleep(1) rospy.logwarn('MMA8451 Accelerometer Offline')
def publish_imu_data(): global imu, imu_publisher imu_data = Imu() imu_data.header.frame_id = "imu" imu_data.header.stamp = rospy.Time.from_sec(imu.last_update_time) # imu 3dmgx1 reports using the North-East-Down (NED) convention # so we need to convert to East-North-Up (ENU) coordinates according to ROS REP103 # see http://answers.ros.org/question/626/quaternion-from-imu-interpreted-incorrectly-by-ros q = imu.orientation imu_data.orientation.w = q[0] imu_data.orientation.x = q[2] imu_data.orientation.y = q[1] imu_data.orientation.z = -q[3] imu_data.orientation_covariance = Config.get_orientation_covariance() av = imu.angular_velocity imu_data.angular_velocity.x = av[1] imu_data.angular_velocity.y = av[0] imu_data.angular_velocity.z = -av[2] imu_data.angular_velocity_covariance = Config.get_angular_velocity_covariance( ) la = imu.linear_acceleration imu_data.linear_acceleration.x = la[1] imu_data.linear_acceleration.y = la[0] imu_data.linear_acceleration.z = -la[2] imu_data.linear_acceleration_covariance = Config.get_linear_acceleration_covariance( ) imu_publisher.publish(imu_data)
def convert_imu(imu): ret = Imu() ret.header = convert_utime(imu.utime) ret.orientation = rpy_to_quat(*imu.tb_angles) ret.orientation_covariance = [0]*9 gyro = imu.gyro w = ret.angular_velocity w.x, w.y, w.z = gyro # 5 dps tolerance at 0 with 3 percent linear tolerance # 2 percent cross axis sensitivity rps = 5/180*math.pi cross = 0.02**2 ret.angular_velocity_covariance = [ (rps + gyro[0]*.03)**2, cross*(gyro[0]*gyro[1]), cross*(gyro[0]*gyro[2]), cross*(gyro[0]*gyro[1]), (rps + gyro[1]*.03)**2, cross*(gyro[1]*gyro[2]), cross*(gyro[0]*gyro[2]), cross*(gyro[1]*gyro[2]), (rps + gyro[2]*.03)**2 ] accel = imu.accel a = ret.linear_acceleration a.x, a.y, a.z = accel # 3 percent sensitivity with 2 percent cross axis sensitivity ret.linear_acceleration_covariance = [ (accel[0]*0.03)**2, cross*(accel[0]*accel[1]), cross*(accel[0]*accel[2]), cross*(accel[0]*accel[1]), (accel[1]*0.03)**2, cross*(accel[1]*accel[2]), cross*(accel[0]*accel[2]), cross*(accel[1]*accel[2]), (accel[2]*0.03)**2 ] return ret
def talker(aArgs): pub = rospy.Publisher('ronin_imu', Imu, queue_size=10) rospy.init_node('circle_test_source', anonymous=True) rate = rospy.Rate(aArgs["sampling_freq"]) # Hz step = 0 imu = Imu() imu.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] imu.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] imu.orientation.x = 0.0 imu.orientation.y = 0.0 imu.orientation.z = 0.0 imu.orientation.w = 0.0 imu.angular_velocity.x = 0.0 imu.angular_velocity.y = 0.0 imu.linear_acceleration.z = 9.81 while not rospy.is_shutdown(): imu.header.seq = step imu.header.stamp = rospy.Time.now() angle = 2.0 * math.pi * aArgs["envelope_freq"] / aArgs[ "sampling_freq"] * step omega = aArgs["omega_max"] * (1.0 - math.cos(angle)) / 2.0 imu.angular_velocity.z = omega imu.linear_acceleration.x = math.pi * aArgs["envelope_freq"] * aArgs[ "omega_max"] * math.sin(angle) * aArgs["radius"] imu.linear_acceleration.y = omega * omega * aArgs["radius"] pub.publish(imu) step = step + 1 rate.sleep()
def cmd_sync_callback(self, msg_pres_int, msg_imu_int): msg_pres = FluidPressure(); msg_pres.header.stamp = rospy.Time.now() msg_pres.header.frame_id = msg_pres_int.header.frame_id msg_pres.fluid_pressure = (msg_pres_int.fluid_pressure * 1000) - self.atm_press self.pub_pres.publish(msg_pres) msg_imu = Imu(); msg_imu.header.stamp = rospy.Time.now() msg_imu.header.frame_id = "/mur/imu_link" qx = msg_imu_int.orientation.x qy = msg_imu_int.orientation.y qz = msg_imu_int.orientation.z qw = msg_imu_int.orientation.w euler_angles = euler_from_quaternion(np.array([qx,qy,qz,qw])) r = euler_angles[1] p = -euler_angles[0] y = euler_angles[2] q = quaternion_from_euler(r,p,y) msg_imu.orientation.x = q[0] msg_imu.orientation.y = q[1] msg_imu.orientation.z = q[2] msg_imu.orientation.w = q[3] msg_imu.orientation_covariance = np.array([0,0,0,0,0,0,0,0,0]) msg_imu.angular_velocity.x = -msg_imu_int.angular_velocity.x msg_imu.angular_velocity.y = msg_imu_int.angular_velocity.y msg_imu.angular_velocity.z = msg_imu_int.angular_velocity.z msg_imu.angular_velocity_covariance = np.array([1.2184E-7,0.0,0.0,0.0,1.2184E-7,0.0,0.0,0.0,1.2184E-7]) msg_imu.linear_acceleration.x = msg_imu_int.linear_acceleration.x # The same configuration in the PIXHAWK msg_imu.linear_acceleration.y = msg_imu_int.linear_acceleration.y msg_imu.linear_acceleration.z = msg_imu_int.linear_acceleration.z msg_imu.linear_acceleration_covariance = np.array([9.0E-8,0.0,0.0,0.0,9.0E-8,0.0,0.0,0.0,9.0E-8]) self.pub_imu.publish(msg_imu)
def grv_data_handler(self, timestamp, grv_w, grv_x, grv_y, grv_z): # publish Imu gyr_scale = (500.0 / 32768.0) / 180.0 * math.pi # 500dps FSR acc_scale = (4.0 / 32768.0) * 9.81 # 4g FSR msg = Imu() msg.header.stamp = rospy.get_rostime() #msg.header.frame_id = 'base_link' # for test msg.angular_velocity.x = self.gyr_raw_x * gyr_scale msg.angular_velocity.y = self.gyr_raw_y * gyr_scale msg.angular_velocity.z = self.gyr_raw_z * gyr_scale msg.angular_velocity_covariance = [ 0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01 ] msg.linear_acceleration.x = self.acc_raw_x * acc_scale msg.linear_acceleration.y = self.acc_raw_y * acc_scale msg.linear_acceleration.z = self.acc_raw_z * acc_scale msg.linear_acceleration_covariance = [ 0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01 ] msg.orientation.w = float(grv_w) / 2**30 msg.orientation.x = float(grv_x) / 2**30 msg.orientation.y = float(grv_y) / 2**30 msg.orientation.z = float(grv_z) / 2**30 msg.orientation_covariance = [ 0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01 ] self.send(self.pub_imu, msg)
def callback_imu(data): # rospy.loginfo(data.poses[]) imu_data = Imu() imu_data.header.frame_id = "base_link" # poses[0].position-> ax, ay, az (linear acc) imu_data.linear_acceleration.x = data.poses[0].position.x imu_data.linear_acceleration.y = data.poses[0].position.y imu_data.linear_acceleration.z = data.poses[0].position.z #poses[0].orientation orientation quaternion imu_data.orientation.x = data.poses[0].orientation.x imu_data.orientation.y = data.poses[0].orientation.y imu_data.orientation.z = data.poses[0].orientation.z imu_data.orientation.w = data.poses[0].orientation.w #poses[1].position gx gy gz imu_data.angular_velocity.x = data.poses[1].position.x imu_data.angular_velocity.y = data.poses[1].position.y imu_data.angular_velocity.z = data.poses[1].position.z imu_data.angular_velocity_covariance = [1e-3, 0, 0, 0, 1e-3, 0, 0, 0, 1e-3] imu_data.linear_acceleration_covariance = [ 1e-3, 0, 0, 0, 1e-3, 0, 0, 0, 1e-3 ] imu_data.orientation_covariance = [1e-3, 0, 0, 0, 1e-3, 0, 0, 0, 1e-3] imu_data.header.stamp = rospy.Time.now() pub.publish(imu_data)
def talker(): imu_data = Imu() odom = Odometry() imu_pub = rospy.Publisher('Imu', Imu, queue_size=1) odom_pub = rospy.Publisher('Odom', Odometry, queue_size=1) rospy.init_node('LISTEN_ODOM_IMU') rate = rospy.Rate(100) while not rospy.is_shutdown(): h = Header() h.stamp = rospy.Time.now() h.frame_id = 'IMU (BASE)' imu_data.header = h q, g, a, twist_linear, twist_angular, pose_pt = serial_listen() imu_data.orientation = q imu_data.angular_velocity = g imu_data.linear_acceleration = a covariance = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] imu_data.orientation_covariance = covariance imu_data.angular_velocity_covariance = covariance imu_data.linear_acceleration_covariance = covariance odom.twist.twist.linear = twist_linear odom.twist.twist.angular = twist_angular odom.pose.pose.position = pose_pt imu_pub.publish(imu_data) odom_pub.publish(odom) rospy.loginfo('PUBLISHED...') rate.sleep()
def ros_imu(info): for index, item in enumerate(info): info[index] = float(item) imuMsg = Imu() imuMsg.orientation_covariance = [0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025] imuMsg.angular_velocity_covariance = [0.02, 0, 0, 0, 0.02, 0, 0, 0, 0.02] imuMsg.linear_acceleration_covariance = [ 0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.04 ] Accel_magnitude = math.sprt(info[0] * info[0] + info[1] * info[1] + info[2] * info[2]) Accel_magnitude = float(Accel_magnitude / GRAVITY) pitch = -math.atan2(info[1], math.sqrt(info[2] * info[2] + info[3] * info[3])) roll = -math.atan2(info[2], info[3]) xh = info[7] * math.cos(pitch) + info[8] * math.sin(roll) * math.sin( pitch) + info[9] * math.sin(pitch) * math.cos(roll) yh = info[8] * math.cos(roll) + info[9] * math.sin(roll) yaw = math.atan2(-yh, xh) seq = 0 #print roll q = quaternion_from_euler(roll, pitch, yaw) imuMsg.orientation.x = q[0] imuMsg.orientation.y = q[1] imuMsg.orientation.z = q[2] imuMsg.orientation.w = q[3] imuMsg.header.stamp = rospy.Time.now() imuMsg.header.frame_id = 'base_imu_link' imuMsg.header.seq = seq seq = seq + 1 imu_pose(imuMsg) gpspub.publish(imuMsg)
def publish_imu(self): imu_msg = Imu() imu_msg.header.stamp = self.time imu_msg.header.frame_id = 'imu_odom' #quat = tf_math.quaternion_from_euler(self.kalman_state[0,0],self.kalman_state[1,0],self.kalman_state[2,0], axes='sxyz') a = self.kalman_state[3,0] b = self.kalman_state[4,0] c = self.kalman_state[5,0] d = self.kalman_state[6,0] q = math.sqrt(math.pow(a,2)+math.pow(b,2)+math.pow(c,2)+math.pow(d,2)) #angles = tf_math.euler_from_quaternion(self.kalman_state[3:7,0]) imu_msg.orientation.x = a/q#angles[0] imu_msg.orientation.y = b/q imu_msg.orientation.z = c/q imu_msg.orientation.w = d/q imu_msg.orientation_covariance = list(self.kalman_covariance[3:6,3:6].flatten()) imu_msg.angular_velocity.x = self.kalman_state[0,0] imu_msg.angular_velocity.y = self.kalman_state[1,0] imu_msg.angular_velocity.z = self.kalman_state[2,0] imu_msg.angular_velocity_covariance = list(self.kalman_covariance[0:3,0:3].flatten()) imu_msg.linear_acceleration.x = self.kalman_state[10,0] imu_msg.linear_acceleration.y = self.kalman_state[11,0] imu_msg.linear_acceleration.z = self.kalman_state[12,0] imu_msg.linear_acceleration_covariance = list(self.kalman_covariance[10:13,10:13].flatten()) self.pub.publish(imu_msg)
def publish(self): imu_msg = Imu() imu_msg.header.stamp = rospy.get_rostime() imu_msg.header.frame_id = "imu_link" imu_msg.linear_acceleration_covariance = self.linear_acceleration_covariance imu_msg.angular_velocity_covariance = self.angular_velocity_covariance #imu_msg.orientation_covariance = self.orientation_covariance imu_msg.linear_acceleration.x = self.linear_acceleration[0] - self.la_offset[0] imu_msg.linear_acceleration.y = self.linear_acceleration[1] - self.la_offset[1] imu_msg.linear_acceleration.z = self.linear_acceleration[2] - self.la_offset[2] imu_msg.angular_velocity.x = (self.angular_velocity[0] - self.av_offset[0]) * (np.pi / 180.0) imu_msg.angular_velocity.y = (self.angular_velocity[1] - self.av_offset[1]) * (np.pi / 180.0) imu_msg.angular_velocity.z = (self.angular_velocity[2] - self.av_offset[2]) * (np.pi / 180.0) #if abs(self.angular_velocity[2] - self.av_offset[2]) < 3: # imu_msg.linear_acceleration.x = 0.0 # imu_msg.linear_acceleration.y = 0.0 # imu_msg.linear_acceleration.z = 0.0 # imu_msg.angular_velocity.x = 0.0 # imu_msg.angular_velocity.y = 0.0 # imu_msg.angular_velocity.z = 0.0 self.imu_pub.publish(imu_msg)
def _get_sensor_msg_imu(self, timestamp): # 1 g-unit is equal to 9.80665 meter/square second. # Linear Acceleration # 1° × pi/180 = 0.01745rad # Angular velocity # Gyro gunit_to_mps_squ = 9.80665 msg = Imu() msg.header.stamp = timestamp msg.header.frame_id = IMU_FRAME gyr = self.sense.get_gyroscope_raw() acc = self.sense.get_accelerometer_raw() msg.orientation.x = 0.0 msg.orientation.y = 0.0 msg.orientation.z = 0.0 msg.orientation.w = 0.0 msg.orientation_covariance = [-1, 0.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0] #msg.angular_velocity.x = gyr['x'] * np.pi msg.angular_velocity.x = round( gyr['x'] * ( np.pi / 180 ), 8 ) * (-1) # Inverted x axis msg.angular_velocity.y = round( gyr['y'] * ( np.pi / 180 ), 8 ) msg.angular_velocity.z = round( gyr['z'] * ( np.pi / 180 ), 8 ) msg.angular_velocity_covariance = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] msg.linear_acceleration.x = round((acc['x'] * gunit_to_mps_squ), 8 ) * (-1) # Inverted x axis msg.linear_acceleration.y = round((acc['y'] * gunit_to_mps_squ), 8 ) msg.linear_acceleration.z = round((acc['z'] * gunit_to_mps_squ), 8 ) #msg.linear_acceleration.x = np.radians( acc['x'] ) #msg.linear_acceleration.y = np.radians( acc['y'] ) #msg.linear_acceleration.z = np.radians( acc['z'] ) msg.linear_acceleration_covariance = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] return msg
def get_rotation(msg): global roll, pitch, yaw orientation_q = msg.orientation orientation_list = [ orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w ] (roll, pitch, yaw) = euler_from_quaternion(orientation_list) print 'yaw', yaw, 'roll', roll, 'pitch', pitch current_time = rospy.Time.now() imu = Imu() imu.header.stamp = current_time imu.header.frame_id = "base_link" imu.orientation.x = -msg.orientation.y #pitch imu.orientation.y = msg.orientation.x #roll imu.orientation.z = msg.orientation.z #yaw imu.orientation.w = msg.orientation.w imu.orientation_covariance = [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01] imu.angular_velocity.x = -msg.angular_velocity.y imu.angular_velocity.y = msg.angular_velocity.x imu.angular_velocity.z = msg.angular_velocity.z imu.angular_velocity_covariance = [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01] imu.linear_acceleration.x = -msg.linear_acceleration.y imu.linear_acceleration.y = msg.linear_acceleration.x imu.linear_acceleration.z = msg.linear_acceleration.z imu.linear_acceleration_covariance = [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01] euler_pub.publish(imu)
def update_sensors(self): # Accelerometer accel = self._driver.get_accelerometer() accel_msg = Imu() accel_msg.header.stamp = rospy.Time.now() accel_msg.header.frame_id = self._name + "/base_link" accel_msg.linear_acceleration.x = ( accel[1] - 2048.0 ) / 800.0 * 9.81 # 1 g = about 800, then transforms in m/s^2. accel_msg.linear_acceleration.y = (accel[0] - 2048.0) / 800.0 * 9.81 accel_msg.linear_acceleration.z = (accel[2] - 2048.0) / 800.0 * 9.81 accel_msg.linear_acceleration_covariance = [ 0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01 ] #print "accel raw: " + str(accel[0]) + ", " + str(accel[1]) + ", " + str(accel[2]) #print "accel (m/s2): " + str((accel[0]-2048.0)/800.0*9.81) + ", " + str((accel[1]-2048.0)/800.0*9.81) + ", " + str((accel[2]-2048.0)/800.0*9.81) accel_msg.angular_velocity.x = 0 accel_msg.angular_velocity.y = 0 accel_msg.angular_velocity.z = 0 accel_msg.angular_velocity_covariance = [ 0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01 ] q = tf.transformations.quaternion_from_euler(0, 0, 0) accel_msg.orientation = Quaternion(*q) accel_msg.orientation_covariance = [ 0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01 ] self.accel_publisher.publish(accel_msg)
def parseImuData(pData, qx_pub, qy_pub, qz_pub, qw_pub, imu_pub): parseData = pData.split('|') if len(parseData) == 6: qxVal = float(parseData[1]) qyVal = float(parseData[2]) qzVal = float(parseData[3]) qwVal = float(parseData[4]) qx_pub.publish(qxVal) qy_pub.publish(qyVal) qz_pub.publish(qzVal) qw_pub.publish(qwVal) imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = "base_link" imu_msg.header = h imu_msg.orientation_covariance = (-1., ) * 9 imu_msg.angular_velocity_covariance = (-1., ) * 9 imu_msg.linear_acceleration_covariance = (-1., ) * 9 imu_msg.orientation.x = qxVal imu_msg.orientation.y = qyVal imu_msg.orientation.z = qzVal imu_msg.orientation.w = qwVal imu_pub.publish(imu_msg)
def publish_imu_data(): global imu, imu_publisher imu_data = Imu() imu_data.header.frame_id = "imu" imu_data.header.stamp = rospy.Time.from_sec(imu.last_update_time) # imu 3dmgx1 reports using the North-East-Down (NED) convention # so we need to convert to East-North-Up (ENU) coordinates according to ROS REP103 # see http://answers.ros.org/question/626/quaternion-from-imu-interpreted-incorrectly-by-ros q = imu.orientation imu_data.orientation.w = q[0] imu_data.orientation.x = q[2] imu_data.orientation.y = q[1] imu_data.orientation.z = -q[3] imu_data.orientation_covariance = Config.get_orientation_covariance() av = imu.angular_velocity imu_data.angular_velocity.x = av[1] imu_data.angular_velocity.y = av[0] imu_data.angular_velocity.z = -av[2] imu_data.angular_velocity_covariance = Config.get_angular_velocity_covariance() la = imu.linear_acceleration imu_data.linear_acceleration.x = la[1] imu_data.linear_acceleration.y = la[0] imu_data.linear_acceleration.z = - la[2] imu_data.linear_acceleration_covariance = Config.get_linear_acceleration_covariance() imu_publisher.publish(imu_data)
def main(): rospy.init_node('SensorLocalisationReciever', anonymous=True) print("Running SensorLocalisationReciever") # ser = serial.Serial(rospy.get_param('~sensorserialport','/dev/ttyACM0')) imu_data = Imu() gps_data = NavSatFix() imu_data.header.frame_id = 'base_link' gps_data.header.frame_id = 'map' rate = rospy.Rate(rospy.get_param("~rate", 100)) imu_pub = rospy.Publisher('/imu/data', Imu, queue_size=10) gps_pub = rospy.Publisher('/gps/fix', NavSatFix, queue_size=10) while not rospy.is_shutdown(): # print(x,y) # data = ser.readline() # if data[0]=='#': # data = data[1:] # linear,angular = data.split('/')[0],data.split('/')[1] linear = [float(x) for x in linear.split(' ')] angular = [float(x) for x in angular.split(' ')] imu_data.orientation.x = 1 ##angular[3] imu_data.orientation.y = 1 ## angular[4] imu_data.orientation.z = 1 ##angular[5] imu_data.linear_acceleration.x = 1 # linear[6] imu_data.linear_acceleration.y = 1 # linear[7] imu_data.linear_acceleration.z = 1 #linear[8] imu_data.angular_velocity_covariance = [ 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0 ] imu_data.linear_acceleration_covariance = [ 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0 ] gps_data.latitude = 1 # linear[0] gps_data.longitude = 1 #linear[1] gps_data.position_covariance = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0] gps_data.position_covariance_type = 3 gps_data.header.stamp = imu_data.header.stamp = rospy.Time.now() imu_pub.publish(imu_data) gps_pub.publish(gps_data) print(linear) # print(angular) rate.sleep()
def readIMU(self): imuMsg = Imu() # Orientation covariance estimation: # Observed orientation noise: 0.3 degrees in x, y, 0.6 degrees in z # Magnetometer linearity: 0.1% of full scale (+/- 2 gauss) => 4 milligauss # Earth's magnetic field strength is ~0.5 gauss, so magnetometer nonlinearity could # cause ~0.8% yaw error (4mgauss/0.5 gauss = 0.008) => 2.8 degrees, or 0.050 radians # i.e. variance in yaw: 0.0025 # Accelerometer non-linearity: 0.2% of 4G => 0.008G. This could cause # static roll/pitch error of 0.8%, owing to gravity orientation sensing # error => 2.8 degrees, or 0.05 radians. i.e. variance in roll/pitch: 0.0025 # 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 ] imuMsg.header.frame_id = 'imu_link' imuMsg.header.stamp = rospy.Time.now() accel_factor = 9.806 / 256.0 #rospy.loginfo('Reading IMU data') self.ser.write(b'?IMU\n') countout = 0 while True: data = self.ser.read_until('\n', None) #rospy.loginfo(data) if data.startswith('IMU='): imu = data.strip().replace('IMU=', '').split(' ') imuMsg.orientation.x = float(imu[0]) imuMsg.orientation.y = float(imu[1]) imuMsg.orientation.z = float(imu[2]) imuMsg.orientation.w = float(imu[3]) imuMsg.linear_acceleration.x = -float(imu[5]) * accel_factor imuMsg.linear_acceleration.y = float(imu[5]) * accel_factor imuMsg.linear_acceleration.z = float(imu[6]) * accel_factor imuMsg.angular_velocity.x = float(imu[7]) imuMsg.angular_velocity.y = -float(imu[8]) imuMsg.angular_velocity.z = -float(imu[9]) self.imu_pub.publish(imuMsg) break countout = countout + 1 if countout > 3: break
def _HandleReceivedLine(self, line): self._Counter = self._Counter + 1 self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line)) if(len(line) > 0): lineParts = line.split('\t') try: if(lineParts[0] == 'quat'): self._qx = float(lineParts[1]) self._qy = float(lineParts[2]) self._qz = float(lineParts[3]) self._qw = float(lineParts[4]) if(lineParts[0] == 'ypr'): self._ax = float(lineParts[1]) self._ay = float(lineParts[2]) self._az = float(lineParts[3]) if(lineParts[0] == 'areal'): self._lx = float(lineParts[1]) self._ly = float(lineParts[2]) self._lz = float(lineParts[3]) imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id imu_msg.header = h imu_msg.orientation_covariance = (-1., )*9 imu_msg.angular_velocity_covariance = (-1., )*9 imu_msg.linear_acceleration_covariance = (-1., )*9 imu_msg.orientation.x = self._qx imu_msg.orientation.y = self._qy imu_msg.orientation.z = self._qz imu_msg.orientation.w = self._qw imu_msg.angular_velocity.x = self._ax imu_msg.angular_velocity.y = self._ay imu_msg.angular_velocity.z = self._az imu_msg.linear_acceleration.x = self._lx imu_msg.linear_acceleration.y = self._ly imu_msg.linear_acceleration.z = self._lz self.imu_pub.publish(imu_msg) except: rospy.logwarn("Error in Sensor values") rospy.logwarn(lineParts) pass
def _create_imu_msg(self): # Check if data is available if 'ATTITUDE' not in self.get_data(): raise Exception('no ATTITUDE data') #TODO: move all msgs creating to msg msg = Imu() self._create_header(msg) #http://mavlink.org/messages/common#SCALED_IMU imu_data = None for i in ['', '2', '3']: try: imu_data = self.get_data()['SCALED_IMU{}'.format(i)] break except Exception as e: pass if imu_data is None: raise Exception('no SCALED_IMUX data') acc_data = [imu_data['{}acc'.format(i)] for i in ['x', 'y', 'z']] gyr_data = [imu_data['{}gyro'.format(i)] for i in ['x', 'y', 'z']] mag_data = [imu_data['{}mag'.format(i)] for i in ['x', 'y', 'z']] #http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html msg.linear_acceleration.x = acc_data[0] / 100 msg.linear_acceleration.y = acc_data[1] / 100 msg.linear_acceleration.z = acc_data[2] / 100 msg.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg.angular_velocity.x = gyr_data[0] / 1000 msg.angular_velocity.y = gyr_data[1] / 1000 msg.angular_velocity.z = gyr_data[2] / 1000 msg.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] #http://mavlink.org/messages/common#ATTITUDE attitude_data = self.get_data()['ATTITUDE'] orientation = [attitude_data[i] for i in ['roll', 'pitch', 'yaw']] #https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_Angles_to_Quaternion_Conversion cy = math.cos(orientation[2] * 0.5) sy = math.sin(orientation[2] * 0.5) cr = math.cos(orientation[0] * 0.5) sr = math.sin(orientation[0] * 0.5) cp = math.cos(orientation[1] * 0.5) sp = math.sin(orientation[1] * 0.5) msg.orientation.w = cy * cr * cp + sy * sr * sp msg.orientation.x = cy * sr * cp - sy * cr * sp msg.orientation.y = cy * cr * sp + sy * sr * cp msg.orientation.z = sy * cr * cp - cy * sr * sp msg.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] self.pub.set_data('/imu/data', msg)
def _HandleReceivedLine(self, line): self._Counter = self._Counter + 1 self._SerialPublisher.publish( String(str(self._Counter) + ", in: " + line)) if (len(line) > 0): lineParts = line.split('\t') try: if (lineParts[0] == 'e'): self._first_encoder_value = long(lineParts[1]) self._second_encoder_value = long(lineParts[2]) self._third_encoder_value = long(lineParts[3]) ####################################################################################################################### self._First_Encoder.publish(self._first_encoder_value) self._Second_Encoder.publish(self._second_encoder_value) self._Third_Encoder.publish(self._third_encoder_value) if (lineParts[0] == 'i'): self._qx = float(lineParts[1]) self._qy = float(lineParts[2]) self._qz = float(lineParts[3]) self._qw = float(lineParts[4]) ####################################################################################################################### self._qx_.publish(self._qx) self._qy_.publish(self._qy) self._qz_.publish(self._qz) self._qw_.publish(self._qw) ####################################################################################################################### imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id imu_msg.header = h imu_msg.orientation_covariance = (-1., ) * 9 imu_msg.angular_velocity_covariance = (-1., ) * 9 imu_msg.linear_acceleration_covariance = (-1., ) * 9 imu_msg.orientation.x = self._qx imu_msg.orientation.y = self._qy imu_msg.orientation.z = self._qz imu_msg.orientation.w = self._qw self.imu_pub.publish(imu_msg) except: rospy.logwarn("Error in Sensor values") rospy.logwarn(lineParts) pass
def update_imu(self): """ Reads all characters in the buffer until finding \r\n Messages should have the following format: "Q: w x y z | A: x y z | G: x y z" :return: """ # Create new message try: imuMsg = Imu() # Set the sensor covariances imuMsg.orientation_covariance = [ 0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025 ] imuMsg.angular_velocity_covariance = [-1., 0, 0, 0, 0, 0, 0, 0, 0] imuMsg.linear_acceleration_covariance = [ -1., 0, 0, 0, 0, 0, 0, 0, 0 ] imu_data = self.read_data() if len(imu_data) == 0: self.log("IMU is not answering", 2) return q_data = transformations.quaternion_from_euler( imu_data[2], imu_data[1], imu_data[0]) q1 = Quaternion() q1.x = float(q_data[0]) q1.y = float(q_data[1]) q1.z = float(q_data[2]) q1.w = float(q_data[3]) q_off = self.imu_offset new_q = transformations.quaternion_multiply( [q1.x, q1.y, q1.z, q1.w], [q_off.x, q_off.y, q_off.z, q_off.w]) imuMsg.orientation.x = new_q[0] imuMsg.orientation.y = new_q[1] imuMsg.orientation.z = new_q[2] imuMsg.orientation.w = new_q[3] # Handle message header imuMsg.header.frame_id = "base_link_imu" imuMsg.header.stamp = rospy.Time.now() + rospy.Duration(nsecs=5000) self.imu_reading = imuMsg except SerialException as serial_exc: self.log( "SerialException while reading from IMU: {}".format( serial_exc), 3) self.calibration = True except ValueError as val_err: self.log("Value error from IMU data - {}".format(val_err), 5) self.val_exc = self.val_exc + 1 except Exception as imu_exc: self.log(imu_exc, 3) raise imu_exc
def GetImuFromLine(line): (ts, ax, ay, az, gx, gy, gz) = [t(s) for t,s in zip((int,int, int, int, int, int, int),line.split())] imu = Imu() ts = float(ts)/1000000.0 ImuStamp = rospy.rostime.Time.from_sec(ts) imu.angular_velocity = create_vector3(gyro_raw_to_rads(gx), gyro_raw_to_rads(gy), gyro_raw_to_rads(gz)) imu.angular_velocity_covariance = create_diag_mat(gyro_raw_to_rads(1.0), gyro_raw_to_rads(1.0), gyro_raw_to_rads(1.0)); imu.linear_acceleration = create_vector3(acc_raw_to_ms(ax), acc_raw_to_ms(ay), acc_raw_to_ms(az)) imu.linear_acceleration_covariance = create_diag_mat(acc_raw_to_ms(1.0), acc_raw_to_ms(1.0), acc_raw_to_ms(1.0)) return (ts, ImuStamp, imu)
def main(): rospy.init_node("imu_node") pub = rospy.Publisher('imu/data', Imu, queue_size=10) global yaw global rover_accx yaw_old = yaw vth = 0 imuMsg = Imu() imuMsg.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] imuMsg.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] imuMsg.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] current_time = rospy.Time.now() last_time = rospy.Time.now() print("Sleeping 1 second") rospy.sleep(1) # while(1) rate = rospy.Rate(10) while not rospy.is_shutdown(): # Will be obtained from sensor current_time = rospy.Time.now() dt = (current_time - last_time).to_sec() if (yaw > 0): dyaw = yaw - yaw_old vth = dyaw / dt if (yaw < 0): dyaw = yaw - yaw_old vth = dyaw / dt # Acceloremeter imuMsg.linear_acceleration.x = rover_accx imuMsg.linear_acceleration.y = 0 imuMsg.linear_acceleration.z = 0 # Gyro imuMsg.angular_velocity.x = 0 imuMsg.angular_velocity.y = 0 imuMsg.angular_velocity.z = vth q = tf.transformations.quaternion_from_euler(0, 0, yaw) imuMsg.orientation.x = q[0] #magnetometer imuMsg.orientation.y = q[1] imuMsg.orientation.z = q[2] imuMsg.orientation.w = q[3] imuMsg.header.stamp = rospy.Time.now() imuMsg.header.frame_id = 'base_link' rospy.loginfo(imuMsg) pub.publish(imuMsg) yaw_old = yaw last_time = rospy.Time.now() rospy.Subscriber('/rover_serial_imu', String, callback_sensor) rate.sleep()
def handle_received_line(self, line): """Calculate orientation from accelerometer and gyrometer""" self._counter = self._counter + 1 self._SerialPublisher.publish( String(str(self._counter) + ", in: " + line)) if line: lineParts = line.split('\t') try: if lineParts[0] == 'b': self._battery_value = float(lineParts[1]) self._battery_pub.publish(self._battery_value) if lineParts[0] == 'i': # self._qx, self._qy, self._qz, self._qw, \ # self._gx, self._gy, self._gz, \ # self._ax, self._ay, self.az = [float(i) for i in lineParts[1:11]] self._qx = float(lineParts[1]) self._qy = float(lineParts[2]) self._qz = float(lineParts[3]) self._qw = float(lineParts[4]) self._gx = float(lineParts[5]) self._gy = float(lineParts[6]) self._gz = float(lineParts[7]) self._ax = float(lineParts[8]) self._ay = float(lineParts[9]) self._az = float(lineParts[10]) imu_msg = Imu() header = Header() header.stamp = rospy.Time.now() header.frame_id = self._frame_id imu_msg.header = header imu_msg.orientation_covariance = self._imu_data.orientation_covariance imu_msg.angular_velocity_covariance = self._imu_data.angular_velocity_covariance imu_msg.linear_acceleration_covariance = self._imu_data.linear_acceleration_covariance imu_msg.orientation.x = self._qx imu_msg.orientation.y = self._qy imu_msg.orientation.z = self._qz imu_msg.orientation.w = self._qw imu_msg.angular_velocity.x = self._gx imu_msg.angular_velocity.y = self._gy imu_msg.angular_velocity.z = self._gz imu_msg.linear_acceleration.x = self._ax imu_msg.linear_acceleration.y = self._ay imu_msg.linear_acceleration.z = self._az # q_rot = quaternion_from_euler(self.pi, -self.pi/2, 0) # q_ori = Quaternion(self._qx, self._qy, self._qz, self._qw) # imu_msg.orientation = quaternion_multiply(q_ori, q_rot) self._imu_pub.publish(imu_msg) except: rospy.logwarn("Error in Sensor values") rospy.logwarn(lineParts)
def call_imu(self, msg_imu_int): # IMU msg_imu = Imu(); msg_imu.header.stamp = rospy.Time.now() msg_imu.header.frame_id = "world" r,p,y = euler_from_quaternion([msg_imu_int.orientation.x,msg_imu_int.orientation.y,msg_imu_int.orientation.z,msg_imu_int.orientation.w]) self.roll = -p self.pitch = r self.yaw = y q_new = quaternion_from_euler(self.roll, self.pitch, self.yaw) #rospy.loginfo(q_new) rospy.loginfo(euler_from_quaternion(q_new)) msg_imu.orientation.x = q_new[0] msg_imu.orientation.y = q_new[1] msg_imu.orientation.z = q_new[2] msg_imu.orientation.w = q_new[3] msg_imu.orientation_covariance = msg_imu_int.orientation_covariance msg_imu.angular_velocity.x = -msg_imu_int.angular_velocity.x msg_imu.angular_velocity.y = -msg_imu_int.angular_velocity.y msg_imu.angular_velocity.z = msg_imu_int.angular_velocity.z msg_imu.angular_velocity_covariance = msg_imu_int.angular_velocity_covariance msg_imu.linear_acceleration.x = msg_imu_int.linear_acceleration.y # The same configuration in the PIXHAWK msg_imu.linear_acceleration.y = -msg_imu_int.linear_acceleration.x msg_imu.linear_acceleration.z = msg_imu_int.linear_acceleration.z msg_imu.linear_acceleration_covariance = msg_imu_int.linear_acceleration_covariance self.pub_imu.publish(msg_imu) br = tf2_ros.TransformBroadcaster() t = TransformStamped() t.header.frame_id = "world" t.header.stamp = rospy.Time.now() t.child_frame_id = "odom" t.transform.translation.x = 0.0 t.transform.translation.y = 0.0 t.transform.translation.z = self.z t.transform.rotation.x = q_new[0] t.transform.rotation.y = q_new[1] t.transform.rotation.z = q_new[2] t.transform.rotation.w = q_new[3] br.sendTransform(t) # ORIENTATION msg_ori = PointStamped() msg_ori.header.stamp = rospy.Time.now() msg_ori.header.frame_id = "world" msg_ori.point.x = self.pitch msg_ori.point.y = self.roll msg_ori.point.z = self.yaw self.pub_ori.publish(msg_ori) # ANGULAR VELOCITY msg_vel = PointStamped() msg_vel.header.stamp = rospy.Time.now() msg_vel.header.frame_id = "world" msg_vel.point.x = msg_imu.angular_velocity.x msg_vel.point.y = msg_imu.angular_velocity.y msg_vel.point.z = msg_imu.angular_velocity.z self.pub_omega.publish(msg_vel)
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 talker(): global oldTime pubString = rospy.Publisher('sensor/string', String, queue_size=1000) pubIMU = rospy.Publisher('imuBoat', Imu, queue_size=1000) rospy.init_node('sensornimureader', anonymous=True) rate = rospy.Rate(100) # 100hz while not rospy.is_shutdown(): # sample data currentTime = rospy.Time.now() timeCheck = currentTime.to_sec() - oldTime.to_sec() print timeCheck if (timeCheck) > 2.0: data = 'Z1.341725,103.965008,1.5100,0.0000' oldTime = currentTime; else: data = 'Y0.0000,0.0730,255.4516,1.5100,0.0000,0.0000,0.000,0.000,0.000' # data = ser.readline() pubString.publish(data) if data[0] == 'Y': data = data.replace("Y","").replace("\n","").replace("\r","") data_list = data.split(',') if len(data_list) == 9: try: ##data_list structure: accel, magni, gyro float_list = [float(i) for i in data_list] imuData = Imu() imuData.header.frame_id = "base_link" imuData.header.stamp = rospy.Time.now() ##data form in yaw, pitch, roll quat = tf.transformations.quaternion_from_euler(float_list[3], float_list[4], float_list[5], 'rzyx') imuData.orientation.x = quat[0] imuData.orientation.y = quat[1] imuData.orientation.z = quat[2] imuData.orientation.w = quat[3] imuData.angular_velocity.x = math.radians(float_list[6]*gyro_scale) imuData.angular_velocity.y = math.radians(-float_list[7]*gyro_scale) imuData.angular_velocity.z = math.radians(-float_list[8]*gyro_scale) imuData.linear_acceleration.x = float_list[0]*accel_scale imuData.linear_acceleration.y = -float_list[1]*accel_scale imuData.linear_acceleration.z = -float_list[2]*accel_scale imuData.orientation_covariance = [1.5838e-6, 0, 0, 0, 1.49402e-6, 0, 0, 0, 1.88934e-6] imuData.angular_velocity_covariance = [7.84113e-7, 0, 0, 0, 5.89609e-7, 0, 0, 0, 6.20293e-7] imuData.linear_acceleration_covariance = [9.8492e-4, 0, 0, 0, 7.10809e-4, 0, 0, 0, 1.42516e-3] pubIMU.publish(imuData) log = "IMU Data: %f %f %f %f %f %f %f %f %f Publish at Time: %s" \ % (imuData.linear_acceleration.x, imuData.linear_acceleration.y, imuData.linear_acceleration.z, float_list[3], float_list[4], float_list[5], imuData.angular_velocity.x, imuData.angular_velocity.y, imuData.angular_velocity.z, rospy.get_time()) except: log = "IMU Data Error! Data : %s" % data else: log = "Data Error! Data : %s" % data rospy.loginfo(log) rate.sleep()
def _HandleReceivedLine(self, line): self._Counter = self._Counter + 1 self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line)) if(len(line) > 0): lineParts = line.split('\t') try: if(lineParts[0] == 'e'): self._left_encoder_value = long(lineParts[1]) self._right_encoder_value = long(lineParts[2]) self._Left_Encoder.publish(self._left_encoder_value) self._Right_Encoder.publish(self._right_encoder_value) #if(lineParts[0] == 'b'): #self._battery_value = float(lineParts[1]) #self._Battery_Level.publish(self._battery_value) if(lineParts[0] == 'u'): self._ultrasonic_value = float(lineParts[1]) self._Ultrasonic_Value.publish(self._ultrasonic_value) if(lineParts[0] == 'i'): self._qx = float(lineParts[1]) self._qy = float(lineParts[2]) self._qz = float(lineParts[3]) self._qw = float(lineParts[4]) self._qx_.publish(self._qx) self._qy_.publish(self._qy) self._qz_.publish(self._qz) self._qw_.publish(self._qw) imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id imu_msg.header = h imu_msg.orientation_covariance = (-1., ) * 9 imu_msg.angular_velocity_covariance = (-1., ) * 9 imu_msg.linear_acceleration_covariance = (-1., ) * 9 imu_msg.orientation.x = self._qx imu_msg.orientation.y = self._qy imu_msg.orientation.z = self._qz imu_msg.orientation.w = self._qw self.imu_pub.publish(imu_msg) except: rospy.logwarn("Error in Sensor values") rospy.logwarn(lineParts) pass
def talker(): pub = rospy.Publisher('IMUBoat', Imu, queue_size=1000) rospy.init_node('callibIMU', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): data = ser.readline() # print data # print len(data) if data[0] == 'Y' and len(data) >= 70 and len(data) <= 85: data = data.replace("Y","").replace("\n","").replace("\r","") data_list = data.split(',') if len(data_list) == 9: try: ##data_list structure: accel, magni, gyro float_list = [float(i) for i in data_list] imuData = Imu() imuData.header.frame_id = "imu" imuData.header.stamp = rospy.Time.now() quat = tf.transformations.quaternion_from_euler(float_list[3], float_list[4], float_list[5], 'rzyx') imuData.orientation.x = quat[0] imuData.orientation.y = quat[1] imuData.orientation.z = quat[2] imuData.orientation.w = quat[3] imuData.angular_velocity.x = math.radians(float_list[6]*gyro_scale) imuData.angular_velocity.y = math.radians(-float_list[7]*gyro_scale) imuData.angular_velocity.z = math.radians(-float_list[8]*gyro_scale) imuData.linear_acceleration.x = float_list[0]*accel_scale imuData.linear_acceleration.y = -float_list[1]*accel_scale imuData.linear_acceleration.z = -float_list[2]*accel_scale imuData.orientation_covariance = [] imuData.angular_velocity_covariance = [] imuData.linear_acceleration_covariance = [] pub.publish(imuData) # log = "Data: %f %f %f %f %f %f %f %f %f Publish at Time: %s" \ # % (float_list[0], float_list[1], float_list[2], float_list[3], # float_list[4], float_list[5], float_list[6], float_list[7], # float_list[8], rospy.get_time()) print "%f %f %f %f %f %f %f %f %f" % ( imuData.linear_acceleration.x, imuData.linear_acceleration.y, imuData.linear_acceleration.z, float_list[3], float_list[4], float_list[5], imuData.angular_velocity.x, imuData.angular_velocity.y, imuData.angular_velocity.z) except: continue #log = "Data Error! Data : %s" % data else: continue #log = "Data Error! Data : %s" % data #rospy.loginfo(log) else: continue #log = "Non Data Serial Message: %s" % data #rospy.loginfo(log) rate.sleep()
def _HandleReceivedLine(self, line): self._Counter = self._Counter + 1 self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line)) while len(line) > 0: lineParts = line.split("\t") try: if lineParts[0] == "e": self.angle_z = float(lineParts[1]) self._qx = 0.0 self._qy = 0.0 self._qz = math.sin(math.pi * self.angle_z / 360.0) self._qw = math.cos(math.pi * self.angle_z / 360.0) ####################################################################################################################### self._qx_.publish(self._qx) self._qy_.publish(self._qy) self._qz_.publish(self._qz) self._qw_.publish(self._qw) ####################################################################################################################### imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id imu_msg.header = h imu_msg.orientation_covariance = (-1.0,) * 9 imu_msg.angular_velocity_covariance = (-1.0,) * 9 imu_msg.linear_acceleration_covariance = (-1.0,) * 9 imu_msg.orientation.x = self._qx imu_msg.orientation.y = self._qy imu_msg.orientation.z = self._qz imu_msg.orientation.w = self._qw self.imu_pub.publish(imu_msg) except: rospy.logwarn("Error in Sensor values") rospy.logwarn(lineParts) pass
def msg_template(self): msg = Imu() msg.header.frame_id = "robocape" msg.linear_acceleration_covariance = (0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1) msg.angular_velocity_covariance = (1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) msg.orientation_covariance = (10.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 10.0) return msg
def Handle_Received_Line(self, line): self._counter = self._counter + 1 self.SerialPublisher.publish(String(str(self._counter) + ", in: " + line)) if(len(line) > 0): lineParts = line.split('\t') try: if(lineParts[0] == 'e'): self._left_encoder_val = long(lineParts[1]) self._right_encoder_val = long(lineParts[2]) self.left_encoder_pub.publish(self._left_encoder_val) self.right_encoder_pub.publish(self._right_encoder_val) if(lineParts[0] == 'u'): self._ultrasonic_val = float(lineParts[1]) self.ultrasonic_pub.publish(self._ultrasonic_val) if(lineParts[0] == 'i'): self._qx = float(lineParts[1]) self._qy = float(lineParts[2]) self._qz = float(lineParts[3]) self._qw = float(lineParts[4]) self._qx_pub.publish(self._qx) self._qy_pub.publish(self._qy) self._qz_pub.publish(self._qz) self._qw_pub.publish(self._qw) imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id imu_msg.header = h imu_msg.orientation_covariance = (-1., ) * 9 imu_msg.angular_velocity_covariance = (-1., ) * 9 imu_msg.linear_acceleration_covariance = (-1., ) * 9 imu_msg.orientation.x = self._qx imu_msg.orientation.y = self._qy imu_msg.orientation.z = self._qz imu_msg.orientation.w = self._qw self.imu_pub.publish(imu_msg) except: rospy.logwarn("Error in the sensor values") rospy.logwarn(lineParts) pass
def publish_imu(self): imu_msg = Imu() imu_msg.header.stamp = self.time imu_msg.header.frame_id = 'imu' imu_msg.orientation = Quaternion(*SF9DOF_UKF.recover_quat(self.kalman_state)) imu_msg.orientation_covariance = \ list(self.kalman_covariance[0:3,0:3].flatten()) imu_msg.angular_velocity.x = self.kalman_state[3,0] imu_msg.angular_velocity.y = self.kalman_state[4,0] imu_msg.angular_velocity.z = self.kalman_state[5,0] imu_msg.angular_velocity_covariance = \ list(self.kalman_covariance[3:6,3:6].flatten()) imu_msg.linear_acceleration.x = self.kalman_state[6,0] imu_msg.linear_acceleration.y = self.kalman_state[7,0] imu_msg.linear_acceleration.z = self.kalman_state[8,0] imu_msg.linear_acceleration_covariance = \ list(self.kalman_covariance[6:9,6:9].flatten()) self.pub.publish(imu_msg)
def publish_imu(self): imu_msg = Imu() imu_msg.header.stamp = self.time imu_msg.header.frame_id = 'imu_odom' quat = tf_math.quaternion_from_euler(self.kalman_state[0,0],self.kalman_state[1,0],self.kalman_state[2,0], axes='sxyz') imu_msg.orientation.x = quat[0] imu_msg.orientation.y = quat[1] imu_msg.orientation.z = quat[2] imu_msg.orientation.w = quat[3] imu_msg.orientation_covariance = list(self.kalman_covariance[0:3,0:3].flatten()) imu_msg.angular_velocity.x = self.kalman_state[3,0] imu_msg.angular_velocity.y = self.kalman_state[4,0] imu_msg.angular_velocity.z = self.kalman_state[5,0] imu_msg.angular_velocity_covariance = list(self.kalman_covariance[3:6,3:6].flatten()) imu_msg.linear_acceleration.x = self.kalman_state[11,0] imu_msg.linear_acceleration.y = self.kalman_state[12,0] imu_msg.linear_acceleration.z = self.kalman_state[13,0] imu_msg.linear_acceleration_covariance = list(self.kalman_covariance[11:14,11:14].flatten())#[.01, 0, 0, 0, .01, 0, 0, 0, .01] self.pub.publish(imu_msg)
def talker(): #Create publisher ('Topic name', msg type) pub = rospy.Publisher('IMU', Imu) #Tells rospy name of the node to allow communication to ROS master rospy.init_node('IMUtalker') while not rospy.is_shutdown(): #Grab relevant information from parse() try: (accel,gyro,magne) = parse() #If data is bad, uses presvious data except: continue #Define IMUmsg to be of the Imu msg type IMUmsg = Imu() #Set header time stamp IMUmsg.header.stamp = rospy.Time.now() #Set orientation parameters IMUmsg.orientation = Quaternion() IMUmsg.orientation.x = magne[0] IMUmsg.orientation.y = magne[1] IMUmsg.orientation.z = magne[2] IMUmsg.orientation_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) #Set angular velocity parameters IMUmsg.angular_velocity = Vector3() IMUmsg.angular_velocity.x = gyro[0] IMUmsg.angular_velocity.y = gyro[1] IMUmsg.angular_velocity.z = gyro[2] IMUmsg.angular_velocity_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) #Set linear acceleration parameters IMUmsg.linear_acceleration = Vector3() IMUmsg.linear_acceleration.x = accel[0] IMUmsg.linear_acceleration.y = accel[1] IMUmsg.linear_acceleration.z = accel[2] IMUmsg.linear_acceleration_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) #Publish the data pub.publish(IMUmsg)
def run_arduino(self): rospy.init_node('arduino_driver', anonymous=False) print "Arduino Driver is Running!" while not rospy.is_shutdown(): message=self.ser_obj.readline() result=message.strip().split(',') if len(result)==7: imu_message=Imu() imu_message.header.stamp=rospy.Time.now() imu_message.header.frame_id="gyro_link" imu_message.orientation.x=0.0 imu_message.orientation.y=0.0 imu_message.orientation.z=0.0 imu_message.orientation.w=1.0 imu_message.orientation_covariance=[-1, 0, 0, 0, -1, 0, 0, 0, -1] imu_message.linear_acceleration.x=float(result[0]) imu_message.linear_acceleration.y=float(result[1]) imu_message.linear_acceleration.z=float(result[2]) imu_message.linear_acceleration_covariance=[1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] imu_message.angular_velocity.x=float(result[3]) imu_message.angular_velocity.y=float(result[4]) imu_message.angular_velocity.z=float(result[5]) imu_message.angular_velocity_covariance=[1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] # accel=result[0:3] # gyro=result[3:6] # batt=result[6] #res=[accel, gyro, batt] # self.imu_pub.publish(imu_message) self.imu_pub.publish(imu_message) self.batt_pub.publish(float(result[6]))
def publish_imu(self): imu_msg = Imu() imu_msg.header.stamp = self.time imu_msg.header.frame_id = 'imu_odom' imu_msg.orientation.x = self.kalman_state[0,0] imu_msg.orientation.y = self.kalman_state[1,0] imu_msg.orientation.z = self.kalman_state[2,0] imu_msg.orientation.w = self.kalman_state[3,0] imu_msg.orientation_covariance = list(self.kalman_covariance[0:3,0:3].flatten()) imu_msg.angular_velocity.x = self.kalman_state[4,0] imu_msg.angular_velocity.y = self.kalman_state[5,0] imu_msg.angular_velocity.z = self.kalman_state[6,0] # Check this covariance imu_msg.angular_velocity_covariance = list(self.kalman_covariance[4:7,4:7].flatten()) imu_msg.linear_acceleration.x = self.measurement[0,0] imu_msg.linear_acceleration.y = self.measurement[1,0] imu_msg.linear_acceleration.z = self.measurement[2,0] acc_cov = SF9DOF_UKF.measurement_noise(self.measurement, 1.0)[0:3,0:3] imu_msg.linear_acceleration_covariance = list(acc_cov.flatten()) self.pub.publish(imu_msg)
def update_sensors(self): # Accelerometer accel = self._driver.get_accelerometer() accel_msg = Imu() accel_msg.header.stamp = rospy.Time.now() accel_msg.header.frame_id = self._name+"/base_link" accel_msg.linear_acceleration.x = (accel[1]-2048.0)/800.0*9.81 # 1 g = about 800, then transforms in m/s^2. accel_msg.linear_acceleration.y = (accel[0]-2048.0)/800.0*9.81 accel_msg.linear_acceleration.z = (accel[2]-2048.0)/800.0*9.81 accel_msg.linear_acceleration_covariance = [0.01,0.0,0.0, 0.0,0.01,0.0, 0.0,0.0,0.01] #print "accel raw: " + str(accel[0]) + ", " + str(accel[1]) + ", " + str(accel[2]) #print "accel (m/s2): " + str((accel[0]-2048.0)/800.0*9.81) + ", " + str((accel[1]-2048.0)/800.0*9.81) + ", " + str((accel[2]-2048.0)/800.0*9.81) accel_msg.angular_velocity.x = 0 accel_msg.angular_velocity.y = 0 accel_msg.angular_velocity.z = 0 accel_msg.angular_velocity_covariance = [0.01,0.0,0.0, 0.0,0.01,0.0, 0.0,0.0,0.01] q = tf.transformations.quaternion_from_euler(0, 0, 0) accel_msg.orientation = Quaternion(*q) accel_msg.orientation_covariance = [0.01,0.0,0.0, 0.0,0.01,0.0, 0.0,0.0,0.01] self.accel_publisher.publish(accel_msg)
def _HandleReceivedLine(self, line): self._Counter = self._Counter + 1 self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line)) if(len(line) > 0): lineParts = line.split('\t') try: if(lineParts[0] == 'yaw'): self._ax = float(lineParts[1]) yaw_msg = Float64() imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id imu_msg.header = h yaw_msg.data = self._ax q = quaternion_from_euler(0,0,self._ax*-degrees2rad) imu_msg.orientation.x = q[0] imu_msg.orientation.y = q[1] imu_msg.orientation.z = q[2] imu_msg.orientation.w = q[3] imu_msg.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] imu_msg.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e6] imu_msg.linear_acceleration_covariance = [-1,0,0,0,0,0,0,0,0] self.imu_pub.publish(imu_msg) self.yaw_pub.publish(yaw_msg) except: rospy.logwarn("Error in Sensor values") rospy.logwarn(lineParts) 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 publish_imu(): global buffer,gyro_cal,previousTime,yaw,angular_vel currentTime=rospy.Time.now() dt=currentTime-previousTime dt=dt.to_sec() previousTime=currentTime if rVel==0 and lVel==0: buffer.append(gyroRaw) if len(buffer) > buffer_length: buffer.pop(0) gyro_cal=numpy.mean(buffer) angular_vel=0 else: angular_vel=-(((gyroRaw-gyro_cal)/gyro_cal)*300*gyroScale)*math.pi/180; if math.fabs(angular_vel) < 0: angular_vel=0 yaw+=(angular_vel*dt) rospy.loginfo("Yaw %s" % yaw) rospy.loginfo("Yaw Vel %s" % angular_vel) imu=Imu() imu.header.stamp = currentTime imu.header.frame_id = 'gyro_link' imu.orientation.x=0 imu.orientation.y=0 imu.orientation.z=math.sin(yaw/2) imu.orientation.w=math.cos(yaw/2) imu.orientation_covariance=[1e6,0,0,0,1e6,0,0,0,1e-10] imu.angular_velocity.x=0 imu.angular_velocity.y=0 imu.angular_velocity.z=angular_vel imu.angular_velocity_covariance=[1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-10] imu.linear_acceleration_covariance=[-1,0,0,0,0,0,0,0,0] imuPub.publish(imu)
def talker(message): corrected = Imu() corrected.header.seq = message.header.seq corrected.header.stamp = rospy.Time.now() corrected.header.frame_id = message.header.frame_id # Transpose corrected.linear_acceleration.x = message.linear_acceleration.y corrected.linear_acceleration.y = -message.linear_acceleration.x corrected.linear_acceleration.z = -message.linear_acceleration.z corrected.angular_velocity.x = message.angular_velocity.x corrected.angular_velocity.y = message.angular_velocity.y corrected.angular_velocity.z = -message.angular_velocity.z corrected.orientation.x = 1 corrected.orientation.y = 0 corrected.orientation.z = 0 corrected.orientation.w = 0 corrected.orientation_covariance = [99999, 0, 0, 99999, .1, 0, 0, 0, 99999] corrected.angular_velocity_covariance = [.000004, 0, 0, 0, .000004, 0, 0, 0, .000004] corrected.linear_acceleration_covariance = [.000025, 0, 0, 0, .000025, 0, 0, 0, .000025] pub = rospy.Publisher('imu_w_cov', Imu) #print >> f, "%f %f %f %f %f %f \n" % (message.linear_acceleration.x, message.linear_acceleration.y, message.linear_acceleration.z, message.angular_velocity.x, message.angular_velocity.y, message.angular_velocity.z) pub.publish(corrected)
def magnetic_cb(msg): global x_max, x_min, y_max, y_min, x_center, y_center, x_scale, y_scale if autocompute: x_max = max(x_max, msg.vector.x) x_min = min(x_min, msg.vector.x) y_max = max(y_max, msg.vector.y) y_min = min(y_min, msg.vector.y) x_center = (x_max + x_min) / 2.0 y_center = (y_max + y_min) / 2.0 x_scale = x_max - x_min y_scale = y_max - y_min x = (msg.vector.x - x_center) / x_scale y = (msg.vector.y - y_center) / y_scale heading = atan2(x, y) imu = Imu() imu.header = msg.header imu.header.frame_id = 'odom' q = tf.transformations.quaternion_from_euler(0, 0, heading) imu.orientation = Quaternion(*q) imu.orientation_covariance = [ compass_var, 0.0, 0.0, 0.0, compass_var, 0.0, 0.0, 0.0, compass_var ] imu.angular_velocity_covariance = [ -1, 0, 0, 0, 0, 0, 0, 0, 0 ] imu.linear_acceleration_covariance = [ -1, 0, 0, 0, 0, 0, 0, 0, 0 ] imu_pub.publish(imu)
def cb_osc_accxyz(self, address_list, value_list, send_address): """ Callback for when accel data is received from a device. Populates a sensor_msgs/Imu message, including the ip address of the sending client in the msg.header.frame_id field. @param address_list: A list with the OSC address parts in it. @type address_list: C{list} @param value_list: A list with the OSC value arguments in it. @type value_list: C{list} @param send_address: A tuple with the (ip, port) of the sender. @type send_address: C{tuple} """ msg = Imu() msg.linear_acceleration.x = value_list[0] * 9.80665 msg.linear_acceleration.y = value_list[1] * 9.80665 msg.linear_acceleration.z = value_list[2] * 9.80665 msg.header.frame_id = send_address[0] msg.header.stamp = rospy.Time.now() # Covariance was calculated from about 20 minutes of static data # Conditions: # * Back down # * Plugged In # * Vibrate Off # * Cell and Wifi On # Results: # x y z # Mean: 0.2934510093 -0.2174349315 -9.8049353269 # Stdev: 0.0197007054 0.0205649244 0.0259846818 # Var: 0.0003881178 0.0004229161 0.0006752037 var = 0.0008 msg.linear_acceleration_covariance = [var, 0, 0, 0, var, 0, 0, 0, var] msg.angular_velocity_covariance = [0.0] * 9 msg.angular_velocity_covariance[0] = -1.0 msg.orientation_covariance = msg.angular_velocity_covariance self.accel_pub.publish(msg)
def callback(self, data): imu_message = Imu() imu_message.header.stamp = rospy.Time.now() imu_message.header.frame_id = "IMU" imu_message.orientation.x = data.orientation.y imu_message.orientation.y = data.orientation.x imu_message.orientation.z = -data.orientation.z imu_message.orientation.w = -data.orientation.w imu_message.orientation_covariance = data.orientation_covariance imu_message.linear_acceleration.x = data.linear_acceleration.y imu_message.linear_acceleration.y = data.linear_acceleration.x imu_message.linear_acceleration.z = -data.linear_acceleration.z imu_message.linear_acceleration_covariance = data.linear_acceleration_covariance imu_message.angular_velocity.x = data.angular_velocity.y imu_message.angular_velocity.y = data.angular_velocity.x imu_message.angular_velocity.z = -data.angular_velocity.z imu_message.angular_velocity_covariance = data.linear_acceleration_covariance self.pub.publish(imu_message)
def talker(): """Initializes the publisher node""" global pub pub = rospy.Publisher("mpu6050", Imu) rospy.init_node("MPU6050-Driver") # calibrate(imu, accel+gyro, samples = 0) global seq seq = 0 while not rospy.is_shutdown(): sample = Imu() sample.header = make_header() sample.orientation_covariance[0] = -1 sample.angular_velocity_covariance = [0] * 9 sample.angular_velocity = read_gyros() sample.linear_acceleration_covariance = [0] * 9 sample.linear_acceleration = read_accels() rospy.loginfo(str(sample)) pub.publish(sample) time.sleep(0.1)
def publish_imu(self): imu_msg = Imu() imu_msg.header.stamp = self.time imu_msg.header.frame_id = 'imu_odom' a = self.kalman_state[0,0] b = self.kalman_state[1,0] c = self.kalman_state[2,0] d = self.kalman_state[3,0] imu_msg.orientation.x = a imu_msg.orientation.y = b imu_msg.orientation.z = c imu_msg.orientation.w = d imu_msg.orientation_covariance = list(self.kalman_covariance[0:3,0:3].flatten()) imu_msg.angular_velocity.x = 0 imu_msg.angular_velocity.y = 0 imu_msg.angular_velocity.z = 0 imu_msg.angular_velocity_covariance = list(zeros((3,3)).flatten()) imu_msg.linear_acceleration.x = self.measurement[0,0] imu_msg.linear_acceleration.y = self.measurement[1,0] imu_msg.linear_acceleration.z = self.measurement[2,0] acc_cov = SF9DOF_UKF.measurement_noise(self.measurement, 1.0)[0:4][0:4] imu_msg.linear_acceleration_covariance = list(acc_cov.flatten()) self.pub.publish(imu_msg)