EmbodiedAI
/
Go2Py_SIM
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

nav2 bugs fixed

This commit is contained in:
Rooholla-KhorramBakht 2024-04-24 06:51:05 +08:00
parent 7a8ec45cbc
commit 5b5eb831fa
16 changed files with 1662 additions and 230 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
deploy/nav2_ws/sportmode_nav2_new/CMakeLists.txt Normal file
View File

@ -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()

27
deploy/nav2_ws/sportmode_nav2_new/launch/localization.launch.py Normal file
View File

@ -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,
])

59
deploy/nav2_ws/sportmode_nav2_new/launch/localization_async.launch.py Normal file
View File

@ -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

27
deploy/nav2_ws/sportmode_nav2_new/launch/mapping.launch.py Normal file
View File

@ -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,
])

59
deploy/nav2_ws/sportmode_nav2_new/launch/mapping_async.launch.py Normal file
View File

@ -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

146
deploy/nav2_ws/sportmode_nav2_new/launch/navigation.launch.py Normal file
View File

@ -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}]),
])

272
deploy/nav2_ws/sportmode_nav2_new/launch/navigation_launch.py Normal file
View File

@ -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

35
deploy/nav2_ws/sportmode_nav2_new/launch/ros_ekf.launch.py Normal file
View File

@ -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')],
),
])

22
deploy/nav2_ws/sportmode_nav2_new/package.xml Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>sportmode_nav2</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="zeendi@163.com">zeen</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>robot_locoalization</test_depend>
<test_depend>slam_toolbox</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

73
deploy/nav2_ws/sportmode_nav2_new/params/localization_async.yaml Normal file
View File

@ -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

73
deploy/nav2_ws/sportmode_nav2_new/params/mapping_async.yaml Normal file
View File

@ -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

349
deploy/nav2_ws/sportmode_nav2_new/params/nav2_params.yaml Normal file
View File

@ -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

219
deploy/nav2_ws/sportmode_nav2_new/params/ros_ekf.yaml Normal file
View File

@ -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<double>::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]

244
deploy/nav2_ws/src/sportmode_nav2/launch/navigation_launch.py
View File

