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