ROS上位机发送数据
首先,这里要引入一个名称为serial的包,这个包的安装命令为:
$ sudo apt-get install ros-<版本号>-serial
serial包的介绍:http://wiki.ros.org/serial
接下来,创建一个自己的包,借助serial这个包来编写串口通信的代码。
1、创建一个包,依赖roscpp和serial两个包
catkin_create_pkg serial_port roscpp serial
新建工作空间级程序包:
cd
mkdir -p catkin_ws/src
cd catkin_ws
catkin_make
source devel/setup.bash
cd src
catkin_create_pkg serial_communicate std_msgs roscpp serial
cd serial_communicate/src
新建代码文本key_input_send.cpp
代码:
#include<ros/ros.h>
#include<ros/package.h>
#include<tf/transform_broadcaster.h>
#include<nav_msgs/Odometry.h>
#include<geometry_msgs/Twist.h>
#include<iostream>
#include<serial/serial.h>
#include<sstream>
#include<fstream>
#include<stdio.h>
#include<string.h>
using namespace std;
//创建一个serial类
serial::Serial sp;
typedef union{ //定义一个共用体,用于double数据与16进制的转换
unsigned char cvalue[4];
float fvalue;
}float_union;
static uint8_t s_buffer[10];
void callback(const geometry_msgs::Twist::ConstPtr& cmd_vel)
{
float_union linear_x ,angular_z;
memset(s_buffer,0,sizeof(s_buffer));
linear_x.fvalue = cmd_vel->linear.x;
angular_z.fvalue = cmd_vel->angular.z;
//打包数据
s_buffer[0] = 0xff;//帧头
s_buffer[1] = linear_x.cvalue[0];
s_buffer[2] = linear_x.cvalue[1];
s_buffer[3] = linear_x.cvalue[2];
s_buffer[4] = linear_x.cvalue[3];
s_buffer[5] = angular_z.cvalue[0];
s_buffer[6] = angular_z.cvalue[1];
s_buffer[7] = angular_z.cvalue[2];
s_buffer[8] = angular_z.cvalue[3];
s_buffer[9] = 0xfe;//帧尾
sp.write(s_buffer,10);
ROS_INFO("data send successful");
}
int rosSerial_init(string name, int baud)
{
//创建timeout
serial::Timeout to = serial::Timeout::simpleTimeout(100);
//设置要打开的串口名称
sp.setPort(name);
//设置串口通信的波特率
sp.setBaudrate(baud);
//串口设置timeout
sp.setTimeout(to);
try
{
sp.open();
}
catch(serial::IOException& e)
{
ROS_ERROR_STREAM("Unable to open port.");
return -1;
}
//判断串口是否打开成功
if(sp.isOpen())
{
ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
}
else
{
return -1;
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "cmd_vel_listener");
ros::NodeHandle n;
//ros::Subscriber sub = n.subscribe("cmd_vel", 1000, callback);
rosSerial_init("/dev/ttyUSB0",115200);
ros::Subscriber sub = n.subscribe("/turtle1/cmd_vel", 1000, callback);//用/turtle1/cmd_vel
ros::Rate loop_rate(50);
while(n.ok())
{
ros::spinOnce();
loop_rate.sleep();
}
sp.close();
return 1;
}
先打开roscore
这里是借助自带的小海龟发布的功能包,发布的`/turtle1/cmd_vel`消息类型,触发回调函数:
rosrun turtlesim turtle_teleop_key
运行功能包
rosrun serial_communitcate key_input_send
stm32接收数据
使用触发中断函数来接收ROS发来的信息,由于串口之间都是16进制进行传输,所以涉及使用共用体来讲double数据与16进制数据进行转换。
帧头与帧尾一样才会接收数据
switch(recv_state)
{
case 0:
if(s_buf == 0xff)
{
recv_state = 1;
rxd_index = 0;
}
else
{
recv_state = 0;
}
break;
case 1:
rxd_buf[rxd_index] = s_buf;
rxd_index++;
if(rxd_index >= 8)
{
recv_state = 2;
}
break;
case 2:
if(s_buf == 0xfe)
{
rxd_flag = 1;
recv_state = 0;
}
break;
}
本次是判断接收的数据是否等于2,如果等于2,则点亮开发板的灯PFout(9)=0,低电平;(已提前知道,ROS使用小海龟发送的线速度等于2)。
while(1)
{
if(rxd_flag == 1)
{
float value = transform(rxd_buf);
if(value == 2)
{
PFout(9) = 0;
}
rxd_flag = 0;
}
stm32F407zet6,具体代码:
#include "stm32f4xx.h"
#include "sys.h"
static GPIO_InitTypeDef GPIO_InitStructure;
static NVIC_InitTypeDef NVIC_InitStructure;
static USART_InitTypeDef USART_InitStructure;
typedef union
{
unsigned char cvalue[4];
float fvalue;
}float_union;
uint8_t rxd_buf[8];//接收固定包长度缓冲区
uint8_t rxd_flag = 0;//接收完成标志
uint8_t rxd_index = 0;//接收字节索引
void delay_us(uint32_t n)
{
SysTick->CTRL = 0; // Disable SysTick,关闭系统定时器
SysTick->LOAD = (168*n)-1; // 配置计数值(168*n)-1 ~ 0
SysTick->VAL = 0; // Clear current value as well as count flag
SysTick->CTRL = 5; // Enable SysTick timer with processor clock
while ((SysTick->CTRL & 0x10000)==0);// Wait until count flag is set
SysTick->CTRL = 0; // Disable SysTick
}
void delay_ms(uint32_t n)
{
while(n--)
{
SysTick->CTRL = 0; // Disable SysTick,关闭系统定时器
SysTick->LOAD = (168000)-1; // 配置计数值(168000)-1 ~ 0
SysTick->VAL = 0; // Clear current value as well as count flag
SysTick->CTRL = 5; // Enable SysTick timer with processor clock
while ((SysTick->CTRL & 0x10000)==0);// Wait until count flag is set
}
SysTick->CTRL = 0; // Disable SysTick
}
float transform(uint8_t rxd_buf_trans[8])//使用共用体进行数据的转换
{
float_union fr;
float_union fe;
fr.cvalue[0] = rxd_buf_trans[0];
fr.cvalue[1] = rxd_buf_trans[1];
fr.cvalue[2] = rxd_buf_trans[2];
fr.cvalue[3] = rxd_buf_trans[3];
fe.cvalue[0] = rxd_buf_trans[4];
fe.cvalue[1] = rxd_buf_trans[5];
fe.cvalue[2] = rxd_buf_trans[6];
fe.cvalue[3] = rxd_buf_trans[7];
return fr.fvalue;
}
void usart1_init(uint32_t baud)
{
//打开PA硬件时钟
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
//打开串口1硬件时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
//配置PA9和PA10为复用功能模式
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9|GPIO_Pin_10; //第9 10根引脚
GPIO_InitStructure.GPIO_Mode= GPIO_Mode_AF; //多功能模式
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽输出,增加输出电流能力。
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//高速响应
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; //没有使能上下拉电阻
GPIO_Init(GPIOA,&GPIO_InitStructure);
//将PA9和PA10引脚连接到串口1的硬件
GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1);
GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1);
//配置串口1相关参数:波特率、无校验位、8位数据位、1个停止位......
USART_InitStructure.USART_BaudRate = baud; //波特率
USART_InitStructure.USART_WordLength = USART_WordLength_8b; //8位数据位
USART_InitStructure.USART_StopBits = USART_StopBits_1; //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);
//配置串口1的中断优先级
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//中断向量的入口地址
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
//使能串口1工作
USART_Cmd(USART1, ENABLE);
}
int main(void)
{
//使能(打开)端口E的硬件时钟,就是对端口E供电
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
//使能(打开)端口F的硬件时钟,就是对端口F供电
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOF, ENABLE);
//初始化GPIO引脚
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9|GPIO_Pin_10; //第9 10根引脚
GPIO_InitStructure.GPIO_Mode= GPIO_Mode_OUT; //输出模式
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽输出,增加输出电流能力。
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//高速响应
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; //没有使能上下拉电阻
GPIO_Init(GPIOF,&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13|GPIO_Pin_14; //第13 14根引脚
GPIO_InitStructure.GPIO_Mode= GPIO_Mode_OUT; //输出模式
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽输出,增加输出电流能力。
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//高速响应
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; //没有使能上下拉电阻
GPIO_Init(GPIOE,&GPIO_InitStructure);
PFout(9)=1;
PFout(10)=1;
PEout(13)=1;
PEout(14)=1;
//串口1波特率:115200bps
usart1_init(115200);
//将接收到的数据,返发给PC
USART_SendData(USART1,rxd_buf[0]);
while(USART_GetFlagStatus(USART1,USART_FLAG_TXE)==RESET);
while(1)
{
if(rxd_flag == 1)
{
float value = transform(rxd_buf);
if(value == 2)
{
PFout(9) = 0;
}
rxd_flag = 0;
}
delay_ms(30);
}
}
void USART1_IRQHandler(void)
{
uint8_t s_buf;
static uint8_t recv_state = 0;
static USART_RX_STA = 0;
//检测标志位
if(USART_GetITStatus(USART1,USART_IT_RXNE) == SET)
{
//接收数据
s_buf = USART_ReceiveData(USART1);
switch(recv_state)
{
case 0:
if(s_buf == 0xff)
{
recv_state = 1;
rxd_index = 0;
}
else
{
recv_state = 0;
}
break;
case 1:
rxd_buf[rxd_index] = s_buf;
rxd_index++;
if(rxd_index >= 8)
{
recv_state = 2;
}
break;
case 2:
if(s_buf == 0xfe)
{
rxd_flag = 1;
recv_state = 0;
}
break;
}
//清空标志位
USART_ClearITPendingBit(USART1,USART_IT_RXNE);
}
}
借鉴:
https://blog.csdn.net/qq_41091426/article/details/114221640