解决python读取四元素异常的问题,使用numbpy.short类型强转才行

rasp_imu_hat_6dof/cfg/imu_display.rviz

@@ -6,11 +6,8 @@ Panels:
       Expanded:
         - /Global Options1
         - /Grid1/Offset1
-        - /Imu1/Status1
-        - /Imu1/Box properties1
-        - /Imu1/Axes properties1
-      Splitter Ratio: 0.486111104
-    Tree Height: 730
+      Splitter Ratio: 0.6222222447395325
+    Tree Height: 311
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -19,7 +16,7 @@ Panels:
       - /2D Nav Goal1
       - /Publish Point1
     Name: Tool Properties
-    Splitter Ratio: 0.588679016
+    Splitter Ratio: 0.5886790156364441
   - Class: rviz/Views
     Expanded:
       - /Current View1
@@ -30,18 +27,20 @@ Panels:
     Name: Time
     SyncMode: 0
     SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
 Toolbars:
   toolButtonStyle: 2
 Visualization Manager:
   Class: ""
   Displays:
-    - Alpha: 0.699999988
+    - Alpha: 0.699999988079071
       Cell Size: 1
       Class: rviz/Grid
       Color: 50; 115; 54
       Enabled: true
       Line Style:
-        Line Width: 0.0299999993
+        Line Width: 0.029999999329447746
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -54,21 +53,21 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Value: true
     - Acceleration properties:
-        Acc. vector alpha: 0.800000012
+        Acc. vector alpha: 0.800000011920929
         Acc. vector color: 179; 200; 255
-        Acc. vector scale: 0.0500000007
-        Derotate acceleration: true
-        Enable acceleration: true
+        Acc. vector scale: 0.05000000074505806
+        Derotate acceleration: false
+        Enable acceleration: false
       Axes properties:
         Axes scale: 1
         Enable axes: true
       Box properties:
-        Box alpha: 0.600000024
+        Box alpha: 0.6000000238418579
         Box color: 255; 0; 0
         Enable box: true
-        x_scale: 0.5
-        y_scale: 0.5
-        z_scale: 0.5
+        x_scale: 0.10000000149011612
+        y_scale: 0.10000000149011612
+        z_scale: 0.10000000149011612
       Class: rviz_imu_plugin/Imu
       Enabled: true
       Name: Imu
@@ -91,7 +90,10 @@ Visualization Manager:
     - Class: rviz/FocusCamera
     - Class: rviz/Measure
     - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
       Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
     - Class: rviz/SetGoal
       Topic: /move_base_simple/goal
     - Class: rviz/PublishPoint
@@ -101,33 +103,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 8.40952873
+      Distance: 0.9545544385910034
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.0599999987
+        Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.0269238316
-        Y: 0.0480619296
-        Z: 0.0609069802
+        X: 0.026923831552267075
+        Y: 0.048061929643154144
+        Z: 0.06090698018670082
       Focal Shape Fixed Size: true
-      Focal Shape Size: 0.0500000007
+      Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.00999999978
-      Pitch: 0.545399547
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.4303995668888092
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 1.29048622
+      Yaw: 1.730486273765564
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: false
-  Height: 1056
-  Hide Left Dock: false
+    collapsed: true
+  Height: 714
+  Hide Left Dock: true
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000006100000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000021bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006500fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000440000021b000000de00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000040000000044fc0100000002fb0000000800540069006d00650100000000000004000000028000fffffffb0000000800540069006d00650100000000000004500000000000000000000004000000021b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -136,6 +138,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1920
-  X: 1600
+  Width: 1024
+  X: -2
   Y: 24

