cartographer使用

1.cartographer开启 launch文件

  <launch>
  <arg name="configuration_directory" default="$(find cartographer_launch)/configuration_files"/> 
  <arg name="configuration_basename" default="turtlebot_urg_lidar_2d.lua"/>
  <arg name="imu_in_topic" default="/mobile_base/sensors/imu_data_raw"/>
  <arg name="imu_transform" default="true"/>
  <arg name="carto_odom_topic" default="wheel_odom"/>


  <node name="cartographer_node" pkg="cartographer_ros"
    type="cartographer_node" args="
        -configuration_directory $(arg configuration_directory)
        -configuration_basename $(arg configuration_basename)"
    output="log">
    <remap from="/odom" to="$(arg carto_odom_topic)"/>
  </node>


  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />


  <group if="$(arg imu_transform)">
      <node name="flat_world_imu_node" pkg="cartographer_turtlebot"
        type="cartographer_flat_world_imu_node" output="screen">
          <remap from="imu_in" to="$(arg imu_in_topic)" />
          <remap from="imu_out" to="/imu" />
      </node>
  </group>
  </launch>

2.turtlebot_urg_lidar_2d.lua文件

include "map_builder.lua"
  include "trajectory_builder.lua"
  options = {
    map_builder = MAP_BUILDER,
    trajectory_builder = TRAJECTORY_BUILDER,
    map_frame = "map",
    tracking_frame = "gyro_link",
    published_frame = "odom",
    odom_frame = "odom",
    provide_odom_frame = false,
    publish_frame_projected_to_2d = false,
    use_odometry = true,
    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.min_range = 0.1
  TRAJECTORY_BUILDER_2D.max_range = 8.
  TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
  TRAJECTORY_BUILDER_2D.use_imu_data = true
  TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
  TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)
  POSE_GRAPH.constraint_builder.min_score = 0.65
  POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7
  return options

  3.cartographer保存地图服务

   rosservice call /write_state "filename: ''"    #.pbstream
   
  4.cartographer加载地图 launch 文件
 

<launch>
    <arg name = "configuration_directory" default = "$(find cartographer_launch)/configuration_files"/>
  <arg name = "configuration_basename" default = "turtlebot_urg_lidar_2d_localization.lua"/>
  <arg name = "load_state_filename" default = "/home/mayun/self_developed_robot/src/slam/cartographer_launch/map_save.pbstream"/>


  <arg name="imu_in_topic" default="/mobile_base/sensors/imu_data_raw"/>
  <arg name="imu_transform" default="true"/>
  <arg name="carto_odom_topic" default="wheel_odom"/>
  <arg name="load_frozen_state" default="true"/>


  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(arg configuration_directory)
          -configuration_basename $(arg configuration_basename)
          -load_state_filename $(arg load_state_filename)
          -load_frozen_state $(arg load_frozen_state)"
      output="screen">
    <remap from="/odom" to="$(arg carto_odom_topic)"/>
  </node>


    <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
        type="cartographer_occupancy_grid_node" args="-resolution 0.05" />




  <group if="$(arg imu_transform)">
      <node name="flat_world_imu_node" pkg="cartographer_turtlebot"
        type="cartographer_flat_world_imu_node" output="screen">
          <remap from="imu_in" to="$(arg imu_in_topic)" />
          <remap from="imu_out" to="/imu" />
      </node>
  </group>
  </launch> 

  5.加载地图所需 turtlebot_urg_lidar_2d_localization.lua

 include "turtlebot_urg_lidar_2d.lua"
  TRAJECTORY_BUILDER.pure_localization = true
  POSE_GRAPH.optimize_every_n_nodes = 20
  return options

 6.轨迹停止时服务 

•    rosservice call /finish_trajectory "trajectory_id: 0"

  7.轨迹开启时launch文件

   <launch>
      <arg name = "configuration_directory" default = "$(find cartographer_launch)/configuration_files"/>
      <arg name = "configuration_basename" default = "backpack_2d.lua"/>
      <node name="cartographer_start_trajectory" pkg="cartographer_ros"
          type="cartographer_start_trajectory" args="
              -configuration_directory $(arg configuration_directory)
              -configuration_basename $(arg configuration_basename)
              -initial_pose '{to_trajectory_id = 0, relative_pose =
              { translation = { -15.66,-4.4885, 0. }, rotation = { w=0.97856615,x=0.070, y=-0.0573,z=-0.185  } } }'"
          output="screen">
      </node>
   </launch>

8. urdf 加载 传感器外参

launch中：

 <param name="robot_description" textfile="$(find carto_test)/launch/backpack_2d.urdf" />
 <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

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="horizontal_laser_link">
    <visual>
      <origin xyz="0 0 0" />
      <geometry>
        <cylinder length="0.05" radius="0.03" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

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

  <link name="base_link" />

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

  <joint name="horizontal_laser_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="horizontal_laser_link" />
    <origin xyz="0.1007 0 0.0558" />
  </joint>

  <joint name="vertical_laser_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="vertical_laser_link" />
    <origin rpy="0 -1.570796 3.141593" xyz="0.1007 0 0.1814" />
  </joint>
</robot>