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 publish_costly_imu(debug, coef): if debug: pub_raw = rospy.Publisher(COSTLY_IMU_RAW_TOPIC, Imu, queue_size=10) global anguler_mf pub = rospy.Publisher(COSTLY_IMU_TOPIC, Imu, queue_size=10) rospy.sleep(0.2) #rospy.Subscriber("/imu/data", Imu, callback) ser = serial.Serial('/dev/costly_imu', 115200, timeout=0.5) #ser = serial.Serial('/dev/ttyUSB4', 115200, timeout=0.5) rate = rospy.Rate(115200) # 100hz #rate = rospy.Rate(10) # 100hz while not rospy.is_shutdown(): try: head = ser.read(1) if ord(head[0]) == 0xdd: package = ser.read(7) if check(package) == False: continue angle1 = (get_angle(package, coef) * math.pi / 180.0 + math.pi) % (2 * math.pi) - math.pi velocity1 = get_velocity(package) * math.pi / 180.0 #angle1 = get_angle(package) angle = anguler_mf.filter(angle1, velocity1) #velocity = velocity_mf.filter(velocity1) velocity = filter_velocity(velocity1, 0.0005, 2.05) #rospy.loginfo("angle1: %s, angle:%s, velocity1: %s, velocity: %s", angle1, angle, velocity1, velocity) if angle is not None and velocity is not None: q = tf.transformations.quaternion_from_euler(0, 0, -angle) imu = Imu() imu.header.frame_id = "base_link" imu.header.stamp = rospy.Time.now() imu.orientation.x = q[0] imu.orientation.y = q[1] imu.orientation.z = q[2] imu.orientation.w = q[3] # TODO reset covariance imu.orientation_covariance = [1e-05, 0., 0., 0., 1e-5, 0., 0., 0., 1e-06] imu.angular_velocity_covariance = [1e-05, 0., 0., 0., 1e-5, 0., 0., 0., 1e-06] imu.angular_velocity.z = -velocity pub.publish(imu) if debug: q = tf.transformations.quaternion_from_euler(0, 0, -angle1) imu = Imu() imu.header.frame_id = "costly_imu_link" imu.header.stamp = rospy.Time.now() imu.orientation.x = q[0] imu.orientation.y = q[1] imu.orientation.z = q[2] imu.orientation.w = q[3] # TODO reset covariance imu.orientation_covariance = [1e-05, 0., 0., 0., 1e-05, 0., 0., 0., 1e-06] imu.angular_velocity_covariance = [1e-05, 0., 0., 0., 1e-5, 0., 0., 0., 1e-06] imu.angular_velocity.z = -velocity1 pub_raw.publish(imu) except Exception as e: rospy.logerr("failed to publish imu data:%s", e.message) ser = serial.Serial('/dev/costlyimu', 115200, timeout=0.5) 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 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 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 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 _send_one(self, now, angle): msg = Imu() msg.header.stamp = now msg.header.seq = self._seq self._seq += 1 msg.header.frame_id = self._frame_id msg.angular_velocity_covariance = np.zeros(9) msg.orientation_covariance = np.zeros(9) if self._gyro.current_mode == self._gyro.MODE_RATE: msg.angular_velocity.z = angle - self._rate_bias msg.angular_velocity_covariance[8] = (self._sigma_omega*self._sample_period)**2 if self._invert_rotation: msg.angular_velocity.z *= -1 if self._gyro.current_mode == self._gyro.MODE_DTHETA: msg.angular_velocity = angle/self._sample_period - self._rate_bias msg.angular_velocity_covariance[8] = (self._sigma_omega*self._sample_period)**2 if self._invert_rotation: msg.angular_velocity.z *= -1 if self._gyro.current_mode == self._gyro.MODE_INTEGRATED: dt = (now - self._bias_measurement_time).to_sec() corrected_angle = angle - self._rate_bias*dt msg.orientation.w = cos(corrected_angle/2.0) msg.orientation.z = sin(corrected_angle/2.0) if self._invert_rotation: msg.orientation.z *= -1 msg.orientation_covariance[8] = self._sigma_theta*self._sigma_theta self._pub.publish(msg)
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 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 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 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 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 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 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 _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 trigger(self): sample = self.gyro.get_sample() gs = Gyro() gs.header.frame_id = self.frame_id gs.header.stamp = rospy.Time.now() gs.calibration_offset.x = 0.0 gs.calibration_offset.y = 0.0 gs.calibration_offset.z = self.offset gs.angular_velocity.x = 0.0 gs.angular_velocity.y = 0.0 gs.angular_velocity.z = (sample - self.offset) * math.pi / 180.0 gs.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] self.pub.publish(gs) imu = Imu() imu.header.frame_id = self.frame_id imu.header.stamp = rospy.Time.now() imu.angular_velocity.x = 0.0 imu.angular_velocity.y = 0.0 imu.angular_velocity.z = (sample - self.offset) * math.pi / 180.0 imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1] self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec() self.prev_time = imu.header.stamp (imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion() self.pub2.publish(imu)
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_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 trigger(self): sample = self.gyro.get_sample() gs = Gyro() gs.header.frame_id = self.frame_id gs.header.stamp = rospy.Time.now() gs.calibration_offset.x = 0.0 gs.calibration_offset.y = 0.0 gs.calibration_offset.z = self.offset gs.angular_velocity.x = 0.0 gs.angular_velocity.y = 0.0 gs.angular_velocity.z = (sample-self.offset)*math.pi/180.0 gs.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] self.pub.publish(gs) imu = Imu() imu.header.frame_id = self.frame_id imu.header.stamp = rospy.Time.now() imu.angular_velocity.x = 0.0 imu.angular_velocity.y = 0.0 imu.angular_velocity.z = 1*math.pi/180.0 imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1] self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec() self.prev_time = imu.header.stamp (imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion() self.pub2.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 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 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 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 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 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 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 trigger(self): cs = Compass() # create frame id cs.header.frame_id = self.frame_id # create header (provides time stamp) cs.header.stamp = rospy.Time.now() # convert orientation from the CompassSensor.get_heading_lsb byte (heading in the 0-255 range) # to quaternion self.orientation = self.compass.get_sample()*-2.0 * math.pi/180.0 sample = self.compass.get_sample() # create new imu msgs for the compass with time stamp as before imu = Imu() imu.header.frame_id = self.frame_id imu.header.stamp = rospy.Time.now() imu.angular_velocity.x = 0.0 imu.angular_velocity.y = 0.0 imu.angular_velocity.z = 1*math.pi/180.0 # the rotation speed # covariance required by robot_pose_ekf imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1] self.orienation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec() imu.orientation = cs.orientation self.prev_time = imu.header.stamp (imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion() self.pub.publish(imu) # publish the message
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 spin(self): self.prev_time = rospy.Time.now() while not rospy.is_shutdown(): if self.calibrating: self.calibrate() self.calibrating = False self.prev_time = rospy.Time.now() acceldata = self.accelerometer.read6Reg(accel_x_low) compassdata = self.compass.read6Reg(compass_x_high) gyrodata = self.gyro.read6Reg(gyro_x_low) # prepare Imu frame imu = Imu() imu.header.frame_id = self.frame_id self.linear_acceleration = Vector3(); # get line from device #str = self.ser.readline() # timestamp imu.header.stamp = rospy.Time.now() #nums = str.split() # check, if it was correct line #if (len(nums) != 5): # continue self.linear_acceleration.x = self.twosComplement(acceldata[0], acceldata[1]) #/16384.0 self.linear_acceleration.y = self.twosComplement(acceldata[2], acceldata[3]) #/16384.0 self.linear_acceleration.z = self.twosComplement(acceldata[4], acceldata[5]) #/16384.0 imu.orientation.x = self.twosComplement(compassdata[1], compassdata[0]) #/1055.0, imu.orientation.y = self.twosComplement(compassdata[3], compassdata[2]) #/1055.0, imu.orientation.z = self.twosComplement(compassdata[5], compassdata[4]) #/950.0 imu.angular_velocity.x = self.twosComplement(gyrodata[0], gyrodata[1]) imu.angular_velocity.y = self.twosComplement(gyrodata[2], gyrodata[3]) imu.angular_velocity.z = self.twosComplement(gyrodata[4], gyrodata[5]) #gyro = int(nums[2]) #ref = int(nums[3]) #temp = int(nums[4]) #val = (ref-gyro - self.bias) * 1000 / 3 / 1024 * self.scale #imu.angular_velocity.x = 0 #imu.angular_velocity.y = 0 #imu.angular_velocity.z = val * math.pi / 180 imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1] self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec() self.prev_time = imu.header.stamp (imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion() self.pub.publish(imu)
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 _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 _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 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 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 imuCallback(data): imuData = Imu() imuData.header.stamp = rospy.Time.now() imuData.header.frame_id = 'imu_link' imuData.orientation = data.orientation imuData.orientation_covariance = data.orientation_covariance imuData.angular_velocity = data.angular_velocity imuData.angular_velocity_covariance = data.angular_velocity_covariance imuData.linear_acceleration = data.linear_acceleration imuData.linear_acceleration_covariance = data.linear_acceleration_covariance pub.publish(imuData)
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 convert_to_baselink(self, imu_msg, imu_model, transform): # convert imu_msg from its frame to baselink frame # Assumption! TODO: I'm going to assume that the axis are aligned between frames to start # come back to this later, and rotate to align axis first # proposed path: # -> rorate-transform data (orientation, angular velocity, linear acceleration) to align with base_link # -> run the same code below ''' [sensor_msgs/Imu]: std_msgs/Header header - DONE uint32 seq time stamp string frame_id geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w float64[9] orientation_covariance geometry_msgs/Vector3 angular_velocity float64 x float64 y float64 z float64[9] angular_velocity_covariance geometry_msgs/Vector3 linear_acceleration - DONE float64 x float64 y float64 z float64[9] linear_acceleration_covariance - DONE ''' new_msg = Imu() # Header new_msg.header = imu_msg.header new_msg.header.frame_id = '/base_link' # Orientation (same based on Assumption! above) new_msg.orientation = imu_msg.orientation # including covariance, because same. will likely drop for rotation new_msg.orientation_covariance = imu_msg.orientation_covariance # Angular Velocity (same based on Assumption! above) new_msg.angular_velocity = imu_msg.angular_velocity # including covariance, because same. will likely drop for rotation new_msg.angular_velocity_covariance = imu_msg.angular_velocity_covariance # Linear Acceleration (not the same, even with assumption) new_msg.linear_acceleration = self.solid_body_translate_lin_acc(imu_msg.linear_acceleration, imu_model, transform) return new_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._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(): 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 convertData(self): if self.msgid < self.msgs_len: msg = Imu() msg.header.seq = self.timestamp_ms[self.msgid][0] msg.header.stamp = rospy.Time.from_sec(self.stamp) # TODO: # SCALED_IMU.msg:int16 # HIGHRES_IMU.msg:float32 # RAW_IMU.msg:int16 msg.angular_velocity.x = self.getMsgValue("GyrX") msg.angular_velocity.y = self.getMsgValue("GyrY") msg.angular_velocity.z = self.getMsgValue("GyrZ") msg.linear_acceleration.x = self.getMsgValue("AccX") msg.linear_acceleration.y = self.getMsgValue("AccY") msg.linear_acceleration.z = self.getMsgValue("AccZ") # TODO: This is not enough for ROS IMU msg, add cov matrices and calculate orientation # simple complementary filter for orientation GYROSCOPE_SENSITIVITY = 1 # dt = self.last_stamp - self.stamp #pitch += (msg.angular_velocity.x / GYROSCOPE_SENSITIVITY) * dt # Angle around the X-axis #roll -= (msg.angular_velocity.y / GYROSCOPE_SENSITIVITY) * dt # Angle around the Y-axis # Turning around the X axis results in a vector on the Y-axis #pitchAcc = atan2(msg.linear_acceleration.x, ( sqrt(pow(msg.linear_acceleration.y,2.0) + pow(msg.linear_acceleration.z,2.0)) )) * 180.0 / M_PI #pitch = pitch * 0.98 + pitchAcc * 0.02 # Turning around the Y axis results in a vector on the X-axis #rollAcc = atan2(msg.linear_acceleration.y, (sqrt(pow(msg.linear_acceleration.x,2.0) + pow(msg.linear_acceleration.z,2.0)) )) * 180.0 / M_PI #roll = roll * 0.98 + rollAcc * 0.02 #yaw = (msg.angular_velocity.z / GYROSCOPE_SENSITIVITY) * dt #orientation.setRPY(roll, pitch, yaw) #tf::quaternionTFToMsg(orientation, orientation_msg) #msg.header.frame_id = frame_id_imu_link; #imu_transform.setRotation(orientation); msg.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] msg.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1] #self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec() #self.prev_time = imu.header.stamp #(imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion() self.bag.write(self.topic, msg, msg.header.stamp) self.msgid = self.msgid + 1
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 pub_imu(msg_type, msg, bridge): pub = bridge.get_ros_pub("imu", Imu, queue_size=1) imu = Imu() imu.header.stamp = bridge.project_ap_time(msg) imu.header.frame_id = 'base_footprint' # Orientation as a Quaternion, with unknown covariance quat = quaternion_from_euler(msg.roll, msg.pitch, msg.yaw, 'sxyz') imu.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3]) imu.orientation_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) # Angular velocities, with unknown covariance imu.angular_velocity.x = msg.rollspeed imu.angular_velocity.y = msg.pitchspeed imu.angular_velocity.z = msg.yawspeed imu.angular_velocity_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) # Not supplied with linear accelerations imu.linear_acceleration_covariance[0] = -1 pub.publish(imu)
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 imuDataReceived(data): global imu_repub angles = tf.transformations.euler_from_quaternion( [data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w] ) imu_msg = Imu() yaw = wrapToPi(angles[2] - math.pi / 2) orientation = tf.transformations.quaternion_from_euler(0, 0, yaw) imu_msg.orientation.x = orientation[0] imu_msg.orientation.y = orientation[1] imu_msg.orientation.z = orientation[2] imu_msg.orientation.w = orientation[3] imu_msg.header = data.header imu_msg.orientation_covariance = data.orientation_covariance imu_msg.angular_velocity = data.angular_velocity imu_repub.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 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 publish_imu(self): imu_msg = Imu() imu_msg.header.stamp = self.time imu_msg.header.frame_id = 'imu_odom' # Ugh, stupid quats, just publish raw for now :-D 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 = 0 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 = -1#self.kalman_state[6,0] imu_msg.linear_acceleration.y = -1#self.kalman_state[7,0] imu_msg.linear_acceleration.z = -1#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' 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 spin(self): self.prev_time = rospy.Time.now() while not rospy.is_shutdown(): if self.calibrating: self.calibrate() self.calibrating = False self.prev_time = rospy.Time.now() # prepare Imu frame imu = Imu() imu.header.frame_id = self.frame_id # get line from device str = self.ser.readline() # timestamp imu.header.stamp = rospy.Time.now() nums = str.split() # check, if it was correct line if (len(nums) != 5): continue gyro = int(nums[2]) ref = int(nums[3]) temp = int(nums[4]) val = (ref-gyro - self.bias) * 1000 / 3 / 1024 * self.scale imu.angular_velocity.x = 0 imu.angular_velocity.y = 0 imu.angular_velocity.z = val * math.pi / 180 imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1] self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec() self.prev_time = imu.header.stamp (imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion() self.pub.publish(imu)