初学turtlebot ---turtlebot 避障源码解析

最新推荐文章于 2024-08-08 08:27:50 发布

Zaki_gd

最新推荐文章于 2024-08-08 08:27:50 发布

阅读量1.7k

点赞数 3

本文链接：https://blog.csdn.net/weixin_42957722/article/details/103834099

版权

该文章为turtlebot3中避障源码分析，其启动方位查看避障启动方法

该源码主要实现启动turtlebot3后，小车前进，碰到障碍物后，小车停止运动。

get_scan()函数为获取的激光雷达采集的样点距离列表，其中samples 激光雷达距离列表长度为360，

而samples_view 为需要采集的实际样本数，，，当samples_view =1时，及样本数为1，采集的距离正常scan.range[0],为小车运动的正前房，当1 <= samples_view <= samples ，根据程序算法中，是以小车运动方向为分界线，分别向雷达两边采集相同的样点数，最后将需要比对的样点缓存至scan_filter[]列表中，返回数据

import rospy
import math
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

LINEAR_VEL = 0.22
STOP_DISTANCE = 0.2
LIDAR_ERROR = 0.05
SAFE_STOP_DISTANCE = STOP_DISTANCE + LIDAR_ERROR

class Obstacle():
    def __init__(self):
        self._cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
        self.obstacle()
        
    def get_scan(self):
        scan = rospy.wait_for_message('scan', LaserScan)   #从‘scan’ 画图读取消息类型laserscan 
        scan_filter = []
       
        samples = len(scan.ranges)  # The number of samples is defined in     返回列表元素个数
                                    # turtlebot3_<model>.gazebo.xacro file,
                                    # the default is 360.

        samples_view = 1            # 1 <= samples_view <= samples
        
        if samples_view > samples:  #samples 小于等于1
            samples_view = samples  

        if samples_view is 1:       
            scan_filter.append(scan.ranges[0])  #将雷达距离数据range【0】，添加到列表scanfilter中

        else:   #samples_view不等于1 ，大于1   ；小于1
            left_lidar_samples_ranges = -(samples_view//2 + samples_view % 2)
            right_lidar_samples_ranges = samples_view//2
            
            left_lidar_samples = scan.ranges[left_lidar_samples_ranges:]  #[m : ] 代表列表中的第m+1项到最后一项
            right_lidar_samples = scan.ranges[:right_lidar_samples_ranges] #[ : n] 代表列表中的第一项到第n项
            scan_filter.extend(left_lidar_samples + right_lidar_samples)  #extend() 函数用于在列表末尾一次性追加另一个序列中的多个值

        for i in range(samples_view):   # Python for循环可以遍历任何序列的项目，如一个列表或者一个字符串。
            if scan_filter[i] == float('Inf'):  #正负无穷
                scan_filter[i] = 3.5
            elif math.isnan(scan_filter[i]):   #是不是非数字浮点数
                scan_filter[i] = 0
        
        return scan_filter

    def obstacle(self):
      # this is the msgs variant, has Twist type, no data now
        twist = Twist()      
        turtlebot_moving = True

        while not rospy.is_shutdown():
            lidar_distances = self.get_scan()
            min_distance = min(lidar_distances) #min() 方法返回给定参数的最小值，

            if min_distance < SAFE_STOP_DISTANCE:
                if turtlebot_moving:
                    twist.linear.x = 0.0
                    twist.angular.z = 0.0
                    self._cmd_pub.publish(twist)
                    turtlebot_moving = False
                    rospy.loginfo('Stop!')
            else:
                twist.linear.x = LINEAR_VEL
                twist.angular.z = 0.0
                self._cmd_pub.publish(twist)
                turtlebot_moving = True
                rospy.loginfo('Distance of the obstacle : %f', min_distance)

def main():
    rospy.init_node('turtlebot3_obstacle')
    try:
        obstacle = Obstacle()
    except rospy.ROSInterruptException:
        pass

if __name__ == '__main__':
    main()