BUPT smart car simulation training (four) - Detailed simulation of Principle

Foreword

Previous: Beijing University of Posts and Telecommunications smart car simulation training (three) - to stage car ride it
ros common tools and python scripts to control content on a car under this section describes. You will see, and you can modify the code it.

ROS communication mechanism

Briefly communication mechanism of the ros, ros by the entire network of interconnected nodes, data transfer between nodes is the TCP protocol.
Let's here to do with Taobao, Taobao's business as nodes, each node as customers, Taobao businesses will publish articles in their own shop, then there is a corresponding item type, each client node can find our own commodity type of interest, and then to find the corresponding business Taobao to communicate to transmit information. The same is true here ros communication, each node can publish and subscribe messaging like the same Taobao business to publish products and to purchase, if that time interested in a certain type of message will be set up between two other nodes TCP link to deliver the message, just as you want to buy a pen, to a brand of orange light, you will find this type of news release on the market, if found, you can whisper with the business Send message. So how this process is to find the underlying ROS have to help you achieve, and we do not have to care.

ROS Tools

rostopic

The tool displays the topic, as seen above you can see what the market has been released by this command.

rosnode

Tools display nodes, as said above, each customer and the merchant is one of the nodes

rqt_graph

ros of qt drawing tool that can help debug some data

rqt_image_view

qt ros image viewing tool, can be visualized view picture messages

rqt_plot

Visualization of a drawing waveform data of FIG.

rosmsg

ros news viewer, as said above, you can see the business posted pen is any specific brand
Each of the above commands can add --help to see how to use specific, not setting them up here
E.g:

rostopic --help

Python code to explain

#!/usr/bin/env python

• This tells the compiler that I wrote a python script

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

• The first line is the introduction ros py interface packet, opencv package, cv_bridge ros and opencv for image conversion package, numpy open source package numeric extension
• Introducing the second row from the sensor type Image Image message packets, because we are dealing with a camera image of ROS is required,
• The third line ROS Twist robot control message type

class Follower:

• The definition of a class

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()

• The first line is the constructor of the class, in a statement this time will call the class object
• The second line is a member of the class definition, he is the type cv_bridge
• The third line is the definition of a member of the class to receive picture messages, it subscribed to the camera / image_raw news, news of this type is Image, if someone posts this message will reach the self.image_callback this callback function
• The fourth line defines a class member posted speed node, which is the topic of publishing / cmd_vel, the message type is Twist, queue_size = 1 This parameter is necessary, meaning the tube.
• The fifth line defines a Twist of members of the types of messages used to control the robot

#回调函数，每次有对应的消息发布时就会进来，和单片机的中断差不多
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)

Here too much to write in the comments in the code

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

• The first line defines a node
• The second line calls the constructor of the class
• The third line is equivalent to let the script do not quit, you can always handle the callback function