PointCloud2插入costmap的仿真实现

首先我们先启动所需要的节点

1. 仿真的节点
   在创客智造的Turtlebot与仿真教程中，我们通过安装初始的仿真包，可以使用
   "roslaunch turtlebot_gazebo turtlebot_world.launch"
   来启动仿真环境；这个节点输出的有

/camera/depth/camera_info	/camera/depth/image_raw	/camera/depth/points
/camera/rgb/camera_info	/camera/rgb/image_raw	/camera/rgb/image_raw/compressed
/camera/rgb/image_raw/compressedDepth
/clock		/depthimage_to_laserscan/parameter_descriptions	/depthimage_to_laserscan/parameter_updates
/laserscan_nodelet_manager/bond
/map	/map_metadata	/map_updates
/mobile_base/sensors/imu_data
/odom
/scan

2. 定位与导航的节点
   通过roslaunch turtlebot_gazebo amcl_demo.launch map_file:=/home/asber/map/simmap.yaml
   来启动定位与导航，后面的/home/asber/map/simmap.yaml是我先前通过仿真程序中深度图转换得到的/scan数据(node pkg=“nodelet” type=“nodelet” name=“depthimage_to_laserscan”—来自turtlebot_world.launch文件)，通过gmapping得到的地图。这个launch最后还是调用了turtlebot_navigtion/launch/include下面的move_base.xml来实现的导航。
3. 可视化
   最后通过
   “roslaunch turtlebot_rviz_launchers view_navigation.launch”
   可以可视化的看到navigtion的情况。
   所以我们的任务就是添加一层的plugin层来让navigtion的时候可以使用到pointcloud的数据。

首次尝试–失败，只改param文件夹中参数

obstacle_layer其实之前听说是按照kinect的配置，所以是可以接受三种数据的，pointcloud/pcl2/laserscan，所以在直接在costmap_common_params.yaml里面加了

  observation_sources:  scan point_cloud
  scan:
    data_type: LaserScan
    topic: "/scan"
    marking: true
    clearing: true
    expected_update_rate: 0
  point_cloud:
    data_type: PointCloud2
    topic: /camera/depth/points
    expected_update_rate: 0
    observation_persistence: 0.0
    marking: true
    clearing: true
    inf_is_valid: true

报错如下

