发布温度话题

serial_6dof_imu/cfg/param.yaml

@@ -23,6 +23,7 @@ pub_topic_hz: 100
 
 imu_link_name: base_imu_link
 pub_data_topic: imu_data
+temp_pub_topic: imu_temp
 yaw_pub_topic: yaw_data
 pitch_pub_topic: pitch_data
 roll_pub_topic: roll_data

serial_6dof_imu/include/imu_data.h

@@ -17,6 +17,7 @@ float getAcc(int flag);
 float getAngular(int flag);
 float getAngle(int flag);
 float getQuat(int flag);
+float getTemp();
 
 int makeYawZero(void);
 int updateIICAddr(std::string input);

serial_6dof_imu/src/proc_serial_data.cpp

@@ -22,7 +22,7 @@
 
 static unsigned char r_buf[BYTE_CNT];  //一次从串口中读取的数据存储缓冲区
 static int port_fd = -1;  //串口打开时的文件描述符
-static float acce[3],angular[3],angle[3],quater[4];
+static float acce[3],angular[3],angle[3],quater[4],temp;
 
 /**************************************
  * Description:关闭串口文件描述符.
@@ -152,6 +152,7 @@ static void parse_serialData(unsigned char chr)
             angular[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ANGULAR_CONST;
             angular[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))*ANGULAR_CONST;
             angular[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))*ANGULAR_CONST;
+            temp       = ((float)(((short)chrBuf[9]<<8)|chrBuf[8]))/100.0;
             break;
         case 0x53: //欧拉角度输出, roll, pitch, yaw
             angle[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ANGLE_CONST;
@@ -323,6 +324,14 @@ float getQuat(int flag)
     return quater[flag];
 }
 
+/********************************************
+ * Description:返回温度值.
+ ********************************************/
+float getTemp(void)
+{
+    return temp;
+}
+
 /*************************************************************
  * Description:从串口读取数据,然后解析出各有效数据段,这里
  *  一次性从串口中读取88个字节,然后需要从这些字节中进行

serial_6dof_imu/src/serial_imu_node.cpp

@@ -75,6 +75,7 @@ int main(int argc, char **argv)
     std::string yaw_pub_topic;
     std::string pitch_pub_topic;
     std::string roll_pub_topic;
+    std::string temp_pub_topic;
     std::string yaw_zero_topic;
     std::string yaw_zero_service;
     std::string yaw_data_service;
@@ -95,6 +96,7 @@ int main(int argc, char **argv)
     ros::param::get("~yaw_zero_topic",   yaw_zero_topic);
     ros::param::get("~yaw_zero_service", yaw_zero_service);
     ros::param::get("~yaw_pub_topic",    yaw_pub_topic);
+    ros::param::get("~temp_pub_topic",   temp_pub_topic);
     ros::param::get("~pitch_pub_topic",  pitch_pub_topic);
     ros::param::get("~roll_pub_topic",   roll_pub_topic);
     ros::param::get("~get_yaw_data_srv", yaw_data_service);
@@ -120,6 +122,7 @@ int main(int argc, char **argv)
     ros::Publisher yaw_pub   = handle.advertise<std_msgs::Float32>(yaw_pub_topic, 2);
     ros::Publisher pitch_pub = handle.advertise<std_msgs::Float32>(pitch_pub_topic, 2);
     ros::Publisher roll_pub  = handle.advertise<std_msgs::Float32>(roll_pub_topic, 2);
+    ros::Publisher temp_pub  = handle.advertise<std_msgs::Float32>(temp_pub_topic, 2);
     ros::Rate loop_rate(pub_topic_hz);
 
     sensor_msgs::Imu imu_msg;
@@ -127,6 +130,7 @@ int main(int argc, char **argv)
     std_msgs::Float32 yaw_msg;
     std_msgs::Float32 pitch_msg;
     std_msgs::Float32 roll_msg;
+    std_msgs::Float32 temp_msg;
     while(ros::ok())
     {
         if(getImuData() < 0)
@@ -143,9 +147,11 @@ int main(int argc, char **argv)
         yaw_msg.data   = yaw;
         pitch_msg.data = pitch;
         roll_msg.data  = roll;
+        temp_msg.data  = getTemp();
         yaw_pub.publish(yaw_msg);     //将yaw值通过话题发布出去
         pitch_pub.publish(pitch_msg); //将pitch值通过话题发布出去
         roll_pub.publish(roll_msg);   //将roll值通过话题发布出去
+        temp_pub.publish(temp_msg);   //将temp值通过话题发布出去
 
         //ROS_INFO("yaw:%f pitch:%f roll:%f",yaw, pitch, roll);
         imu_msg.orientation.x = getQuat(1);