rasp_imu_hat_6dof/nodes/imu_data.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python
+
+import smbus
+import numpy as np
+
+class MyIMU(object):
+    def __init__(self, addr):
+        self.addr = addr
+        self.i2c = smbus.SMBus(1)
+
+    def get_YPRAG(self):
+        try:
+            roll_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x3d, 2)
+            pitch_tmp = self.i2c.read_i2c_block_data(self.addr, 0x3e, 2)
+            yaw_tmp   = self.i2c.read_i2c_block_data(self.addr, 0x3f, 2)
+
+            ax_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x34, 2)
+            ay_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x35, 2)
+            az_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x36, 2)
+
+            gx_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x37, 2)
+            gy_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x38, 2)
+            gz_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x39, 2)
+        except IOError:
+            rospy.logerr("Read IMU YPRAG date error !")
+        else:
+            self.raw_roll  = float(((roll_tmp[1]<<8) |roll_tmp[0])/32768.0*180.0)
+            self.raw_pitch = float(((pitch_tmp[1]<<8)|pitch_tmp[0])/32768.0*180.0)
+            self.raw_yaw  = (yaw_tmp[1] << 8 | yaw_tmp[0])/32768.0*180
+
+            self.raw_ax  = float(((ax_tmp[1]<<8)|ax_tmp[0])/32768.0*16.0)
+            self.raw_ay  = float(((ay_tmp[1]<<8)|ay_tmp[0])/32768.0*16.0)
+            self.raw_az  = float(((az_tmp[1]<<8)|az_tmp[0])/32768.0*16.0)
+
+            self.raw_gx  = float(((gx_tmp[1]<<8)|gx_tmp[0])/32768.0*2000.0)
+            self.raw_gy  = float(((gy_tmp[1]<<8)|gy_tmp[0])/32768.0*2000.0)
+            self.raw_gz  = float(((gz_tmp[1]<<8)|gz_tmp[0])/32768.0*2000.0)
+
+    def get_quatern(self):
+        try:
+            q0 = self.i2c.read_i2c_block_data(self.addr, 0x51, 2)
+            q1 = self.i2c.read_i2c_block_data(self.addr, 0x52, 2)
+            q2 = self.i2c.read_i2c_block_data(self.addr, 0x53, 2)
+            q3 = self.i2c.read_i2c_block_data(self.addr, 0x54, 2)
+        except IOError:
+            rospy.logerr("Read IMU quaternion date error !")
+        else:
+            self.raw_q0 = float((np.short((q0[1]<<8)|q0[0]))/32768.0)
+            self.raw_q1 = float((np.short((q1[1]<<8)|q1[0]))/32768.0)
+            self.raw_q2 = float((np.short((q2[1]<<8)|q2[0]))/32768.0)
+            self.raw_q3 = float((np.short((q3[1]<<8)|q3[0]))/32768.0)

rasp_imu_hat_6dof/nodes/imu_node.py

@@ -1,6 +1,5 @@
 #!/usr/bin/env python
 
-import smbus
 import rospy
 import string
 import math
@@ -11,6 +10,7 @@ from tf.transformations import quaternion_from_euler
 from dynamic_reconfigure.server import Server
 from rasp_imu_hat_6dof.cfg import imuConfig
 from sensor_msgs.msg import Imu
+from imu_data import MyIMU
 
 
 degrees2rad = math.pi/180.0
@@ -23,7 +23,6 @@ def reconfig_callback(config, level):
     rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration))
     return config
 
-
 rospy.init_node("imu_node")
 
 #Get DIY param
