diff --git a/.vscode/tasks.json b/.vscode/tasks.json
index cf9adc71..9648a074 100644
--- a/.vscode/tasks.json
+++ b/.vscode/tasks.json
@@ -6,7 +6,8 @@
"args": [
"build",
"--symlink-install",
- "--packages-skip-regex zed_*"
+ "--packages-skip-regex",
+ "zed_*"
],
"problemMatcher": [
"$colcon-gcc"
@@ -17,12 +18,14 @@
},
"label": "colcon: Colcon Build Release"
},
+
{
"type": "colcon",
"args": [
"build",
"--symlink-install",
- "--packages-skip-regex zed_*",
+ "--packages-skip-regex",
+ "zed_*",
"--cmake-args",
"-DCMAKE_BUILD_TYPE=RelWithDebInfo"
],
@@ -38,7 +41,8 @@
"args": [
"build",
"--symlink-install",
- "--packages-skip-regex zed_*",
+ "--packages-skip-regex",
+ "zed_*",
"--cmake-args",
"-DCMAKE_BUILD_TYPE=Debug"
],
diff --git a/src/igvc_bringup/launch/bringup.launch.py b/src/igvc_bringup/launch/bringup.launch.py
index f2c046be..4abb00c6 100644
--- a/src/igvc_bringup/launch/bringup.launch.py
+++ b/src/igvc_bringup/launch/bringup.launch.py
@@ -1,15 +1,18 @@
-from launch import LaunchDescription
+import os
+
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription, LaunchDescriptionSource
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition
-from launch.substitutions import LaunchConfiguration
-from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
use_sim = LaunchConfiguration('use_sim')
use_mock_hardware = LaunchConfiguration('use_mock_hardware')
-
+
return LaunchDescription([
# Launch Arguments
DeclareLaunchArgument(
@@ -77,16 +80,14 @@ def generate_launch_description():
),
condition = UnlessCondition(use_mock_hardware)
),
-
+
# SLAM
- IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
+ IncludeLaunchDescription(
+ FrontendLaunchDescriptionSource(
[FindPackageShare('igvc_slam'),
- '/launch',
- '/sim_rtabmap.launch.py']
- ),
+ '/launch',
+ '/igvc_slam.launch']
+ ),
condition = IfCondition(LaunchConfiguration('use_slam'))
- ),
-
- # TODO Nav
+ )
])
diff --git a/src/igvc_slam/CMakeLists.txt b/src/igvc_slam/CMakeLists.txt
index 97eea4ba..044be5fa 100644
--- a/src/igvc_slam/CMakeLists.txt
+++ b/src/igvc_slam/CMakeLists.txt
@@ -13,6 +13,11 @@ install(
DESTINATION share/${PROJECT_NAME}
)
+install(
+ DIRECTORY config
+ DESTINATION share/${PROJECT_NAME}
+)
+
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
diff --git a/src/igvc_slam/config/rtabmap.yaml b/src/igvc_slam/config/rtabmap.yaml
new file mode 100644
index 00000000..3c6d2523
--- /dev/null
+++ b/src/igvc_slam/config/rtabmap.yaml
@@ -0,0 +1,69 @@
+# RTAB-Map configuration
+
+subscribe_depth: true
+subscribe_rgbd: false
+subscribe_stereo: false
+subscribe_stereo: false
+subscribe_scan: false
+subscribe_scan_cloud: false
+subscribe_user_data: false
+subscribe_odom_info: false
+
+database_path: "~/.ros/rtabmap.db"
+config_path: "~/.ros/rtabmap.cfg"
+
+frame_id: "base_link"
+map_frame_id: "map"
+odom_frame_id: "odom" # odometry from odom msg to have covariance - Remapped by launch file
+odom_tf_angular_variance: 0.001 # If TF is used to get odometry, this is the default angular variance
+odom_tf_linear_variance: 0.001 # If TF is used to get odometry, this is the default linear variance
+tf_delay: 0.02
+publish_tf: false # Set to false if fusing different poses (map->odom) with UKF
+
+odom_sensor_sync: true
+wait_for_transform_duration: 0.2
+approx_sync: true
+
+queue_size: 10
+
+scan_normal_k: 0
+
+Grid:
+ 3D: true
+ FlatObstacleDetected: true
+ MapFrameProjection: false
+ GroundIsObstacle: false
+ PreVoxelFiltering: true
+ RayTracing: true
+ FromDepth: true
+ NormalsSegmentation: false
+ CellSize: 0.05
+ ClusterRadius: 0.1
+ MinClusterSize: 3
+ DepthDecimation: 1
+ DepthRoiRatios: [0.0, 0.0, 0.0, 0.0]
+ FootprintHeight: 2.0
+ FootprintLength: 0.18
+ FootprintWidth: 0.18
+ MaxGroundAngle: 30.0
+ MinGroundHeight: -0.5
+ MaxGroundHeight: -0.4
+ MaxObstacleHeight: 0.1
+ NoiseFilteringMinNeighbors: 5
+ NoiseFilteringRadius: 0.1
+ NormalK: 20
+ RangeMin: 0.7
+ RangeMax: 3.0
+
+GridGlobal:
+ Eroded: false # Erode obstacle cells
+ FootprintRadius: 0.18 # Footprint radius (m) used to clear all obstacles under the graph
+ FullUpdate: true # When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not
+ MaxNodes: 0 # Maximum nodes assembled in the map starting from the last node (0=unlimited)
+ MinSize: 1.0 # Minimum map size (m)
+ OccupancyThr: 0.55 # Occupancy threshold (value between 0 and 1)
+ ProbClampingMax: 0.971 # Probability clamping maximum (value between 0 and 1)
+ ProbClampingMin: 0.1192 # Probability clamping minimum (value between 0 and 1)
+ ProbHit: 0.7 # Probability of a hit (value between 0.5 and 1)
+ ProbMiss: 0.4 # Probability of a miss (value between 0 and 0.5)
+ UpdateError: 0.01 # Graph changed detection error (m). Update map only if poses in new optimized graph have moved more than this value
\ No newline at end of file
diff --git a/src/igvc_slam/launch/igvc_slam.launch b/src/igvc_slam/launch/igvc_slam.launch
new file mode 100644
index 00000000..48628ef5
--- /dev/null
+++ b/src/igvc_slam/launch/igvc_slam.launch
@@ -0,0 +1,56 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/igvc_slam/launch/include/rtabmap.launch b/src/igvc_slam/launch/include/rtabmap.launch
new file mode 100644
index 00000000..1f5b5140
--- /dev/null
+++ b/src/igvc_slam/launch/include/rtabmap.launch
@@ -0,0 +1,52 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/igvc_slam/launch/rtabmap.launch.py b/src/igvc_slam/launch/rtabmap.launch.py
deleted file mode 100644
index 958ad292..00000000
--- a/src/igvc_slam/launch/rtabmap.launch.py
+++ /dev/null
@@ -1,85 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch_ros.actions import Node
-
-import xacro
-
-def generate_launch_description():
- rtabmap_package = get_package_share_directory('rtabmap_launch')
- realsense_package = get_package_share_directory('realsense2_camera')
-
- rtabmap_launch_path = os.path.join(rtabmap_package, 'launch', 'rtabmap.launch.py')
- realsense_launch_path = os.path.join(realsense_package, 'launch', 'rs_launch.py')
-
- imu_filter = Node(
- package='imu_filter_madgwick',
- executable='imu_filter_madgwick_node',
- output='screen',
- remappings=[
- ('/imu/data_raw', '/camera/camera/imu'),
- ('imu/data', '/rtabmap/imu')
- ],
- parameters=[
- {'use_mag' : False},
- {'publish_tf' : True},
- {'fixed_frame' : 'camera_link'}
- ]
- )
-
- rtabmap = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(rtabmap_launch_path),
- launch_arguments={
- 'args' : '--delete_db_on_start',
- 'depth_topic' : '/camera/camera/aligned_depth_to_color/image_raw',
- 'rgb_topic' : '/camera/camera/color/image_raw',
- 'camera_info_topic' : '/camera/camera/color/camera_info',
- 'frame_id' : 'base_footprint',
- 'publish_tf_odom' : 'true',
- 'odom_topic' : '/odom',
- 'odom_frame_id' : 'odom',
- 'approx_sync' : 'true',
- 'rgbd_sync' : 'true',
- 'approx_rgbd_sync' : 'true',
- 'subscribe_rgbd' : 'true',
- 'visual_odometry' : 'true',
- 'qos' : '1',
- 'approx_sync_max_interval' : '0.01',
- 'sync_queue_size' : '50',
- 'topic_queue_size' : '50',
- 'wait_for_transform' : '0.4',
- 'imu_topic' : '/rtabmap/imu',
- 'wait_imu_to_init' : 'true',
- 'rtabmap_viz' : 'true',
- 'map_topic' : '/map'
- }.items()
- )
-
- realsense = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(realsense_launch_path),
- launch_arguments={
- 'enable_color' : 'true',
- 'enable_depth' : 'true',
- 'align_depth.enable' : 'true',
- 'pointcloud.enable' : 'false',
- 'enable_sync' : 'false',
- 'unite_imu_method' : '2',
- 'enable_gyro' : 'true',
- 'enable_accel' : 'true',
- 'color_fps' : '60',
- 'depth_fps' : '60',
- 'gyro_fps' : '200',
- 'accel_fps' : '63',
- 'publish_tf' : 'true'
- }.items()
- )
-
- return LaunchDescription([
- imu_filter,
- rtabmap,
- realsense
- ])
diff --git a/src/igvc_slam/launch/sim_rtabmap.launch.py b/src/igvc_slam/launch/sim_rtabmap.launch.py
deleted file mode 100644
index 41b3991f..00000000
--- a/src/igvc_slam/launch/sim_rtabmap.launch.py
+++ /dev/null
@@ -1,49 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-
-from launch_ros.actions import Node
-
-def generate_launch_description():
- rtabmap_package = get_package_share_directory('rtabmap_launch')
-
- rtabmap_launch_path = os.path.join(rtabmap_package, 'launch', 'rtabmap.launch.py')
-
- rtabmap = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(rtabmap_launch_path),
- launch_arguments={
- 'args' : '--delete_db_on_start',
- 'depth_topic' : '/camera/camera/depth/image_rect_raw',
- 'rgb_topic' : '/camera/camera/color/image_raw',
- 'camera_info_topic' : '/camera/camera/depth/camera_info',
- 'approx_sync' : 'true',
- 'frame_id' : 'base_footprint',
- 'log_level' : 'debug',
- 'publish_tf_odom' : 'true',
- 'odom_topic' : '/odom',
- 'odom_frame_id' : 'odom',
- 'sync_queue_size' : '10', #TODO verify
- 'topic_queue_size' : '10', #TODO verify
- 'rgbd_sync' : 'true',
- 'approx_rgbd_sync' : 'true',
- 'subscribe_depth' : 'false',
- 'subscribe_rgbd' : 'true',
- 'visual_odometry' : 'true',
- 'approx_sync_max_interval' : '0.01',
- 'qos' : '1',
- 'rviz' : 'true',
- 'use_sim_time' : 'true',
- 'wait_for_transform' : '0.4',
- 'imu_topic' : '/rtabmap/imu',
- 'wait_imu_to_init' : 'false',
- 'map_topic' : '/map'
- }.items()
- )
-
- return LaunchDescription([
- rtabmap
- ])
\ No newline at end of file