|
|
@@ -1,12 +1,15 @@
|
|
|
-/*****************************************************************
|
|
|
+/******************************************************************
|
|
|
* Copyright:2016-2021 www.corvin.cn ROS小课堂
|
|
|
* Description:使用串口来读取IMU的数据,并通过ROS话题topic将数据发布出来.
|
|
|
* 芯片中默认读取出来的加速度数据单位是g,需要将其转换为ROS中加速度规定的
|
|
|
- * m/s2才能发布.
|
|
|
+ * m/s2才能发布.ROS中使用的坐标系为ENU东北天坐标系,x轴指向东,y轴指向北,
|
|
|
+ * z轴指向天空.
|
|
|
* Author: corvin
|
|
|
* History:
|
|
|
- * 20211122: init this file.
|
|
|
-*****************************************************************/
|
|
|
+ * 20211122:init this file.
|
|
|
+ * 20220220:ROS中IMU的坐标系为ENU东北天坐标系,所以需要修改线加速度的
|
|
|
+ * 数据正方向,与XYZ三轴正方向都相反,此时Z轴加速度G为正值.
|
|
|
+******************************************************************/
|
|
|
#include <ros/ros.h>
|
|
|
#include <tf/tf.h>
|
|
|
#include <sensor_msgs/Imu.h>
|
|
|
@@ -20,8 +23,8 @@
|
|
|
static float g_yawData; //全局变量,存储当前yaw值,可以通过服务来得到该值
|
|
|
|
|
|
/**********************************************************
|
|
|
- * Description:将yaw角度归零的话题回调函数,只要往话题中
|
|
|
- * 发布一条Empty消息,即可将yaw角度归零.
|
|
|
+ * Description:将yaw角度归零的话题回调函数,只要往话题中发布一条
|
|
|
+ * std_msgs::Empty类型消息,即可将yaw角度归零.
|
|
|
*********************************************************/
|
|
|
void yawZeroCallback(const std_msgs::Empty::ConstPtr& msg)
|
|
|
{
|
|
|
@@ -82,13 +85,13 @@ int main(int argc, char **argv)
|
|
|
std::string set_iic_addr_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配置文件，然后从yaml配置文件中读取参数
|
|
|
+ //launch文件中加载yaml配置文件，然后从yaml配置文件中读取各参数
|
|
|
ros::param::get("~imu_dev", imu_dev);
|
|
|
ros::param::get("~imu_link_name", imu_link_name);
|
|
|
ros::param::get("~pub_topic_hz", pub_topic_hz);
|
|
|
@@ -168,11 +171,10 @@ int main(int argc, char **argv)
|
|
|
imu_msg.angular_velocity_covariance = {0, 0, 0,
|
|
|
0, 0, 0,
|
|
|
0, 0, 0};
|
|
|
- //三轴线加速度
|
|
|
- //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;
|
|
|
+ //三轴线加速度,这里规定xyz三轴线加速度坐标系为ENU
|
|
|
+ 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, 0, 0,
|
|
|
0, 0, 0,
|
|
|
0, 0, 0};
|
|
|
@@ -184,4 +186,4 @@ int main(int argc, char **argv)
|
|
|
|
|
|
closeSerialPort(); //关闭与IMU模块的串口连接
|
|
|
return 0;
|
|
|
-}
|
|
|
+}
|