@@ -71,53 +70,6 @@ imuMsg.linear_acceleration_covariance = [
 seq=0
 accel_factor = 9.806/256.0    # sensor reports accel as 256.0 = 1G (9.8m/s^2). Convert to m/s^2.
 
-class MyIMU(object):
-    def __init__(self, addr):
-        self.addr = addr
-        self.i2c = smbus.SMBus(1)
-
-    def get_YPRAG(self):
-        try:
-            roll_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x3d, 2)
-            pitch_tmp = self.i2c.read_i2c_block_data(self.addr, 0x3e, 2)
-            yaw_tmp   = self.i2c.read_i2c_block_data(self.addr, 0x3f, 2)
-
-            ax_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x34, 2)
-            ay_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x35, 2)
-            az_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x36, 2)
-
-            gx_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x37, 2)
-            gy_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x38, 2)
-            gz_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x39, 2)
-        except IOError:
-            rospy.logerr("Read IMU YPRAG date error !")
-        else:
-            self.raw_roll  = float(((roll_tmp[1]<<8) |roll_tmp[0])/32768.0*180.0)
-            self.raw_pitch = float(((pitch_tmp[1]<<8)|pitch_tmp[0])/32768.0*180.0)
-            self.raw_yaw  = (yaw_tmp[1] << 8 | yaw_tmp[0])/32768.0*180
-
-            self.raw_ax  = float(((ax_tmp[1]<<8)|ax_tmp[0])/32768.0*16.0)
-            self.raw_ay  = float(((ay_tmp[1]<<8)|ay_tmp[0])/32768.0*16.0)
-            self.raw_az  = float(((az_tmp[1]<<8)|az_tmp[0])/32768.0*16.0)
-
-            self.raw_gx  = float(((gx_tmp[1]<<8)|gx_tmp[0])/32768.0*2000.0)
-            self.raw_gy  = float(((gy_tmp[1]<<8)|gy_tmp[0])/32768.0*2000.0)
-            self.raw_gz  = float(((gz_tmp[1]<<8)|gz_tmp[0])/32768.0*2000.0)
-
-    def get_quatern(self):
-        try:
-            q0 = self.i2c.read_i2c_block_data(self.addr, 0x51, 2)
-            q1 = self.i2c.read_i2c_block_data(self.addr, 0x52, 2)
-            q2 = self.i2c.read_i2c_block_data(self.addr, 0x53, 2)
-            q3 = self.i2c.read_i2c_block_data(self.addr, 0x54, 2)
-        except IOError:
-            rospy.logerr("Read IMU quaternion date error !")
-        else:
-            self.raw_q0 = float(((q0[1]<<8)|q0[0])/32768.0)
-            self.raw_q1 = float(((q1[1]<<8)|q1[0])/32768.0)
-            self.raw_q2 = float(((q2[1]<<8)|q2[0])/32768.0)
-            self.raw_q3 = float(((q3[1]<<8)|q3[0])/32768.0)
-
 
 myIMU = MyIMU(0x50)
 rate = rospy.Rate(pub_hz)
@@ -132,8 +84,6 @@ while not rospy.is_shutdown():
 
     #rospy.loginfo("yaw_deg: %f", yaw_deg)
     yaw = yaw_deg*degrees2rad
-
-    #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103)
     pitch = float(myIMU.raw_pitch)*degrees2rad
     roll  = float(myIMU.raw_roll)*degrees2rad
     #rospy.loginfo("yaw:%f pitch:%f  rool:%f", yaw, pitch, roll)
@@ -147,16 +97,23 @@ while not rospy.is_shutdown():
     imuMsg.angular_velocity.y = float(myIMU.raw_gy)*degrees2rad
     imuMsg.angular_velocity.z = float(myIMU.raw_gz)*degrees2rad
 
-    #myIMU.get_quatern()
-    q = quaternion_from_euler(roll, pitch, yaw)
-    imuMsg.orientation.x = q[0]
-    imuMsg.orientation.y = q[1]
-    imuMsg.orientation.z = q[2]
-    imuMsg.orientation.w = q[3]
+    myIMU.get_quatern()
+    imuMsg.orientation.x = myIMU.raw_q1
+    imuMsg.orientation.y = myIMU.raw_q2
+    imuMsg.orientation.z = myIMU.raw_q3
+    imuMsg.orientation.w = myIMU.raw_q0
+
+    #q = quaternion_from_euler(roll, pitch, yaw)
+    #imuMsg.orientation.x = q[0]
+    #imuMsg.orientation.y = q[1]
+    #imuMsg.orientation.z = q[2]
+    #imuMsg.orientation.w = q[3]
+
     imuMsg.header.stamp= rospy.Time.now()
     imuMsg.header.frame_id = link_name
     imuMsg.header.seq = seq
     pub.publish(imuMsg)
+
     seq = seq + 1
     rate.sleep()