Gazebo环境下基于ROS和OpenCV的阿克曼小车综合实验-阿里云帮助中心

本实验是一个综合实验，包括了二维码识别、车道线识别、停车标识识别。

实验室资源方式简介

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

个人账号资源
- 使用您个人的云资源进行操作，资源归属于个人。
- 平台仅提供手册参考，不会对资源做任何操作。
确保已完成云工开物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文件夹中的single_car.py文件。

在代码中首先新建了get_camera节点，并且订阅了名为/car1/camera/zed_left/image_rect_color_left的消息。该消息通过Gazebo中的小车上的左摄像头获取。在获取到摄像头图像信息后，把它转换为cv2图像数据。

随后订阅/car1/base_pose_ground_truth消息，获取小车初始位置以及可以计算出当前位置和初始位置的距离。

最后订阅红绿灯消息，用来控制小车的停止和行驶。

def image_callback(msg):
    # 获取图像并开始寻线
    global id, pre_id, signal, start_point
    bridge = cv_bridge.CvBridge()
    img = bridge.imgmsg_to_cv2(msg, 'bgr8')
    
    if get_tag(img):
        if id != pre_id:
            pre_id = id
            change_path()
        else:
            pass

    
    
    if signal != 1 and start_point:
        stop()
        print("Stop Now")
    else:
        follow_line(img)

def traffic_light_callback(msg):
    global signal
    signal = int(msg.data)

def pose_callback(msg):
    global init_pos, start_point
    if init_pos is None:
        init_pos = msg.pose.pose.position
        start_point = True
    else:
        distance = ((msg.pose.pose.position.x - init_pos.x) ** 2 + (msg.pose.pose.position.y - init_pos.y) ** 2 + (msg.pose.pose.position.z - init_pos.z) ** 2) ** 0.5
        if distance < 0.6:
            start_point = True
        else:
            start_point = False
            
if __name__ == '__main__':
    signal = -1 #交通灯信号
    start_point = False #是否位于初始位置附近
    init_pos = None #初始位置
    
    rospy.init_node("single_car")
    pub = rospy.Publisher("/car1/ackermann_cmd_mux/output", AckermannDriveStamped, queue_size=10)
    rospy.Subscriber("/car1/camera/zed_left/image_rect_color_left", Image, image_callback)
    rospy.Subscriber("/car1/base_pose_ground_truth", Odometry, pose_callback)
    rospy.Subscriber("traffic_light", String, traffic_light_callback)
    
    rospy.spin()
    pass

以follow_line_in函数为例，先对图像进行HSV转换和二值图转换操作。

def follow_line_in(image):
    global pub
    print("内道")
    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)
    
cx1 = 0
cx2 = 0
cy  = 0
if M1['m00'] > 0:
    cx1 = int(M1['m10'] / M1['m00'])
    cy = int(M1['m01'] / M1['m00'])
if M2['m00'] > 0:
    cx2 = int(M2['m10'] / M2['m00'])
cx = int((cx1 + cx2) / 2)
#在质心画圆
cv2.circle(image, (cx, cy), 20, (0, 0, 255), -1)
#cv2.circle(image, (int(w / 2 - 60), cy), 10, (255, 0, 0), -1)
err = cx - (w / 2 - 60)
print(err)
#angle = min(max(-1, -float(err / 2.0) / 20), 1)
angle = min(max(-1, -float(err / 5.0) / 50), 1)
akm = AckermannDriveStamped()
akm.drive.speed = 0.06
#print("Speed:",akm.drive.speed)
akm.drive.steering_angle = angle
print("Steering_Angle:",akm.drive.steering_angle)
pub.publish(akm)
        
cv2.namedWindow("Camera",0)
cv2.imshow("Camera", image)
cv2.waitKey(1)

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

当小车接近初始位置init_pos以及红绿灯信号signal≠1时，停止行驶等待中间信号绿灯。

if signal != 1 and start_point:
    stop()
    print("Stop Now")
else:
    follow_line(img)

def stop():
    global pub
    akm = AckermannDriveStamped()
    akm.drive.speed = 0
    akm.drive.steering_angle = 0
    for i in range(20):
        pub.publish(akm)
    time.sleep(2)

