Tb2 repos refactor

depth_to_pointcloud/CMakeLists.txt

@@ -29,7 +29,7 @@ ament_target_dependencies(depth_to_pointcloud_node
 )
 
 install(TARGETS depth_to_pointcloud_node
-  DESTINATION ${CMAKE_INSTALL_PREFIX}/bin/
+  DESTINATION lib/${PROJECT_NAME}
 )
 
 if(BUILD_TESTING)

turtlebot2_cartographer/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(turtlebot2_cartographer)
+
+find_package(ament_cmake REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+install(
+  DIRECTORY configuration_files launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

turtlebot2_cartographer/configuration_files/turtlebot_2d.lua

@@ -0,0 +1,53 @@
+-- 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"
+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,
+  use_odometry = true,
+  use_laser_scan = true,
+  use_multi_echo_laser_scan = false,
+  num_point_clouds = 0,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 5e-3,
+}
+
+MAP_BUILDER.use_trajectory_builder_2d = true
+
+TRAJECTORY_BUILDER_2D.min_range = 0.1
+TRAJECTORY_BUILDER_2D.max_range = 4.0
+TRAJECTORY_BUILDER_2D.missing_data_ray_length = 2.0
+TRAJECTORY_BUILDER_2D.use_imu_data = true
+TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.3
+TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.2
+TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(3.)
+TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 70
+TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 300
+
+TRAJECTORY_BUILDER_2D.submaps.resolution = 0.035
+TRAJECTORY_BUILDER_2D.submaps.num_range_data = 120
+SPARSE_POSE_GRAPH.optimize_every_n_scans = 120
+SPARSE_POSE_GRAPH.constraint_builder.min_score = 0.82
+SPARSE_POSE_GRAPH.constraint_builder.sampling_ratio = 1.
+
+return options

turtlebot2_cartographer/configuration_files/turtlebot_3d.lua

@@ -0,0 +1,71 @@
+-- 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"
+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,
+  use_odometry = true,
+  use_laser_scan = false,
+  use_multi_echo_laser_scan = false,
+  num_point_clouds = 1,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 5e-3,
+}
+
+MAP_BUILDER.use_trajectory_builder_3d = true
+MAP_BUILDER.num_background_threads = 4
+
+MAX_3D_LASER_RANGE = 3.5
+TRAJECTORY_BUILDER_3D.min_range = 0.1
+TRAJECTORY_BUILDER_3D.max_range = MAX_3D_LASER_RANGE
+TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter.max_range = MAX_3D_LASER_RANGE
+TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter.min_num_points = 500
+TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter.max_range = MAX_3D_LASER_RANGE
+TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter.min_num_points = 250
+TRAJECTORY_BUILDER_3D.submaps.high_resolution_max_range = MAX_3D_LASER_RANGE
+
+TRAJECTORY_BUILDER_3D.motion_filter.max_time_seconds = 0.25
+TRAJECTORY_BUILDER_3D.motion_filter.max_angle_radians = math.rad(0.1)
+TRAJECTORY_BUILDER_3D.submaps.high_resolution = 0.035
+TRAJECTORY_BUILDER_3D.submaps.low_resolution = 0.2
+
+TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight_0 = 10.
+TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight_1 = 15.
+TRAJECTORY_BUILDER_3D.ceres_scan_matcher.translation_weight = 4.
+TRAJECTORY_BUILDER_3D.ceres_scan_matcher.rotation_weight = 1e3
+TRAJECTORY_BUILDER_3D.ceres_scan_matcher.only_optimize_yaw = true
+TRAJECTORY_BUILDER_3D.kalman_local_trajectory_builder.scan_matcher_variance = 1e-8
+
+SPARSE_POSE_GRAPH.constraint_builder.sampling_ratio = 0.2
+SPARSE_POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
+SPARSE_POSE_GRAPH.constraint_builder.adaptive_voxel_filter = TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter
+SPARSE_POSE_GRAPH.constraint_builder.min_score = 0.48
+SPARSE_POSE_GRAPH.constraint_builder.log_matches = true
+SPARSE_POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.min_rotational_score = 0.
+SPARSE_POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_xy_search_window = 2.
+
+SPARSE_POSE_GRAPH.optimization_problem.huber_scale = 1e1
+SPARSE_POSE_GRAPH.optimization_problem.acceleration_weight = 1e-1
+SPARSE_POSE_GRAPH.optimization_problem.rotation_weight = 1e3
+
+return options

