Gazebo环境下基于ROS和OpenCV的阿克曼小车识别路牌停车-阿里云开发者社区

Gazebo环境下基于ROS和OpenCV的阿克曼小车识别路牌停车

2023-09-17 96

版权

本文内容由阿里云实名注册用户自发贡献，版权归原作者所有，阿里云开发者社区不拥有其著作权，亦不承担相应法律责任。具体规则请查看《阿里云开发者社区用户服务协议》和《阿里云开发者社区知识产权保护指引》。如果您发现本社区中有涉嫌抄袭的内容，填写侵权投诉表单进行举报，一经查实，本社区将立刻删除涉嫌侵权内容。

本文涉及的产品

资源编排，不限时长

简介： 巡线行驶时识别路牌停车

+关注继续查看

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文件夹中的stop.py文件。在代码中首先新建了get_camera节点，并且订阅了名为/deepracer1/camera/zed_left/image_rect_color_left的消息。该消息通过Gazebo中的deepracer小车上的左摄像头获取。在获取到摄像头图像信息后，把它转换为cv2图像数据。

def get_camera():
    rospy.Subscriber("/deepracer1/camera/zed_left/image_rect_color_left", Image, image_callback)
    pass
if __name__ == '__main__':
    rospy.init_node("get_camera")
    get_camera()
    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

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

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

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)

4. 实验操作

打开终端输入如图命令，启动raceworld1场景。

cd aliyun_demo
source devel/setup.bash
roslaunch raceworld raceworld1.launch

场景如图所示。

随后新建另一个终端并输入如图所示命令运行识别路牌停车程序。

cd aliyun_demo
source devel/setup.bash
rosrun raceworld stop.py

当识别到路牌的时候，小车停止行驶，终端输出停止信息，并且程序自动终止运行。

实验地址：https://developer.aliyun.com/adc/scenario/0ba8714af8c04226a61d28d74516c776