修改ROS下IMU消息中线加速度值正负,ROS中IMU使用ENU坐标系,那么静止时z轴线加速度为G是正值

serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -1,5 +1,5 @@
 /*******************************************************************
- * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Copyright:2016-2022 www.corvin.cn ROS小课堂
  * Description:使用串口方式读取和控制IMU模块信息.
  * Author: corvin
  * History:
@@ -20,7 +20,7 @@
 #include<sys/time.h>
 #include<sys/types.h>
 
-#define   BYTE_CNT      88  //每次从串口中读取的字节数
+#define   BYTE_CNT      55            //每次从串口中读取的总字节数
 #define   ACCE_CONST    0.000488281   //用于计算加速度的常量16.0/32768.0
 #define   ANGULAR_CONST 0.061035156   //用于计算角速度的常量2000.0/32768.0
 #define   ANGLE_CONST   0.005493164   //用于计算欧拉角的常量180.0/32768.0
@@ -230,7 +230,6 @@ void parse_serialData(unsigned char chr)
             quater[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0;
             quater[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0;
             quater[3] = ((short)(((short)chrBuf[9]<<8)|chrBuf[8]))/32768.0;
-            //printf("%f %f %f %f\n", quater[0], quater[1], quater[2], quater[3]);
             break;
         default:
             ROS_ERROR("parse imu data frame type error !");
@@ -260,10 +259,9 @@ int makeYawZero(void)
 }
 
 /******************************************************************
- * Description:更新串口通信波特率,根据输入参数的不同,修改为不同的
- *   波特率,1修改为9600,2修改为57600,3修改为115200.修改波特率就是
- *   发送3条指令,第一条命令是解除锁定,然后是发送串口波特率更新命令,
- *   最后是保存操作的指令.
+ * Description:更新串口通信波特率,根据输入参数的不同,修改为不同的波特率,
+ *   1修改为9600,2修改为57600,3修改为115200.修改波特率就是发送3条指令,
+ *   第一条命令是解除锁定,然后是发送串口波特率更新命令,最后是保存操作的指令.
  ******************************************************************/
 int setIMUBaudRate(const int flag)
 {
@@ -372,9 +370,8 @@ int initSerialPort(const char* path, const int baud)
 }
 
 /*****************************************
- * Description:得到三轴加速度信息,输入
- * 　参数可以为0,1,2分别代表x,y,z轴的
- * 　加速度信息.
+ * Description:得到三轴加速度信息,输入参数可以
+ * 　为0,1,2分别代表x,y,z轴的加速度信息.
  *****************************************/
 float getAcc(int flag)
 {
@@ -382,9 +379,8 @@ float getAcc(int flag)
 }
 
 /******************************************
- * Description:得到角速度信息,输入参数可
- * 　以为0,1,2,分别代表x,y,z三轴的角速度
- *   信息.
+ * Description:得到角速度信息,输入参数可以为
+ * 　0,1,2,分别代表x,y,z三轴的角速度信息.
  *****************************************/
 float getAngular(int flag)
 {
@@ -413,7 +409,7 @@ float getQuat(int flag)
 
 /*************************************************************
  * Description:从串口读取数据,然后解析出各有效数据段,这里
- *  一次性从串口中读取88个字节,然后需要从这些字节中进行
+ *  一次性从串口中读取55个字节,然后需要从这些字节中进行
  *  解析读取44个字节,正好是4帧数据,每一帧数据是11个字节.
  *  有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
  *  角度输出(0x55 0x53),四元素输出(0x55 0x59).

serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -1,11 +1,11 @@
 /*****************************************************************
- * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Copyright:2016-2022 www.corvin.cn ROS小课堂
  * Description:使用串口来读取IMU模块的数据,并通过ROS话题topic将
  *   数据发布出来.芯片中默认读取出来的加速度数据单位是g,需要将其
  *   转换为ROS中加速度规定的m/s2才能发布.
  * Author: corvin
  * History:
- *   20200403: init this file.
+ *   20200403:init this file.
  *   20200406:增加发布yaw数据的话题.
  *   20200703:修改三轴加速度数据正负,按照ROS规定沿着各轴
  *     正方向时数据为正,否则为负.单位m/s2。
@@ -18,6 +18,8 @@
  *   20210321:增加可以设置D0,D1,D2,D3共4个IO引脚输出数字高低电平
  *     的服务,这里的高电平为3.3V.增加通过服务方式获取yaw数据.
  *   20211003:增加发布pitch和roll数据的话题.
+ *   20220220:修改三轴加速度数据正负值,ROS中使用ENU坐标系,规定Z轴线
+ *     加速度的正方向为下,所以IMU静止时G为正值.
 *****************************************************************/
 #include <ros/ros.h>
 #include <tf/tf.h>
@@ -30,24 +32,22 @@
 #include "serial_imu_hat_6dof/setPinOutHL.h"
 #include "serial_imu_hat_6dof/getYawData.h"
 
-
 static float g_yawData; //全局变量,存储当前yaw值,可以通过服务来得到该值
 
-/**********************************************************
- * Description:将yaw角度归零的话题回调函数,只要往话题中
- *   发布一条Empty消息,即可将yaw角度归零.
- *********************************************************/
+/***********************************************************
+ * Description:将yaw角度归零的话题回调函数,只要往话题中发布一条
+ *   std_msgs::Empty类型消息,即可将yaw角度归零.
+ **********************************************************/
 void yawZeroCallback(const std_msgs::Empty::ConstPtr& msg)
 {
     makeYawZero();
 }
 
 /******************************************************************
- * Description:使用service方式来进行yaw角度归零,这里是服务的回调
- *   函数,当有客户端发送yaw归零的服务时,自动调用该函数,如果正确
- *   执行了yaw归零,则response反馈为0,如果为其他负数则表明yaw归零
- *   命令执行失败.
- *****************************************************************/
+ * Description:使用service方式来进行yaw角度归零,这里是服务的回调函数,
+ *   当有客户端发送yaw归零的服务时,自动调用该函数,如果正确执行了yaw归零,
+ *   则response反馈为0,如果为其他负数则表明yaw归零命令执行失败.
+ ******************************************************************/
 bool yawZeroService(serial_imu_hat_6dof::setYawZero::Request &req,
                     serial_imu_hat_6dof::setYawZero::Response &res)
 {
@@ -112,15 +112,15 @@ int main(int argc, char **argv)
     std::string yaw_data_service;
 
     int pub_topic_hz = 0;  //话题发布imu数据的频率
-    float degree2Rad = 3.1415926/180.0;
-    float acc_factor = 9.806;
+    const float degree2Rad = 0.017453292; //3.1415926/180.0 角度转换为弧度的系数
+    const float acc_factor = 9.806; //重力加速度常量
 
     ros::init(argc, argv, "imu_data_pub_node");
     ros::NodeHandle handle;
 
     //launch文件中加载yaml配置文件，然后从配置文件中读取参数
-    ros::param::get("~imu_dev",   imu_dev);
-    ros::param::get("~baud_rate", baud);
+    ros::param::get("~imu_dev",          imu_dev);
+    ros::param::get("~baud_rate",        baud);
     ros::param::get("~imu_link_name",    imu_link_name);
     ros::param::get("~pub_topic_hz",     pub_topic_hz);
     ros::param::get("~pub_data_topic",   imu_topic_name);
@@ -189,7 +189,7 @@ int main(int argc, char **argv)
         imu_msg.orientation_covariance = {0.0025, 0, 0,
                                           0, 0.0025, 0,
                                           0, 0, 0.0025};
-
+        //三轴角速度数据
         imu_msg.angular_velocity.x = getAngular(0)*degree2Rad;
         imu_msg.angular_velocity.y = getAngular(1)*degree2Rad;
         imu_msg.angular_velocity.z = getAngular(2)*degree2Rad;
@@ -197,10 +197,10 @@ 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",getAcc(0), getAcc(1), getAcc(2));
-        imu_msg.linear_acceleration.x = -getAcc(0)*acc_factor;
-        imu_msg.linear_acceleration.y = -getAcc(1)*acc_factor;
-        imu_msg.linear_acceleration.z = -getAcc(2)*acc_factor;
+        //三轴线加速度数据,这里都为正值
+        imu_msg.linear_acceleration.x = getAcc(0)*acc_factor;
+        imu_msg.linear_acceleration.y = getAcc(1)*acc_factor;
+        imu_msg.linear_acceleration.z = getAcc(2)*acc_factor;
         imu_msg.linear_acceleration_covariance = {0.04, 0, 0,
                                                    0, 0.04, 0,
                                                    0, 0, 0.04};
@@ -212,4 +212,4 @@ int main(int argc, char **argv)
 
     closeSerialPort(); //关闭与IMU模块的串口连接
     return 0;
-}
+}