北郵智能車仿真培訓（四）—— 仿真原理詳解

前言

上一節：北郵智能車仿真培訓（三）—— 給車舞臺讓它馳騁
這一節主要介紹下ros常用工具和上一節控制小車的python腳本內容。看會了，你就也可以修改代碼啦。

ROS通信機制

簡單說下ros的通信機制，ros是通過節點將整個網絡互聯的，節點間數據傳輸是TCP協議。
這裏用淘寶做比方，淘寶的商家看成節點，每個客戶也看成節點，淘寶的商家會在自己的店鋪發佈物品，那麼物品就有對應的類型，每個客戶的節點就可以找到我們自己感興趣的商品類型，然後去找到對應的淘寶商家去溝通交流傳遞信息。這裏ros通信也是如此，每個節點都可以發佈和訂閱消息就好比淘寶商家要發佈產品和去進貨一樣，那如果別的節點對某種類型的消息感興趣時，就會在兩者之間建立TCP鏈接，來傳遞消息，就像你想去買一支筆，要的是橙光的牌子，你就會去找市面上發佈的這一類消息，如果找到了，你就可以跟商傢俬聊傳遞信息。那麼怎麼去找的這個過程是ROS底層已經幫你實現了，也不用我們去關心。

ROS工具

rostopic

顯示話題的工具，就像上面說的可以通過這個命令看市面上已經發布的東西。

rosnode

顯示節點的工具，就像上面說的每個客戶和商家是一個個的節點

rqt_graph

ros的qt畫圖工具，可以幫助調試一些數據

rqt_image_view

ros的qt圖像查看工具，可以可視化查看圖片消息

rqt_plot

可視化繪製某一數據的波形圖

rosmsg

ros的消息查看工具，就像上面說的你可以查看商家發佈的筆是什麼具體的牌子
以上的每個命令都可以加 --help來查看具體怎麼使用,這裏不一一說明了
例如:

rostopic --help

Python代碼講解

#!/usr/bin/env python

• 這是告訴編譯器我寫的是一個python的腳本

import rospy, cv2, cv_bridge, numpy
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist

• 第一行是導入ros的py接口包，opencv包，cv_bridge用於ros和opencv圖像轉換的包，numpy是開源的數值計算擴展包
• 第二行從傳感器消息包中導入Image圖片類型，因爲我們要處理ROS的攝像頭的圖片所以需要，
• 第三行ROS用Twist類型的消息控制機器人

class Follower:

• 定義一個類

def __init__(self):
    self.bridge = cv_bridge.CvBridge()

    self.image_sub = rospy.Subscriber('camera/image_raw', 
                                      Image, self.image_callback)
    self.cmd_vel_pub = rospy.Publisher('/cmd_vel',
                                       Twist, queue_size=1)
    self.twist = Twist()

• 第一行是類的構造函數，在聲明這個類對象的時候會調用
• 第二行是定義類的一個成員，他是cv_bridge類型
• 第三行是定義了一個接收圖片消息的類的成員，它訂閱了camera/image_raw這個消息，這個消息的類型是Image，如果有人發佈了這個消息就會進到self.image_callback這個回調函數中
• 第四行定義了一個速度發佈節點的類成員，它發佈的話題是/cmd_vel，消息類型是Twist，queue_size=1這個參數是必要的，意義不用管。
• 第五行定義了一個Twist類型的消息的成員用來控制機器人

#回調函數，每次有對應的消息發佈時就會進來，和單片機的中斷差不多
def image_callback(self, msg):
	#將ros的CvBridge()類型的圖片消息轉換成opencv可以處理的圖片類型
    image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
    #這一段用的是HSV色相環來從藍色和白色混合中過濾出白色，這用單片機是寫不來的，調用的是opencv的庫函數，這裏你們可以用opencv的二值化函數來代替
    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
    lower_white = numpy.array([ 0,  0, 221])
    upper_white = numpy.array([180, 30, 255])
    mask = cv2.inRange(hsv, lower_white, upper_white)
    #這一段是來計算二值化後圖片的重心在哪裏，因爲二值化後白色是1，所以重心便是白色塊的中心
    h, w, d = image.shape
    #這裏縮小搜索的範圍，因爲視角原因一部分圖是無效的不用處理
    search_top = 3*h/4
    search_bot = 3*h/4 + 20
    mask[0:search_top, 0:w] = 0
    mask[search_bot:h, 0:w] = 0
    #這裏計算出重心在圖片上的x,y座標
    M = cv2.moments(mask)
    if M['m00'] > 0:
      cx = int(M['m10']/M['m00'])
      cy = int(M['m01']/M['m00'])
      #將重心畫在圖片上
      cv2.circle(image, (cx, cy), 20, (0,0,255), -1)
      #下面是用來PID計算方向偏差的
      err = cx - w/2
      #這是線速度，這裏的0.2相當於定速0.2m/s
      self.twist.linear.x = 0.2
      #這是角速度這裏的除以50相當於在PID的KP
      self.twist.angular.z = -float(err) / 50
      #這裏將速度消息發佈出去
      self.cmd_vel_pub.publish(self.twist)
      #下面是調試用的顯示處理的圖片
    cv2.imshow("window", image)
    cv2.waitKey(3)

這裏太多了就寫在代碼的註釋裏

rospy.init_node('follower')
follower = Follower()
rospy.spin()

• 第一行定義一個節點
• 第二行調用類的構造函數
• 第三行相當於讓這個腳本不退出，可以一直處理回調函數

結束

下一節：北郵智能車仿真培訓（五）—— 數據可視化工具的使用