imu数据修改为通过串口获取，原来的iic因改用gd芯片需调时序

src/infrared_maze/src/infrared_maze.cpp

@@ -15,7 +15,7 @@
 #include <geometry_msgs/Twist.h>
 #include <tf/transform_listener.h>
 #include <cmath>
-#include "rasp_imu_hat_6dof/GetYawData.h"
+#include "serial_imu_hat_6dof/GetYawData.h"
 #include "ros_arduino_msgs/InfraredSensors.h"
 #include "ros_arduino_msgs/GetInfraredDistance.h"
 
@@ -36,7 +36,7 @@
 //global variable
 ros::Publisher twist_pub;
 ros::ServiceClient yawSrvClient;
-rasp_imu_hat_6dof::GetYawData yawSrv;
+serial_imu_hat_6dof::GetYawData yawSrv;
 
 //红外测距相关的服务
 ros::ServiceClient distSrvClient;
@@ -635,7 +635,7 @@ int main(int argc, char **argv)
     ros::param::get("~init_angular_speed", init_angular_speed);
 
     twist_pub = handle.advertise<geometry_msgs::Twist>(cmd_topic, 1);
-    yawSrvClient = handle.serviceClient<rasp_imu_hat_6dof::GetYawData>(yaw_service);
+    yawSrvClient = handle.serviceClient<serial_imu_hat_6dof::GetYawData>(yaw_service);
     distSrvClient = handle.serviceClient<ros_arduino_msgs::GetInfraredDistance>(infrared_service);
     
     if(init_switch){

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/CMakeLists.txt

@@ -11,6 +11,8 @@ find_package(catkin REQUIRED COMPONENTS
   dynamic_reconfigure
   roscpp
   sensor_msgs
+  std_msgs
+  message_generation
 )
 
 ## System dependencies are found with CMake's conventions
@@ -54,11 +56,10 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+  FILES
+  GetYawData.srv
+)
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -68,10 +69,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 ##
@@ -143,7 +145,7 @@ add_executable(serial_imu_node src/proc_serial_data.cpp src/serial_imu_node.cpp)
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(serial_imu_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(serial_imu_node

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/cfg/param.yaml

@@ -28,5 +28,7 @@ pub_data_topic: imu_data
 pub_temp_topic: imu_temp
 yaw_topic: yaw_data
 link_name: base_imu_link
-pub_hz: 10
+pub_hz: 120
+
+yaw_service: /imu_node/GetYawData
 

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/package.xml

@@ -49,15 +49,27 @@
   <!-- 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>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>message_runtime</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>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>message_generation</build_export_depend>
+  <build_export_depend>message_runtime</build_export_depend>
+
   <exec_depend>dynamic_reconfigure</exec_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_generation</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -190,8 +190,9 @@ void parse_serialData(char chr)
     static unsigned char chrCnt = 0;
     signed short sData[4]; //save 8 Byte valid info
     unsigned char i = 0;
-    char frameSum = 0;
+    unsigned char frameSum = 0;
 
+    //printf("%d \n",chrCnt);
     chrBuf[chrCnt++] = chr; //保存当前字节,字节计数加1
 
     //判断是否满一个完整数据帧11个字节
@@ -204,6 +205,12 @@ void parse_serialData(char chr)
         frameSum += chrBuf[i];
     }
 
+    //for(i=0;i<11; i++)
+      //ROS_INFO("0x%02x ",chrBuf[i]);
+      //printf("0x%02x ",chrBuf[i]);
+    //printf("\n");
+    //printf("sum is 0x%02x\n",frameSum);
+
     //找到数据帧第一个字节是0x55,校验和是否正确,若两者有一个不正确,
     //都需要移动到最开始的字节,再读取新的字节进行判断数据帧完整性
     if ((chrBuf[0] != 0x55)||(frameSum != chrBuf[10]))
@@ -214,7 +221,7 @@ void parse_serialData(char chr)
     }
 
     //for(i=0;i<11; i++)
-    //  printf("0x%x ",chrBuf[i]);
+    //  printf("0x%02x ",chrBuf[i]);
     //printf("\n");
 
     memcpy(&sData[0], &chrBuf[2], 8);

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -15,8 +15,13 @@
 #include "imu_data.h"
 #include <sensor_msgs/Imu.h>
 #include <std_msgs/Float32.h>