@ -20,7 +20,7 @@ from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
from launch.conditions import IfCondition from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PythonExpression 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.actions import Node
from launch_ros.descriptions import ComposableNode, ParameterFile from launch_ros.descriptions import ComposableNode, ParameterFile
from nav2_common.launch import RewrittenYaml from nav2_common.launch import RewrittenYaml
@ -40,16 +40,13 @@ def generate_launch_description():
use_respawn = LaunchConfiguration('use_respawn') use_respawn = LaunchConfiguration('use_respawn')
log_level = LaunchConfiguration('log_level') log_level = LaunchConfiguration('log_level')
lifecycle_nodes = [ lifecycle_nodes = ['controller_server',
'controller_server', 'smoother_server',
'smoother_server', 'planner_server',
'planner_server', 'behavior_server',
'behavior_server', 'bt_navigator',
'velocity_smoother', 'waypoint_follower',
'collision_monitor', 'velocity_smoother']
'bt_navigator',
'waypoint_follower',
]
# Map fully qualified names to relative ones so the node's namespace can be prepended. # 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 # 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 # https://github.com/ros/robot_state_publisher/pull/30
# TODO(orduno) Substitute with `PushNodeRemapping` # TODO(orduno) Substitute with `PushNodeRemapping`
# https://github.com/ros2/launch_ros/issues/56 # https://github.com/ros2/launch_ros/issues/56
remappings = [('/tf', '/tf'), remappings = [('/tf', 'tf'),
('/tf_static', '/tf_static'), ('/tf_static', 'tf_static'),('/cmd_vel', '/go2/cmd_vel')]
('/cmd_vel', '/go2/cmd_vel')]
# Create our own temporary YAML files that include substitutions # 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( configured_params = ParameterFile(
RewrittenYaml( RewrittenYaml(
source_file=params_file, source_file=params_file,
root_key=namespace, root_key=namespace,
param_rewrites=param_substitutions, param_rewrites=param_substitutions,
convert_types=True, convert_types=True),
), allow_substs=True)
allow_substs=True,
)
stdout_linebuf_envvar = SetEnvironmentVariable( stdout_linebuf_envvar = SetEnvironmentVariable(
'RCUTILS_LOGGING_BUFFERED_STREAM', '1' 'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
)
declare_namespace_cmd = DeclareLaunchArgument( declare_namespace_cmd = DeclareLaunchArgument(
'namespace', default_value='', description='Top-level namespace' 'namespace',
) default_value='',
description='Top-level namespace')
declare_use_sim_time_cmd = DeclareLaunchArgument( declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time', 'use_sim_time',
default_value='false', default_value='false',
description='Use simulation (Gazebo) clock if true', description='Use simulation (Gazebo) clock if true')
)
declare_params_file_cmd = DeclareLaunchArgument( declare_params_file_cmd = DeclareLaunchArgument(
'params_file', 'params_file',
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), 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( declare_autostart_cmd = DeclareLaunchArgument(
'autostart', 'autostart', default_value='true',
default_value='true', description='Automatically startup the nav2 stack')
description='Automatically startup the nav2 stack',
)
declare_use_composition_cmd = DeclareLaunchArgument( declare_use_composition_cmd = DeclareLaunchArgument(
'use_composition', 'use_composition', default_value='False',
default_value='False', description='Use composed bringup if True')
description='Use composed bringup if True',
)
declare_container_name_cmd = DeclareLaunchArgument( declare_container_name_cmd = DeclareLaunchArgument(
'container_name', 'container_name', default_value='nav2_container',
default_value='nav2_container', description='the name of conatiner that nodes will load in if use composition')
description='the name of conatiner that nodes will load in if use composition',
)
declare_use_respawn_cmd = DeclareLaunchArgument( declare_use_respawn_cmd = DeclareLaunchArgument(
'use_respawn', 'use_respawn', default_value='False',
default_value='False', description='Whether to respawn if a node crashes. Applied when composition is disabled.')
description='Whether to respawn if a node crashes. Applied when composition is disabled.',
)
declare_log_level_cmd = DeclareLaunchArgument( declare_log_level_cmd = DeclareLaunchArgument(
'log_level', default_value='info', description='log level' 'log_level', default_value='info',
) description='log level')
load_nodes = GroupAction( load_nodes = GroupAction(
condition=IfCondition(PythonExpression(['not ', use_composition])), condition=IfCondition(PythonExpression(['not ', use_composition])),
actions=[ actions=[
SetParameter('use_sim_time', use_sim_time),
Node( Node(
package='nav2_controller', package='nav2_controller',
executable='controller_server', executable='controller_server',
@ -134,8 +119,7 @@ def generate_launch_description():
respawn_delay=2.0, respawn_delay=2.0,
parameters=[configured_params], parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level], arguments=['--ros-args', '--log-level', log_level],
remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]),
),
Node( Node(
package='nav2_smoother', package='nav2_smoother',
executable='smoother_server', executable='smoother_server',
@ -145,8 +129,7 @@ def generate_launch_description():
respawn_delay=2.0, respawn_delay=2.0,
parameters=[configured_params], parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level], arguments=['--ros-args', '--log-level', log_level],
remappings=remappings, remappings=remappings),
),
Node( Node(
package='nav2_planner', package='nav2_planner',
executable='planner_server', executable='planner_server',
@ -156,8 +139,7 @@ def generate_launch_description():
respawn_delay=2.0, respawn_delay=2.0,
parameters=[configured_params], parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level], arguments=['--ros-args', '--log-level', log_level],
remappings=remappings, remappings=remappings),
),
Node( Node(
package='nav2_behaviors', package='nav2_behaviors',
executable='behavior_server', executable='behavior_server',
@ -167,8 +149,7 @@ def generate_launch_description():
respawn_delay=2.0, respawn_delay=2.0,
parameters=[configured_params], parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level], arguments=['--ros-args', '--log-level', log_level],
remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], remappings=remappings),
),
Node( Node(
package='nav2_bt_navigator', package='nav2_bt_navigator',
executable='bt_navigator', executable='bt_navigator',
@ -178,8 +159,7 @@ def generate_launch_description():
respawn_delay=2.0, respawn_delay=2.0,
parameters=[configured_params], parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level], arguments=['--ros-args', '--log-level', log_level],
remappings=remappings, remappings=remappings),
),
Node( Node(
package='nav2_waypoint_follower', package='nav2_waypoint_follower',
executable='waypoint_follower', executable='waypoint_follower',
@ -189,8 +169,7 @@ def generate_launch_description():
respawn_delay=2.0, respawn_delay=2.0,
parameters=[configured_params], parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level], arguments=['--ros-args', '--log-level', log_level],
remappings=remappings, remappings=remappings),
),
Node( Node(
package='nav2_velocity_smoother', package='nav2_velocity_smoother',
executable='velocity_smoother', executable='velocity_smoother',
@ -200,105 +179,74 @@ def generate_launch_description():
respawn_delay=2.0, respawn_delay=2.0,
parameters=[configured_params], parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level], arguments=['--ros-args', '--log-level', log_level],
remappings=remappings remappings=remappings +
+ [('cmd_vel', 'cmd_vel_nav')], [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
),
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,
),
Node( Node(
package='nav2_lifecycle_manager', package='nav2_lifecycle_manager',
executable='lifecycle_manager', executable='lifecycle_manager',
name='lifecycle_manager_navigation', name='lifecycle_manager_navigation',
output='screen', output='screen',
arguments=['--ros-args', '--log-level', log_level], 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), condition=IfCondition(use_composition),
actions=[ target_container=container_name_full,
SetParameter('use_sim_time', use_sim_time), composable_node_descriptions=[
LoadComposableNodes( ComposableNode(
target_container=container_name_full, package='nav2_controller',
composable_node_descriptions=[ plugin='nav2_controller::ControllerServer',
ComposableNode( name='controller_server',
package='nav2_controller', parameters=[configured_params],
plugin='nav2_controller::ControllerServer', remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]),
name='controller_server', ComposableNode(
parameters=[configured_params], package='nav2_smoother',
remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], plugin='nav2_smoother::SmootherServer',
), name='smoother_server',
ComposableNode( parameters=[configured_params],
package='nav2_smoother', remappings=remappings),
plugin='nav2_smoother::SmootherServer', ComposableNode(
name='smoother_server', package='nav2_planner',
parameters=[configured_params], plugin='nav2_planner::PlannerServer',
remappings=remappings, name='planner_server',
), parameters=[configured_params],
ComposableNode( remappings=remappings),
package='nav2_planner', ComposableNode(
plugin='nav2_planner::PlannerServer', package='nav2_behaviors',
name='planner_server', plugin='behavior_server::BehaviorServer',
parameters=[configured_params], name='behavior_server',
remappings=remappings, parameters=[configured_params],
), remappings=remappings),
ComposableNode( ComposableNode(
package='nav2_behaviors', package='nav2_bt_navigator',
plugin='behavior_server::BehaviorServer', plugin='nav2_bt_navigator::BtNavigator',
name='behavior_server', name='bt_navigator',
parameters=[configured_params], parameters=[configured_params],
remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], remappings=remappings),
), ComposableNode(
ComposableNode( package='nav2_waypoint_follower',
package='nav2_bt_navigator', plugin='nav2_waypoint_follower::WaypointFollower',
plugin='nav2_bt_navigator::BtNavigator', name='waypoint_follower',
name='bt_navigator', parameters=[configured_params],
parameters=[configured_params], remappings=remappings),
remappings=remappings, ComposableNode(
), package='nav2_velocity_smoother',
ComposableNode( plugin='nav2_velocity_smoother::VelocitySmoother',
package='nav2_waypoint_follower', name='velocity_smoother',
plugin='nav2_waypoint_follower::WaypointFollower', parameters=[configured_params],
name='waypoint_follower', remappings=remappings +
parameters=[configured_params], [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
remappings=remappings, ComposableNode(
), package='nav2_lifecycle_manager',
ComposableNode( plugin='nav2_lifecycle_manager::LifecycleManager',
package='nav2_velocity_smoother', name='lifecycle_manager_navigation',
plugin='nav2_velocity_smoother::VelocitySmoother', parameters=[{'use_sim_time': use_sim_time,
name='velocity_smoother', 'autostart': autostart,
parameters=[configured_params], 'node_names': lifecycle_nodes}]),
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}
],
),
],
),
], ],
) )

