调试IMU串口发布imu数据完成,经过测试和IIC发布数据一致

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/nodes/imu_node.py

@@ -13,10 +13,7 @@
 #    20191209: 新增发布IMU芯片温度的话题，发布频率与发布imu数据频率相同.
 
 import rospy
-import string
 import math
-import time
-import sys
 
 from tf.transformations import quaternion_from_euler
 from dynamic_reconfigure.server import Server
@@ -81,6 +78,8 @@ rospy.loginfo("Rasp IMU Module is woking ...")
 while not rospy.is_shutdown():
     myIMU.get_YPRAG()
 
+    #rospy.loginfo("yaw:%f pitch:%f  roll:%f", myIMU.raw_yaw,
+    #              myIMU.raw_pitch, myIMU.raw_roll)
     yaw_deg = float(myIMU.raw_yaw)
     yaw_deg = yaw_deg + imu_yaw_calibration
     if yaw_deg >= 180.0:
@@ -90,9 +89,10 @@ while not rospy.is_shutdown():
     yaw = yaw_deg*degrees2rad
     pitch = float(myIMU.raw_pitch)*degrees2rad
     roll  = float(myIMU.raw_roll)*degrees2rad
-    #rospy.loginfo("yaw:%f pitch:%f  rool:%f", yaw, pitch, roll)
 
     # Publish imu message
+    #rospy.loginfo("acc_x:%f acc_y:%f acc_z:%f", myIMU.raw_ax,
+    #              myIMU.raw_ay, myIMU.raw_az)
     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

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -219,24 +219,22 @@ void parse_serialData(char chr)
     memcpy(&sData[0], &chrBuf[2], 8);
     switch(chrBuf[1])
     {
-        case 0x51: //加速度输出
-            //ROS_INFO("AccX:%d %d",chrBuf[2], chrBuf[3]);
-            //ROS_INFO("AccY:%d %d",chrBuf[4], chrBuf[5]);
-            acce[0] = (short)(((short)chrBuf[3]<<8)|chrBuf[2])/32768.0*16.0;
-            acce[1] = (short)(((short)chrBuf[5]<<8)|chrBuf[4])/32768.0*16.0;
-            acce[2] = (short)(((short)chrBuf[7]<<8)|chrBuf[6])/32768.0*16.0;
+        case 0x51: //x,y,z轴 加速度输出
+            acce[0] = (((short)chrBuf[3]<<8)|chrBuf[2])/32768.0*16.0;
+            acce[1] = (((short)chrBuf[5]<<8)|chrBuf[4])/32768.0*16.0;
+            acce[2] = (((short)chrBuf[7]<<8)|chrBuf[6])/32768.0*16.0;
             break;
 
         case 0x52: //角速度输出
-            angular[0] = (short)(((short)chrBuf[3]<<8)|chrBuf[2])/32768.0*2000.0;
-            angular[1] = (short)(((short)chrBuf[5]<<8)|chrBuf[4])/32768.0*2000.0;
-            angular[2] = (short)(((short)chrBuf[7]<<8)|chrBuf[6])/32768.0*2000.0;
+            angular[0] = (((short)chrBuf[3]<<8)|chrBuf[2])/32768.0*2000.0;
+            angular[1] = (((short)chrBuf[5]<<8)|chrBuf[4])/32768.0*2000.0;
+            angular[2] = (((short)chrBuf[7]<<8)|chrBuf[6])/32768.0*2000.0;
             break;
 
-        case 0x53: //角度输出, roll, pitch, yaw
-            angle[0] = (short)(((short)chrBuf[3]<<8)|chrBuf[2])/32768.0*180.0;
-            angle[1] = (short)(((short)chrBuf[5]<<8)|chrBuf[4])/32768.0*180.0;
-            angle[2] = (short)(((short)chrBuf[7]<<8)|chrBuf[6])/32768.0*180.0;
+        case 0x53: //欧拉角度输出, roll, pitch, yaw
+            angle[0] = (((short)chrBuf[3]<<8)|chrBuf[2])/32768.0*180.0;
+            angle[1] = (((short)chrBuf[5]<<8)|chrBuf[4])/32768.0*180.0;
+            angle[2] = (((short)chrBuf[7]<<8)|chrBuf[6])/32768.0*180.0;
             break;
 
         case 0x59: //四元素输出

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -7,7 +7,7 @@
 int main(int argc, char **argv)
 {
     float yaw, pitch, roll;
-    tf::Quaternion q;
+    geometry_msgs::Quaternion q;
     std::string imu_dev;
     int baud = 0;
     int dataBit = 0;
@@ -19,7 +19,7 @@ int main(int argc, char **argv)
     std::string temp_topic;
     int pub_hz;
     float degree2Rad = 3.1415926/180.0;
-    float acc_factor= 9.806/256.0;
+    float acc_factor = 9.806/256.0;
 
     ros::init(argc, argv, "imu_data_pub_node");
     ros::NodeHandle handle;
@@ -53,17 +53,18 @@ int main(int argc, char **argv)
             break;
 
         imu_msg.header.stamp = ros::Time::now();
-        roll  = getAngleX();
-        pitch = getAngleY();
-        yaw   = getAngleZ();
+        roll  = getAngleX()*degree2Rad;
+        pitch = getAngleY()*degree2Rad;
+        yaw   = getAngleZ()*degree2Rad;
         if(yaw >= 180.0)
             yaw -= 360.0;
 
-        q.setRPY(roll, pitch, yaw);
-        imu_msg.orientation.x = q[0];
-        imu_msg.orientation.y = q[1];
-        imu_msg.orientation.z = q[2];
-        imu_msg.orientation.w = q[3];
+        //ROS_INFO("yaw:%f pitch:%f roll:%f",yaw, pitch, roll);
+        q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
+        imu_msg.orientation.x = q.x;
+        imu_msg.orientation.y = q.y;
+        imu_msg.orientation.z = q.z;
+        imu_msg.orientation.w = q.w;
         imu_msg.orientation_covariance = {0.0025, 0, 0,
                                           0, 0.0025, 0,
                                           0, 0, 0.0025};
@@ -75,6 +76,7 @@ int main(int argc, char **argv)
                                                0, 0.02, 0,
                                                0, 0, 0.02};
 
+        //ROS_INFO("acc_x:%f acc_y:%f acc_z:%f",getAccX(), getAccY(), getAccZ());
         imu_msg.linear_acceleration.x = getAccX()*acc_factor;
         imu_msg.linear_acceleration.y = getAccY()*acc_factor;
         imu_msg.linear_acceleration.z = getAccZ()*acc_factor;