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

def image_callback(msg):
    bridge = cv_bridge.CvBridge()
    frame = bridge.imgmsg_to_cv2(msg, 'bgr8')
    follow_line(frame)
    pass

if __name__ == '__main__':
    rospy.init_node("follower")
    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.spin()
    pass

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

def follow_line(image):
    global pub

    #转换为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)。

h, w = mask.shape
#print(mask.shape)
    
#设置感兴趣区域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.1
    print("Speed:",akm.drive.speed)
    akm.drive.steering_angle = -float(err / 2.0) / 20
    print("Steering_Angle:",akm.drive.steering_angle)

    pub.publish(akm)
cv2.namedWindow("camera",0)
cv2.imshow("camera", image)
cv2.waitKey(1)
pass

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

3、实验操作
在aliyun_ws路径下新建终端，输入命令开启raceworld_1car Gazebo场景。
```
source devel/setup.bash
roslaunch raceworld raceworld_1car.launch
```
在aliyun_ws路径下新建终端，输入命令运行巡线程序。
```
source devel/setup.bash
rosrun raceworld follow.py
```
在终端中可以查看到消息的发布与接收。
小车也会沿着车道线自动行驶。