更新串口发布话题数据的频率为100hz,新增发布pitch和roll数据的话题

serial_imu_hat_6dof/cfg/param.yaml

@@ -1,6 +1,6 @@
 ###########################################################
 # Copyright: 2016-2021 www.corvin.cn ROS小课堂
-# Description:使用串口与IMU模块进行通信时的配置信息.
+# Description:使用串口与IMU模块进行通信时的配置信息,可以根据需要修改
 # 可以配置的各参数如下,这些话题名或者服务名,发布频率这些都
 # 可以根据需要来修改,这里需要注意参数名称和参数之间要有空格隔开:
 #  imu_dev:串口设备在linux系统的挂载点,如果使用usb转串口
@@ -10,13 +10,18 @@
 #     就需要在raspi-config中关闭串口登录功能,打开硬件
 #     串口功能,这同时也需要将树莓派的蓝牙给禁用掉.
 #  baud_rate:串口通信波特率,默认为57600,如果查看发布的
-#  　　话题中没有IMU数据可以尝试换波特率.
+#  　　话题中没有IMU数据可以尝试换波特率为9600或115200.
+#  pub_topic_hz:话题发布的数据频率,默认设置为100Hz.
+#  imu_link_name:imu模块的link名,该名称一般在urdf模型中定义.
 #  pub_data_topic:发布imu数据的ROS话题名.
-#  pub_temp_topic:发布imu模块温度的ROS话题名.
-#  imu_link_name:imu模块的link名.
-#  pub_topic_hz:话题发布的数据频率,默认设置为50Hz.
 #  yaw_zero_topic:通过往该话题中发消息即可将yaw角度归零.
 #  yaw_pub_topic:将yaw信息通过该话题发送出来.
+#  pitch_pub_topic:将pitch数据通过该话题发布出来.
+#  roll_pub_topic:将roll数据通过该话题发布出来.
+#  yaw_zero_topic:通过向该话题发布空消息即可将yaw角度归零.
+#  yaw_zero_service:通过向该服务发送空请求即可将yaw归零.
+#  baud_update_srv:通过向该服务发送1,2,3可以将波特率更新为
+#    9600bps,57600bps,115200bps,三种通信波特率.
 #  pin_outHL_srv:控制IMU板上D0-D4的输出电平高低的服务.
 #  yaw_data_srv:可以获取到yaw当前角度的服务.
 # Author: corvin
@@ -31,15 +36,19 @@
 #   20210321:增加可以设置D0-D4共4个引脚输出数字高低电平的
 #     服务配置参数pin_outHL_srv.增加获取yaw数据的服务.
 #   20210910:更改默认话题发布imu数据的频率为50hz.
+#   20211003:更新发布imu数据话题的默认频率为100hz;
+#     新增发布俯仰角数据的话题参数:pitch_pub_topic;
+#     新增发布横滚角数据的话题参数:roll_pub_topic;
 ###########################################################
 imu_dev: /dev/ttyAMA0
 baud_rate: 57600
-pub_topic_hz: 50
+pub_topic_hz: 100
 
 imu_link_name: base_imu_link
 pub_data_topic: imu_data
-pub_temp_topic: imu_temp
 yaw_pub_topic: yaw_data
+pitch_pub_topic: pitch_data
+roll_pub_topic: roll_data
 yaw_zero_topic: yaw_zero_topic
 yaw_zero_service: yaw_zero_service
 baud_update_srv: baud_update_service

serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -432,7 +432,7 @@ int getImuData(void)
         return -1;
     }
 
-    //printf("recv data len:%d\n", data_len); //一般读取都是44-45个字节
+    //printf("recv data len:%d\n", data_len); //一次性从串口中读取的总数据字节数
     for (int i=0; i<data_len; i++) //将读取到的数据进行解析
     {
         //printf("0x%02x ", r_buf[i]);

serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -17,9 +17,10 @@
  *     注意修改了波特率后,需要将IMU模块短开使用新波特率连接.
  *   20210321:增加可以设置D0,D1,D2,D3共4个IO引脚输出数字高低电平
  *     的服务,这里的高电平为3.3V.增加通过服务方式获取yaw数据.
+ *   20211003:增加发布pitch和roll数据的话题.
 *****************************************************************/
-#include<ros/ros.h>
-#include<tf/tf.h>
+#include <ros/ros.h>
+#include <tf/tf.h>
 #include <imu_data.h>
 #include <sensor_msgs/Imu.h>
 #include <std_msgs/Float32.h>
@@ -99,10 +100,11 @@ int main(int argc, char **argv)
     std::string imu_dev;
     int baud = 0;
 
-    std::string temp_topic;
     std::string imu_link_name;
     std::string imu_topic_name;
     std::string yaw_pub_topic;
+    std::string pitch_pub_topic;
+    std::string roll_pub_topic;
     std::string yaw_zero_topic;
     std::string yaw_zero_service;
     std::string baud_update_service;
@@ -122,15 +124,16 @@ int main(int argc, char **argv)
     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);
-    ros::param::get("~pub_temp_topic",   temp_topic);
     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("~pitch_pub_topic",  pitch_pub_topic);
+    ros::param::get("~roll_pub_topic",   roll_pub_topic);
     ros::param::get("~baud_update_srv",  baud_update_service);
     ros::param::get("~pin_outHL_srv",    pin_outHL_service);
     ros::param::get("~yaw_data_srv",     yaw_data_service);
 
-    //初始化imu模块串口,根据设备号和波特率进行连接
+    //初始化imu模块串口,根据设备号和波特率建立连接
     int ret = initSerialPort(imu_dev.c_str(), baud);
     if(ret < 0) //通过串口连接IMU模块失败
     {
@@ -147,13 +150,17 @@ int main(int argc, char **argv)
     ros::ServiceServer getYawSrv = handle.advertiseService(yaw_data_service,  getYawDataService);
 
     ros::Subscriber yawZeroSub = handle.subscribe(yaw_zero_topic, 1, yawZeroCallback);
-    ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>(imu_topic_name, 1);
-    ros::Publisher yaw_pub = handle.advertise<std_msgs::Float32>(yaw_pub_topic, 1);
+    ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>(imu_topic_name, 2);
+    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::Rate loop_rate(pub_topic_hz);
 
     sensor_msgs::Imu imu_msg;
     imu_msg.header.frame_id = imu_link_name;
     std_msgs::Float32 yaw_msg;
+    std_msgs::Float32 pitch_msg;
+    std_msgs::Float32 roll_msg;
     while(ros::ok())
     {
         if(getImuData() < 0)
@@ -167,9 +174,12 @@ int main(int argc, char **argv)
             yaw -= 6.2831852;
 
         g_yawData = yaw; //获取yaw值,可以通过服务来得到该值
-        //publish yaw data to topic
-        yaw_msg.data = yaw;
-        yaw_pub.publish(yaw_msg);
+        yaw_msg.data   = yaw;
+        pitch_msg.data = pitch;
+        roll_msg.data  = roll;
+        yaw_pub.publish(yaw_msg);     //将yaw值通过话题发布出去
+        pitch_pub.publish(pitch_msg); //将pitch值通过话题发布出去
+        roll_pub.publish(roll_msg);   //将roll值通过话题发布出去
 
         //ROS_INFO("yaw:%f pitch:%f roll:%f",yaw, pitch, roll);
         imu_msg.orientation.x = getQuat(1);