当前位置：首页 > news >正文

ROS主控和stm32小车底盘通过串口进行通讯

news 2025/8/2 8:31:05

ROS主控和stm32小车底盘通过串口进行通讯(ROS主控为第一人称)

在这个双向通信中,我们首先需要了解一些背景信息:

1.数据流向(下发什么信息,此信息是处理后的信息; 接收什么信息,这个信息是原始信息;) ,然后我们创建一个ros1工程,来实现收发信息,并将信息打印出来.

2.信息处理(接收到的原始信息首先要做的就是数据的解析,哪一部分代表什么,然后这些信息是否还需要额外处理才可以进行使用,然后如何使用这些信息等等; 下发的信息如何打包成符合串口通信格式的,然后在去下发下去),然后在第一个ros1工程的基础上,实现ros主控和stm32底盘的串口通信

一.数据流向

(1)下发信息

🚙 主控（ROS） → 串口 → 发给stm32底盘：
这是控制指令，包括：

内容	来源	含义
线速度、角速度	如:ROS 的 `/cmd_vel`	机器人想要怎么动
控制标志（是否使能）	ROS 控制程序内部设定	电机启动/停止、蜂鸣器、灯控等（看协议）

👉 这些是 ROS 主控通过串口发送给底盘控制板的指令。

(2)反馈信息

⚡ stm32底盘 → 串口 → 发给主控（ROS）：
这是底盘的反馈信息，包括：

内容	含义
里程计信息	编码器累计值/位移/速度，用于估算里程计位姿
IMU 数据	加速度、角速度（如果底盘集成了 IMU）
电池电压	监控供电情况
错误状态	比如急停状态、电机故障标志、通信错误等

👉 这些是底盘通过串口发送给 ROS 主控的“反馈数据”。

(3) ros1工程实现步骤

首先ros1环境要有的,可能还需要安装一些功能包.本教程只演示一些必备的命令和代码.

1. 创建工作空间

mkdir ~/catkin_ws/src
cd ~/catkin_ws
catkin_make

2. 创建功能包

cd ~/catkin_ws/src
catkin_create_pkg ros_stm_serial std_msgs geometry_msgs roscpp serial

3.创建程序代码(c++)

 cd ~/catkin_ws/src/ros_stm_serial/touch ros_stm_serial.cpp

ros_stm_serial.cpp代码如下:

#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>serial::Serial ser;  // 创建串口对象int main(int argc, char** argv)
{ros::init(argc, argv, "serial_node");  // 初始化 ROS 节点ros::NodeHandle nh;// 创建一个发布器，用于发布从串口读取的数据ros::Publisher read_pub = nh.advertise<std_msgs::String>("serial/read", 1000);try{ser.setPort("/dev/ttyUSB0");           // 设置串口端口，根据你的实际串口名称修改ser.setBaudrate(115200);               // 设置波特率serial::Timeout to = serial::Timeout::simpleTimeout(1000);  // 设置超时时间ser.setTimeout(to);ser.open();                             // 打开串口}catch (serial::IOException& e){ROS_ERROR_STREAM("Unable to open port.");  // 打开失败return -1;}if (ser.isOpen()){ROS_INFO_STREAM("Serial port initialized.");  // 串口打开成功}else{return -1;  // 串口打开失败}ros::Rate loop_rate(10);  // 主循环频率 10Hzwhile (ros::ok()){// 如果串口中有可读数据if (ser.available()){std_msgs::String msg;msg.data = ser.read(ser.available());  // 一次性读取全部数据ROS_INFO_STREAM("Read from serial: " << msg.data);  // 控制台输出读取内容read_pub.publish(msg);  // 发布到话题 serial/read}// 写入固定字符串到串口std::string hello = "hello ros\n";ser.write(hello);  // 发送字符串给下位机ros::spinOnce();loop_rate.sleep();}return 0;
}

4.修改CMakeLists.txt

add_executable(serial_node src/serial_node.cpp)
target_link_libraries(serial_node ${catkin_LIBRARIES})find_package(catkin REQUIRED COMPONENTSroscppstd_msgsserial
)

5.编译工程

cd ~/catkin_ws
catkin_make

6.运行

一个终端启动roscore

roscore

另开一个终端

source ~/catkin_ws/devel/setup.bash
rosrun serial_test serial_node

在开一个终端,查看读到的数据(待完善)

rostopic echo /serial/read

7.测试(待完善)

此部分是要对刚刚编写的程序进行一个验证,确保其达到预期效果.

二信息处理(待完善)

此部分涉及到大量信息的处理,如下:
A: 串口数据解析:
读取的串口进行解析,分发到不同种类的数据上,进行单独处理.
a:里程计原始数据(X,Y,Z三个方向的速度):首先进行一个修正(乘以一个修正系数,一般为1.0);
然后根据时间差计算一下位移(速度*时间=位移（里程计）);
b:IMU的加速度(xyz),角速度(xyz): 首先原始数据要进行一个量程计算,恢复到准确的单位;
然后将这六个信息放到四元数计算(待解读),计算得出三轴姿态信息.
c:电池电压(Power_voltage):单位转换 -毫伏(mv)->伏(v)
B:话题发布
将A中处理好的数据,进行话题的发布
a:将计算出的位置(XYZ),姿态(四元数), 速度(X Y, Wz) 装填到话题odom中,并进行发布
b:将处理好的imu数据:四元数表达三轴姿态(xyzw),三轴姿态协方差矩阵;三轴角速度(xyz),
三轴角速度协方差矩阵;三轴线性加速度(xyz)装填到imu的话题中,并进行发布
c:发布电源电压话题, 单位：V、伏特
C:控制信号下发:目标速度(xy线速度,z是角速度),装填并通过串口进行发送给stm32.

