Add a launch file for 3D SLAM with a depth camera. (#7)

Also other fixes to unbreak TurtleBot 2 configurations.

cartographer_turtlebot2/CMakeLists.txt

@@ -47,6 +47,13 @@ add_executable(flat_world_imu_node src/flat_world_imu_node_main.cc)
 target_link_libraries(flat_world_imu_node ${catkin_LIBRARIES})
 add_dependencies(flat_world_imu_node ${catkin_EXPORTED_TARGETS})
 
+install(TARGETS
+  flat_world_imu_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 install(DIRECTORY launch/
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch/
 )

cartographer_turtlebot2/configuration_files/demo_turtlebot2.rviz

@@ -224,8 +224,8 @@ Visualization Manager:
       Enabled: true
       Map frame: map
       Name: Submaps
-      Submap query service: /cartographer_node/submap_query
-      Topic: /cartographer_node/submap_list
+      Submap query service: /submap_query
+      Topic: /submap_list
       Tracking frame: base_link
       Unreliable: false
       Value: true

cartographer_turtlebot2/configuration_files/turtlebot2.lua → cartographer_turtlebot2/configuration_files/turtlebot2_2d.lua

@@ -18,6 +18,7 @@ options = {
   map_builder = MAP_BUILDER,
   map_frame = "map",
   tracking_frame = "gyro_link",
+  published_frame = "odom",
   odom_frame = "odom",
   provide_odom_frame = false,
   use_odometry_data = true,

cartographer_turtlebot2/configuration_files/turtlebot2_3d.lua

@@ -0,0 +1,52 @@
+-- Copyright 2016 The Cartographer Authors
+--
+-- Licensed under the Apache License, Version 2.0 (the "License");
+-- you may not use this file except in compliance with the License.
+-- You may obtain a copy of the License at
+--
+--      http://www.apache.org/licenses/LICENSE-2.0
+--
+-- Unless required by applicable law or agreed to in writing, software
+-- distributed under the License is distributed on an "AS IS" BASIS,
+-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+-- See the License for the specific language governing permissions and
+-- limitations under the License.
+
+include "map_builder.lua"
+
+options = {
+  map_builder = MAP_BUILDER,
+  map_frame = "map",
+  tracking_frame = "gyro_link",
+  published_frame = "odom",
+  odom_frame = "odom",
+  provide_odom_frame = false,
+  use_odometry_data = true,
+  use_constant_odometry_variance = true,
+  constant_odometry_translational_variance = 1e-7,
+  constant_odometry_rotational_variance = 1e-7,
+  publish_occupancy_grid = false,
+  use_horizontal_laser = false,
+  use_horizontal_multi_echo_laser = false,
+  horizontal_laser_min_range = 0.,
+  horizontal_laser_max_range = 30.,
+  horizontal_laser_missing_echo_ray_length = 5.,
+  num_lasers_3d = 1,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 5e-3,
+}
+
+options.map_builder.use_trajectory_builder_3d = true
+options.map_builder.sparse_pose_graph.optimize_every_n_scans = 320
+options.map_builder.sparse_pose_graph.constraint_builder.sampling_ratio = 0.03
+options.map_builder.sparse_pose_graph.optimization_problem.ceres_solver_options.max_num_iterations = 10
+-- Reuse the coarser 3D voxel filter to speed up the computation of loop closure
+-- constraints.
+options.map_builder.sparse_pose_graph.constraint_builder.adaptive_voxel_filter = TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter
+options.map_builder.sparse_pose_graph.constraint_builder.min_score = 0.62
+options.map_builder.sparse_pose_graph.constraint_builder.log_matches = true
+
+options.map_builder.trajectory_builder_3d.scans_per_accumulation = 1
+
+return options

cartographer_turtlebot2/launch/demo_turtlebot2.launch

@@ -21,7 +21,7 @@
       type="cartographer_node" args="
           -configuration_directory
               $(find cartographer_turtlebot2)/configuration_files
-          -configuration_basename turtlebot2.lua"
+          -configuration_basename turtlebot2_2d.lua"
       output="screen" />
   <node name="flat_world_imu_node" pkg="cartographer_turtlebot2"
       type="flat_world_imu_node" output="screen">

cartographer_turtlebot2/launch/turtlebot2.launch

@@ -15,6 +15,8 @@
 -->
 
 <launch>
+  <arg name="configuration_basename" />
+
   <param name="robot_description"
     textfile="$(find cartographer_turtlebot2)/urdf/turtlebot2.urdf" />
 
@@ -22,8 +24,9 @@
       type="cartographer_node" args="
           -configuration_directory
               $(find cartographer_turtlebot2)/configuration_files
-          -configuration_basename turtlebot2.lua"
-      output="screen" />
+          -configuration_basename $(arg configuration_basename)"
+      output="screen">
+    <remap from="points2" to="/camera/depth/points" />
   </node>
   <node name="flat_world_imu_node" pkg="cartographer_turtlebot2"
       type="flat_world_imu_node" output="screen">

cartographer_turtlebot2/launch/turtlebot2_depth_camera_2d.launch

@@ -23,5 +23,7 @@
     <remap from="image" to="/camera/depth/image_raw" />
   </node>
 
-  <include file="$(find cartographer_turtlebot2)/launch/turtlebot2.launch" />
+  <include file="$(find cartographer_turtlebot2)/launch/turtlebot2.launch">
+    <arg name="configuration_basename" value="turtlebot2_2d.lua" />
+  </include>
 </launch>

cartographer_turtlebot2/launch/turtlebot2_depth_camera_3d.launch

@@ -0,0 +1,26 @@
+<!--
+  Copyright 2016 The Cartographer Authors
+
+  Licensed under the Apache License, Version 2.0 (the "License");
+  you may not use this file except in compliance with the License.
+  You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+  Unless required by applicable law or agreed to in writing, software
+  distributed under the License is distributed on an "AS IS" BASIS,
+  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  See the License for the specific language governing permissions and
+  limitations under the License.
+-->
+
+<launch>
+  <param name="/camera/driver/data_skip" value="2" />
+
+  <include file="$(find turtlebot_bringup)/launch/minimal.launch" />
+  <include file="$(find openni2_launch)/launch/openni2.launch" />
+
+  <include file="$(find cartographer_turtlebot2)/launch/turtlebot2.launch">
+    <arg name="configuration_basename" value="turtlebot2_3d.lua" />
+  </include>
+</launch>

cartographer_turtlebot2/launch/turtlebot2_urg_lidar_2d.launch

@@ -20,5 +20,7 @@
   <param name="urg_node/frame_id" value="horizontal_laser_link" />
   <node name="urg_node" pkg="urg_node" type="urg_node" />
 
-  <include file="$(find cartographer_turtlebot2)/launch/turtlebot2.launch" />
+  <include file="$(find cartographer_turtlebot2)/launch/turtlebot2.launch">
+    <arg name="configuration_basename" value="turtlebot2_2d.lua" />
+  </include>
 </launch>