ROS : Navigation 基于碰撞传感器、悬崖传感器的实时避障 [kobuki]

最新推荐文章于 2025-02-21 10:57:24 发布

eln

最新推荐文章于 2025-02-21 10:57:24 发布

阅读量3.8k

点赞数 3

分类专栏： ROS

本文链接：https://blog.youkuaiyun.com/enland_lan/article/details/104854714

版权

ROS 专栏收录该内容

7 篇文章

订阅专栏

ROS : Navigation 基于碰撞传感器、悬崖传感器的实时避障 [kobuki]

话题消息

碰撞传感器

~events/bumper ( kobuki_msgs/BumperEvent ) 提供一个保险杠事件，每当按下或释放保险杠时都会发生此事件。

~$ rostopic info /mobile_base/events/bumper
Type: kobuki_msgs/BumperEvent

Publishers:
 * /mobile_base_nodelet_manager (http://10.42.0.1:44834/)

Subscribers:
 * /mobile_base_nodelet_manager (http://10.42.0.1:44834/)

~$ rosmsg show kobuki_msgs/BumperEvent
uint8 LEFT=0
uint8 CENTER=1
uint8 RIGHT=2
uint8 RELEASED=0
uint8 PRESSED=1
uint8 bumper
uint8 state

~$ rostopic echo /mobile_base/events/bumper
bumper: 1  // 左前 0 ，正前 1 ，右前 2
state: 1  // 保险杠按压（撞击）为 1 ，未撞击为 0
---
bumper: 1
state: 0
---

悬崖传感器

~events/cliff ( kobuki_msgs/CliffEvent ) 提供悬崖传感器事件，每当机器人接近或离开悬崖时就会发生。

~$ rostopic info /mobile_base/events/cliff
Type: kobuki_msgs/CliffEvent

Publishers:
 * /mobile_base_nodelet_manager (http://10.42.0.1:44834/)

Subscribers: None
 * /mobile_base_nodelet_manager (http://10.42.0.1:44834/)


~$ rosmsg info kobuki_msgs/CliffEvent
uint8 LEFT=0
uint8 CENTER=1
uint8 RIGHT=2
uint8 FLOOR=0
uint8 CLIFF=1
uint8 sensor
uint8 state
uint16 bottom


~$ rostopic echo /mobile_base/events/cliff

---
sensor: 1
state: 1
bottom: 616
---
sensor: 2
state: 1
bottom: 571
---
sensor: 2
state: 0
bottom: 544
---

点云数据

~sensors/bumper_pointcloud ( sensor_msgs/PointCloud2 ) 触发碰撞传感器、悬崖传感器事件时转 点云数据 ，代码见 kobuki_bumper2pc.cpp 。

# The point cloud data may be organized 2d (image-like) or 1d
# (unordered). Point clouds organized as 2d images may be produced by
# camera depth sensors such as stereo or time-of-flight.

# Time of sensor data acquisition, and the coordinate frame ID (for 3d
# points).
Header header

# 2D structure of the point cloud. If the cloud is unordered, height is
# 1 and width is the length of the point cloud.
uint32 height
uint32 width

# Describes the channels and their layout in the binary data blob.
PointField[] fields

bool    is_bigendian # Is this data bigendian?
uint32  point_step   # Length of a point in bytes
uint32  row_step     # Length of a row in bytes
uint8[] data         # Actual point data, size is (row_step*height)

bool is_dense        # True if there are no invalid points

height 与 width 是指点云数据的高和宽，一般无序点云的高为 1 ，宽为点云中激光点的个数；结构化点云的高和宽均大于 1 。

sensor_msgs/PointField[] fields 由以下组成：

# This message holds the description of one point entry in the
# PointCloud2 message format.
uint8 INT8    = 1
uint8 UINT8   = 2
uint8 INT16   = 3
uint8 UINT16  = 4
uint8 INT32   = 5
uint8 UINT32  = 6
uint8 FLOAT32 = 7
uint8 FLOAT64 = 8

string name      # Name of field
uint32 offset    # Offset from start of point struct
uint8  datatype  # Datatype enumeration, see above
uint32 count     # How many elements in the field

name 是指点云包含的域的名称，如 x 、 y 、 z 、 intensity 、 ring 等；
offset 是指在 data 的每个数据中该域的偏移量；
datatype 是指以上 1~8 的数据类型；
count 是指该域有多少个元素，一般为 1 。
每一帧点云均有该项数据。

is_bigendian 数据是大端存储还是小端存储的标志。
pointstep 表示每个点的字节长度，常见的为 32 。
rowstep 表示每行的字节长度。
data 表示所有的点的数据，以字节存储。 uint8[] data 代表 vector 类型，有 size 、 push_back 、 clear 等操作。
is_dense 若是 true ，代表点云数据中不包含无效点（ nan 点）；若是 false ，代表点云中包含无效点。

~$ rostopic info /mobile_base/sensors/bumper_pointcloud
Type: sensor_msgs/PointCloud2

Publishers:
 * /mobile_base_nodelet_manager (http://10.42.0.1:44834/)

Subscribers:
 * /move_base (http://10.42.0.1:42621/)


~$ rosmsg show sensor_msgs/PointCloud2
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height
uint32 width
sensor_msgs/PointField[] fields
  uint8 INT8=1
  uint8 UINT8=2
  uint8 INT16=3
  uint8 UINT16=4
  uint8 INT32=5
  uint8 UINT32=6
  uint8 FLOAT32=7
  uint8 FLOAT64=8
  string name
  uint32 offset
  uint8 datatype
  uint32 count
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense


~$ rostopic echo /mobile_base/sensors/bumper_pointcloud
header:  // 点云的头信息
  seq: 0
  stamp:  // 时间戳
    secs: 1455218508
    nsecs: 292744004
  frame_id: "/base_link"
height: 1  // 如果 cloud 是无序的，则 height 是 1
width: 3  // 点云的长度
fields:  // sensor_msgs/PointField[] fields
  -
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  -
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  -
    name: "z"
    offset: 8
    datatype: 7
    count: 1
is_bigendian: False
point_step: 12  // 一个点占的比特数
row_step: 36  // 一行的长度占用的比特数
data: [221, 206, 8, 66, 67, 240, 187, 66, 10, 215, 35, 61, 143, 194, 117, 62, 0, 0, 0, 0, 10, 215, 35, 61, 221, 206, 8, 66, 67, 240, 187, 194, 10, 215, 35, 61]  // Actual point data, size is (row_step*height) 这部分数据是点云的二进制数据流，必须要单独解析，直接读取没有任何意义
is_dense: True  // True 代表没有非法数据点
---
header:
  seq: 1
  stamp:
    secs: 1455218508
    nsecs: 347700570
  frame_id: "/base_link"
height: 1
width: 3
fields:
  -
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  -
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  -
    name: "z"
    offset: 8
    datatype: 7
    count: 1
is_bigendian: False
point_step: 12
row_step: 36
data: [221, 206, 8, 66, 67, 240, 187, 66, 10, 215, 35, 61, 143, 194, 117, 62, 0, 0, 0, 0, 10, 215, 35, 61, 221, 206, 8, 66, 67, 240, 187, 194, 10, 215, 35, 61]
is_dense: True
---

传感器详细信息

~sensors/core ( kobuki_msgs/SensorState ) kobuki 整体传感器详细信息， 50Hz 。

note :

kobuki_bumper2pc.cpp 是利用该话题消息将碰撞传感器和悬崖传感器数据转点云数据的。主要利用了 uint8 bumper 和 uint8 cliff ，这两个值在多个传感器触发时会将各个传感器对应的状态值求和得到。

~$ rostopic info /mobile_base/sensors/core
Type: kobuki_msgs/SensorState

Publishers:
 * /mobile_base_nodelet_manager (http://10.42.0.1:44834/)

Subscribers:
 * /mobile_base_nodelet_manager (http://10.42.0.1:44834/)


~$ rosmsg info kobuki_msgs/SensorState
uint8 BUMPER_RIGHT=1
uint8 BUMPER_CENTRE=2
uint8 BUMPER_LEFT=4
uint8 WHEEL_DROP_RIGHT=1
uint8 WHEEL_DROP_LEFT=2
uint8 CLIFF_RIGHT=1
uint8 CLIFF_CENTRE=2
uint8 CLIFF_LEFT=4
uint8 BUTTON0=1
uint8 BUTTON1=2
uint8 BUTTON2=4
uint8 DISCHARGING=0
uint8 DOCKING_CHARGED=2
uint8 DOCKING_CHARGING=6
uint8 ADAPTER_CHARGED=18
uint8 ADAPTER_CHARGING=22
uint8 OVER_CURRENT_LEFT_WHEEL=1
uint8 OVER_CURRENT_RIGHT_WHEEL=2
uint8 OVER_CURRENT_BOTH_WHEELS=3
uint8 DIGITAL_INPUT0=1
uint8 DIGITAL_INPUT1=2
uint8 DIGITAL_INPUT2=4
uint8 DIGITAL_INPUT3=8
uint8 DB25_TEST_BOARD_CONNECTED=64
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint16 time_stamp
uint8 bumper
uint8 wheel_drop
uint8 cliff
uint16 left_encoder
uint16 right_encoder
int8 left_pwm
int8 right_pwm
uint8 buttons
uint8 charger
uint8 battery
uint16[] bottom
uint8[] current
uint8 over_current
uint16 digital_input
uint16[] analog_input


# 碰撞传感器
~$ rostopic echo /mobile_base/sensors/core
---
header:
  seq: 13711
  stamp:
    secs: 1455208898
    nsecs: 480690468
  frame_id: ''
time_stamp: 38084
bumper: 4
wheel_drop: 0
cliff: 0
left_encoder: 5062
right_encoder: 5013
left_pwm: 0
right_pwm: 0
buttons: 0
charger: 0
battery: 156
bottom: [1572, 1895, 1633]
current: [0, 0]
over_current: 0
digital_input: 8
analog_input: [4095, 4095, 4095, 4095]
---
header:
  seq: 13754
  stamp:
    secs: 1455208900
    nsecs: 856141130
  frame_id: ''
time_stamp: 40464
bumper: 6
wheel_drop: 0
cliff: 0
left_encoder: 5046
right_encoder: 5012
left_pwm: 0
right_pwm: 0
buttons: 0
charger: 0
battery: 156
bottom: [1573, 1893, 1653]
current: [0, 0]
over_current: 0
digital_input: 8
analog_input: [4095, 4095, 4095, 4095]
---
header:
  seq: 13757
  stamp:
    secs: 1455208900
    nsecs: 996265722
  frame_id: ''
time_stamp: 40604
bumper: 7
wheel_drop: 0
cliff: 0
left_encoder: 5044
right_encoder: 5012
left_pwm: 0
right_pwm: 0
buttons: 0
charger: 0
battery: 156
bottom: [1584, 1898, 1634]
current: [0, 0]
over_current: 0
digital_input: 8
analog_input: [4095, 4095, 4095, 4095]
---

# 悬崖传感器
~$ rostopic echo /mobile_base/sensors/core
---
header:
  seq: 20113
  stamp:
    secs: 1455209221
    nsecs: 398578496
  frame_id: ''
time_stamp: 33904
bumper: 0
wheel_drop: 3
cliff: 1
left_encoder: 5333
right_encoder: 5051
left_pwm: 0
right_pwm: 0
buttons: 0
charger: 0
battery: 156
bottom: [383, 636, 666]
current: [0, 0]
over_current: 0
digital_input: 8
analog_input: [4095, 4095, 4095, 4095]
---
header:
  seq: 20111
  stamp:
    secs: 1455209221
    nsecs: 316537941
  frame_id: ''
time_stamp: 33824
bumper: 0
wheel_drop: 3
cliff: 3
left_encoder: 5333
right_encoder: 5051
left_pwm: 0
right_pwm: 0
buttons: 0
charger: 0
battery: 156
bottom: [213, 519, 1202]
current: [0, 0]
over_current: 0
digital_input: 8
analog_input: [4095, 4095, 4095, 4095]
---
header:
  seq: 20175
  stamp:
    secs: 1455209224
    nsecs: 513620609
  frame_id: ''
time_stamp: 37024
bumper: 0
wheel_drop: 3
cliff: 7
left_encoder: 5333
right_encoder: 5051
left_pwm: 0
right_pwm: 0
buttons: 0
charger: 0
battery: 155
bottom: [191, 149, 78]
current: [0, 0]
over_current: 0
digital_input: 8
analog_input: [4095, 4095, 4095, 4095]
---

基于碰撞传感器、悬崖传感器的实时避障

通过碰撞传感器、悬崖传感器实时添加障碍物的方式是：通过修改 global_costmap 或者 local_costmap 配置文件，调用 Costmap 的障碍物层（也就是，自定义创建 costmap layer 的最后一步， costmap_2d 功能包已经完成前面几步）。示例如下：

修改 costmap_common_params_nav.yaml 文件，在障碍物层添加障碍数据来源 bump ，指定数据类型是 PointCloud2 ，指定话题为 mobile_base/sensors/bumper_pointcloud 。

由于触发碰撞传感器、悬崖传感器事件后均转为 topic 为 /mobile_base/sensors/bumper_pointcloud 的点云数据，因此，这里指定的数据来源 bump 不仅仅是碰撞传感器的，还包括悬崖传感器的。
```
# ...

obstacle_layer:
  # ...
  observation_sources:  scan bump
  scan:
    data_type: LaserScan
    topic: scan
    # ...
  bump:
    data_type: PointCloud2
    topic: mobile_base/sensors/bumper_pointcloud
    marking: true
    clearing: true
    min_obstacle_height: 0.0
    max_obstacle_height: 0.1
    expected_update_rate: 0
# ...
```

修改 global_costmap_params.yaml 、 local_costmap_params.yaml 文件，调用 Costmap 的障碍物层，这里指定的是 costmap_2d::VoxelLayer 。需要注意的是，插件的调用顺序是从最底层到最顶层。

local_costmap:
   # ...
   plugins:
    - {name: static_layer,        type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,      type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}

global_costmap:
   # ...
   plugins:
     - {name: static_layer,            type: "costmap_2d::StaticLayer"}
     - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
     - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

转点云数据源码解读

kobuki_bumper2pc 是碰撞传感器和悬崖传感器点云 Nodelet 节点实现。

├── CHANGELOG.rst
├── CMakeLists.txt
├── include
│   └── kobuki_bumper2pc
│       └── kobuki_bumper2pc.hpp  # 碰撞传感器和悬崖传感器点云 Nodelet 类定义
├── launch
│   └── standalone.launch         # 独立启动保险杆和悬崖 Nodelet 节点
├── package.xml
├── plugins
│   └── nodelet_plugins.xml       # Nodelet 插件
└── src
    └── kobuki_bumper2pc.cpp      # 碰撞传感器和悬崖传感器点云 Nodelet 类实现

kobuki_bumper2pc.cpp

kobuki_bumper2pc.cpp 碰撞传感器和悬崖传感器点云 Nodelet 类实现：

发布导航包可使用的点云话题
在导航中作为 Nodelet 与 kobuki_node 一起工作

Bumper2PcNodelet::onInit

void Bumper2PcNodelet::onInit()
{
  ros::NodeHandle nh = this->getPrivateNodeHandle();

  // Bumper/cliff pointcloud distance to base frame; should be something like the robot radius plus
  // costmap resolution plus an extra to cope with robot inertia. This is a bit tricky parameter: if
  // it's too low, costmap will ignore this pointcloud (the robot footprint runs over the hit obstacle),
  // but if it's too big, hit obstacles will be mapped too far from the robot and the navigation around
  // them will probably fail.
  std::string base_link_frame;
  double r, h, angle;
  nh.param("pointcloud_radius", r, 0.25); pc_radius_ = r;  // 碰撞传感器/悬崖传感器点云到底座半径范围
  nh.param("pointcloud_height", h, 0.04); pc_height_ = h;  // 碰撞传感器/悬崖传感器点云高度
  nh.param("side_point_angle", angle, 0.34906585);    // 默认约 20 度
      nh.param<std::string>("base_link_frame", base_link_frame, "/base_link");  // 发布的点云数据是相对于 /base_link 的，也就是相对于机器人中心位置的

      // Lateral points x/y coordinates; we need to store float values to memcopy later  // 左右位置的传感器的坐标
      p_side_x_ = + pc_radius_*sin(angle); // angle degrees from vertical  // 左右位置的碰撞传感器/悬崖传感器发布的点云数据的 x 轴坐标是 pc_radius_*sin(angle)
      p_side_y_ = + pc_radius_*cos(angle); // angle degrees from vertical  // 左侧碰撞传感器/悬崖传感器发布的点云数据的 y 轴坐标是 pc_radius_*cos(angle)
      n_side_y_ = - pc_radius_*cos(angle); // angle degrees from vertical  // 右侧碰撞传感器/悬崖传感器发布的点云数据的 y 轴坐标是 -1.0*pc_radius_*cos(angle)

  // Prepare constant parts of the pointcloud message to be  published
  pointcloud_.header.frame_id = base_link_frame;
  pointcloud_.width  = 3;  // 点云的宽为 3 (左中右)
  pointcloud_.height = 1;  // 无序点云的高为 1
  pointcloud_.fields.resize(3);

  // Set x/y/z as the only fields  // 点云包含的域的名称
  pointcloud_.fields[0].name = "x";
  pointcloud_.fields[1].name = "y";
  pointcloud_.fields[2].name = "z";

  int offset = 0;
  // All offsets are *4, as all field data types are float32
  for (size_t d = 0; d < pointcloud_.fields.size(); ++d, offset += 4)
  {
    pointcloud_.fields[d].count    = 1;  // 指该域有多少个元素，一般为 1
    pointcloud_.fields[d].offset   = offset;  // offset 是在 data 的每个数据中该域的偏移量
    pointcloud_.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
  }

  pointcloud_.point_step = offset;  // 每个点的字节长度
  pointcloud_.row_step   = pointcloud_.point_step * pointcloud_.width;  // 每行的字节长度

  pointcloud_.data.resize(3 * pointcloud_.point_step);  // data 表示所有的点的数据，以字节存储
  pointcloud_.is_bigendian = false;  // 数据是大端存储还是小端存储的标志
  pointcloud_.is_dense     = true;  // 点云数据中不包含无效点（ nan 点）

  // Bumper/cliff "points" fix coordinates (the others depend on sensor activation/deactivation)

  // y: always 0 for central bumper  // 中间传感器的点云 y 的值总为 0
  memcpy(&pointcloud_.data[1 * pointcloud_.point_step + pointcloud_.fields[1].offset], &ZERO, sizeof(float));

  // z: constant elevation from base frame  // 点云 z 的值为 pc_height_
  memcpy(&pointcloud_.data[0 * pointcloud_.point_step + pointcloud_.fields[2].offset], &pc_height_, sizeof(float));
  memcpy(&pointcloud_.data[1 * pointcloud_.point_step + pointcloud_.fields[2].offset], &pc_height_, sizeof(float));
  memcpy(&pointcloud_.data[2 * pointcloud_.point_step + pointcloud_.fields[2].offset], &pc_height_, sizeof(float));

  // 这两个话题均做了重映射，见 standalone.launch
  pointcloud_pub_  = nh.advertise <sensor_msgs::PointCloud2> ("pointcloud", 10);  // 发布到 pointcloud 话题
  core_sensor_sub_ = nh.subscribe("core_sensors", 10, &Bumper2PcNodelet::coreSensorCB, this);  // 订阅 core_sensors 话题，数据类型是 SensorState ，进入回调函数 Bumper2PcNodelet::coreSensorCB

  ROS_INFO("Bumper/cliff pointcloud configured at distance %f and height %f from base frame", pc_radius_, pc_height_);
}

memcpy()

void *memcpy ( void *destination, const void *source, size_t num );

destination 指向用于存储复制内容的目标数组，类型强制转换为 void* 指针。
source 指向要复制的数据源，类型强制转换为 void* 指针。
num 要被复制的字节数。

Bumper2PcNodelet::coreSensorCB

在 Bumper2PcNodelet::onInit 中已经固定设置好中间传感器的 y 的值以及所有传感器的 z 的值。该回调函数将利用 SensorState 消息类型的数据设置中间传感器 x 的值以及两侧传感器 x y 的值。

void Bumper2PcNodelet::coreSensorCB(const kobuki_msgs::SensorState::ConstPtr& msg)
{
  if (pointcloud_pub_.getNumSubscribers() == 0)
    return;

  // We publish just one "no events" pc (with all three points far away) and stop spamming when bumper/cliff conditions disappear
  if (! msg->bumper && ! msg->cliff && ! prev_bumper && ! prev_cliff)
    return;

  prev_bumper = msg->bumper;
  prev_cliff  = msg->cliff;

  // We replicate the sensors order of bumper/cliff event messages: LEFT = 0, CENTER = 1 and RIGHT = 2
  // For any of {left/center/right} with no bumper/cliff event, we publish a faraway point that won't get used
  if ((msg->bumper & kobuki_msgs::SensorState::BUMPER_LEFT) ||
      (msg->cliff  & kobuki_msgs::SensorState::CLIFF_LEFT))  //左侧传感器
  {
    memcpy(&pointcloud_.data[0 * pointcloud_.point_step + pointcloud_.fields[0].offset], &p_side_x_, sizeof(float));
    memcpy(&pointcloud_.data[0 * pointcloud_.point_step + pointcloud_.fields[1].offset], &p_side_y_, sizeof(float));
  }
  else
  {
    memcpy(&pointcloud_.data[0 * pointcloud_.point_step + pointcloud_.fields[0].offset], &P_INF_X, sizeof(float));
    memcpy(&pointcloud_.data[0 * pointcloud_.point_step + pointcloud_.fields[1].offset], &P_INF_Y, sizeof(float));
  }

  if ((msg->bumper & kobuki_msgs::SensorState::BUMPER_CENTRE) ||
      (msg->cliff  & kobuki_msgs::SensorState::CLIFF_CENTRE))  //中间传感器
  {
    memcpy(&pointcloud_.data[1 * pointcloud_.point_step + pointcloud_.fields[0].offset], &pc_radius_, sizeof(float));
  }
  else
  {
    memcpy(&pointcloud_.data[1 * pointcloud_.point_step + pointcloud_.fields[0].offset], &P_INF_X, sizeof(float));
  }

  if ((msg->bumper & kobuki_msgs::SensorState::BUMPER_RIGHT) ||
      (msg->cliff  & kobuki_msgs::SensorState::CLIFF_RIGHT))  //右侧传感器
  {
    memcpy(&pointcloud_.data[2 * pointcloud_.point_step + pointcloud_.fields[0].offset], &p_side_x_, sizeof(float));
    memcpy(&pointcloud_.data[2 * pointcloud_.point_step + pointcloud_.fields[1].offset], &n_side_y_, sizeof(float));
  }
  else
  {
    memcpy(&pointcloud_.data[2 * pointcloud_.point_step + pointcloud_.fields[0].offset], &P_INF_X, sizeof(float));
    memcpy(&pointcloud_.data[2 * pointcloud_.point_step + pointcloud_.fields[1].offset], &N_INF_Y, sizeof(float));
  }

  pointcloud_.header.stamp = msg->header.stamp;
  pointcloud_pub_.publish(pointcloud_);  //发布点云数据
}

在 kobuki_bumper2pc.hpp 中定义了 P_INF_X 、 P_INF_Y 、 N_INF_Y 的默认值：

Bumper2PcNodelet()
  : P_INF_X(+100*sin(0.34906585)),
    P_INF_Y(+100*cos(0.34906585)),
    N_INF_Y(-100*cos(0.34906585)),
    ZERO(0), prev_bumper(0), prev_cliff(0) { }
~Bumper2PcNodelet() { }

standalone.launch

standalone.launch 独立启动保险杆和悬崖 Nodelet 节点：

启动 nodelet_manager Nodelet 管理器
启动碰撞传感器保险杆和悬崖传感器点云 Nodelet
设置 pointcloud_radius 参数，碰撞传感器、悬崖传感器点云到底座半径范围。大概就是机器人半径加上 costmap 识别区用于应对机器人的惯性。这个参数不容易设置：设置太小 costmap 会忽略点云，导致机器人撞上障碍；设置太大，障碍占据地方太大，围绕他们做导航，就可能失败。
重映射 pointcloud ， core_sensors 两个话题

<!--
  Example standalone launcher for the bumper/cliff to pointcloud nodelet.
  Parameter pointcloud_radius gives the bumper/cliff pointcloud distance to base frame;
  should be something like the robot radius plus plus costmap resolution plus an extra to
  cope with robot inertia. This is a bit tricky parameter: if it's too low, costmap will
  ignore this pointcloud (the robot footprint runs over the hit obstacle), but if it's too
  big, hit obstacles will be mapped too far from the robot and the navigation around them
  will probably fail.
 -->
<launch>
  <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
  <node pkg="nodelet" type="nodelet" name="bumper2pointcloud" args="load kobuki_bumper2pc/Bumper2PcNodelet nodelet_manager">
    <param name="pointcloud_radius" value="0.25"/>
    <remap from="bumper2pointcloud/pointcloud"   to="mobile_base/sensors/bumper_pointcloud"/>
    <remap from="bumper2pointcloud/core_sensors" to="mobile_base/sensors/core"/>
  </node>
</launch>