從零搭建一輛ROS小車 （一）串口通信功能包

這一小節將介紹使用鍵盤，通過串口發佈速度指令。

本文所有代碼均在  網盤 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 直接複製就可以用，但是需要注意以下內容：

 

 

 

 

 

原本的代碼使用的是無刷電機，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中顯示位置。