def main(): rospy.init_node("imu") port = rospy.get_param("~port", "GPG3_AD1") sensor = InertialMeasurementUnit(bus=port) pub_imu = rospy.Publisher("~imu", Imu, queue_size=10) pub_temp = rospy.Publisher("~temp", Temperature, queue_size=10) pub_magn = rospy.Publisher("~magnetometer", MagneticField, queue_size=10) br = TransformBroadcaster() msg_imu = Imu() msg_temp = Temperature() msg_magn = MagneticField() hdr = Header(stamp=rospy.Time.now(), frame_id="IMU") rate = rospy.Rate(rospy.get_param('~hz', 30)) while not rospy.is_shutdown(): q = sensor.read_quaternion() # x,y,z,w mag = sensor.read_magnetometer() # micro Tesla (µT) gyro = sensor.read_gyroscope() # deg/second accel = sensor.read_accelerometer() # m/s² temp = sensor.read_temperature() # °C msg_imu.header = hdr hdr.stamp = rospy.Time.now() msg_temp.header = hdr msg_temp.temperature = temp pub_temp.publish(msg_temp) msg_imu.header = hdr msg_imu.linear_acceleration.x = accel[0] msg_imu.linear_acceleration.y = accel[1] msg_imu.linear_acceleration.z = accel[2] msg_imu.angular_velocity.x = math.radians(gyro[0]) msg_imu.angular_velocity.y = math.radians(gyro[1]) msg_imu.angular_velocity.z = math.radians(gyro[2]) msg_imu.orientation.w = q[3] msg_imu.orientation.x = q[0] msg_imu.orientation.y = q[1] msg_imu.orientation.z = q[2] pub_imu.publish(msg_imu) msg_magn.header = hdr msg_magn.magnetic_field.x = mag[0] * 1e-6 msg_magn.magnetic_field.y = mag[1] * 1e-6 msg_magn.magnetic_field.z = mag[2] * 1e-6 pub_magn.publish(msg_magn) transform = TransformStamped(header=Header(stamp=rospy.Time.now(), frame_id="world"), child_frame_id="IMU") transform.transform.rotation = msg_imu.orientation br.sendTransformMessage(transform) rate.sleep()
def sensor_interface(): rospy.init_node('imu_razor9dof', anonymous = False) interface.flushInput() # Perform a loop checking on the enabled status of the drivers while not rospy.is_shutdown(): # Fetch a JSON message from the controller and process (or atleast attempt to) try: # Get the object and create some messages _object = json.loads(interface.readline()) _imu = Imu(); _heading = Float32() # Get the imu data (f**k quaternions!!) _imu.header = rospy.Header() roll = _object['angles'][0] pitch = _object['angles'][1] yaw = _object['angles'][2] #_imu.orientation = tf::createQuaternionFromRPY(roll, pitch, yaw) _imu.linear_acceleration.x = _object['accel'][0] _imu.linear_acceleration.y = _object['accel'][1] _imu.linear_acceleration.z = _object['accel'][2] # Get the heading data #_heading.data = _object['heading'] _heading.data = _object['angles'][2] # Publish the data imuPublisher.publish(_imu); cmpPublisher.publish(_heading); except json.decoder.JSONDecodeError: rospy.logwarn("Invalid message received") except: pass
def imuCallback(self, imu): self.acceleration[0] = imu.linear_acceleration.x self.acceleration[1] = imu.linear_acceleration.y self.acceleration[2] = imu.linear_acceleration.z #print("Accel", self.acceleration) accel_temp = np.array([-imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z]) #print("AccelRaw",accel_temp) accel_temp_trans = self.getQuaternionOrientation().rotate(accel_temp) accel_temp_trans[0]= -accel_temp_trans[0] accel_temp_trans[1]= -accel_temp_trans[1] accel_temp_trans[2] = -(accel_temp_trans[2] - 9.81) self.acceleration_rot = accel_temp_trans self.accelMedianX = np.roll(self.accelMedianX, 1) self.accelMedianY = np.roll(self.accelMedianY, 1) self.accelMedianZ = np.roll(self.accelMedianZ, 1) self.accelMedianX[0] = accel_temp_trans[0] self.accelMedianY[1] = accel_temp_trans[1] self.accelMedianZ[2] = accel_temp_trans[2] self.filteredAccelValue[0]= (np.sum(self.accelMedianX) / len(self.accelMedianX)) self.filteredAccelValue[1] = (np.sum(self.accelMedianY) / len(self.accelMedianY)) self.filteredAccelValue[2] = (np.sum(self.accelMedianZ) / len(self.accelMedianZ)) # print(imu.linear_acceleration.x) #print("X",self.accelMedianX) #print("X Filtered", self.filteredAccelValue[0]) accelNED=Imu() accelNED.header=imu.header accelNED.linear_acceleration.x=self.filteredAccelValue[0] accelNED.linear_acceleration.y = self.filteredAccelValue[1] accelNED.linear_acceleration.z = self.filteredAccelValue[2] self.accel_publish.publish(accelNED)
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(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 run(self): while not rospy.is_shutdown(): time_now = time.time() if time_now - self.timestamp > 0.5: self.cmd = (0, 0) self.lock.acquire() self.zumy.cmd(*self.cmd) imu_data = self.zumy.read_imu() self.lock.release() imu_msg = Imu() imu_msg.header = Header(self.imu_count, rospy.Time.now(), self.name) imu_msg.linear_acceleration.x = 9.81 * imu_data[0] imu_msg.linear_acceleration.y = 9.81 * imu_data[1] imu_msg.linear_acceleration.z = 9.81 * imu_data[2] imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3] imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4] imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5] self.imu_pub.publish(imu_msg) self.heartBeat.publish("I am alive from Glew") if self.msg != None: self.publisher.publish(self.msg.linear.y) self.rate.sleep() # If shutdown, turn off motors self.zumy.cmd(0, 0)
def sensor_data_updated(self, carla_imu_measurement): """ Function to transform a received imu measurement into a ROS Imu message :param carla_imu_measurement: carla imu measurement object :type carla_imu_measurement: carla.IMUMeasurement """ imu_msg = Imu() imu_msg.header = self.get_msg_header(timestamp=carla_imu_measurement.timestamp) # Carla uses a left-handed coordinate convention (X forward, Y right, Z up). # Here, these measurements are converted to the right-handed ROS convention # (X forward, Y left, Z up). imu_msg.angular_velocity.x = -carla_imu_measurement.gyroscope.x imu_msg.angular_velocity.y = carla_imu_measurement.gyroscope.y imu_msg.angular_velocity.z = -carla_imu_measurement.gyroscope.z imu_msg.linear_acceleration.x = carla_imu_measurement.accelerometer.x imu_msg.linear_acceleration.y = -carla_imu_measurement.accelerometer.y imu_msg.linear_acceleration.z = carla_imu_measurement.accelerometer.z imu_rotation = carla_imu_measurement.transform.rotation quat = trans.carla_rotation_to_numpy_quaternion(imu_rotation) imu_msg.orientation = trans.numpy_quaternion_to_ros_quaternion(quat) self.publish_message( self.get_topic_prefix(), imu_msg)
def imuResponseCallback(self, msg): ############################################################################# self.num_imu_packets += 1 if self.num_imu_packets > 100: imu_msg = Imu() imu_msg.header = msg.header imu_msg.header.frame_id = "imu_link"
def do_transform_imu(imu_in, transform): transform = copy.deepcopy(transform) kdl_tf = transform_to_kdl(transform) quaternion = (kdl_tf.M * PyKDL.Rotation.Quaternion( imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z, imu_in.orientation.w)).GetQuaternion() angular_velocity = kdl_tf * PyKDL.Vector( imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z) linear_acceleration = kdl_tf * PyKDL.Vector( imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z) imu_out = Imu() imu_out.header = transform.header imu_out.orientation.x = quaternion[0] imu_out.orientation.y = quaternion[1] imu_out.orientation.z = quaternion[2] imu_out.orientation.w = quaternion[3] imu_out.angular_velocity.x = angular_velocity[0] imu_out.angular_velocity.y = angular_velocity[1] imu_out.angular_velocity.z = angular_velocity[2] imu_out.linear_acceleration.x = linear_acceleration[0] imu_out.linear_acceleration.y = linear_acceleration[1] imu_out.linear_acceleration.z = linear_acceleration[2] return imu_out
def imuResponseCallback(self, msg): ############################################################################# imu_msg = Imu() imu_msg.header = msg.header imu_msg.orientation = msg.orientation imu_msg.angular_velocity = msg.angular_velocity imu_msg.linear_acceleration = msg.linear_acceleration imu_msg.orientation_covariance[0] = msg.orientation_covariance[0] imu_msg.orientation_covariance[4] = msg.orientation_covariance[4] if self.rtkEnabled == 0: imu_msg.orientation_covariance[8] = msg.orientation_covariance[8] else: imu_msg.orientation_covariance[8] = msg.orientation_covariance[ 8] + 99999 / (1 + math.exp(-8 * (abs(self.totalVel) - 2)) ) # sigmoid function to mix yaw from the IMU imu_msg.angular_velocity_covariance[ 0] = msg.angular_velocity_covariance[0] imu_msg.angular_velocity_covariance[ 4] = msg.angular_velocity_covariance[4] imu_msg.angular_velocity_covariance[ 8] = msg.angular_velocity_covariance[8] imu_msg.linear_acceleration_covariance[ 0] = msg.linear_acceleration_covariance[0] imu_msg.linear_acceleration_covariance[ 4] = msg.linear_acceleration_covariance[4] imu_msg.linear_acceleration_covariance[ 8] = msg.linear_acceleration_covariance[8] #rospy.logerr("Vel: %f Z Rot Covariance: %f", self.totalVel, imu_msg.orientation_covariance[8]) self.imuPub.publish(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 publishIMU(publisher): imu_raw = Imu() # Header: h = std_msgs.msg.Header() h.stamp = rospy.Time.now() h.frame_id = 'imu_filter' imu_raw.header = h # Data: imu_raw.orientation_covariance[0] = -1 imu_raw.linear_acceleration.x = 0 imu_raw.linear_acceleration.y = 0 imu_raw.linear_acceleration.z = 0 imu_raw.linear_acceleration_covariance[0] = -1 imu_raw.angular_velocity.x = 0 imu_raw.angular_velocity.y = 0 imu_raw.angular_velocity.z = 0 imu_raw.angular_velocity_covariance[0] = -1 imu_raw.orientation.w = 1 imu_raw.orientation.x = 0 imu_raw.orientation.y = 0 imu_raw.orientation.z = 0 imu_raw.orientation_covariance[0] = -1 publisher.publish(imu_raw)
def on_imu(channel, data): m = ins_t.decode(data) pyTime = rospy.Time.from_sec(m.utime * 1E-6) #print "Received ins message", pyTime pose = PoseStamped() t = rospy.get_rostime() pose.header.stamp = pyTime pose.header.frame_id = 'imu_link' pose.pose.orientation.w = m.quat[0] pose.pose.orientation.x = m.quat[1] pose.pose.orientation.y = m.quat[2] pose.pose.orientation.z = m.quat[3] imuRosPosePub.publish(pose) imu = Imu() imu.header = pose.header imu.orientation = pose.pose.orientation imu.angular_velocity.x = m.gyro[0] imu.angular_velocity.y = m.gyro[1] imu.angular_velocity.z = m.gyro[2] imu.linear_acceleration.x = m.accel[0] imu.linear_acceleration.y = m.accel[1] imu.linear_acceleration.z = m.accel[2] imuRosPub.publish(imu)
def sensor_data_updated(self, carla_imu_measurement): """ Function to transform a received imu measurement into a ROS Imu message :param carla_imu_measurement: carla imu measurement object :type carla_imu_measurement: carla.IMUMeasurement """ imu_msg = Imu() imu_msg.header = self.get_msg_header( timestamp=carla_imu_measurement.timestamp) # Carla uses a left-handed coordinate convention (X forward, Y right, Z up). # Here, these measurements are converted to the right-handed ROS convention # (X forward, Y left, Z up). imu_msg.angular_velocity.x = -carla_imu_measurement.gyroscope.x imu_msg.angular_velocity.y = carla_imu_measurement.gyroscope.y imu_msg.angular_velocity.z = -carla_imu_measurement.gyroscope.z imu_msg.linear_acceleration.x = carla_imu_measurement.accelerometer.x imu_msg.linear_acceleration.y = -carla_imu_measurement.accelerometer.y imu_msg.linear_acceleration.z = carla_imu_measurement.accelerometer.z roll, pitch, yaw = trans.carla_rotation_to_RPY( carla_imu_measurement.transform.rotation) quat = euler2quat(roll, pitch, yaw) imu_msg.orientation.w = quat[0] imu_msg.orientation.x = quat[1] imu_msg.orientation.y = quat[2] imu_msg.orientation.z = quat[3] self.imu_publisher.publish(imu_msg)
def decode_regs_encode_msg(data, header=None, calibration=False): global td ax = np.int16(int(binascii.hexlify(data[0:2]), 16)) * IMUPI_A_GAIN * M_G ay = np.int16(int(binascii.hexlify(data[2:4]), 16)) * IMUPI_A_GAIN * M_G az = np.int16(int(binascii.hexlify(data[4:6]), 16)) * IMUPI_A_GAIN * M_G gx = np.int16(int(binascii.hexlify(data[8:10]), 16)) * IMUPI_G_GAIN * M_PI / 180 - GYRO_OFF_X gy = np.int16(int(binascii.hexlify(data[10:12]), 16)) * IMUPI_G_GAIN * M_PI / 180 - GYRO_OFF_Y gz = np.int16(int(binascii.hexlify(data[12:14]), 16)) * IMUPI_G_GAIN * M_PI / 180 - GYRO_OFF_Z if calibration == False: msg = Imu() msg.linear_acceleration.x = ax msg.linear_acceleration.y = ay msg.linear_acceleration.z = az msg.angular_velocity.x = gx msg.angular_velocity.y = gy msg.angular_velocity.z = gz msg.header = header msg.header.stamp = msg.header.stamp + td return msg else: #print('ax: {0}, ay: {1}, az: {2}, gx: {3}, gy: {4}, gz: {5}'.format(ax, ay, az, gx, gy, gz)) return ax, ay, az, gx, gy, gz
def imuCallback(self, data): self.stamp = data.header.stamp dt = self.stamp - self.last_stamp self.last_stamp = data.header.stamp # print("dt = ", dt.to_sec()) data_output = Imu() data_output.orientation.w = self.ori_w.predic(data.orientation.w, dt.to_sec()) data_output.orientation.x = self.ori_x.predic(data.orientation.x if False else 0.0, dt.to_sec()) data_output.orientation.y = self.ori_y.predic(data.orientation.y if False else 0.0, dt.to_sec()) data_output.orientation.z = self.ori_z.predic(data.orientation.z, dt.to_sec()) data_output.angular_velocity.x = self.av_x.predic( data.angular_velocity.x if False else 0.0, dt.to_sec()) data_output.angular_velocity.y = self.av_y.predic( data.angular_velocity.y if False else 0.0, dt.to_sec()) data_output.angular_velocity.z = self.av_z.predic( data.angular_velocity.z, dt.to_sec()) data_output.linear_acceleration.x = self.la_x.predic( data.linear_acceleration.x, dt.to_sec()) data_output.linear_acceleration.y = self.la_y.predic( data.linear_acceleration.y, dt.to_sec()) data_output.linear_acceleration.z = self.la_z.predic( data.linear_acceleration.z if False else 0.0, dt.to_sec()) data_output.header = data.header self.pub_imuCorrect.publish(data_output)
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(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 odom_subscriber_cb(data): imu_msg = Imu() imu_msg.header = data.header imu_msg.header.frame_id = 'base_link' imu_msg.orientation = data.pose.pose.orientation imu_msg.linear_acceleration.z = -10 pub.publish(imu_msg)
def publish_imu(self): if not self.last_euler or not self.last_accelerometer: return with self._imu_pub_lock: imu_msg = Imu() imu_msg.header = Header() imu_msg.header.stamp = rospy.Time.now() imu_msg.header.frame_id = c.BASE_LINK # Our sensor returns Euler angles in degrees, but ROS requires radians. roll = self.last_euler['x'] * pi / 180. pitch = self.last_euler['y'] * pi / 180. yaw = self.last_euler['z'] * pi / 180. # http://answers.ros.org/question/69754/quaternion-transformations-in-python/ quaternion = tf.transformations.quaternion_from_euler( roll, pitch, yaw) imu_msg.orientation.x = quaternion[0] imu_msg.orientation.y = quaternion[1] imu_msg.orientation.z = quaternion[2] imu_msg.orientation.w = quaternion[3] #imu_msg.angular_velocity_covariance = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; # imu_msg.angular_velocity.x = ns.omgx(); # imu_msg.angular_velocity.y = ns.omgy(); # imu_msg.angular_velocity.z = ns.omgz(); #imu_msg.angular_velocity_covariance = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; imu_msg.linear_acceleration.x = self.last_accelerometer['x'] imu_msg.linear_acceleration.y = self.last_accelerometer['y'] imu_msg.linear_acceleration.z = self.last_accelerometer['z'] #imu_msg.angular_velocity_covariance = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; self.imu_pub.publish(imu_msg)
def talker(): pub = rospy.Publisher('imu', Imu, queue_size=10) rospy.init_node('imusensor', anonymous=True) r = rospy.Rate(500) while not rospy.is_shutdown(): #str = "hello world %s"%print_gyro().__str__() arr = print_gyro() i = Imu() i.header = std_msgs.msg.Header() i.header.frame_id="my_frame" i.header.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work i.linear_acceleration.x=arr[0][0] i.linear_acceleration.y=arr[0][1] i.linear_acceleration.z=arr[0][2] i.angular_velocity.x=arr[1][0] i.angular_velocity.y=arr[1][1] i.angular_velocity.z=arr[1][2] i.orientation.x=0 i.orientation.y=0 i.orientation.z=0 i.orientation.w=0 #rospy.loginfo(str) pub.publish(i) r.sleep()
def run(self): while not rospy.is_shutdown(): self.lock.acquire() self.zumy.cmd(*self.cmd) imu_data = self.zumy.read_imu() enc_data = self.zumy.read_enc() self.lock.release() imu_msg = Imu() imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name) imu_msg.linear_acceleration.x = 9.81 * imu_data[0] imu_msg.linear_acceleration.y = 9.81 * imu_data[1] imu_msg.linear_acceleration.z = 9.81 * imu_data[2] imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3] imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4] imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5] self.imu_pub.publish(imu_msg) enc_msg = Int32() enc_msg.data = enc_data[0] self.r_enc_pub.publish(enc_msg) enc_msg.data = enc_data[1] self.l_enc_pub.publish(enc_msg) v_bat = self.zumy.read_voltage() self.batt_pub.publish(v_bat) self.heartBeat.publish("I am alive") self.rate.sleep() # If shutdown, turn off motors self.zumy.cmd(0,0)
def sensor_data_updated(self, carla_imu_measurement): """ Function to transform a received imu measurement into a ROS Imu message :param carla_imu_measurement: carla imu measurement object :type carla_imu_measurement: carla.IMUMeasurement """ imu_msg = Imu() imu_msg.header = self.get_msg_header(timestamp=carla_imu_measurement.timestamp) imu_msg.angular_velocity.x = carla_imu_measurement.gyroscope.x imu_msg.angular_velocity.y = carla_imu_measurement.gyroscope.y imu_msg.angular_velocity.z = carla_imu_measurement.gyroscope.z imu_msg.linear_acceleration.x = carla_imu_measurement.accelerometer.x imu_msg.linear_acceleration.y = carla_imu_measurement.accelerometer.y imu_msg.linear_acceleration.z = carla_imu_measurement.accelerometer.z imu_rotation = carla_imu_measurement.transform.rotation quat = tf.transformations.quaternion_from_euler(math.radians(imu_rotation.roll), math.radians(imu_rotation.pitch), math.radians(imu_rotation.yaw)) imu_msg.orientation = trans.numpy_quaternion_to_ros_quaternion(quat) self.publish_message( self.get_topic_prefix(), imu_msg)
def imu_msg_callback(msg, cb_args): pub = cb_args[0] global last_lin_acc global last_ang_vel send = Imu() send.header = msg.header send.header.stamp = rospy.Time.now() # send.header.frame_id = "camera_imu_frame" # if msg.linear_acceleration_covariance[0] != -1.0: # last_lin_acc.x = msg.linear_acceleration.z # last_lin_acc.y = msg.linear_acceleration.x * -1 # last_lin_acc.z = msg.linear_acceleration.y * -1 # elif msg.angular_velocity_covariance[0] != -1.0: # last_ang_vel.x = msg.angular_velocity.z # last_ang_vel.y = msg.angular_velocity.x * -1 # last_ang_vel.z = msg.angular_velocity.y * -1 if msg.linear_acceleration_covariance[0] != -1.0: last_lin_acc = msg.linear_acceleration elif msg.angular_velocity_covariance[0] != -1.0: last_ang_vel = msg.angular_velocity send.angular_velocity = last_ang_vel send.linear_acceleration = last_lin_acc send.orientation_covariance[0] = -1.0 pub.publish(send)
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 run(self): while not rospy.is_shutdown(): self.lock.acquire() self.zumy.cmd(*self.cmd) imu_data = self.zumy.read_imu() enc_data = self.zumy.read_enc() self.lock.release() imu_msg = Imu() imu_msg.header = Header(self.imu_count, rospy.Time.now(), self.name) imu_msg.linear_acceleration.x = 9.81 * imu_data[0] imu_msg.linear_acceleration.y = 9.81 * imu_data[1] imu_msg.linear_acceleration.z = 9.81 * imu_data[2] imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3] imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4] imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5] self.imu_pub.publish(imu_msg) enc_msg = Int32() enc_msg.data = enc_data[0] self.r_enc_pub.publish(enc_msg) enc_msg.data = enc_data[1] self.l_enc_pub.publish(enc_msg) v_bat = self.zumy.read_voltage() self.batt_pub.publish(v_bat) self.heartBeat.publish("I am alive") self.rate.sleep() # If shutdown, turn off motors self.zumy.cmd(0, 0)
def parse_imu(self, data): ''' Given data from jaguar imu, parse and return a standard IMU message. Return None when given bad data, or no complete message was found in data. ''' # Use regular expressions to extract complete message # message format: $val,val,val,val,val,val,val,val,val,val#\n hit = re.search(r"\$-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*#", data) if not hit: # if there are no hits, return None return None else: imu_data = hit.group(0) try: # Try to format the data imu_data = imu_data[1:-1].split(",") #Format (from drrobot docs): seq, accelx, accely, accelz, gyroY, gyroZ, gyroX, magnetonX, magnetonY, magnetonZ seq = int(imu_data[0]) accelx = float(imu_data[1]) accely = float(imu_data[2]) accelz = float(imu_data[3]) gyroy = float(imu_data[4]) gyroz = float(imu_data[5]) gyrox = float(imu_data[6]) magnetonx = float(imu_data[7]) magnetony = float(imu_data[8]) magnetonz = float(imu_data[9]) rot_mat = [ [float(imu_data[10]), float(imu_data[11]), float(imu_data[12])], [float(imu_data[13]), float(imu_data[14]), float(imu_data[15])], [float(imu_data[16]), float(imu_data[17]), float(imu_data[18])] ] except: # bad data in match, pass return None else: # data formatted fine, build message and publish # if we didn't get a magnetometer update, set to current reading if magnetonz == 0: magnetonx = self.current_mag[0] magnetony = self.current_mag[1] magnetonz = self.current_mag[2] # otherwise, update current magnetometer else: self.current_mag = [magnetonx, magnetony, magnetonz] # Calculate quaternion from given rotation matrix quat = rot_mat_to_quat(rot_mat); # Build message msg = Imu() msg.header = Header(stamp = rospy.Time.now(), frame_id = "imu") msg.linear_acceleration = Vector3(accelx, accely, accelz) msg.angular_velocity = Vector3(gyrox, gyroy, gyroz) if quat != None: msg.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3]) return 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] == '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 callback(data): quaternion = tf.transformations.quaternion_from_euler( data.angle.x, -data.angle.y, -data.angle.z + np.pi / 2) imu_data = Imu() imu_data.orientation.x = quaternion[0] imu_data.orientation.y = quaternion[1] imu_data.orientation.z = quaternion[2] imu_data.orientation.w = quaternion[3] imu_data.header = data.header pub.publish(imu_data)
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 handleIMU(data): rospy.logdebug("TorsoIMU received for conversion: %f %f", data.angleX, data.angleY) imu_msg = Imu() q = tf.quaternion_from_euler(data.angleX, data.angleY, 0.0) imu_msg.header = data.header imu_msg.orientation.x = q[0] imu_msg.orientation.y = q[1] imu_msg.orientation.z = q[2] imu_msg.orientation.w = q[3] pub.publish(imu_msg)
def imu_raw_callback(self, msg): if self.start_vo: imu_msg = Imu() imu_msg.header = msg.header imu_msg.angular_velocity = msg.angular_velocity imu_msg.angular_velocity_covariance = msg.angular_velocity_covariance imu_msg.linear_acceleration = msg.linear_acceleration imu_msg.linear_acceleration_covariance = msg.linear_acceleration_covariance imu_msg.header.frame_id = "imu" for i in range(0, 9, 4): imu_msg.orientation_covariance[i] = IMU_SIGMA**2 self.imupub.publish(imu_msg)
def callbackImu(msg): # Grab accelerometer and gyro data. imu_msg = Imu() imu_msg.header = msg.header imu_msg.header.frame_id = 'imu_link' q = np.array([msg.quaternion.x, msg.quaternion.y, msg.quaternion.z, msg.quaternion.w]) q = normalize(q) euler = tf.transformations.euler_from_quaternion(q) q = tf.transformations.quaternion_from_euler(-(euler[0] + math.pi / 2), euler[1], euler[2] - math.pi) imu_msg.orientation.x = q[0] imu_msg.orientation.y = q[1] imu_msg.orientation.z = q[2] imu_msg.orientation.w = q[3] euler_new = tf.transformations.euler_from_quaternion(q) rospy.loginfo("euler[0] = %s, euler[1] = %s, euler = [2] = %s", str(euler_new[0]), str(euler_new[1]), str(euler_new[2])) imu_msg.orientation_covariance = [0.9, 0, 0, \ 0, 0.9, 0, \ 0, 0, 0.9] imu_msg.angular_velocity = msg.gyro imu_msg.angular_velocity_covariance = [0.9, 0, 0, \ 0, 0.9, 0, \ 0, 0, 0.9] # Estimate gyro bias. global gyro_readings global MAX_GYRO_READINGS global gyro_bias if len(gyro_readings) < MAX_GYRO_READINGS: gyro_readings.append(imu_msg.angular_velocity.z) elif math.isnan(gyro_bias): gyro_bias = sum(gyro_readings)/MAX_GYRO_READINGS if not math.isnan(gyro_bias): imu_msg.angular_velocity.z -= gyro_bias imu_msg.linear_acceleration = msg.accelerometer imu_msg.linear_acceleration_covariance = [0.90, 0, 0, \ 0, 0.90, 0, \ 0, 0, 0.90] # Grab magnetometer data. mag_msg = MagneticField() mag_msg.header = msg.header mag_msg.magnetic_field = msg.magnetometer # TODO(gbrooks): Add covariance. # Publish sensor data. imu_pub.publish(imu_msg) mag_pub.publish(mag_msg)
def callback(raw_data, pub): imu = Imu() imu.header = raw_data.header # set imu.orientation quaternion # Btw, robot_pose_ekf expects the Roll and Pitch angles to be # absolute, but the Yaw angle to be relative. Compute the relative # angle from the currently reported and previously reported absolute # angle r = raw_data.roll p = raw_data.pitch y = raw_data.yaw - callback.old_yaw callback.old_yaw = raw_data.yaw quat = quaternion_from_euler(r,p,y,"sxyz") imu.orientation.x = quat[0] imu.orientation.y = quat[1] imu.orientation.z = quat[2] imu.orientation.w = quat[3] #TODO: set imu.orientation_covariance #row major about x, y, z # According to OS5000 datasheet, accuracy # depends on tilt: # 0.5 deg RMS level heading, # 1 deg typical RMS accuracy < +-30deg tilt, # 1.5deg < +-60deg tilt # Roll and Pitch: # Typical 1deg accuracy for tilt < +-30 deg # Assume tilt < +- 30 deg, set all standard # deviations to 1 degree std_dev = from_degrees(1) imu.orientation_covariance[0] = std_dev**2 imu.orientation_covariance[4] = std_dev**2 # standard deviation on yaw is doubled since # it's computed as a difference of two measurements, # each with a std_dev of 1 degree imu.orientation_covariance[8] = (2*std_dev)**2 # no angular velocity data is available, so set # element 0 of covariance matrix to -1 # alternatively, set the covariance really high imu.angular_velocity = Vector3(0,0,0) imu.angular_velocity_covariance[0] = -1 # Set linear acceleration imu.linear_acceleration = raw_data.acceleration # TODO: Set linear acceleration covariance imu.linear_acceleration_covariance[0] = -1 pub.publish(imu)
def callback(raw_data, pub): imu = Imu() imu.header = raw_data.header # set imu.orientation quaternion # Btw, robot_pose_ekf expects the Roll and Pitch angles to be # absolute, but the Yaw angle to be relative. Compute the relative # angle from the currently reported and previously reported absolute # angle r = raw_data.roll p = raw_data.pitch y = raw_data.yaw - callback.old_yaw callback.old_yaw = raw_data.yaw quat = quaternion_from_euler(r, p, y, "sxyz") imu.orientation.x = quat[0] imu.orientation.y = quat[1] imu.orientation.z = quat[2] imu.orientation.w = quat[3] #TODO: set imu.orientation_covariance #row major about x, y, z # According to OS5000 datasheet, accuracy # depends on tilt: # 0.5 deg RMS level heading, # 1 deg typical RMS accuracy < +-30deg tilt, # 1.5deg < +-60deg tilt # Roll and Pitch: # Typical 1deg accuracy for tilt < +-30 deg # Assume tilt < +- 30 deg, set all standard # deviations to 1 degree std_dev = from_degrees(1) imu.orientation_covariance[0] = std_dev**2 imu.orientation_covariance[4] = std_dev**2 # standard deviation on yaw is doubled since # it's computed as a difference of two measurements, # each with a std_dev of 1 degree imu.orientation_covariance[8] = (2 * std_dev)**2 # no angular velocity data is available, so set # element 0 of covariance matrix to -1 # alternatively, set the covariance really high imu.angular_velocity = Vector3(0, 0, 0) imu.angular_velocity_covariance[0] = -1 # Set linear acceleration imu.linear_acceleration = raw_data.acceleration # TODO: Set linear acceleration covariance imu.linear_acceleration_covariance[0] = -1 pub.publish(imu)
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 __call__(self, channel, data): lcm_msg = agile.imuRaw_t.decode(data) ros_msg = Imu() ros_msg.header = self.make_header(lcm_msg.utime) ros_msg.angular_velocity.x = lcm_msg.gyro[0] ros_msg.angular_velocity.y = lcm_msg.gyro[1] ros_msg.angular_velocity.z = lcm_msg.gyro[2] ros_msg.angular_velocity_covariance = self.angular_velocity_cov ros_msg.linear_acceleration.x = lcm_msg.accel[0] ros_msg.linear_acceleration.y = lcm_msg.accel[1] ros_msg.linear_acceleration.z = lcm_msg.accel[2] ros_msg.linear_acceleration_covariance = self.linear_acceleration_cov return [self.rostopic], [ros_msg]
def on_imu_relay(self, msg): parts = msg.data.split(':') # Validate type. typ = parts[0] assert typ in 'aeg', 'Invalid typ: %s' % typ # Convert the integers to the original floats. nums = ltof(parts[1:]) for num, axis in zip(nums, 'xyz'): self._imu_data['%s%s' % (typ, axis)] = num # If we've received the final segment, re-publish the complete IMU message. if typ == 'a': # https://docs.ros.org/api/sensor_msgs/html/msg/Imu.html imu_msg = Imu() imu_msg.header = Header() imu_msg.header.stamp = rospy.Time.now() imu_msg.header.frame_id = c.BASE_LINK #TODO # Our sensor returns Euler angles in degrees, but ROS requires radians. # http://answers.ros.org/question/69754/quaternion-transformations-in-python/ roll = self._imu_data['ex'] pitch = self._imu_data['ey'] yaw = self._imu_data['ez'] quaternion = tf.transformations.quaternion_from_euler( roll, pitch, yaw) imu_msg.orientation.x = quaternion[0] imu_msg.orientation.y = quaternion[1] imu_msg.orientation.z = quaternion[2] imu_msg.orientation.w = quaternion[3] imu_msg.orientation_covariance = [ 1, 0.001, 0.001, 0.001, 1, 0.001, 0.001, 0.001, 1 ] imu_msg.angular_velocity.x = self._imu_data['gx'] imu_msg.angular_velocity.y = self._imu_data['gy'] imu_msg.angular_velocity.z = self._imu_data['gz'] imu_msg.angular_velocity_covariance = [ 1, 0.001, 0.001, 0.001, 1, 0.001, 0.001, 0.001, 1 ] imu_msg.linear_acceleration.x = self._imu_data['ax'] imu_msg.linear_acceleration.y = self._imu_data['ay'] imu_msg.linear_acceleration.z = self._imu_data['az'] imu_msg.linear_acceleration_covariance = [ 1, 0.001, 0.001, 0.001, 1, 0.001, 0.001, 0.001, 1 ] self.imu_pub.publish(imu_msg)
def default(self, ci='unused'): imu = Imu() imu.header = self.get_ros_header() imu.orientation = self.component_instance.bge_object.worldOrientation.to_quaternion() imu.angular_velocity.x = self.data['angular_velocity'][0] imu.angular_velocity.y = self.data['angular_velocity'][1] imu.angular_velocity.z = self.data['angular_velocity'][2] imu.linear_acceleration.x = self.data['linear_acceleration'][0] imu.linear_acceleration.y = self.data['linear_acceleration'][1] imu.linear_acceleration.z = self.data['linear_acceleration'][2] self.publish(imu)
def map_imu(values): """ Map the values generated with the hypothesis-ros imu strategy to a rospy imu type. """ if not isinstance(values, _Imu): raise TypeError('Wrong type. Use appropriate hypothesis-ros type.') ros_imu = Imu() ros_imu.header = values.header ros_imu.orientation = values.orientation ros_imu.orientation_covariance = values.orientation_covariance ros_imu.angular_velocity = values.angular_velocity ros_imu.angular_velocity_covariance = values.angular_velocity_covariance ros_imu.linear_acceleration = values.linear_acceleration ros_imu.linear_acceleration_covariance = values.linear_acceleration_covariance return ros_imu
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 default(self, ci='unused'): """ Publish the data of the IMU sensor as a custom ROS Imu message """ imu = Imu() imu.header = self.get_ros_header() imu.orientation = self.orientation imu.angular_velocity.x = self.data['angular_velocity'][0] imu.angular_velocity.y = self.data['angular_velocity'][1] imu.angular_velocity.z = self.data['angular_velocity'][2] imu.linear_acceleration.x = self.data['linear_acceleration'][0] imu.linear_acceleration.y = self.data['linear_acceleration'][1] imu.linear_acceleration.z = self.data['linear_acceleration'][2] self.publish(imu)
def subCB(msg_in): global pub msg_out = Imu() msg_out.header = msg_in.header q = quaternion_from_euler(msg_in.RPY.x/180.0 * pi, msg_in.RPY.y/180.0 * pi, msg_in.RPY.z/180.0 * pi) msg_out.orientation.x = q[0] msg_out.orientation.y = q[1] msg_out.orientation.z = q[2] msg_out.orientation.w = q[3] pub.publish(msg_out)
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 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 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 _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 run(self): counter = 0 rospy.loginfo("Simulation started") while not rospy.is_shutdown(): self.q_pub.publish(Quaternion(self.theta, self.omega, self.bias_yaw, self.delta_mag)) now = rospy.Time.now() rpy = Vector3Stamped() rpy.header.stamp = now rpy.vector.z = self.theta - self.bias_yaw + self.noise(self.noise_yaw) self.rpy_pub.publish(rpy) mag = Vector3Stamped() mag.header = rpy.header magnitude = 150 + self.noise(self.noise_magnitude_mag) theta = self.theta - self.delta_mag + self.noise(self.noise_mag) mag.vector.x = magnitude * math.cos(theta) mag.vector.y = magnitude * math.sin(theta) self.mag_pub.publish(mag) imu = Imu() imu.header = mag.header imu.angular_velocity.z = self.omega + self.noise(self.noise_omega) self.imu_pub.publish(imu) if (counter % 20) == 0: gps = GPSFix() gps.header = rpy.header gps.speed = 1.0 gps.track = self.theta + self.noise(self.noise_gps) gps.track = norm_angle(gps.track) self.gps_pub.publish(gps) counter += 1 self.bias_index += self.bias_speed * self.dt self.bias_yaw = 2 * math.pi * math.cos(self.bias_index) self.theta += self.omega * self.dt self.theta = norm_angle(self.theta) rospy.sleep(self.dt)
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 velodyne_Dat(): nmea = rospy.Publisher('nmea_sentence', Sentence, queue_size=1) imu = rospy.Publisher('imu_data', Imu, queue_size=1) rospy.init_node('velodyne_GPS', anonymous=True) r = rospy.Rate(100) # 10hz while not rospy.is_shutdown(): data, addr = sock.recvfrom(1206) # buffer size is 1206 bytes if (addr[0] == "192.168.1.201"): # Ensure message is from Velodyne HDL32-E val = data[206:278].encode('ascii') gyro1_temp =(data[15:16] + data[14:15]).encode("hex") gyro1 = int( gyro1_temp[1:],16) if gyro1 > 2047: gyro1 = (-4096 + gyro1) gyro1 = gyro1 * GYRO_SCALE_FACTOR temp1_temp =(data[17:18] + data[16:17]).encode("hex") temp1 = int( temp1_temp[1:],16) if temp1 > 2047: temp1 = (-4096 + temp1) temp1 = (temp1 * TEMP_SCALE_FACTOR) + TEMP_SUM_FACTOR accel1X_temp =(data[19:20] + data[18:19]).encode("hex") accel1X = int( accel1X_temp[1:],16) if accel1X > 2047: accel1X = (-4096 + accel1X) accel1X = accel1X * ACCEL_SCALE_FACTOR accel1Y_temp =(data[21:22] + data[20:21]).encode("hex") accel1Y = int( accel1Y_temp[1:],16) if accel1Y > 2047: accel1Y = (-4096 + accel1Y) accel1Y = accel1Y * ACCEL_SCALE_FACTOR gyro2_temp =(data[23:24] + data[22:23]).encode("hex") gyro2 = int( gyro2_temp[1:],16) if gyro2 > 2047: gyro2 = (-4096 + gyro2) gyro2 = gyro2 * GYRO_SCALE_FACTOR temp2_temp =(data[25:26] + data[24:25]).encode("hex") temp2 = int( temp2_temp[1:],16) if temp2 > 2047: temp2 = (-4096 + temp2) temp2 = (temp2 * TEMP_SCALE_FACTOR) + TEMP_SUM_FACTOR print "temp2 ",temp2 accel2X_temp =(data[27:28] + data[26:27]).encode("hex") accel2X = int( accel2X_temp[1:],16) if accel2X > 2047: accel2X = (-4096 + accel2X) accel2X = accel2X * ACCEL_SCALE_FACTOR accel2Y_temp =(data[29:30] + data[28:29]).encode("hex") accel2Y = int( accel2Y_temp[1:],16) if accel2Y > 2047: accel2Y = (-4096 + accel2Y) accel2Y = accel2Y * ACCEL_SCALE_FACTOR gyro3_temp =(data[31:32] + data[30:31]).encode("hex") gyro3 = int( gyro3_temp[1:],16) if gyro3 > 2047: gyro3 = (-4096 + gyro3) gyro3 = gyro3 * GYRO_SCALE_FACTOR temp3_temp =(data[33:34] + data[32:33]).encode("hex") temp3 = int( temp3_temp[1:],16) if temp3 > 2047: temp3 = (-4096 + temp3) temp3 = (temp3 * TEMP_SCALE_FACTOR) + TEMP_SUM_FACTOR accel3X_temp =(data[35:36] + data[34:35]).encode("hex") accel3X = int( accel3X_temp[1:],16) if accel3X > 2047: accel3X = (-4096 + accel3X) accel3X = accel3X * ACCEL_SCALE_FACTOR accel3Y_temp =(data[37:38] + data[36:37]).encode("hex") accel3Y = int( accel3Y_temp[1:],16) if accel3Y > 2047: accel3Y = (-4096 + accel3Y) accel3Y = accel3Y * ACCEL_SCALE_FACTOR h = std_msgs.msg.Header() h.stamp = rospy.Time.now() h.frame_id = "velodyne" Sen = nmea_msgs.msg.Sentence() Sen.header = h Sen.sentence = val # "$GPRMC,192326,A,4324.2427,N,08028.2217,W,000.0,000.0,080914,009.7,W,D*1F" imu_msg = Imu(); imu_msg.header = h imu_msg.linear_acceleration.x = ((accel1X + accel2X) / 2) * CONVERT_MSEC2 imu_msg.linear_acceleration.y = ((accel2Y + accel3Y) / 2) * CONVERT_MSEC2 imu_msg.linear_acceleration.z = ((accel3X + (-accel1Y)) / 2) * CONVERT_MSEC2 imu_msg.angular_velocity.x = -gyro3 * CONVERT_RADSEC imu_msg.angular_velocity.y = gyro1 * CONVERT_RADSEC imu_msg.angular_velocity.z = gyro2 * CONVERT_RADSEC imu.publish(imu_msg) nmea.publish(Sen) r.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] == "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.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] # imu_msg.angular_velocity_covariance = (1., )*9 imu_msg.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e6] # imu_msg.linear_acceleration_covariance = (1., )*9 imu_msg.linear_acceleration_covariance = [-1, 0, 0, 0, 0, 0, 0, 0, 0] imu_msg.orientation.x = self._qx # imu_msg.orientation.x = 0 imu_msg.orientation.y = self._qy # imu_msg.orientation.y = 0 imu_msg.orientation.z = self._qz # imu_msg.orientation.z = 0 imu_msg.orientation.w = self._qw # imu_msg.orientation.w = 1 imu_msg.angular_velocity.x = self._ax # imu_msg.angular_velocity.x = 0 imu_msg.angular_velocity.y = self._ay # imu_msg.angular_velocity.y = 0 imu_msg.angular_velocity.z = self._az # imu_msg.angular_velocity.z = 0 # imu_msg.linear_acceleration.x = self._lx imu_msg.linear_acceleration.x = 0 # imu_msg.linear_acceleration.y = self._ly imu_msg.linear_acceleration.y = 0 # imu_msg.linear_acceleration.z = self._lz imu_msg.linear_acceleration.z = 0 self.imu_pub.publish(imu_msg) except: rospy.logwarn("Error in Sensor values") rospy.logwarn(lineParts) pass
def run(self): while not rospy.is_shutdown(): self.lock.acquire() self.zumy.cmd(*self.cmd) try: imu_data = self.zumy.read_imu() imu_msg = Imu() imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name) imu_msg.linear_acceleration.x = 9.81 * imu_data[0] imu_msg.linear_acceleration.y = 9.81 * imu_data[1] imu_msg.linear_acceleration.z = 9.81 * imu_data[2] imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3] imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4] imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5] self.imu_pub.publish(imu_msg) except ValueError: pass try: enc_data = self.zumy.enc_pos() enc_msg = Float32() enc_msg.data = enc_data[0] self.r_enc_pub.publish(enc_msg) enc_msg.data = enc_data[1] self.l_enc_pub.publish(enc_msg) except ValueError: pass try: vel_data = self.zumy.enc_vel() vel_msg = Float32() vel_msg.data = vel_data[0] self.l_vel_pub.publish(vel_msg) vel_msg.data = vel_data[1] self.r_vel_pub.publish(vel_msg) except ValueError: pass try: v_bat = self.zumy.read_voltage() self.batt_pub.publish(v_bat) except ValueError: pass if time.time() > (self.last_message_at + self.timeout): #i've gone too long without seeing the watchdog. self.watchdog = False self.zumy.disable() self.zumy.battery_protection() # a function that needs to be called with some regularity. #handle the robot status message status_msg = ZumyStatus() status_msg.enabled_status = self.zumy.enabled status_msg.battery_unsafe = self.zumy.battery_unsafe() #update the looptimes, which will get published in status_msg self.laptimes = np.delete(self.laptimes,0) self.laptimes = np.append(self.laptimes,time.time()) #take average over the difference of the times... and then invert. That will give you average freq. status_msg.loop_freq = 1/float(np.mean(np.diff(self.laptimes))) self.status_pub.publish(status_msg) self.lock.release() #must be last command involving the zumy. self.rate.sleep() # If shutdown, turn off motors & disable anything else. self.zumy.disable()
def spin_once(self): '''Read data from device and publishes ROS messages.''' # common header h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id # create messages and default values imu_msg = Imu() imu_msg.orientation_covariance = (-1., )*9 imu_msg.angular_velocity_covariance = (-1., )*9 imu_msg.linear_acceleration_covariance = (-1., )*9 pub_imu = False gps_msg = NavSatFix() xgps_msg = GPSFix() pub_gps = False vel_msg = TwistStamped() pub_vel = False mag_msg = Vector3Stamped() pub_mag = False temp_msg = Float32() pub_temp = False press_msg = Float32() pub_press = False anin1_msg = UInt16() pub_anin1 = False anin2_msg = UInt16() pub_anin2 = False pub_diag = False def fill_from_raw(raw_data): '''Fill messages with information from 'raw' MTData block.''' # don't publish raw imu data anymore # TODO find what to do with that pass def fill_from_rawgps(rawgps_data): '''Fill messages with information from 'rawgps' MTData block.''' global pub_hps, xgps_msg, gps_msg if rawgps_data['bGPS']<self.old_bGPS: pub_gps = True # LLA xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7 xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7 xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3 # NED vel # TODO? # Accuracy # 2 is there to go from std_dev to 95% interval xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3 xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3 self.old_bGPS = rawgps_data['bGPS'] def fill_from_Temp(temp): '''Fill messages with information from 'temperature' MTData block.''' global pub_temp, temp_msg pub_temp = True temp_msg.data = temp def fill_from_Calib(imu_data): '''Fill messages with information from 'calibrated' MTData block.''' global pub_imu, imu_msg, pub_vel, vel_msg, pub_mag, mag_msg try: pub_imu = True imu_msg.angular_velocity.x = imu_data['gyrX'] imu_msg.angular_velocity.y = imu_data['gyrY'] imu_msg.angular_velocity.z = imu_data['gyrZ'] imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0., radians(0.025), 0., 0., 0., radians(0.025)) pub_vel = True vel_msg.twist.angular.x = imu_data['gyrX'] vel_msg.twist.angular.y = imu_data['gyrY'] vel_msg.twist.angular.z = imu_data['gyrZ'] except KeyError: pass try: pub_imu = True imu_msg.linear_acceleration.x = imu_data['accX'] imu_msg.linear_acceleration.y = imu_data['accY'] imu_msg.linear_acceleration.z = imu_data['accZ'] imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0., 0.0004, 0., 0., 0., 0.0004) except KeyError: pass try: pub_mag = True mag_msg.vector.x = imu_data['magX'] mag_msg.vector.y = imu_data['magY'] mag_msg.vector.z = imu_data['magZ'] except KeyError: pass def fill_from_Vel(velocity_data): '''Fill messages with information from 'velocity' MTData block.''' global pub_vel, vel_msg pub_vel = True vel_msg.twist.linear.x = velocity_data['Vel_X'] vel_msg.twist.linear.y = velocity_data['Vel_Y'] vel_msg.twist.linear.z = velocity_data['Vel_Z'] def fill_from_Orient(orient_data): '''Fill messages with information from 'orientation' MTData block.''' global pub_imu, imu_msg pub_imu = True if orient.has_key('quaternion'): w, x, y, z = orient['quaternion'] elif orient.has_key('roll'): # FIXME check that Euler angles are in radians x, y, z, w = quaternion_from_euler(radians(orient['roll']), radians(orient['pitch']), radians(orient['yaw'])) elif orient.has_key('matrix'): m = identity_matrix() m[:3,:3] = orient['matrix'] x, y, z, w = quaternion_from_matrix(m) imu_msg.orientation.x = x imu_msg.orientation.y = y imu_msg.orientation.z = z imu_msg.orientation.w = w imu_msg.orientation_covariance = (radians(1.), 0., 0., 0., radians(1.), 0., 0., 0., radians(9.)) def fill_from_Pos(position_data): '''Fill messages with information from 'position' MTData block.''' global pub_gps, xgps_msg, gps_msg pub_gps = True xgps_msg.latitude = gps_msg.latitude = position_data['Lat'] xgps_msg.longitude = gps_msg.longitude = position_data['Lon'] xgps_msg.altitude = gps_msg.altitude = position_data['Alt'] def fill_from_Stat(status): '''Fill messages with information from 'status' MTData block.''' global pub_diag, pub_gps, gps_msg, xgps_msg pub_diag = True if status & 0b0001: self.stest_stat.level = DiagnosticStatus.OK self.stest_stat.message = "Ok" else: self.stest_stat.level = DiagnosticStatus.ERROR self.stest_stat.message = "Failed" if status & 0b0010: self.xkf_stat.level = DiagnosticStatus.OK self.xkf_stat.message = "Valid" else: self.xkf_stat.level = DiagnosticStatus.WARN self.xkf_stat.message = "Invalid" if status & 0b0100: self.gps_stat.level = DiagnosticStatus.OK self.gps_stat.message = "Ok" gps_msg.status.status = NavSatStatus.STATUS_FIX xgps_msg.status.status = GPSStatus.STATUS_FIX gps_msg.status.service = NavSatStatus.SERVICE_GPS xgps_msg.status.position_source = 0b01101001 xgps_msg.status.motion_source = 0b01101010 xgps_msg.status.orientation_source = 0b01101010 else: self.gps_stat.level = DiagnosticStatus.WARN self.gps_stat.message = "No fix" gps_msg.status.status = NavSatStatus.STATUS_NO_FIX xgps_msg.status.status = GPSStatus.STATUS_NO_FIX gps_msg.status.service = 0 xgps_msg.status.position_source = 0b01101000 xgps_msg.status.motion_source = 0b01101000 xgps_msg.status.orientation_source = 0b01101000 def fill_from_Auxiliary(aux_data): '''Fill messages with information from 'Auxiliary' MTData block.''' global pub_anin1, pub_anin2, anin1_msg, anin2_msg try: anin1_msg.data = o['Ain_1'] pub_anin1 = True except KeyError: pass try: anin2_msg.data = o['Ain_2'] pub_anin2 = True except KeyError: pass def fill_from_Temperature(o): '''Fill messages with information from 'Temperature' MTData2 block.''' global pub_temp, temp_msg pub_temp = True temp_msg.data = o['Temp'] def fill_from_Timestamp(o): '''Fill messages with information from 'Timestamp' MTData2 block.''' global h try: # put timestamp from gps UTC time if available y, m, d, hr, mi, s, ns, f = o['Year'], o['Month'], o['Day'],\ o['Hour'], o['Minute'], o['Second'], o['ns'], o['Flags'] if f&0x4: secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0)) h.stamp.secs = secs h.stamp.nsecs = ns except KeyError: pass # TODO find what to do with other kind of information pass def fill_from_Orientation_Data(o): '''Fill messages with information from 'Orientation Data' MTData2 block.''' global pub_imu, imu_msg pub_imu = True try: x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0'] except KeyError: pass try: # FIXME check that Euler angles are in radians x, y, z, w = quaternion_from_euler(radians(o['Roll']), radians(o['Pitch']), radians(o['Yaw'])) except KeyError: pass try: a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\ o['e'], o['f'], o['g'], o['h'], o['i'] m = identity_matrix() m[:3,:3] = ((a, b, c), (d, e, f), (g, h, i)) x, y, z, w = quaternion_from_matrix(m) except KeyError: pass imu_msg.orientation.x = x imu_msg.orientation.y = y imu_msg.orientation.z = z imu_msg.orientation.w = w imu_msg.orientation_covariance = (radians(1.), 0., 0., 0., radians(1.), 0., 0., 0., radians(9.)) def fill_from_Pressure(o): '''Fill messages with information from 'Pressure' MTData2 block.''' global pub_press, press_msg press_msg.data = o['Pressure'] def fill_from_Acceleration(o): '''Fill messages with information from 'Acceleration' MTData2 block.''' global pub_imu, imu_msg pub_imu = True # FIXME not sure we should treat all in that same way try: x, y, z = o['Delta v.x'], o['Delta v.y'], o['Delta v.z'] except KeyError: pass try: x, y, z = o['freeAccX'], o['freeAccY'], o['freeAccZ'] except KeyError: pass try: x, y, z = o['accX'], o['accY'], o['accZ'] except KeyError: pass imu_msg.linear_acceleration.x = x imu_msg.linear_acceleration.y = y imu_msg.linear_acceleration.z = z imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0., 0.0004, 0., 0., 0., 0.0004) def fill_from_Position(o): '''Fill messages with information from 'Position' MTData2 block.''' global pub_gps, xgps_msg, gps_msg # TODO publish ECEF try: xgps_msg.latitude = gps_msg.latitude = o['lat'] xgps_msg.longitude = gps_msg.longitude = o['lon'] pub_gps = True alt = o.get('altMsl', o.get('altEllipsoid', 0)) xgps_msg.altitude = gps_msg.altitude = alt except KeyError: pass def fill_from_Angular_Velocity(o): '''Fill messages with information from 'Angular Velocity' MTData2 block.''' global pub_imu, imu_msg, pub_vel, vel_msg try: imu_msg.angular_velocity.x = o['gyrX'] imu_msg.angular_velocity.y = o['gyrY'] imu_msg.angular_velocity.z = o['gyrZ'] imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0., radians(0.025), 0., 0., 0., radians(0.025)) pub_imu = True vel_msg.twist.angular.x = o['gyrX'] vel_msg.twist.angular.y = o['gyrY'] vel_msg.twist.angular.z = o['gyrZ'] pub_vel = True except KeyError: pass # TODO decide what to do with 'Delta q' def fill_from_GPS(o): '''Fill messages with information from 'GPS' MTData2 block.''' global pub_gps, h, xgps_msg try: # DOP xgps_msg.gdop = o['gDOP'] xgps_msg.pdop = o['pDOP'] xgps_msg.hdop = o['hDOP'] xgps_msg.vdop = o['vDOP'] xgps_msg.tdop = o['tDOP'] pub_gps = True except KeyError: pass try: # Time UTC y, m, d, hr, mi, s, ns, f = o['year'], o['month'], o['day'],\ o['hour'], o['min'], o['sec'], o['nano'], o['valid'] if f&0x4: secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0)) h.stamp.secs = secs h.stamp.nsecs = ns except KeyError: pass # TODO publish SV Info def fill_from_SCR(o): '''Fill messages with information from 'SCR' MTData2 block.''' # TODO that's raw information pass def fill_from_Analog_In(o): '''Fill messages with information from 'Analog In' MTData2 block.''' global pub_anin1, pub_anin2, anin1_msg, anin2_msg try: anin1_msg.data = o['analogIn1'] pub_anin1 = True except KeyError: pass try: anin2_msg.data = o['analogIn2'] pub_anin2 = True except KeyError: pass def fill_from_Magnetic(o): '''Fill messages with information from 'Magnetic' MTData2 block.''' global pub_mag, mag_msg mag_msg.vector.x = o['magX'] mag_msg.vector.y = o['magY'] mag_msg.vector.z = o['magZ'] pub_mag = True def fill_from_Velocity(o): '''Fill messages with information from 'Velocity' MTData2 block.''' global pub_vel, vel_msg vel_msg.twist.linear.x = o['velX'] vel_msg.twist.linear.y = o['velY'] vel_msg.twist.linear.z = o['velZ'] pub_vel = True def fill_from_Status(o): '''Fill messages with information from 'Status' MTData2 block.''' try: status = o['StatusByte'] fill_from_Stat(status) except KeyError: pass try: status = o['StatusWord'] fill_from_Stat(status) except KeyError: pass # TODO RSSI def find_handler_name(name): return "fill_from_%s"%(name.replace(" ", "_")) # get data data = self.mt.read_measurement() # fill messages based on available data fields for n, o in data: try: locals()[find_handler_name(n)](o) except KeyError: rospy.logwarn("Unknown MTi data packet: '%s', ignoring."%n) # publish available information if pub_imu: imu_msg.header = h self.imu_pub.publish(imu_msg) if pub_gps: xgps_msg.header = gps_msg.header = h self.gps_pub.publish(gps_msg) self.xgps_pub.publish(xgps_msg) if pub_vel: vel_msg.header = h self.vel_pub.publish(vel_msg) if pub_mag: mag_msg.header = h self.mag_pub.publish(mag_msg) if pub_temp: self.temp_pub.publish(temp_msg) if pub_press: self.press_pub.publish(press_msg) if pub_anin1: self.analog_in1_pub.publish(anin1_msg) if pub_anin2: self.analog_in2_pub.publish(anin2_msg) if pub_diag: self.diag_msg.header = h self.diag_pub.publish(self.diag_msg) # publish string representation self.str_pub.publish(str(data))
def spin_once(self): def baroPressureToHeight(value): c1 = 44330.0 c2 = 9.869232667160128300024673081668e-6 c3 = 0.1901975534856 intermediate = 1-math.pow(c2*value, c3) height = c1*intermediate return height # get data data = self.mt.read_measurement() # common header h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id # get data (None if not present) #temp = data.get('Temp') # float orient_data = data.get('Orientation Data') velocity_data = data.get('Velocity') position_data = data.get('Latlon') altitude_data = data.get('Altitude') acc_data = data.get('Acceleration') gyr_data = data.get('Angular Velocity') mag_data = data.get('Magnetic') pressure_data = data.get('Pressure') time_data = data.get('Timestamp') gnss_data = data.get('Gnss PVT') status = data.get('Status') # int # create messages and default values "Imu message supported with Modes 1 & 2" imu_msg = Imu() pub_imu = False "SensorSample message supported with Mode 2" ss_msg = sensorSample() pub_ss = False "Mag message supported with Modes 1 & 2" mag_msg = Vector3Stamped() pub_mag = False "Baro in meters" baro_msg = baroSample() pub_baro = False "GNSS message supported only with MTi-G-7xx devices" "Valid only for modes 1 and 2" gnssPvt_msg = gnssSample() pub_gnssPvt = False #gnssSatinfo_msg = GPSStatus() #pub_gnssSatinfo = False # All filter related outputs "Supported in mode 3" ori_msg = orientationEstimate() pub_ori = False "Supported in mode 3 for MTi-G-7xx devices" vel_msg = velocityEstimate() pub_vel = False "Supported in mode 3 for MTi-G-7xx devices" pos_msg = positionEstimate() pub_pos = False secs = 0 nsecs = 0 if time_data: # first getting the sampleTimeFine time = time_data['SampleTimeFine'] secs = 100e-6*time nsecs = 1e5*time - 1e9*math.floor(secs) if acc_data: if 'Delta v.x' in acc_data: # found delta-v's pub_ss = True ss_msg.internal.imu.dv.x = acc_data['Delta v.x'] ss_msg.internal.imu.dv.y = acc_data['Delta v.y'] ss_msg.internal.imu.dv.z = acc_data['Delta v.z'] elif 'accX' in acc_data: # found acceleration pub_imu = True imu_msg.linear_acceleration.x = acc_data['accX'] imu_msg.linear_acceleration.y = acc_data['accY'] imu_msg.linear_acceleration.z = acc_data['accZ'] else: raise MTException("Unsupported message in XDI_AccelerationGroup.") if gyr_data: if 'Delta q0' in gyr_data: # found delta-q's pub_ss = True ss_msg.internal.imu.dq.w = gyr_data['Delta q0'] ss_msg.internal.imu.dq.x = gyr_data['Delta q1'] ss_msg.internal.imu.dq.y = gyr_data['Delta q2'] ss_msg.internal.imu.dq.z = gyr_data['Delta q3'] elif 'gyrX' in gyr_data: # found rate of turn pub_imu = True imu_msg.angular_velocity.x = gyr_data['gyrX'] imu_msg.angular_velocity.y = gyr_data['gyrY'] imu_msg.angular_velocity.z = gyr_data['gyrZ'] else: raise MTException("Unsupported message in XDI_AngularVelocityGroup.") if mag_data: # magfield ss_msg.internal.mag.x = mag_msg.vector.x = mag_data['magX'] ss_msg.internal.mag.y = mag_msg.vector.y = mag_data['magY'] ss_msg.internal.mag.z = mag_msg.vector.z = mag_data['magZ'] pub_mag = True if pressure_data: pub_baro = True height = baroPressureToHeight(pressure_data['Pressure']) baro_msg.height = ss_msg.internal.baro.height = height if gnss_data: pub_gnssPvt = True gnssPvt_msg.itow = gnss_data['iTOW'] gnssPvt_msg.fix = gnss_data['fix'] gnssPvt_msg.latitude = gnss_data['lat'] gnssPvt_msg.longitude = gnss_data['lon'] gnssPvt_msg.hEll = gnss_data['hEll'] gnssPvt_msg.hMsl = gnss_data['hMsl'] gnssPvt_msg.vel.x = gnss_data['velE'] gnssPvt_msg.vel.y = gnss_data['velN'] gnssPvt_msg.vel.z = -gnss_data['velD'] gnssPvt_msg.hAcc = gnss_data['horzAcc'] gnssPvt_msg.vAcc = gnss_data['vertAcc'] gnssPvt_msg.sAcc = gnss_data['speedAcc'] gnssPvt_msg.pDop = gnss_data['PDOP'] gnssPvt_msg.hDop = gnss_data['HDOP'] gnssPvt_msg.vDop = gnss_data['VDOP'] gnssPvt_msg.numSat = gnss_data['nSat'] gnssPvt_msg.heading = gnss_data['heading'] gnssPvt_msg.headingAcc = gnss_data['headingAcc'] if orient_data: if 'Q0' in orient_data: pub_imu = True imu_msg.orientation.x = orient_data['Q0'] imu_msg.orientation.y = orient_data['Q1'] imu_msg.orientation.z = orient_data['Q2'] imu_msg.orientation.w = orient_data['Q3'] elif 'Roll' in orient_data: pub_ori = True ori_msg.roll = orient_data['Roll'] ori_msg.pitch = orient_data['Pitch'] ori_msg.yaw = orient_data['Yaw'] else: raise MTException('Unsupported message in XDI_OrientationGroup') if velocity_data: pub_vel = True vel_msg.velE = velocity_data['velX'] vel_msg.velN = velocity_data['velY'] vel_msg.velU = velocity_data['velZ'] if position_data: pub_pos = True pos_msg.latitude = position_data['lat'] pos_msg.longitude = position_data['lon'] if altitude_data: pub_pos = True tempData = altitude_data['ellipsoid'] pos_msg.hEll = tempData[0] #if status is not None: # if status & 0b0001: # self.stest_stat.level = DiagnosticStatus.OK # self.stest_stat.message = "Ok" # else: # self.stest_stat.level = DiagnosticStatus.ERROR # self.stest_stat.message = "Failed" # if status & 0b0010: # self.xkf_stat.level = DiagnosticStatus.OK # self.xkf_stat.message = "Valid" # else: # self.xkf_stat.level = DiagnosticStatus.WARN # self.xkf_stat.message = "Invalid" # if status & 0b0100: # self.gps_stat.level = DiagnosticStatus.OK # self.gps_stat.message = "Ok" # else: # self.gps_stat.level = DiagnosticStatus.WARN # self.gps_stat.message = "No fix" # self.diag_msg.header = h # self.diag_pub.publish(self.diag_msg) # publish available information if pub_imu: imu_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time imu_msg.header.stamp.secs = secs imu_msg.header.stamp.nsecs = nsecs self.imu_pub.publish(imu_msg) #if pub_gps: # xgps_msg.header = gps_msg.header = h # self.gps_pub.publish(gps_msg) # self.xgps_pub.publish(xgps_msg) if pub_mag: mag_msg.header = h self.mag_pub.publish(mag_msg) #if pub_temp: # self.temp_pub.publish(temp_msg) if pub_ss: ss_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time ss_msg.header.stamp.secs = secs ss_msg.header.stamp.nsecs = nsecs self.ss_pub.publish(ss_msg) if pub_baro: baro_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time baro_msg.header.stamp.secs = secs baro_msg.header.stamp.nsecs = nsecs self.baro_pub.publish(baro_msg) if pub_gnssPvt: gnssPvt_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time baro_msg.header.stamp.secs = secs baro_msg.header.stamp.nsecs = nsecs self.gnssPvt_pub.publish(gnssPvt_msg) if pub_ori: ori_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time ori_msg.header.stamp.secs = secs ori_msg.header.stamp.nsecs = nsecs self.ori_pub.publish(ori_msg) if pub_vel: vel_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time vel_msg.header.stamp.secs = secs vel_msg.header.stamp.nsecs = nsecs self.vel_pub.publish(vel_msg) if pub_pos: pos_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time pos_msg.header.stamp.secs = secs pos_msg.header.stamp.nsecs = nsecs self.pos_pub.publish(pos_msg)
time = shifted_header.stamp.nsecs + shifted_header.stamp.secs*1e9 # time += (0.0150117606625+0.000657665377756+0.00119533281712-0.000487851613732)*1e9 shifted_header.stamp.nsecs = time % 1e9 shifted_header.stamp.secs = int(time/1e9) left_image.header = shifted_header right_image = msg.right_image right_image.header = msg.header out_bag.write('/duo3d_camera/left/image_raw', msg.left_image, t) out_bag.write('/duo3d_camera/right/image_raw', msg.right_image, t) imu_msg = Imu() imu_msg.angular_velocity.x = msg.imu.angular_velocity.x imu_msg.angular_velocity.y = -msg.imu.angular_velocity.y imu_msg.angular_velocity.z = msg.imu.angular_velocity.z imu_msg.linear_acceleration.x = msg.imu.linear_acceleration.x * 9.81 imu_msg.linear_acceleration.y = -msg.imu.linear_acceleration.y * 9.81 imu_msg.linear_acceleration.z = -msg.imu.linear_acceleration.z * 9.81 imu_msg.header = msg.header out_bag.write('/duo3d_camera/cam_imu', imu_msg, t) imu_cnt += 1 print('\nDone. Output bag:') print(out_bag) in_bag.close() out_bag.close()
def spin_once(self): def quat_from_orient(orient): '''Build a quaternion from orientation data.''' try: w, x, y, z = orient['quaternion'] return (x, y, z, w) except KeyError: pass try: return quaternion_from_euler(pi*orient['roll']/180., pi*orient['pitch']/180, pi*orient['yaw']/180.) except KeyError: pass try: m = identity_matrix() m[:3,:3] = orient['matrix'] return quaternion_from_matrix(m) except KeyError: pass # get data data = self.mt.read_measurement() # common header h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id # get data (None if not present) temp = data.get('Temp') # float raw_data = data.get('RAW') imu_data = data.get('Calib') orient_data = data.get('Orient') velocity_data = data.get('Velocity') position_data = data.get('Position') rawgps_data = data.get('RAWGPS') status = data.get('Status') # int sample = data.get('Sample') # TODO by @demmeln: use sample counter for timestamp # correction. Watch out for counter wrap. # create messages and default values imu_msg = Imu() imu_msg.orientation_covariance = (-1., )*9 imu_msg.angular_velocity_covariance = (-1., )*9 imu_msg.linear_acceleration_covariance = (-1., )*9 pub_imu = False gps_msg = NavSatFix() xgps_msg = GPSFix() pub_gps = False vel_msg = TwistStamped() pub_vel = False mag_msg = Vector3Stamped() pub_mag = False temp_msg = Float32() pub_temp = False # fill information where it's due # start by raw information that can be overriden if raw_data: # TODO warn about data not calibrated pub_imu = True pub_vel = True pub_mag = True pub_temp = True # acceleration imu_msg.linear_acceleration.x = raw_data['accX'] imu_msg.linear_acceleration.y = raw_data['accY'] imu_msg.linear_acceleration.z = raw_data['accZ'] imu_msg.linear_acceleration_covariance = (0., )*9 # gyroscopes imu_msg.angular_velocity.x = raw_data['gyrX'] imu_msg.angular_velocity.y = raw_data['gyrY'] imu_msg.angular_velocity.z = raw_data['gyrZ'] imu_msg.angular_velocity_covariance = (0., )*9 vel_msg.twist.angular.x = raw_data['gyrX'] vel_msg.twist.angular.y = raw_data['gyrY'] vel_msg.twist.angular.z = raw_data['gyrZ'] # magnetometer mag_msg.vector.x = raw_data['magX'] mag_msg.vector.y = raw_data['magY'] mag_msg.vector.z = raw_data['magZ'] # temperature # 2-complement decoding and 1/256 resolution x = raw_data['temp'] if x&0x8000: temp_msg.data = (x - 1<<16)/256. else: temp_msg.data = x/256. if rawgps_data: if rawgps_data['bGPS']<self.old_bGPS: pub_gps = True # LLA xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7 xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7 xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3 # NED vel # TODO? # Accuracy # 2 is there to go from std_dev to 95% interval xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3 xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3 self.old_bGPS = rawgps_data['bGPS'] if temp is not None: pub_temp = True temp_msg.data = temp if imu_data: try: imu_msg.angular_velocity.x = imu_data['gyrX'] imu_msg.angular_velocity.y = imu_data['gyrY'] imu_msg.angular_velocity.z = imu_data['gyrZ'] imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0., radians(0.025), 0., 0., 0., radians(0.025)) pub_imu = True vel_msg.twist.angular.x = imu_data['gyrX'] vel_msg.twist.angular.y = imu_data['gyrY'] vel_msg.twist.angular.z = imu_data['gyrZ'] pub_vel = True except KeyError: pass try: imu_msg.linear_acceleration.x = imu_data['accX'] imu_msg.linear_acceleration.y = imu_data['accY'] imu_msg.linear_acceleration.z = imu_data['accZ'] imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0., 0.0004, 0., 0., 0., 0.0004) pub_imu = True except KeyError: pass try: mag_msg.vector.x = imu_data['magX'] mag_msg.vector.y = imu_data['magY'] mag_msg.vector.z = imu_data['magZ'] pub_mag = True except KeyError: pass if velocity_data: pub_vel = True vel_msg.twist.linear.x = velocity_data['Vel_X'] vel_msg.twist.linear.y = velocity_data['Vel_Y'] vel_msg.twist.linear.z = velocity_data['Vel_Z'] if orient_data: pub_imu = True orient_quat = quat_from_orient(orient_data) imu_msg.orientation.x = orient_quat[0] imu_msg.orientation.y = orient_quat[1] imu_msg.orientation.z = orient_quat[2] imu_msg.orientation.w = orient_quat[3] imu_msg.orientation_covariance = (radians(1.), 0., 0., 0., radians(1.), 0., 0., 0., radians(9.)) if position_data: pub_gps = True xgps_msg.latitude = gps_msg.latitude = position_data['Lat'] xgps_msg.longitude = gps_msg.longitude = position_data['Lon'] xgps_msg.altitude = gps_msg.altitude = position_data['Alt'] if status is not None: if status & 0b0001: self.stest_stat.level = DiagnosticStatus.OK self.stest_stat.message = "Ok" else: self.stest_stat.level = DiagnosticStatus.ERROR self.stest_stat.message = "Failed" if status & 0b0010: self.xkf_stat.level = DiagnosticStatus.OK self.xkf_stat.message = "Valid" else: self.xkf_stat.level = DiagnosticStatus.WARN self.xkf_stat.message = "Invalid" if status & 0b0100: self.gps_stat.level = DiagnosticStatus.OK self.gps_stat.message = "Ok" else: self.gps_stat.level = DiagnosticStatus.WARN self.gps_stat.message = "No fix" self.diag_msg.header = h self.diag_pub.publish(self.diag_msg) if pub_gps: if status & 0b0100: gps_msg.status.status = NavSatStatus.STATUS_FIX xgps_msg.status.status = GPSStatus.STATUS_FIX gps_msg.status.service = NavSatStatus.SERVICE_GPS xgps_msg.status.position_source = 0b01101001 xgps_msg.status.motion_source = 0b01101010 xgps_msg.status.orientation_source = 0b01101010 else: gps_msg.status.status = NavSatStatus.STATUS_NO_FIX xgps_msg.status.status = GPSStatus.STATUS_NO_FIX gps_msg.status.service = 0 xgps_msg.status.position_source = 0b01101000 xgps_msg.status.motion_source = 0b01101000 xgps_msg.status.orientation_source = 0b01101000 # publish available information if pub_imu: imu_msg.header = h self.imu_pub.publish(imu_msg) if pub_gps: xgps_msg.header = gps_msg.header = h self.gps_pub.publish(gps_msg) self.xgps_pub.publish(xgps_msg) if pub_vel: vel_msg.header = h self.vel_pub.publish(vel_msg) if pub_mag: mag_msg.header = h self.mag_pub.publish(mag_msg) if pub_temp: self.temp_pub.publish(temp_msg)