diff --git a/deploy/nav2_ws/sportmode_nav2_new/CMakeLists.txt b/deploy/nav2_ws/sportmode_nav2_new/CMakeLists.txt
deleted file mode 100644
index b214568..0000000
--- a/deploy/nav2_ws/sportmode_nav2_new/CMakeLists.txt
+++ /dev/null
@@ -1,63 +0,0 @@
-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
deleted file mode 100644
index 839f1ce..0000000
--- a/deploy/nav2_ws/sportmode_nav2_new/launch/localization.launch.py
+++ /dev/null
@@ -1,27 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-
- 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
deleted file mode 100644
index 70db31e..0000000
--- a/deploy/nav2_ws/sportmode_nav2_new/launch/localization_async.launch.py
+++ /dev/null
@@ -1,59 +0,0 @@
-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
deleted file mode 100644
index 526fc21..0000000
--- a/deploy/nav2_ws/sportmode_nav2_new/launch/mapping.launch.py
+++ /dev/null
@@ -1,27 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-
- 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
deleted file mode 100644
index 939c31a..0000000
--- a/deploy/nav2_ws/sportmode_nav2_new/launch/mapping_async.launch.py
+++ /dev/null
@@ -1,59 +0,0 @@
-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
deleted file mode 100644
index c48a741..0000000
--- a/deploy/nav2_ws/sportmode_nav2_new/launch/navigation.launch.py
+++ /dev/null
@@ -1,146 +0,0 @@
-# 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
deleted file mode 100644
index 685b1c6..0000000
--- a/deploy/nav2_ws/sportmode_nav2_new/launch/navigation_launch.py
+++ /dev/null
@@ -1,272 +0,0 @@
-# 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
deleted file mode 100644
index 1ee570a..0000000
--- a/deploy/nav2_ws/sportmode_nav2_new/launch/ros_ekf.launch.py
+++ /dev/null
@@ -1,35 +0,0 @@
-# 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
deleted file mode 100644
index 6e7a413..0000000
--- a/deploy/nav2_ws/sportmode_nav2_new/package.xml
+++ /dev/null
@@ -1,22 +0,0 @@
-
-
-
- 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
deleted file mode 100644
index 3506980..0000000
--- a/deploy/nav2_ws/sportmode_nav2_new/params/localization_async.yaml
+++ /dev/null
@@ -1,73 +0,0 @@
-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
deleted file mode 100644
index b4dfd4c..0000000
--- a/deploy/nav2_ws/sportmode_nav2_new/params/mapping_async.yaml
+++ /dev/null
@@ -1,73 +0,0 @@
-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
deleted file mode 100644
index 6602ad5..0000000
--- a/deploy/nav2_ws/sportmode_nav2_new/params/nav2_params.yaml
+++ /dev/null
@@ -1,349 +0,0 @@
-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
deleted file mode 100644
index f1b2cfd..0000000
--- a/deploy/nav2_ws/sportmode_nav2_new/params/ros_ekf.yaml
+++ /dev/null
@@ -1,219 +0,0 @@
-### 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]
-