增加可以通过服务调用方式获取到当前的yaw角度

serial_imu_hat_6dof/CMakeLists.txt

@@ -52,6 +52,7 @@ add_service_files(
   setYawZero.srv
   setBaudRate.srv
   setPinOutHL.srv
+  getYawData.srv
 )
 
 ## Generate added messages and services with any dependencies listed here
@@ -144,13 +145,6 @@ target_link_libraries(serial_imu_node
 # all install targets should use catkin DESTINATION variables
 # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
 
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
 ## Mark executables for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
 # install(TARGETS ${PROJECT_NAME}_node

serial_imu_hat_6dof/cfg/param.yaml

@@ -1,7 +1,8 @@
 ###########################################################
 # Copyright: 2016-2021 www.corvin.cn ROS小课堂
 # Description:使用串口与IMU模块进行通信时的配置信息.
-# 可以配置的各参数如下:
+# 可以配置的各参数如下,这些话题名或者服务名,发布频率这些都
+# 可以根据需要来修改:
 #  imu_dev:串口设备在linux系统的挂载点,如果使用usb转串口
 #     模块进行通信,都是写ttyUSB0,1,2等.如果使用树莓派的
 #     GPIO排针处串口,则使用ttyAMA0.需要注意是这个ttyAMA0
@@ -16,6 +17,8 @@
 #  pub_hz:话题发布的数据频率,默认设置为30Hz.
 #  yaw_zero_topic:通过往该话题中发消息即可将yaw角度归零.
 #  yaw_pub_topic:将yaw信息通过该话题发送出来.
+#  pin_outHL_srv:控制IMU板上D0-D4的输出电平高低的服务.
+#  yaw_data_srv:可以获取到yaw当前角度的服务.
 # Author: corvin
 # History:
 #   20200402:init this file.
@@ -26,7 +29,7 @@
 #     增加使用服务方式将yaw归零的名称yaw_zero_service.
 #   20210319:增加修改串口波特率的配置参数baud_update_srv.
 #   20210321:增加可以设置D0-D4共4个引脚输出数字高低电平的
-#     服务配置参数pin_outHL_srv.
+#     服务配置参数pin_outHL_srv.增加获取yaw数据的服务.
 ###########################################################
 imu_dev: /dev/ttyAMA0
 baud_rate: 57600
@@ -40,3 +43,4 @@ yaw_zero_topic: yaw_zero_topic
 yaw_zero_service: yaw_zero_service
 baud_update_srv: baud_update_service
 pin_outHL_srv: pin_outHL_service
+yaw_data_srv: /imu_node/GetYawData

serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -1,4 +1,4 @@
-/****************************************************************
+/*****************************************************************
  * Copyright:2016-2021 www.corvin.cn ROS小课堂
  * Description:使用串口来读取IMU模块的数据,并通过ROS话题topic将
  *   数据发布出来.芯片中默认读取出来的加速度数据单位是g,需要将其
@@ -16,8 +16,8 @@
  *   20210319:增加使用service方式可以修改imu模块串口通信波特率.
  *     注意修改了波特率后,需要将IMU模块短开使用新波特率连接.
  *   20210321:增加可以设置D0,D1,D2,D3共4个IO引脚输出数字高低电平
- *     的服务,这里的高电平为3.3V.
-****************************************************************/
+ *     的服务,这里的高电平为3.3V.增加通过服务方式获取yaw数据.
+*****************************************************************/
 #include <ros/ros.h>
 #include <tf/tf.h>
 #include <imu_data.h>
@@ -27,8 +27,11 @@
 #include "serial_imu_hat_6dof/setYawZero.h"
 #include "serial_imu_hat_6dof/setBaudRate.h"
 #include "serial_imu_hat_6dof/setPinOutHL.h"
+#include "serial_imu_hat_6dof/getYawData.h"
 
 
+static float g_yawData; //全局变量,存储当前yaw值,可以通过服务来得到该值
+
 /**********************************************************
  * Description:将yaw角度归零的话题回调函数,只要往话题中
  *   发布一条Empty消息,即可将yaw角度归零.
@@ -80,6 +83,13 @@ bool pinOutHLService(serial_imu_hat_6dof::setPinOutHL::Request &req,
     return true;
 }
 
+bool getYawDataService(serial_imu_hat_6dof::getYawData::Request &req,
+                       serial_imu_hat_6dof::getYawData::Response &res)
+{
+    res.yaw = g_yawData;
+    return true;
+}
+
 int main(int argc, char **argv)
 {
     float yaw, pitch, roll;
@@ -94,6 +104,7 @@ int main(int argc, char **argv)
     std::string yaw_zero_service;
     std::string baud_update_service;
     std::string pin_outHL_service;
+    std::string yaw_data_service;
 
     int pub_hz = 0;
     float degree2Rad = 3.1415926/180.0;
@@ -107,13 +118,14 @@ int main(int argc, char **argv)
     ros::param::get("~baud_rate", baud);
     ros::param::get("~pub_hz",    pub_hz);
     ros::param::get("~link_name", link_name);
-    ros::param::get("~pub_data_topic", imu_topic);
-    ros::param::get("~pub_temp_topic", temp_topic);
-    ros::param::get("~yaw_zero_topic", yaw_zero_topic);
+    ros::param::get("~pub_data_topic",   imu_topic);
+    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("~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模块串口,根据设备号和波特率进行连接
     int ret = initSerialPort(imu_dev.c_str(), baud);
@@ -125,12 +137,13 @@ int main(int argc, char **argv)
     }
     ROS_INFO("IMU module is working...");
 
-    //定义三个服务,分别是更新波特率,yaw角度归零和控制引脚输出高低电平
-    ros::ServiceServer baudSrv = handle.advertiseService(baud_update_service, setBaudService);
-    ros::ServiceServer yawSrv  = handle.advertiseService(yaw_zero_service, yawZeroService);
-    ros::ServiceServer pinSrv  = handle.advertiseService(pin_outHL_service, pinOutHLService);
+    //定义四个服务,分别是更新波特率,yaw角度归零,控制引脚输出高低电平和获取yaw当前角度
+    ros::ServiceServer setBaudSrv= handle.advertiseService(baud_update_service, setBaudService);
+    ros::ServiceServer setyawSrv = handle.advertiseService(yaw_zero_service,  yawZeroService);
+    ros::ServiceServer pinOutSrv = handle.advertiseService(pin_outHL_service, pinOutHLService);
+    ros::ServiceServer getYawSrv = handle.advertiseService(yaw_data_service,  getYawDataService);
 
-    ros::Subscriber yaw_zero_sub = handle.subscribe(yaw_zero_topic, 1, yawZeroCallback);
+    ros::Subscriber yawZeroSub = handle.subscribe(yaw_zero_topic, 1, yawZeroCallback);
     ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>(imu_topic, 1);
     ros::Publisher yaw_pub = handle.advertise<std_msgs::Float32>(yaw_pub_topic, 1);
     ros::Rate loop_rate(pub_hz);
@@ -150,6 +163,7 @@ int main(int argc, char **argv)
         if(yaw >= 3.1415926)
             yaw -= 6.2831852;
 
+        g_yawData = yaw; //获取yaw值,可以通过服务来得到该值
         //publish yaw data to topic
         yaw_msg.data = yaw;
         yaw_pub.publish(yaw_msg);

serial_imu_hat_6dof/srv/getYawData.srv

@@ -0,0 +1,2 @@
+---
+float32 yaw