diff --git a/.gitignore b/.gitignore index 61609e7d68b8a74625ef02ced2a2d5ccc30450f7..d473938f9898ef4eac1e64d8b46fb3f7ca21cc32 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,7 @@ install/ log/ .vscode/ +.cache/ __pycache__/ .catkin_workspace *.gv @@ -11,4 +12,6 @@ __pycache__/ mat_pre.txt mat_out.txt -328.txt \ No newline at end of file +imu.txt +dbg.txt +pos_log.txt \ No newline at end of file diff --git a/README.md b/README.md index 8725cd49e24ec8bb6fc9f1ada15f5242608aca54..1b2b067d7e2a80b1ccf698b19283f8ce34c93758 100644 --- a/README.md +++ b/README.md @@ -2,17 +2,17 @@ 深圳北理莫斯科大学 北极熊战队 哨兵导航仿真包 ## 一. 项目介绍 -本项目使用麦克纳姆轮仿真小车，附加 Livox Mid360 雷达与 IMU，在 2023 RMUC/RMUL 地图进行导航算法仿真。 +本项目使用麦克纳姆轮仿真小车，附加 Livox Mid360 雷达与 IMU，在 RMUC/RMUL 地图进行导航算法仿真。 Mid360 点云仿真：参考了 [livox_laser_simulation](https://github.com/Livox-SDK/livox_laser_simulation/blob/main/src/livox_points_plugin.cpp)、 [livox_laser_simulation_RO2](https://github.com/stm32f303ret6/livox_laser_simulation_RO2/blob/main/src/livox_points_plugin.cpp)、 [Issue15: CustomMsg](https://github.com/Livox-SDK/livox_laser_simulation/issues/15) 导航算法框架：基于 [中南大学 FYT 战队 RM 哨兵上位机算法 ](https://github.com/baiyeweiguang/CSU-RM-Sentry) 略微修改以适配仿真。 - 非常非常感谢以上开源项目的帮助！！！ + |Gazebo 仿真|Fast_LIO + Navigation2| |:-:|:-:| -||| +||| |Mid360点云仿真 + FAST_LIO 里程计 + 障碍物/地面点云分割 + 三维点云转二维 + Navigation2| |:-:| @@ -40,6 +40,7 @@ Mid360 点云仿真：参考了 [livox_laser_simulation](https://github.com/Livo sudo apt install -y ros-humble-joint-state-publisher sudo apt install -y ros-humble-rviz2 sudo apt install -y ros-humble-nav2* + sudo apt install -y ros-humble-slam-toolbox sudo apt install -y ros-humble-pcl-ros sudo apt install -y ros-humble-pcl-conversions sudo apt install -y ros-humble-libpointmatcher @@ -68,7 +69,7 @@ Mid360 点云仿真：参考了 [livox_laser_simulation](https://github.com/Livo ## 四. 编译运行 ``` -git clone https://gitee.com/SMBU-POLARBEAR/pb_rmsimulation --recursive +git clone --recursive https://gitee.com/SMBU-POLARBEAR/pb_rmsimulation ``` ``` @@ -76,44 +77,77 @@ cd pb_rmsimulation ./build.sh ``` -### 仿真模式 +### 可选参数 -- 可选参数 +1. `world`: - world: - - `RMUL` - [Robomaster 1V1 场地（带动态障碍物）](https://github.com/SCAU-RM-NAV/rm2022_auto_infantry_ws) - - `RMUC` - [2023 Robomaster 7V7 场地](https://bbs.robomaster.com/thread-22576-1-1.html) + （仿真模式） + - `RMUL` - [Robomaster 1V1 场地（带动态障碍物）](https://github.com/SCAU-RM-NAV/rm2022_auto_infantry_ws) + - `RMUC` - [2024 Robomaster 7V7 场地](https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22942&extra=page%3D1) - rviz: - - `true` - 可视化 robot_descrition, FAST_LIO - - `false` - 仅提供 nav2 可视化窗口 + （真实环境） + - 自定，world 等价于 `.pcd(ICP使用的点云图)` 文件和 `.yaml(Nav使用的栅格地图)` 的名称， -- 边建图边导航 +2. `mode`: + - `mapping` - 边建图边导航 + - `nav` - 已知全局地图导航 - ``` - ros2 launch ros2 launch rm_bringup bringup_sim_mapping.launch.py rviz:=false world:=RMUL - ``` +3. `localization` (仅 `mode:=nav` 时本参数有效) + - `amcl` - 使用 [AMCL](https://navigation.ros.org/configuration/packages/configuring-amcl.html) 算法定位（经典算法） + - `slam_toolbox` - 使用 [slam_toolbox](https://github.com/SteveMacenski/slam_toolbox) localization 模式定位（动态场景中性能更好） + - `icp` - 使用 [ICP_Localization](https://github.com/baiyeweiguang/icp_localization_ros2) 算法定位（性能开销较多） -- 已知全局地图导航 - ``` - ros2 launch rm_bringup bringup_sim_navigation.launch.py rviz:=false world:=RMUL - ``` + Tips: + 1. 若使用 AMCL 算法定位时，启动后需要在 rviz2 中手动给定初始位姿。 + 2. 若使用 slam_toolbox 定位，需要提供 .posegraph 地图，详见 [localization using SLAM_Toolbox](https://answers.ros.org/question/365472/localization-using-slam_toolbox/) + 3. 若使用 ICP_Localization 定位，需要提供 .pcd 点云图 + +4. `rviz`: + - `True` - 可视化 robot_descrition, FAST_LIO + - `False` - 仅提供 nav2 可视化窗口 + +### 仿真模式 + +- 示例： + - 边建图边导航 + + ``` + ros2 launch rm_bringup bringup_sim.launch.py \ + world:=RMUL \ + mode:=mapping + ``` + + - 已知全局地图导航 + ``` + ros2 launch rm_bringup bringup_sim.launch.py \ + world:=RMUL \ + mode:=nav \ + localization:=slam_toolbox + ``` ### 真实环境 -- 边建图边导航 +- 示例： + - 边建图边导航 + ``` - ros2 launch ros2 launch rm_bringup bringup_real_mapping.launch.py rviz:=false + ros2 launch rm_bringup bringup_real.launch.py \ + world:=YOUR_WORLD_NAME \ + mode:=mapping ``` - Tips: + Tips: 1. 保存点云 pcd 文件：需先在 [fastlio_mid360.yaml](src/rm_bringup/config/fastlio_mid360.yaml) 中 将 `pcd_save_en` 改为 `true`，并设置 .pcd 文件的名称 2. 保存栅格地图：需先在 [save_grid_map.sh](save_grid_map.sh) 将 `YOUR_MAP_NAME` 更改为与 .pcd 文件相同的名称。保存后在当前工作空间新开终端，运行 `./save_grid_map.sh` 即可 + 3. 保存 .posegraph 地图：请参考 [localization using SLAM_Toolbox](https://answers.ros.org/question/365472/localization-using-slam_toolbox/) -- 已知全局地图导航 + - 已知全局地图导航 ``` - ros2 launch ros2 launch rm_bringup bringup_real_mapping.launch.py rviz:=false world:=YOUR_WORLD_NAME + ros2 launch rm_bringup bringup_real.launch.py \ + world:=YOUR_WORLD_NAME \ + mode:=nav \ + localization:=slam_toolbox ``` Tips: 栅格地图文件和 pcd 文件需具为相同名称，分别存放在 `src/rm_bringup/map` 和 `src/rm_bringup/PCD` 中，启动导航时 world 指定为文件名前缀即可。 diff --git a/docs/FAST_LIO+Nav2.png b/docs/FAST_LIO+Nav2.png index 88ebdfd210b2b9474bcfe88694d6a4344f6b53f5..8dfa102f047c40a209c5571ff41bfc16cd9d9059 100644 Binary files a/docs/FAST_LIO+Nav2.png and b/docs/FAST_LIO+Nav2.png differ diff --git a/docs/RMUC_2024_mapping.png b/docs/RMUC_2024_mapping.png new file mode 100644 index 0000000000000000000000000000000000000000..fbfe6f0af60d67ce421cbf563f2d9caaab1f0848 Binary files /dev/null and b/docs/RMUC_2024_mapping.png differ diff --git a/src/rm_bringup/PCD/RMUC.pcd b/src/rm_bringup/PCD/RMUC.pcd index 39755c16d59bb45c2c2f5af34c108704431ec5cf..d170740c6e7b7ddaf30c779c79dd4adca91a6b54 100644 Binary files a/src/rm_bringup/PCD/RMUC.pcd and b/src/rm_bringup/PCD/RMUC.pcd differ diff --git a/src/rm_bringup/config/icp_node_params.yaml b/src/rm_bringup/config/icp_node_params.yaml index f2fc7f0820fdca5a6361f352bbdc5716a04faad0..d24c3153bf62d3e353dd7ed61c718ff446492094 100644 --- a/src/rm_bringup/config/icp_node_params.yaml +++ b/src/rm_bringup/config/icp_node_params.yaml @@ -1,6 +1,6 @@ /icp_localization: ros__parameters: - # pcd_file_path: "src/rm_simulation/pb_rm_simulation/meshes/RMUC2023_world/preview/RMUC.pcd" + # pcd_file_path: "src/rm_simulation/pb_rm_simulation/meshes/RMUC2024_world/preview/RMUC.pcd" # pcd_file_path: "src/rm_simulation/pb_rm_simulation/meshes/auto_world/preview/RMUL.pcd" icp_localization_ros2: range_data_topic: "/cloud_registered_body_downsampling" diff --git a/src/rm_bringup/config/mapper_params_localization.yaml b/src/rm_bringup/config/mapper_params_localization.yaml new file mode 100644 index 0000000000000000000000000000000000000000..04f00857fe39a28855bc2a12a2365a4867cda29e --- /dev/null +++ b/src/rm_bringup/config/mapper_params_localization.yaml @@ -0,0 +1,67 @@ +slam_toolbox: + ros__parameters: + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + mode: localization #localization + + # if you'd like to start localizing on bringup in a map and pose + # map_file_name: + map_start_pose: [0.0, 0.0, 0.0] + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 3 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + do_loop_closing: true + loop_match_minimum_chain_size: 3 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + loop_search_maximum_distance: 3.0 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true \ No newline at end of file diff --git a/src/rm_bringup/config/nav2_params.yaml b/src/rm_bringup/config/nav2_params.yaml index 90291ad76e1c5d7d68b3766ff878e4dc84e100b2..e957d87686ca343f2d1fba0c6c6e815f472b6739 100644 --- a/src/rm_bringup/config/nav2_params.yaml +++ b/src/rm_bringup/config/nav2_params.yaml @@ -159,7 +159,7 @@ controller_server: RotateToGoal.scale: 32.0 RotateToGoal.slowing_factor: 5.0 RotateToGoal.lookahead_time: -1.0 - + controller_server_rclcpp_node: ros__parameters: use_sim_time: False @@ -170,13 +170,12 @@ local_costmap: update_frequency: 20.0 publish_frequency: 10.0 global_frame: odom - global_frame: map - # robot_base_frame: base_link + robot_base_frame: base_link use_sim_time: False rolling_window: true width: 5 height: 5 - resolution: 0.05 + resolution: 0.02 robot_radius: 0.2 plugins: ["voxel2d_layer", "voxel3d_layer", "inflation_layer"] inflation_layer: @@ -192,7 +191,7 @@ local_costmap: z_voxels: 16 max_obstacle_height: 2.0 mark_threshold: 0 - observation_sources: scan + observation_sources: scan scan: topic: /scan obstacle_max_range: 10.0 @@ -200,7 +199,7 @@ local_costmap: raytrace_min_range: 0.0 raytrace_max_range: 20.0 min_obstacle_height: 0.0 - max_obstacle_height: 2.0 + max_obstacle_height: 2.0 clearing: True marking: True data_type: "LaserScan" @@ -214,15 +213,15 @@ local_costmap: mark_threshold: 1 observation_sources: livox min_obstacle_height: 0.00 - max_obstacle_height: 2.0 + max_obstacle_height: 2.0 livox: - topic: /livox/lidar/pointcloud + topic: /segmentation/obstacle obstacle_max_range: 10.0 obstacle_min_range: 0.0 raytrace_min_range: 0.0 raytrace_max_range: 20.0 min_obstacle_height: 0.0 - max_obstacle_height: 2.0 + max_obstacle_height: 2.0 clearing: True marking: True data_type: "PointCloud2" @@ -245,7 +244,7 @@ global_costmap: robot_base_frame: base_link use_sim_time: False robot_radius: 0.2 - resolution: 0.05 + resolution: 0.02 track_unknown_space: true plugins: ["static_layer", "voxel2d_layer", "voxel3d_layer", "inflation_layer"] voxel2d_layer: @@ -259,7 +258,7 @@ global_costmap: raytrace_min_range: 0.0 raytrace_max_range: 20.0 min_obstacle_height: 0.0 - max_obstacle_height: 2.0 + max_obstacle_height: 2.0 clearing: True marking: True data_type: "LaserScan" @@ -273,15 +272,15 @@ global_costmap: mark_threshold: 1 observation_sources: livox min_obstacle_height: 0.00 - max_obstacle_height: 2.0 + max_obstacle_height: 2.0 livox: - topic: /livox/lidar/pointcloud + topic: /segmentation/obstacle obstacle_max_range: 10.0 obstacle_min_range: 0.0 raytrace_min_range: 0.0 raytrace_max_range: 20.0 min_obstacle_height: 0.0 - max_obstacle_height: 2.0 + max_obstacle_height: 2.0 clearing: True marking: True data_type: "PointCloud2" @@ -357,7 +356,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false - waypoint_task_executor_plugin: "wait_at_waypoint" + waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True diff --git a/src/rm_bringup/config/segmentation_params.yaml b/src/rm_bringup/config/segmentation_params.yaml index ce455747378e5cf61066dbafeba59282c841524e..1b0593da29f17aae9ed3e57e62a39f9c86962dd3 100644 --- a/src/rm_bringup/config/segmentation_params.yaml +++ b/src/rm_bringup/config/segmentation_params.yaml @@ -9,14 +9,14 @@ ground_segmentation: max_dist_to_line: 0.1 # maximum vertical distance of point to line to be considered ground. - sensor_height: 0.225 # sensor height above ground. - min_slope: 0.0 # minimum slope of a ground line. - max_slope: 0.3 # maximum slope of a ground line. + sensor_height: 0.275 # sensor height above ground. + min_slope: -0.4 # minimum slope of a ground line. + max_slope: 0.4 # maximum slope of a ground line. max_fit_error: 0.05 # maximum error of a point during line fit. long_threshold: 1.0 # distance between points after which they are considered far from each other. max_long_height: 0.1 # maximum height change to previous point in long line. - max_start_height: 0.2 # maximum difference to estimated ground height to start a new line. - line_search_angle: 0.1 # how far to search in angular direction to find a line [rad]. + max_start_height: 0.5 # maximum difference to estimated ground height to start a new line. + line_search_angle: 0.8 # how far to search in angular direction to find a line [rad]. gravity_aligned_frame: "" # Frame which has its z axis aligned with gravity. (Sensor frame if empty.) diff --git a/src/rm_bringup/launch/bringup_real_navigation.launch.py b/src/rm_bringup/launch/bringup_real.launch.py similarity index 42% rename from src/rm_bringup/launch/bringup_real_navigation.launch.py rename to src/rm_bringup/launch/bringup_real.launch.py index c4e94bcb4ea6fd0daa9cc1b1f6ffd69574c2ae67..2c98f14a834553444bc9f3bd9fa8cb393fe8614f 100644 --- a/src/rm_bringup/launch/bringup_real_navigation.launch.py +++ b/src/rm_bringup/launch/bringup_real.launch.py @@ -3,34 +3,37 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, TimerAction +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, GroupAction from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +from launch.conditions import LaunchConfigurationEquals, LaunchConfigurationNotEquals def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('rm_bringup') pb_rm_simulation_launch_dir = os.path.join(get_package_share_directory('pb_rm_simulation'), 'launch') navigation2_launch_dir = os.path.join(get_package_share_directory('rm_navigation'), 'launch') - pointcloud_downsampling_config = os.path.join(bringup_dir, 'config', 'pointcloud_downsampling.yaml') - # Select world (map file share the same name prefix as the 'world' parameter) - world = LaunchConfiguration('world', default='328') + # Create the launch configuration variables + world = LaunchConfiguration('world') + use_sim_time = LaunchConfiguration('use_sim_time') + use_rviz = LaunchConfiguration('rviz') # Get the default map/config directory - map_dir = PathJoinSubstitution([bringup_dir, 'map', world]), ".yaml" - icp_pcd_dir = PathJoinSubstitution([bringup_dir, 'PCD', world]), ".pcd" + slam_toolbox_map_dir = PathJoinSubstitution([bringup_dir, 'map', world]) + slam_toolbox_localization_file_dir = os.path.join(bringup_dir, 'config', 'mapper_params_localization.yaml') + slam_toolbox_mapping_file_dir = os.path.join(bringup_dir, 'config', 'mapper_params_online_async.yaml') + + nav2_map_dir = PathJoinSubstitution([bringup_dir, 'map', world]), ".yaml" nav2_params_file_dir = os.path.join(bringup_dir, 'config', 'nav2_params.yaml') + + icp_pcd_dir = PathJoinSubstitution([bringup_dir, 'PCD', world]), ".pcd" icp_node_params_dir = os.path.join(bringup_dir, 'config', 'icp_node_params.yaml') icp_config_dir = os.path.join(bringup_dir, 'config', 'icp_config.yaml') icp_input_filters_config_path = os.path.join(bringup_dir, 'config', 'icp_input_filters_mid360.yaml') - - # Create the launch configuration variables - use_sim_time = LaunchConfiguration('use_sim_time', default='false') - use_rviz = LaunchConfiguration('rviz', default='false') # Visualize robot descrption - map_yaml_file = LaunchConfiguration('map', default=map_dir) - nav2_params_file = LaunchConfiguration('params_file', default=nav2_params_file_dir) + + pointcloud_downsampling_config_dir = os.path.join(bringup_dir, 'config', 'pointcloud_downsampling.yaml') ####################### Livox_ros_driver2 parameters start ####################### xfer_format = 4 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format @@ -59,6 +62,31 @@ def generate_launch_description(): ] ####################### Livox_ros_driver2 parameters end ######################### + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='False', + description='Use simulation (Gazebo) clock if true') + + declare_use_rviz_cmd = DeclareLaunchArgument( + 'rviz', + default_value='False', + description='Visualize robot tf and FAST_LIO cloud_map if true') + + declare_world_cmd = DeclareLaunchArgument( + 'world', + default_value='328', + description='Select world (map file, pcd file, world file share the same name prefix as the this parameter)') + + declare_mode_cmd = DeclareLaunchArgument( + 'mode', + default_value='', + description='Choose mode: nav, mapping') + + declare_localization_cmd = DeclareLaunchArgument( + 'localization', + default_value='', + description='Choose localization method: slam_toolbox, amcl, icp') + # Specify the actions start_livox_ros_driver2_node = Node( package='livox_ros_driver2', @@ -74,54 +102,107 @@ def generate_launch_description(): ) pointcloud_downsampling_node = Node( + # Only run pointcloud_downsampling_node when using icp_localization + condition = LaunchConfigurationEquals('mode', 'nav') and LaunchConfigurationEquals('localization', 'icp'), package='pointcloud_downsampling', executable='pointcloud_downsampling_node', output='screen', parameters=[ - pointcloud_downsampling_config, + pointcloud_downsampling_config_dir, {'use_sim_time': use_sim_time } ] ) - icp_node = Node( - package='icp_localization_ros2', - executable='icp_localization', - output='screen', - parameters=[ - icp_node_params_dir, - {'pcd_file_path': icp_pcd_dir, - 'icp_config_path': icp_config_dir, - 'input_filters_config_path': icp_input_filters_config_path, - 'use_sim_time': use_sim_time } + start_localization_group = GroupAction( + condition = LaunchConfigurationEquals('mode', 'nav'), + actions=[ + Node( + condition = LaunchConfigurationEquals('localization', 'slam_toolbox'), + package='slam_toolbox', + executable='localization_slam_toolbox_node', + name='slam_toolbox', + parameters=[ + slam_toolbox_localization_file_dir, + {'use_sim_time': use_sim_time, + 'map_file_name': slam_toolbox_map_dir, + 'map_start_pose': [0.0, 0.0, 0.0]} + ], + ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(navigation2_launch_dir,'localization_amcl_launch.py')), + condition = LaunchConfigurationEquals('localization', 'amcl'), + launch_arguments = { + 'use_sim_time': use_sim_time, + 'params_file': nav2_params_file_dir, + 'map': nav2_map_dir}.items() + ), + + Node( + condition = LaunchConfigurationEquals('localization', 'icp'), + package='icp_localization_ros2', + executable='icp_localization', + output='screen', + parameters=[ + icp_node_params_dir, + {'use_sim_time': use_sim_time, + 'pcd_file_path': icp_pcd_dir, + 'icp_config_path': icp_config_dir, + 'input_filters_config_path': icp_input_filters_config_path} + ] + ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(navigation2_launch_dir, 'map_server_launch.py')), + condition = LaunchConfigurationNotEquals('localization', 'slam_toolbox'), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'map': nav2_map_dir, + 'params_file': nav2_params_file_dir, + 'container_name': 'nav2_container'}.items()) ] ) - - icp_delay_node = TimerAction( - period=1.5, - actions=[icp_node] + + start_mapping = Node( + condition = LaunchConfigurationEquals('mode', 'mapping'), + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + parameters=[ + slam_toolbox_mapping_file_dir, + {'use_sim_time': use_sim_time,} + ], ) start_navigation2 = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(navigation2_launch_dir, 'bringup_launch.py')), - launch_arguments={ - 'map': map_yaml_file, + PythonLaunchDescriptionSource(os.path.join(navigation2_launch_dir, 'bringup_rm_navigation.py')), + launch_arguments={ 'use_sim_time': use_sim_time, - 'params_file': nav2_params_file}.items() + 'map': nav2_map_dir, + 'params_file': nav2_params_file_dir}.items() ) start_common = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(bringup_dir, 'launch', 'common.py')) + PythonLaunchDescriptionSource(os.path.join(bringup_dir, 'launch', 'common.py')), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'rviz': use_rviz}.items() ) ld = LaunchDescription() # Declare the launch options + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(declare_mode_cmd) + ld.add_action(declare_localization_cmd) ld.add_action(start_livox_ros_driver2_node) ld.add_action(start_robot_description) ld.add_action(start_common) ld.add_action(pointcloud_downsampling_node) - ld.add_action(icp_delay_node) + ld.add_action(start_localization_group) + ld.add_action(start_mapping) ld.add_action(start_navigation2) return ld - \ No newline at end of file diff --git a/src/rm_bringup/launch/bringup_real_mapping.launch.py b/src/rm_bringup/launch/bringup_real_mapping.launch.py deleted file mode 100644 index 600c0318cd883fb0162f45168684e8b2f4897ed8..0000000000000000000000000000000000000000 --- a/src/rm_bringup/launch/bringup_real_mapping.launch.py +++ /dev/null @@ -1,105 +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.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node - - -def generate_launch_description(): - # Get the launch directory - bringup_dir = get_package_share_directory('rm_bringup') - pb_rm_simulation_launch_dir = os.path.join(get_package_share_directory('pb_rm_simulation'), 'launch') - navigation2_launch_dir = os.path.join(get_package_share_directory('rm_navigation'), 'launch') - - # Select world (map file share the same name prefix as the 'world' parameter) - world = LaunchConfiguration('world', default='RMUL') - - map_dir = PathJoinSubstitution([bringup_dir, 'map', world]), ".yaml" - nav2_params_file_dir = os.path.join(bringup_dir, 'config', 'nav2_params.yaml') - slam_params_file_dir = os.path.join(bringup_dir, 'config', 'mapper_params_online_async.yaml') - - # Create the launch configuration variables - use_sim_time = LaunchConfiguration('use_sim_time', default='false') - use_rviz = LaunchConfiguration('rviz', default='false') # Visualize robot descrption - map_yaml_file = LaunchConfiguration('map', default=map_dir) - nav2_params_file = LaunchConfiguration('params_file', default=nav2_params_file_dir) - - ####################### Livox_ros_driver2 parameters start ####################### - xfer_format = 4 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format - multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic - data_src = 0 # 0-lidar, others-Invalid data src - publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. - output_type = 0 - frame_id = 'livox_frame' - lvx_file_path = '/home/livox/livox_test.lvx' - cmdline_bd_code = 'livox0000000001' - - cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' - cur_config_path = cur_path + '../config' - user_config_path = os.path.join(cur_config_path, 'MID360_config.json') - - livox_ros2_params = [ - {"xfer_format": xfer_format}, - {"multi_topic": multi_topic}, - {"data_src": data_src}, - {"publish_freq": publish_freq}, - {"output_data_type": output_type}, - {"frame_id": frame_id}, - {"lvx_file_path": lvx_file_path}, - {"user_config_path": user_config_path}, - {"cmdline_input_bd_code": cmdline_bd_code} - ] - ####################### Livox_ros_driver2 parameters end ######################### - - # Specify the actions - start_livox_ros_driver2_node = Node( - package='livox_ros_driver2', - executable='livox_ros_driver2_node', - name='livox_lidar_publisher', - output='screen', - parameters=livox_ros2_params - ) - - start_robot_description = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(pb_rm_simulation_launch_dir, 'rm_real.launch.py')), - launch_arguments={'rviz': use_rviz}.items() - ) - - start_async_slam_toolbox_node = Node( - package='slam_toolbox', - executable='async_slam_toolbox_node', - name='slam_toolbox', - parameters=[ - slam_params_file_dir, - {'use_sim_time': use_sim_time} - ], - output='screen' - ) - - start_navigation2 = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(navigation2_launch_dir, 'bringup_no_amcl_launch.py')), - launch_arguments={ - 'map': map_yaml_file, - 'use_sim_time': use_sim_time, - 'params_file': nav2_params_file}.items() - ) - - start_common = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(bringup_dir, 'launch', 'common.py')) - ) - - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(start_livox_ros_driver2_node) - ld.add_action(start_robot_description) - ld.add_action(start_common) - ld.add_action(start_async_slam_toolbox_node) - ld.add_action(start_navigation2) - - return ld - \ No newline at end of file diff --git a/src/rm_bringup/launch/bringup_sim_navigation.launch.py b/src/rm_bringup/launch/bringup_sim.launch.py similarity index 34% rename from src/rm_bringup/launch/bringup_sim_navigation.launch.py rename to src/rm_bringup/launch/bringup_sim.launch.py index 1f62faf2b9e54172cbd96315850de6258cde710f..76e339dc327ab01b7ffff74a52cf45a8005fd592 100644 --- a/src/rm_bringup/launch/bringup_sim_navigation.launch.py +++ b/src/rm_bringup/launch/bringup_sim.launch.py @@ -3,104 +3,186 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, TimerAction +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, GroupAction from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch.actions.append_environment_variable import AppendEnvironmentVariable +from launch.conditions import LaunchConfigurationEquals, LaunchConfigurationNotEquals + +def mode_and_localization(context): + return context.launch_configurations['mode'] == 'nav' and context.launch_configurations['localization'] != 'slam_toolbox' def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('rm_bringup') pb_rm_simulation_launch_dir = os.path.join(get_package_share_directory('pb_rm_simulation'), 'launch') navigation2_launch_dir = os.path.join(get_package_share_directory('rm_navigation'), 'launch') - pointcloud_downsampling_config = os.path.join(bringup_dir, 'config', 'pointcloud_downsampling.yaml') - # Select world (map file, pcd file, world file share the same name prefix as the 'world' parameter) - world = LaunchConfiguration('world', default='RMUL') + # Create the launch configuration variables + world = LaunchConfiguration('world') + use_sim_time = LaunchConfiguration('use_sim_time') + use_rviz = LaunchConfiguration('rviz') # Get the default map/config directory - map_dir = PathJoinSubstitution([bringup_dir, 'map', world]), ".yaml" - icp_pcd_dir = PathJoinSubstitution([bringup_dir, 'PCD', world]), ".pcd" + slam_toolbox_map_dir = PathJoinSubstitution([bringup_dir, 'map', world]) + slam_toolbox_localization_file_dir = os.path.join(bringup_dir, 'config', 'mapper_params_localization.yaml') + slam_toolbox_mapping_file_dir = os.path.join(bringup_dir, 'config', 'mapper_params_online_async.yaml') + + nav2_map_dir = PathJoinSubstitution([bringup_dir, 'map', world]), ".yaml" nav2_params_file_dir = os.path.join(bringup_dir, 'config', 'nav2_params.yaml') + + icp_pcd_dir = PathJoinSubstitution([bringup_dir, 'PCD', world]), ".pcd" icp_node_params_dir = os.path.join(bringup_dir, 'config', 'icp_node_params.yaml') icp_config_dir = os.path.join(bringup_dir, 'config', 'icp_config.yaml') icp_input_filters_config_path = os.path.join(bringup_dir, 'config', 'icp_input_filters_mid360.yaml') - - # Create the launch configuration variables - use_sim_time = LaunchConfiguration('use_sim_time', default='true') - use_rviz = LaunchConfiguration('rviz', default='false') # Visualize robot descrption - map_yaml_file = LaunchConfiguration('map', default=map_dir) - nav2_params_file = LaunchConfiguration('params_file', default=nav2_params_file_dir) - + + pointcloud_downsampling_config_dir = os.path.join(bringup_dir, 'config', 'pointcloud_downsampling.yaml') # Set Gazebo plugin path append_enviroment = AppendEnvironmentVariable( 'GAZEBO_PLUGIN_PATH', os.path.join(os.path.join(get_package_share_directory('pb_rm_simulation'), 'meshes', 'obstacles', 'obstacle_plugin', 'lib')) ) - + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='True', + description='Use simulation (Gazebo) clock if true') + + declare_use_rviz_cmd = DeclareLaunchArgument( + 'rviz', + default_value='False', + description='Visualize robot tf and FAST_LIO cloud_map if true') + + declare_world_cmd = DeclareLaunchArgument( + 'world', + default_value='RMUL', + description='Select world (map file, pcd file, world file share the same name prefix as the this parameter)') + + declare_mode_cmd = DeclareLaunchArgument( + 'mode', + default_value='', + description='Choose mode: nav, mapping') + + declare_localization_cmd = DeclareLaunchArgument( + 'localization', + default_value='', + description='Choose localization method: slam_toolbox, amcl, icp') + # Specify the actions start_rm_simulation = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(pb_rm_simulation_launch_dir, 'rm_simulation.launch.py')), - launch_arguments={ + launch_arguments={ 'use_sim_time': use_sim_time, 'world': world, 'rviz': use_rviz}.items() ) pointcloud_downsampling_node = Node( + # Only run pointcloud_downsampling_node when using icp_localization + condition = LaunchConfigurationEquals('mode', 'nav') and LaunchConfigurationEquals('localization', 'icp'), package='pointcloud_downsampling', executable='pointcloud_downsampling_node', output='screen', parameters=[ - pointcloud_downsampling_config, + pointcloud_downsampling_config_dir, {'use_sim_time': use_sim_time } ] ) - icp_node = Node( - package='icp_localization_ros2', - executable='icp_localization', - output='screen', - parameters=[ - icp_node_params_dir, - {'pcd_file_path': icp_pcd_dir, - 'icp_config_path': icp_config_dir, - 'input_filters_config_path': icp_input_filters_config_path, - 'use_sim_time': use_sim_time } + start_localization_group = GroupAction( + condition = LaunchConfigurationEquals('mode', 'nav'), + actions=[ + Node( + condition = LaunchConfigurationEquals('localization', 'slam_toolbox'), + package='slam_toolbox', + executable='localization_slam_toolbox_node', + name='slam_toolbox', + parameters=[ + slam_toolbox_localization_file_dir, + {'use_sim_time': use_sim_time, + 'map_file_name': slam_toolbox_map_dir, + 'map_start_pose': [0.0, 0.0, 0.0]} + ], + ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(navigation2_launch_dir,'localization_amcl_launch.py')), + condition = LaunchConfigurationEquals('localization', 'amcl'), + launch_arguments = { + 'use_sim_time': use_sim_time, + 'params_file': nav2_params_file_dir, + 'map': nav2_map_dir}.items() + ), + + Node( + condition = LaunchConfigurationEquals('localization', 'icp'), + package='icp_localization_ros2', + executable='icp_localization', + output='screen', + parameters=[ + icp_node_params_dir, + {'use_sim_time': use_sim_time, + 'pcd_file_path': icp_pcd_dir, + 'icp_config_path': icp_config_dir, + 'input_filters_config_path': icp_input_filters_config_path} + ] + ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(navigation2_launch_dir, 'map_server_launch.py')), + condition = LaunchConfigurationNotEquals('localization', 'slam_toolbox'), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'map': nav2_map_dir, + 'params_file': nav2_params_file_dir, + 'container_name': 'nav2_container'}.items()) ] ) - - icp_delay_node = TimerAction( - period=1.5, - actions=[icp_node] + + start_mapping = Node( + condition = LaunchConfigurationEquals('mode', 'mapping'), + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + parameters=[ + slam_toolbox_mapping_file_dir, + {'use_sim_time': use_sim_time,} + ], ) start_navigation2 = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(navigation2_launch_dir, 'bringup_launch.py')), - launch_arguments={ - 'map': map_yaml_file, + PythonLaunchDescriptionSource(os.path.join(navigation2_launch_dir, 'bringup_rm_navigation.py')), + launch_arguments={ 'use_sim_time': use_sim_time, - 'params_file': nav2_params_file}.items() + 'map': nav2_map_dir, + 'params_file': nav2_params_file_dir}.items() ) start_common = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(bringup_dir, 'launch', 'common.py')) + PythonLaunchDescriptionSource(os.path.join(bringup_dir, 'launch', 'common.py')), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'rviz': use_rviz}.items() ) - ld = LaunchDescription() # Set environment variables ld.add_action(append_enviroment) # Declare the launch options + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(declare_mode_cmd) + ld.add_action(declare_localization_cmd) ld.add_action(start_rm_simulation) + ld.add_action(start_common) ld.add_action(pointcloud_downsampling_node) - ld.add_action(icp_delay_node) + ld.add_action(start_localization_group) + ld.add_action(start_mapping) ld.add_action(start_navigation2) - ld.add_action(start_common) return ld - \ No newline at end of file diff --git a/src/rm_bringup/launch/bringup_sim_mapping.launch.py b/src/rm_bringup/launch/bringup_sim_mapping.launch.py deleted file mode 100644 index fa22b6fbf742e4459b51d8d412bf1ca655d41d50..0000000000000000000000000000000000000000 --- a/src/rm_bringup/launch/bringup_sim_mapping.launch.py +++ /dev/null @@ -1,83 +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.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch.actions.append_environment_variable import AppendEnvironmentVariable - - -def generate_launch_description(): - # Get the launch directory - bringup_dir = get_package_share_directory('rm_bringup') - pb_rm_simulation_launch_dir = os.path.join(get_package_share_directory('pb_rm_simulation'), 'launch') - navigation2_launch_dir = os.path.join(get_package_share_directory('rm_navigation'), 'launch') - - # Select world (map file, world file share the same name prefix as the 'world' parameter) - world = LaunchConfiguration('world', default='RMUL') - - # Get the default map/config directory - map_dir = PathJoinSubstitution([bringup_dir, 'map', world]), ".yaml" - nav2_params_file_dir = os.path.join(bringup_dir, 'config', 'nav2_params.yaml') - slam_params_file_dir = os.path.join(bringup_dir, 'config', 'mapper_params_online_async.yaml') - - # Create the launch configuration variables - use_sim_time = LaunchConfiguration('use_sim_time', default='true') - use_rviz = LaunchConfiguration('rviz', default='false') # Visualize robot descrption - map_yaml_file = LaunchConfiguration('map', default=map_dir) - nav2_params_file = LaunchConfiguration('params_file', default=nav2_params_file_dir) - - # Set Gazebo plugin path - append_enviroment = AppendEnvironmentVariable( - 'GAZEBO_PLUGIN_PATH', - os.path.join(os.path.join(get_package_share_directory('pb_rm_simulation'), 'meshes', 'obstacles', 'obstacle_plugin', 'lib')) - ) - - # Specify the actions - start_rm_simulation = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(pb_rm_simulation_launch_dir, 'rm_simulation.launch.py')), - launch_arguments={ - 'use_sim_time': use_sim_time, - 'world': world, - 'rviz': use_rviz}.items() - ) - - start_async_slam_toolbox_node = Node( - package='slam_toolbox', - executable='async_slam_toolbox_node', - name='slam_toolbox', - parameters=[ - slam_params_file_dir, - {'use_sim_time': use_sim_time} - ], - output='screen' - ) - - start_navigation2 = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(navigation2_launch_dir, 'bringup_no_amcl_launch.py')), - launch_arguments={ - 'map': map_yaml_file, - 'use_sim_time': use_sim_time, - 'params_file': nav2_params_file}.items() - ) - - start_common = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(bringup_dir, 'launch', 'common.py')) - ) - - ld = LaunchDescription() - - # Set environment variables - ld.add_action(append_enviroment) - - # Declare the launch options - ld.add_action(start_rm_simulation) - ld.add_action(start_async_slam_toolbox_node) - ld.add_action(start_navigation2) - ld.add_action(start_common) - - return ld - \ No newline at end of file diff --git a/src/rm_bringup/map/RMUC.data b/src/rm_bringup/map/RMUC.data new file mode 100644 index 0000000000000000000000000000000000000000..8e5282bebc89298c751fe591bc199b69086f1756 Binary files /dev/null and b/src/rm_bringup/map/RMUC.data differ diff --git a/src/rm_bringup/map/RMUC.pgm b/src/rm_bringup/map/RMUC.pgm index 211a9eb0f3dea91b934e5bd88c53bc161a97eac3..1025a85c6e8c2c6a90fcc2b6d753bd60df5fb2f6 100644 Binary files a/src/rm_bringup/map/RMUC.pgm and b/src/rm_bringup/map/RMUC.pgm differ diff --git a/src/rm_bringup/map/RMUC.posegraph b/src/rm_bringup/map/RMUC.posegraph new file mode 100644 index 0000000000000000000000000000000000000000..fbb9c7c30c11db9caf601d6aa1b7847e6db25a2c Binary files /dev/null and b/src/rm_bringup/map/RMUC.posegraph differ diff --git a/src/rm_bringup/map/RMUC.yaml b/src/rm_bringup/map/RMUC.yaml index 6e2c7a0151f0fa59c84456ea3dd8ea2d4dc788bb..81ac4bd1f808070f7c11de4e9f22a8a8842072b0 100644 --- a/src/rm_bringup/map/RMUC.yaml +++ b/src/rm_bringup/map/RMUC.yaml @@ -1,7 +1,7 @@ image: RMUC.pgm mode: trinary resolution: 0.05 -origin: [-6.64, -10.3, 0] +origin: [-7.61, -9.74, 0] negate: 0 occupied_thresh: 0.65 -free_thresh: 0.25 +free_thresh: 0.25 \ No newline at end of file diff --git a/src/rm_bringup/map/RMUL.data b/src/rm_bringup/map/RMUL.data new file mode 100644 index 0000000000000000000000000000000000000000..5904df20e09a260e646413e8777174a07ebb3fd4 Binary files /dev/null and b/src/rm_bringup/map/RMUL.data differ diff --git a/src/rm_bringup/map/RMUL.posegraph b/src/rm_bringup/map/RMUL.posegraph new file mode 100644 index 0000000000000000000000000000000000000000..4ff3fd67517eac006ad7fd9a6fb9d65812977dc7 Binary files /dev/null and b/src/rm_bringup/map/RMUL.posegraph differ diff --git a/src/rm_localization/icp_localization_ros2/config/node_params.yaml b/src/rm_localization/icp_localization_ros2/config/node_params.yaml index e1bc09c26323e715fc6f821f033f4f17a5ebffdc..8881f0fd9a2895296bf97d9def9268cdf2ac9efd 100644 --- a/src/rm_localization/icp_localization_ros2/config/node_params.yaml +++ b/src/rm_localization/icp_localization_ros2/config/node_params.yaml @@ -1,6 +1,6 @@ /icp_localization: ros__parameters: - # pcd_file_path: "src/rm_simulation/pb_rm_simulation/meshes/RMUC2023_world/preview/RMUC.pcd" + # pcd_file_path: "src/rm_simulation/pb_rm_simulation/meshes/RMUC2024_world/preview/RMUC.pcd" pcd_file_path: "src/rm_simulation/pb_rm_simulation/meshes/auto_world/preview/RMUL.pcd" icp_localization_ros2: range_data_topic: "/cloud_registered_body" diff --git a/src/rm_localization/slam_toolbox/CMakeLists.txt b/src/rm_localization/slam_toolbox/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..3d58a3ea695117df05ed970998a048171992fcc6 --- /dev/null +++ b/src/rm_localization/slam_toolbox/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.5) +project(rm_slam_toolbox) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + config + launch + ) diff --git a/src/rm_localization/slam_toolbox/README.md b/src/rm_localization/slam_toolbox/README.md new file mode 100644 index 0000000000000000000000000000000000000000..06801ab750f8c8aa0e97c2800f86ae333eb7b7db --- /dev/null +++ b/src/rm_localization/slam_toolbox/README.md @@ -0,0 +1,370 @@ +## Slam Toolbox + +We've received feedback from users and have robots operating in the following environments with SLAM Toolbox: +- Retail +- Warehouses +- Libraries +- Research + +It is also the currently supported ROS2-SLAM library. See tutorials for working with it in [ROS2 Navigation here](https://navigation.ros.org/tutorials/docs/navigation2_with_slam.html). + +### Cite This Work + +You can find this work [here](https://joss.theoj.org/papers/10.21105/joss.02783) and clicking on the image below. + +> Macenski, S., Jambrecic I., "SLAM Toolbox: SLAM for the dynamic world", Journal of Open Source Software, 6(61), 2783, 2021. + +> Macenski, S., "On Use of SLAM Toolbox, A fresh(er) look at mapping and localization for the dynamic world", ROSCon 2019. + +[![IMAGE ALT TEXT](https://user-images.githubusercontent.com/14944147/74176653-f69beb80-4bec-11ea-906a-a233541a6064.png)](https://vimeo.com/378682207) + +# Introduction + +Slam Toolbox is a set of tools and capabilities for 2D SLAM built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101) while at [Simbe Robotics](https://www.simberobotics.com/), maintained while at Samsung Research, and largely in his free time. + +This project contains the ability to do most everything any other available SLAM library, both free and paid, and more. This includes: +- Ordinary point-and-shoot 2D SLAM mobile robotics folks expect (start, map, save pgm file) with some nice built in utilities like saving maps +- Continuing to refine, remap, or continue mapping a saved (serialized) pose-graph at any time +- life-long mapping: load a saved pose-graph continue mapping in a space while also removing extraneous information from newly added scans +- an optimization-based localization mode built on the pose-graph. Optionally run localization mode without a prior map for "lidar odometry" mode with local loop closures +- synchronous and asynchronous modes of mapping +- kinematic map merging (with an elastic graph manipulation merging technique in the works) +- plugin-based optimization solvers with a new optimized Google Ceres based plugin +- RVIZ plugin for interacting with the tools +- graph manipulation tools in RVIZ to manipulate nodes and connections during mapping +- Map serialization and lossless data storage +- ... more but those are the highlights + +For running on live production robots, I recommend using the snap: slam-toolbox, it has optimizations in it that make it about 10x faster. You need the deb/source install for the other developer level tools that don't need to be on the robot (rviz plugins, etc). + +This package has been benchmarked mapping building at 5x+ real-time up to about 30,000 sq. ft. and 3x real-time up to about 60,000 sq. ft. with the largest area (I'm aware of) used was a 200,000 sq. ft. building in synchronous mode (i.e. processing all scans, regardless of lag), and *much* larger spaces in asynchronous mode. + +The video below was collected at [Circuit Launch](https://www.circuitlaunch.com/) in Oakland, California. Thanks to [Silicon Valley Robotics](https://svrobo.org/) & Circuit Launch for being a testbed for some of this work. + +![map_image](/images/circuit_launch.gif?raw=true "Map Image") + +An overview of how the map was generated is presented below: +![slam_toolbox_sync_diagram](/images/slam_toolbox_sync.png) +1. ROS Node: SLAM toolbox is run in synchronous mode, which generates a ROS node. This node subscribes to laser scan and odometry topics, and publishes map to odom transform and a map. +2. Get odometry and LIDAR data: A callback for the laser topic will generate a pose (using odometry) and a laser scan tied at that node. These PosedScan objects form a queue, which are processed by the algorithm. +3. Process Data: The queue of PosedScan objects are used to construct a pose graph; odometry is refined using laser scan matching. This pose graph is used to compute robot pose, and find loop closures. If a loop closure is found, the pose graph is optimized, and pose estimates are updated. Pose estimates are used to compute and publish a map to odom transform for the robot. +4. Mapping: Laser scans associated with each pose in the pose graph are used to construct and publish a map. + +# Support and Contribution + +If you have any questions on use or configuration, please post your questions on [ROS Answers](answers.ros.org) and someone from the community will work their hardest to get back to you. Tangible issues in the codebase or feature requests should be made with GitHub issues. + +If you're interested in contributing to this project in a substantial way, please file a public GitHub issue on your new feature / patch. If for some reason the development of this feature is sensitive, please email the maintainers at their email addresses listed in the `package.xml` file. + +For all contributions, please properly fill in the GitHub issue and PR templates with all necessary context. All PRs must be passing CI and maintaining ABI compatibility within released ROS distributions. A maintainer will follow up shortly thereafter. + +# 03/23/2021 Note On Serialized Files + +As of 03/23/2021, the contents of the serialized files has changed. For all new users after this date, this regard this section it does not impact you. + +If you have previously existing serialized files (e.g. not `pgm` maps, but `.posegraph` serialized slam sessions), after this date, you may need to take some action to maintain current features. Unfortunately, an ABI breaking change was required to be made in order to fix a very large bug affecting any 360 or non-axially-mounted LIDAR system. + +[This Discourse post](https://discourse.ros.org/t/request-for-input-potential-existing-slam-toolbox-serialized-file-invalidation/19520) highlights the issues. The frame storing the scan data for the optimizer was incorrect leading to explosions or flipping of maps for 360 and non-axially-aligned robots when using conservative loss functions. This change permanently fixes this issue, however it changes the frame of reference that this data is stored and serialized in. If your system as a non-360 lidar and it is mounted with its frame aligned with the robot base frame, you're unlikely to notice a problem and can disregard this statement. For all others noticing issues, you have the following options: +- Use the `-devel-unfixed` branch rather than `-devel`, which contains the unfixed version of this distribution's release which will be maintained in parallel to the main branches to have an option to continue with your working solution +- Convert your serialized files into the new reference frame with an offline utility +- Take the raw data and rerun the SLAM sessions to get a new serialized file with the right content + +More of the conversation can be seen on tickets #198 and #281. I apologize for the inconvenience, however this solves a very large bug that was impacting a large number of users. I've worked hard to make sure there's a viable path forward for everyone. + +# LifeLong Mapping + + + +LifeLong mapping is the concept of being able to map a space, completely or partially, and over time, refine and update that map as you continue to interact with the space. Our approach implements this and also takes care to allow for the application of operating in the cloud, as well as mapping with many robots in a shared space (cloud distributed mapping). While Slam Toolbox can also just be used for a point-and-shoot mapping of a space and saving that map as a .pgm file as maps are traditionally stored in, it also allows you to save the pose-graph and metadata losslessly to reload later with the same or different robot and continue to map the space. + +Our lifelong mapping consists of a few key steps +- Serialization and Deserialization to store and reload map information +- KD-Tree search matching to locate the robot in its position on reinitialization +- pose-graph optimization based SLAM with 2D scan matching abstraction + +This will allow the user to create and update existing maps, then serialize the data for use in other mapping sessions, something sorely lacking from most SLAM implementations and nearly all planar SLAM implementations. Other good libraries that do this include RTab-Map and Cartographer, though they themselves have their own quirks that make them (in my opinion) unusable for production robotics applications. This library provides the mechanics to save not only the data, but the pose graph, and associated metadata to work with. This has been used to create maps by merging techniques (taking 2 or more serialized objects and creating 1 globally consistent one) as well as continuous mapping techniques (updating 1, same, serialized map object over time and refining it). The major benefit of this over RTab-Map or Cartographer is the maturity of the underlying (but heavily modified) `open_karto` library the project is based on. The scan matcher of Karto is well known as an extremely good matcher for 2D laser scans and modified versions of Karto can be found in companies across the world. + +Slam Toolbox supports all the major modes: +- Starting from a predefined dock (assuming to be near start region) +- Starting at any particular node - select a node ID to start near +- Starting in any particular area - indicate current pose in the map frame to start at, like AMCL + +In the RVIZ interface (see section below) you'll be able to re-localize in a map or continue mapping graphically or programmatically using ROS services. + +On time of writing: there a **highly** experimental implementation of what I call "true lifelong" mapping that does support the method for removing nodes over time as well as adding nodes, this results in a true ability to map for life since the computation is bounded by removing extraneous or outdated information. Its recommended to run the non-full LifeLong mapping mode in the cloud for the increased computational burdens if you'd like to be continuously refining a map. However a real and desperately needed application of this is to have multi-session mapping to update just a section of the map or map half an area at a time to create a full (and then static) map for AMCL or Slam Toolbox localization mode, which this will handle in spades. The immediate plan is to create a mode within LifeLong mapping to decay old nodes to bound the computation and allow it to run on the edge by refining the experimental node. Continuing mapping (lifelong) should be used to build a complete map then switch to the pose-graph deformation localization mode until node decay is implemented, and **you should not see any substantial performance impacts**. + + +# Localization + + + +Localization mode consists of 3 things: +- Loads existing serialized map into the node +- Maintains a rolling buffer of recent scans in the pose-graph +- After expiring from the buffer scans are removed and the underlying map is not affected + +Localization methods on image map files has been around for years and works relatively well. There has not been a great deal of work in academia to refine these algorithms to a degree that satisfies me. However SLAM is a rich and well benchmarked topic. The inspiration of this work was the concept of "Can we make localization, SLAM again?" such that we can take advantage of all the nice things about SLAM for localization, but remove the unbounded computational increase. + +To enable, set `mode: localization` in the configuration file to allow for the Ceres plugin to set itself correctly to be able to quickly add *and remove* nodes and constraints from the pose graph, but isn't strictly required, but a performance optimization. The localization mode will automatically load your pose graph, take the first scan and match it against the local area to further refine your estimated position, and start localizing. + +To minimize the amount of changes required for moving to this mode over AMCL, we also expose a subscriber to the `/initialpose` topic used by AMCL to relocalize to a position, which also hooks up to the `2D Pose Estimation` tool in RVIZ. This way you can enter localization mode with our approach but continue to use the same API as you expect from AMCL for ease of integration. + +In summary, this approach I dub `elastic pose-graph localization` is where we take existing map pose-graphs and localized with-in them with a rolling window of recent scans. This way we can localize in an existing map using the scan matcher, but not update the underlaying map long-term should something go wrong. It can be considered a replacement to AMCL and results is not needing any .pgm maps ever again. The lifelong mapping/continuous slam mode above will do better if you'd like to modify the underlying graph while moving. This method of localization might not be suitable for all applications, it does require quite a bit of tuning for your particular robot and needs high quality odometry. If in doubt, you're always welcome to use other 2D map localizers in the ecosystem like AMCL. For most beginners or users looking for a good out of the box experience, I'd recommend AMCL. + +## Tools + +### Plugin based Optimizers + +I have created a pluginlib interface for the ScanSolver abstract class so that you can change optimizers on runtime to test many different ones if you like. + +Then I generated plugins for a few different solvers that people might be interested in. I like to swap them out for benchmarking and make sure its the same code running for all. I have supported Ceres, G2O, SPA, and GTSAM. + +GTSAM/G2O/SPA is currently "unsupported" although all the code is there. They don't outperform Ceres settings I describe below so I stopped compiling them to save on build time, but they're there and work if you would like to use them. PRs to implement other optimizer plugins are welcome. + +### Map Merging - Example uses of serialized raw data & posegraphs + +#### Kinematic + +This uses RVIZ and the plugin to load any number of posegraphs that will show up in RVIZ under `map_N` and a set of interactive markers to allow you to move them around. Once you have them all positioned relative to each other in the way you like, you can merge the submaps into a global `map` which can be downloaded with your map server implementation of choice. + +It's more of a demonstration of other things you can do once you have the raw data to work with, but I don't suspect many people will get much use out of it unless you're used to stitching maps by hand. + +More information in the RVIZ Plugin section below. + +### RVIZ Plugin + +An rviz plugin is furnished to help with manual loop closures and online / offline mapping. By default interactive mode is off (allowing you to move nodes) as this takes quite a toll on rviz. When you want to move nodes, tick the interactive box, move what you want, and save changes to prompt a manual loop closure. Clear if you made a mistake. When done, exit interactive mode again. + +There's also a tool to help you control online and offline data. You can at any time stop processing new scans or accepting new scans into the queue. This is desirable when you want to allow the package to catch up while the robot sits still (**This option is only meaningful in synchronous mode. In asynchronous mode the robot will never fall behind.**) or you want to stop processing new scans while you do a manual loop closure / manual "help". If there's more in the queue than you want, you may also clear it. + +Additionally there's exposed buttons for the serialization and deserialization services to load an old pose-graph to update and refine, or continue mapping, then save back to file. The "Start By Dock" checkbox will try to scan match against the first node (assuming you started at your dock) to give you an odometry estimate to start with. Another option is to start using an inputted position in the GUI or by calling the underlying service. Additionally, you can use the current odometric position estimation if you happened to have just paused the robot or not moved much between runs. Finally (and most usefully), you can use the RVIZ tool for **2D Pose Estimation** to tell it where to go in **localization mode** just like AMCL. + +Additionally the RVIZ plugin will allow you to add serialized map files as submaps in RVIZ. They will be displayed with an interactive marker you can translate and rotate to match up, then generate a composite map with the Generate Map button. At that point the composite map is being broadcasted on the `/map` topic and you can save it with the `map_saver`. + +It's recommended to always continue mapping near the dock, if that's not possible, look into the starting from pose or map merging techniques. This RVIZ plugin is mostly here as a debug utility, but if you often find yourself mapping areas using rviz already, I'd just have it open. All the RVIZ buttons are implemented using services that a master application can control. + +The interface is shown below. + +![rviz_plugin](/images/rviz_plugin.png?raw=true "Rviz Plugin") + +### Graph Manipulation + +By enabling `Interactive Mode`, the graph nodes will change from markers to interactive markers which you can manipulate. When you move a node(s), you can Save Changes and it will send the updated position to the pose-graph and cause an optimization run to occur to change the pose-graph with your new node location. This is helpful if the robot gets pushed, slips, runs into a wall, or otherwise has drifting odometry and you would like to manually correct it. + +When a map is sufficiently large, the number of interactive markers in RVIZ may be too large and RVIZ may start to lag. I only recommend using this feature as a testing debug tool and not for production. However if you are able to make it work with 10,000 interactive markers, I'll merge that PR in a heartbeat. Otherwise I'd restrict the use of this feature to small maps or with limited time to make a quick change and return to static mode by unchecking the box. + +## Metrics + +If you're a weirdo like me and you want to see how I came up with the settings I had for the Ceres optimizer, see below. + +![ceres_solver_comparison](https://user-images.githubusercontent.com/14944147/41576505-a6802d76-733c-11e8-8eca-334da2c8bd50.png) + +The data sets present solve time vs number of nodes in the pose graph on a large dataset, as that is not open source, but suffice to say that the settings I recommend work well. I think anyone would be hard set in a normal application to exceed or find that another solver type is better (that super low curve on the bottom one, yeah, that's it). Benchmark on a low power 7th gen i7 machine. + +It can map _very_ large spaces with reasonable CPU and memory consumption. My default settings increase O(N) on number of elements in the pose graph. I recommend from extensive testing to use the `SPARSE_NORMAL_CHOLESKY` solver with Ceres and the `SCHUR_JACOBI` preconditioner. Using `LM` at the trust region strategy is comparable to the dogleg subspace strategy, but `LM` is much better supported so why argue with it. + +You can get away without a loss function if your odometry is good (i.e. likelihood for outliers is extremely low). If you have an abnormal application or expect wheel slippage, I might recommend a `HuberLoss` function, which is a really good catch-all loss function if you're looking for a place to start. All these options and more are available from the ROS parameter server. + +# API + +The following are the services/topics that are exposed for use. See the rviz plugin for an implementation of their use. + +## Subscribed topics + +| /scan | `sensor_msgs/LaserScan` | the input scan from your laser to utilize | +|-----|----|----| +| **tf** | N/A | a valid transform from your configured odom_frame to base_frame | + +## Published topics + +| Topic | Type | Description | +|-----|----|----| +| map | `nav_msgs/OccupancyGrid` | occupancy grid representation of the pose-graph at `map_update_interval` frequency | +| pose | `geometry_msgs/PoseWithCovarianceStamped` | pose of the base_frame in the configured map_frame along with the covariance calculated from the scan match | + +## Exposed Services + +| Topic | Type | Description | +|-----|----|----| +| `/slam_toolbox/clear_changes` | `slam_toolbox/Clear` | Clear all manual pose-graph manipulation changes pending | +| `/slam_toolbox/deserialize_map` | `slam_toolbox/DeserializePoseGraph` | Load a saved serialized pose-graph files from disk | +| `/slam_toolbox/dynamic_map` | `nav_msgs/OccupancyGrid` | Request the current state of the pose-graph as an occupancy grid | +| `/slam_toolbox/manual_loop_closure` | `slam_toolbox/LoopClosure` | Request the manual changes to the pose-graph pending to be processed | +| `/slam_toolbox/pause_new_measurements` | `slam_toolbox/Pause` | Pause processing of new incoming laser scans by the toolbox | +| `/slam_toolbox/save_map` | `slam_toolbox/SaveMap` | Save the map image file of the pose-graph that is useable for display or AMCL localization. It is a simple wrapper on `map_server/map_saver` but is useful. | +| `/slam_toolbox/serialize_map` | `slam_toolbox/SerializePoseGraph` | Save the map pose-graph and datathat is useable for continued mapping, slam_toolbox localization, offline manipulation, and more | +| `/slam_toolbox/toggle_interactive_mode` | `slam_toolbox/ToggleInteractive` | Toggling in and out of interactive mode, publishing interactive markers of the nodes and their positions to be updated in an application | + +# Configuration + +The following settings and options are exposed to you. My default configuration is given in `config` directory. + +## Solver Params + +`solver_plugin` - The type of nonlinear solver to utilize for karto's scan solver. Options: `solver_plugins::CeresSolver`, `solver_plugins::SpaSolver`, `solver_plugins::G2oSolver`. Default: `solver_plugins::CeresSolver`. + +`ceres_linear_solver` - The linear solver for Ceres to use. Options: `SPARSE_NORMAL_CHOLESKY`, `SPARSE_SCHUR`, `ITERATIVE_SCHUR`, `CGNR`. Defaults to `SPARSE_NORMAL_CHOLESKY`. + +`ceres_preconditioner` - The preconditioner to use with that solver. Options: `JACOBI`, `IDENTITY` (none), `SCHUR_JACOBI`. Defaults to `JACOBI`. + +`ceres_trust_strategy` - The trust region strategy. Line search strategies are not exposed because they perform poorly for this use. Options: `LEVENBERG_MARQUARDT`, `DOGLEG`. Default: `LEVENBERG_MARQUARDT`. + +`ceres_dogleg_type` - The dogleg strategy to use if the trust strategy is `DOGLEG`. Options: `TRADITIONAL_DOGLEG`, `SUBSPACE_DOGLEG`. Default: `TRADITIONAL_DOGLEG` + +`ceres_loss_function` - The type of loss function to reject outlier measurements. None is equatable to a squared loss. Options: `None`, `HuberLoss`, `CauchyLoss`. Default: `None`. + +`mode` - "mapping" or "localization" mode for performance optimizations in the Ceres problem creation + +## Toolbox Params + +`odom_frame` - Odometry frame + +`map_frame` - Map frame + +`base_frame` - Base frame + +`scan_topic` - scan topic, *absolute* path, i.e. `/scan` not `scan` + +`scan_queue_size` - The number of scan messages to queue up before throwing away old ones. Should always be set to 1 in async mode + +`use_map_saver` - Instantiate the map saver service and self-subscribe to the map topic + +`map_file_name` - Name of the pose-graph file to load on startup if available + +`map_start_pose` - Pose to start pose-graph mapping/localization in, if available + +`map_start_at_dock` - Starting pose-graph loading at the dock (first node), if available. If both pose and dock are set, it will use pose + +`debug_logging` - Change logger to debug + +`throttle_scans` - Number of scans to throttle in synchronous mode + +`transform_publish_period` - The map to odom transform publish period. 0 will not publish transforms + +`map_update_interval` - Interval to update the 2D occupancy map for other applications / visualization + +`enable_interactive_mode` - Whether or not to allow for interactive mode to be enabled. Interactive mode will retain a cache of laser scans mapped to their ID for visualization in interactive mode. As a result the memory for the process will increase. This is manually disabled in localization and lifelong modes since they would increase the memory utilization over time. Valid for either mapping or continued mapping modes. + +`position_covariance_scale` - Amount to scale position covariance when publishing pose from scan match. This can be used to tune the influence of the pose position in a downstream localization filter. The covariance represents the uncertainty of the measurement, so scaling up the covariance will result in the pose position having less influence on downstream filters. Default: 1.0 + +`yaw_covariance_scale` - Amount to scale yaw covariance when publishing pose from scan match. See description of position_covariance_scale. Default: 1.0 + +`resolution` - Resolution of the 2D occupancy map to generate + +`max_laser_range` - Maximum laser range to use for 2D occupancy map rasterizing + +`minimum_time_interval` - The minimum duration of time between scans to be processed in synchronous mode + +`transform_timeout` - TF timeout for looking up transforms + +`tf_buffer_duration` - Duration to store TF messages for lookup. Set high if running offline at multiple times speed in synchronous mode. + +`stack_size_to_use` - The number of bytes to reset the stack size to, to enable serialization/deserialization of files. A liberal default is 40000000, but less is fine. + +`minimum_travel_distance` - Minimum distance of travel before processing a new scan + +## Matcher Params + +`use_scan_matching` - whether to use scan matching to refine odometric pose (uh, why would you not?) + +`use_scan_barycenter` - Whether to use the barycenter or scan pose + +`minimum_travel_heading` - Minimum changing in heading to justify an update + +`scan_buffer_size` - The number of scans to buffer into a chain, also used as the number of scans in the circular buffer of localization mode + +`scan_buffer_maximum_scan_distance` - Maximum distance of a scan from the pose before removing the scan from the buffer + +`link_match_minimum_response_fine` - The threshold link matching algorithm response for fine resolution to pass + +`link_scan_maximum_distance` - Maximum distance between linked scans to be valid + +`loop_search_maximum_distance` - Maximum threshold of distance for scans to be considered for loop closure + +`do_loop_closing` - Whether to do loop closure (if you're not sure, the answer is "true") + +`loop_match_minimum_chain_size` - The minimum chain length of scans to look for loop closure + +`loop_match_maximum_variance_coarse` - The threshold variance in coarse search to pass to refine + +`loop_match_minimum_response_coarse` - The threshold response of the loop closure algorithm in coarse search to pass to refine + +`loop_match_minimum_response_fine` - The threshold response of the loop closure algorithm in fine search to pass to refine + +`correlation_search_space_dimension` - Search grid size to do scan correlation over + +`correlation_search_space_resolution` - Search grid resolution to do scan correlation over + +`correlation_search_space_smear_deviation` - Amount of multimodal smearing to smooth out responses + +`loop_search_space_dimension` - Size of the search grid over the loop closure algorithm + +`loop_search_space_resolution` - Search grid resolution to do loop closure over + +`loop_search_space_smear_deviation` - Amount of multimodal smearing to smooth out responses + +`distance_variance_penalty` - A penalty to apply to a matched scan as it differs from the odometric pose + +`angle_variance_penalty` - A penalty to apply to a matched scan as it differs from the odometric pose + +`fine_search_angle_offset` - Range of angles to test for fine scan matching + +`coarse_search_angle_offset` - Range of angles to test for coarse scan matching + +`coarse_angle_resolution` - Resolution of angles over the Offset range to test in scan matching + +`minimum_angle_penalty` - Smallest penalty an angle can have to ensure the size doesn't blow up + +`minimum_distance_penalty` - Smallest penalty a scan can have to ensure the size doesn't blow up + +`use_response_expansion` - Whether to automatically increase the search grid size if no viable match is found + +# Install + +ROSDep will take care of the major things + +``` +rosdep install -q -y -r --from-paths src --ignore-src +``` + +Or install via apt + +``` +apt install ros-eloquent-slam-toolbox +``` + +Run your colcon build procedure of choice. + +You can run via `ros2 launch slam_toolbox online_sync_launch.py` + +# Etc + +## NanoFlann! + +In order to do some operations quickly for continued mapping and localization, I make liberal use of NanoFlann (shout out!). + + +## Brief incursion into snaps + +Snap are completely isolated containerized packages that one can run through the Canonical organization on a large number of Linux distributions. They're similar to Docker containers but it doesn't share the kernel or any of the libraries, and rather has everything internal as essentially a separate partitioned operating system based on Ubuntu Core. + +We package up slam toolbox in this way for a nice multiple-on speed up in execution from a couple of pretty nuanced reasons in this particular project, but generally speaking you shouldn't expect a speedup from a snap. + +Since Snaps are totally isolated and there's no override flags like in Docker, there's only a couple of fixed directories that both the snap and the host system can write and read from, including SNAP_COMMON (usually in `/var/snap/[snap name]/common`). Therefore, this is the place that if you're serializing and deserializing maps, you need to have them accessible to that directory. + +You can optionally store all your serialized maps there, move maps there as needed, take maps from there after serialization, or do my favorite option and `link` the directories with `ln` to where ever you normally store your maps and you're wanting to dump your serialized map files. + +Example of `ln`: +``` +# Source Linked +sudo ln -s /home/steve/maps/serialized_map/ /var/snap/slam-toolbox/common +``` + +and then all you have to do when you specify a map to use is set the filename to `slam-toolbox/map_name` and it should work no matter if you're running in a snap, docker, or on bare metal. The `-s` makes a symbol link so rather than `/var/snap/slam-toolbox/common/*` containing the maps, `/var/snap/slam-toolbox/common/serialized_map/*` will. By default on bare metal, the maps will be saved in `.ros` + + +## More Gifs! + +![map_image](/images/mapping_steves_apartment.gif?raw=true "Map Image") + +If someone from iRobot can use this to tell me my Roomba serial number by correlating to its maps, I'll buy them lunch and probably try to hire them. diff --git a/src/rm_localization/slam_toolbox/config/mapper_params_lifelong.yaml b/src/rm_localization/slam_toolbox/config/mapper_params_lifelong.yaml new file mode 100644 index 0000000000000000000000000000000000000000..364fdd5e47a3a5bd55f778e6843b9467942fcbf2 --- /dev/null +++ b/src/rm_localization/slam_toolbox/config/mapper_params_lifelong.yaml @@ -0,0 +1,84 @@ + +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + use_map_saver: true + mode: mapping + + # lifelong params + lifelong_search_use_tree: false + lifelong_minimum_score: 0.1 + lifelong_iou_match: 0.85 + lifelong_node_removal_score: 0.04 + lifelong_overlap_score_scale: 0.06 + lifelong_constraint_multiplier: 0.08 + lifelong_nearby_penalty: 0.001 + lifelong_candidates_scale: 0.03 + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + #map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 10. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/src/rm_localization/slam_toolbox/config/mapper_params_localization.yaml b/src/rm_localization/slam_toolbox/config/mapper_params_localization.yaml new file mode 100644 index 0000000000000000000000000000000000000000..1c73d5d87d3700f1322c218bc517bb577aa1f77b --- /dev/null +++ b/src/rm_localization/slam_toolbox/config/mapper_params_localization.yaml @@ -0,0 +1,67 @@ +slam_toolbox: + ros__parameters: + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + mode: localization #mapping + + # if you'd like to start localizing on bringup in a map and pose + # map_file_name: /xxx/xxx + # map_start_pose: [0.0, 0.0, 0.0] + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 3 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + do_loop_closing: true + loop_match_minimum_chain_size: 3 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + loop_search_maximum_distance: 3.0 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/src/rm_localization/slam_toolbox/config/mapper_params_offline.yaml b/src/rm_localization/slam_toolbox/config/mapper_params_offline.yaml new file mode 100644 index 0000000000000000000000000000000000000000..2110fa067f56d3be56ec0f4e81dc51dc904a0bc0 --- /dev/null +++ b/src/rm_localization/slam_toolbox/config/mapper_params_offline.yaml @@ -0,0 +1,66 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + use_map_saver: true + mode: mapping #localization + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 10.0 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 14400. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/src/rm_localization/slam_toolbox/config/mapper_params_online_async.yaml b/src/rm_localization/slam_toolbox/config/mapper_params_online_async.yaml new file mode 100644 index 0000000000000000000000000000000000000000..ca49f6890d65fbe473b37f2dd4d089edb1ebaaa6 --- /dev/null +++ b/src/rm_localization/slam_toolbox/config/mapper_params_online_async.yaml @@ -0,0 +1,74 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + use_map_saver: true + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + # map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/src/rm_localization/slam_toolbox/config/mapper_params_online_sync.yaml b/src/rm_localization/slam_toolbox/config/mapper_params_online_sync.yaml new file mode 100644 index 0000000000000000000000000000000000000000..7ab41b7a68cfbabc0811dba9fa6cb3e84b9aaacd --- /dev/null +++ b/src/rm_localization/slam_toolbox/config/mapper_params_online_sync.yaml @@ -0,0 +1,74 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + use_map_saver: true + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + #map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/src/rm_localization/slam_toolbox/config/slam_toolbox_default.rviz b/src/rm_localization/slam_toolbox/config/slam_toolbox_default.rviz new file mode 100644 index 0000000000000000000000000000000000000000..c079a07cd472e5e203e2019c43989ebfb95074dd --- /dev/null +++ b/src/rm_localization/slam_toolbox/config/slam_toolbox_default.rviz @@ -0,0 +1,137 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 154 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: slam_toolbox::SlamToolboxPlugin + Name: SlamToolboxPlugin +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000217000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000125000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000220053006c0061006d0054006f006f006c0062006f00780050006c007500670069006e0100000168000001850000018500ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000017e000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + SlamToolboxPlugin: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 72 + Y: 60 diff --git a/src/rm_localization/slam_toolbox/launch/lifelong_launch.py b/src/rm_localization/slam_toolbox/launch/lifelong_launch.py new file mode 100644 index 0000000000000000000000000000000000000000..f9d2d60f5ccdc487f6ac954eac89697982e70723 --- /dev/null +++ b/src/rm_localization/slam_toolbox/launch/lifelong_launch.py @@ -0,0 +1,17 @@ +from launch import LaunchDescription +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + return LaunchDescription([ + launch_ros.actions.Node( + parameters=[ + get_package_share_directory("rm_slam_toolbox") + '/config/mapper_params_lifelong.yaml' + ], + package='slam_toolbox', + executable='lifelong_slam_toolbox_node', + name='slam_toolbox', + output='screen' + ) + ]) diff --git a/src/rm_localization/slam_toolbox/launch/localization_launch.py b/src/rm_localization/slam_toolbox/launch/localization_launch.py new file mode 100644 index 0000000000000000000000000000000000000000..295d1dda1a8a36fc57042b9ae3d1f7b343457d0a --- /dev/null +++ b/src/rm_localization/slam_toolbox/launch/localization_launch.py @@ -0,0 +1,17 @@ +from launch import LaunchDescription +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + return LaunchDescription([ + launch_ros.actions.Node( + parameters=[ + get_package_share_directory("rm_slam_toolbox") + '/config/mapper_params_localization.yaml' + ], + package='slam_toolbox', + executable='localization_slam_toolbox_node', + name='slam_toolbox', + output='screen' + ) + ]) diff --git a/src/rm_localization/slam_toolbox/launch/merge_maps_kinematic_launch.py b/src/rm_localization/slam_toolbox/launch/merge_maps_kinematic_launch.py new file mode 100644 index 0000000000000000000000000000000000000000..325d2a83956ebf744646f382318661c282517d61 --- /dev/null +++ b/src/rm_localization/slam_toolbox/launch/merge_maps_kinematic_launch.py @@ -0,0 +1,13 @@ +from launch import LaunchDescription +import launch_ros.actions + + +def generate_launch_description(): + return LaunchDescription([ + launch_ros.actions.Node( + package='slam_toolbox', + executable='merge_maps_kinematic', + name='slam_toolbox', + output='screen' + ) + ]) diff --git a/src/rm_localization/slam_toolbox/launch/offline_launch.py b/src/rm_localization/slam_toolbox/launch/offline_launch.py new file mode 100644 index 0000000000000000000000000000000000000000..507c2f177cfec27860a347c1061a11529219ff96 --- /dev/null +++ b/src/rm_localization/slam_toolbox/launch/offline_launch.py @@ -0,0 +1,17 @@ +from launch import LaunchDescription +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + return LaunchDescription([ + launch_ros.actions.Node( + parameters=[ + get_package_share_directory("rm_slam_toolbox") + '/config/mapper_params_offline.yaml' + ], + package='slam_toolbox', + executable='sync_slam_toolbox_node', + name='slam_toolbox', + output='screen' + ) + ]) diff --git a/src/rm_navigation/launch/online_async_launch.py b/src/rm_localization/slam_toolbox/launch/online_async_launch.py similarity index 94% rename from src/rm_navigation/launch/online_async_launch.py rename to src/rm_localization/slam_toolbox/launch/online_async_launch.py index ec901701850bdacb6143fdc7d8c67dc4e6f988d9..f2e40556e514591462802e98adacd202126aacec 100644 --- a/src/rm_navigation/launch/online_async_launch.py +++ b/src/rm_localization/slam_toolbox/launch/online_async_launch.py @@ -17,8 +17,8 @@ def generate_launch_description(): description='Use simulation/Gazebo clock') declare_slam_params_file_cmd = DeclareLaunchArgument( 'slam_params_file', - default_value=os.path.join(get_package_share_directory("rm_navigation"), - 'params', 'mapper_params_online_async.yaml'), + default_value=os.path.join(get_package_share_directory("rm_slam_toolbox"), + 'config', 'mapper_params_online_async.yaml'), description='Full path to the ROS2 parameters file to use for the slam_toolbox node') start_async_slam_toolbox_node = Node( diff --git a/src/rm_localization/slam_toolbox/launch/online_sync_launch.py b/src/rm_localization/slam_toolbox/launch/online_sync_launch.py new file mode 100644 index 0000000000000000000000000000000000000000..dc351248f5b963ea3cd16f982c4576bcedabf530 --- /dev/null +++ b/src/rm_localization/slam_toolbox/launch/online_sync_launch.py @@ -0,0 +1,40 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time') + slam_params_file = LaunchConfiguration('slam_params_file') + + declare_use_sim_time_argument = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation/Gazebo clock') + declare_slam_params_file_cmd = DeclareLaunchArgument( + 'slam_params_file', + default_value=os.path.join(get_package_share_directory("rm_slam_toolbox"), + 'config', 'mapper_params_online_sync.yaml'), + description='Full path to the ROS2 parameters file to use for the slam_toolbox node') + + start_sync_slam_toolbox_node = Node( + parameters=[ + slam_params_file, + {'use_sim_time': use_sim_time} + ], + package='slam_toolbox', + executable='sync_slam_toolbox_node', + name='slam_toolbox', + output='screen') + + ld = LaunchDescription() + + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_slam_params_file_cmd) + ld.add_action(start_sync_slam_toolbox_node) + + return ld diff --git a/src/rm_localization/slam_toolbox/package.xml b/src/rm_localization/slam_toolbox/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..b57f44ca4e7c508fd9c11ad2caff7f312b9b114d --- /dev/null +++ b/src/rm_localization/slam_toolbox/package.xml @@ -0,0 +1,22 @@ + + + + rm_slam_toolbox + 2.6.6 + + This package provides a sped up improved slam karto with updated SDK and visualization and modification toolsets + + Steve Macenski + Michel Hidalgo + LGPL + Steve Macenski + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/rm_navigation/launch/bringup_no_amcl_launch.py b/src/rm_navigation/launch/bringup_rm_navigation.py similarity index 74% rename from src/rm_navigation/launch/bringup_no_amcl_launch.py rename to src/rm_navigation/launch/bringup_rm_navigation.py index 94e3da22b6d67e56884ea4e0250b822436e7ab09..4433199c3cc226172b3eee1e823e697bd0887bde 100644 --- a/src/rm_navigation/launch/bringup_no_amcl_launch.py +++ b/src/rm_navigation/launch/bringup_rm_navigation.py @@ -1,17 +1,3 @@ -# Copyright (c) 2018 Intel Corporation -# -# 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. - import os from ament_index_python.packages import get_package_share_directory @@ -35,21 +21,15 @@ def generate_launch_description(): # Create the launch configuration variables namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') - slam = LaunchConfiguration('slam') map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') + slam_params_file = LaunchConfiguration('slam_params_file') autostart = LaunchConfiguration('autostart') use_composition = LaunchConfiguration('use_composition') use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') - # Map fully qualified names to relative ones so the node's namespace can be prepended. - # In case of the transforms (tf), currently, there doesn't seem to be a better alternative - # https://github.com/ros/geometry2/issues/32 - # https://github.com/ros/robot_state_publisher/pull/30 - # TODO(orduno) Substitute with `PushNodeRemapping` - # https://github.com/ros2/launch_ros/issues/56 remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] @@ -77,19 +57,25 @@ def generate_launch_description(): default_value='false', description='Whether to apply a namespace to the navigation stack') - declare_slam_cmd = DeclareLaunchArgument( - 'slam', - default_value='False', + declare_use_slam_cmd = DeclareLaunchArgument( + 'use_slam', + default_value='True', description='Whether run a SLAM') + declare_use_amcl_cmd = DeclareLaunchArgument( + 'use_amcl', + default_value='True', + description='Whether run AMCL (only affect when navigating with a map)') + declare_map_yaml_cmd = DeclareLaunchArgument( 'map', - default_value= os.path.join(bringup_dir,'map','rmuc_map_draw.yaml'), + # default_value= os.path.join(bringup_dir,'map', 'RMUC.yaml'), + default_value= os.path.join(bringup_dir,'map', 'RMUL.yaml'), description='Full path to map yaml file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', - default_value='true', + default_value='True', description='Use simulation (Gazebo) clock if true') declare_params_file_cmd = DeclareLaunchArgument( @@ -97,8 +83,14 @@ def generate_launch_description(): default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), description='Full path to the ROS2 parameters file to use for all launched nodes') + declare_slam_params_file_cmd = DeclareLaunchArgument( + 'slam_params_file', + default_value=os.path.join(get_package_share_directory("rm_navigation"), + 'params', 'mapper_params_online_async.yaml'), + description='Full path to the ROS2 parameters file to use for the slam_toolbox node') + declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='true', + 'autostart', default_value='True', description='Automatically startup the nav2 stack') declare_use_composition_cmd = DeclareLaunchArgument( @@ -106,7 +98,7 @@ def generate_launch_description(): description='Whether to use composed bringup') declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='False', + 'use_respawn', default_value='True', description='Whether to respawn if a node crashes. Applied when composition is disabled.') declare_log_level_cmd = DeclareLaunchArgument( @@ -123,46 +115,64 @@ def generate_launch_description(): condition=IfCondition(use_composition), name='nav2_container', package='rclcpp_components', - executable='component_container_isolated', + executable='component_container_mt', parameters=[configured_params, {'autostart': autostart}], arguments=['--ros-args', '--log-level', log_level], remappings=remappings, output='screen'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')), + launch_arguments={'namespace': namespace, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'params_file': params_file, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container'}.items()), + # IncludeLaunchDescription( - # PythonLaunchDescriptionSource(os.path.join(launch_dir, 'slam_launch.py')), - # condition=IfCondition(slam), + # PythonLaunchDescriptionSource(os.path.join(launch_dir, + # 'localization_launch.py')), + # # condition=IfCondition(PythonExpression(['not ', use_slam])), + # condition = IfCondition(PythonExpression([mode, " == 'nav' and ", localization, " == 'amcl'"])), # launch_arguments={'namespace': namespace, + # 'map': map_yaml_file, # 'use_sim_time': use_sim_time, # 'autostart': autostart, + # 'params_file': params_file, + # 'use_amcl': use_amcl, + # 'use_composition': use_composition, # 'use_respawn': use_respawn, - # 'params_file': params_file}.items()), + # 'container_name': 'nav2_container'}.items()), # IncludeLaunchDescription( # PythonLaunchDescriptionSource(os.path.join(launch_dir, - # 'localization_launch.py')), - # condition=IfCondition(PythonExpression(['not ', slam])), + # 'localization_slam_toolbox_launch.py')), + # # condition=IfCondition(use_slam), # launch_arguments={'namespace': namespace, - # 'map': map_yaml_file, # 'use_sim_time': use_sim_time, # 'autostart': autostart, - # 'params_file': params_file, + # 'slam_params_file': slam_params_file, # 'use_composition': use_composition, # 'use_respawn': use_respawn, # 'container_name': 'nav2_container'}.items()), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')), - launch_arguments={'namespace': namespace, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'params_file': params_file, - 'use_composition': use_composition, - 'use_respawn': use_respawn, - 'container_name': 'nav2_container'}.items()), + # Node( + # condition=IfCondition(use_slam), + # package='slam_toolbox', + # executable='async_slam_toolbox_node', + # name='slam_toolbox', + # parameters=[ + # slam_params_file, + # {'use_sim_time': use_sim_time} + # ], + # output='screen' + # ), + IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')) - ), + ), ]) # Create the launch description and populate @@ -174,10 +184,12 @@ def generate_launch_description(): # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) - ld.add_action(declare_slam_cmd) + ld.add_action(declare_use_slam_cmd) + ld.add_action(declare_use_amcl_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) + ld.add_action(declare_slam_params_file_cmd) ld.add_action(declare_autostart_cmd) ld.add_action(declare_use_composition_cmd) ld.add_action(declare_use_respawn_cmd) diff --git a/src/rm_navigation/launch/bringup_launch.py b/src/rm_navigation/launch/localization_amcl_launch.py similarity index 37% rename from src/rm_navigation/launch/bringup_launch.py rename to src/rm_navigation/launch/localization_amcl_launch.py index 3b6eae692015297413b3709e1881ae670961812c..7de64cde675373ae156121a290609422bb908963 100644 --- a/src/rm_navigation/launch/bringup_launch.py +++ b/src/rm_navigation/launch/localization_amcl_launch.py @@ -1,56 +1,36 @@ -# Copyright (c) 2018 Intel Corporation -# -# 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. - import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import (DeclareLaunchArgument, GroupAction, - IncludeLaunchDescription, SetEnvironmentVariable) +from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, SetEnvironmentVariable from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import Node -from launch_ros.actions import PushRosNamespace +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import LoadComposableNodes, Node +from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml def generate_launch_description(): # Get the launch directory - bringup_dir = get_package_share_directory('rm_navigation') - launch_dir = os.path.join(bringup_dir, 'launch') - + bringup_dir = get_package_share_directory('nav2_bringup') - # Create the launch configuration variables namespace = LaunchConfiguration('namespace') - use_namespace = LaunchConfiguration('use_namespace') - slam = LaunchConfiguration('slam') map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') - params_file = LaunchConfiguration('params_file') autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_amcl = LaunchConfiguration('use_amcl') use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') - # Map fully qualified names to relative ones so the node's namespace can be prepended. - # In case of the transforms (tf), currently, there doesn't seem to be a better alternative - # https://github.com/ros/geometry2/issues/32 - # https://github.com/ros/robot_state_publisher/pull/30 - # TODO(orduno) Substitute with `PushNodeRemapping` - # https://github.com/ros2/launch_ros/issues/56 + # lifecycle_nodes = PythonExpression(["['map_server', 'amcl'] if ", use_amcl, " else ['map_server']"]) + lifecycle_nodes = ['amcl'] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] @@ -59,11 +39,13 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, 'yaml_filename': map_yaml_file} - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') @@ -73,25 +55,13 @@ def generate_launch_description(): default_value='', description='Top-level namespace') - declare_use_namespace_cmd = DeclareLaunchArgument( - 'use_namespace', - default_value='false', - description='Whether to apply a namespace to the navigation stack') - - declare_slam_cmd = DeclareLaunchArgument( - 'slam', - default_value='False', - description='Whether run a SLAM') - declare_map_yaml_cmd = DeclareLaunchArgument( 'map', - # default_value= os.path.join(bringup_dir,'map', 'RMUC.yaml'), - default_value= os.path.join(bringup_dir,'map', 'RMUL.yaml'), description='Full path to map yaml file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', - default_value='true', + default_value='false', description='Use simulation (Gazebo) clock if true') declare_params_file_cmd = DeclareLaunchArgument( @@ -103,9 +73,18 @@ def generate_launch_description(): 'autostart', default_value='true', description='Automatically startup the nav2 stack') + declare_use_amcl_cmd = DeclareLaunchArgument( + 'use_amcl', + default_value='True', + description='Whether run AMCL (only affect when navigating with a map)') + declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='True', - description='Whether to use composed bringup') + 'use_composition', default_value='False', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') declare_use_respawn_cmd = DeclareLaunchArgument( 'use_respawn', default_value='False', @@ -115,57 +94,68 @@ def generate_launch_description(): 'log_level', default_value='info', description='log level') - # Specify the actions - bringup_cmd_group = GroupAction([ - PushRosNamespace( - condition=IfCondition(use_namespace), - namespace=namespace), - - Node( - condition=IfCondition(use_composition), - name='nav2_container', - package='rclcpp_components', - executable='component_container_mt', - parameters=[configured_params, {'autostart': autostart}], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - output='screen'), - - # IncludeLaunchDescription( - # PythonLaunchDescriptionSource(os.path.join(launch_dir, 'slam_launch.py')), - # condition=IfCondition(slam), - # launch_arguments={'namespace': namespace, - # 'use_sim_time': use_sim_time, - # 'autostart': autostart, - # 'use_respawn': use_respawn, - # 'params_file': params_file}.items()), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, - 'localization_launch.py')), - condition=IfCondition(PythonExpression(['not ', slam])), - launch_arguments={'namespace': namespace, - 'map': map_yaml_file, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'params_file': params_file, - 'use_composition': use_composition, - 'use_respawn': use_respawn, - 'container_name': 'nav2_container'}.items()), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')), - launch_arguments={'namespace': namespace, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'params_file': params_file, - 'use_composition': use_composition, - 'use_respawn': use_respawn, - 'container_name': 'nav2_container'}.items()), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')) - ), - ]) + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + # Node( + # package='nav2_map_server', + # executable='map_server', + # name='map_server', + # output='screen', + # respawn=use_respawn, + # respawn_delay=2.0, + # parameters=[configured_params], + # arguments=['--ros-args', '--log-level', log_level], + # remappings=remappings), + Node( + condition=IfCondition(use_amcl), + package='nav2_amcl', + executable='amcl', + name='amcl', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_localization', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + ] + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name_full, + composable_node_descriptions=[ + # ComposableNode( + # package='nav2_map_server', + # plugin='nav2_map_server::MapServer', + # name='map_server', + # parameters=[configured_params], + # remappings=remappings), + ComposableNode( + condition=IfCondition(use_amcl), + package='nav2_amcl', + plugin='nav2_amcl::AmclNode', + name='amcl', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_localization', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) # Create the launch description and populate ld = LaunchDescription() @@ -175,17 +165,18 @@ def generate_launch_description(): # Declare the launch options ld.add_action(declare_namespace_cmd) - ld.add_action(declare_use_namespace_cmd) - ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_amcl_cmd) ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) ld.add_action(declare_use_respawn_cmd) ld.add_action(declare_log_level_cmd) - # Add the actions to launch all of the navigation nodes - ld.add_action(bringup_cmd_group) + # Add the actions to launch all of the localiztion nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) - return ld + return ld \ No newline at end of file diff --git a/src/rm_navigation/launch/localization_launch.py b/src/rm_navigation/launch/map_server_launch.py similarity index 73% rename from src/rm_navigation/launch/localization_launch.py rename to src/rm_navigation/launch/map_server_launch.py index 17b742791ca27194af49741ae3ed6ed817afd81f..ecc31b5094be55a7df1f2262cbf18379ff080419 100644 --- a/src/rm_navigation/launch/localization_launch.py +++ b/src/rm_navigation/launch/map_server_launch.py @@ -1,17 +1,3 @@ -# Copyright (c) 2018 Intel Corporation -# -# 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. - import os from ament_index_python.packages import get_package_share_directory @@ -22,7 +8,7 @@ from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node -from launch_ros.descriptions import ComposableNode +from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml @@ -41,14 +27,8 @@ def generate_launch_description(): use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') - lifecycle_nodes = ['map_server']#, 'amcl'] + lifecycle_nodes = ['map_server'] - # Map fully qualified names to relative ones so the node's namespace can be prepended. - # In case of the transforms (tf), currently, there doesn't seem to be a better alternative - # https://github.com/ros/geometry2/issues/32 - # https://github.com/ros/robot_state_publisher/pull/30 - # TODO(orduno) Substitute with `PushNodeRemapping` - # https://github.com/ros2/launch_ros/issues/56 remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] @@ -57,11 +37,13 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, 'yaml_filename': map_yaml_file} - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') @@ -73,12 +55,11 @@ def generate_launch_description(): declare_map_yaml_cmd = DeclareLaunchArgument( 'map', - default_value= os.path.join(bringup_dir,'map','rmuc_map_draw.yaml'), description='Full path to map yaml file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', - default_value='true', + default_value='false', description='Use simulation (Gazebo) clock if true') declare_params_file_cmd = DeclareLaunchArgument( @@ -119,16 +100,7 @@ def generate_launch_description(): parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], remappings=remappings), - # Node( - # package='nav2_amcl', - # executable='amcl', - # name='amcl', - # output='screen', - # respawn=use_respawn, - # respawn_delay=2.0, - # parameters=[configured_params], - # arguments=['--ros-args', '--log-level', log_level], - # remappings=remappings), + Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', @@ -151,12 +123,7 @@ def generate_launch_description(): name='map_server', parameters=[configured_params], remappings=remappings), - # ComposableNode( - # package='nav2_amcl', - # plugin='nav2_amcl::AmclNode', - # name='amcl', - # parameters=[configured_params], - # remappings=remappings), + ComposableNode( package='nav2_lifecycle_manager', plugin='nav2_lifecycle_manager::LifecycleManager', @@ -188,4 +155,4 @@ def generate_launch_description(): ld.add_action(load_nodes) ld.add_action(load_composable_nodes) - return ld + return ld \ No newline at end of file diff --git a/src/rm_navigation/launch/slam_launch.py b/src/rm_navigation/launch/slam_launch.py deleted file mode 100644 index c64c48ffc53fd0f409962e7a68bbd23b1789e822..0000000000000000000000000000000000000000 --- a/src/rm_navigation/launch/slam_launch.py +++ /dev/null @@ -1,139 +0,0 @@ -# Copyright (c) 2020 Samsung Research Russia -# -# 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. - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.conditions import IfCondition, UnlessCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from nav2_common.launch import HasNodeParams, RewrittenYaml - - -def generate_launch_description(): - # Input parameters declaration - namespace = LaunchConfiguration('namespace') - params_file = LaunchConfiguration('params_file') - use_sim_time = LaunchConfiguration('use_sim_time') - autostart = LaunchConfiguration('autostart') - use_respawn = LaunchConfiguration('use_respawn') - log_level = LaunchConfiguration('log_level') - - # Variables - lifecycle_nodes = ['map_saver'] - - # Getting directories and launch-files - bringup_dir = get_package_share_directory('rm_navigation') - slam_toolbox_dir = get_package_share_directory('slam_toolbox') - slam_launch_file = os.path.join(slam_toolbox_dir, 'launch', 'online_sync_launch.py') - - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'use_sim_time': use_sim_time} - - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) - - # Declare the launch arguments - declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') - - declare_params_file_cmd = DeclareLaunchArgument( - 'params_file', - default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - 'use_sim_time', - default_value='True', - description='Use simulation (Gazebo) clock if true') - - declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='True', - description='Automatically startup the nav2 stack') - - declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='False', - description='Whether to respawn if a node crashes. Applied when composition is disabled.') - - declare_log_level_cmd = DeclareLaunchArgument( - 'log_level', default_value='info', - description='log level') - - # Nodes launching commands - - start_map_saver_server_cmd = Node( - package='nav2_map_server', - executable='map_saver_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - arguments=['--ros-args', '--log-level', log_level], - parameters=[configured_params]) - - start_lifecycle_manager_cmd = Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_slam', - output='screen', - arguments=['--ros-args', '--log-level', log_level], - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]) - - # If the provided param file doesn't have slam_toolbox params, we must remove the 'params_file' - # LaunchConfiguration, or it will be passed automatically to slam_toolbox and will not load - # the default file - has_slam_toolbox_params = HasNodeParams(source_file=params_file, - node_name='slam_toolbox') - - start_slam_toolbox_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(slam_launch_file), - launch_arguments={'use_sim_time': use_sim_time}.items(), - condition=UnlessCondition(has_slam_toolbox_params)) - - start_slam_toolbox_cmd_with_params = IncludeLaunchDescription( - PythonLaunchDescriptionSource(slam_launch_file), - launch_arguments={'use_sim_time': use_sim_time, - 'slam_params_file': params_file}.items(), - condition=IfCondition(has_slam_toolbox_params)) - - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_namespace_cmd) - ld.add_action(declare_params_file_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_autostart_cmd) - ld.add_action(declare_use_respawn_cmd) - ld.add_action(declare_log_level_cmd) - - # Running Map Saver Server - ld.add_action(start_map_saver_server_cmd) - ld.add_action(start_lifecycle_manager_cmd) - - # Running SLAM Toolbox (Only one of them will be run) - ld.add_action(start_slam_toolbox_cmd) - ld.add_action(start_slam_toolbox_cmd_with_params) - - return ld diff --git a/src/rm_perception/pointcloud_downsampling b/src/rm_perception/pointcloud_downsampling index 7d0f65ba6294a92a783e323efe12d6a11bc67ece..8192f9bd9243dc46acc8b7c226379845042271b9 160000 --- a/src/rm_perception/pointcloud_downsampling +++ b/src/rm_perception/pointcloud_downsampling @@ -1 +1 @@ -Subproject commit 7d0f65ba6294a92a783e323efe12d6a11bc67ece +Subproject commit 8192f9bd9243dc46acc8b7c226379845042271b9 diff --git a/src/rm_simulation/livox_laser_simulation_RO2/CMakeLists.txt b/src/rm_simulation/livox_laser_simulation_RO2/CMakeLists.txt index 61c1d324b51075e1679afe7ce500576febb10794..a6c1dadeb96c6f47a863fefe2262cdda256bf792 100644 --- a/src/rm_simulation/livox_laser_simulation_RO2/CMakeLists.txt +++ b/src/rm_simulation/livox_laser_simulation_RO2/CMakeLists.txt @@ -1,87 +1,59 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.10) project(ros2_livox_simulation) -add_compile_options(-std=c++17) -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() +## Use C++14 +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() +## Export compile commands for clangd +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -# find dependencies -find_package(ament_cmake REQUIRED) +####################### +## Find dependencies ## +####################### +find_package(ament_cmake_auto REQUIRED) find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) find_package(gazebo_dev REQUIRED) find_package(gazebo_ros REQUIRED) find_package(sensor_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) find_package(livox_ros_driver2 REQUIRED) find_package(Boost REQUIRED COMPONENTS chrono) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) -include_directories( - include -) -include_directories(${GAZEBO_INCLUDE_DIRS}) -include_directories(/usr/include/gazebo-11/gazebo) -link_directories(${GAZEBO_LIBRARY_DIRS}) +ament_auto_find_build_dependencies() +########### +## Build ## +########### -install(DIRECTORY include/ - DESTINATION include +include_directories( + include + ${GAZEBO_INCLUDE_DIRS} ) -add_library(ros2_livox SHARED src/livox_points_plugin.cpp src/livox_ode_multiray_shape.cpp) -target_link_libraries(ros2_livox ${GAZEBO_LIBRARIES} RayPlugin GpuRayPlugin) -ament_target_dependencies(ros2_livox rclcpp std_msgs sensor_msgs geometry_msgs gazebo_dev gazebo_ros tf2_ros livox_ros_driver2 ) -target_link_libraries(ros2_livox libprotobuf.so.9) -target_link_libraries(ros2_livox libboost_chrono.so.1.71.0) -target_include_directories(ros2_livox PRIVATE include) - -#install(TARGETS ros2_livox DESTINATION lib/${PROJECT_NAME}) - -install(TARGETS ros2_livox - LIBRARY DESTINATION lib) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() +link_directories(${GAZEBO_LIBRARY_DIRS}) -ament_export_libraries( - ros2_livox +ament_auto_add_library(ros2_livox SHARED + src/livox_points_plugin.cpp + src/livox_ode_multiray_shape.cpp ) -set(CSV_FILES - scan_mode/mid360.csv +target_link_libraries(ros2_livox + RayPlugin + GpuRayPlugin + libprotobuf.so.9 + libboost_chrono.so.1.71.0 ) -foreach(CSV_FILE ${CSV_FILES}) - configure_file(${CSV_FILE} ${CMAKE_CURRENT_BINARY_DIR}/${CSV_FILE} COPYONLY) - install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${CSV_FILE} - DESTINATION share/${PROJECT_NAME}/scan_mode - ) -endforeach() -set(URDF_FILES - urdf/mid360.xacro -) -foreach(URDF_FILES ${URDF_FILES}) - configure_file(${URDF_FILES} ${CMAKE_CURRENT_BINARY_DIR}/${URDF_FILES} COPYONLY) - install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${URDF_FILES} - DESTINATION share/${PROJECT_NAME}/urdf - ) -endforeach() +############# +## Testing ## +############# -ament_package() +############# +## Install ## +############# + +ament_auto_package( + INSTALL_TO_SHARE + scan_mode + urdf +) diff --git a/src/rm_simulation/livox_laser_simulation_RO2/package.xml b/src/rm_simulation/livox_laser_simulation_RO2/package.xml index 4da750ab15a9c608b9b0053a57c0373590081eaf..9c9f323965632874fc57a16e538f7d4d6c1681a5 100644 --- a/src/rm_simulation/livox_laser_simulation_RO2/package.xml +++ b/src/rm_simulation/livox_laser_simulation_RO2/package.xml @@ -8,22 +8,23 @@ TODO: License declaration ament_cmake + rclcpp - std_msgs + rclcpp_components + gazebo gazebo_ros - gazebo_dev sensor_msgs tf2_ros geometry_msgs livox_ros_driver2 -gazebo_plugins -gazebo_plugins + ament_lint_auto ament_lint_common - + ament_cmake_clang_format ament_cmake + diff --git a/src/rm_simulation/pb_rm_simulation/launch/rm_simulation.launch.py b/src/rm_simulation/pb_rm_simulation/launch/rm_simulation.launch.py index 3844af133a85c5caa69e589e88ef2d10a39ff3d6..36a312e80d5c643159b074b82a88993aa37a69df 100644 --- a/src/rm_simulation/pb_rm_simulation/launch/rm_simulation.launch.py +++ b/src/rm_simulation/pb_rm_simulation/launch/rm_simulation.launch.py @@ -12,6 +12,7 @@ from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue from launch.conditions import LaunchConfigurationEquals from launch.conditions import IfCondition +from launch.actions.append_environment_variable import AppendEnvironmentVariable # Enum for world types class WorldType: @@ -22,11 +23,11 @@ class WorldType: def get_world_config(world_type): world_configs = { WorldType.RMUC: { - 'x': '2.5', - 'y': '0.0', - 'z': '0.08', - 'yaw': '0.0', - 'world_path': 'RMUC2023_world/RMUC2023_world.world' + 'x': '8.0', + 'y': '6.55', + 'z': '0.46', + 'yaw': '1.6', + 'world_path': 'RMUC2024_world/RMUC2024_world.world' }, WorldType.RMUL: { 'x': '0.0', @@ -50,12 +51,18 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') use_rviz = LaunchConfiguration('rviz', default='false') + # Set Gazebo plugin path + append_enviroment = AppendEnvironmentVariable( + 'GAZEBO_PLUGIN_PATH', + os.path.join(os.path.join(get_package_share_directory('pb_rm_simulation'), 'meshes', 'obstacles', 'obstacle_plugin', 'lib')) + ) + declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='True', description='Use simulation (Gazebo) clock if true' ) - + declare_world_cmd = DeclareLaunchArgument( 'world', default_value=WorldType.RMUC, @@ -67,7 +74,7 @@ def generate_launch_description(): default_value=os.path.join(bringup_dir, 'rviz', 'rviz2.rviz'), description='Full path to the RVIZ config file to use' ) - + # Specify the actions gazebo_client_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')), @@ -136,18 +143,21 @@ def generate_launch_description(): bringup_RMUC_cmd_group = create_gazebo_launch_group(WorldType.RMUC) bringup_RMUL_cmd_group = create_gazebo_launch_group(WorldType.RMUL) - + # Create the launch description and populate ld = LaunchDescription() + # Set environment variables + ld.add_action(append_enviroment) + ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_world_cmd) ld.add_action(declare_rviz_config_file_cmd) ld.add_action(gazebo_client_launch) ld.add_action(start_joint_state_publisher_cmd) ld.add_action(start_robot_state_publisher_cmd) - ld.add_action(bringup_RMUL_cmd_group) - ld.add_action(bringup_RMUC_cmd_group) + ld.add_action(bringup_RMUL_cmd_group) # type: ignore + ld.add_action(bringup_RMUC_cmd_group) # type: ignore # Uncomment this line if you want to start RViz ld.add_action(start_rviz_cmd) diff --git a/src/rm_simulation/pb_rm_simulation/meshes/RMUC2023_world/meshes/rm_env.dae b/src/rm_simulation/pb_rm_simulation/meshes/RMUC2024_world/meshes/RMUC_2024.stl similarity index 84% rename from src/rm_simulation/pb_rm_simulation/meshes/RMUC2023_world/meshes/rm_env.dae rename to src/rm_simulation/pb_rm_simulation/meshes/RMUC2024_world/meshes/RMUC_2024.stl index 851f03366708830b34ca476d4dcdaf56b115e462..0d399b76f1ef42580a924e517088b8fd8369312c 100644 Binary files a/src/rm_simulation/pb_rm_simulation/meshes/RMUC2023_world/meshes/rm_env.dae and b/src/rm_simulation/pb_rm_simulation/meshes/RMUC2024_world/meshes/RMUC_2024.stl differ diff --git a/src/rm_simulation/pb_rm_simulation/meshes/RMUC2023_world/model.config b/src/rm_simulation/pb_rm_simulation/meshes/RMUC2024_world/model.config similarity index 51% rename from src/rm_simulation/pb_rm_simulation/meshes/RMUC2023_world/model.config rename to src/rm_simulation/pb_rm_simulation/meshes/RMUC2024_world/model.config index 5b2e3271aa0b7d81b9b76d6e5b3cb935836fe59a..53e46009d974adebeb069df6882e5151c106d5ae 100644 --- a/src/rm_simulation/pb_rm_simulation/meshes/RMUC2023_world/model.config +++ b/src/rm_simulation/pb_rm_simulation/meshes/RMUC2024_world/model.config @@ -1,17 +1,17 @@ - RMUC2023_world + RMUC2024_world 1.0 model.sdf - Livox-SDK - www.livoxtech.com + Lihan Chen + 1120220476@smbu.edu.cn - livox robomaster map + RoboMaster University Championship 2024 world diff --git a/src/rm_simulation/pb_rm_simulation/meshes/RMUC2023_world/model.sdf b/src/rm_simulation/pb_rm_simulation/meshes/RMUC2024_world/model.sdf similarity index 80% rename from src/rm_simulation/pb_rm_simulation/meshes/RMUC2023_world/model.sdf rename to src/rm_simulation/pb_rm_simulation/meshes/RMUC2024_world/model.sdf index 81a9cc49ea60c90fffc2eb24d6068c9ab90133ae..daac0ab20950c6be73be5d9b4491593f0c22e7a5 100644 --- a/src/rm_simulation/pb_rm_simulation/meshes/RMUC2023_world/model.sdf +++ b/src/rm_simulation/pb_rm_simulation/meshes/RMUC2024_world/model.sdf @@ -1,6 +1,6 @@ - + true @@ -18,14 +18,14 @@ - model://RMUC2023_world/meshes/rm_env.dae + model://RMUC2024_world/meshes/RMUC_2024.stl - model://RMUC2023_world/meshes/rm_env.dae + model://RMUC2024_world/meshes/RMUC2024_world diff --git a/src/rm_simulation/pb_rm_simulation/world/RMUC2023_world/RMUC2023_world.world b/src/rm_simulation/pb_rm_simulation/world/RMUC2024_world/RMUC2024_world.world similarity index 88% rename from src/rm_simulation/pb_rm_simulation/world/RMUC2023_world/RMUC2023_world.world rename to src/rm_simulation/pb_rm_simulation/world/RMUC2024_world/RMUC2024_world.world index b79d8da121132bf12c9a6c28e0136779ffd6032c..242ab04d3bee9f91e60ff448abcab1248831ddae 100644 --- a/src/rm_simulation/pb_rm_simulation/world/RMUC2023_world/RMUC2023_world.world +++ b/src/rm_simulation/pb_rm_simulation/world/RMUC2024_world/RMUC2024_world.world @@ -60,7 +60,7 @@ - -6.19976 -0.001985 5.28063 0 0.366792 -0.013587 + 8.09586 -0.793752 4.29672 0 0.326792 1.55841 orbit perspective @@ -74,8 +74,8 @@ - model://RMUC2023_world/meshes/rm_env.dae - + model://RMUC2024_world/meshes/RMUC_2024.stl + 0.001 0.001 0.001 10 @@ -98,8 +98,8 @@ - model://RMUC2023_world/meshes/rm_env.dae - + model://RMUC2024_world/meshes/RMUC_2024.stl + 0.001 0.001 0.001