增加代码开头的注释信息，更新rviz显示配置文件

imu_tools/rviz_imu_plugin/src/imu_orientation_visual.cpp

@@ -174,6 +174,5 @@ inline bool ImuOrientationVisual::checkQuaternionValidity(
   return true;
 }
 
-
 } // end namespace rviz
 

rasp_imu_hat_6dof/cfg/imu_display.rviz

@@ -7,7 +7,7 @@ Panels:
         - /Global Options1
         - /Grid1/Offset1
       Splitter Ratio: 0.6222222447395325
-    Tree Height: 311
+    Tree Height: 344
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -71,7 +71,7 @@ Visualization Manager:
       Class: rviz_imu_plugin/Imu
       Enabled: true
       Name: Imu
-      Topic: /imu
+      Topic: /imu_data
       Unreliable: false
       Value: true
       fixed_frame_orientation: true
@@ -139,5 +139,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1024
-  X: -2
-  Y: 24
+  X: 0
+  Y: 96

rasp_imu_hat_6dof/cfg/param.yaml

@@ -1,5 +1,4 @@
 pub_data_topic: imu_data
 pub_temp_topic: imu_temp
 link_name: base_imu_link
-data_pub_hz: 10
-temp_pub_hz: 1
+pub_hz: 10

rasp_imu_hat_6dof/nodes/imu_data.py

@@ -8,6 +8,7 @@
 #    中读取IMU模块的三轴加速度、角度、四元数。
 # History:
 #    20191031: Initial this file.
+#    20191209： Add get imu chip temperature function-get_temp().
 
 import smbus
 import numpy as np
@@ -72,5 +73,5 @@ class MyIMU(object):
             rospy.logerr("Read IMU temperature data error !")
         else:
             self.temp = float((temp[1]<<8)|temp[0])/100.0
-            self.temp = float(self.get_two_float(self.temp, 2)) #keep 2 decimal places
+            #self.temp = float(self.get_two_float(self.temp, 2)) #keep 2 decimal places
 

rasp_imu_hat_6dof/nodes/imu_node.py

@@ -10,6 +10,7 @@
 #    直接订阅该话题即可获取到imu扩展板当前的数据。
 # History:
 #    20191031: Initial this file.
+#    20191209: 新增发布IMU芯片温度的话题，发布频率与发布imu数据频率相同.
 
 import rospy
 import string
@@ -41,9 +42,7 @@ rospy.init_node("imu_node")
 data_topic_name = rospy.get_param('~pub_data_topic', 'imu_data')
 temp_topic_name = rospy.get_param('~pub_temp_topic', 'imu_temp')
 link_name  = rospy.get_param('~link_name', 'base_imu_link')
-data_pub_hz = rospy.get_param('~data_pub_hz', '10')
-temp_pub_hz = rospy.get_param('~temp_pub_hz', '1')
-
+pub_hz = rospy.get_param('~pub_hz', '10')
 
 data_pub = rospy.Publisher(data_topic_name, Imu, queue_size=1)
 temp_pub = rospy.Publisher(temp_topic_name, Float32, queue_size=1)
@@ -76,8 +75,9 @@ seq=0
 accel_factor = 9.806/256.0
 
 myIMU = MyIMU(0x50)
-rate = rospy.Rate(data_pub_hz)
+rate = rospy.Rate(pub_hz)
 
+rospy.loginfo("Rasp IMU Module is woking ...")
 while not rospy.is_shutdown():
     myIMU.get_YPRAG()
 
@@ -102,18 +102,18 @@ while not rospy.is_shutdown():
     imuMsg.angular_velocity.z = float(myIMU.raw_gz)*degrees2rad
 
     # From IMU module get quatern param
-    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
+    #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
 
     # Calculate quatern param from roll,pitch,yaw by ourselves
-    #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]
+    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