北郵智能車仿真培訓（三）—— 給車舞臺讓它馳騁

前言

上一節：北郵智能車仿真培訓（二）—— 搭建仿真模型
這一節內容如題，搭建跑道，並讓車尋跡，至於是馳騁還是龜速，就看你了。

搭建跑道

怎麼搭建也不是重點，這裏直接拷貝我的包吧。

• csdn資源：點這裏
• 北郵智能車的羣文件

下面幾步注意一下，小朋友們～

• 來到home目錄下，如圖：
  
• 按ctrl + h顯示隱藏文件如圖：
  
  看到了嗎多了很多以“.”開頭的文件夾
• 打開.gazebo文件夾如圖：
  
• 打開models文件夾（如果沒有你就自己建一個）
  把剛纔下載的文件放進去，有些憨憨肯定解壓又多了一層目錄，記得刪了
  
  我這裏還下了別的模型，你用不上，不用管。
• 文件my_ground_plane裏面是這樣的:
  
  到這裏就大功告成了。

加載跑道

打開我們的模型

roslaunch smartcar_description smartcar_display_gazebo3.launch 

左上角一欄點擊Insert

找到My Ground Plane(你那裏應該沒有我這裏這麼多文件)

點擊一下My Groud Plane鬆開，鼠標移到你的車模上地圖就帶過來了：

然後左上方有個可以移動車模的位置，我們把車放到跑道上：

讓它給我爬！

在我們第一節講的程序包的src下創建Python腳本讓它動起來：

cd ~/smartcar_ws/src/buptsmartcar/src
gedit findLine.py

findLine.py:

#!/usr/bin/env python
# BEGIN ALL
import rospy, cv2, cv_bridge, numpy
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist

class Follower:
  def __init__(self):
    self.bridge = cv_bridge.CvBridge()
    #cv2.namedWindow("window", 1)
    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()
  def image_callback(self, msg):
    image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
    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)
    
    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
    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)
      # BEGIN CONTROL
      err = cx - w/2
      self.twist.linear.x = 0.2
      self.twist.angular.z = -float(err) / 50
      self.cmd_vel_pub.publish(self.twist)
      # END CONTROL
    cv2.imshow("window", image)
    cv2.waitKey(3)

rospy.init_node('follower')
follower = Follower()
rospy.spin()
# END ALL

給文本賦予可執行的權限：

chmod 777 *

執行腳本：

cd ~/smartcar_ws/src/buptsmartcar/src
python findLine.py

好了，小車開始巡線，右邊的圖是我用opencv顯示的圖像方便調試。

結束

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