Gazebo环境下基于ROS和OpenCV的阿克曼小车识别路牌停车-阿里云帮助中心

本实验使用OpenCV进行检测，通过图像处理工具调用相关的API函数，进而识别特征参数，得到检测结果。

实验室资源方式简介

进入实操前，请确保阿里云账号满足以下条件：

个人账号资源
- 使用您个人的云资源进行操作，资源归属于个人。
- 平台仅提供手册参考，不会对资源做任何操作。
确保已完成云工开物300元代金券领取。
已通过实名认证且账户余额≥100元。

本次实验将在您的账号下开通实操所需计算型实例规格族c7a，费用约为：25元（以实验时长2小时预估，具体金额取决于实验完成的时间），需要您通过阿里云云工开物学生专属300元抵扣金兑换本次实操的云资源。

如果您调整了资源规格、使用时长，或执行了本方案以外的操作，可能导致费用发生变化，请以控制台显示的实际价格和最终账单为准。

领取专属权益及创建实验资源

在开始实验之前，请先点击右侧屏幕的“进入实操”再进行后续操作

领取300元高校专属权益优惠券（若已领取请跳过）

重要

实验产生的费用优先使用优惠券，优惠券使用完毕后需您自行承担。

学生认证

实验步骤

1、服务部署
- 点击链接，进入部署页面
- 按弹窗提示进行权限申请。其中【姓名】、【电话】、【邮箱】为必填项，完成填写后点击【确定】
  说明
  请填写您的学校邮箱（.edu）,便于审核
- 提交申请后将提示
- 当申请通过后，将会收到短信提示可以进行部署
- 刷新部署页面，按下图设置【服务实例名称】、【地域】、【实例密码】
  - 服务实例名称：test（可自定义命名）
  - 地域：华东2（上海）
  - 实例密码：Sjtu@520
    说明
    输入实例密码时请注意大小写，请记住您设置的实例名称及对应密码，后续实验过程会用到。
- 完成填写后点击【下一步：确认订单】
- 核对实例信息及价格预览，无误请点击【立即创建】
  重要
  领取300元优惠券后，资源应为0元/小时，且会提示【您当前账户的余额充足】！若提示余额不足等，请检查是否正确领取优惠券
- 创建成功，点击【去列表查看】
- 查看实例，点击左侧的图标展开目录
  选择目录中的【云服务器ECS】
- 云服务器ECS—实例—远程连接
- 下拉展开更多登录方式，选择【通过VNC远程连接】
- 输入实例密码：Sjtu@520（请输入您设置的密码）后回车
- 点击左侧第3个图标，点击aliyun_ws文件夹
- 进入aliyun_ws文件夹后，在空白处点击鼠标右键，选择Open in Terminal

2、代码部分

识别路牌停车程序使用的是raceworld_1car场景，由raceworld_1car.launch文件启动。找到aliyun_ws/src/raceworld/launch文件夹中的raceworld_1car.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_comannds.py的代码文件。

<node pkg="raceworld" type="servo_commands.py" name="servo_commands" output="screen" >

打开aliyun_ws/src/raceworld/src文件夹中的servo_comannds.py文件我们可以看到，代码中新建了一个名为servo_comannds的节点，他订阅并接收/car1/ackermann_cmd_mux/output消息。

robot_name = "car"
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):
    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)

识别路牌停车程序需要运行aliyun_ws/src/raceworld/src文件夹中的stop.py文件。在代码中首先新建了get_camera节点，并且订阅了名为/car1/camera/zed_left/image_rect_color_left的消息。该消息通过Gazebo中的小车上的左摄像头获取。在获取到摄像头图像信息后，把它转换为cv2图像数据。

def get_camera():
    rospy.Subscriber("/car1/camera/zed_left/image_rect_color_left", Image, image_callback)
    pass

if __name__ == '__main__':
    rospy.init_node("stop")
    pub = rospy.Publisher("/car1/ackermann_cmd_mux/output", AckermannDriveStamped, queue_size=10)
    get_camera()
    rospy.spin()
    pass

在follow_line函数中，先对图像进行HSV转换和二值图转换操作。

def follow_line(image):
    #转换为HSV图像
    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
    #cv2.namedWindow("hsv image",0)
    #cv2.imshow("hsv image",hsv)
    lower_yellow = np.array([26, 43, 46])
    upper_yellow = np.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'])
    cy = int(M['m01'] / M['m00'])
    # print(cx, cy)
    #在质心画圆
    cv2.circle(image, (cx, cy), 20, (0, 0, 255), -1)
    err = cx - w / 2 - 50
    #根据质心与图像中线的偏移设置转角
    akm = AckermannDriveStamped()
    akm.drive.speed = 0.08
    print("Speed:",akm.drive.speed)
    #akm.drive.steering_angle = -float(err / 2.0) / 20
    akm.drive.steering_angle = atan((-float(err / 2.0) / 200 /0.18)*0.133)
    print("Steering_Angle:",akm.drive.steering_angle)
    pub.publish(akm)
cv2.namedWindow("camera",0)
cv2.imshow("camera", image)
cv2.waitKey(1)
pass

同时以质心为中心点在原始图像上绘制红色的圆形图案作为参考，如图所示。

在行驶过程中，对摄像头获取到的图像信息与本地路牌图片进行模板匹配并获得其中最大值。

def do_match(frame):
    max = 0.0
    #读取本地图片
    template = cv2.imread(r"src/raceworld/stop_sign.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.7:
    stop()
    print("stop now!")
    time.sleep(3)
    rospy.signal_shutdown("~~~")

#停止行驶
def stop():
    akm = AckermannDriveStamped()
    akm.drive.speed = 0
    akm.drive.steering_angle = 0
    for i in range(20):
        pub.publish(akm)
    time.sleep(2)

3、实验操作
在aliyun_ws路径下新建终端，输入命令开启raceworld_1car Gazebo场景。
```
source devel/setup.bash
roslaunch raceworld raceworld_1car.launch
```
在aliyun_ws路径下新建另一个终端并输入如图所示命令运行识别路牌停车程序。
```
source devel/setup.bash
rosrun raceworld stop.py
```
当识别到路牌的时候，小车停止行驶，终端输出停止信息，并且程序自动终止运行。