为模块增加发布温度的功能

rasp_imu_hat_6dof/cfg/param.yaml

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

rasp_imu_hat_6dof/nodes/imu_data.py

@@ -59,3 +59,11 @@ class MyIMU(object):
             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)
 
+    def get_temp(self):
+        try:
+            temp = self.i2c.read_i2c_block_data(self.addr, 0x40, 2)
+        except IOError:
+            rospy.logerr("Read IMU temperature data error !")
+        else:
+            self.temp = float((temp[1]<<8)|temp[0])/100.0
+            

rasp_imu_hat_6dof/nodes/imu_node.py

@@ -11,7 +11,6 @@
 # History:
 #    20191031: Initial this file.
 
-
 import rospy
 import string
 import math
@@ -21,6 +20,7 @@ import sys
 from tf.transformations import quaternion_from_euler
 from dynamic_reconfigure.server import Server
 from rasp_imu_hat_6dof.cfg import imuConfig
+from std_msgs.msg import Float32
 from sensor_msgs.msg import Imu
 from imu_data import MyIMU
 
@@ -37,42 +37,33 @@ def reconfig_callback(config, level):
 
 rospy.init_node("imu_node")
 
-#Get DIY param
-topic_name = rospy.get_param('~pub_topic', 'imu')
+# Get DIY config param
+data_topic_name = rospy.get_param('~pub_data_topic', 'imu_data')
 link_name  = rospy.get_param('~link_name', 'base_imu_link')
-pub_hz     = rospy.get_param('~pub_hz', '10')
+data_pub_hz = rospy.get_param('~data_pub_hz', '10')
+temp_pub_hz = rospy.get_param('~temp_pub_hz', '1')
 
 
-pub = rospy.Publisher(topic_name, Imu, queue_size=1)
+data_pub = rospy.Publisher(data_topic_name, Imu, queue_size=1)
+temp_pub = rospy.Publisher(temp_topic_name, Float32, queue_size=1)
 srv = Server(imuConfig, reconfig_callback)  # define dynamic_reconfigure callback
 imuMsg = Imu()
 
-# Orientation covariance estimation:
-# Observed orientation noise: 0.3 degrees in x, y, 0.6 degrees in z
-# Accelerometer non-linearity: 0.2% of 4G => 0.008G. This could cause
-# static roll/pitch error of 0.8%, owing to gravity orientation sensing
-# error => 2.8 degrees, or 0.05 radians. i.e. variance in roll/pitch: 0.0025
-# so set all covariances the same.
+# Orientation covariance estimation
 imuMsg.orientation_covariance = [
 0.0025 , 0 , 0,
 0, 0.0025, 0,
 0, 0, 0.0025
 ]
 
-# Angular velocity covariance estimation:
-# Observed gyro noise: 4 counts => 0.28 degrees/sec
-# nonlinearity spec: 0.2% of full scale => 8 degrees/sec = 0.14 rad/sec
-# Choosing the larger (0.14) as std dev, variance = 0.14^2 ~= 0.02
+# Angular velocity covariance estimation
 imuMsg.angular_velocity_covariance = [
 0.02, 0 , 0,
 0 , 0.02, 0,
 0 , 0 , 0.02
 ]
 
-# linear acceleration covariance estimation:
-# observed acceleration noise: 5 counts => 20milli-G's ~= 0.2m/s^2
-# nonliniarity spec: 0.5% of full scale => 0.2m/s^2
-# Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
+# linear acceleration covariance estimation
 imuMsg.linear_acceleration_covariance = [
 0.04 , 0 , 0,
 0 , 0.04, 0,
@@ -83,9 +74,8 @@ seq=0
 # sensor reports accel as 256.0 = 1G (9.8m/s^2). Convert to m/s^2.
 accel_factor = 9.806/256.0
 
-
 myIMU = MyIMU(0x50)
-rate = rospy.Rate(pub_hz)
+rate = rospy.Rate(data_pub_hz)
 
 while not rospy.is_shutdown():
     myIMU.get_YPRAG()
@@ -101,7 +91,7 @@ while not rospy.is_shutdown():
     roll  = float(myIMU.raw_roll)*degrees2rad
     #rospy.loginfo("yaw:%f pitch:%f  rool:%f", yaw, pitch, roll)
 
-    # Publish message
+    # Publish imu message
     imuMsg.linear_acceleration.x = float(myIMU.raw_ax)*accel_factor
     imuMsg.linear_acceleration.y = float(myIMU.raw_ay)*accel_factor
     imuMsg.linear_acceleration.z = float(myIMU.raw_az)*accel_factor
@@ -110,12 +100,14 @@ while not rospy.is_shutdown():
     imuMsg.angular_velocity.y = float(myIMU.raw_gy)*degrees2rad
     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
 
+    # 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]
@@ -125,8 +117,12 @@ while not rospy.is_shutdown():
     imuMsg.header.stamp= rospy.Time.now()
     imuMsg.header.frame_id = link_name
     imuMsg.header.seq = seq
-    pub.publish(imuMsg)
-
     seq = seq + 1
+    data_pub.publish(imuMsg)
+
+    # Get imu module temperature, then publish to topic
+    myIMU.get_temp()
+    temp_pub.publish(myIMU.temp)
+
     rate.sleep()