Python Imu.orientation方法代码示例

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation [as 别名]
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

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation [as 别名]
    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)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation [as 别名]
    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)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation [as 别名]
    def read_from_imu(self):
        while True:
            try:
                imu_msg = Imu()
                imu_msg.header.stamp = rospy.Time.now()
                imu_msg.header.frame_id = 'imu'

                orientation = self.bno.read_quaternion()
                linear_acceleration = self.bno.read_linear_acceleration()
                angular_velocity = self.bno.read_gyroscope()

                imu_msg.orientation_covariance[0] = -1
                imu_msg.linear_acceleration_covariance[0] = -1
                imu_msg.angular_velocity_covariance[0] = -1

                imu_msg.orientation = Quaternion(orientation[1],
                                                 orientation[2],
                                                 orientation[3],
                                                 orientation[0])
                imu_msg.linear_acceleration = Vector3(linear_acceleration[0],
                                                      linear_acceleration[1],
                                                      linear_acceleration[2])
                imu_msg.angular_velocity = Vector3(np.deg2rad(angular_velocity[0]),
                                                   np.deg2rad(angular_velocity[1]),
                                                   np.deg2rad(angular_velocity[2]))
                return imu_msg
            except IOError:
                pass

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation [as 别名]
    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

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation [as 别名]
    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

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation [as 别名]
    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)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation [as 别名]
    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)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation [as 别名]
def pub_imu(msg_type, msg, bridge):
    pub = bridge.get_ros_pub("imu", Imu, queue_size=1)
    imu = Imu()
    imu.header.stamp = bridge.project_ap_time(msg)
    imu.header.frame_id = 'base_footprint'
    # Orientation as a Quaternion, with unknown covariance
    quat = quaternion_from_euler(msg.roll, msg.pitch, msg.yaw, 'sxyz')
    imu.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])
    imu.orientation_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
    # Angular velocities, with unknown covariance
    imu.angular_velocity.x = msg.rollspeed
    imu.angular_velocity.y = msg.pitchspeed
    imu.angular_velocity.z = msg.yawspeed
    imu.angular_velocity_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
    # Not supplied with linear accelerations
    imu.linear_acceleration_covariance[0] = -1
    pub.publish(imu)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation [as 别名]
 def publish_imu(self):
     imu_msg = Imu()
     imu_msg.header.stamp = self.time
     imu_msg.header.frame_id = 'imu'
     imu_msg.orientation = Quaternion(*SF9DOF_UKF.recover_quat(self.kalman_state))
     imu_msg.orientation_covariance = \
            list(self.kalman_covariance[0:3,0:3].flatten())
     imu_msg.angular_velocity.x = self.kalman_state[3,0]
     imu_msg.angular_velocity.y = self.kalman_state[4,0]
     imu_msg.angular_velocity.z = self.kalman_state[5,0]
     imu_msg.angular_velocity_covariance = \
             list(self.kalman_covariance[3:6,3:6].flatten())
     imu_msg.linear_acceleration.x = self.kalman_state[6,0]
     imu_msg.linear_acceleration.y = self.kalman_state[7,0]
     imu_msg.linear_acceleration.z = self.kalman_state[8,0]
     imu_msg.linear_acceleration_covariance = \
            list(self.kalman_covariance[6:9,6:9].flatten())
     self.pub.publish(imu_msg)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation [as 别名]
 def publish_imu(self):
     imu_msg = Imu()
     imu_msg.header.stamp = rospy.Time.now()
     imu_msg.header.frame_id = 'base_link'
     #Orientation from Accelerometer
     roll = self.accel_data["roll"]
     pitch = self.accel_data["pitch"]
     yaw = 0
     q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
     imu_msg.orientation = Quaternion(*q)
     #Angular Velocity from Gyroscope
     imu_msg.angular_velocity.x = self.gyro_data["x"]
     imu_msg.angular_velocity.y = self.gyro_data["y"]
     imu_msg.angular_velocity.z = self.gyro_data["z"]
     #Linear Acceleration from Accelerometer
     imu_msg.linear_acceleration.x = self.accel_data["x"]  
     imu_msg.linear_acceleration.y = self.accel_data["y"]  
     imu_msg.linear_acceleration.z = self.accel_data["z"]   
     self.imu_pub.publish(imu_msg)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation [as 别名]
