Velodyne16激光雷达-Cartographer 2D建图 （point_to_scan）

雷达采用架设在tracer_mini机器人上的方式（如果有问题的话。可能是机器人底盘没有传数据，检查can0通信）

首先配置vlp16激光雷达(下载驱动，改连接ip)

GitHub - ros-drivers/velodyne: ROS support for Velodyne 3D LIDARs

 velodyne_pointcloud文件夹中launch内的 VLP16_points.launch文件：

<!-- run velodyne_pointcloud/TransformNodelet in a nodelet manager for a VLP-16 -->

<launch>

  <!-- declare arguments with default values -->
  <arg name="calibration" default="$(find velodyne_pointcloud)/params/VLP16db.yaml"/>
  <arg name="device_ip" default="" />
  <arg name="frame_id" default="velodyne" />
  <arg name="manager" default="$(arg frame_id)_nodelet_manager" />
  <arg name="max_range" default="130.0" />
  <arg name="min_range" default="0.4" />
  <arg name="pcap" default="" />
  <arg name="port" default="2369" />
  <arg name="read_fast" default="false" />
  <arg name="read_once" default="false" />
  <arg name="repeat_delay" default="0.0" />
  <arg name="rpm" default="600.0" />
  <arg name="gps_time" default="false" />
  <arg name="pcap_time" default="false" />
  <arg name="cut_angle" default="-0.01" />
  <arg name="timestamp_first_packet" default="false" />
  <arg name="laserscan_ring" default="-1" />
  <arg name="laserscan_resolution" default="0.007" />
  <arg name="organize_cloud" default="false" />

  <!-- start nodelet manager and driver nodelets -->
  <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
    <arg name="device_ip" value="$(arg device_ip)"/>
    <arg name="frame_id" value="$(arg frame_id)"/>
    <arg name="manager" value="$(arg manager)" />
    <arg name="model" value="VLP16"/>
    <arg name="pcap" value="$(arg pcap)"/>
    <arg name="port" value="$(arg port)"/>
    <arg name="read_fast" value="$(arg read_fast)"/>
    <arg name="read_once" value="$(arg read_once)"/>
    <arg name="repeat_delay" value="$(arg repeat_delay)"/>
    <arg name="rpm" value="$(arg rpm)"/>
    <arg name="gps_time" value="$(arg gps_time)"/>
    <arg name="pcap_time" value="$(arg pcap_time)"/>
    <arg name="cut_angle" value="$(arg cut_angle)"/>
    <arg name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
  </include>

  <!-- start transform nodelet -->
  <include file="$(find velodyne_pointcloud)/launch/transform_nodelet.launch">
    <arg name="model" value="VLP16"/>
    <arg name="calibration" value="$(arg calibration)"/>
    <arg name="manager" value="$(arg manager)" />
    <arg name="fixed_frame" value="" />
    <arg name="target_frame" value="" />
    <arg name="max_range" value="$(arg max_range)"/>
    <arg name="min_range" value="$(arg min_range)"/>
    <arg name="organize_cloud" value="$(arg organize_cloud)"/>
  </include>

  <!-- start laserscan nodelet -->
  <include file="$(find velodyne_pointcloud)/launch/laserscan_nodelet.launch">
    <arg name="manager" value="$(arg manager)" />
    <arg name="ring" value="$(arg laserscan_ring)"/>
    <arg name="resolution" value="$(arg laserscan_resolution)"/>
  </include>

</launch>

 配置完之后

source devel/setup.bash

roslaunch velodyne_pointcloud VLP16_points.launch

 用rviz测试

rosrun rviz rviz -f velodyne

保存录制的bag
rosbag record -a

 接下来配置将3d激光转为2d scan的包

GitHub - ros-perception/pointcloud_to_laserscan: Converts a 3D Point Cloud into a 2D laser scan.

主要修改的是<remap from="cloud_in" to="/velodyne_points"/> 因为velodyne发布的数据是/velodyne_points；

target_frame: velodyne 这里一般使用vlp16的都是velodyne，可以使用以下命令查询，<topic>换成雷达发布的话题，比如vlp16是/velodyne_points。

rostopic echo <topic>  | grep frame_id

这是我的launch文件：

<?xml version="1.0"?>
 
<launch>
    <!--copy from sample_node.launch-->
    <!-- run pointcloud_to_laserscan node -->
    <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
 
        <remap from="cloud_in" to="/velodyne_points"/>
        <rosparam>
            target_frame: velodyne # Leave disabled to output scan in pointcloud frame
            transform_tolerance: 0.01
            min_height: 0.0
            max_height: 1.0
 
            angle_min: -3.1415926 # -M_PI
            angle_max: 3.1415926 # M_PI
            angle_increment: 0.003 # 0.17degree
            scan_time: 0.1
            range_min: 0.2
            range_max: 100
            use_inf: true
            inf_epsilon: 1.0
 
            # Concurrency level, affects number of pointclouds queued for processing and number of threads used
            # 0 : Detect number of cores
            # 1 : Single threaded
            # 2->inf : Parallelism level
            concurrency_level: 1
        </rosparam>
 
    </node>
 
</launch>

 进行测试，可以看到，3d激光成功变成了2d的scan，便于接下来进行2d的cartographer建图。

接下来进行cartographer的配置

在cartographer_ros下的launch，创建关于vlp16的 比如 vlp16.launch（与文件夹内其他文件不重名） 主要是remap from =“scan” to=“/scan” 这里，/scan是point_to_scan转换出来的数据（在前面图里可以看到/velodyne_points是vlp16发布的数据，/scan是转换后的数据），用于2d建图。这里我引用的是velodyne.lua，如果你要使用其他配置，记得修改-configuration_basename velodyne.lua。

<launch>
  <param name="/use_sim_time" value="false" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename velodyne.lua"
      output="screen">
    <remap from="scan" to="/scan" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
//resolution 0.05 这个参数可以修改，主要是建图时rviz中的分辨率
  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />

</launch>

接下来是在configuration_files中的velodyne.lua

我主要修改的是这里， 

tracking_frame = "velodyne",
published_frame = "velodyne",

因为不涉及imu，所以进行的修改比较简单。

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "velodyne",
  published_frame = "velodyne",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1

POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65

return options

 下面进行建图，成功出图(每次修改launch请重新catkin_make 或者 )

vlp16_ws
source devel/setup.bash
roslaunch velodyne_pointcloud VLP16_points.launch

point2scan_ws
source devel/setup.bash
roslaunch pointcloud_to_laserscan point_to_scan.launch

carto_ws
source install_isolated/setup.bash
roslaunch cartographer_ros vlp16.launch

更新，在构建大地图的情况下，Cartographer性能优于Gmapping；

下图为采用Cartographer 2D建图 2000平左右走廊的地图