雷达简介与使用

功能源码在 docker 中，

#A1雷达
/root/yahboomcar_ros2_ws/software/library_ws/src/sllidar_ros2/launch/sllidar_lau
nch.py
#A1雷达可视化
/root/yahboomcar_ros2_ws/software/library_ws/src/sllidar_ros2/launch/view_sllida
r_launch.py

启动雷达

#启动A1雷达
ros2 launch sllidar_ros2 sllidar_launch.py
#启动A1雷达+rviz可视化数据
ros2 launch sllidar_ros2 view_sllidar_launch.py

打印雷达数据 ros2 topic echo /scan

雷达避障趣味玩法

代码: /root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_laser/yahboomcar_laser/las er_Avoidance_a1_X3.py

左，右，中，三个方向，每个方向如果小于设定的距离，那么就对应的方向累加。

    for i in range(len(ranges)):
        angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG
        if 160 > angle > 180 - self.LaserAngle:
            if ranges[i] < self.ResponseDist*1.5:
                self.Right_warning += 1
        if - 160 < angle < self.LaserAngle - 180:
            if ranges[i] < self.ResponseDist*1.5:
                self.Left_warning += 1
        if abs(angle) > 160:
             if ranges[i] <= self.ResponseDist*1.5: 
                    self.front_warning += 1

根据不同方向上面的累加值，来决策是直行还是左右转。

    if self.front_warning > 10 and self.Left_warning > 10 and self.Right_warning > 10:
        print ('1, there are obstacles in the left and right, turn right')
        twist.linear.x = self.linear
        twist.angular.z = -self.angular
        self.pub_vel.publish(twist)
        sleep(0.2)
    elif self.front_warning > 10 and self.Left_warning <= 10 and self.Right_warning > 10:
        print ('2, there is an obstacle in the middle right, turn left')
        twist.linear.x = 0.0
        twist.angular.z = self.angular
        self.pub_vel.publish(twist)
        sleep(0.2)
        if self.Left_warning > 10 and self.Right_warning <= 10:
            twist.linear.x = 0.0
            twist.angular.z = -self.angular
            self.pub_vel.publish(twist)
            sleep(0.5)
    elif self.front_warning > 10 and self.Left_warning > 10 and self.Right_warning <= 10:
        print ('4. There is an obstacle in the middle left, turn right')
        twist.linear.x = 0.0
        twist.angular.z = -self.angular
        self.pub_vel.publish(twist)
        sleep(0.2)
        if self.Left_warning <= 10 and self.Right_warning > 10:
            twist.linear.x = 0.0
            twist.angular.z = self.angular
            self.pub_vel.publish(twist)
            sleep(0.5)
    elif self.front_warning > 10 and self.Left_warning < 10 and self.Right_warning < 10:
        ...

运行:

启动:

#启动小车底盘
ros2 run yahboomcar_bringup Mcnamu_driver_X3
#启动A1雷达
ros2 launch sllidar_ros2 sllidar_launch.py
#启动雷达避障程序 X3车型、A1/S2雷达
ros2 run yahboomcar_laser laser_Avoidance_a1_X3
#启动手柄，如果需要的话
ros2 run yahboomcar_ctrl yahboom_joy_X3
ros2 run joy joy_node

查看话题通讯节点图 ros2 run rqt_graph rqt_graph
动态参数调节 ros2 run rqt_reconfigure rqt_reconfigure，除了 switch，其他的都要是浮点数。

参数名称参数含义

linear 线速度

angular 角速度

LaserAngle 雷达检测角度

ResponseDist 障碍物检测距离

Switch 玩法开关

雷达跟踪

代码: /root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_laser/yahboomcar_laser/laser_Tracker_a1_X3.py

