Python msg.Imu类代码示例

    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 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 (fuck 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 publish_imu(self):
     imu_msg = Imu()
     imu_msg.header.stamp = self.time
     imu_msg.header.frame_id = 'imu_odom'
     #quat = tf_math.quaternion_from_euler(self.kalman_state[0,0],self.kalman_state[1,0],self.kalman_state[2,0], axes='sxyz')
     a = self.kalman_state[3,0]
     b = self.kalman_state[4,0]
     c = self.kalman_state[5,0]
     d = self.kalman_state[6,0]
     q = math.sqrt(math.pow(a,2)+math.pow(b,2)+math.pow(c,2)+math.pow(d,2))
     #angles = tf_math.euler_from_quaternion(self.kalman_state[3:7,0])
     imu_msg.orientation.x = a/q#angles[0]
     imu_msg.orientation.y = b/q
     imu_msg.orientation.z = c/q
     imu_msg.orientation.w = d/q
     imu_msg.orientation_covariance = list(self.kalman_covariance[3:6,3:6].flatten())
     imu_msg.angular_velocity.x = self.kalman_state[0,0]
     imu_msg.angular_velocity.y = self.kalman_state[1,0]
     imu_msg.angular_velocity.z = self.kalman_state[2,0]
     imu_msg.angular_velocity_covariance = list(self.kalman_covariance[0:3,0:3].flatten())
     imu_msg.linear_acceleration.x = self.kalman_state[10,0]
     imu_msg.linear_acceleration.y = self.kalman_state[11,0]
     imu_msg.linear_acceleration.z = self.kalman_state[12,0]
     imu_msg.linear_acceleration_covariance = list(self.kalman_covariance[10:13,10:13].flatten())
     self.pub.publish(imu_msg)

    def publish(self, event):
        compass_accel = self.compass_accel.read()
        compass = compass_accel[0:3]
        accel = compass_accel[3:6]
        gyro = self.gyro.read()
        
        # Put together an IMU message
	imu_msg = Imu()
	imu_msg.header.stamp = rospy.Time.now()
        imu_msg.orientation_covariance[0] = -1
        imu_msg.angular_velocity = gyro[0] * DEG_TO_RAD
        imu_msg.angular_velocity = gyro[1] * DEG_TO_RAD
        imu_msg.angular_velocity = gyro[2] * DEG_TO_RAD
	imu_msg.linear_acceleration.x = accel[0] * G
	imu_msg.linear_acceleration.y = accel[1] * G
	imu_msg.linear_acceleration.z = accel[2] * G
    
	self.pub_imu.publish(imu_msg)

        # Put together a magnetometer message
	mag_msg = MagneticField()
	mag_msg.header.stamp = rospy.Time.now()
        mag_msg.magnetic_field.x = compass[0]
        mag_msg.magnetic_field.y = compass[1]
        mag_msg.magnetic_field.z = compass[2]
    
	self.pub_mag.publish(mag_msg)

def rospyrtimulib():
    imuData = Imu()
    print("Using settings file " + SETTINGS_FILE + ".ini")
    if not os.path.exists(SETTINGS_FILE + ".ini"):
        print("Settings file does not exist, will be created")

    s = RTIMU.Settings(SETTINGS_FILE)
    imu = RTIMU.RTIMU(s)

    print("IMU Name: " + imu.IMUName())

    if (not imu.IMUInit()):
        print("IMU Init Failed")
        sys.exit(1)
    else:
        print("IMU Init Succeeded")

    # this is a good time to set any fusion parameters

    imu.setSlerpPower(0.02)
    imu.setGyroEnable(True)
    imu.setAccelEnable(True)
    imu.setCompassEnable(True)

    pub = rospy.Publisher('imu', Imu, queue_size=10)
    rospy.init_node('rospyrtimulib', anonymous=True)
    rate = rospy.Rate(1000 / imu.IMUGetPollInterval())

    while not rospy.is_shutdown():
        if imu.IMURead():
            # collect the data
            data = imu.getIMUData()

            fusionQPose = data["fusionQPose"]
            imuData.orientation.x = fusionQPose[1]
            imuData.orientation.y = fusionQPose[2]
            imuData.orientation.z = fusionQPose[3]
            imuData.orientation.w = fusionQPose[0]
 
            gyro = data["gyro"]
            imuData.angular_velocity.x = gyro[0]
            imuData.angular_velocity.y = gyro[1]
            imuData.angular_velocity.z = gyro[2]

            accel = data["accel"]
            imuData.linear_acceleration.x = accel[0] * 9.81
            imuData.linear_acceleration.y = accel[1] * 9.81
            imuData.linear_acceleration.z = accel[2] * 9.81
            imuData.header.stamp = rospy.Time.now()

            # no covariance
            imuData.orientation_covariance[0] = -1
            imuData.angular_velocity_covariance[0] = -1
            imuData.linear_acceleration_covariance[0] = -1

            imuData.header.frame_id = 'map'

            # publish it
            pub.publish(imuData)
        rate.sleep()

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 post_imu(self, component_instance):
    """ Publish the data of the IMU sensor as a custom ROS Imu message
    """
    parent = component_instance.robot_parent
    parent_name = parent.blender_obj.name
    current_time = rospy.Time.now()

    imu = Imu()
    imu.header.seq = self._seq
    imu.header.stamp = current_time
    imu.header.frame_id = "/imu"
    
    imu.orientation = self.orientation.to_quaternion()

    imu.angular_velocity.x = component_instance.local_data['angular_velocity'][0]
    imu.angular_velocity.y = component_instance.local_data['angular_velocity'][1]
    imu.angular_velocity.z = component_instance.local_data['angular_velocity'][2]

    imu.linear_acceleration.x = component_instance.local_data['linear_acceleration'][0]
    imu.linear_acceleration.y = component_instance.local_data['linear_acceleration'][1]
    imu.linear_acceleration.z = component_instance.local_data['linear_acceleration'][2]



    for topic in self._topics:
        # publish the message on the correct w
        if str(topic.name) == str("/" + parent_name + "/" + component_instance.blender_obj.name):
            topic.publish(imu)

    self._seq += 1

	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 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 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 publish(self, data):
        q = data.getOrientation()
        roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])

        # print "Before ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"

        array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))

        res = Quaternion()
        res.w = array[0]
        res.x = array[1]
        res.y = array[2]
        res.z = array[3]

        roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])

        # print "after ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"

        msg = Imu()
        msg.header.frame_id = self._frameId
        msg.header.stamp = rospy.get_rostime()
        msg.orientation = res
        msg.linear_acceleration = data.getAcceleration()
        msg.angular_velocity = data.getVelocity()

        magMsg = MagneticField()
        magMsg.header.frame_id = self._frameId
        magMsg.header.stamp = rospy.get_rostime()
        magMsg.magnetic_field = data.getMagnetometer()

        self._pub.publish(msg)
        self._pubMag.publish(magMsg)

    def publish(self, data):
        if not self._calib and data.getImuMsgId() == PUB_ID:
            q = data.getOrientation()
            roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
            array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))

            res = Quaternion()
            res.w = array[0]
            res.x = array[1]
            res.y = array[2]
            res.z = array[3]

            msg = Imu()
            msg.header.frame_id = self._frameId
            msg.header.stamp = rospy.get_rostime()
            msg.orientation = res
            msg.linear_acceleration = data.getAcceleration()
            msg.angular_velocity = data.getVelocity()

            magMsg = MagneticField()
            magMsg.header.frame_id = self._frameId
            magMsg.header.stamp = rospy.get_rostime()
            magMsg.magnetic_field = data.getMagnetometer()

            self._pub.publish(msg)
            self._pubMag.publish(magMsg)

        elif data.getImuMsgId() == CALIB_ID:
            x, y, z = data.getValues()
            msg = imuCalib()

            if x > self._xMax:
                self._xMax = x
            if x < self._xMin:
                self._xMin = x

            if y > self._yMax:
                self._yMax = y
            if y < self._yMin:
                self._yMin = y

            if z > self._zMax:
                self._zMax = z
            if z < self._zMin:
                self._zMin = z


            msg.x.data = x
            msg.x.max = self._xMax
            msg.x.min = self._xMin

            msg.y.data = y
            msg.y.max = self._yMax
            msg.y.min = self._yMin

            msg.z.data = z
            msg.z.max = self._zMax
            msg.z.min = self._zMin

            self._pubCalib.publish(msg)

def fake_imu():
    pub = rospy.Publisher('IMU_ned', Imu, queue_size=10)
    rospy.init_node('test_convertimu', anonymous=True)
    rate = rospy.Rate(0.5) # 10hz
    
    while not rospy.is_shutdown():
        imu_message=Imu()

        imu_message.header.stamp=rospy.Time.now()
        imu_message.header.frame_id="IMU"

        imu_message.orientation.x=1
        imu_message.orientation.y=2
        imu_message.orientation.z=3
        imu_message.orientation.w=4
        imu_message.orientation_covariance=[-1,0,0,0,-1,0,0,0,-1]

        imu_message.linear_acceleration.x=6
        imu_message.linear_acceleration.y=7
        imu_message.linear_acceleration.z=8
        imu_message.linear_acceleration_covariance=[1e6, 0,0,0,1e6,0,0,0,1e6]

        imu_message.angular_velocity.x=10
        imu_message.angular_velocity.y=11
        imu_message.angular_velocity.z=12
        imu_message.angular_velocity_covariance=[1e6, 0,0,0,1e6,0,0,0,1e6]

        pub.publish(imu_message)
        rate.sleep()

def publish_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 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)