一开始先出现警告
[ WARN] [1579691920.571201043, 15.210000000]: The /camera/depth/points observation buffer has not been updated for 1.34 seconds, and it should be updated every 1.00 seconds.
然后就一直循环报错
[ERROR] [1579690438.373010634, 80.270000000]: TF Exception that should never happen for sensor frame: base_footprint, cloud frame: camera_depth_optical_frame, Lookup would require extrapolation into the past.  Requested time 68.400000000 but the earliest data is at time 71.140000000, when looking up transform from frame [base_footprint] to frame [odom]
[ERROR] [1579690438.373259362, 80.270000000]: TF Exception that should never happen for sensor frame: , cloud frame: camera_depth_frame, Lookup would require extrapolation into the past.  Requested time 68.400000000 but the earliest data is at time 70.270000000, when looking up transform from frame [camera_depth_frame] to frame [map]
[ERROR] [1579690438.373413210, 80.270000000]: TF Exception that should never happen for sensor frame: , cloud frame: camera_depth_frame, Lookup would require extrapolation into the past.  Requested time 68.400000000 but the earliest data is at time 71.140000000, when looking up transform from frame [camera_depth_frame] to frame [odom]
[ WARN] [1579690438.636721822, 80.500000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.2700 seconds

一开始我怀疑是tf没有给但是后面rosrun rqt_tf_tree rqt_tf_tree
发现其实amcl都有给出和odom的关系，上面的提示应该是因为CPU高负荷运作，导致tf的时间都超出了，这里有一个相似的wikiROS提问
然后我自己发送了一个pcl2的topic，经过测试可以很好的进行导航避障。所以大体上可以确定:
PCL2的真实数据是在是CPU高消耗，obstacle layer无法实时的进行处理，可以使用一下两种方案
1.点云数据转激光数据(有一个包叫depthimage_to_laserscan)，来减少信息量。(仿真的点云转激光貌似只是平面转换，没有进行投影映射)
2.如果高端一点就对分割出来的特定物体进行边缘提取，然后作为少量的pointcloud2数据传入
3.限制点云数据的分辨率(在gazebo中没有找到分辨率的设置点，但是zed warper中 params的common.yaml文件中可以通过修改 resolution: 2 # ‘0’: HD2K, ‘1’: HD1080, ‘2’: HD720, ‘3’: VGA)来改变分辨率)
其实在运行ZED程序的时候除非分辨率很高HD2K 不然其实并不是特别卡钝。

不过发现了一个融合激光和点云数据的node
和一个高效率低CPU消耗的voxel图 (虽然点云数据输出就已经占据很大的数据量、cpu量)

---

网上的教程

range_sensor_layer 的写法
关于range_sensor_layer的介绍

social_navigation_layers
social_navigation_layers的github代码
social_navigation_layers的wiki介绍
csdn的social_navigation_layers介绍

ROS与navigation教程-obstacle层
csdn的obstacle介绍

通过查看别人的博客可以知道，如果要自己定义一层plugin层，有两个步骤

1. 首先需要编写plugin层的pkg，cpp h costmap_plugin.xml文件等
2. 需要向movebase中的yaml文件声明，让其找到编译后的.so(在devel/lib中的)文件后，加载plugin层

obstacle和voxel层包含了 PointClouds 或者 LaserScans 形式的传感器消息。对于observe_sources参数列表中列出的每个“PointCloud”源，其信息用于更新代价地图。也就说，我到时候只要在obstacle layer加上点云数据就可以了。

---

靠谱的中文教材!

1. ROS探索总结
2. realsense R200转成costmap_2d:待仿真验证，但是现在明白了，现在有一下这样的方案
3. ROS Navigation-----costmap_2d之创建自定义用户层

1.直接将三维稠密地图传给静态的全局costmap
2.将得到的点云数据传给local planner进行规划
3.将得到的点云数据利用depthimage_to_laserscan模拟成激光数据传输到localplanner

靠谱的英文资料

1. 3D pointcloud to 2d costmap layer projection github repo
2. Empty Costmap using PointCloud2

---

WIKI关于添加一层新layer的翻译

如何创建从脚本里面创建一层costmap?
此教程指导你去创建一个自定义的costmap，使用costmap_2d节点执行，尽管参数可以被移植到movebase中
1.建立一个TF变换
costmap需要的最基础的东西就是从costmap到机器人的坐标变换，所以我们需要在launch文件中输出这个变换

<node name="static_tf0" pkg="tf" type="static_transform_publisher" args="2 0 0 0 0 0 /map /base_link 100"/>

这里的创建TF变换我表示有点怀疑因为在其他教程中貌似没有这一步

2.基本参数
接下来制定costmap的行为，我们创建一个yaml文件，这个yaml文件可以被加载到参数空间里，下面是最简单的yaml文件(文件名:minimal.yaml)

plugins: []
publish_frequency: 1.0
footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]

我们一开始为了简便没有创建任何layer，为了调试方便，发布频率publish_frequency是1hz(因为costmap希望footprint被定义所以我们设定一个简单的footprint)
参数文件可以以以下的方式被加载

 <rosparam file="$(find simple_costmap_configuration)/params/minimal.yaml" command="load" ns="/costmap_node/costmap" />

注意，如果你没有使用这些plugins的参数，会默认使用pre-heydro版本的参数，并且会自动根据之前的costmap创建layers。

3.制定plugins
为了真正的指定我们的layer，我们需要将对应的字典存储在对应的plugins队列中，比如

plugins: 
    - {name: static_map,       type: "costmap_2d::StaticLayer"}

并且你需要有静态地图的发布者，比如launch文件中需要有如下的代码

    <node name="map_server" pkg="map_server" type="map_server" args="$(find simple_costmap_configuration)/realmap.yaml"/>

如果还有其他的layer可以继续添加，比如

plugins: 
    - {name: static_map,       type: "costmap_2d::StaticLayer"}
    - {name: obstacles,        type: "costmap_2d::VoxelLayer"}

每指定一个layer，都可以制定layer的namespace，所以制定障碍物层我们可以这样增加我们的参数文件

plugins: 
    - {name: static_map,       type: "costmap_2d::StaticLayer"}
    - {name: obstacles,        type: "costmap_2d::VoxelLayer"}
publish_frequency: 1.0
obstacles:
    observation_sources: base_scan
    base_scan: {data_type: LaserScan, sensor_frame: /base_laser_link, clearing: true, marking: true, topic: /base_scan}