1.串口数据解析

这个部分主要分为三个部分:里程计数据,IMU数据,电池电压数据,也有公共的部分:
读取串口数据并进行数据的分割.
此部分是获取底盘传感器数据,并将他划分给不同的变量,这个需要参考底盘的串口协议.确定哪一部分是什么含义,然后创建相关的变量进行后续处理.
此代码为轮趣代码,侵权删. 后面有时间会重构此代码

bool turn_on_robot::Get_Sensor_Data_New()//创建一个bool型的,成员函数:获取传感器最新数据
{short transition_16=0; //Intermediate variable //设置的中间变量uint8_t i=0,check=0, error=1,Receive_Data_Pr[1]; //Temporary variable to save the data of the lower machine //临时变量，保存下位机stm32的数据static int count; //Static variable for counting //静态变量，用于计数Stm32_Serial.read(Receive_Data_Pr,sizeof(Receive_Data_Pr)); //Read the data sent by the lower computer through the serial port //通过串口读取下位机发送过来的数据Receive_Data.rx[count] = Receive_Data_Pr[0]; //Fill the array with serial data //串口数据填入数组Receive_Data.Frame_Header = Receive_Data.rx[0]; //The first part of the data is the frame header 0X7B //数据的第一位是帧头0X7BReceive_Data.Frame_Tail = Receive_Data.rx[23];  //The last bit of data is frame tail 0X7D //数据的最后一位是帧尾0X7Dif(Receive_Data_Pr[0] == FRAME_HEADER || count>0) //Ensure that the first data in the array is FRAME_HEADER //确保数组第一个数据为FRAME_HEADERcount++;else count=0;if(count == 24) //Verify the length of the packet //验证数据包的长度{count=0;  //Prepare for the serial port data to be refill into the array //为串口数据重新填入数组做准备if(Receive_Data.Frame_Tail == FRAME_TAIL) //Verify the frame tail of the packet //验证数据包的帧尾{check=Check_Sum(22,READ_DATA_CHECK);  //BCC check passes or two packets are interlaced //BCC校验通过或者两组数据包交错if(check == Receive_Data.rx[22])  {error=0;  //XOR bit check successful //异或位校验成功}if(error == 0){/*//Check receive_data.rx for debugging use //查看Receive_Data.rx，调试使用 ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x",Receive_Data.rx[0],Receive_Data.rx[1],Receive_Data.rx[2],Receive_Data.rx[3],Receive_Data.rx[4],Receive_Data.rx[5],Receive_Data.rx[6],Receive_Data.rx[7],Receive_Data.rx[8],Receive_Data.rx[9],Receive_Data.rx[10],Receive_Data.rx[11],Receive_Data.rx[12],Receive_Data.rx[13],Receive_Data.rx[14],Receive_Data.rx[15],Receive_Data.rx[16],Receive_Data.rx[17],Receive_Data.rx[18],Receive_Data.rx[19],Receive_Data.rx[20],Receive_Data.rx[21],Receive_Data.rx[22],Receive_Data.rx[23]); */Receive_Data.Flag_Stop=Receive_Data.rx[1]; //set aside //预留位Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]); //Get the speed of the moving chassis in the X direction //获取运动底盘X方向速度Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); //Get the speed of the moving chassis in the Y direction, The Y speed is only valid in the omnidirectional mobile robot chassis//获取运动底盘Y方向速度，Y速度仅在全向移动机器人底盘有效Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); //Get the speed of the moving chassis in the Z direction //获取运动底盘Z方向速度   //MPU6050 stands for IMU only and does not refer to a specific model. It can be either MPU6050 or MPU9250//Mpu6050仅代表IMU，不指代特定型号，既可以是MPU6050也可以是MPU9250Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]);   //Get the X-axis acceleration of the IMU     //获取IMU的X轴加速度  Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]); //Get the Y-axis acceleration of the IMU     //获取IMU的Y轴加速度Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]); //Get the Z-axis acceleration of the IMU     //获取IMU的Z轴加速度Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]);  //Get the X-axis angular velocity of the IMU //获取IMU的X轴角速度  Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]);  //Get the Y-axis angular velocity of the IMU //获取IMU的Y轴角速度  Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]);  //Get the Z-axis angular velocity of the IMU //获取IMU的Z轴角速度  //Linear acceleration unit conversion is related to the range of IMU initialization of STM32, where the range is ±2g=19.6m/s^2//线性加速度单位转化，和STM32的IMU初始化的时候的量程有关,这里量程±2g=19.6m/s^2Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO;Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO;Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO;//The gyroscope unit conversion is related to the range of STM32's IMU when initialized. Here, the range of IMU's gyroscope is ±500°/s//Because the robot generally has a slow Z-axis speed, reducing the range can improve the accuracy//陀螺仪单位转化，和STM32的IMU初始化的时候的量程有关，这里IMU的陀螺仪的量程是±500°/s//因为机器人一般Z轴速度不快，降低量程可以提高精度Mpu6050.angular_velocity.x =  Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;Mpu6050.angular_velocity.y =  Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;Mpu6050.angular_velocity.z =  Mpu6050_Data.gyros_z_data * GYROSCOPE_RATIO;//Get the battery voltage//获取电池电压transition_16 = 0;transition_16 |=  Receive_Data.rx[20]<<8;transition_16 |=  Receive_Data.rx[21];  Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001; //Unit conversion millivolt(mv)->volt(v) //单位转换毫伏(mv)->伏(v)return true;}}}return false;
}

