Gazebo环境下基于ROS和OpenCV的阿克曼小车巡线
1. 创建资源
开始实验之前，您需要先创建实验相关资源。
- 在实验室页面，单击创建资源。
- （可选）在实验室页面左侧导航栏中，单击云产品资源列表，可查看本次实验资源相关信息（例如IP地址、子用户信息等）。
说明：资源创建过程需要3～5分钟（视资源不同开通时间有所差异，ACK等资源开通时间较长）。完成实验资源的创建后，您可以在云产品资源列表查看已创建的资源信息，例如：子用户名称、子用户密码、AK ID、AK Secret、资源中的项目名称等。
实验环境一旦开始创建则进入计时阶段，建议学员先基本了解实验具体的步骤、目的，真正开始做实验时再进行创建。
资源创建成功，可在左侧的资源卡片中查看相关资源信息以及RAM子账号信息
2. 进入Docker镜像
打开火狐浏览器。
通过RAM用户名密码登录页面，输入子用户密码登录账号。
登陆后选择左侧产品与服务中的云服务器ECS。
在所有云服务器中通过左下角地域找到正确的服务器并点击远程连接。
选择通过VNC远程连接。
选择重置VNC密码并设置新的密码后登录。
设置好进入各选项后进入Ubuntu桌面。
右键打开终端，输入sudo docker run -it --rm -p 6080:80 aliyun_demo命令运行镜像aliyun_demo。
运行成功后打开浏览器，在地址一栏输入127.0.0.1:6080进入镜像系统。
3. 代码部分
巡线程序使用的是raceworld1场景，由raceworld1.launch文件启动。找到/root/aliyun_demo/src/raceworld/launch文件夹中的raceworld1.launch文件并打开。
在launch文件内加载了多个控制器，其中left_rear_wheel_velocity_controller, right_rear_wheel_velocity_controller,left_front_wheel_velocity_controller,right_front_wheel_velocity_controller,left_steering_hinge_position_controller和right_steering_hinge_position_controller接收ROS消息，分别控制了左右后轮速度，左右前轮速度和转向角度。
<node name="controller_manager" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="left_rear_wheel_velocity_controller right_rear_wheel_velocity_controller left_front_wheel_velocity_controller right_front_wheel_velocity_controller left_steering_hinge_position_controller right_steering_hinge_position_controller joint_state_controller"/>
在加载控制器代码的下面，还加载了一个名为servo_comannds1.py的代码文件。
<nodepkg="raceworld" type="servo_commands.py" name="servo_commands" output="screen" >
打开/root/aliyun_demo/src/raceworld/scripts文件夹中的servo_comannds1.py文件我们可以看到，代码中新建了一个名为servo_comannds的节点，他订阅并接收/deepracer1/ackermann_cmd_mux/output消息。
def servo_commands(): rospy.init_node('servo_commands', anonymous=True) robot_name = rospy.get_param('~robot_name') rospy.Subscriber("ackermann_cmd_mux/output", AckermannDriveStamped, set_throttle_steer) # spin() simply keeps python from exiting until this node is stopped rospy.spin() if __name__ == '__main__': try: servo_commands() except rospy.ROSInterruptException: pass
在接收到消息后，经过适当的转换，将速度和转向信息发布给之前所述六个控制器。
def set_throttle_steer(data): global flag_move pub_vel_left_rear_wheel = rospy.Publisher("left_rear_wheel_velocity_controller/command", Float64, queue_size=1) pub_vel_right_rear_wheel = rospy.Publisher("right_rear_wheel_velocity_controller/command", Float64, queue_size=1) pub_vel_left_front_wheel = rospy.Publisher("left_front_wheel_velocity_controller/command", Float64, queue_size=1) pub_vel_right_front_wheel = rospy.Publisher("right_front_wheel_velocity_controller/command", Float64, queue_size=1) pub_pos_left_steering_hinge = rospy.Publisher("left_steering_hinge_position_controller/command", Float64, queue_size=1) pub_pos_right_steering_hinge = rospy.Publisher("right_steering_hinge_position_controller/command", Float64, queue_size=1) throttle = data.drive.speed*28 steer = data.drive.steering_angle pub_vel_left_rear_wheel.publish(throttle) pub_vel_right_rear_wheel.publish(throttle) pub_vel_left_front_wheel.publish(throttle) pub_vel_right_front_wheel.publish(throttle) pub_pos_left_steering_hinge.publish(steer) pub_pos_right_steering_hinge.publish(steer)
在launch文件中，还加载了一个名为control_servo.py的文件。
<nodepkg="raceworld" type="control_servo.py" name="control_servo" output="screen"/>
打开该文件，可以看到新建了一个名为control_servo的节点，并订阅了Twist类型的cmd_akm1消息。
def cmd_to_akm(): rospy.init_node('control_servo', anonymous=True) rospy.Subscriber("cmd_akm1", Twist, callback1) rospy.spin() pass if __name__ =='__main__': cmd_to_akm() pass
当接收到订阅的消息后，新建/deepracer1/ackermann_cmd_mux/output主题的发布者，并将AckermannDriveStamped类型的消息数据设置为合适的值后发布出去。
def callback1(msg): pub = rospy.Publisher("/deepracer1/ackermann_cmd_mux/output", AckermannDriveStamped, queue_size=10) akm = AckermannDriveStamped() if msg.linear.x != 0: akm.drive.speed = msg.linear.x*1.80 akm.drive.steering_angle = atan((msg.angular.z/msg.linear.x)*0.133) else: akm.drive.speed = 0 akm.drive.steering_angle = 0 print("Speed:",akm.drive.speed) print("Steering_Angle:",akm.drive.steering_angle) print("Message From control_servo.py Published\n") pub.publish(akm) pass
巡线程序需要运行/root/aliyun_demo/src/raceworld/scripts文件夹中的follow.py文件。在代码中首先新建了follower节点，并且订阅了名为/deepracer1/camera/zed_left/image_rect_color_left的消息。该消息通过Gazebo中的deepracer小车上的左摄像头获取。在获取到摄像头图像信息后，把它转换为cv2图像数据。
def image_callback(msg): bridge = cv_bridge.CvBridge() frame = bridge.imgmsg_to_cv2(msg, 'bgr8') follow_line(frame, "yellow") pass if __name__ == '__main__': rospy.init_node("follower") rospy.Subscriber("/deepracer1/camera/zed_left/image_rect_color_left", Image, image_callback) rospy.spin() pass
在follow_line函数中，先对图像进行HSV转换和二值图转换操作。
def follow_line(image, color): cmd_vel_pub = rospy.Publisher("cmd_akm1", Twist, queue_size=10) #转换为HSV图像 hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) #cv2.namedWindow("hsv image",0) #cv2.imshow("hsv image",hsv) lower_yellow = numpy.array([26, 43, 46]) upper_yellow = numpy.array([34, 255, 255]) #转换为二值图，黄色范围内的车道线为白色，其他范围为黑色 mask = cv2.inRange(hsv, lower_yellow, upper_yellow) #cv2.namedWindow("binary image",0) #cv2.imshow("binary image",mask)
HSV图如下所示。
二值图如下所示。
接着设置图像的感兴趣区域(ROI)。
#设置感兴趣区域ROI mask=set_roi_forward(h,w,mask) cv2.namedWindow("region of interest",0) cv2.imshow("region of interest",mask)
ROI图像如下所示。可以看到感兴趣区域只显示小车镜头前的一部份区域。
接着调用cv2.moments函数获得图像的矩并计算质心，根据质心与图像中线的偏移设置转角，并将速度设置为0.1。
#获得图像矩 M = cv2.moments(mask) if M['m00'] > 0: cx = int(M['m10'] / M['m00'])-235 cy = int(M['m01'] / M['m00']) # print(cx, cy) #在质心画圆 cv2.circle(image, (cx, cy), 20, (0, 0, 255), -1) err = cx - w / 2 - 50 twist = Twist() twist.linear.x = 0.1 #根据质心与图像中线的偏移设置转角 twist.angular.z = -float(err / 2.0) / 50 print("Linear:",twist.linear.x) print("Angular:",twist.angular.z) print("Message From follow.py Published\n") cmd_vel_pub.publish(twist) cv2.namedWindow("camera",0) cv2.imshow("camera", image) cv2.waitKey(1) pass
同时以质心为中心点在原始图像上绘制红色的圆形图案作为参考，如图所示。
4. 实验操作
打开终端输入如图命令，启动raceworld1场景。
cd aliyun_demo source devel/setup.bash roslaunch raceworld raceworld1.launch
打开新的终端输入如图所示命令，开启巡线程序。
cd aliyun_demo source devel/setup.bash rosrun raceworld follow.py
在终端中可以查看到消息的发布与接收。
小车也会沿着车道线自动行驶。
在程序运行的同时新建终端，输入rostopic list命令，我们可以找到/cmd_akm1主题。
输入rostopic info /cmd_akm1命令后可以查看该主题的发布者与订阅者，分别为follow.py中创建的follower节点和control_servo.py中创建的control_servo节点，与代码部分一致。
实验地址：https://developer.aliyun.com/adc/scenario/330fdee29ae947f7b18bc3b059ba1c46
