使用jetson nano 进行ros端和stm32之间的通讯1

硬件条件stm32向ros端发送消息1、 stm32需要告知ros端 imu和速度信息2 、ros端接受stm32发送的数据

硬件条件

jetson nano 、stm32单片机（我使用轮趣科技的底板）、麦克纳姆轮车

stm32向ros端发送消息

1、 stm32需要告知ros端 imu和速度信息

发送数据的结构体变量，数据主要是包括imu信息和速度信息

/*****用于存放陀螺仪加速度计三轴数据的结构体*********************************/typedef struct __Mpu6050_Data_ {short X_data; //2 bytes //2个字节short Y_data; //2 bytes //2个字节short Z_data; //2 bytes //2个字节}Mpu6050_Data;/*******串口发送数据的结构体*************************************/typedef struct _SEND_DATA_ {unsigned char buffer[SEND_DATA_SIZE];struct _Sensor_Str_{unsigned char Frame_Header; //1个字节short X_speed; //2 bytes //2个字节short Y_speed; //2 bytes //2个字节short Z_speed; //2 bytes //2个字节short Power_Voltage; //2 bytes //2个字节Mpu6050_Data Accelerometer; //6 bytes //6个字节Mpu6050_Data Gyroscope;//6 bytes //6个字节unsigned char Frame_Tail; //1 bytes //1个字节}Sensor_Str;}SEND_DATA;

对结构体数据进行赋值操作

void data_transition(void){Send_Data.Sensor_Str.Frame_Header = FRAME_HEADER; //Frame_header //帧头Send_Data.Sensor_Str.Frame_Tail = FRAME_TAIL;//Frame_tail //帧尾//根据不同车型选择不同运动学算法进行运动学正解，从各车轮速度求出三轴速度Send_Data.Sensor_Str.X_speed = ((MOTOR_A.Encoder+MOTOR_B.Encoder+MOTOR_C.Encoder+MOTOR_D.Encoder)/4)*1000;Send_Data.Sensor_Str.Y_speed = ((MOTOR_A.Encoder-MOTOR_B.Encoder+MOTOR_C.Encoder-MOTOR_D.Encoder)/4)*1000; Send_Data.Sensor_Str.Z_speed = ((-MOTOR_A.Encoder-MOTOR_B.Encoder+MOTOR_C.Encoder+MOTOR_D.Encoder)/4/(Axle_spacing+Wheel_spacing))*1000; //The acceleration of the triaxial acceleration //加速度计三轴加速度Send_Data.Sensor_Str.Accelerometer.X_data= accel[1]; //加速度计Y轴转换到ROS坐标X轴Send_Data.Sensor_Str.Accelerometer.Y_data=-accel[0]; //加速度计X轴转换到ROS坐标Y轴Send_Data.Sensor_Str.Accelerometer.Z_data= accel[2]; //加速度计Z轴转换到ROS坐标Z轴//The Angle velocity of the triaxial velocity //角速度计三轴角速度Send_Data.Sensor_Str.Gyroscope.X_data= gyro[1]; //角速度计Y轴转换到ROS坐标X轴Send_Data.Sensor_Str.Gyroscope.Y_data=-gyro[0]; //角速度计X轴转换到ROS坐标Y轴if(Flag_Stop==0) //如果电机控制位使能状态，那么正常发送Z轴角速度Send_Data.Sensor_Str.Gyroscope.Z_data=gyro[2]; else //如果机器人是静止的（电机控制位失能），那么发送的Z轴角速度为0Send_Data.Sensor_Str.Gyroscope.Z_data=0; //电池电压(这里将浮点数放大一千倍传输，相应的在接收端在接收到数据后也会缩小一千倍)Send_Data.Sensor_Str.Power_Voltage = Voltage*1000; Send_Data.buffer[0]=Send_Data.Sensor_Str.Frame_Header; //Frame_heade //帧头Send_Data.buffer[1]=Flag_Stop; //Car software loss marker //小车软件失能标志位//小车三轴速度,各轴都拆分为两个8位数据再发送Send_Data.buffer[2]=Send_Data.Sensor_Str.X_speed >>8; Send_Data.buffer[3]=Send_Data.Sensor_Str.X_speed ; Send_Data.buffer[4]=Send_Data.Sensor_Str.Y_speed>>8; Send_Data.buffer[5]=Send_Data.Sensor_Str.Y_speed;Send_Data.buffer[6]=Send_Data.Sensor_Str.Z_speed >>8; Send_Data.buffer[7]=Send_Data.Sensor_Str.Z_speed ; //IMU加速度计三轴加速度,各轴都拆分为两个8位数据再发送Send_Data.buffer[8]=Send_Data.Sensor_Str.Accelerometer.X_data>>8; Send_Data.buffer[9]=Send_Data.Sensor_Str.Accelerometer.X_data; Send_Data.buffer[10]=Send_Data.Sensor_Str.Accelerometer.Y_data>>8;Send_Data.buffer[11]=Send_Data.Sensor_Str.Accelerometer.Y_data;Send_Data.buffer[12]=Send_Data.Sensor_Str.Accelerometer.Z_data>>8;Send_Data.buffer[13]=Send_Data.Sensor_Str.Accelerometer.Z_data;//IMU角速度计三轴角速度,各轴都拆分为两个8位数据再发送Send_Data.buffer[14]=Send_Data.Sensor_Str.Gyroscope.X_data>>8;Send_Data.buffer[15]=Send_Data.Sensor_Str.Gyroscope.X_data;Send_Data.buffer[16]=Send_Data.Sensor_Str.Gyroscope.Y_data>>8;Send_Data.buffer[17]=Send_Data.Sensor_Str.Gyroscope.Y_data;Send_Data.buffer[18]=Send_Data.Sensor_Str.Gyroscope.Z_data>>8;Send_Data.buffer[19]=Send_Data.Sensor_Str.Gyroscope.Z_data;//电池电压,拆分为两个8位数据发送Send_Data.buffer[20]=Send_Data.Sensor_Str.Power_Voltage >>8; Send_Data.buffer[21]=Send_Data.Sensor_Str.Power_Voltage; //数据校验位计算，模式1是发送数据校验Send_Data.buffer[22]=Check_Sum(22,1); Send_Data.buffer[23]=Send_Data.Sensor_Str.Frame_Tail; //Frame_tail //帧尾}

进行异或位校验

u8 Check_Sum(unsigned char Count_Number,unsigned char Mode){unsigned char check_sum=0,k;//对要发送的数据进行校验if(Mode==1)for(k=0;k<Count_Number;k++){check_sum=check_sum^Send_Data.buffer[k];}//对接收到的数据进行校验if(Mode==0)for(k=0;k<Count_Number;k++){check_sum=check_sum^Receive_Data.buffer[k];}return check_sum;}

2 、ros端接受stm32发送的数据

对数据进行转换

short 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 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;}

通过串口读取并逐帧校验下位机发送过来的数据，然后数据转换为国际单位

bool Get_Sensor_Data_New(){short transition_16=0; //中间变量uint8_t i=0,check=0, error=1,Receive_Data_Pr[1]; //临时变量，保存下位机数据static int count; //静态变量，用于计数Stm32_Serial.read(Receive_Data_Pr,sizeof(Receive_Data_Pr)); //通过串口读取下位机发送过来的数据/*//直接查看接收到的原始数据，调试使用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]; //串口数据填入数组Receive_Data.Frame_Header = Receive_Data.rx[0]; //数据的第一位是帧头0X7BReceive_Data.Frame_Tail = Receive_Data.rx[23]; //数据的最后一位是帧尾0X7Dif(Receive_Data_Pr[0] == FRAME_HEADER || count>0) //确保数组第一个数据为FRAME_HEADERcount++;else count=0;if(count == 24) //Verify the length of the packet //验证数据包的长度{count=0; //为串口数据重新填入数组做准备if(Receive_Data.Frame_Tail == FRAME_TAIL) //验证数据包的帧尾{check=Check_Sum(22,READ_DATA_CHECK); //BCC校验通过或者两组数据包交错if(check == Receive_Data.rx[22]) {error=0; //异或位校验成功}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]); //获取运动底盘X方向速度Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); //获取运动底盘Y方向速度Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); //获取运动底盘Z方向速度 //Mpu6050仅代表IMU，不指代特定型号，既可以是MPU6050也可以是MPU9250Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]); //获取IMU的X轴加速度 Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]); //获取IMU的Y轴加速度Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]); //获取IMU的Z轴加速度Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]); //获取IMU的X轴角速度 Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]); //获取IMU的Y轴角速度 Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]); //获取IMU的Z轴角速度 //线性加速度单位转化，和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;//陀螺仪单位转化，和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;//获取电池电压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; //单位转换毫伏(mv)->伏(v)return true;}}}return false;}

四元数解算获取方位角

volatile float twoKp = 1.0f;// 2 * proportional gain (Kp)volatile float twoKi = 0.0f;// 2 * integral gain (Ki)volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;// quaternion of sensor frame relative to auxiliary framevolatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Kivoid Quaternion_Solution(float gx, float gy, float gz, float ax, float ay, float az){float recipNorm;float halfvx, halfvy, halfvz;float halfex, halfey, halfez;float qa, qb, qc;// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {// 首先把加速度计采集到的值(三维向量)转化为单位向量，即向量除以模recipNorm = InvSqrt(ax * ax + ay * ay + az * az);ax *= recipNorm;ay *= recipNorm;az *= recipNorm;// 把四元数换算成方向余弦中的第三行的三个元素halfvx = q1 * q3 - q0 * q2;halfvy = q0 * q1 + q2 * q3;halfvz = q0 * q0 - 0.5f + q3 * q3;//误差是估计的重力方向和测量的重力方向的交叉乘积之和halfex = (ay * halfvz - az * halfvy);halfey = (az * halfvx - ax * halfvz);halfez = (ax * halfvy - ay * halfvx);// 计算并应用积分反馈（如果启用）if(twoKi > 0.0f) {integralFBx += twoKi * halfex * (1.0f / SAMPLING_FREQ); // integral error scaled by KiintegralFBy += twoKi * halfey * (1.0f / SAMPLING_FREQ);integralFBz += twoKi * halfez * (1.0f / SAMPLING_FREQ);gx += integralFBx; // apply integral feedbackgy += integralFBy;gz += integralFBz;}else {integralFBx = 0.0f; // prevent integral windupintegralFBy = 0.0f;integralFBz = 0.0f;}// Apply proportional feedbackgx += twoKp * halfex;gy += twoKp * halfey;gz += twoKp * halfez;}// Integrate rate of change of quaterniongx *= (0.5f * (1.0f / SAMPLING_FREQ)); // pre-multiply common factorsgy *= (0.5f * (1.0f / SAMPLING_FREQ));gz *= (0.5f * (1.0f / SAMPLING_FREQ));qa = q0;qb = q1;qc = q2;q0 += (-qb * gx - qc * gy - q3 * gz);q1 += (qa * gx + qc * gz - q3 * gy);q2 += (qa * gy - qb * gz + q3 * gx);q3 += (qa * gz + qb * gy - qc * gx); // Normalise quaternionrecipNorm = InvSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);q0 *= recipNorm;q1 *= recipNorm;q2 *= recipNorm;q3 *= recipNorm;Mpu6050.orientation.w = q0;Mpu6050.orientation.x = q1;Mpu6050.orientation.y = q2;Mpu6050.orientation.z = q3;}

循环获取stm32下位机数据与发布话题

void Control(){_Last_Time = ros::Time::now();while(ros::ok()){Sampling_Time = (_Now - _Last_Time).toSec(); //获取时间间隔，用于积分速度获得位移(里程)_Now = ros::Time::now();if (true == Get_Sensor_Data_New()) //通过串口读取并校验下位机发送过来的数据，然后数据转换为国际单位{Robot_Pos.X+=(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time; //计算X方向的位移，单位：mRobot_Pos.Y+=(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time; //计算Y方向的位移，单位：mRobot_Pos.Z+=Robot_Vel.Z * Sampling_Time; //绕Z轴的角位移，单位：rad //通过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();//发布里程计话题Publish_ImuSensor(); //发布IMU话题 Publish_Voltage(); //发布电源电压话题_Last_Time = _Now; //记录时间，用于计算时间间隔}ros::spinOnce(); //循环等待回调函数}}

发布IMU话题

void Publish_ImuSensor(){sensor_msgs::Imu Imu_Data_Pub; //实例化IMU话题数据Imu_Data_Pub.header.stamp = ros::Time::now(); Imu_Data_Pub.header.frame_id = gyro_frame_id; //IMU对应TF坐标，使用robot_pose_ekf功能包需要设置此项Imu_Data_Pub.orientation.x = Mpu6050.orientation.x;//四元数表达三轴姿态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; //三轴姿态协方差矩阵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; //三轴角速度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; //三轴角速度协方差矩阵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; //三轴线性加速度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); //发布IMU话题}

发布里程计话题，包含位置、姿态、三轴速度、绕三轴角速度、TF父子坐标、协方差矩阵

//协方差矩阵，用于里程计话题数据，用于robt_pose_ekf功能包const double odom_pose_covariance[36] = {1e-3, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e3 };const double odom_pose_covariance2[36] = {1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e-9 };const double odom_twist_covariance[36] = {1e-3, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e3 };const double odom_twist_covariance2[36] = {1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e-9} ;

void Publish_Odom(){//把Z轴转角转换为四元数进行表达geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(Robot_Pos.Z);nav_msgs::Odometry odom; //实例化里程计话题数据odom.header.stamp = ros::Time::now(); odom.header.frame_id = odom_frame_id; //里程计TF父坐标odom.pose.pose.position.x = Robot_Pos.X; //位置odom.pose.pose.position.y = Robot_Pos.Y;odom.pose.pose.position.z = Robot_Pos.Z;odom.pose.pose.orientation = odom_quat; //姿态，通过Z轴转角转换的四元数odom.child_frame_id = robot_frame_id; //里程计TF子坐标odom.twist.twist.linear.x = Robot_Vel.X; //X方向速度odom.twist.twist.linear.y = Robot_Vel.Y; //Y方向速度odom.twist.twist.angular.z = Robot_Vel.Z; //绕Z轴角速度 //这个矩阵有两种，分别在机器人静止和运动的时候使用。扩展卡尔曼滤波官方提供的2个矩阵，用于robot_pose_ekf功能包if(Robot_Vel.X== 0&&Robot_Vel.Y== 0&&Robot_Vel.Z== 0)//如果velocity是零，说明编码器的误差会比较小，认为编码器数据更可靠memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2)),memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2));else//如果小车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 //发布里程计话题}

发布电池电量信息

void turn_on_robot::Publish_Voltage(){std_msgs::Float32 voltage_msgs; //定义电源电压发布话题的数据类型static float Count_Voltage_Pub=0;if(Count_Voltage_Pub++>10) //等待电压稳定{Count_Voltage_Pub=0; voltage_msgs.data = Power_voltage; //电源供电的电压获取voltage_publisher.publish(voltage_msgs);//发布电源电压话题单位：V、伏特}}