ROS导航-向cost-map中添加超声波障碍图层

两种思路：
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 <sstream>
#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;
}

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

4 观察新加层是否起作用

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

在rviz中打开map和Range,RobotModel后可以得到下图所示的结果：

在左边map菜单的topic选择loca_cost_map将得到超声波加入后新增的障碍物图层：