ROS32:cartgrapher(2D)のデータを保存、再生する。

前提

メモです。
ROS31を実行したあとが前提です。

調べる。

Cartgrapherのデモを調べる。

$ rosbag info cartographer_paper_deutsches_museum.bag

path:         cartographer_paper_deutsches_museum.bag
version:      2.0
duration:     31:52s (1912s)
start:        May 26 2015 22:30:16.48 (1432647016.48)
end:          May 26 2015 23:02:09.46 (1432648929.46)
size:         470.5 MB
messages:     617965
compression:  bz2 [3334/3334 chunks; 18.31%]
uncompressed:   2.5 GB @   1.3 MB/s
compressed:   462.3 MB @ 247.5 KB/s (18.31%)
types:        sensor_msgs/Imu                [6a62c6daae103f4ff57a132d6f95cec2]
              sensor_msgs/MultiEchoLaserScan [6fefb0c6da89d7c8abe4b339f5c2f8fb]
topics:       horizontal_laser_2d    70358 msgs    : sensor_msgs/MultiEchoLaserScan
              imu                   478244 msgs    : sensor_msgs/Imu               
              vertical_laser_2d      69363 msgs    : sensor_msgs/MultiEchoLaserScan

ついでに３Dの方も

$ rosbag info b3-2016-04-05-14-14-00.bag 
path:         b3-2016-04-05-14-14-00.bag
version:      2.0
duration:     20:21s (1221s)
start:        Apr 05 2016 23:14:00.74 (1459865640.74)
end:          Apr 05 2016 23:34:21.97 (1459866861.97)
size:         8.4 GB
messages:     3986178
compression:  lz4 [29447/29447 chunks; 38.65%]
uncompressed: 21.7 GB @ 18.2 MB/s
compressed:    8.4 GB @  7.0 MB/s (38.65%)
types:        sensor_msgs/Imu         [6a62c6daae103f4ff57a132d6f95cec2]
              sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181]
topics:       horizontal_laser_3d   1840435 msgs    : sensor_msgs/PointCloud2
              imu                    305308 msgs    : sensor_msgs/Imu        
              vertical_laser_3d     1840435 msgs    : sensor_msgs/PointCloud2

してみよう

ほとんど変わらないのでコピーを作成して編集します。

$ cd ~/catkin_ws/src/cartographer_example/launch
$ cp urg.launch urg_2d_rec.launch
$ cp urg.launch urg_2d_play.launch

urg_2d_rec.launch

<launch>
  <node name="horizontal_laser" pkg="urg_node"
    type="urg_node" >
    <param name="ip_address" value="192.168.1.31" />
    <param name="frame_id" value="horizontal_laser_link" />
    <!-- remap from="scan" to="horizontal_laser_2d" / -->
  </node>

  <!-- node name="vertical_laser" pkg="urg_node"
    type="urg_node" >
    <param name="serial_port" value="/dev/ttyACM1" />
    <param name="frame_id" value="vertical_laser_link" />
    <remap from="scan" to="vertical_laser_2d" />
  </node -->

  <include file="$(find razor_imu_9dof)/launch/razor-pub.launch" />

  <node pkg="tf" type="static_transform_publisher" name="imu_link_connect" args="0 0 0 0 0 0 /imu_link /base_imu_link 100"/>

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

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

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

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

  <node pkg="rosbag" type="record" name="rosbag_record"
       args=" -O /tmp/cartographer_2d.bag scan imu"/>

</launch>

urg_2d_play.launch

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

  <node pkg="rosbag" type="play" name="rosbag_play"
       args="--clock $(arg bag_filename)" />

  <!--node name="horizontal_laser" pkg="urg_node"
    type="urg_node" >
    <param name="ip_address" value="192.168.1.31" />
    <param name="frame_id" value="horizontal_laser_link" />
    <remap from="scan" to="horizontal_laser_2d" />
  </node -->

  <!-- node name="vertical_laser" pkg="urg_node"
    type="urg_node" >
    <param name="serial_port" value="/dev/ttyACM1" />
    <param name="frame_id" value="vertical_laser_link" />
    <remap from="scan" to="vertical_laser_2d" />
  </node -->

  <!--include file="$(find razor_imu_9dof)/launch/razor-pub.launch" /-->

  <node pkg="tf" type="static_transform_publisher" name="imu_link_connect" args="0 0 0 0 0 0 /imu_link /base_imu_link 100"/>

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

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

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

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

</launch>

編集が終わったら $ catkin_make_isolated --install --use-ninja を忘れないように。

実行

保存は

roslaunch cartographer_example urg_2d_rec.launch 

再生は、

$ roslaunch cartographer_example urg_2d_play.launch bag_filename:=/tmp/cartographer_2d.bag 

ですね。