ROS 常常会用到导航系统的开发,为了有助于理解导航原理的背后是什么,该篇主要以得到距离信息为主写了非常简单的demo。
rosmsg show 展示的激光雷达数据信息:

Header header            # Header也是一个结构体,包含了seq,stamp,frame_id,其中seq指的是扫描顺序增加的id,stamp包含了开始扫描的时间和与开始扫描的时间差,frame_id是扫描的参考系名称.注意扫描是逆时针从正前方开始扫描的.

float32 angle_min        # 开始扫描的角度(角度)
float32 angle_max        # 结束扫描的角度(角度)
float32 angle_increment  # 每一次扫描增加的角度(角度)

float32 time_increment   # 测量的时间间隔(s)
float32 scan_time        # 扫描的时间间隔(s)

float32 range_min        # 距离最小值(m)
float32 range_max        # 距离最大值(m)

float32[] ranges         # 距离数组(长度360)
float32[] intensities    # 与设备有关,强度数组(长度360)

代码接受距离信息的实现:

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

class Obstacle():
    def __init__(self):
        self.LIDAR_ERR = 0.05
        self._cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
        self.obstacle()

    def get_scan(self):
        msg = rospy.wait_for_message("scan", LaserScan)
        self.scan_filter = []
        for i in range(360):
            if i <= 15 or i > 335:
                if msg.ranges[i] >= self.LIDAR_ERR:
                    self.scan_filter.append(msg.ranges[i])

    def obstacle(self):
        self.twist = Twist()
        while not rospy.is_shutdown():
            self.get_scan()

            if min(self.scan_filter) < 0.5:
                self.twist.linear.x = 0.0
                self.twist.angular.z = 0.0
                self._cmd_pub.publish(self.twist)
                rospy.loginfo('Stop!')

            else:
                self.twist.linear.x = 0.0
                self.twist.angular.z = 0.0
                rospy.loginfo('distance of the obstacle : %f', min(self.scan_filter))

            self._cmd_pub.publish(self.twist)

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

if __name__ == '__main__':
    main()

其中get_scan()函数用到得到雷达距离探测物的距离,刷新时间随着你扫描周期变化,注意该例只是对某个角度的探测,想要全方位的探测的话自己改动探测范围即可。

over!!!

Logo

有“AI”的1024 = 2048,欢迎大家加入2048 AI社区

更多推荐