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. 代码部分
跟车程序使用的是racecars场景，由racecars.launch文件启动。找到/root/aliyun_demo/src/raceworld/launch文件夹中的racecars.launch文件并打开。
在launch文件内加载了deepracer1和deepracer2两个组，每个组加载多个控制器，其中各个组中的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的代码文件。
<nodepkg="raceworld" type="servo_commands.py" name="servo_commands" output="screen" >
打开/root/aliyun_demo/src/raceworld/scripts文件夹中的servo_comannds.py文件我们可以看到，代码中新建了一个名为servo_comannds的节点，他订阅并接收/robot_name(deepracer1或者deepracer2)/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)
跟车程序由follow.launch文件启动。其中调用了lane.py，pure.cpp和main.cpp三个程序。
<launch> <node pkg="raceworld" type="pure" name="pure" output="screen"/> <node pkg="raceworld" type="main" name="main" output="screen"/> <node pkg="raceworld" type="lane.py" name="lane" output="screen"/> </launch>
其中main.cpp的功能比较简单，新建一个/status主题的发布者，将leader和follower的名字设置好后就可以发布出去。
ros::Publisher status_publisher; std::string leader_name; int main(int argc, char **argv) { ros::init(argc, argv, "main"); ros::NodeHandle mainNode; status_publisher = mainNode.advertise<raceworld::status>("/status", 1000); raceworld::status a_msg, b_msg; a_msg.formation = 1; a_msg.leader = "deepracer1"; a_msg.follower1 = "deepracer2"; b_msg.formation = 2; b_msg.leader = "deepracer1"; b_msg.follower1 = "deepracer2"; }
follower的跟车由pure.cpp实现。首先订阅/status，/deepracer1/base_pose_ground_truth和/deepracer2/base_pose_ground_truth主题，获取leader和follower的名字信息以及两辆车的位姿和转角信息。
ros::init(argc, argv, "follower1"); ros::NodeHandle node; //subscribe status message.include follower info. ros::Subscriber platoon_status = node.subscribe("/status", 1, statusCallBack); //subscribe car position message from sensor ros::Subscriber pose1 = node.subscribe("/deepracer1/base_pose_ground_truth", 10, poseCallback1); ros::Subscriber pose2 = node.subscribe("/deepracer2/base_pose_ground_truth", 10, poseCallback2);
void statusCallBack(const raceworld::status::ConstPtr & status_msg) { leader_name = status_msg->leader; isformation = status_msg->formation; follower1 = status_msg->follower1; } void poseCallback1(const nav_msgs::Odometry::ConstPtr& msg) { pose_flag = true; msg1.child_frame_id = msg->child_frame_id; msg1.header = msg->header; msg1.pose = msg->pose; msg1.twist = msg->twist; } void poseCallback2(const nav_msgs::Odometry::ConstPtr& msg) { pose_flag = true; msg2.child_frame_id = msg->child_frame_id; msg2.header = msg->header; msg2.pose = msg->pose; msg2.twist = msg->twist; }
由于follower1为deepracer2，所以pure.cpp中创建的是/deepracer2/ackermann_cmd_mux/output消息的发布者。
slave_vel1 = node.advertise<ackermann_msgs::AckermannDriveStamped>(follower1+"/ackermann_cmd_mux/output", 100); vel_msg1.header.stamp = ros::Time::now(); vel_msg1.header.frame_id = follower1+"/base_link";
将leader也就是deepracer1的位置信息作为目标，开始进行跟随。
if(target_point1.pose.pose.position.x == 0 && target_point1.pose.pose.position.y == 0) target_point1 = ldmsg; if(distance1 < thresh_distance){ target_point1 = ldmsg_queue1->front(); ldmsg_queue1->pop(); } else if(distance1 > 5){ delete ldmsg_queue1; ldmsg_queue1 = new std::queue<nav_msgs::Odometry>; target_point1 = ldmsg; } follow(target_point1);
在计算并设置好速度与转角之后，将AckermannDriveStamped类型的消息发布出去。
double r1 = sqrt(pow(ldmsg.pose.pose.position.x-f1msg.pose.pose.position.x, 2) + pow(ldmsg.pose.pose.position.y-f1msg.pose.pose.position.y, 2)); double k = 1.0; if(r1 > 0.4) { vel_msg1.drive.speed = 0.3 * r1; }else { vel_msg1.drive.speed = 0.05; } vel_msg1.drive.steering_angle = k * theta1; slave_vel1.publish(vel_msg1);
leader的巡线由lane.py实现。在代码中首先新建了lane节点，并且订阅了名为/deepracer1/camera/zed_left/image_rect_color_left的消息。该消息通过Gazebo中的deepracer1小车上的左摄像头获取。在获取到摄像头图像信息后，把它转换为cv2图像数据。此外，还创建了一个/deepracer1/ackermann_cmd_mux消息的发布者。
if __name__ == '__main__': try: print("exec!") rospy.init_node('lane', anonymous=True) rospy.Subscriber("/deepracer1/camera/zed_left/image_rect_color_left", Image, camera_callback) rospy.spin() except rospy.ROSInterruptException: pass
pub = rospy.Publisher("/deepracer1/ackermann_cmd_mux/output", AckermannDriveStamped,queue_size=1)
获得的图像信息通过了高斯滤波，HSV图像转换，腐蚀，二值图转换，视角转换，灰度值处理等一系列操作。
#进行高斯滤波 blur = cv2.GaussianBlur(img,(1,1),0) #转换为HSV图像 hsv_img = cv2.cvtColor(blur,cv2.COLOR_BGR2HSV) #cv2.namedWindow("hsv image",0) #cv2.imshow('hsv image', hsv_img) kernel = np.ones((3, 3), dtype=np.uint8) #腐蚀操作 erode_hsv = cv2.erode(hsv_img,kernel, 1) #cv2.namedWindow("erode image",0) #cv2.imshow('erode image', erode_hsv) #转换为二值图，范围内的颜色为白色，其他范围为黑色 inRange_hsv = cv2.inRange(erode_hsv, color_dist['blue']['Lower'], color_dist['blue']['Upper']) #cv2.namedWindow("binary image",0) #cv2.imshow('binary image', inRange_hsv) gray_img = inRange_hsv #利用透视变换矩阵进行图片转换 gray_img = cv2.warpPerspective(gray_img, M, (640, 480), cv2.INTER_LINEAR) #cv2.namedWindow("gray image",0) #cv2.imshow('gray image', gray_img) #二值化 ret, origin_thr = cv2.threshold(gray_img, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU) #cv2.namedWindow("origin_thr image",0) #cv2.imshow('origin_thr image', origin_thr)
原始图像如下。
腐蚀操作后图像如下。
视角转换后图像如下。
随后根据图像大小设置滑动窗口的参数。
#7个滑动窗口 nwindows=7 #设置每个滑动窗口的高度 window_height=int(binary_warped.shape[0] / nwindows) nonzero=binary_warped.nonzero() nonzeroy=np.array(nonzero[0]) nonzerox=np.array(nonzero[1]) lane_current=lane_base #滑动窗口宽度的一半 margin=100 minpix=25
并且在视角转换后的图像中绘制出来。
for window in range(nwindows): #设置滑动窗口坐标点 win_y_low = binary_warped.shape[0] - (window + 1) * window_height win_y_high = binary_warped.shape[0] - window * window_height win_x_low = lane_current - margin win_x_high = lane_current + margin good_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_x_low) & (nonzerox < win_x_high)).nonzero()[0] lane_inds.append(good_inds) img1 = cv2.rectangle(img1, (win_x_low, win_y_low), (win_x_high, win_y_high), (0, 255, 0), 3) if len(good_inds) > minpix: lane_current = int(np.mean(nonzerox[good_inds])) elif window >= 3: break
随后绘制车道线，计算出质心坐标后绘制两个定位用的圆形。其中蓝色圆形位于车道线上，绿色圆形使用的是质心的坐标。
for num in range(len(ploty) - 1): cv2.line(img1, (int(plotx[num]), int(ploty[num])), (int(plotx[num + 1]), int(ploty[num + 1])),(0, 0, 255), 8) aP[0] = aimLaneP[0] - math.sin(theta) * (LorR) * roadWidth / 2 aP[1] = aimLaneP[1] - math.cos(theta) * (LorR) * roadWidth / 2 #车道线上绘制蓝色圆 img1 = cv2.circle(img1, (int(aimLaneP[0]), int(aimLaneP[1])), 10, (255, 0, 0), -1) #质心绘制绿色圆 img1 = cv2.circle(img1, (int(aP[0]), int(aP[1])), 10, (0, 255, 0), -1)
绘制后的图像如下。
计算目标点的真实坐标。并根据偏差计算转角数据。
# 计算目标点的真实坐标 if lastP[0] > 0.001 and lastP[1] > 0.001: if (((aP[0] - lastP[0]) ** 2 + (aP[1] - lastP[1]) ** 2 > 2500) and Timer < 2): # To avoid the mislead by walkers aP = lastP[:] Timer += 1 else: Timer = 0 lastP = aP[:] steerAngle = k * math.atan(2 * I * aP[0] / (aP[0] * aP[0] + (aP[1] + D) * (aP[1] + D))) #print("steerAngle=", steerAngle) st = steerAngle * 4.0 / 3.1415
新建一个AckermannDriveStamped类型数据，给速度和转角信息复制后即可发布。
msg=AckermannDriveStamped(); msg.header.stamp=rospy.Time.now() msg.header.frame_id="base_link" msg.drive.speed=0.35 msg.drive.steering_angle=steerAngle*0.95; print("Speed:",msg.drive.speed) print("Steering_Angle:",msg.drive.steering_angle) pub.publish(msg) print("Message From lan.py Published\n")
4. 实验操作
打开终端，输入如下命令启动racecars场景。
cd aliyun_demo source devel/setup.bash roslaunch raceworld racecars.launch
场景如下所示。
新建终端，输入如下命令启动follow.launch。
cd aliyun_demo source devel/setup.bash roslaunch raceworld follow.launch
随后我们可以看到跟车程序的实际运行状况。
在follow.launch终端中可以看到打印出来的控制信息。
再次新建一个终端，输入rostopic list，显示如下。其中可以找到/status，/deepracer1/ackermann_cmd_mux/output和/deepracer2/ackermann_cmd_mux/output主题。
输入rostopic info + 主题名查看内容，可以看到该主题相关的发布者与订阅者。
实验链接：https://developer.aliyun.com/adc/scenario/f15aa58e3ff442ffb74017c49428373b