turtlebot2_drivers/launch/turtlebot_carto_2d.py → turtlebot2_cartographer/launch/turtlebot_carto_2d.py

@@ -16,31 +16,41 @@
 
 from ament_index_python.packages import get_package_share_directory
 from launch.exit_handler import restart_exit_handler
+from ros2run.api import get_executable_path
 
 
 def launch(launch_descriptor, argv):
     ld = launch_descriptor
+    package = 'turtlebot2_drivers'
     ld.add_process(
-        cmd=['kobuki_node'],
+        cmd=[get_executable_path(package_name=package, executable_name='kobuki_node')],
         name='kobuki_node',
         exit_handler=restart_exit_handler,
     )
+    package = 'astra_camera'
     ld.add_process(
-        cmd=['astra_camera_node', '-dw', '320', '-dh', '240', '-C', '-I'],
+        cmd=[
+            get_executable_path(package_name=package, executable_name='astra_camera_node'),
+            '-dw', '320', '-dh', '240', '-C', '-I'],
         name='astra_camera_node',
         exit_handler=restart_exit_handler,
     )
+    package = 'depthimage_to_laserscan'
     ld.add_process(
-        cmd=['depthimage_to_laserscan_node'],
+        cmd=[
+            get_executable_path(
+                package_name=package, executable_name='depthimage_to_laserscan_node')],
         name='depthimage_to_laserscan_node',
         exit_handler=restart_exit_handler,
     )
