1 cd ~/catkin_ws/src
2 catkin_create_pkg serial_port std_msgs rospy roscpp

int16 num1
int16 num2
int16 num3

 1 #CmakeLists.txt
 2  find_package(catkin REQUIRED COMPONENTS
 3   rospy
 4   std_msgs
 5   message_generation
 6 )
 7 
 8  add_message_files(
 9   FILES
10   header.msg
11  )
12 
13  generate_messages(
14    DEPENDENCIES
15    std_msgs
16    serial_port
17  )
18  catkin_package(
19 CATKIN_DEPENDS rospy message_runtime
20 DEPENDS system_lib
21 )

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

#!/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


########################

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)

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 = "";
}