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-master/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/0final/singleCar_changePath文件夹中的running_car2.py文件。在代码中首先新建了get_camera节点，并且订阅了名为/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("get_camera") rospy.Subscriber("/deepracer1/camera/zed_left/image_rect_color_left", Image, image_callback) rospy.spin() pass
在follow_line_out和follow_line_in函数中，先对图像进行HSV转换和二值图转换操作。
cmd_vel_pub = rospy.Publisher("cmd_akm1", Twist, queue_size=10) kernel = np.ones((7,7), np.uint8) #转换为HSV图像 hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) lower_yellow = np.array([26, 43, 46]) lower_yellow = np.array([26, 43, 0]) upper_yellow = np.array([34, 255, 255]) #对车道线图像二值化 mask1 = cv2.inRange(hsv, lower_yellow, upper_yellow) h, w = mask1.shape
HSV图如下所示。
二值图如下所示。
接着进行膨胀操作，并分别设置车道线和车道边界的感兴趣区域(ROI)。
#膨胀操作 mask1 = cv2.dilate(mask1, kernel, 2) #设置感兴趣区域ROI mask1 = set_roi_forward1(h, w, mask1) lower_gray = np.array([0, 0, 120]) upper_gray = np.array([0, 0, 200]) #对车道边界二值化 mask2 = cv2.inRange(hsv, lower_gray, upper_gray) #膨胀操作 mask2 = cv2.dilate(mask2, kernel, 2) #设置感兴趣区域ROI mask2 = set_roi_forward2(h, w, mask2) #cv2.namedWindow("mask1",0) #cv2.imshow("mask1", mask1) #cv2.namedWindow("mask2",0) #cv2.imshow("mask2", mask2)
ROI图像如下所示。可以看到感兴趣区域只显示小车镜头前的一部份区域。
接着调用cv2.moments函数获得图像的矩并计算质心，根据质心与图像中线的偏移设置转角，并设置速度。
#获得图像矩 M1 = cv2.moments(mask1) M2 = cv2.moments(mask2) if M1['m00'] == 0: cx1 = 630 if M2['m00'] > 0: cx2 = int(M2['m10'] / M2['m00']) elif M2['m00'] == 0: cx2 = 0 cy = 0 pass elif M1['m00'] > 0: cx1 = int(M1['m10'] / M1['m00']) if M2['m00'] > 0: cx2 = int(M2['m10'] / M2['m00']) elif M2['m00'] == 0: cx2 = 0 cy = int(M1['m01'] / M1['m00']) cx = int((cx1-cx2)*50/64+cx2) #在质心画圆 cv2.circle(image, (cx, cy), 20, (0, 0, 255), -1) err = cx - w / 2 - 60 twist = Twist() twist.linear.x = 0.05 #根据质心与图像中线的偏移设置转角 twist.angular.z = -float(err / 5.0) / 50 cmd_vel_pub.publish(twist) cv2.namedWindow("Camera",0) cv2.imshow("Camera", image)
同时以质心为中心点在原始图像上绘制红色的圆形图案作为参考，如图所示。
在行驶过程中，对摄像头获取到的图像信息与本地路牌图片进行模板匹配并获得其中最大值。
def do_match(frame): max = 0.0 #读取本地图片 template = cv2.imread(r"/home/chouu/aliyun_demo/src/raceworld/scripts/01.png") #摄像头图像与本地图片进行模板匹配，返回一个矩阵，每个像素值代表匹配程度 res = cv2.matchTemplate(frame, template, cv2.TM_CCOEFF_NORMED) #获得矩阵中最大值及位置和最小值及位置 min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(res) if max_val>max: max = max_val return max pass
当最大值达到一定程度后表示识别到停车指示路牌，调用stop函数实现停车。
match = do_match(img) #print(match) #由于行驶在内道，距离路牌较远，所以匹配值要求降低 if match > 0.57: stop(1) print("stop now!") time.sleep(3) rospy.signal_shutdown("~~~")
#停止行驶 def stop(id): index = id cmd_vel_pub = rospy.Publisher("cmd_akm"+str(index), Twist, queue_size=10) twist = Twist() twist.linear.x = 0.0 twist.angular.z = 0.0 for i in range(20): cmd_vel_pub.publish(twist) time.sleep(2)
同时调用opencv的detect函数识别地面上的二维码，当获取到的二维码ID与当前ID不同时，小车开始变道行驶。
if get_tag(img): delay=0 if id != pre_id: pre_id = id change_path() else: pass
#换道 def change_path(): global path if path == 0: path = 1 else: path = 0 #读取二维码 def get_tag(frame): global id global tag_flag global delay detector = apriltag.Detector() gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) tags = detector.detect(gray) if tags == []: tag_flag=0 return False else: delay = 0 tag_flag = 1 for tag in tags: print("Tag detected with id: ", tag.tag_id) id = tag.tag_id return True pass
4. 实验操作
打开终端输入如图命令，启动raceworld1场景。
cd aliyun_demo source devel/setup.bash roslaunch raceworld_master raceworld1.launch
打开新的终端输入如图所示命令，开启巡线程序。
cd aliyun_demo source devel/setup.bash rosrun raceworld_master running_car2.py
此时可以观察到小车的运行状况。当识别到地面二维码时，将当前行驶车道输出于终端。
当识别到停车路牌时，小车自动停止，并中断程序运行。
实验链接：https://developer.aliyun.com/adc/scenario/788e5f50f03049069ca60dc24ec381c2
