使用jetson nano 进行ros端和stm32之间的通讯2
ros端向stm32发送消息
1、stm32端接受程序
串口初始化,主要是打开接受中断
void uart3_init(u32 bound)
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); //Enable the gpio clock //使能GPIO时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); //Enable the Usart clock //使能USART时钟
GPIO_PinAFConfig(GPIOC,GPIO_PinSource10,GPIO_AF_USART2);
GPIO_PinAFConfig(GPIOC,GPIO_PinSource11,GPIO_AF_USART2);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11;
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF; //输出模式
GPIO_InitStructure.GPIO_OType=GPIO_OType_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz; //高速50MHZ
GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP; //上拉
GPIO_Init(GPIOC, &GPIO_InitStructure); //初始化
//UsartNVIC configuration //UsartNVIC配置
NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
//Preempt priority //抢占优先级
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2 ;
//Preempt priority //抢占优先级
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
//Enable the IRQ channel //IRQ通道使能
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
//Initialize the VIC register with the specified parameters
//根据指定的参数初始化VIC寄存器
NVIC_Init(&NVIC_InitStructure);
//USART Initialization Settings 初始化设置
USART_InitStructure.USART_BaudRate = bound; //Port rate //串口波特率
USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //字长为8位数据格式
USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //一个停止
USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //无奇偶校验位
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //无硬件数据流控制
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //Sending and receiving mode //收发模式
USART_Init(USART3, &USART_InitStructure); //Initialize serial port 3 //初始化串口3
USART_ITConfig(USART3, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //开启串口接受中断
USART_Cmd(USART3, ENABLE); //Enable serial port 3 //使能串口3
}
串口接受程序,在中断中处理
//将上位机发过来的高8位和低8位数据整合成一个short型数据后,再做单位还原换算
float XYZ_Target_Speed_transition(u8 High,u8 Low)
{
//数据转换的中间变量
short transition;
//将高8位和低8位整合成一个16位的short型数据
transition=((High<<8)+Low);
return
transition/1000+(transition%1000)*0.001; mm/s->m/s //单位转换, mm/s->m/s
}
int USART3_IRQHandler(void)
{
static u8 Count=0;
u8 Usart_Receive;
if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)//判断是否接收到数据
{
Usart_Receive = USART_ReceiveData(USART3); //读取数据
//串口数据填入数组
Receive_Data.buffer[Count]=Usart_Receive;
//确保数组第一个数据为FRAME_HEADER
if(Usart_Receive == FRAME_HEADER||Count>0)
Count++;
else
Count=0;
if (Count == 11) //验证数据包的长度
{
Count=0; //为串口数据重新填入数组做准备
if(Receive_Data.buffer[10] == FRAME_TAIL)//验证数据包的帧尾
{
//数据异或位校验计算,模式0是发送数据校验
if(Receive_Data.buffer[9] ==Check_Sum(9,0))
{
float Vz;
//从串口数据求三轴目标速度, 单位m/s
Move_X=XYZ_Target_Speed_transition(Receive_Data.buffer[3],Receive_Data.buffer[4]);
Move_Y=XYZ_Target_Speed_transition(Receive_Data.buffer[5],Receive_Data.buffer[6]);
Move_Z=XYZ_Target_Speed_transition(Receive_Data.buffer[7],Receive_Data.buffer[8]);
Vz =XYZ_Target_Speed_transition(Receive_Data.buffer[7],Receive_Data.buffer[8]);
}
}
}
}
return 0;
}
对得到的数据进行运动学分解,分解为每个轮子的转速
Drive_Motor(Move_X,Move_Y,Move_Z)
void Drive_Motor(float Vx,float Vy,float Vz)
{
float amplitude=3.5;//车轮目标速度限幅
//运动学逆解
MOTOR_A.Target = +Vy+Vx-Vz*(Axle_spacing+Wheel_spacing);
MOTOR_B.Target = -Vy+Vx-Vz*(Axle_spacing+Wheel_spacing);
MOTOR_C.Target = +Vy+Vx+Vz*(Axle_spacing+Wheel_spacing);
MOTOR_D.Target = -Vy+Vx+Vz*(Axle_spacing+Wheel_spacing);
//车轮(电机)目标速度限幅
MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-amplitude,amplitude);
MOTOR_D.Target=target_limit_float(MOTOR_D.Target,-amplitude,amplitude);
}
2、ros端发送程序
turn_on_robot::turn_on_robot():Sampling_Time(0),Power_voltage(0)
{
//清空数据
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("~");//创建节点句柄
//private_nh.param()入口参数分别对应:参数服务器上的名称 参数变量名 初始值
private_nh.param<std::string>("usart_port_name", usart_port_name, "/dev/wheeltec_controller"); //固定串口号
private_nh.param<int> ("serial_baud_rate", serial_baud_rate, 115200); //和下位机通信波特率115200
private_nh.param<std::string>("odom_frame_id", odom_frame_id, "odom_combined"); //里程计话题对应父TF坐标
private_nh.param<std::string>("robot_frame_id", robot_frame_id, "base_footprint");coordinates //里程计话题对应子TF坐标
private_nh.param<std::string>("gyro_frame_id", gyro_frame_id, "gyro_link"); //IMU话题对应TF坐标
voltage_publisher = n.advertise<std_msgs::Float32>("PowerVoltage", 10); //创建电池电压话题发布者
odom_publisher = n.advertise<nav_msgs::Odometry>("odom", 50); //创建里程计话题发布者
imu_publisher = n.advertise<sensor_msgs::Imu>("imu", 20); //创建IMU话题发布者
//速度控制命令订阅回调函数设置
Cmd_Vel_Sub = n.subscribe("cmd_vel", 100, &turn_on_robot::Cmd_Vel_Callback, this);
ROS_INFO_STREAM("Data ready"); //提示信息
try
{
//尝试初始化与开启串口
Stm32_Serial.setPort(usart_port_name); //选择要开启的串口号
Stm32_Serial.setBaudrate(serial_baud_rate); //设置波特率
serial::Timeout _time = serial::Timeout::simpleTimeout(2000); //超时等待
Stm32_Serial.setTimeout(_time);
Stm32_Serial.open(); //开启串口
}
catch (serial::IOException& e)
{
ROS_ERROR_STREAM("wheeltec_robot can not open serial port,Please check the serial port cable! "); //如果开启串口失败,打印错误信息
}
if(Stm32_Serial.isOpen())
{
ROS_INFO_STREAM("wheeltec_robot serial port opened"); //串口开启成功提示
}
}
速度话题订阅回调函数Callback,根据订阅的指令通过串口发指令控制下位机
void turn_on_robot::Cmd_Vel_Callback(const geometry_msgs::Twist &twist_aux)
{
short transition; //中间变量
Send_Data.tx[0]=FRAME_HEADER; //帧头0X7B
Send_Data.tx[1] = 0; //预留位
Send_Data.tx[2] = 0; //预留位
//机器人x轴的目标线速度
transition=0;
transition = twist_aux.linear.x*1000; //将浮点数放大一千倍,简化传输
Send_Data.tx[4] = transition; //取数据的低8位
Send_Data.tx[3] = transition>>8; //取数据的高8位
//机器人y轴的目标线速度
transition=0;
transition = twist_aux.linear.y*1000;
Send_Data.tx[6] = transition;
Send_Data.tx[5] = transition>>8;
//机器人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); //BCC校验位,规则参见Check_Sum函数
Send_Data.tx[10]=FRAME_TAIL; //帧尾0X7D
try
{
Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //通过串口向下位机发送数据
}
catch (serial::IOException& e)
{
ROS_ERROR_STREAM("Unable to send data through serial port"); //如果发送数据失败,打印错误信息
}
}
3 、配置启动launch文件
<launch>
<!--打开节点wheeltec_robot,初始化串口等操作-->
<arg name="odom_frame_id" default="odom_combined"/>
<node pkg="turn_on_wheeltec_robot" type="wheeltec_robot_node" name="wheeltec_robot" output="screen" respawn="false">
<param name="usart_port_name" type="string" value="/dev/wheeltec_controller"/>
<param name="serial_baud_rate" type="int" value="115200"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="robot_frame_id" type="string" value="base_footprint"/>
<param name="gyro_frame_id" type="string" value="gyro_link"/>
</node>
</launch>
4 、 配置串口软连接
通过写脚本***.sh
echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0002", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_controller"' >/etc/udev/rules.d/wheeltec_controller.rules
运行
sudo ./***.sh