diff --git a/deploy/nav2_ws/sportmode_nav2_new/CMakeLists.txt b/deploy/nav2_ws/sportmode_nav2_new/CMakeLists.txt new file mode 100644 index 0000000..b214568 --- /dev/null +++ b/deploy/nav2_ws/sportmode_nav2_new/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.5) +project(sportmode_nav2) + +# 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() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +include_directories(include include/common include/nlohmann) +link_directories(src) + +set ( + DEPENDENCY_LIST + rclcpp + std_msgs + rosbag2_cpp + sensor_msgs + geometry_msgs + nav_msgs + tf2 + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs +) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosbag2_cpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_sensor_msgs REQUIRED) + +# Install params config files. +install(DIRECTORY + params + launch + DESTINATION share/${PROJECT_NAME} + USE_SOURCE_PERMISSIONS +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/deploy/nav2_ws/sportmode_nav2_new/launch/localization.launch.py b/deploy/nav2_ws/sportmode_nav2_new/launch/localization.launch.py new file mode 100644 index 0000000..839f1ce --- /dev/null +++ b/deploy/nav2_ws/sportmode_nav2_new/launch/localization.launch.py @@ -0,0 +1,27 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + + robot_localization = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('sportmode_nav2'), 'launch'), + '/ros_ekf.launch.py']) + ) + async_mapping = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('sportmode_nav2'), 'launch'), + '/localization_async.launch.py']) + ) + + return LaunchDescription([ + robot_localization, + async_mapping, + ]) \ No newline at end of file diff --git a/deploy/nav2_ws/sportmode_nav2_new/launch/localization_async.launch.py b/deploy/nav2_ws/sportmode_nav2_new/launch/localization_async.launch.py new file mode 100644 index 0000000..70db31e --- /dev/null +++ b/deploy/nav2_ws/sportmode_nav2_new/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/sportmode_nav2_new/launch/mapping.launch.py b/deploy/nav2_ws/sportmode_nav2_new/launch/mapping.launch.py new file mode 100644 index 0000000..526fc21 --- /dev/null +++ b/deploy/nav2_ws/sportmode_nav2_new/launch/mapping.launch.py @@ -0,0 +1,27 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + + robot_localization = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('sportmode_nav2'), 'launch'), + '/ros_ekf.launch.py']) + ) + async_mapping = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('sportmode_nav2'), 'launch'), + '/mapping_async.launch.py']) + ) + + return LaunchDescription([ + robot_localization, + async_mapping, + ]) \ No newline at end of file diff --git a/deploy/nav2_ws/sportmode_nav2_new/launch/mapping_async.launch.py b/deploy/nav2_ws/sportmode_nav2_new/launch/mapping_async.launch.py new file mode 100644 index 0000000..939c31a --- /dev/null +++ b/deploy/nav2_ws/sportmode_nav2_new/launch/mapping_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', '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/sportmode_nav2_new/launch/navigation.launch.py b/deploy/nav2_ws/sportmode_nav2_new/launch/navigation.launch.py new file mode 100644 index 0000000..c48a741 --- /dev/null +++ b/deploy/nav2_ws/sportmode_nav2_new/launch/navigation.launch.py @@ -0,0 +1,146 @@ +# 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'), + ('/cmd_vel', '/go2/cmd_vel')] + + # 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/sportmode_nav2_new/launch/navigation_launch.py b/deploy/nav2_ws/sportmode_nav2_new/launch/navigation_launch.py new file mode 100644 index 0000000..685b1c6 --- /dev/null +++ b/deploy/nav2_ws/sportmode_nav2_new/launch/navigation_launch.py @@ -0,0 +1,272 @@ +# 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, SetEnvironmentVariable +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, ParameterFile +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') + use_sim_time = LaunchConfiguration('use_sim_time') + 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_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['controller_server', + 'smoother_server', + 'planner_server', + 'behavior_server', + 'bt_navigator', + 'waypoint_follower', + 'velocity_smoother'] + + # 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'),('/cmd_vel', '/go2/cmd_vel')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'autostart': autostart} + + 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') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + 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_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + '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', + 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') + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_velocity_smoother', + executable='velocity_smoother', + name='velocity_smoother', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + 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_controller', + plugin='nav2_controller::ControllerServer', + name='controller_server', + parameters=[configured_params], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + ComposableNode( + package='nav2_smoother', + plugin='nav2_smoother::SmootherServer', + name='smoother_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_planner', + plugin='nav2_planner::PlannerServer', + name='planner_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_behaviors', + plugin='behavior_server::BehaviorServer', + name='behavior_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_bt_navigator', + plugin='nav2_bt_navigator::BtNavigator', + name='bt_navigator', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_waypoint_follower', + plugin='nav2_waypoint_follower::WaypointFollower', + name='waypoint_follower', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_velocity_smoother', + plugin='nav2_velocity_smoother::VelocitySmoother', + name='velocity_smoother', + parameters=[configured_params], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_navigation', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + + # 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_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_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(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/deploy/nav2_ws/sportmode_nav2_new/launch/ros_ekf.launch.py b/deploy/nav2_ws/sportmode_nav2_new/launch/ros_ekf.launch.py new file mode 100644 index 0000000..1ee570a --- /dev/null +++ b/deploy/nav2_ws/sportmode_nav2_new/launch/ros_ekf.launch.py @@ -0,0 +1,35 @@ +# Copyright 2018 Open Source Robotics Foundation, Inc. +# Copyright 2019 Samsung Research America +# +# 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. + +from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory +import launch_ros.actions +import os +import yaml +from launch.substitutions import EnvironmentVariable +import pathlib +import launch.actions +from launch.actions import DeclareLaunchArgument + +def generate_launch_description(): + return LaunchDescription([ + launch_ros.actions.Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node', + output='screen', + parameters=[os.path.join(get_package_share_directory("sportmode_nav2"), 'params', 'ros_ekf.yaml')], + ), +]) diff --git a/deploy/nav2_ws/sportmode_nav2_new/package.xml b/deploy/nav2_ws/sportmode_nav2_new/package.xml new file mode 100644 index 0000000..6e7a413 --- /dev/null +++ b/deploy/nav2_ws/sportmode_nav2_new/package.xml @@ -0,0 +1,22 @@ + + + + sportmode_nav2 + 0.0.0 + TODO: Package description + zeen + TODO: License declaration + + ament_cmake + + rclcpp + + ament_lint_auto + ament_lint_common + robot_locoalization + slam_toolbox + + + ament_cmake + + diff --git a/deploy/nav2_ws/sportmode_nav2_new/params/localization_async.yaml b/deploy/nav2_ws/sportmode_nav2_new/params/localization_async.yaml new file mode 100644 index 0000000..3506980 --- /dev/null +++ b/deploy/nav2_ws/sportmode_nav2_new/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/maps/feb23_basement + #map_start_pose: [0.33, 1.49, 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/sportmode_nav2_new/params/mapping_async.yaml b/deploy/nav2_ws/sportmode_nav2_new/params/mapping_async.yaml new file mode 100644 index 0000000..b4dfd4c --- /dev/null +++ b/deploy/nav2_ws/sportmode_nav2_new/params/mapping_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: 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/deploy/nav2_ws/sportmode_nav2_new/params/nav2_params.yaml b/deploy/nav2_ws/sportmode_nav2_new/params/nav2_params.yaml new file mode 100644 index 0000000..6602ad5 --- /dev/null +++ b/deploy/nav2_ws/sportmode_nav2_new/params/nav2_params.yaml @@ -0,0 +1,349 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + 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: "nav2_amcl::DifferentialMotionModel" + 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: scan + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_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 + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # 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 + +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: True + 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" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +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: True + 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" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + 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 + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + 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: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/deploy/nav2_ws/sportmode_nav2_new/params/ros_ekf.yaml b/deploy/nav2_ws/sportmode_nav2_new/params/ros_ekf.yaml new file mode 100644 index 0000000..f1b2cfd --- /dev/null +++ b/deploy/nav2_ws/sportmode_nav2_new/params/ros_ekf.yaml @@ -0,0 +1,219 @@ +### ekf config file ### +ekf_filter_node: + ros__parameters: +# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin +# computation until it receives at least one message from one of the inputs. It will then run continuously at the +# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. + frequency: 100.0 + +# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict +# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the +# filter will generate new output. Defaults to 1 / frequency if not specified. + sensor_timeout: 0.1 + +# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is +# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar +# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected +# by, for example, an IMU. Defaults to false if unspecified. + two_d_mode: false + +# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for +# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if +# unspecified. + transform_time_offset: 0.0 + +# Use this parameter to provide specify how long the tf listener should wait for a transform to become available. +# Defaults to 0.0 if unspecified. + transform_timeout: 0.0 + +# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is +# unhappy with any settings or data. + print_diagnostics: true + +# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by +# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious +# effects on the performance of the node. Defaults to false if unspecified. + debug: false + +# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. + debug_out_file: /path/to/debug/file.txt + +# Whether we'll allow old measurements to cause a re-publication of the updated state + permit_corrected_publication: false + +# Whether to publish the acceleration state. Defaults to false if unspecified. + publish_acceleration: false + +# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. + publish_tf: true + +# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and +# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. +# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be +# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom +# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your +# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based +# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. +# ekf_localization_node and ukf_localization_node are not concerned with the earth frame. +# Here is how to use the following settings: +# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. +# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of +# odom_frame. +# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set +# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. +# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates +# from landmark observations) then: +# 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: 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 + +# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, +# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, +# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, +# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no +# default values, and must be specified. + odom0: /utlidar/robot_odom + +# Each sensor reading updates some or all of the filter's state. These options give you greater control over which +# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only +# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the +# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types +# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message +# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false +# if unspecified, effectively making this parameter required for each sensor. + odom0_config: [true, true, true, + true, true, true, + true, true, true, + true, true, true, + false, false, false] + +# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase +# the size of the subscription queue so that more measurements are fused. + odom0_queue_size: 2 + +# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result +# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's +# algorithm. + odom0_nodelay: false + +# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- +# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they +# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also +# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't +# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose +# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then +# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true +# for twist measurements has no effect. + odom0_differential: false + +# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" +# for all future measurements. While you can achieve the same effect with the differential paremeter, the key +# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before +# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. + odom0_relative: true + +# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to +# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to +# numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not +# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. +# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying +# the thresholds. + odom0_pose_rejection_threshold: 5.0 + odom0_twist_rejection_threshold: 1.0 + + # imu0: /utlidar/imu + # imu0_config: [false, false, false, + # true, true, true, + # false, false, false, + # true, true, true, + # true, true, true] + # imu0_nodelay: false + # imu0_differential: false + # imu0_relative: true + # imu0_queue_size: 5 + # imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names + # imu0_twist_rejection_threshold: 0.8 # + # imu0_linear_acceleration_rejection_threshold: 0.8 # + +# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set +# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. + imu0_remove_gravitational_acceleration: true + +# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no +# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During +# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be +# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When +# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially +# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance +# for the velocity variable in question, or decrease the variance of the variable in question in the measurement +# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we +# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during +# predicition. Note that if an acceleration measurement for the variable in question is available from one of the +# inputs, the control term will be ignored. +# Whether or not we use the control input during predicition. Defaults to false. + use_control: true +# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to +# false. + stamped_control: false +# The last issued control command will be used in prediction for this period. Defaults to 0.2. + control_timeout: 0.2 +# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. + control_config: [true, false, false, false, false, true] +# Places limits on how large the acceleration term will be. Should match your robot's kinematics. + acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] +# Acceleration and deceleration limits are not always the same for robots. + deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] +# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these +# gains + acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] +# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these +# gains + deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] +# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is +# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each +# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be. +# However, if users find that a given variable is slow to converge, one approach is to increase the +# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error +# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are +# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if +# unspecified. + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] +# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal +# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in +# question. Users should take care not to use large values for variables that will not be measured directly. The values +# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below +#if unspecified. + initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9] + diff --git a/deploy/nav2_ws/src/sportmode_nav2/launch/navigation_launch.py b/deploy/nav2_ws/src/sportmode_nav2/launch/navigation_launch.py index e34b671..685b1c6 100644 --- a/deploy/nav2_ws/src/sportmode_nav2/launch/navigation_launch.py +++ b/deploy/nav2_ws/src/sportmode_nav2/launch/navigation_launch.py @@ -20,7 +20,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import LoadComposableNodes, SetParameter +from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml @@ -40,16 +40,13 @@ def generate_launch_description(): use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') - lifecycle_nodes = [ - 'controller_server', - 'smoother_server', - 'planner_server', - 'behavior_server', - 'velocity_smoother', - 'collision_monitor', - 'bt_navigator', - 'waypoint_follower', - ] + lifecycle_nodes = ['controller_server', + 'smoother_server', + 'planner_server', + 'behavior_server', + 'bt_navigator', + 'waypoint_follower', + 'velocity_smoother'] # 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 @@ -57,75 +54,63 @@ def generate_launch_description(): # 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'), - ('/cmd_vel', '/go2/cmd_vel')] + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static'),('/cmd_vel', '/go2/cmd_vel')] # Create our own temporary YAML files that include substitutions - param_substitutions = {'autostart': autostart} + param_substitutions = { + 'use_sim_time': use_sim_time, + 'autostart': autostart} configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites=param_substitutions, - convert_types=True, - ), - allow_substs=True, - ) + convert_types=True), + allow_substs=True) stdout_linebuf_envvar = SetEnvironmentVariable( - 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' - ) + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', default_value='', description='Top-level namespace' - ) + 'namespace', + default_value='', + description='Top-level namespace') declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true', - ) + 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', - ) + description='Full path to the ROS2 parameters file to use for all launched nodes') declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', - default_value='true', - description='Automatically startup the nav2 stack', - ) + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', - default_value='False', - description='Use composed bringup if True', - ) + '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', - ) + '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', - description='Whether to respawn if a node crashes. Applied when composition is disabled.', - ) + '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' - ) + 'log_level', default_value='info', + description='log level') load_nodes = GroupAction( condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ - SetParameter('use_sim_time', use_sim_time), Node( package='nav2_controller', executable='controller_server', @@ -134,8 +119,7 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], - ), + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), Node( package='nav2_smoother', executable='smoother_server', @@ -145,8 +129,7 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - ), + remappings=remappings), Node( package='nav2_planner', executable='planner_server', @@ -156,8 +139,7 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - ), + remappings=remappings), Node( package='nav2_behaviors', executable='behavior_server', @@ -167,8 +149,7 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], - ), + remappings=remappings), Node( package='nav2_bt_navigator', executable='bt_navigator', @@ -178,8 +159,7 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - ), + remappings=remappings), Node( package='nav2_waypoint_follower', executable='waypoint_follower', @@ -189,8 +169,7 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - ), + remappings=remappings), Node( package='nav2_velocity_smoother', executable='velocity_smoother', @@ -200,105 +179,74 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings - + [('cmd_vel', 'cmd_vel_nav')], - ), - Node( - package='nav2_collision_monitor', - executable='collision_monitor', - name='collision_monitor', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - ), + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_navigation', output='screen', arguments=['--ros-args', '--log-level', log_level], - parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], - ), - ], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]), + ] ) - load_composable_nodes = GroupAction( + load_composable_nodes = LoadComposableNodes( condition=IfCondition(use_composition), - actions=[ - SetParameter('use_sim_time', use_sim_time), - LoadComposableNodes( - target_container=container_name_full, - composable_node_descriptions=[ - ComposableNode( - package='nav2_controller', - plugin='nav2_controller::ControllerServer', - name='controller_server', - parameters=[configured_params], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], - ), - ComposableNode( - package='nav2_smoother', - plugin='nav2_smoother::SmootherServer', - name='smoother_server', - parameters=[configured_params], - remappings=remappings, - ), - ComposableNode( - package='nav2_planner', - plugin='nav2_planner::PlannerServer', - name='planner_server', - parameters=[configured_params], - remappings=remappings, - ), - ComposableNode( - package='nav2_behaviors', - plugin='behavior_server::BehaviorServer', - name='behavior_server', - parameters=[configured_params], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], - ), - ComposableNode( - package='nav2_bt_navigator', - plugin='nav2_bt_navigator::BtNavigator', - name='bt_navigator', - parameters=[configured_params], - remappings=remappings, - ), - ComposableNode( - package='nav2_waypoint_follower', - plugin='nav2_waypoint_follower::WaypointFollower', - name='waypoint_follower', - parameters=[configured_params], - remappings=remappings, - ), - ComposableNode( - package='nav2_velocity_smoother', - plugin='nav2_velocity_smoother::VelocitySmoother', - name='velocity_smoother', - parameters=[configured_params], - remappings=remappings - + [('cmd_vel', 'cmd_vel_nav')], - ), - ComposableNode( - package='nav2_collision_monitor', - plugin='nav2_collision_monitor::CollisionMonitor', - name='collision_monitor', - parameters=[configured_params], - remappings=remappings, - ), - ComposableNode( - package='nav2_lifecycle_manager', - plugin='nav2_lifecycle_manager::LifecycleManager', - name='lifecycle_manager_navigation', - parameters=[ - {'autostart': autostart, 'node_names': lifecycle_nodes} - ], - ), - ], - ), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_controller', + plugin='nav2_controller::ControllerServer', + name='controller_server', + parameters=[configured_params], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + ComposableNode( + package='nav2_smoother', + plugin='nav2_smoother::SmootherServer', + name='smoother_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_planner', + plugin='nav2_planner::PlannerServer', + name='planner_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_behaviors', + plugin='behavior_server::BehaviorServer', + name='behavior_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_bt_navigator', + plugin='nav2_bt_navigator::BtNavigator', + name='bt_navigator', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_waypoint_follower', + plugin='nav2_waypoint_follower::WaypointFollower', + name='waypoint_follower', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_velocity_smoother', + plugin='nav2_velocity_smoother::VelocitySmoother', + name='velocity_smoother', + parameters=[configured_params], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_navigation', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), ], ) diff --git a/deploy/nav2_ws/src/sportmode_nav2/params/nav2_params.yaml b/deploy/nav2_ws/src/sportmode_nav2/params/nav2_params.yaml index b27eea8..6602ad5 100644 --- a/deploy/nav2_ws/src/sportmode_nav2/params/nav2_params.yaml +++ b/deploy/nav2_ws/src/sportmode_nav2/params/nav2_params.yaml @@ -1,12 +1,12 @@ amcl: ros__parameters: - use_sim_time: False + use_sim_time: True alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 alpha4: 0.2 alpha5: 0.2 - base_frame_id: "base_link" + base_frame_id: "base_footprint" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 @@ -26,7 +26,7 @@ amcl: recovery_alpha_fast: 0.0 recovery_alpha_slow: 0.0 resample_interval: 1 - robot_model_type: "differential" + robot_model_type: "nav2_amcl::DifferentialMotionModel" save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: true @@ -37,63 +37,87 @@ amcl: 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 + scan_topic: scan bt_navigator: ros__parameters: - use_sim_time: False + use_sim_time: True 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" + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. 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 + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node -bt_navigator_rclcpp_node: +bt_navigator_navigate_through_poses_rclcpp_node: ros__parameters: - use_sim_time: False + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True controller_server: ros__parameters: - use_sim_time: False + use_sim_time: True controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 progress_checker_plugin: "progress_checker" - goal_checker_plugin: "goal_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] # Progress checker parameters @@ -102,11 +126,16 @@ controller_server: required_movement_radius: 0.5 movement_time_allowance: 10.0 # Goal checker parameters - goal_checker: + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 - stateful: True # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" @@ -150,10 +179,6 @@ controller_server: 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: @@ -161,7 +186,7 @@ local_costmap: publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link - use_sim_time: False + use_sim_time: True rolling_window: true width: 3 height: 3 @@ -188,15 +213,14 @@ local_costmap: clearing: True marking: True data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 static_layer: + plugin: "nav2_costmap_2d::StaticLayer" 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: @@ -205,7 +229,7 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - use_sim_time: False + use_sim_time: True robot_radius: 0.22 resolution: 0.05 track_unknown_space: true @@ -220,6 +244,10 @@ global_costmap: clearing: True marking: True data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True @@ -228,30 +256,26 @@ global_costmap: 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" + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" map_saver: ros__parameters: - use_sim_time: False - save_map_timeout: 5000 + use_sim_time: True + save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 - map_subscribe_transient_local: False + map_subscribe_transient_local: True planner_server: ros__parameters: expected_planner_frequency: 20.0 - use_sim_time: False + use_sim_time: True planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" @@ -259,26 +283,36 @@ planner_server: use_astar: false allow_unknown: true -planner_server_rclcpp_node: +smoother_server: ros__parameters: - use_sim_time: False + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True -recoveries_server: +behavior_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"] + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: - plugin: "nav2_recoveries/Spin" - back_up: - plugin: "nav2_recoveries/BackUp" + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" wait: - plugin: "nav2_recoveries/Wait" + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" global_frame: odom robot_base_frame: base_link - transform_timeout: 0.1 - use_sim_time: False + transform_tolerance: 0.1 + use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 @@ -286,4 +320,30 @@ recoveries_server: robot_state_publisher: ros__parameters: - use_sim_time: False + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/nav2_docker_run.sh b/nav2_docker_run.sh index eb99f5f..457c178 100755 --- a/nav2_docker_run.sh +++ b/nav2_docker_run.sh @@ -1 +1 @@ -docker run --rm -it --privileged --network host -v $(pwd)/deploy/nav2_ws:/home/nav2_ws -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro --runtime nvidia go2py_nav:latest \ No newline at end of file +docker run --rm -it --privileged --network host -v $(pwd)/deploy/nav2_ws/src:/home/nav2_ws/src -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro --runtime nvidia go2py_nav:latest