diff --git a/deploy/nav2_ws/028.data b/deploy/nav2_ws/028.data new file mode 100644 index 0000000..85d7f24 Binary files /dev/null and b/deploy/nav2_ws/028.data differ diff --git a/deploy/nav2_ws/028.posegraph b/deploy/nav2_ws/028.posegraph new file mode 100644 index 0000000..85d7a4c Binary files /dev/null and b/deploy/nav2_ws/028.posegraph differ diff --git a/deploy/nav2_ws/lab_and_hallway.data b/deploy/nav2_ws/lab_and_hallway.data new file mode 100644 index 0000000..5b77d43 Binary files /dev/null and b/deploy/nav2_ws/lab_and_hallway.data differ diff --git a/deploy/nav2_ws/lab_and_hallway.pgm b/deploy/nav2_ws/lab_and_hallway.pgm new file mode 100644 index 0000000..a5884be Binary files /dev/null and b/deploy/nav2_ws/lab_and_hallway.pgm differ diff --git a/deploy/nav2_ws/lab_and_hallway.posegraph b/deploy/nav2_ws/lab_and_hallway.posegraph new file mode 100644 index 0000000..2a9c4f3 Binary files /dev/null and b/deploy/nav2_ws/lab_and_hallway.posegraph differ diff --git a/deploy/nav2_ws/lab_and_hallway.yaml b/deploy/nav2_ws/lab_and_hallway.yaml new file mode 100644 index 0000000..249663b --- /dev/null +++ b/deploy/nav2_ws/lab_and_hallway.yaml @@ -0,0 +1,7 @@ +image: lab_and_hallway.pgm +mode: trinary +resolution: 0.05 +origin: [-20, -27.7, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/deploy/nav2_ws/map_028.pgm b/deploy/nav2_ws/map_028.pgm new file mode 100644 index 0000000..445d035 Binary files /dev/null and b/deploy/nav2_ws/map_028.pgm differ diff --git a/deploy/nav2_ws/map_028.yaml b/deploy/nav2_ws/map_028.yaml new file mode 100644 index 0000000..8f61146 --- /dev/null +++ b/deploy/nav2_ws/map_028.yaml @@ -0,0 +1,7 @@ +image: map_028.pgm +mode: trinary +resolution: 0.05 +origin: [-4.43, -1.69, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/deploy/nav2_ws/src/sportmode_nav2/launch/localization_async.launch.py b/deploy/nav2_ws/src/sportmode_nav2/launch/localization_async.launch.py new file mode 100644 index 0000000..70db31e --- /dev/null +++ b/deploy/nav2_ws/src/sportmode_nav2/launch/localization_async.launch.py @@ -0,0 +1,59 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from nav2_common.launch import HasNodeParams + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time', default=False) + params_file = LaunchConfiguration('params_file') + default_params_file = os.path.join(get_package_share_directory("sportmode_nav2"), + 'params', 'localization_async.yaml') + + declare_use_sim_time_argument = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation/Gazebo clock') + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=default_params_file, + description='Full path to the ROS2 parameters file to use for the slam_toolbox node') + + # If the provided param file doesn't have slam_toolbox params, we must pass the + # default_params_file instead. This could happen due to automatic propagation of + # LaunchArguments. See: + # https://github.com/ros-planning/navigation2/pull/2243#issuecomment-800479866 + has_node_params = HasNodeParams(source_file=params_file, + node_name='slam_toolbox') + + actual_params_file = PythonExpression(['"', params_file, '" if ', has_node_params, + ' else "', default_params_file, '"']) + + log_param_change = LogInfo(msg=['provided params_file ', params_file, + ' does not contain slam_toolbox parameters. Using default: ', + default_params_file], + condition=UnlessCondition(has_node_params)) + + start_async_slam_toolbox_node = Node( + parameters=[ + actual_params_file, + {'use_sim_time': use_sim_time} + ], + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + output='screen') + + ld = LaunchDescription() + + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_params_file_cmd) + ld.add_action(log_param_change) + ld.add_action(start_async_slam_toolbox_node) + + return ld diff --git a/deploy/nav2_ws/src/sportmode_nav2/launch/localization_launch.py b/deploy/nav2_ws/src/sportmode_nav2/launch/localization_launch.py new file mode 100644 index 0000000..e058bba --- /dev/null +++ b/deploy/nav2_ws/src/sportmode_nav2/launch/localization_launch.py @@ -0,0 +1,107 @@ +# 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, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + + namespace = LaunchConfiguration('namespace') + map_yaml_file = LaunchConfiguration('map') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + lifecycle_nodes = ['map_server', 'amcl'] + + # 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')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + '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) + + return LaunchDescription([ + # Set env var to print messages to stdout immediately + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + + DeclareLaunchArgument( + 'namespace', default_value='', + description='Top-level namespace'), + + DeclareLaunchArgument( + 'map', + default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), + description='Full path to map yaml file to load'), + + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack'), + + DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use'), + + Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[configured_params], + remappings=remappings), + + Node( + package='nav2_amcl', + executable='amcl', + name='amcl', + output='screen', + parameters=[configured_params], + remappings=remappings), + + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_localization', + output='screen', + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + ]) diff --git a/deploy/nav2_ws/src/sportmode_nav2/launch/mapping_async.launch.py b/deploy/nav2_ws/src/sportmode_nav2/launch/mapping_async.launch.py index b1ffc79..939c31a 100644 --- a/deploy/nav2_ws/src/sportmode_nav2/launch/mapping_async.launch.py +++ b/deploy/nav2_ws/src/sportmode_nav2/launch/mapping_async.launch.py @@ -17,7 +17,7 @@ def generate_launch_description(): declare_use_sim_time_argument = DeclareLaunchArgument( 'use_sim_time', - default_value='true', + default_value='false', description='Use simulation/Gazebo clock') declare_params_file_cmd = DeclareLaunchArgument( 'params_file', diff --git a/deploy/nav2_ws/src/sportmode_nav2/launch/nav2_bringup_launch.py b/deploy/nav2_ws/src/sportmode_nav2/launch/nav2_bringup_launch.py new file mode 100644 index 0000000..81ee595 --- /dev/null +++ b/deploy/nav2_ws/src/sportmode_nav2/launch/nav2_bringup_launch.py @@ -0,0 +1,141 @@ +# 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.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import PushRosNamespace + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('sportmode_nav2') + launch_dir = os.path.join(bringup_dir, 'launch') + + # 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') + default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename') + autostart = LaunchConfiguration('autostart') + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + 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', + description='Full path to map yaml file to load') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + 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_bt_xml_cmd = DeclareLaunchArgument( + 'default_bt_xml_filename', + default_value=os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), + description='Full path to the behavior tree xml file to use') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + # Specify the actions + bringup_cmd_group = GroupAction([ + PushRosNamespace( + condition=IfCondition(use_namespace), + namespace=namespace), + + 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, + '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_lifecycle_mgr': 'false'}.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, + 'default_bt_xml_filename': default_bt_xml_filename, + 'use_lifecycle_mgr': 'false', + 'map_subscribe_transient_local': 'true'}.items()), + ]) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # 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_bt_xml_cmd) + + # Add the actions to launch all of the navigation nodes + ld.add_action(bringup_cmd_group) + + return ld diff --git a/deploy/nav2_ws/src/sportmode_nav2/launch/navigation_launch.py b/deploy/nav2_ws/src/sportmode_nav2/launch/navigation_launch.py new file mode 100644 index 0000000..9784090 --- /dev/null +++ b/deploy/nav2_ws/src/sportmode_nav2/launch/navigation_launch.py @@ -0,0 +1,145 @@ +# 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, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('sportmode_nav2') + + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename') + map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local') + + lifecycle_nodes = ['controller_server', + 'planner_server', + 'recoveries_server', + 'bt_navigator', + 'waypoint_follower'] + + # 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')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'default_bt_xml_filename': default_bt_xml_filename, + 'autostart': autostart, + 'map_subscribe_transient_local': map_subscribe_transient_local} + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True) + + return LaunchDescription([ + # Set env var to print messages to stdout immediately + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + + DeclareLaunchArgument( + 'namespace', default_value='', + description='Top-level namespace'), + + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack'), + + DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use'), + + DeclareLaunchArgument( + 'default_bt_xml_filename', + default_value=os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), + description='Full path to the behavior tree xml file to use'), + + DeclareLaunchArgument( + 'map_subscribe_transient_local', default_value='false', + description='Whether to set the map subscriber QoS to transient local'), + + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + parameters=[configured_params], + remappings=remappings), + + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + parameters=[configured_params], + remappings=remappings), + + Node( + package='nav2_recoveries', + executable='recoveries_server', + name='recoveries_server', + output='screen', + parameters=[configured_params], + remappings=remappings), + + Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + parameters=[configured_params], + remappings=remappings), + + Node( + package='nav2_waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', + output='screen', + parameters=[configured_params], + remappings=remappings), + + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]), + + ]) diff --git a/deploy/nav2_ws/src/sportmode_nav2/launch/slam_launch.py b/deploy/nav2_ws/src/sportmode_nav2/launch/slam_launch.py new file mode 100644 index 0000000..578b449 --- /dev/null +++ b/deploy/nav2_ws/src/sportmode_nav2/launch/slam_launch.py @@ -0,0 +1,107 @@ +# 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.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import 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') + + # Variables + lifecycle_nodes = ['map_saver'] + + # Getting directories and launch-files + bringup_dir = get_package_share_directory('sportmode_nav2') + slam_toolbox_dir = get_package_share_directory('sportmode_nav2') + slam_launch_file = os.path.join(slam_toolbox_dir, 'launch', 'mapping.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') + + # Nodes launching commands + start_slam_toolbox_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={'use_sim_time': use_sim_time}.items()) + + start_map_saver_server_cmd = Node( + package='nav2_map_server', + node_executable='map_saver_server', + output='screen', + parameters=[configured_params]) + + start_lifecycle_manager_cmd = Node( + package='nav2_lifecycle_manager', + node_executable='lifecycle_manager', + node_name='lifecycle_manager_slam', + output='screen', + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + + 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) + + # Running SLAM Toolbox + ld.add_action(start_slam_toolbox_cmd) + + # Running Map Saver Server + ld.add_action(start_map_saver_server_cmd) + ld.add_action(start_lifecycle_manager_cmd) + + return ld diff --git a/deploy/nav2_ws/src/sportmode_nav2/launch/slam_toolbox_localization.py b/deploy/nav2_ws/src/sportmode_nav2/launch/slam_toolbox_localization.py new file mode 100644 index 0000000..939c31a --- /dev/null +++ b/deploy/nav2_ws/src/sportmode_nav2/launch/slam_toolbox_localization.py @@ -0,0 +1,59 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from nav2_common.launch import HasNodeParams + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time', default=False) + params_file = LaunchConfiguration('params_file') + default_params_file = os.path.join(get_package_share_directory("sportmode_nav2"), + 'params', 'mapping_async.yaml') + + declare_use_sim_time_argument = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation/Gazebo clock') + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=default_params_file, + description='Full path to the ROS2 parameters file to use for the slam_toolbox node') + + # If the provided param file doesn't have slam_toolbox params, we must pass the + # default_params_file instead. This could happen due to automatic propagation of + # LaunchArguments. See: + # https://github.com/ros-planning/navigation2/pull/2243#issuecomment-800479866 + has_node_params = HasNodeParams(source_file=params_file, + node_name='slam_toolbox') + + actual_params_file = PythonExpression(['"', params_file, '" if ', has_node_params, + ' else "', default_params_file, '"']) + + log_param_change = LogInfo(msg=['provided params_file ', params_file, + ' does not contain slam_toolbox parameters. Using default: ', + default_params_file], + condition=UnlessCondition(has_node_params)) + + start_async_slam_toolbox_node = Node( + parameters=[ + actual_params_file, + {'use_sim_time': use_sim_time} + ], + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + output='screen') + + ld = LaunchDescription() + + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_params_file_cmd) + ld.add_action(log_param_change) + ld.add_action(start_async_slam_toolbox_node) + + return ld diff --git a/deploy/nav2_ws/src/sportmode_nav2/params/localization_async.yaml b/deploy/nav2_ws/src/sportmode_nav2/params/localization_async.yaml new file mode 100644 index 0000000..a4d70c9 --- /dev/null +++ b/deploy/nav2_ws/src/sportmode_nav2/params/localization_async.yaml @@ -0,0 +1,73 @@ +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 # ToDo: add a base_footprint to the URDF + scan_topic: /go2/scan + mode: 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: /home/unitree/locomotion/Go2Py/deploy/nav2_ws/lab_and_hallway + # 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/deploy/nav2_ws/src/sportmode_nav2/params/nav2_params.yaml b/deploy/nav2_ws/src/sportmode_nav2/params/nav2_params.yaml new file mode 100644 index 0000000..b27eea8 --- /dev/null +++ b/deploy/nav2_ws/src/sportmode_nav2/params/nav2_params.yaml @@ -0,0 +1,289 @@ +amcl: + ros__parameters: + use_sim_time: False + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_link" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "differential" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: /go2/scan + +amcl_map_client: + ros__parameters: + use_sim_time: False + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: False + +bt_navigator: + ros__parameters: + use_sim_time: False + global_frame: map + robot_base_frame: base_link + odom_topic: /utlidar/robot_odom + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: False + +controller_server: + ros__parameters: + use_sim_time: False + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: False + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /go2/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: False + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /go2/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +map_server: + ros__parameters: + use_sim_time: False + yaml_filename: "/home/unitree/Generated_Maps/basement/hallway-basement.yaml" + +map_saver: + ros__parameters: + use_sim_time: False + save_map_timeout: 5000 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: False + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: False + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "back_up", "wait"] + spin: + plugin: "nav2_recoveries/Spin" + back_up: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: False + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: False diff --git a/deploy/nav2_ws/src/sportmode_nav2/params/ros_ekf.yaml b/deploy/nav2_ws/src/sportmode_nav2/params/ros_ekf.yaml index 3a07fe3..31a508e 100644 --- a/deploy/nav2_ws/src/sportmode_nav2/params/ros_ekf.yaml +++ b/deploy/nav2_ws/src/sportmode_nav2/params/ros_ekf.yaml @@ -66,7 +66,7 @@ ekf_filter_node: # 3a. Set your "world_frame" to your map_frame value # 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state # estimation node from robot_localization! However, that instance should *not* fuse the global data. - map_frame: map # Defaults to "map" if unspecified + map_frame: odom # Defaults to "map" if unspecified odom_frame: odom # Defaults to "odom" if unspecified base_link_frame: base_link # Defaults to "base_link" if unspecified world_frame: odom # Defaults to the value of odom_frame if unspecified