AI 助理
备案控制台
官方文档
输入文档关键字查找
首页Gazebo环境下基于ROS和OpenCV的阿克曼小车跟车（2024版）

Gazebo环境下基于ROS和OpenCV的阿克曼小车跟车（2024版）

更新时间：2025-02-25 02:12:23
我的收藏

依据Stanley算法跟踪的物理原理，将该原理进行应用，用代码进行实现，在仿真环境中进行实验，根据实际效果，调整参数，顺利地通过各个弯道。

实验简介

  • 本实验依据Stanley算法跟踪的物理原理，将该原理进行应用，用代码进行实现，在仿真环境中进行实验，根据实际效果，调整参数，顺利地通过各个弯道。

背景知识

  • 学生需要有一定的C++ 基础或者Python基础来完成此实验。

实验室资源方式简介

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

  • 个人账号资源

    • 使用您个人的云资源进行操作，资源归属于个人。

    • 平台仅提供手册参考，不会对资源做任何操作。

  • 确保已完成云工开物300元代金券领取。

  • 已通过实名认证且账户余额≥100元。

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

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

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

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

image

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

重要

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

学生认证

实验步骤

  • 1、服务部署

    • 点击链接，进入部署页面

    • 按弹窗提示进行权限申请。其中【姓名】、【电话】、【邮箱】为必填项，完成填写后点击【确定】

      说明

      请填写您的学校邮箱（.edu）,便于审核

      image

    • 提交申请后将提示

      image

    • 当申请通过后，将会收到短信提示可以进行部署

      image

    • 刷新部署页面，按下图设置【服务实例名称】、【地域】、【实例密码】

      • 服务实例名称：test（可自定义命名）

      • 地域：华东2（上海）

      • 实例密码：Sjtu@520

        说明

        输入实例密码时请注意大小写，请记住您设置的实例名称及对应密码，后续实验过程会用到。

      image

    • 完成填写后点击【下一步：确认订单】

      image

    • 核对实例信息及价格预览，无误请点击【立即创建】

      image

      重要

      领取300元优惠券后，资源应为0元/小时，且会提示【您当前账户的余额充足】！若提示余额不足等，请检查是否正确领取优惠券

    • 创建成功，点击【去列表查看】

      image

    • 查看实例，点击左侧的图标展开目录

      image

      选择目录中的【云服务器ECS】

      image

    • 云服务器ECS—实例—远程连接

      image

    • 下拉展开更多登录方式，选择【通过VNC远程连接】

      image

    • 输入实例密码：Sjtu@520（请输入您设置的密码）后回车

      image

    • 点击左侧第3个图标，点击aliyun_demo文件夹

      image

    • 进入aliyun_demo文件夹后，在空白处点击鼠标右键，选择Open in Terminal

      image

      image

  • 2、代码部分

    跟车程序使用的是racecars场景，由racecars.launch文件启动。找到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" >

    打开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)

    原始图像如下。

    image.png

    腐蚀操作后图像如下。

    image.png

    视角转换后图像如下。

    image.png

    随后根据图像大小设置滑动窗口的参数。

    #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)

    绘制后的图像如下。

    image.png

    计算目标点的真实坐标。并根据偏差计算转角数据。

    # 计算目标点的真实坐标
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")

  • 3、实验操作

    在aliyun_demo路径下新建终端，输入命令开启racecars Gazebo场景。

    source devel/setup.bash
roslaunch raceworld racecars.launch

    image.png

    在aliyun_demo路径下新建终端，新建终端，输入如下命令启动follow.launch。

    source devel/setup.bash
roslaunch raceworld follow.launch

    随后我们可以看到跟车程序的实际运行状况。

    image.png

    在follow.launch终端中可以看到打印出来的控制信息。

    image.png

    再次新建一个终端，输入rostopic list，显示如下。其中可以找到/status，/deepracer1/ackermann_cmd_mux/output和/deepracer2/ackermann_cmd_mux/output主题。

    image.png

    输入rostopic info + 主题名查看内容，可以看到该主题相关的发布者与订阅者。

    image.png

    在运行实验时，可以对以下参数进行修改以达到更好的运行效果

    image.png

    image.png

清理资源

  • 计算巢—服务实例—复制服务实例ID，点击【删除】

    image

  • 在弹窗粘贴服务实例ID，并进行勾选，点击【确定删除】

    image

  • 完成安全验证后，即可成功释放实例。

    image

  • 回到云服务器ECS——实例，检查是否成功释放资源

    image

关闭实验

  • 在完成实验后，如果无需继续使用资源，选择不保留资源，单击结束实操。在结束实操对话框中，单击确定。

    image

  • 在完成实验后，如果需要继续使用资源，选择付费保留资源，单击结束实操。在结束实操对话框中，单击确定。请随时关注账户扣费情况，避免发生欠费。

    image

  • 本页导读 （0）
  • 实验简介
  • 背景知识
  • 实验室资源方式简介
  • 领取专属权益及创建实验资源
  • 实验步骤
  • 1、服务部署
  • 2、代码部分
  • 3、实验操作
  • 清理资源
  • 关闭实验