无人驾驶虚拟仿真（十一）--图像处理之红线检测

最新推荐文章于 2023-11-28 11:55:15 发布

溪风沐雪

最新推荐文章于 2023-11-28 11:55:15 发布

阅读量469

点赞数 1

分类专栏：无人驾驶虚拟仿真文章标签： python 自动驾驶

本文链接：https://blog.csdn.net/aibingjin/article/details/123842870

版权

无人驾驶虚拟仿真专栏收录该内容

25 篇文章 98 订阅

订阅专栏

简介：在交通地图的路口设计了红线，其主要作用是用来提醒车辆需要进行某些功能检测了，比如红绿灯识别，路口交通标志识别等。在车道线识别中，我们已经提取到了红线的车道线，接下来只需要考虑如何计算红线与车辆之间的距离。

考虑到红线理论上应该是跟车辆行驶方向（x轴）垂直的，所以红线与/车的距离就是红线线段端点x坐标值，去除距离太大的（0.3米以上），去除离车头太远的，剩下的取距离均值做为车辆与红线的参考距离，具体实现请参考源码，接下来先创建ROS节点：

1、新建功能包

进入工作空间目录：

$ cd ~/myros/catkin_ws/src

创建新功能包：

$ catkin_create_pkg stop_line rospy duckietown_msgs std_msgs

创建配置文件：

$ mkdir -p stop_line/config/stop_line_node

$ touch stop_line/config/stop_line_node/default.yaml

新建启动脚本：

$ mkdir -p stop_line/launch

$ touch stop_line/launch/start.launch

新建源码文件

$ touch stop_line/src/stop_line_node.py

修改编译配置文件

$ gedit stop_line/CMakeLists.txt

修改为：

2、编辑配置文件

$ gedit stop_line/config/stop_line_node/default.yaml

min_red_line: 3
alarm_dist: 0.08

3、编辑源码文件

$ gedit stop_line/src/stop_line_node.py

#!/usr/bin/python3

import rospy
import numpy as np

from std_msgs.msg import Bool
from duckietown_msgs.msg import Segment, SegmentList

class StopLineNode():
    def __init__(self):
        rospy.init_node("stop_line_detect_node",anonymous=False)
        self.min_red_line = rospy.get_param("~min_red_line", default=3)
        self.alarm_dist = rospy.get_param("~alarm_dist", default=0.08)
        #订阅车道线数据
        rospy.Subscriber('~segments', SegmentList, self.cb_segments)
        #发布红线检测结果
        self.pub_msg = rospy.Publisher("~detected", Bool, queue_size=10)   
    def cb_segments(self, msg):
        segments = msg.segments
        dist_list = [] #距离列表
        for segment in segments: #遍历车道线数据
            if segment.color==2: #取红色车道线
                dist = self.getDistance(segment) #计算车道线与车的距离
                if dist!=0 and dist<0.3: #距离为0代表无效车道线，大于0.3米的暂时不处理
                    dist_list.append(dist)
        if len(dist_list)>self.min_red_line: #有效数据大于3个认为是有效的，小于4个认为是误检测        
            mean_dist = np.mean(dist_list)
            if(mean_dist<self.alarm_dist): #距离0.08米发布检测到红线的消息
                self.pub_msg.publish(Bool(data=True))
            else: 
                self.pub_msg.publish(Bool(data=False))
        else: 
            self.pub_msg.publish(Bool(data=False))  
    #计算车道线线段与车的距离
    def getDistance(self, segment):
        x1 = segment.points[0].x
        y1 = segment.points[0].y
        x2 = segment.points[1].x
        y2 = segment.points[1].y
        x_c = (x1+x2)/2 #中点x轴坐标
        y_c = (y1+y2)/2 #中点y轴坐标
        dist_to_ori = np.sqrt(x_c**2+y_c**2)  #中点距原点距离
        dist_max = np.sqrt(x_c**2+0.12**2) #有效车道线范围内相同x坐标最远点距原点距离
        if dist_to_ori<dist_max: #中点在有效距离内，以中点x坐标值做为近似距离返回，否则返回0
            return x_c
        else:
            return 0

if __name__=='__main__':
    node = StopLineNode()
    rospy.spin()

4、编辑启动脚本

$ gedit stop_line/launch/start.launch

<launch>
  <arg name="veh"/>
  <arg name="pkg_name" value="stop_line"/>
  <arg name="node_name" value="stop_line_node"/>
  <arg name="param_file_name" default="default" doc="Specify a param file. ex:megaman"/>
  <arg name="required" default="false" />

  <group ns="$(arg veh)">
    <remap from="stop_line_node/segments" to="line_detect_node/segments"/>
    <node name="$(arg node_name)" pkg="$(arg pkg_name)" type="$(arg node_name).py" respawn="true" respawn_delay="10" output="screen" required="$(arg required)">
      <rosparam command="load" file="$(find stop_line)/config/$(arg node_name)/$(arg param_file_name).yaml"/>
    </node>
  </group>
</launch>

5、编译

$ cd ~/myros/catkin_ws

$ catkin_make

6、编辑多节点启动文件

$ gedit start.launch

<launch>
  <arg name="veh" default="duckiebot"/>
  <group>
    <include file="$(find duckiebot)/launch/duckiebot.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
    <include file="$(find anti_instagram)/launch/start.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
    <include file="$(find line_detect)/launch/start.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
    <include file="$(find lane_filter)/launch/start.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
    <include file="$(find car_control)/launch/start.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
    <include file="$(find stop_line)/launch/start.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
  </group>
</launch>