增加使用服务的方式来进行yaw角度归零

serial_imu_hat_6dof/CMakeLists.txt

@@ -11,12 +11,13 @@ find_package(catkin REQUIRED COMPONENTS
   dynamic_reconfigure
   roscpp
   sensor_msgs
+  std_msgs
+  message_generation
 )
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 
-
 ################################################
 ## Declare ROS messages, services and actions ##
 ################################################
@@ -49,11 +50,10 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+  FILES
+  setYawZero.srv
+)
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -63,10 +63,11 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   sensor_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  sensor_msgs
+  std_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##

serial_imu_hat_6dof/cfg/param.yaml

@@ -4,7 +4,10 @@
 # 的各参数如下:
 #   imu_dev:指示串口设备挂在点,如果使用usb转串口
 #     模块,都是写ttyUSB0,1,2等.如果使用树莓派的
-#     GPIO排针处串口,则使用ttyS0.
+#     GPIO排针处串口,则使用ttyAMA0.需要注意是这个ttyAMA0
+#     默认是给树莓派蓝牙使用的,所以要想使用该硬件串口,
+#     就需要在raspi-config中关闭串口登录功能,打开硬件
+#     串口功能,这同时也需要将树莓派的蓝牙给禁用掉.
 #   baud_rate:串口通信波特率,旧模块的波特率为9600,
 #     新版本的波特率为57600,如果串口没数据可以修改.
 #   pub_data_topic:发布imu数据的ROS话题名.
@@ -12,6 +15,7 @@
 #   link_name:imu模块的link名.
 #   pub_hz:话题发布的数据频率,默认设置为20Hz.
 #   yaw_zero_topic:通过往该话题中发消息即可将yaw角度归零.
+#   yaw_pub_topic:将yaw信息通过该话题发送出来.
 # Author: corvin
 # History:
 #   20200402:init this file.
@@ -19,14 +23,16 @@
 #   20210317:去掉数据位,奇偶校验,停止位参数,这几个
 #     参数模块已经固定死了,无需修改.
 #   20200318:增加yaw角度归零的话题配置参数yaw_zero_topic.
+#     增加使用服务方式将yaw归零的名称yaw_zero_service.
 #########################################################
-imu_dev: /dev/ttyS0
+imu_dev: /dev/ttyAMA0
 baud_rate: 57600
 
 pub_data_topic: imu_data
 pub_temp_topic: imu_temp
-yaw_topic: yaw_data
-yaw_zero_topic: yaw_zero
+yaw_pub_topic: yaw_data
+yaw_zero_topic: yaw_zero_topic
+yaw_zero_service: yaw_zero_service
 link_name: base_imu_link
 pub_hz: 20
 

serial_imu_hat_6dof/package.xml

@@ -1,13 +1,11 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>serial_imu_hat_6dof</name>
-  <version>0.0.0</version>
+  <version>0.0.1</version>
   <description>The serial_imu_hat_6dof package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="corvin_zhang@corvin.cn">corvin</maintainer>
+  <maintainer email="corvin_zhang@corvin.cn">corvin_zhang</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->
@@ -49,20 +47,23 @@
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
+
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>sensor_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+
   <build_export_depend>dynamic_reconfigure</build_export_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>sensor_msgs</build_export_depend>
+
   <exec_depend>dynamic_reconfigure</exec_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
-
+  <exec_depend>message_runtime</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
-
   </export>
 </package>

serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -19,6 +19,7 @@
 #include <sensor_msgs/Imu.h>
 #include <std_msgs/Float32.h>
 #include <std_msgs/Empty.h>
+#include "serial_imu_hat_6dof/setYawZero.h"
 
 
 /*********************************************************
@@ -30,6 +31,19 @@ void yawZeroCallback(const std_msgs::Empty::ConstPtr& msg)
     makeYawZero();
 }
 
+/*****************************************************************
+ * Description:使用service方式来进行yaw角度归零,这里是服务的回调
+ *   函数,当有客户端发送yaw归零的服务时,自动调用该函数,如果正确
+ *   执行了yaw归零,则response反馈为0,如果为其他负数则表明yaw归零
+ *   命令执行失败.
+ *****************************************************************/
+bool yawZeroService(serial_imu_hat_6dof::setYawZero::Request &req,
+                    serial_imu_hat_6dof::setYawZero::Response &res)
+{
+    res.status = makeYawZero();
+    return true;
+}
+
 int main(int argc, char **argv)
 {
     float yaw, pitch, roll;
@@ -39,8 +53,9 @@ int main(int argc, char **argv)
     std::string link_name;
     std::string imu_topic;
     std::string temp_topic;
-    std::string yaw_topic;
+    std::string yaw_pub_topic;
     std::string yaw_zero_topic;
+    std::string yaw_zero_service;
 
     int pub_hz = 0;
     float degree2Rad = 3.1415926/180.0;
@@ -56,7 +71,8 @@ int main(int argc, char **argv)
     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_topic", yaw_topic);
+    ros::param::get("~yaw_zero_service", yaw_zero_service);
+    ros::param::get("~yaw_pub_topic",  yaw_pub_topic);
     ros::param::get("~pub_hz",    pub_hz);
 
     //初始化imu模块串口,根据设备号和波特率进行连接
@@ -69,9 +85,10 @@ int main(int argc, char **argv)
     }
     ROS_INFO("IMU module is working... ");
 
+    ros::ServiceServer service = handle.advertiseService(yaw_zero_service, yawZeroService);
     ros::Subscriber yaw_zero_sub = 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_topic, 1);
+    ros::Publisher yaw_pub = handle.advertise<std_msgs::Float32>(yaw_pub_topic, 1);
     ros::Rate loop_rate(pub_hz);
 
     sensor_msgs::Imu imu_msg;

serial_imu_hat_6dof/srv/setYawZero.srv

@@ -0,0 +1,2 @@
+---
+int32 status