参数名称	参数含义
linear	线速度
angular	角速度
LaserAngle	雷达检测角度
ResponseDist	障碍物检测距离
Switch	玩法开关

    def registerScan(self, scan_data):
        if not isinstance(scan_data, LaserScan): return
        ranges = np.array(scan_data.ranges)
        offset = 0.5
        frontDistList = []
        frontDistIDList = []
        minDistList = []
        minDistIDList = []

        for i in range(len(ranges)):
            angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG
            if abs(angle) > (180 - self.priorityAngle):
                if ranges[i] < (self.ResponseDist + offset):
                    frontDistList.append(ranges[i])
                    frontDistIDList.append(angle)
            elif (180 - self.LaserAngle) < angle < (180 - self.priorityAngle):
                minDistList.append(ranges[i])
                minDistIDList.append(angle)
            elif (self.priorityAngle - 180) < angle < (self.LaserAngle - 180):
                minDistList.append(ranges[i])
                minDistIDList.append(angle)
        if len(frontDistIDList) != 0:
            minDist = min(frontDistList)
            minDistID = frontDistIDList[frontDistList.index(minDist)]
        else:
            minDist = min(minDistList)
            minDistID = minDistIDList[minDistList.index(minDist)]
        if self.Joy_active or self.Switch == True:
            if self.Moving == True:
                self.pub_vel.publish(Twist())
                self.Moving = not self.Moving
            return
        self.Moving = True
        velocity = Twist()
        if abs(minDist - self.ResponseDist) < 0.1: minDist = self.ResponseDist
        velocity.linear.x = -self.lin_pid.pid_compute(self.ResponseDist, minDist)
        ang_pid_compute = self.ang_pid.pid_compute((180 - abs(minDistID)) / 72, 0)
        if minDistID > 0: velocity.angular.z = -ang_pid_compute
        else: velocity.angular.z = ang_pid_compute
        if ang_pid_compute < 0.02: velocity.angular.z = 0.0
        self.pub_vel.publish(velocity)

frontDistList = [] frontDistIDList = [] ：frontDistList 存储距离值，frontDistIDList 存储角度值。
angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG 计算当前扫描点的角度
if len(frontDistIDList) != 0: 检查前方区域是否有扫描点。

运行:

启动

#启动小车底盘
ros2 run yahboomcar_bringup Mcnamu_driver_X3
#启动A1雷达
ros2 launch sllidar_ros2 sllidar_launch.py
#启动雷达跟踪程序 X3车型、A1/S2雷达
ros2 run yahboomcar_laser laser_Tracker_a1_X3
#启动手柄，如果需要的话
ros2 run yahboomcar_ctrl yahboom_joy_X3
ros2 run joy joy_node

ros2 run rqt_graph rqt_graph
ros2 run rqt_reconfigure rqt_reconfigure 动态参数设置

雷达警卫

代码: /root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_laser/yahboomcar_laser/laser_Warning_a1_X3.py

    def registerScan(self, scan_data):
        if not isinstance(scan_data, LaserScan): return
        ranges = np.array(scan_data.ranges)
        minDistList = []
        minDistIDList = []

        for i in range(len(ranges)):
            angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG
            if abs(angle) > (180 - self.LaserAngle):
                minDistList.append(ranges[i])
                minDistIDList.append(angle)
        if len(minDistList) == 0: return
        minDist = min(minDistList)
        minDistID = minDistIDList[minDistList.index(minDist)]
        if self.Joy_active or self.Switch == True:
            if self.Moving == True:
                self.pub_vel.publish(Twist())
                self.Moving = not self.Moving
            return
        self.Moving = True
        if minDist <= self.ResponseDist:
            if self.Buzzer_state == False:
                b = Bool()
                b.data = True
                self.pub_Buzzer.publish(b)
                self.Buzzer_state = True
        else:
            if self.Buzzer_state == True:
                self.pub_Buzzer.publish(Bool())
                self.Buzzer_state = False
        velocity = Twist()
        ang_pid_compute = self.ang_pid.pid_compute((180 - abs(minDistID)) / 36, 0)
        if minDistID > 0: velocity.angular.z = -ang_pid_compute
        else: velocity.angular.z = ang_pid_compute
        if ang_pid_compute < 0.02: velocity.angular.z = 0.0
        self.pub_vel.publish(velocity)

运行:

启动

#启动小车底盘
ros2 run yahboomcar_bringup Mcnamu_driver_X3
#启动A1雷达
ros2 launch sllidar_ros2 sllidar_launch.py
#启动雷达警卫程序 X3车型、A1/S2雷达
ros2 run yahboomcar_laser laser_Warning_a1_X3
#启动手柄，如果需要的话
ros2 run yahboomcar_ctrl yahboom_joy_X3
ros2 run joy joy_node

ros2 run rqt_graph rqt_graph
ros2 run rqt_reconfigure rqt_reconfigure

巡逻功能

暂缺

gmapping建图

暂缺

cartographer建图

暂缺

导航与避障

暂缺

yahboom rosmaster x3, 激光雷达系列课程学习笔记

雷达简介与使用

雷达避障趣味玩法

雷达跟踪

雷达警卫

巡逻功能

gmapping建图

cartographer建图

导航与避障

发表评论

博客信息

最新文章

目录

目录