添加超声波障碍图层并用rbx1仿真

１安装rbx1

sudo apt-get install ros-indigo-turtlebot-bringup \
ros-indigo-turtlebot-create-desktop ros-indigo-openni-* \ros-indigo-openni2-* ros-indigo-freenect-* ros-indigo-usb-cam \ros-indigo-laser-* ros-indigo-hokuyo-node \ros-indigo-audio-common gstreamer0.10-pocketsphinx \
ros-indigo-pocketsphinx ros-indigo-slam-gmapping \
ros-indigo-joystick-drivers python-rosinstall \
ros-indigo-orocos-kdl ros-indigo-python-orocos-kdl \python-setuptools ros-indigo-dynamixel-motor-* \
libopencv-dev python-opencv ros-indigo-vision-opencv \
ros-indigo-depthimage-to-laserscan ros-indigo-arbotix-* \ros-indigo-turtlebot-teleop ros-indigo-move-base \ros-indigo-map-server ros-indigo-fake-localization \ros-indigo-amcl git subversion mercurial

２rbx1 package 的下载：

cd ~/catkin_ws/src
git clone https://github.com/pirobot/rbx1.git 
cd rbx1
git checkout indigo-devel
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

编译的时候可能出现某个文件未经授权导致编译失败，修改一下该文件的权限即可．

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_msgs，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;
    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: sonar,   type: "range_sensor_layer::RangeSensorLayer"}

  

4 观察新加层是否起作用

在roslaunch rbx1_nav fake_move_base_blank_map.launch中添加一个静态转换<node pkg="tf" type="static_transform_publisher" name="base_link_ultrasound_broadcaster" args="0 0 0 0 0 0 /base_link /ultrasound 100" />（根据自己的实际坐标系定/base_link /ultrasound ）

1.roslaunch rbx1_bringup fake_turtlebot.launch
2.roslaunch rbx1_nav fake_move_base_blank_map.launch

• 在启动之前超声波发布的cpp对应的可执行文件

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

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