同时调用opencv的detect函数识别地面上的二维码，当获取到的二维码ID与当前ID不同时，小车开始变道行驶。

if get_tag(img):
    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
    detector = pyapriltags.Detector()
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    tags = detector.detect(gray)
    if tags == []:
        return False
    else:
        for tag in tags:
            print("Tag detected with id: ", tag.tag_id)
            id = tag.tag_id
        return True

红绿灯ROS消息的发送和模型的变化由aliyun_ws/src/raceworld/src/traffic_light.py实现。

if __name__ == '__main__':
    changeRate = 8
    currentSignal = 0 #0左 1中 2右
    lastTime = time.time()

    rospy.init_node("traffic_light")
    pub = rospy.Publisher("traffic_light", String,queue_size=1)

    # 等待并调用gazebo中get_link_state和set_link_state服务
    rospy.wait_for_service('/gazebo/get_link_state')
    rospy.wait_for_service('/gazebo/set_link_state')
    get_link_state_proxy= rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState)
    set_link_state_proxy = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState)
    reference_frame = 'world'
    
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        traffic_light_signal()
        #signal = "1"
        #rospy.loginfo("Current Traffic Light is "+signal)
        #pub.publish(signal)
        rate.sleep()

changeRate为信号变化的频率，每隔一定时间通过traffic_light消息的发布者发布当前信号灯消息。

#定时切换左中右信号
def traffic_light_signal():
    global currentSignal, lastTime
    if time.time() - lastTime >= changeRate:
        traffic_light_change(currentSignal)
        currentSignal = (currentSignal + 1) % 3
        traffic_light_change(currentSignal)
        lastTime = time.time()
    pub.publish(str(currentSignal))
    rospy.loginfo("Current Traffic Light is " + str(currentSignal))

通过GetLinkState获得模型link的位置，使用SetLinkState将红灯和绿灯link位置交换达到红绿灯变化的效果。

#改模型红绿灯位置
#切换信号时先把之前信号模型调换复原再修改新的信号灯
def traffic_light_change(signal):
    # 根据当前信号选择需要修改的link名
    if signal == 0:
        link_name0 = 'cantilevered_light::traffic_light::left_off'
        link_name1 = 'cantilevered_light::traffic_light::left_on'
    elif signal == 1:
        link_name0 = 'cantilevered_light::traffic_light::middle_off'
        link_name1 = 'cantilevered_light::traffic_light::middle_on'
    elif signal == 2:
        link_name0 = 'cantilevered_light::traffic_light::right_off'
        link_name1 = 'cantilevered_light::traffic_light::right_on'
    
    # 获取左/中/右的红绿灯模型link state
    current_state0 = get_link_state_proxy(link_name0, reference_frame)
    link_state0 = LinkState()
    link_state0.link_name = link_name0
    current_state1 = get_link_state_proxy(link_name1, reference_frame)
    link_state1 = LinkState()
    link_state1.link_name = link_name1
    
    # 调换左/中/右的红绿灯模型
    link_state0.pose = current_state1.link_state.pose
    link_state0.twist = current_state0.link_state.twist
    link_state0.reference_frame = reference_frame
    set_link_state_proxy(link_state0)
    
    link_state1.pose = current_state0.link_state.pose
    link_state1.twist = current_state1.link_state.twist
    link_state1.reference_frame = reference_frame
    set_link_state_proxy(link_state1)

3、实验操作
在aliyun_ws路径下新建终端，输入命令开启raceworld_1car Gazebo场景。
```
source devel/setup.bash
roslaunch raceworld raceworld_1car.launch
```
在aliyun_ws路径下新建另一个终端并输入如下命令启动信号灯程序。
```
source devel/setup.bash
roslaunch raceworld traffic_light.launch
```
终端中会显示当前信号，并且信号灯模型也会相应改变。
在aliyun_ws路径下新建另一个终端并输入如下命令启动小车。
```
source devel/setup.bash
rosrun raceworld single_car.py
```
当小车处于初始位置一段距离内（信号灯前方）并且信号灯中间不为绿灯时（信号≠1），小车停止并等待信号灯切换。