(1)里程计数据解析

(2)IMU数据解析

(3) 电池电压数据解析

2.解析后的数据进行话题发布

这个部分主要是为了后面一些算法调用原始的数据进行后续的处理,所以需要将处理好的数据进行话题的发布.

(1)处理后的里程计数据-话题发布

(2)处理后的IMU数据-话题发布

(3) 处理后的电池电压数据-话题发布

3.下发指令的串口发送

三完整代码(待重构)

此部分为轮趣代码,侵权删.

#include "wheeltec_robot.h"
#include "Quaternion_Solution.h"sensor_msgs::Imu Mpu6050;//Instantiate an IMU object //实例化IMU对象 /**************************************
Date: January 28, 2021
Function: The main function, ROS initialization, creates the Robot_control object through the Turn_on_robot class and automatically calls the constructor initialization
功能: 主函数，ROS初始化，通过turn_on_robot类创建Robot_control对象并自动调用构造函数初始化
***************************************/
int main(int argc, char** argv)
{ros::init(argc, argv, "wheeltec_robot"); //ROS initializes and sets the node name //ROS初始化 并设置节点名称 turn_on_robot Robot_Control; //Instantiate an object //实例化一个对象Robot_Control.Control(); //Loop through data collection and publish the topic //循环执行数据采集和发布话题等操作return 0;  
} /**************************************
Date: January 28, 2021
Function: Data conversion function
功能: 数据转换函数
***************************************/
short turn_on_robot::IMU_Trans(uint8_t Data_High,uint8_t Data_Low)
{short transition_16;transition_16 = 0;transition_16 |=  Data_High<<8;   transition_16 |=  Data_Low;return transition_16;     
}
float turn_on_robot::Odom_Trans(uint8_t Data_High,uint8_t Data_Low)
{float data_return;short transition_16;transition_16 = 0;transition_16 |=  Data_High<<8;  //Get the high 8 bits of data   //获取数据的高8位transition_16 |=  Data_Low;      //Get the lowest 8 bits of data //获取数据的低8位data_return   =  (transition_16 / 1000)+(transition_16 % 1000)*0.001; // The speed unit is changed from mm/s to m/s //速度单位从mm/s转换为m/sreturn data_return;
}
/**************************************
Date: January 28, 2021
Function: The speed topic subscription Callback function, according to the subscribed instructions through the serial port command control of the lower computer
功能: 速度话题订阅回调函数Callback，根据订阅的指令通过串口发指令控制下位机
***************************************/
void turn_on_robot::Cmd_Vel_Callback(const geometry_msgs::Twist &twist_aux)
{short  transition;  //intermediate variable //中间变量Send_Data.tx[0]=FRAME_HEADER; //frame head 0x7B //帧头0X7BSend_Data.tx[1] = 0; //set aside //预留位Send_Data.tx[2] = 0; //set aside //预留位//The target velocity of the X-axis of the robot//机器人x轴的目标线速度transition=0;transition = twist_aux.linear.x*1000; //将浮点数放大一千倍，简化传输Send_Data.tx[4] = transition;     //取数据的低8位Send_Data.tx[3] = transition>>8;  //取数据的高8位//The target velocity of the Y-axis of the robot//机器人y轴的目标线速度transition=0;transition = twist_aux.linear.y*1000;Send_Data.tx[6] = transition;Send_Data.tx[5] = transition>>8;//The target angular velocity of the robot's Z axis//机器人z轴的目标角速度transition=0;transition = twist_aux.angular.z*1000;Send_Data.tx[8] = transition;Send_Data.tx[7] = transition>>8;Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //For the BCC check bits, see the Check_Sum function //BCC校验位，规则参见Check_Sum函数Send_Data.tx[10]=FRAME_TAIL; //frame tail 0x7D //帧尾0X7Dtry{Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Sends data to the downloader via serial port //通过串口向下位机发送数据 }catch (serial::IOException& e)   {ROS_ERROR_STREAM("Unable to send data through serial port"); //If sending data fails, an error message is printed //如果发送数据失败，打印错误信息}
}
/**************************************
Date: January 28, 2021
Function: Publish the IMU data topic
功能: 发布IMU数据话题
***************************************/
void turn_on_robot::Publish_ImuSensor()
{sensor_msgs::Imu Imu_Data_Pub; //Instantiate IMU topic data //实例化IMU话题数据Imu_Data_Pub.header.stamp = ros::Time::now(); Imu_Data_Pub.header.frame_id = gyro_frame_id; //IMU corresponds to TF coordinates, which is required to use the robot_pose_ekf feature pack //IMU对应TF坐标，使用robot_pose_ekf功能包需要设置此项Imu_Data_Pub.orientation.x = Mpu6050.orientation.x; //A quaternion represents a three-axis attitude //四元数表达三轴姿态Imu_Data_Pub.orientation.y = Mpu6050.orientation.y; Imu_Data_Pub.orientation.z = Mpu6050.orientation.z;Imu_Data_Pub.orientation.w = Mpu6050.orientation.w;Imu_Data_Pub.orientation_covariance[0] = 1e6; //Three-axis attitude covariance matrix //三轴姿态协方差矩阵Imu_Data_Pub.orientation_covariance[4] = 1e6;Imu_Data_Pub.orientation_covariance[8] = 1e-6;Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x; //Triaxial angular velocity //三轴角速度Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y;Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z;Imu_Data_Pub.angular_velocity_covariance[0] = 1e6; //Triaxial angular velocity covariance matrix //三轴角速度协方差矩阵Imu_Data_Pub.angular_velocity_covariance[4] = 1e6;Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6;Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x; //Triaxial acceleration //三轴线性加速度Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y; Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z;  imu_publisher.publish(Imu_Data_Pub); //Pub IMU topic //发布IMU话题
}
/**************************************
Date: January 28, 2021
Function: Publish the odometer topic, Contains position, attitude, triaxial velocity, angular velocity about triaxial, TF parent-child coordinates, and covariance matrix
功能: 发布里程计话题，包含位置、姿态、三轴速度、绕三轴角速度、TF父子坐标、协方差矩阵
***************************************/
void turn_on_robot::Publish_Odom()
{//Convert the Z-axis rotation Angle into a quaternion for expression //把Z轴转角转换为四元数进行表达geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(Robot_Pos.Z);nav_msgs::Odometry odom; //Instance the odometer topic data //实例化里程计话题数据odom.header.stamp = ros::Time::now(); odom.header.frame_id = odom_frame_id; // Odometer TF parent coordinates //里程计TF父坐标odom.pose.pose.position.x = Robot_Pos.X; //Position //位置odom.pose.pose.position.y = Robot_Pos.Y;odom.pose.pose.position.z = Robot_Pos.Z;odom.pose.pose.orientation = odom_quat; //Posture, Quaternion converted by Z-axis rotation //姿态，通过Z轴转角转换的四元数odom.child_frame_id = robot_frame_id; // Odometer TF subcoordinates //里程计TF子坐标odom.twist.twist.linear.x =  Robot_Vel.X; //Speed in the X direction //X方向速度odom.twist.twist.linear.y =  Robot_Vel.Y; //Speed in the Y direction //Y方向速度odom.twist.twist.angular.z = Robot_Vel.Z; //Angular velocity around the Z axis //绕Z轴角速度 //There are two types of this matrix, which are used when the robot is at rest and when it is moving.Extended Kalman Filtering officially provides 2 matrices for the robot_pose_ekf feature pack//这个矩阵有两种，分别在机器人静止和运动的时候使用。扩展卡尔曼滤波官方提供的2个矩阵，用于robot_pose_ekf功能包if(Robot_Vel.X== 0&&Robot_Vel.Y== 0&&Robot_Vel.Z== 0)//If the velocity is zero, it means that the error of the encoder will be relatively small, and the data of the encoder will be considered more reliable//如果velocity是零，说明编码器的误差会比较小，认为编码器数据更可靠memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2)),memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2));else//If the velocity of the trolley is non-zero, considering the sliding error that may be brought by the encoder in motion, the data of IMU is considered to be more reliable//如果小车velocity非零，考虑到运动中编码器可能带来的滑动误差，认为imu的数据更可靠memcpy(&odom.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance)),memcpy(&odom.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance));       odom_publisher.publish(odom); //Pub odometer topic //发布里程计话题
}
/**************************************
Date: January 28, 2021
Function: Publish voltage-related information
功能: 发布电压相关信息
***************************************/
void turn_on_robot::Publish_Voltage()
{std_msgs::Float32 voltage_msgs; //Define the data type of the power supply voltage publishing topic //定义电源电压发布话题的数据类型static float Count_Voltage_Pub=0;if(Count_Voltage_Pub++>10){Count_Voltage_Pub=0;  voltage_msgs.data = Power_voltage; //The power supply voltage is obtained //电源供电的电压获取voltage_publisher.publish(voltage_msgs); //Post the power supply voltage topic unit: V, volt //发布电源电压话题单位：V、伏特}
}
/**************************************
Date: January 28, 2021
Function: Serial port communication check function, packet n has a byte, the NTH -1 byte is the check bit, the NTH byte bit frame end.Bit XOR results from byte 1 to byte n-2 are compared with byte n-1, which is a BCC check
Input parameter: Count_Number: Check the first few bytes of the packet
功能: 串口通讯校验函数，数据包n有个字节，第n-1个字节为校验位，第n个字节位帧尾。第1个字节到第n-2个字节数据按位异或的结果与第n-1个字节对比，即为BCC校验
输入参数： Count_Number：数据包前几个字节加入校验   mode：对发送数据还是接收数据进行校验
***************************************/
unsigned char turn_on_robot::Check_Sum(unsigned char Count_Number,unsigned char mode)
{unsigned char check_sum=0,k;if(mode==0) //Receive data mode //接收数据模式{for(k=0;k<Count_Number;k++){check_sum=check_sum^Receive_Data.rx[k]; //By bit or by bit //按位异或}}if(mode==1) //Send data mode //发送数据模式{for(k=0;k<Count_Number;k++){check_sum=check_sum^Send_Data.tx[k]; //By bit or by bit //按位异或}}return check_sum; //Returns the bitwise XOR result //返回按位异或结果
}
/**************************************
Date: November 18, 2021
Function: The serial port reads and verifies the data sent by the lower computer, and then the data is converted to international units
Update Note: This checking method can lead to read error data or correct data not to be processed. Instead of this checking method, frame-by-frame checking is now used. Refer to Get_ Sensor_ Data_ New() function
功能: 通过串口读取并校验下位机发送过来的数据，然后数据转换为国际单位
更新说明：该校验方法会导致出现读取错误数据或者正确数据不处理的情况，现在已不用该校验方法，换成逐帧校验方式，参考Get_Sensor_Data_New()函数
***************************************/
bool turn_on_robot::Get_Sensor_Data()
{ short transition_16=0, j=0, Header_Pos=0, Tail_Pos=0; //Intermediate variable //中间变量static int flag_error=0,temp=1; //Static variable that records the error flag and location //静态变量，用于记录出错标志位和出错位置uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE]={0},Receive_Data_Tr[temp]={0}; //Temporary variable to save the data of the lower machine //临时变量，保存下位机数据if(flag_error==0) //Normal condition detected //检测到正常情况Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr)); //Read the data sent by the lower computer through the serial port //通过串口读取下位机发送过来的数据else if (flag_error==1) //Error condition detected 检测到错误情况{//Read wrong bit data through serial port, read only and do not process, so that the correct data is read next time//通过串口读取错位数据，只读取不处理，以便于下次读取到的是正确的数据Stm32_Serial.read(Receive_Data_Tr,sizeof (Receive_Data_Tr)); flag_error=0; //Error flag position 0 //错误标志位置0}/*//View the received raw data directly and debug it for use//直接查看接收到的原始数据，调试使用ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x",Receive_Data_Pr[0],Receive_Data_Pr[1],Receive_Data_Pr[2],Receive_Data_Pr[3],Receive_Data_Pr[4],Receive_Data_Pr[5],Receive_Data_Pr[6],Receive_Data_Pr[7],Receive_Data_Pr[8],Receive_Data_Pr[9],Receive_Data_Pr[10],Receive_Data_Pr[11],Receive_Data_Pr[12],Receive_Data_Pr[13],Receive_Data_Pr[14],Receive_Data_Pr[15],Receive_Data_Pr[16],Receive_Data_Pr[17],Receive_Data_Pr[18],Receive_Data_Pr[19],Receive_Data_Pr[20],Receive_Data_Pr[21],Receive_Data_Pr[22],Receive_Data_Pr[23]);*/  //Record the position of the head and tail of the frame //记录帧头帧尾位置for(j=0;j<24;j++){if(Receive_Data_Pr[j]==FRAME_HEADER)Header_Pos=j;else if(Receive_Data_Pr[j]==FRAME_TAIL)Tail_Pos=j;    }if(Tail_Pos==(Header_Pos+23)){//If the end of the frame is the last bit of the packet, copy the packet directly to receive_data.rx//如果帧尾在数据包最后一位，直接复制数据包到Receive_Data.rx// ROS_INFO("1-----");memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr));flag_error=0; //Error flag position 0 for next reading //错误标志位置0，便于下次读取}else if(Header_Pos==(1+Tail_Pos)){//If the header is behind the end of the frame, record the position of the header so that the next reading of the error bit data can correct the data position//如果帧头在帧尾后面，记录帧头出现的位置，便于下次读取出错位数据以纠正数据位置//|********7D (7B************|**********7D) 7B************|// ROS_INFO("2-----");temp=Header_Pos; //Record the length of the next read, calculated to be exactly the position of the frame head //记录下一次读取的长度，经计算正好为帧头的位置flag_error=1; //Error flag position 1, error bit array for next read //错误标志位置1，让下一次读取出错位数组return false;}else {////其它情况则认为数据包有错误，这种情况一般是正常的数据，但是除帧头帧尾在数据中间出现了7B或7D的数据// In other cases, the packet is considered to be faulty// This is generally normal data, but there is 7B or 7D data in the middle of the data except for the frame header and end.// ROS_INFO("3-----");return false;}    /* //Check receive_data.rx for debugging use //查看Receive_Data.rx，调试使用ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x",Receive_Data.rx[0],Receive_Data.rx[1],Receive_Data.rx[2],Receive_Data.rx[3],Receive_Data.rx[4],Receive_Data.rx[5],Receive_Data.rx[6],Receive_Data.rx[7],Receive_Data.rx[8],Receive_Data.rx[9],Receive_Data.rx[10],Receive_Data.rx[11],Receive_Data.rx[12],Receive_Data.rx[13],Receive_Data.rx[14],Receive_Data.rx[15],Receive_Data.rx[16],Receive_Data.rx[17],Receive_Data.rx[18],Receive_Data.rx[19],Receive_Data.rx[20],Receive_Data.rx[21],Receive_Data.rx[22],Receive_Data.rx[23]); */Receive_Data.Frame_Header= Receive_Data.rx[0]; //The first part of the data is the frame header 0X7B //数据的第一位是帧头0X7BReceive_Data.Frame_Tail= Receive_Data.rx[23];  //The last bit of data is frame tail 0X7D //数据的最后一位是帧尾0X7Dif (Receive_Data.Frame_Header == FRAME_HEADER ) //Judge the frame header //判断帧头{if (Receive_Data.Frame_Tail == FRAME_TAIL) //Judge the end of the frame //判断帧尾{ if (Receive_Data.rx[22] == Check_Sum(22,READ_DATA_CHECK)) //BCC check passes or two packets are interlaced //BCC校验通过或者两组数据包交错{Receive_Data.Flag_Stop=Receive_Data.rx[1]; //set aside //预留位Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]); //Get the speed of the moving chassis in the X direction //获取运动底盘X方向速度Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); //Get the speed of the moving chassis in the Y direction, The Y speed is only valid in the omnidirectional mobile robot chassis//获取运动底盘Y方向速度，Y速度仅在全向移动机器人底盘有效Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); //Get the speed of the moving chassis in the Z direction //获取运动底盘Z方向速度   //MPU6050 stands for IMU only and does not refer to a specific model. It can be either MPU6050 or MPU9250//Mpu6050仅代表IMU，不指代特定型号，既可以是MPU6050也可以是MPU9250Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]);   //Get the X-axis acceleration of the IMU     //获取IMU的X轴加速度  Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]); //Get the Y-axis acceleration of the IMU     //获取IMU的Y轴加速度Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]); //Get the Z-axis acceleration of the IMU     //获取IMU的Z轴加速度Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]);  //Get the X-axis angular velocity of the IMU //获取IMU的X轴角速度  Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]);  //Get the Y-axis angular velocity of the IMU //获取IMU的Y轴角速度  Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]);  //Get the Z-axis angular velocity of the IMU //获取IMU的Z轴角速度  //Linear acceleration unit conversion is related to the range of IMU initialization of STM32, where the range is ±2g=19.6m/s^2//线性加速度单位转化，和STM32的IMU初始化的时候的量程有关,这里量程±2g=19.6m/s^2Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO;Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO;Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO;//The gyroscope unit conversion is related to the range of STM32's IMU when initialized. Here, the range of IMU's gyroscope is ±500°/s//Because the robot generally has a slow Z-axis speed, reducing the range can improve the accuracy//陀螺仪单位转化，和STM32的IMU初始化的时候的量程有关，这里IMU的陀螺仪的量程是±500°/s//因为机器人一般Z轴速度不快，降低量程可以提高精度Mpu6050.angular_velocity.x =  Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;Mpu6050.angular_velocity.y =  Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;Mpu6050.angular_velocity.z =  Mpu6050_Data.gyros_z_data * GYROSCOPE_RATIO;//Get the battery voltage//获取电池电压transition_16 = 0;transition_16 |=  Receive_Data.rx[20]<<8;transition_16 |=  Receive_Data.rx[21];  Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001; //Unit conversion millivolt(mv)->volt(v) //单位转换毫伏(mv)->伏(v)return true;}}} return false;
}
/**************************************
Date: November 18, 2021
Function: Read and verify the data sent by the lower computer frame by frame through the serial port, and then convert the data into international units
功能: 通过串口读取并逐帧校验下位机发送过来的数据，然后数据转换为国际单位
***************************************/
bool turn_on_robot::Get_Sensor_Data_New()
{short transition_16=0; //Intermediate variable //中间变量uint8_t i=0,check=0, error=1,Receive_Data_Pr[1]; //Temporary variable to save the data of the lower machine //临时变量，保存下位机数据static int count; //Static variable for counting //静态变量，用于计数Stm32_Serial.read(Receive_Data_Pr,sizeof(Receive_Data_Pr)); //Read the data sent by the lower computer through the serial port //通过串口读取下位机发送过来的数据/*//View the received raw data directly and debug it for use//直接查看接收到的原始数据，调试使用ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x",Receive_Data_Pr[0],Receive_Data_Pr[1],Receive_Data_Pr[2],Receive_Data_Pr[3],Receive_Data_Pr[4],Receive_Data_Pr[5],Receive_Data_Pr[6],Receive_Data_Pr[7],Receive_Data_Pr[8],Receive_Data_Pr[9],Receive_Data_Pr[10],Receive_Data_Pr[11],Receive_Data_Pr[12],Receive_Data_Pr[13],Receive_Data_Pr[14],Receive_Data_Pr[15],Receive_Data_Pr[16],Receive_Data_Pr[17],Receive_Data_Pr[18],Receive_Data_Pr[19],Receive_Data_Pr[20],Receive_Data_Pr[21],Receive_Data_Pr[22],Receive_Data_Pr[23]);*/  Receive_Data.rx[count] = Receive_Data_Pr[0]; //Fill the array with serial data //串口数据填入数组Receive_Data.Frame_Header = Receive_Data.rx[0]; //The first part of the data is the frame header 0X7B //数据的第一位是帧头0X7BReceive_Data.Frame_Tail = Receive_Data.rx[23];  //The last bit of data is frame tail 0X7D //数据的最后一位是帧尾0X7Dif(Receive_Data_Pr[0] == FRAME_HEADER || count>0) //Ensure that the first data in the array is FRAME_HEADER //确保数组第一个数据为FRAME_HEADERcount++;else count=0;if(count == 24) //Verify the length of the packet //验证数据包的长度{count=0;  //Prepare for the serial port data to be refill into the array //为串口数据重新填入数组做准备if(Receive_Data.Frame_Tail == FRAME_TAIL) //Verify the frame tail of the packet //验证数据包的帧尾{check=Check_Sum(22,READ_DATA_CHECK);  //BCC check passes or two packets are interlaced //BCC校验通过或者两组数据包交错if(check == Receive_Data.rx[22])  {error=0;  //XOR bit check successful //异或位校验成功}if(error == 0){/*//Check receive_data.rx for debugging use //查看Receive_Data.rx，调试使用 ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x",Receive_Data.rx[0],Receive_Data.rx[1],Receive_Data.rx[2],Receive_Data.rx[3],Receive_Data.rx[4],Receive_Data.rx[5],Receive_Data.rx[6],Receive_Data.rx[7],Receive_Data.rx[8],Receive_Data.rx[9],Receive_Data.rx[10],Receive_Data.rx[11],Receive_Data.rx[12],Receive_Data.rx[13],Receive_Data.rx[14],Receive_Data.rx[15],Receive_Data.rx[16],Receive_Data.rx[17],Receive_Data.rx[18],Receive_Data.rx[19],Receive_Data.rx[20],Receive_Data.rx[21],Receive_Data.rx[22],Receive_Data.rx[23]); */Receive_Data.Flag_Stop=Receive_Data.rx[1]; //set aside //预留位Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]); //Get the speed of the moving chassis in the X direction //获取运动底盘X方向速度Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); //Get the speed of the moving chassis in the Y direction, The Y speed is only valid in the omnidirectional mobile robot chassis//获取运动底盘Y方向速度，Y速度仅在全向移动机器人底盘有效Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); //Get the speed of the moving chassis in the Z direction //获取运动底盘Z方向速度   //MPU6050 stands for IMU only and does not refer to a specific model. It can be either MPU6050 or MPU9250//Mpu6050仅代表IMU，不指代特定型号，既可以是MPU6050也可以是MPU9250Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]);   //Get the X-axis acceleration of the IMU     //获取IMU的X轴加速度  Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]); //Get the Y-axis acceleration of the IMU     //获取IMU的Y轴加速度Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]); //Get the Z-axis acceleration of the IMU     //获取IMU的Z轴加速度Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]);  //Get the X-axis angular velocity of the IMU //获取IMU的X轴角速度  Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]);  //Get the Y-axis angular velocity of the IMU //获取IMU的Y轴角速度  Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]);  //Get the Z-axis angular velocity of the IMU //获取IMU的Z轴角速度  //Linear acceleration unit conversion is related to the range of IMU initialization of STM32, where the range is ±2g=19.6m/s^2//线性加速度单位转化，和STM32的IMU初始化的时候的量程有关,这里量程±2g=19.6m/s^2Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO;Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO;Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO;//The gyroscope unit conversion is related to the range of STM32's IMU when initialized. Here, the range of IMU's gyroscope is ±500°/s//Because the robot generally has a slow Z-axis speed, reducing the range can improve the accuracy//陀螺仪单位转化，和STM32的IMU初始化的时候的量程有关，这里IMU的陀螺仪的量程是±500°/s//因为机器人一般Z轴速度不快，降低量程可以提高精度Mpu6050.angular_velocity.x =  Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;Mpu6050.angular_velocity.y =  Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;Mpu6050.angular_velocity.z =  Mpu6050_Data.gyros_z_data * GYROSCOPE_RATIO;//Get the battery voltage//获取电池电压transition_16 = 0;transition_16 |=  Receive_Data.rx[20]<<8;transition_16 |=  Receive_Data.rx[21];  Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001; //Unit conversion millivolt(mv)->volt(v) //单位转换毫伏(mv)->伏(v)return true;}}}return false;
}/**************************************
Date: January 28, 2021
Function: Loop access to the lower computer data and issue topics
功能: 循环获取下位机数据与发布话题
***************************************/
void turn_on_robot::Control()
{while(ros::ok()){if (true == Get_Sensor_Data_New()) //The serial port reads and verifies the data sent by the lower computer, and then the data is converted to international units//通过串口读取并校验下位机发送过来的数据，然后数据转换为国际单位{_Now = ros::Time::now();if(_Last_Time.toSec()==0) _Last_Time=_Now; //Perform this operation when entering for the first time to avoid excessive integration time//首次进入时进行此操作，避免积分时间过大Sampling_Time = (_Now - _Last_Time).toSec(); //Retrieves time interval, which is used to integrate velocity to obtain displacement (mileage) //获取时间间隔，用于积分速度获得位移(里程)//Odometer correction parameters//里程计误差修正Robot_Vel.X=Robot_Vel.X*odom_x_scale;Robot_Vel.Y=Robot_Vel.Y*odom_y_scale;if(Robot_Vel.Z>=0)Robot_Vel.Z=Robot_Vel.Z*odom_z_scale_positive;elseRobot_Vel.Z=Robot_Vel.Z*odom_z_scale_negative;//Speed * Time = displacement (odometer)//速度*时间=位移（里程计）Robot_Pos.X+=(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time; //Calculate the displacement in the X direction, unit: m //计算X方向的位移，单位：mRobot_Pos.Y+=(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time; //Calculate the displacement in the Y direction, unit: m //计算Y方向的位移，单位：mRobot_Pos.Z+=Robot_Vel.Z * Sampling_Time; //The angular displacement about the Z axis, in rad //绕Z轴的角位移，单位：rad //Calculate the three-axis attitude from the IMU with the angular velocity around the three-axis and the three-axis acceleration//通过IMU绕三轴角速度与三轴加速度计算三轴姿态Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Mpu6050.angular_velocity.z,\Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z);Publish_Odom();      //Pub the speedometer topic //发布里程计话题Publish_ImuSensor(); //Pub the IMU topic //发布IMU话题    Publish_Voltage();   //Pub the topic of power supply voltage //发布电源电压话题_Last_Time = _Now; //Record the time and use it to calculate the time interval //记录时间，用于计算时间间隔}ros::spinOnce();   //The loop waits for the callback function //循环等待回调函数}
}
/**************************************
Date: January 28, 2021
Function: Constructor, executed only once, for initialization
功能: 构造函数, 只执行一次，用于初始化
***************************************/
turn_on_robot::turn_on_robot():Sampling_Time(0),Power_voltage(0)
{//Clear the data//清空数据memset(&Robot_Pos, 0, sizeof(Robot_Pos));memset(&Robot_Vel, 0, sizeof(Robot_Vel));memset(&Receive_Data, 0, sizeof(Receive_Data)); memset(&Send_Data, 0, sizeof(Send_Data));memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data));ros::NodeHandle private_nh("~"); //Create a node handle //创建节点句柄//The private_nh.param() entry parameter corresponds to the initial value of the name of the parameter variable on the parameter server//private_nh.param()入口参数分别对应：参数服务器上的名称  参数变量名  初始值private_nh.param<std::string>("usart_port_name",  usart_port_name,  "/dev/wheeltec_controller"); //Fixed serial port number //固定串口号private_nh.param<int>        ("serial_baud_rate", serial_baud_rate, 115200); //Communicate baud rate 115200 to the lower machine //和下位机通信波特率115200private_nh.param<std::string>("odom_frame_id",    odom_frame_id,    "odom_combined");      //The odometer topic corresponds to the parent TF coordinate //里程计话题对应父TF坐标private_nh.param<std::string>("robot_frame_id",   robot_frame_id,   "base_footprint"); //The odometer topic corresponds to sub-TF coordinates //里程计话题对应子TF坐标private_nh.param<std::string>("gyro_frame_id",    gyro_frame_id,    "gyro_link"); //IMU topics correspond to TF coordinates //IMU话题对应TF坐标//Odometer correction parameters//里程计误差修正参数private_nh.param<float>("odom_x_scale",    odom_x_scale,    1.0); private_nh.param<float>("odom_y_scale",    odom_y_scale,    1.0); private_nh.param<float>("odom_z_scale_positive",    odom_z_scale_positive,    1.0); private_nh.param<float>("odom_z_scale_negative",    odom_z_scale_negative,    1.0); voltage_publisher = n.advertise<std_msgs::Float32>("PowerVoltage", 10); //Create a battery-voltage topic publisher //创建电池电压话题发布者odom_publisher    = n.advertise<nav_msgs::Odometry>("odom", 50); //Create the odometer topic publisher //创建里程计话题发布者imu_publisher     = n.advertise<sensor_msgs::Imu>("imu", 20); //Create an IMU topic publisher //创建IMU话题发布者//Set the velocity control command callback function//速度控制命令订阅回调函数设置Cmd_Vel_Sub     = n.subscribe("cmd_vel",     100, &turn_on_robot::Cmd_Vel_Callback, this); ROS_INFO_STREAM("Data ready"); //Prompt message //提示信息try{ //Attempts to initialize and open the serial port //尝试初始化与开启串口Stm32_Serial.setPort(usart_port_name); //Select the serial port number to enable //选择要开启的串口号Stm32_Serial.setBaudrate(serial_baud_rate); //Set the baud rate //设置波特率serial::Timeout _time = serial::Timeout::simpleTimeout(2000); //Timeout //超时等待Stm32_Serial.setTimeout(_time);Stm32_Serial.open(); //Open the serial port //开启串口Stm32_Serial.flushInput(); //Clear the input buff //清空输入缓存}catch (serial::IOException& e){ROS_ERROR_STREAM("wheeltec_robot can not open serial port,Please check the serial port cable! "); //If opening the serial port fails, an error message is printed //如果开启串口失败，打印错误信息}if(Stm32_Serial.isOpen()){ROS_INFO_STREAM("wheeltec_robot serial port opened"); //Serial port opened successfully //串口开启成功提示}
}
/**************************************
Date: January 28, 2021
Function: Destructor, executed only once and called by the system when an object ends its life cycle
功能: 析构函数，只执行一次，当对象结束其生命周期时系统会调用这个函数
***************************************/
turn_on_robot::~turn_on_robot()
{//Sends the stop motion command to the lower machine before the turn_on_robot object ends//对象turn_on_robot结束前向下位机发送停止运动命令Send_Data.tx[0]=FRAME_HEADER;Send_Data.tx[1] = 0;  Send_Data.tx[2] = 0; //The target velocity of the X-axis of the robot //机器人X轴的目标线速度 Send_Data.tx[4] = 0;     Send_Data.tx[3] = 0;  //The target velocity of the Y-axis of the robot //机器人Y轴的目标线速度 Send_Data.tx[6] = 0;Send_Data.tx[5] = 0;  //The target velocity of the Z-axis of the robot //机器人Z轴的目标角速度 Send_Data.tx[8] = 0;  Send_Data.tx[7] = 0;    Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //Check the bits for the Check_Sum function //校验位，规则参见Check_Sum函数Send_Data.tx[10]=FRAME_TAIL; try{Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Send data to the serial port //向串口发数据  }catch (serial::IOException& e)   {ROS_ERROR_STREAM("Unable to send data through serial port"); //If sending data fails, an error message is printed //如果发送数据失败,打印错误信息}Stm32_Serial.close(); //Close the serial port //关闭串口  ROS_INFO_STREAM("Shutting down"); //Prompt message //提示信息
}