两轮差速底盘SLAM系列(15)--虚拟墙

前言

在机器人实际建图导航时，因为场地的复杂程度和多变，导致地图很复杂。这时我们就可以使用虚拟墙来在RVIZ上面布置一些虚拟的墙体，并加进去虚拟的激光，好让虚拟的墙体也拥有膨胀区域，机器人到达虚拟墙体能够进行避障。这样可以使机器人进行路径规划导航时提高效率，虚拟墙功能包yocs_virtual_sensor是在turtlebot2那里调包出来修改的。

使用方法

在yocs_virtual_sensor wiki可以找到对应源代码的Github链接，把功能包下载到本地进行修改即可。直接下载回来的是整个yujin_ocs功能包，我们只需要把其中的yocs_virtual_sensor和yocs_math_toolkit功能包放到ROS工作空间的src目录下（例如：~/catkin_ws/src）。
该功能包还需要相关的依赖yocs_msgs功能包，下载回来放到ROS工作空间的src目录下。

注意：
刚下载功能包下来不能直接进行编译，需要先修改驱动包名字。进入驱动包文件夹打开CmakeLists.txt文件，查看project(xxx)可以知道工程名字，回到src目录把驱动包重命名即可编译。

可以直接使用下面命令进行下载功能包和相关依赖：

git clone https://github.com/yujinrobot/yujin_ocs.git
sudo apt-get install ros-kinetic-ecl-*
git clone https://github.com/yujinrobot/yocs_msgs.git
sudo apt-get install ros-kinetic-ar-track-alvar

编译yocs_virtual_sensor包

这个包需要自己更改CMakeLists.txt文件，不然启动launch文件时候会提示找不到节点，主要更改下面这几句：

## Specify additional locations of header files
include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
 
## Declare a cpp executable
add_executable(virtual_sensor_node src/virtual_sensor_node.cpp)
 
## Add cmake target dependencies of the executable/library
add_dependencies(virtual_sensor_node yocs_msgs_gencpp)
 
## Specify libraries to link a library or executable target against
target_link_libraries(virtual_sensor_node ${catkin_LIBRARIES})
 
#############
## Install ##
#############
 
install(TARGETS virtual_sensor_node
        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

配置standalone.launch文件

需要添加yaml文件进去，还有虚拟墙的节点。

<!--
 Virtual sensor that uses semantic map information to "see" obstacles undetectable by robot sensors.
-->
<launch>
  <arg name="virtual_wall" default="$(find yocs_virtual_sensor)/data/wall_list.yaml"/>
  <node name="virtual_sensor" pkg="yocs_virtual_sensor" type="virtual_sensor_node" >
    <param name="range_min"    value="0.0"/>
    <param name="range_max"    value="6.0"/>
    <param name="frequency"    value="10.0"/>
    <param name="hits_count"   value="3"/>
    <param name="global_frame" value="map"/>
    <param name="sensor_frame" value="laser_frame"/>
  </node>
　　<node pkg="tf" type="static_transform_publisher" name="base_to_virtual" args="0 0 0 0 0 0 /base_footprint /virtual_laser 50"/>
  <node name="wall_publisher" pkg="yocs_virtual_sensor" type="wall_publisher.py" required="true" >
    <param name="~filename" value="$(arg virtual_wall)"/>
  </node>
 
</launch>

修改wall_publisher.py。

在yocs_virtual_sensor功能包scripts目录下有一个wall_publisher.py代码文件，修改下面代码：

# 修改
wall_pub   = rospy.Publisher('wall_pose_list', WallList,    latch = True)
# 为
wall_pub   = rospy.Publisher('wall_pose_list', WallList,    latch = True,queue_size= 10)

配置wall_list.yaml

从上面的launch文件可以看到加载了yocs_virtual_sensor功能包data目录下的wall_list.yaml文件，按照自己的实际情况进行配置即可，下面是我修改的例子，我建的地图比较小，所以相应的坐标数字会比较小：

-  
  name: wall1
  frame_id: map
  length: 1.5
  width:  0.000001
  height: 1.0
  pose:
      position:
          x: 0.8
          y: -0.726
          z: 0.0
      orientation:
          x: 1.0
          y: 1.0
          z: 0.0
          w: 0.0

-  
  name: wall2
  frame_id: map
  length: 0.7
  width:  0.000001
  height: 1.0
  pose:
      position:
          x: 1.3
          y: 0.0
          z: 0.0
      orientation:
          x: 0.0
          y: 1.0
          z: 0.0
          w: 0.0

在costmap.yaml添加虚拟激光

本来一个scan的，现在添加为两个scan。

obstacle_layer:
  enabled: true
  combination_method: 1        				# default 1
  track_unknown_space: true    				# default false
  obstacle_range: 2.5          				# default 2.5   检测障碍物的最大范围
  raytrace_range: 3.0          				# default 3.0   检测自由空间的最大范围
  max_obstacle_height: 0.6     				# default 2.0   描述障碍物的最大高度
  min_obstacle_height: 0.0     				# 描述障碍物的最小高度
  observation_sources: scan virtual_scan    # 代价地图需要关注的传感器信息
  scan: {
    sensor_frame: laser_frame, # 激光雷达传感器的坐标系名称;
    topic: scan_filtered,      # 该激光雷达发布的话题名;
    data_type: LaserScan,      # 激光雷达数据类型;
    marking: true,             # 是否使用该传感器来标记障碍物;
    clearing: true,            # 是否使用该传感器来清除障碍物标记为自由空间;
  }
  virtual_scan: {
    sensor_frame: laser_frame, # 激光雷达传感器的坐标系名称;
    topic: virtual_sensor_scan,      # 该激光雷达发布的话题名;
    data_type: LaserScan,      # 激光雷达数据类型;
    marking: true,             # 是否使用该传感器来标记障碍物;
    clearing: false,           # 是否使用该传感器来清除障碍物标记为自由空间;
  }

编译错误

在我的环境中进行编译的时候，提示rospy_message_converter错误，是因为没有相应的依赖。

解决方法：从ROS wiki上找到相应的功能包下载回来放到ROS工作空间src目录下，重新进行编译。

注意：刚下载下来不能直接编译，需要先修改驱动包名字。进入驱动包文件夹打开CmakeLists.txt文件，查看project(xxx)可以知道工程名字，回到src目录把驱动包重命名后即可编译。

git clone git@github.com:uos/rospy_message_converter.git

运行效果

配置完成后进行编译，然后启动导航包就可以看到效果了。

参考链接:

【1】 Ros导航虚拟墙设置