从0制作自己的ros导航小车
- 前言
- 一、准备工作
- 二、下位机端（STM32）
- 三、上位机端（旭日x3派）
- 四、测试
系列文章：
①【从0制作自己的ros导航小车：介绍及准备】
②【从0制作自己的ros导航小车：下位机篇】01、工程准备_标准库移植freertos
③【从0制作自己的ros导航小车：下位机篇】02、电机驱动、转速读取、PID控制
④【从0制作自己的ros导航小车：下位机篇】03、mpu6050偏航角获取
⑤【从0制作自己的ros导航小车：上、下位机通信篇】上、下位机串口DMA通信
⑥【从0制作自己的ros导航小车：上位机篇】01、里程计与坐标变换发布
⑦【从0制作自己的ros导航小车：上位机篇】02、ros1多机通讯与坐标变换可视化
⑧【从0制作自己的ros导航小车：上位机篇】03、添加urdf模型（发布各传感器与小车基坐标系之间的静态坐标变换）
⑨【从0制作自己的ros导航小车：上位机篇】04、使用gmapping建图
⑩【从0制作自己的ros导航小车：上位机篇】05、导航！
前言
下位机的电机驱动、轮速读取、偏航角读取都已经完成，接下来就是上下位机的桥梁：串口通信。
使用USB转TTL模块连接上位机（旭日x3派）和下位机（stm32），只需要连接RX、TX、GND即可。
一、准备工作
确认上位机是否安装了ch340驱动（如果使用的USB转TTL模块使用的是cp210x，那就安装对应的驱动）
远程登录开发板之后，插入USB转TTL模块，在命令行输入：
ls -l /dev/ttyUSB*
如果显示如下：
那就不需要再安装驱动了，如果没有显示的话，那就说明板子没有识别到此串口，那就需要安装驱动：这个我也没有安装过，因为这块板子插上直接就能识别，需要安装的驱动的可以参考这篇博客。
二、下位机端（STM32）
以下代码在此博主开源的代码上进行修改使用(感谢博主的开源)，将代码集成到一个c文件中。
serial.c：
#include "Serial.h"
#include "stm32f10x.h"
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
unsigned char JudgeReceiveBuffer[reBiggestSize*2];//接受最大内存
unsigned char JudgeSend[SendBiggestSize];//发送最大内存
//通信协议常量
const unsigned char header[2] = {0x55, 0xaa};
const unsigned char ender[2] = {0x0d, 0x0a};
uint8_t num_buff[3],think_flag=0;
int8_t fan;
int8_t bianliang;
unsigned char SaveBuffer[100];//接受双缓存区
Ctrl_information chassis_ctrl ={0,0,0,1,1}; //接受上位机控制信息
//extern Chassis F103RC_chassis;//底盘实时数据
//发送数据（左轮速、右轮速、角度）共用体（-32767 - +32768）
union sendData
{
short d;
unsigned char data[2];
}leftVelNow,rightVelNow,angleNow;
//左右轮速控制速度共用体
union receiveData
{
short d;
unsigned char data[2];
}leftVelSet,rightVelSet;
void JudgeBuffReceive(unsigned char ReceiveBuffer[])
{
//uint16_t cmd_id;//识别信息内容，暂未开启
short k=0;
short PackPoint;
memcpy(&SaveBuffer[reBiggestSize],&ReceiveBuffer[0],reBiggestSize); //把ReceiveBuffer[0]地址拷贝到SaveBuffer[13], 依次拷贝13个, 把这一次接收的存到数组后方
for(PackPoint=0;PackPoint<reBiggestSize;PackPoint++) //先处理前半段数据(在上一周期已接收完成)
{
if(SaveBuffer[PackPoint]==header[0] && SaveBuffer[PackPoint + 1]== header[1]) //包头检测
{
short dataLength = SaveBuffer[PackPoint + 2] ;
unsigned char checkSum = getCrc8(&SaveBuffer[PackPoint], 3 + dataLength);
// 检查信息校验值
if (checkSum == SaveBuffer[PackPoint +3 + dataLength]) //SaveBuffer[PackPoint开始的校验位]
{
//说明数据核对成功，开始提取数据
for(k = 0; k < 2; k++)
{
leftVelSet.data[k] = SaveBuffer[PackPoint + k + 3]; //SaveBuffer[3] SaveBuffer[4]
rightVelSet.data[k] = SaveBuffer[PackPoint + k + 5]; //SaveBuffer[5] SaveBuffer[6]
}
//速度赋值操作
chassis_ctrl.leftSpeedSet = (int)leftVelSet.d;
chassis_ctrl.rightSpeedSet = (int)rightVelSet.d;
//ctrlFlag
chassis_ctrl.crtlFlag = SaveBuffer[PackPoint + 7]; //SaveBuffer[7]
}
}
memcpy(&SaveBuffer[0],&SaveBuffer[reBiggestSize],reBiggestSize); //把SaveBuffer[13]地址拷贝到SaveBuffer[0], 依次拷贝13个，把之前存到后面的数据提到前面，准备处理
}
}
void usartSendData(short leftVel, short rightVel,short angle,unsigned char ctrlFlag)
{
int i, length = 0;
// 计算左右轮期望速度
leftVelNow.d = leftVel;
rightVelNow.d = rightVel;
angleNow.d = angle;
// 设置消息头
for(i = 0; i < 2; i++)
JudgeSend[i] = header[i]; // buf[0] buf[1]
// 设置机器人左右轮速度、角度
length = 7;
JudgeSend[2] = length; // buf[2]
for(i = 0; i < 2; i++)
{
JudgeSend[i + 3] = leftVelNow.data[i]; // buf[3] buf[4]
JudgeSend[i + 5] = rightVelNow.data[i]; // buf[5] buf[6]
JudgeSend[i + 7] = angleNow.data[i]; // buf[7] buf[8]
}
// 预留控制指令
JudgeSend[3 + length - 1] = ctrlFlag; // buf[9]
// 设置校验值、消息尾
JudgeSend[3 + length] = getCrc8(JudgeSend, 3 + length); // buf[10]
JudgeSend[3 + length + 1] = ender[0]; // buf[11]
JudgeSend[3 + length + 2] = ender[1]; // buf[12]
DMA1_Channel4->CNDTR = SendBiggestSize;
DMA_Cmd(DMA1_Channel4,ENABLE);
}
unsigned char getCrc8(unsigned char *ptr, unsigned short len)
{
unsigned char crc;
unsigned char i;
crc = 0;
while(len--)
{
crc ^= *ptr++;
for(i = 0; i < 8; i++)
{
if(crc&0x01)
crc=(crc>>1)^0x8C;
else
crc >>= 1;
}
}
return crc;
}
/**********************************************************************************************************
*函 数 名: UART1_DMA_Init
*功能说明: uart1初始化(速度收发)
*形 参: 无
*返 回 值: 无
**********************************************************************************************************/
void UART2_DMA_Init(void)
{
//GPIO端口设置
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO, ENABLE); //使能USART1，GPIOA时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); // 注意这里的时钟使能
//USART1_TX GPIOA.9
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; //PA9
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIOA.9
//USART1_RX GPIOA.10初始化
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;//PA10
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//浮空输入
GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIOA.10
//USART 初始化设置
USART_InitStructure.USART_BaudRate = 115200;//串口波特率
USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位
USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //收发模式
USART_Init(USART1, &USART_InitStructure); //初始化串口1
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启串口接受中断
USART_Cmd(USART1, ENABLE); //使能串口1
//RX DMA1 5通道
USART_DMACmd(USART1, USART_DMAReq_Rx, ENABLE);
NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel5_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
{
DMA_InitTypeDef dma;
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
DMA_DeInit(DMA1_Channel5);
dma.DMA_PeripheralBaseAddr = (uint32_t)&(USART1->DR);
dma.DMA_MemoryBaseAddr = (uint32_t)JudgeReceiveBuffer;
dma.DMA_DIR = DMA_DIR_PeripheralSRC;
dma.DMA_BufferSize = reBiggestSize;
dma.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
dma.DMA_MemoryInc = DMA_MemoryInc_Enable;
dma.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
dma.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
dma.DMA_Mode = DMA_Mode_Circular;
dma.DMA_Priority = DMA_Priority_VeryHigh;
dma.DMA_M2M = DMA_M2M_Disable;
DMA_Init(DMA1_Channel5, &dma);
DMA_ITConfig(DMA1_Channel5, DMA_IT_TC, ENABLE);
DMA_Cmd(DMA1_Channel5, ENABLE);
}
//TX DMA1 4通道
NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);
{
DMA_InitTypeDef dma;
DMA_DeInit(DMA1_Channel4);
dma.DMA_PeripheralBaseAddr = (uint32_t)&(USART1->DR);
dma.DMA_MemoryBaseAddr = (uint32_t)JudgeSend;
dma.DMA_DIR = DMA_DIR_PeripheralDST;
dma.DMA_BufferSize = SendBiggestSize;
dma.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
dma.DMA_MemoryInc = DMA_MemoryInc_Enable;
dma.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
dma.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
dma.DMA_Mode = DMA_Mode_Normal;
dma.DMA_Priority = DMA_Priority_VeryHigh;
dma.DMA_M2M = DMA_M2M_Disable;
DMA_Init(DMA1_Channel4, &dma);
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE);
DMA_ITConfig(DMA1_Channel4, DMA1_FLAG_GL4, ENABLE);
DMA_Cmd(DMA1_Channel4, DISABLE);
}
}
//rx满中断
void DMA1_Channel5_IRQHandler(void)
{
if(DMA_GetFlagStatus(DMA1_FLAG_TC5) == SET)
{
DMA_ClearFlag(DMA1_FLAG_TC5);
//接收处理函数
JudgeBuffReceive(JudgeReceiveBuffer);
}
}
//Tx满中断
void DMA1_Channel4_IRQHandler(void)
{
if(DMA_GetITStatus(DMA1_IT_TC4)!=RESET)
{
DMA_ClearFlag(DMA1_FLAG_TC4 | DMA1_FLAG_GL4);
DMA_ClearITPendingBit(DMA1_IT_TC4 | DMA1_FLAG_GL4);
DMA_Cmd(DMA1_Channel4,DISABLE);
}
}
serial.h
#ifndef USART_H
#define USART_H
#define START 0X11
#define reBiggestSize 13
#define SendBiggestSize 13
//控制位
#define Reset 0x01
#define Stop 0x02
#include "stm32f10x.h"
typedef union
{
short d;
unsigned char data[2];
}sendData;
//左右轮速控制速度共用体
typedef union
{
short d;
unsigned char data[2];
}receiveData;
//控制车体结构体
typedef struct{
int leftSpeedSet;
int rightSpeedSet;
unsigned char crtlFlag;
char left_move;//为1时方向为正
char right_move;
}Ctrl_information;
//void DATA_DISPOSE_task(void *pvParameters);
//串口接受中断中接收函数
void usartReceiveData(void);
//从linux接收并解析数据到参数地址中
extern int usartReceiveOneData(int *p_leftSpeedSet,int *p_rightSpeedSet,unsigned char *p_crtlFlag);
//封装数据，调用USART1_Send_String将数据发送给linux
extern void usartSendData(short leftVel, short rightVel,short angle,unsigned char ctrlFlag);
//发送指定字符数组的函数
void USART_Send_String(unsigned char *p,unsigned short sendSize);
//计算八位循环冗余校验，得到校验值，一定程度上验证数据的正确性
unsigned char getCrc8(unsigned char *ptr, unsigned short len);
void JudgeBuffReceive(unsigned char ReceiveBuffer[]);
void UART2_DMA_Init(void);
#endif
代码简单介绍：
程序中使用共用体和结构体组织发送的数据包，存放下发的经过解析的数据包。提供了接收函数和发送函数，很好理解，对于想要发送更多数据或者更少数据的小伙伴需要更改发送和接收函数、结构体、公用体等等。
UART2_DMA_Init是初始化串口以DMA的形式，这样可以不占用系统资源，会在后台运行（将数据存到Ctrl_information结构体对象中），后面只需要操作Ctrl_information结构体对象即可获取数据。
上传的数据存放在F103RC_chassis结构体中，可以在定时器中断回调函数中组织此结构体数据，以固定频率发送给上位机。
三、上位机端（旭日x3派）
上位机使用ros1开发，这里默认已经熟悉了ros1的开发流程。
①创建工作空间及功能包
mkdir my_nav_car_ws
cd my_nav_car_ws
mkdir src
cd src
catkin_init_workspace
cd ..
catkin_make
catkin_make install
cd src
catkin_create_pkg uart_tf geometry_msgs nav_msgs roscpp rospy sensor_msgs tf2 tf2_ros cv_bridge
cd ..
catkin_make
cd src/uart_tf/src
touch uartandtf.cpp
②uart节点代码
#include <ros/ros.h>
#include <ros/time.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>
#include <nav_msgs/Odometry.h>
#include <boost/asio.hpp>
#include "tf2_ros/transform_broadcaster.h"
#include <geometry_msgs/Quaternion.h>
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2/LinearMath/Quaternion.h"
#include <array>
#include <thread>
using namespace std;
using namespace boost::asio;
bool ttt;
std_msgs::String msg;
std::stringstream ss;
class MyNode {
public:
std::string Node;
MyNode()
: Node("node"),
sp(iosev, "/dev/ttyUSB0"),
odom_pose_covariance_({
{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}}),
odom_twist_covariance({
{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}})
{
serialInit();
// Create Odometry publisher
odom_publisher = nh_.advertise<nav_msgs::Odometry>("odom", 1);
// Create TransformBroadcaster
tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>();
}
void controlLoop();
private:
boost::asio::io_service iosev;
boost::asio::serial_port sp;
ros::NodeHandle nh_;
ros::Timer timer;
ros::Publisher odom_publisher;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
const unsigned char header[2] = {0x55, 0xaa};
const unsigned char ender[2] = {0x0d, 0x0a};
union sendData {
short d;
unsigned char data[2];
} leftVelSet, rightVelSet;
union receiveData {
short d;
unsigned char data[2];
} leftVelNow, rightVelNow, angleNow;
unsigned char getCrc8(unsigned char *ptr, unsigned short len) {
unsigned char crc;
unsigned char i;
crc = 0;
while (len--) {
crc ^= *ptr++;
for (i = 0; i < 8; i++) {
if (crc & 0x01)
crc = (crc >> 1) ^ 0x8C;
else
crc >>= 1;
}
}
return crc;
}
void serialInit() {
sp.set_option(serial_port::baud_rate(115200));
sp.set_option(serial_port::flow_control(serial_port::flow_control::none));
sp.set_option(serial_port::parity(serial_port::parity::none));
sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
sp.set_option(serial_port::character_size(8));
}
void writeSpeed(double Left_v, double Right_v,unsigned char ctrlFlag) {
unsigned char buf[13] = {0};//
int i, length = 0;
leftVelSet.d = Left_v;//mm/s
rightVelSet.d = Right_v;
for(i = 0; i < 2; i++)
buf[i] = header[i]; //buf[0] buf[1]
length = 5;
buf[2] = length; //buf[2]
for(i = 0; i < 2; i++) {
buf[i + 3] = leftVelSet.data[i]; //buf[3] buf[4]
buf[i + 5] = rightVelSet.data[i]; //buf[5] buf[6]
}
buf[3 + length - 1] = ctrlFlag; //buf[7]
buf[3 + length] = getCrc8(buf, 3 + length);//buf[8]
buf[3 + length + 1] = ender[0]; //buf[9]
buf[3 + length + 2] = ender[1]; //buf[10]
boost::asio::write(sp, boost::asio::buffer(buf));
}
bool readSpeed(double &Left_v,double &Right_v,double &Angle,unsigned char &ctrlFlag) {
unsigned char i, length = 0;
unsigned char checkSum;
unsigned char buf[1024]={0};
boost::system::error_code err;
try {
boost::asio::streambuf response;
boost::asio::read_until(sp, response, "\r\n",err);
copy(istream_iterator<unsigned char>(istream(&response)>>std::noskipws), istream_iterator<unsigned char>(), buf);
} catch(boost::system::system_error &err) {
ROS_INFO("read_until error");
return false;
}
if (buf[0]!= header[0] || buf[1] != header[1]) {
ROS_INFO("Received message header error!");
return false;
}
length = buf[2];
checkSum = getCrc8(buf, 3 + length);
if (checkSum != buf[3 + length]) {
ROS_INFO("Received data check sum error!");
return false;
}
for(i = 0; i < 2; i++) {
leftVelNow.data[i] = buf[i + 3]; //buf[3] buf[4]
rightVelNow.data[i] = buf[i + 5]; //buf[5] buf[6]
angleNow.data[i] = buf[i + 7]; //buf[7] buf[8]
}
ctrlFlag = buf[9];
Left_v =leftVelNow.d;
Right_v =rightVelNow.d;
Angle =angleNow.d;
return true;
}
boost::array<double, 36> odom_pose_covariance_;
boost::array<double, 36> odom_twist_covariance;
};
void MyNode::controlLoop() {
double left_speed_now = 0.0;
double right_speed_now = 0.0;
double angle = 0.0;
unsigned char testRece4 = 0x00;
readSpeed(left_speed_now, right_speed_now, angle, testRece4);
ROS_INFO("left_speed_now=%.2f,right_speed_now =%.2f,angle = %.2f", left_speed_now,right_speed_now,angle );
}
int main(int argc, char **argv) {
ros::init(argc, argv, "Mynode");
ros::NodeHandle nh;
/*rclcpp::init(argc, argv);*/
auto node = std::make_shared<MyNode>();
std::thread control_thread([&]() {
ros::Rate loop_rate(20);
/*rclcpp::Rate loop_rate(20);*/
while (ros::ok()) {
/*while (rclcpp::ok()) {*/
node->controlLoop();
loop_rate.sleep();
}
});
ros::spin();
/*rclcpp::spin(node);*/
ros::shutdown();
/*rclcpp::shutdown();*/
return 0;
}
③cmakelists.txt
添加如下代码，根据自己的情况进行修改。
catkin_package(
CATKIN_DEPENDS roscpp geometry_msgs nav_msgs tf2 cv_bridge sensor_msgs
)
add_executable(uartandtf src/uartandtf.cpp)
target_link_libraries(uartandtf
${catkin_LIBRARIES}
)
install(TARGETS
uartandtf
DESTINATION lib/${PROJECT_NAME}
)
四、测试
代码都准备就绪，可以开始测试了，stm32端大致测试过程如下，由于我代码都已经集成好了，懒得修改，所以以文字形式给出：首先初始化uart、mpu6050、定时器中断、pwm等需要用到的，定时器中断回调函数中不断读取当前左右轮速，每隔20ms串口发送一次数据，样例如下：
usartSendData(left_speed_now,right_speed_now,Yaw,num_buff[1]);
在while循环或者freertos任务中不断读取偏航角，并且打印上位机串口下发的数据：
mpu_dmp_get_data(&Pitch,&Roll,&Yaw);
OLED_ShowSignedNum(2, 7, left_speed_now, 2);
OLED_ShowSignedNum(3, 7, right_speed_now, 2);
我这里用的oled显示，没有的话可以使用串口打印，注意这个串口不能是前面用来与上位机通信的串口。
上下位机都运行之后效果如下：
上位机：
下位机
注：下位机我是用oled可视化，也可以使用串口，oled的代码随便网上搜一搜移植一下就好了。
