这一小节将介绍使用键盘，通过串口发布速度指令。本文所有代码均在网盘 gtihub

1|0上位机部分

1|1创建功能包

cd ~/catkin_ws/src
catkin_create_pkg serial_port std_msgs rospy roscpp

1|2编写串口通讯节点

这个节点需要读取arduino发来的数据并发布出去（为后面发布里程计信息做准备），所以我们可以自定义一个消息类型用于发布这些数据。同时要订阅/cmd_vel话题，获取速度指令，并向下位机发送。那么步骤如下：

创建自定义消息

功能包下创建msg文件夹，进入文件夹创建header.msg文件内容如下

int16 num1
int16 num2
int16 num3

由于我们的机器人在平面运动，所以只有x方向，y方向的速度和绕Z轴旋转的角速度，因此我们只需要定义三个变量即可（在这里我们使用自定义消息类型是为了以后增加串口通讯其他功能时便于修改代码，比如调参等功能，如果仅仅只是发布速度信息，也可以使用已有的消息类型）。

修改CmakeLists.txt和packsge.xml

毕竟我们使用了自定义消息类型，所以以下基础需要修改一下。

#CmakeLists.txt
 find_package(catkin REQUIRED COMPONENTS
  rospy
  std_msgs
  message_generation
)
 
 add_message_files(
  FILES
  header.msg
 )
 
 generate_messages(
   DEPENDENCIES
   std_msgs
   serial_port
 )
 catkin_package(
CATKIN_DEPENDS rospy message_runtime
DEPENDS system_lib
)

#package.xml 对应位置添加如下内容
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

编写串口节点

功能包下创建scripts文件夹用于存放python文件，进入文件夹创建port_SubAndPub.py 内如如下串口收发的格式是用数据与数据用逗号隔开，以换行为结束

#!/usr/bin/env python
# -*- coding: utf-8 -*-
 
 
import rospy
import std_msgs.msg
from serial_port.msg import header  #导入刚刚创建的消息类型
from geometry_msgs.msg import Twist
import serial
import time 
import threading 
 
serialPort = "/dev/ch340"  ##这里我是将arduino的串口芯片id和设备号绑定了，后面会说如何操作，这里大家可以先查一下设备号，打开arduino看看能读到哪个就是那个。
baudRate = 115200
ser = serial.Serial(serialPort, baudRate, timeout=0.5)
print("serial port is %s ,baudRate is %d" % (serialPort, baudRate))
time.sleep(1)
pub = rospy.Publisher('serial_data_odom', header, queue_size=1) ##创建一个发布者，使用我们定义的header类型
 
 
def thread_job():    ##我们这个节点需要同时读取和写入出口，但rospy没有C++的spinOnce，所以需要多线程。
    rospy.spin()
 
def callback(data):  ##定义了回调函数
    str1=str(int(data.linear.x*1000))  #这个data便是cmd_vel话题的所传来的内容
    str2=str(int(data.angular.z*1000)) #原本数据是m/s 现在变成mm/s发送下去
    ser.write("%s,%s\n"%(str1,str2))    #这里对于阿克曼转向，不存在横向平移，所以我就发了两个，大家可以根据自己的小车去增加data.linear.y
    print("----%s,%s"%(str1,str2))
 
 
 
def SubscribeAndPublish():
    rospy.init_node('serial_data_contral', anonymous=True)  #初始化节点
    rospy.Subscriber('cmd_vel', Twist, callback,queue_size=1,buff_size=52428800) #订阅cmd_vel
    # rospy.spin()
    rate = rospy.Rate(20) #设置后面程序读取串口的频率
    add_thread = threading.Thread(target = thread_job) 
    add_thread.start()  #启动线程
 
    while not rospy.is_shutdown(): #下面是用于读取串口发来的数据
        
        get_str = ser.readline()  #读取串口数据，以换行符为结束
        get_str = get_str.strip() 
        #get_str = get_str.decode('utf-8','ignore') 
        print(get_str)
        list_str = get_str.split(',') ##将数据以逗号拆分
        #print(list_str)
        #print("######\n")
 
        msg = header()
        data1 = int(list_str[0])
        data2 = int(list_str[1])
        data3 = int(list_str[2])
        msg.num1 = data1
        msg.num2 = data2
        msg.num3 = data3
        pub.publish(msg)    ##发布
        rate.sleep()
 
if __name__ == '__main__':
    try:
        SubscribeAndPublish()
    except rospy.ROSInterruptException:
        pass
 
 
########################

创建键盘节点

同理创建keyboard.py，只要发布cmd_vel话题，消息类型为twist就可以，可以自己写个简单的，这里我复制了一个为阿克曼小车的键盘控制代码来自Hypha-ROS /hypharos_racecar 直接复制就可以用，但是需要注意以下内容：
从零搭建一辆ROS小车（一）串口通信功能包原本的代码使用的是无刷电机，linear.x等于1500时电机是不转的，所以他的control_speed在默认情况下是1500，control_turn控制的是舵机的角度，所以他的默认值为90，但是我还是把他改了一下，把control_speed改回了速度，control_turn 改为弧度，且默认为0。变成如下

twist = Twist()
twist.linear.x = (control_speed-1500)/100; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = (control_turn-90)*3.14/180
pub.publish(twist)

2|0arduino 串口通讯

直接上代码（因为现在没车在旁边，我就把上位机发来的数据，直接发回去了，后面再更新底盘的运动学模型，以及具体的闭环控制）

String message_c;
const char* message;
int vx=0;
int vy=0;
double vth=0;
int th=0;
int send_vx;
int send_vth;
void setup()
{
  Serial.begin(115200);
  while(Serial.read()>= 0){}                 //清空串口0缓存
 
}
void loop()
{
if (Serial.available())
  { 
   // Serial.println("get");
    while(Serial.available()>0)//当有信号的时候
  {
    message_c +=char(Serial.read());
    delay(1);
  }
 
  message = message_c.c_str();   //由于sscanf只能识别const char*类型字符串，将String类型字符串转成const char*类型
  //Serial.println(message);
  sscanf(message,"%d,%d",&vx,&th);   //串口接收字符串拆分
//Serial.println("have cut");
 
while(Serial.read()>=0){};      //清空串口1缓存，保证字符串的长度稳定
}
 
send_vx=vx;
send_vth=vx*tan(double(th)/1000)/0.2;  //这里注意 由于上位机发来的是舵机角度，里程计计算需要角速度，在这需要将角度转为角速度
 
  Serial.print(send_vx);                                
  Serial.print(",");
  Serial.print(vy);                                
  Serial.print(",");
  Serial.println(send_vth);
  delay(100);
  message_c = "";
}

3|0编译并测试

cd ~/catkin_ws catkin_make 现在让我们插上单片机，选择好串口号测试一下吧 roscore rosrun serial_port port_SubAndPub.py rosrun serial_port keyboard.py

如有错误，请指正，欢迎一起交流 下期更新发布里程计信息，并在rviz中显示位置。