ros自己写避障算法_如何在ROS中使用sonar来导航避障

1.下载sonar_layer的代码

实际只需要其中的range_sensor_layer放到工作空间catkin_make。实验时放置于src中,可以考虑放置于navigation文件夹中进行尝试。

2.将plugin插入ros系统

实际上下载的源码配置已完善,编译通过时range_sensor_layer已插入ros系统,在此只需要source一下cartkin_ws,验证一下即可:rospack plugins --attrib=plugin costmap_2d。

3.sonar_layer在costmap_2d(navigation)中的使用

3.1.在costmap_common_params.yaml中配置sonar_layer的参数

sonar_layer:

ns ( string , default : ”” ) : 命名空间,用作所有topic的前缀;

topics(Array of strings , default : [‘/sonar’] ) : 列举可以订阅的距离topic;

no_readings_timeout ( double , default : 0.0 ) : 如果是0,该参数不起作用,否则如果层在该参数指定时间内没有收到传感器任何数据,层会给出warning并被标记为没有数据流;

clear_threshold( double, default : .2 ) : 概率比clear_threshold低的cell在master costmap中被标记为free空间;

mark_threshold ( double , default : .8 ) : 概率比mark_threshold高的cell在master costmap中被标记为lethal obstacles;

clear_on_max_reading ( bool , default : false ) : 是否将超出sonar最大距离清除。

3.2.在global_costmap_params.yaml中插入sonar_layer

plugins:

- { name : sonar_layer , type : “ range_sensor_layer::RangeSensorLayer”}

其实,也可以在local_costmap_params.yaml中插入sonar_layer,方法是一样的。根据具体需求选择插入的位置就行了。

4.超声波测距模块ROS驱动程序

单片机读取到超声波测距模块的值后,通过串口发送给ROS主机,ROS主机需要用ROS驱动程序来接收串口的数据,并将测距值发布到ROS的话题上。这里使用python写的ROS驱动程序,节点的主代码如下,关于ROS功能包的其他配置文件是通用的,篇幅限制就不贴出了。

#!/usr/bin/env python

import rospy

import serial

import binascii

from sensor_msgs.msg import Range

class Sonar():

def __init__(self):

rospy.init_node('sonar_publisher')

self.rate = rospy.get_param('~rate', 10.0)

self.device_port = rospy.get_param('~device_port',"/dev/ttyUSB0")

self.frame_id = rospy.get_param('~sonar_frame_id', '/sonar')

self.field_of_view = rospy.get_param('~field_of_view', 0.1)

self.min_range = rospy.get_param('~min_range', 0.02)

self.max_range = rospy.get_param('~max_range', 4.50)

self.sonar_pub = rospy.Publisher('/sonar', Range, queue_size=50)

self.ser = serial.Serial(self.device_port, 9600, timeout=1)

def handleSonar(self):

sonar = Range()

sonar.header.frame_id = self.frame_id

sonar.radiation_type = Range.ULTRASOUND

sonar.field_of_view = 0.1

sonar.min_range = self.min_range

sonar.max_range = self.max_range

self.ser.write(binascii.a2b_hex('55')) ##modify as your UART command

line = self.ser.readline()

try:

high = ord(line[0])

low = ord(line[1])

distance = (high*256. + low)/1000.

except:

distance = 11.

# print distance,'m'

# print ":".join("{:02x}".format(ord(c)) for c in line)

sonar.header.stamp = rospy.Time.now()

sonar.range = distance

self.sonar_pub.publish(sonar)

def spin(self):

r = rospy.Rate(self.rate)

while not rospy.is_shutdown():

self.handleSonar()

r.sleep()

if __name__ == '__main__':

scanner = Sonar()

rospy.loginfo("=== run")

scanner.spin()

rospy.loginfo("=== end")

对应的节点的launch启动文件也贴在下面吧。

你可能感兴趣的:(ros自己写避障算法)