Feed Cartographer_turtlebot with own bag

Cartographer官网和Cartographer_ROS的demo有指导怎么修改launch和lua文件来运行自己的bag。

但是它们没提到有更简单的Cartographer_turtlebot直接提供了基于Turtlebot相关的launch和lua。

并且可以参考demo bag文件，修改自己的bag文件。

其中cartographer_turtlebot_demo.bag info如下

因为没有用到相机信息，所以我们record一份只包含/dom /tf /scan /imu的bag出来。

 

试着运行一下这个demo bag，检查一下rosnode list

需要注意的是，flat_world_imu_node会订阅/mobile_base/sensors/imu_data_raw话题，处理raw imu data，然后发布在/imu话题上。

cartographer_node节点订阅的是该上述节点处理过后的imu信息。

如果不使用falt_world_imu_node矫正的信息，构图过程中可以观察到机器人pose上下颠倒，点云被错误投影。

 

另外注意，在echo /imu 话题的时候，发现这个信息的frame_id是"gryo_link"，

而我的bag中imu的frame_id是

 

根据上述观察，相应的修改

trutlebot_urg_lidar_2d.lua 最重要的就是frame id，最好是能ros_tf_tree查看一下demo的tree和自己的tree检查各个frame_id和关系

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_footprint",    --change from gyro_link to base_footprint
  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

对应修改demo_liar_2d.launch 这里主要是各个node各自sub和pub的话题名字要根据bag的话题名字相应的修改

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

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory
              $(find cartographer_turtlebot)/configuration_files
          -configuration_basename my_turtlebot_urg_lidar_2d.lua"
      output="screen">
    <remap from="imu" to="/imu_out" /> --default sub to /imu topic, change to /imu_out
  </node>

  <node name="flat_world_imu_node" pkg="cartographer_turtlebot"
      type="cartographer_flat_world_imu_node" output="screen">
    <remap from="imu_in" to="/imu" /> --default input is /imu_in, output is /imu_out
  </node>

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_turtlebot
          )/configuration_files/demo_turtlebot.rviz" />
  <node name="playbag" pkg="rosbag" type="play"
      args="--clock $(arg bag_filename)" />

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

 

 

最后拿模拟环境采集的数据建图完成。

 

 

______BTW_________________________________________________________________________________

这还顺便解决了一个BUG。

之所以我找到cartographer_turtlebot，是因为先跑gazebo虚拟环境的

roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=cartographer

会报Check failed: sensor_to_tracking->translation().norm()

但是我跑真实环境没有问题。但是我需要cartographer在虚拟环境做实验。

就尝试不通过turtlebot，而是直接给cartographer喂bag。

然后一路找到cartographer_turtlebot，像上述一样修改launch和lua的时候复现了这个failed。

于是我知道两边是同一个问题，就是配置文件里面哪里不对。

今天打开turtlebot3_lds_2d.lua，果然。

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug)
  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 = 0.1,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 0.1,
  landmarks_sampling_ratio = 1.,
}

作者直接留下一句，用gazebo要改frame因为有bug。

 

现在不管是

roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=cartographer

还是

roslaunch cartographer_turtlebot demo_lidar_2d.launch bag_filename:=mybag

都能执行了，用的都是同一批launch和lua，触类旁通了。