Gazebo环境下基于ROS和OpenCV的阿克曼小车多车跟随的实验报告

<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" />

<nodepkg="raceworld_master" type="servo_commands.py" name="servo_commands" output="screen">

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

robot_name = "deepracer"

​

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>

    <node pkg="raceworld_master" type="pure_master" name="pure_master" output="screen"/>

    <node pkg="raceworld_master" type="main_master" name="main_master" />

    <node pkg="raceworld_master" type="lane_master.py" name="lane_master" />

</launch>

status_publisher = mainNode.advertise<raceworld_master::status>("/status", 1000);

​

raceworld_master::status a_msg, b_msg;

a_msg.formation = 1;

a_msg.leader = "deepracer1";

a_msg.follower1 = "deepracer2"; a_msg.follower2 = "deepracer3"; 

a_msg.follower3 = "deepracer4"; a_msg.follower4 = "deepracer5";

b_msg.formation = 2;

b_msg.leader = "deepracer1";

b_msg.follower1 = "deepracer2"; b_msg.follower2 = "deepracer3"; 

b_msg.follower3 = "deepracer4"; b_msg.follower4 = "deepracer5";

ros::Subscriber platoon_status = node.subscribe("/status", 1, statusCallBack);

ros::Subscriber pose1 = node.subscribe("/deepracer1/base_pose_ground_truth", 10, poseCallback1);

ros::Subscriber pose2 = node.subscribe("/deepracer2/base_pose_ground_truth", 10, poseCallback2);

ros::Subscriber pose3 = node.subscribe("/deepracer3/base_pose_ground_truth", 10, poseCallback3);

ros::Subscriber pose4 = node.subscribe("/deepracer4/base_pose_ground_truth", 10, poseCallback4);

void statusCallBack(const raceworld_master::status::ConstPtr & status_msg)

{

  leader_name = status_msg->leader;

  isformation = status_msg->formation;

  follower1 = status_msg->follower1;

  follower2 = status_msg->follower2;

  follower3 = status_msg->follower3;

  follower4 = status_msg->follower4;

}

​

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;

double distance1 = sqrt((target_point1.pose.pose.position.x - f1msg.pose.pose.position.x) * (target_point1.pose.pose.position.x - f1msg.pose.pose.position.x) + (target_point1.pose.pose.position.y - f1msg.pose.pose.position.y) * (target_point1.pose.pose.position.y - f1msg.pose.pose.position.y));

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;

}

double distance2 = sqrt((target_point2.pose.pose.position.x - f2msg.pose.pose.position.x) * (target_point2.pose.pose.position.x - f2msg.pose.pose.position.x)+ (target_point2.pose.pose.position.y - f2msg.pose.pose.position.y) * (target_point2.pose.pose.position.y - f2msg.pose.pose.position.y));

if(target_point2.pose.pose.position.x == 0 && target_point2.pose.pose.position.y == 0)

    target_point2 = f1msg;

if(distance2 < thresh_distance){

    target_point2 = ldmsg_queue2->front();

    ldmsg_queue2->pop();

}

else if(distance2 > 5){

    delete ldmsg_queue2;

    ldmsg_queue2 = new std::queue<nav_msgs::Odometry>;

    target_point2 = f1msg;

}

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 r2 = sqrt(pow(f1msg.pose.pose.position.x-f2msg.pose.pose.position.x, 2) + pow(f1msg.pose.pose.position.y-f2msg.pose.pose.position.y, 2));

double r3 = sqrt(pow(f2msg.pose.pose.position.x-f3msg.pose.pose.position.x, 2) + pow(f2msg.pose.pose.position.y-f3msg.pose.pose.position.y, 2));

double r4 = sqrt(pow(f3msg.pose.pose.position.x-f4msg.pose.pose.position.x, 2) + pow(f3msg.pose.pose.position.y-f4msg.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;

}

if(r2 > 0.4){

    vel_msg2.drive.speed = 0.3 * r2;

}else{

    vel_msg2.drive.speed = 0.05;

}

if(r3 > 0.4){

    vel_msg3.drive.speed = 0.3 * r3;

}else{

    vel_msg3.drive.speed = 0.05;

}

if(r4 > 0.4){

    vel_msg4.drive.speed = 0.3 * r4;

}else{

    vel_msg4.drive.speed = 0.05;

}

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)

#进行高斯滤波

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

1.  for num in range(len(ploty) - 1):

2.      cv2.line(img1, (int(plotx[num]), int(ploty[num])), (int(plotx[num + 1]), int(ploty[num + 1])),(0, 0, 255), 8)

​

3.  aP[0] = aimLaneP[0] - math.sin(theta) * (LorR) * roadWidth / 2

4.  aP[1] = aimLaneP[1] - math.cos(theta) * (LorR) * roadWidth / 2

​

5.  #车道线上绘制蓝色圆

6.  img1 = cv2.circle(img1, (int(aimLaneP[0]), int(aimLaneP[1])), 10, (255, 0, 0), -1)

7.  #质心绘制绿色圆

8.  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

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

cd aliyun_demo

source devel/setup.bash

roslaunch raceworld_master multicars.launch

cd aliyun_demo

source devel/setup.bash

roslaunch raceworld multi_follow1.launch