+#include "serial_imu_hat_6dof/GetYawData.h"
 
-
+float response_yaw;
+bool return_yaw_data(serial_imu_hat_6dof::GetYawData::Request &req,serial_imu_hat_6dof::GetYawData::Response &res){
+    res.yaw = response_yaw;
+    return true;
+}
 int main(int argc, char **argv)
 {
     float yaw, pitch, roll;
@@ -31,6 +36,7 @@ int main(int argc, char **argv)
     std::string temp_topic;
     std::string yaw_topic;
     int pub_hz = 0;
+    std::string yaw_service;
     float degree2Rad = 3.1415926/180.0;
     float acc_factor = 9.806;
 
@@ -49,6 +55,10 @@ int main(int argc, char **argv)
     ros::param::get("~yaw_topic", yaw_topic);
     ros::param::get("~pub_hz", pub_hz);
 
+    ros::param::get("~yaw_service", yaw_service);
+
+
+
     int ret = initSerialPort(imu_dev.c_str(), baud, dataBit, parity.c_str(), stopBit);
     if(ret < 0)
     {
@@ -60,6 +70,7 @@ int main(int argc, char **argv)
 
     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::ServiceServer yaw_srv = handle.advertiseService(yaw_service, return_yaw_data);
     ros::Rate loop_rate(pub_hz);
 
     sensor_msgs::Imu imu_msg;
@@ -77,6 +88,7 @@ int main(int argc, char **argv)
         if(yaw >= 3.1415926)
             yaw -= 6.2831852;
 
+        response_yaw = yaw;
         //publish yaw data
         yaw_msg.data = yaw;
         yaw_pub.publish(yaw_msg);

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/srv/GetYawData.srv

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

src/robot_bringup/launch/robot_bringup.launch

@@ -20,8 +20,8 @@
     <!-- (2) startup mobilebase arduino launch -->
     <include file="$(find ros_arduino_python)/launch/arduino.launch" />
 
-    <!-- (3) startup rasp imu-6DOF board-->
-    <include file="$(find rasp_imu_hat_6dof)/launch/imu_data_pub.launch" />
+    <!-- (3) startup serial imu-6DOF board-->
+    <include file="$(find serial_imu_hat_6dof)/launch/serial_imu_hat.launch" />
 
     <!-- (4) startup robot_pose_ekf node -->
     <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">

src/robot_cube/src/robot_cube_node.cpp

@@ -1,7 +1,7 @@
 /*
  * @Author: adam_zhuo
  * @Date: 2020-12-30 10:32:32
- * @LastEditTime: 2020-12-31 19:00:46
+ * @LastEditTime: 2021-03-09 15:03:14
  * @LastEditors: Please set LastEditors
  * @Description: grip cube
  * @FilePath: /blackTornadoRobot/src/robot_cube/src/robot_cube_node.cpp
@@ -16,7 +16,7 @@
 #include <nav_msgs/Odometry.h>
 #include <tf/tf.h>
 #include <tf/transform_listener.h>
-#include "rasp_imu_hat_6dof/GetYawData.h"
+#include "serial_imu_hat_6dof/GetYawData.h"
 #include "ros_arduino_msgs/GripperAngle.h"
 
 using namespace std;
@@ -38,7 +38,7 @@ ros::Subscriber marker_sub;
 tf::Quaternion quat;
 ros::ServiceClient yaw_srv_client;
 ros::ServiceClient gripper_srv_client;
-rasp_imu_hat_6dof::GetYawData yaw_srv;
+serial_imu_hat_6dof::GetYawData yaw_srv;
 ros_arduino_msgs::GripperAngle gripper_srv;
 
 void pub_twist_cmd(float linear_x, float linear_y, float angular_z);
@@ -253,7 +253,7 @@ int main(int argc, char** argv){
 
     cmd_pub    = nh.advertise<geometry_msgs::Twist>(cmd_topic, 1);
     marker_sub = nh.subscribe(marker_topic, 1, rcv_marker_callback);
-    yaw_srv_client = nh.serviceClient<rasp_imu_hat_6dof::GetYawData>(yaw_service);
+    yaw_srv_client = nh.serviceClient<serial_imu_hat_6dof::GetYawData>(yaw_service);
     gripper_srv_client = nh.serviceClient<ros_arduino_msgs::GripperAngle>(gripper_service);
 
     thread spin_thread(run_spin_thread);

src/robot_dynamic/src/robot_dynamic.cpp

@@ -13,7 +13,7 @@
 #include <tf/transform_listener.h>
 #include <dynamic_reconfigure/server.h>
 #include <robot_dynamic/dynamicConfig.h>
-#include "rasp_imu_hat_6dof/GetYawData.h"
+#include "serial_imu_hat_6dof/GetYawData.h"
  
 using namespace std;
 
@@ -25,7 +25,7 @@ string yaw_service = "/imu_node/GetYawData";
 
 ros::Publisher twist_pub;
 ros::ServiceClient yawSrvClient;
-rasp_imu_hat_6dof::GetYawData yawSrv;
+serial_imu_hat_6dof::GetYawData yawSrv;
 
 string odomFrame = "/odom_combined";
 string baseFrame = "/base_footprint";
@@ -210,7 +210,7 @@ int main(int argc, char **argv)
     server.setCallback(callback);
  
     twist_pub = handle.advertise<geometry_msgs::Twist>(cmd_topic, 1);
-    yawSrvClient = handle.serviceClient<rasp_imu_hat_6dof::GetYawData>(yaw_service);
+    yawSrvClient = handle.serviceClient<serial_imu_hat_6dof::GetYawData>(yaw_service);
     yawSrvClient.call(yawSrv);
     start_yaw = yawSrv.response.yaw;
     ROS_INFO("start_yaw is %f",start_yaw);

src/robot_new_year/src/robot_new_year_node.cpp

@@ -1,7 +1,7 @@
 /*
  * @Author: adam_zhuo
  * @Date: 2021-01-21 10:13:20
- * @LastEditTime: 2021-01-22 15:23:19
+ * @LastEditTime: 2021-03-09 15:07:45
  * @LastEditors: Please set LastEditors
  * @Description: In User Settings Edit
  * @FilePath: /blackTornadoRobot/src/robot_new_year/src/robot_new_year_node.cpp
@@ -14,7 +14,7 @@
 #include <nav_msgs/Odometry.h>
 #include <tf/tf.h>
 #include <tf/transform_listener.h>
-#include "rasp_imu_hat_6dof/GetYawData.h"
+#include "serial_imu_hat_6dof/GetYawData.h"
 #include "ros_arduino_msgs/GripperAngle.h"
 #include "ros_arduino_msgs/GripperControl.h"
 
@@ -31,7 +31,7 @@ ros::Publisher voice_pub;
 ros::ServiceClient yaw_srv_client;
 ros::ServiceClient gripper_srv_client;
 ros::ServiceClient control_srv_client;
-rasp_imu_hat_6dof::GetYawData yaw_srv;
+serial_imu_hat_6dof::GetYawData yaw_srv;
 ros_arduino_msgs::GripperAngle gripper_srv;
 ros_arduino_msgs::GripperControl control_srv;
 
@@ -261,7 +261,7 @@ int main(int argc, char** argv){
     
     cmd_pub    = nh.advertise<geometry_msgs::Twist>(cmd_topic, 1);
     voice_pub = nh.advertise<std_msgs::String>(voice_topic, 1);
-    yaw_srv_client = nh.serviceClient<rasp_imu_hat_6dof::GetYawData>(yaw_service);
+    yaw_srv_client = nh.serviceClient<serial_imu_hat_6dof::GetYawData>(yaw_service);
     gripper_srv_client = nh.serviceClient<ros_arduino_msgs::GripperAngle>(gripper_service);
     control_srv_client = nh.serviceClient<ros_arduino_msgs::GripperControl>(control_service);
 

src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -13,7 +13,7 @@
 is_use_serial: True
 
 # /dev/ttyACM# where is # is a number such as 0, 1 etc
-serial_port: /dev/ttyS0
+serial_port: /dev/ttyACM0
 serial_baud: 57600
 
 #raspberryPi:1, huawei altals 200DK: 2

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/base_controller.py

@@ -23,7 +23,7 @@ from nav_msgs.msg import Odometry
 from tf.broadcaster import TransformBroadcaster
 from std_msgs.msg import Int32
 from std_msgs.msg import Float32
-from rasp_imu_hat_6dof.srv import GetYawData
+from serial_imu_hat_6dof.srv import GetYawData
 
 class SimplePID:
 	'''very simple discrete PID controller'''