222
deploy/nav2_ws/src/sportmode_nav2/params/nav2_params.yaml
View File

@ -1,12 +1,12 @@
amcl: amcl:
ros__parameters: ros__parameters:
use_sim_time: False use_sim_time: True
alpha1: 0.2 alpha1: 0.2
alpha2: 0.2 alpha2: 0.2
alpha3: 0.2 alpha3: 0.2
alpha4: 0.2 alpha4: 0.2
alpha5: 0.2 alpha5: 0.2
base_frame_id: "base_link" base_frame_id: "base_footprint"
beam_skip_distance: 0.5 beam_skip_distance: 0.5
beam_skip_error_threshold: 0.9 beam_skip_error_threshold: 0.9
beam_skip_threshold: 0.3 beam_skip_threshold: 0.3
@ -26,7 +26,7 @@ amcl:
recovery_alpha_fast: 0.0 recovery_alpha_fast: 0.0
recovery_alpha_slow: 0.0 recovery_alpha_slow: 0.0
resample_interval: 1 resample_interval: 1
robot_model_type: "differential" robot_model_type: "nav2_amcl::DifferentialMotionModel"
save_pose_rate: 0.5 save_pose_rate: 0.5
sigma_hit: 0.2 sigma_hit: 0.2
tf_broadcast: true tf_broadcast: true
@ -37,63 +37,87 @@ amcl:
z_max: 0.05 z_max: 0.05
z_rand: 0.5 z_rand: 0.5
z_short: 0.05 z_short: 0.05
scan_topic: /go2/scan scan_topic: scan
amcl_map_client:
ros__parameters:
use_sim_time: False
amcl_rclcpp_node:
ros__parameters:
use_sim_time: False
bt_navigator: bt_navigator:
ros__parameters: ros__parameters:
use_sim_time: False use_sim_time: True
global_frame: map global_frame: map
robot_base_frame: base_link robot_base_frame: base_link
odom_topic: /utlidar/robot_odom odom_topic: /odom
enable_groot_monitoring: True bt_loop_duration: 10
groot_zmq_publisher_port: 1666 default_server_timeout: 20
groot_zmq_server_port: 1667 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" # 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: plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node - nav2_compute_path_to_pose_action_bt_node
- nav2_follow_path_action_bt_node - nav2_compute_path_through_poses_action_bt_node
- nav2_back_up_action_bt_node - nav2_smooth_path_action_bt_node
- nav2_spin_action_bt_node - nav2_follow_path_action_bt_node
- nav2_wait_action_bt_node - nav2_spin_action_bt_node
- nav2_clear_costmap_service_bt_node - nav2_wait_action_bt_node
- nav2_is_stuck_condition_bt_node - nav2_assisted_teleop_action_bt_node
- nav2_goal_reached_condition_bt_node - nav2_back_up_action_bt_node
- nav2_goal_updated_condition_bt_node - nav2_drive_on_heading_bt_node
- nav2_initial_pose_received_condition_bt_node - nav2_clear_costmap_service_bt_node
- nav2_reinitialize_global_localization_service_bt_node - nav2_is_stuck_condition_bt_node
- nav2_rate_controller_bt_node - nav2_goal_reached_condition_bt_node
- nav2_distance_controller_bt_node - nav2_goal_updated_condition_bt_node
- nav2_speed_controller_bt_node - nav2_globally_updated_goal_condition_bt_node
- nav2_truncate_path_action_bt_node - nav2_is_path_valid_condition_bt_node
- nav2_goal_updater_node_bt_node - nav2_initial_pose_received_condition_bt_node
- nav2_recovery_node_bt_node - nav2_reinitialize_global_localization_service_bt_node
- nav2_pipeline_sequence_bt_node - nav2_rate_controller_bt_node
- nav2_round_robin_node_bt_node - nav2_distance_controller_bt_node
- nav2_transform_available_condition_bt_node - nav2_speed_controller_bt_node
- nav2_time_expired_condition_bt_node - nav2_truncate_path_action_bt_node
- nav2_distance_traveled_condition_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: 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: controller_server:
ros__parameters: ros__parameters:
use_sim_time: False use_sim_time: True
controller_frequency: 20.0 controller_frequency: 20.0
min_x_velocity_threshold: 0.001 min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5 min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001 min_theta_velocity_threshold: 0.001
failure_tolerance: 0.3
progress_checker_plugin: "progress_checker" progress_checker_plugin: "progress_checker"
goal_checker_plugin: "goal_checker" goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
controller_plugins: ["FollowPath"] controller_plugins: ["FollowPath"]
# Progress checker parameters # Progress checker parameters
@ -102,11 +126,16 @@ controller_server:
required_movement_radius: 0.5 required_movement_radius: 0.5
movement_time_allowance: 10.0 movement_time_allowance: 10.0
# Goal checker parameters # 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" plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25 xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25
stateful: True
# DWB parameters # DWB parameters
FollowPath: FollowPath:
plugin: "dwb_core::DWBLocalPlanner" plugin: "dwb_core::DWBLocalPlanner"
@ -150,10 +179,6 @@ controller_server:
RotateToGoal.slowing_factor: 5.0 RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0 RotateToGoal.lookahead_time: -1.0
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: False
local_costmap: local_costmap:
local_costmap: local_costmap:
ros__parameters: ros__parameters:
@ -161,7 +186,7 @@ local_costmap:
publish_frequency: 2.0 publish_frequency: 2.0
global_frame: odom global_frame: odom
robot_base_frame: base_link robot_base_frame: base_link
use_sim_time: False use_sim_time: True
rolling_window: true rolling_window: true
width: 3 width: 3
height: 3 height: 3
@ -188,15 +213,14 @@ local_costmap:
clearing: True clearing: True
marking: True marking: True
data_type: "LaserScan" 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: static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True map_subscribe_transient_local: True
always_send_full_costmap: 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:
global_costmap: global_costmap:
@ -205,7 +229,7 @@ global_costmap:
publish_frequency: 1.0 publish_frequency: 1.0
global_frame: map global_frame: map
robot_base_frame: base_link robot_base_frame: base_link
use_sim_time: False use_sim_time: True
robot_radius: 0.22 robot_radius: 0.22
resolution: 0.05 resolution: 0.05
track_unknown_space: true track_unknown_space: true
@ -220,6 +244,10 @@ global_costmap:
clearing: True clearing: True
marking: True marking: True
data_type: "LaserScan" 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: static_layer:
plugin: "nav2_costmap_2d::StaticLayer" plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True map_subscribe_transient_local: True
@ -228,30 +256,26 @@ global_costmap:
cost_scaling_factor: 3.0 cost_scaling_factor: 3.0
inflation_radius: 0.55 inflation_radius: 0.55
always_send_full_costmap: True 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: map_server:
ros__parameters: ros__parameters:
use_sim_time: False use_sim_time: True
yaml_filename: "/home/unitree/Generated_Maps/basement/hallway-basement.yaml" # 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: map_saver:
ros__parameters: ros__parameters:
use_sim_time: False use_sim_time: True
save_map_timeout: 5000 save_map_timeout: 5.0
free_thresh_default: 0.25 free_thresh_default: 0.25
occupied_thresh_default: 0.65 occupied_thresh_default: 0.65
map_subscribe_transient_local: False map_subscribe_transient_local: True
planner_server: planner_server:
ros__parameters: ros__parameters:
expected_planner_frequency: 20.0 expected_planner_frequency: 20.0
use_sim_time: False use_sim_time: True
planner_plugins: ["GridBased"] planner_plugins: ["GridBased"]
GridBased: GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner" plugin: "nav2_navfn_planner/NavfnPlanner"
@ -259,26 +283,36 @@ planner_server:
use_astar: false use_astar: false
allow_unknown: true allow_unknown: true
planner_server_rclcpp_node: smoother_server:
ros__parameters: 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: ros__parameters:
costmap_topic: local_costmap/costmap_raw costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0 cycle_frequency: 10.0
recovery_plugins: ["spin", "back_up", "wait"] behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
spin: spin:
plugin: "nav2_recoveries/Spin" plugin: "nav2_behaviors/Spin"
back_up: backup:
plugin: "nav2_recoveries/BackUp" plugin: "nav2_behaviors/BackUp"
drive_on_heading:
plugin: "nav2_behaviors/DriveOnHeading"
wait: wait:
plugin: "nav2_recoveries/Wait" plugin: "nav2_behaviors/Wait"
assisted_teleop:
plugin: "nav2_behaviors/AssistedTeleop"
global_frame: odom global_frame: odom
robot_base_frame: base_link robot_base_frame: base_link
transform_timeout: 0.1 transform_tolerance: 0.1
use_sim_time: False use_sim_time: true
simulate_ahead_time: 2.0 simulate_ahead_time: 2.0
max_rotational_vel: 1.0 max_rotational_vel: 1.0
min_rotational_vel: 0.4 min_rotational_vel: 0.4
@ -286,4 +320,30 @@ recoveries_server:
robot_state_publisher: robot_state_publisher:
ros__parameters: 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

2
nav2_docker_run.sh
View File

@ -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 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