+    package = 'tf2_ros'
     ld.add_process(
         # The XYZ/Quat numbers for base_link -> camera_rgb_frame are taken from the
         # turtlebot URDF in
         # https://github.com/turtlebot/turtlebot/blob/931d045/turtlebot_description/urdf/sensors/astra.urdf.xacro
         cmd=[
-            'static_transform_publisher',
+            get_executable_path(
+                package_name=package, executable_name='static_transform_publisher'),
             '-0.087', '-0.0125', '0.287',
             '0', '0', '0', '1',
             'base_link',
@@ -49,12 +59,14 @@ def launch(launch_descriptor, argv):
         name='static_tf_pub_base_rgb',
         exit_handler=restart_exit_handler,
     )
+    package = 'tf2_ros'
     ld.add_process(
         # The XYZ/Quat numbers for camera_rgb_frame -> camera_depth_frame are taken from the
         # turtlebot URDF in
         # https://github.com/turtlebot/turtlebot/blob/931d045/turtlebot_description/urdf/sensors/astra.urdf.xacro
         cmd=[
-            'static_transform_publisher',
+            get_executable_path(
+                package_name=package, executable_name='static_transform_publisher'),
             '0', '0.0250', '0',
             '0', '0', '0', '1',
             'camera_rgb_frame',
@@ -63,24 +75,29 @@ def launch(launch_descriptor, argv):
         name='static_tf_pub_rgb_depth',
         exit_handler=restart_exit_handler,
     )
+    package = 'joy'
     ld.add_process(
-        cmd=['joy_node'],
+        cmd=[get_executable_path(package_name=package, executable_name='joy_node')],
         name='joy_node',
         exit_handler=restart_exit_handler,
     )
+    package = 'teleop_twist_joy'
     ld.add_process(
-        cmd=['teleop_node'],
+        cmd=[get_executable_path(package_name=package, executable_name='teleop_node')],
         name='teleop_node',
         exit_handler=restart_exit_handler,
     )
-    cartographer_ros_prefix = get_package_share_directory('cartographer_ros')
-    cartographer_config_dir = os.path.join(cartographer_ros_prefix, 'configuration_files')
+    package = 'cartographer_ros'
+    turtlebot2_cartographer_prefix = get_package_share_directory('turtlebot2_cartographer')
+    cartographer_config_dir = os.path.join(turtlebot2_cartographer_prefix, 'configuration_files')
     ld.add_process(
         cmd=[
-            'cartographer_node',
+            get_executable_path(package_name=package, executable_name='cartographer_node'),
             '-configuration_directory', cartographer_config_dir,
             '-configuration_basename', 'turtlebot_2d.lua'
         ],
         name='cartographer_node',
         exit_handler=restart_exit_handler,
     )
+
+    return ld

turtlebot2_drivers/launch/turtlebot_carto_3d.py → turtlebot2_cartographer/launch/turtlebot_carto_3d.py

@@ -16,44 +16,56 @@
 
 from ament_index_python.packages import get_package_share_directory
 from launch.exit_handler import restart_exit_handler
+from ros2run.api import get_executable_path
 
 
 def launch(launch_descriptor, argv):
     ld = launch_descriptor
+    package = 'turtlebot2_drivers'
     ld.add_process(
-        cmd=['kobuki_node'],
+        cmd=[get_executable_path(package_name=package, executable_name='kobuki_node')],
         name='kobuki_node',
         exit_handler=restart_exit_handler,
     )
+    package = 'astra_camera'
     ld.add_process(
-        cmd=['astra_camera_node', '-dw', '320', '-dh', '240', '-C', '-I'],
+        cmd=[
+            get_executable_path(package_name=package, executable_name='astra_camera_node'),
+            '-dw', '320', '-dh', '240', '-C', '-I'],
         name='astra_camera_node',
         exit_handler=restart_exit_handler,
     )
+    package = 'depth_to_pointcloud'
     ld.add_process(
-        cmd=['depth_to_pointcloud_node'],
+        cmd=[
+            get_executable_path(package_name=package, executable_name='depth_to_pointcloud_node')],
         name='depth_to_pointcloud_node',
         exit_handler=restart_exit_handler,
     )
+    package = 'tf2_ros'
     ld.add_process(
         # The XYZ/Quat numbers for base_link -> camera_rgb_frame are taken from the
         # turtlebot URDF in
         # https://github.com/turtlebot/turtlebot/blob/931d045/turtlebot_description/urdf/sensors/astra.urdf.xacro
         cmd=[
-            'static_transform_publisher',
+            get_executable_path(
+                package_name=package, executable_name='static_transform_publisher'),
             '-0.087', '-0.0125', '0.287',
             '0', '0', '0', '1',
-            'base_link', 'camera_rgb_frame'
+            'base_link',
+            'camera_rgb_frame'
         ],
         name='static_tf_pub_base_rgb',
         exit_handler=restart_exit_handler,
     )
+    package = 'tf2_ros'
     ld.add_process(
         # The XYZ/Quat numbers for camera_rgb_frame -> camera_depth_frame are taken from the
         # turtlebot URDF in
         # https://github.com/turtlebot/turtlebot/blob/931d045/turtlebot_description/urdf/sensors/astra.urdf.xacro
         cmd=[
-            'static_transform_publisher',
+            get_executable_path(
+                package_name=package, executable_name='static_transform_publisher'),
             '0', '0.0250', '0',
             '0', '0', '0', '1',
             'camera_rgb_frame',
@@ -62,24 +74,29 @@ def launch(launch_descriptor, argv):
         name='static_tf_pub_rgb_depth',
         exit_handler=restart_exit_handler,
     )
+    package = 'joy'
     ld.add_process(
-        cmd=['joy_node'],
+        cmd=[get_executable_path(package_name=package, executable_name='joy_node')],
         name='joy_node',
         exit_handler=restart_exit_handler,
     )
+    package = 'teleop_twist_joy'
     ld.add_process(
-        cmd=['teleop_node'],
+        cmd=[get_executable_path(package_name=package, executable_name='teleop_node')],
         name='teleop_node',
         exit_handler=restart_exit_handler,
     )
-    cartographer_ros_prefix = get_package_share_directory('cartographer_ros')
-    cartographer_config_dir = os.path.join(cartographer_ros_prefix, 'configuration_files')
+    package = 'cartographer_ros'
+    turtlebot2_cartographer_prefix = get_package_share_directory('turtlebot2_cartographer')
+    cartographer_config_dir = os.path.join(turtlebot2_cartographer_prefix, 'configuration_files')
     ld.add_process(
         cmd=[
-            'cartographer_node',
+            get_executable_path(package_name=package, executable_name='cartographer_node'),
             '-configuration_directory', cartographer_config_dir,
-            '-configuration_basename', 'turtlebot_3d.lua'
+            '-configuration_basename', 'turtlebot_2d.lua'
         ],
         name='cartographer_node',
         exit_handler=restart_exit_handler,
     )
+
+    return ld

turtlebot2_cartographer/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>turtlebot2_cartographer</name>
+  <version>0.0.0</version>
+  <description>Launch files for turtlebot2 cartographer demo</description>
+  <maintainer email="clalancette@openrobotics.org">Chris Lalancette</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>astra_camera</exec_depend>
+  <exec_depend>cartographer_ros</exec_depend>
+  <exec_depend>depthimage_to_laserscan</exec_depend>
+  <exec_depend>depth_to_pointcloud</exec_depend>
+  <exec_depend>joy</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>ros2run</exec_depend>
+  <exec_depend>teleop_twist_joy</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>turtlebot2_drivers</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>