室内四足机器人SLAM+导航（cartographer2D+move_base）

室内四足机器人SLAM+导航（cartographer2D+move_base）

cartographer环境安装和使用

安装流程这里省略，直接百度一下安装教程即可，安装完成后进入src中的cartographer_ros包，我们主要修改launch，cartographer_files和urdf。如果机器人的tf的发布已经有了，则不需要写urdf模型。我用的传感器配置有velodyne16线激光雷达和imu(狗提供)。

cartographer建图

urdf模型

需要将激光雷达，imu和狗的base_link连接在一起，直接上代码：
my_demo.urdf

<robot name="cartographer_backpack_2d">
  <material name="orange">
    <color rgba="1.0 0.5 0.2 1" />
  </material>
  <material name="gray">
    <color rgba="0.2 0.2 0.2 1" />
  </material>

  <link name="imu_link">
    <visual>
      <origin xyz="0 0 0" />
      <geometry>
        <box size="0.06 0.04 0.02" />
      </geometry>
      <material name="orange" />
    </visual>
  </link>

  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" />
      <geometry>
        <box size="0.6 0.52 0.4" />
      </geometry>
	<origin rpy="0 0 0" xyz="0 0 .2"/>
      <material name="blue" >
		<color rgba="0 1 .8 0.5"/>
	</material>
    </visual>
  </link>

  <link name="velodyne">
    <visual>
      <origin xyz="0 0 0" />
      <geometry>
        <cylinder length="0.05" radius="0.03" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

  <joint name="imu_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="imu_link" />

    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>

  <joint name="laser_joint" type="fixed">
    <parent link="base_link" />
    <child link="velodyne" />
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>

</robot>

要注意的是激光雷达和imu的link名字要和ROS版本驱动发布的link名一样，否则获取不到传感器数据。

lua文件修改

这里面参数比较多，小白建议直接复制一个lua文件然后修改参数。我这里修改的是backpack_2d.lua。代码如下：

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  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.num_accumulated_range_data = 1

return options

这里提供了imu和雷达，注意这两处修改tracking_frame和published_frame。

launch文件修改

话不多说，直接上代码

<launch>
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/my_demo.urdf" />

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />

  <include file="$(find velodyne_pointcloud)/launch/TM16.launch"/>
<node name="ros_control" pkg="a2_ros2udp" type="ros_control" />

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

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
<!-- <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d /home/unitree/build_map2.rviz" />-->
</launch>

这里解释一下，robot_state_publisher结点用于发布机器人数据。接着是启动雷达发布雷达数据，启动机器人控制结点，主要用于发布imu数据，cartographer_node可以理解成定位跟建图，cartographer_occupancy_grid_node则是将建好的地图（此时地图在sub_list话题中）转成ROS自带地图格式发布到/map话题中，此时如果直接用map_save保存pgm的话是没有障碍物显示的。
因此如果想获取完整的pgm地图则需要先保存为pbstream格式的文件，这个文件在后续导航中纯定位中也要用到，然后用cartographer_node中自带的一个结点可以将pbstream直接转成pgm和对应的yaml文件。此时整个建图流程已经完成。

cartographer纯定位＋导航

lua文件修改

纯定位模式下的lua和建图是的lua是一样的，直接上代码：

include "my_demo.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3,
}

POSE_GRAPH.optimize_every_n_nodes = 20


return options

这里的include的lua和建图的lua是一模一样的。

launch文件修改

launch文件和建图是的launch文件很相似，上代码：

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

  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/my_demo.urdf" />

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />

<include file="$(find velodyne_pointcloud)/launch/TM16.launch"/>
<node name="ros_control" pkg="a2_ros2udp" type="ros_control" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename backpack_2d_localization.lua
          -load_state_filename /home/unitree/Cartographer_map/temp.pbstream"
      output="screen">
    <remap from="scan" to="/scan" />
	<remap from="imu" to="/imu_raw" />
  </node>
 <node pkg="start" type="local_map_tf_publisher" name="local_map_tf_publisher"/>
 <include file="$(find navigation)/launch/navigation.launch">
        <arg name="map_file" value="/home/unitree/Cartographer_map/temp_pgm/temp.yaml"/>
        <arg name="odom_topic" value="/odom"/>
    </include>

 
 <!-- <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />-->
 
</launch>

导航需要用到/odom里程的发布，但是cartographer默认是不发布/odmo，因此需要修改源码，修改方法参考https://wenku.baidu.com/view/eec49e5e1411cc7931b765ce05087632311274a8.html。建图和纯定位区别就是删掉了cartographer_occupancy_grid_node，这样就不能发布/map了，因为cartographer_occupancy_grid_node发布的地图是动态的，当存在低矮的障碍物时，地图中就不能扫描出来，若使用静态的/map就可以人为的画出障碍物即可。cartographer_occupancy_grid_node删掉后，我们用map_server发布静态地图即可。此时的/map和sub_list地图在坐标上时对应的上的。

16线转单线

因为单线雷达扫描不到低矮的障碍物，因此在建图时就无法将不可忽略的障碍物建如map中，我使用pointcloud_to_laserscan这个包可以将16线的雷达数据映射成/scan数据，这样有两个好处：增加了雷达数据量和能够扫到雷达水平面以下和以上的障碍物