https://blog.csdn.net/MLlearnerTJ/article/details/71791573

两种思路： 1.将超声波反馈信息作为一个新的layer添加到cost-map当中; 2.将超声波数据（ros_msgs/Range）转换为move_base包需要的输入格式（LaserScan或者PointCloud; 这里重点讲解方式一，因为方式二尚未尝试。

步骤（以ros_by_example书中第八章例程为基础）： 1 编译range_sensor_layer插件并添加到ROS环境 主要参考 为ROS添加插件 http://wiki.ros.org/range_sensor_layer 1.1）根据ROS分支下载对应git包: https://github.com/DLu/navigation_layers.git 1.2）编译range_sensor_layer 注意可只编译range_sensor_layer，将该包拎出来单独建立一个工作空间（当然也可以扔到已有工程项目的工作空间中）进行编译，记得source（当时被这个坑了好久，结果就是一直无法识别这个层）。 1.3）检查该插件是否加入ROS系统 rospack plugins --attrib=plugin costmap_2d 若未加入成功：那么输出将是这样： 这里写图片描述 这个时候需要确认一下是否source devel文件夹下的setup.bash文件了。成功的话，将是下面的样子，即除了ROS系统自带的cost-map图层，还有刚刚编译的插件： 这里写图片描述

2 发送超声波数据到ROS（模拟） 根据ros定义的Range topic类型发送模拟数据sensor_msgs/Range 2.1）新建工作空间，新建包，依赖roscpp，sensor_msg，tf. 2.2) 代码段：

#include "ros/ros.h" #include "sensor_msgs/Range.h" #include "std_msgs/Header.h" #include <time.h> #include #include <tf/transform_broadcaster.h>

int main(int argc, char **argv) { ros::init(argc, argv, "talkerUltraSound"); ros::NodeHandle n; ros::Publisher ultrasound_pub = n.advertise<sensor_msgs::Range>("UltraSoundPublisher", 10); ros::Rate loop_rate(10); int count = 0; while (ros::ok()) {

sensor_msgs::Range msg;
std_msgs::Header header;
header.stamp = ros::Time::now();
header.frame_id = "/ultrasound";
msg.header = header;
msg.field_of_view = 1;
msg.min_range = 0;
msg.max_range = 5;
msg.range = rand()%3;//rand()%3;

tf::TransformBroadcaster broadcaster;
  broadcaster.sendTransform(
  tf::StampedTransform(
    tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
    ros::Time::now(),"base_link", "ultrasound"));


ultrasound_pub.publish(msg);

loop_rate.sleep();
++count;

} return 0; }

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41

3 配置导航包参数文件，添加range_data_layer ROS导航包中有关于cost-map的配置文件有三个, 由于只是作为测试，我之配置了local_costmap_params.yaml文件，即只是让超声波作用于局部避障规划，配置如下;

local_costmap: global_frame: /odom robot_base_frame: /base_link update_frequency: 5.0 publish_frequency: 5.0 static_map: false rolling_window: true width: 6.0 height: 6.0 resolution: 0.05 transform_tolerance: 5.0

plugins: #- {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} - {name: sonar, type: "range_sensor_layer::RangeSensorLayer"}

sonar: topics: ["/UltraSoundPublisher"]#注意替换成自己的topic名字 no_readings_timeout: 0.0

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

4 观察新加层是否起作用

roslaunch rbx1_nav test_ultrasound_nav.launch(换成自己的launch)

1

在rviz中打开map和Range,RobotModel后可以得到下图所示的结果： 机器人发送的锥型面就是模拟的超声波数据 在左边map菜单的topic选择loca_cost_map将得到超声波加入后新增的障碍物图层： 如图机器人前方黑色部分就是超声波返回的障碍物信息