def talker():
	#Create publisher ('Topic name', msg type)
	pub = rospy.Publisher('IMU', Imu)
	#Tells rospy name of the node to allow communication to ROS master
	rospy.init_node('IMUtalker')

	while not rospy.is_shutdown():
		#Grab relevant information from parse()
		try:
			(accel,gyro,magne) = parse()
		#If data is bad, uses presvious data
		except: continue
		
		#Define IMUmsg to be of the Imu msg type
		IMUmsg = Imu()
		
		#Set header time stamp
		IMUmsg.header.stamp = rospy.Time.now()
		
		#Set orientation parameters
		IMUmsg.orientation = Quaternion()
		IMUmsg.orientation.x = magne[0]
		IMUmsg.orientation.y = magne[1]
		IMUmsg.orientation.z = magne[2]
		IMUmsg.orientation_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
		
		#Set angular velocity parameters
		IMUmsg.angular_velocity = Vector3()
		IMUmsg.angular_velocity.x = gyro[0]
		IMUmsg.angular_velocity.y = gyro[1]
		IMUmsg.angular_velocity.z = gyro[2]
		IMUmsg.angular_velocity_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
		
		#Set linear acceleration parameters
		IMUmsg.linear_acceleration = Vector3()
		IMUmsg.linear_acceleration.x = accel[0]
		IMUmsg.linear_acceleration.y = accel[1]
		IMUmsg.linear_acceleration.z = accel[2]
		IMUmsg.linear_acceleration_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
		
		#Publish the data
		pub.publish(IMUmsg)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation [as 别名]
	def update_sensors(self):
		# Accelerometer
		accel = self._driver.get_accelerometer()
		accel_msg = Imu()
		accel_msg.header.stamp = rospy.Time.now()
		accel_msg.header.frame_id = self._name+"/base_link"
		accel_msg.linear_acceleration.x = (accel[1]-2048.0)/800.0*9.81 # 1 g = about 800, then transforms in m/s^2.
		accel_msg.linear_acceleration.y = (accel[0]-2048.0)/800.0*9.81
		accel_msg.linear_acceleration.z = (accel[2]-2048.0)/800.0*9.81
		accel_msg.linear_acceleration_covariance = [0.01,0.0,0.0, 0.0,0.01,0.0, 0.0,0.0,0.01]
		#print "accel raw: " + str(accel[0]) + ", " + str(accel[1]) + ", " + str(accel[2])
		#print "accel (m/s2): " + str((accel[0]-2048.0)/800.0*9.81) + ", " + str((accel[1]-2048.0)/800.0*9.81) + ", " + str((accel[2]-2048.0)/800.0*9.81)
		accel_msg.angular_velocity.x = 0
		accel_msg.angular_velocity.y = 0
		accel_msg.angular_velocity.z = 0
		accel_msg.angular_velocity_covariance = [0.01,0.0,0.0, 0.0,0.01,0.0, 0.0,0.0,0.01]
		q = tf.transformations.quaternion_from_euler(0, 0, 0)
		accel_msg.orientation = Quaternion(*q)
		accel_msg.orientation_covariance = [0.01,0.0,0.0, 0.0,0.01,0.0, 0.0,0.0,0.01]
		self.accel_publisher.publish(accel_msg)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation [as 别名]
 def run(self):
   while not rospy.is_shutdown():
     # Get and publish all the sensors data
     for sensor, id_number in SENSORS.items():
       imu_msg = Imu()
       mag_msg = Vector3Stamped()
       # Order: GyroRate[0:2], AccelerometerVector[3:5], CompassVector[6:8]
       #~ raw_data = self.pvr_system.getAllCorrectedComponentSensorData(id_number)
       tared_quat = self.pvr_system.getTaredOrientationAsQuaternion(id_number)
       #~ if raw_data and tared_quat:
       if tared_quat:
         imu_msg.orientation = Quaternion(*tared_quat)
         #~ imu_msg.angular_velocity = Vector3(*raw_data[0:3])
         #~ imu_msg.linear_acceleration = Vector3(*raw_data[3:6])
         #~ mag_msg.vector = Vector3(*raw_data[6:9])
         # Publish the data
         imu_msg.header.stamp = rospy.Time.now()
         imu_msg.header.frame_id = 'world'
         mag_msg.header = imu_msg.header
         self.mag_pub[sensor].publish(mag_msg)
         self.imu_pub[sensor].publish(imu_msg)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation [as 别名]
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)