diff --git a/Makefile b/Makefile
index 98562d8..63b06df 100644
--- a/Makefile
+++ b/Makefile
@@ -5,7 +5,7 @@ docker_start:
@./scripts/run_dev.sh
nav2:
- @cd deploy && docker build --no-cache --tag go2py_nav2:latest -f docker/Dockerfile.nav2 .
+ @cd deploy && docker build --tag go2py_nav2:latest -f docker/Dockerfile.nav2 .
nav2_start:
@ ./scripts/run_nav2.sh
@@ -64,4 +64,4 @@ bridge_uninstall:
robot_description_uninstall:
@systemctl disable go2py-robot-description.service
@systemctl stop go2py-robot-description.service
- @rm /etc/systemd/system/go2py-robot-description.service
\ No newline at end of file
+ @rm /etc/systemd/system/go2py-robot-description.service
diff --git a/deploy/docker/Dockerfile.nav2 b/deploy/docker/Dockerfile.nav2
index 0366e92..048d454 100644
--- a/deploy/docker/Dockerfile.nav2
+++ b/deploy/docker/Dockerfile.nav2
@@ -1,37 +1,149 @@
# FROM isaac_ros_dev-aarch64
-FROM ros:humble
-ENV DEBIAN_FRONTEND=noninteractive
-SHELL ["/bin/bash", "-c"]
-# uodate and install dependencies
-RUN apt-get update && apt-get install -y \
- ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \
- libyaml-cpp-dev \
- libboost-all-dev\
- build-essential \
- cmake \
- git \
- python3-pip \
- ros-humble-nav2-* ros-humble-navigation2 \
- ros-humble-slam-toolbox \
- ros-humble-robot-localization\
- ros-humble-image-geometry \
- ros-humble-test-msgs \
+# This dockerfile can be configured via --build-arg
+# Build context must be the /navigation2 root folder for COPY.
+# Example build command:
+# export UNDERLAY_MIXINS="debug ccache lld"
+# export OVERLAY_MIXINS="debug ccache coverage-gcc lld"
+# docker build -t nav2:latest \
+# --build-arg UNDERLAY_MIXINS \
+# --build-arg OVERLAY_MIXINS ./
+ARG FROM_IMAGE=ros:rolling
+ARG UNDERLAY_WS=/opt/underlay_ws
+ARG OVERLAY_WS=/opt/overlay_ws
+
+# multi-stage for caching
+FROM $FROM_IMAGE AS cacher
+
+# clone underlay source
+ARG UNDERLAY_WS
+WORKDIR $UNDERLAY_WS/src
+# Clone the repositories
+RUN cd /home && mkdir nav2_ws && cd nav2_ws && mkdir src && cd src && \
+ git clone -b master https://github.com/BehaviorTree/BehaviorTree.CPP.git && \
+ git clone -b ros2 https://github.com/ros/angles.git && \
+ git clone -b rolling https://github.com/ros-perception/vision_opencv.git && \
+ git clone -b ros2 https://github.com/ros/bond_core.git && \
+ git clone -b ros2 https://github.com/ros/diagnostics.git && \
+ git clone -b ros2 https://github.com/ros-geographic-info/geographic_info.git && \
+ git clone -b main https://github.com/ompl/ompl.git && \
+ git clone -b ros2 https://github.com/cra-ros-pkg/robot_localization.git && \
+ git clone -b 1.0.1 https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git
+
+# copy overlay source
+ARG OVERLAY_WS
+WORKDIR $OVERLAY_WS/src
+RUN git clone https://github.com/ros-navigation/navigation2.git
+
+# copy manifests for caching
+WORKDIR /opt
+RUN find . -name "src" -type d \
+ -mindepth 1 -maxdepth 2 -printf '%P\n' \
+ | xargs -I % mkdir -p /tmp/opt/% && \
+ find . -name "package.xml" \
+ | xargs cp --parents -t /tmp/opt && \
+ find . -name "COLCON_IGNORE" \
+ | xargs cp --parents -t /tmp/opt || true
+
+# multi-stage for building
+FROM $FROM_IMAGE AS builder
+
+# config dependencies install
+ARG DEBIAN_FRONTEND=noninteractive
+RUN echo '\
+APT::Install-Recommends "0";\n\
+APT::Install-Suggests "0";\n\
+' > /etc/apt/apt.conf.d/01norecommend
+ENV PYTHONUNBUFFERED 1
+
+# install CI dependencies
+ARG RTI_NC_LICENSE_ACCEPTED=yes
+RUN apt-get update && \
+ apt-get upgrade -y --with-new-pkgs && \
+ apt-get install -y \
+ ccache \
+ lcov \
+ lld \
+ python3-pip \
+ ros-$ROS_DISTRO-rmw-fastrtps-cpp \
+ ros-$ROS_DISTRO-rmw-connextdds \
+ ros-$ROS_DISTRO-rmw-cyclonedds-cpp \
+ ros-$ROS_DISTRO-rmw-cyclonedds-cpp\
+ ros-$ROS_DISTRO-rosidl-generator-dds-idl \
+ && pip3 install --break-system-packages \
+ fastcov \
+ git+https://github.com/ruffsl/colcon-cache.git@a937541bfc496c7a267db7ee9d6cceca61e470ca \
+ git+https://github.com/ruffsl/colcon-clean.git@a7f1074d1ebc1a54a6508625b117974f2672f2a9 \
+ && rosdep update \
+ && colcon mixin update \
+ && colcon metadata update \
&& rm -rf /var/lib/apt/lists/*
-# Cheange the ROS2 RMW to CycloneDDS as instructed by Unitree
-RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /unitree_ros2/cyclonedds_ws/src && \
-git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\
-cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build
+# install underlay dependencies
+ARG UNDERLAY_WS
+ENV UNDERLAY_WS $UNDERLAY_WS
+WORKDIR $UNDERLAY_WS
+COPY --from=cacher /tmp/$UNDERLAY_WS ./
+RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
+ apt-get update && rosdep install -q -y \
+ --from-paths src \
+ --skip-keys " \
+ slam_toolbox \
+ " \
+ --ignore-src \
+ && rm -rf /var/lib/apt/lists/*
+
+# build underlay source
+COPY --from=cacher $UNDERLAY_WS ./
+ARG UNDERLAY_MIXINS="release ccache lld"
+ARG CCACHE_DIR="$UNDERLAY_WS/.ccache"
+RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
+ colcon cache lock && \
+ colcon build \
+ --symlink-install \
+ --mixin $UNDERLAY_MIXINS \
+ --event-handlers console_direct+ \
+ --cmake-args -DCMAKE_BUILD_TYPE=Release
+
+# install overlay dependencies
+ARG OVERLAY_WS
+ENV OVERLAY_WS $OVERLAY_WS
+WORKDIR $OVERLAY_WS
+COPY --from=cacher /tmp/$OVERLAY_WS ./
+
+RUN . $UNDERLAY_WS/install/setup.sh && \
+ apt-get update && rosdep install -q -y \
+ --from-paths src \
+ --skip-keys " \
+ slam_toolbox \
+ "\
+ --ignore-src \
+ && rm -rf /var/lib/apt/lists/*
+
+# multi-stage for testing
+FROM builder AS tester
+
+# build overlay source
+COPY --from=cacher $OVERLAY_WS ./
+ARG OVERLAY_MIXINS="release ccache lld"
+ARG CCACHE_DIR="$OVERLAY_WS/.ccache"
+RUN . $UNDERLAY_WS/install/setup.sh && \
+ colcon cache lock && \
+ colcon build \
+ --symlink-install \
+ --mixin $OVERLAY_MIXINS \
+ --cmake-args -DCMAKE_BUILD_TYPE=Release
+
+# source overlay from entrypoint
+RUN sed --in-place \
+ 's|^source .*|source "$OVERLAY_WS/install/setup.bash"|' \
+ /ros_entrypoint.sh
+
# copy the go2py ros2 nodes
-COPY ros2_nodes/sportmode_nav2 /home/nav2_ws/src/sportmode_nav2
COPY ros2_nodes/m-explore-ros2 /home/nav2_ws/src/m-explore-ros2
-COPY ros2_nodes/go2py_messages /home/nav2_ws/src/go2py_messages
-RUN cd /home/nav2_ws && source /opt/ros/humble/setup.bash && source /unitree_ros2/cyclonedds_ws/install/setup.bash && colcon build --symlink-install
+# COPY ros2_nodes/sportmode_nav2 /home/nav2_ws/src/sportmode_nav2
+
+# RUN cd /home/nav2_ws && source /opt/ros/rolling/setup.bash && \
+# colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install
-RUN cd / && \
- python3 -m pip install --upgrade pip && \
- python3 -m pip install numpy scipy paramiko openai
-
-# set the entrypoint to bash
ENTRYPOINT ["/bin/bash"]
diff --git a/deploy/launch_files/utlidar_to_laserscan.py b/deploy/launch_files/utlidar_to_laserscan.py
new file mode 100644
index 0000000..042bafd
--- /dev/null
+++ b/deploy/launch_files/utlidar_to_laserscan.py
@@ -0,0 +1,32 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ return LaunchDescription([
+ DeclareLaunchArgument(
+ name='scanner', default_value='scanner',
+ description='Namespace for sample topics'
+ ),
+ Node(
+ package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node',
+ remappings=[('cloud_in', '/utlidar/cloud'), ('scan', '/utlidar/scan')],
+ parameters=[{
+ 'target_frame': 'base_link',
+ 'transform_tolerance': 0.01,
+ 'min_height': -0.15,
+ 'max_height': 1.3,
+ 'angle_min': -1.5708, # -M_PI/2
+ 'angle_max': 1.5708, # M_PI/2
+ 'angle_increment': 0.1, # M_PI/360.0
+ 'scan_time': 0.3333,
+ 'range_min': 0.1,
+ 'range_max': 20.0,
+ 'use_inf': True,
+ 'inf_epsilon': 1.0
+ }],
+ name='pointcloud_to_laserscan'
+ )
+ ])
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2_description/xacro/robot.xacro b/deploy/ros2_nodes/go2_description/xacro/robot.xacro
index 10bb0cb..fe8ca35 100755
--- a/deploy/ros2_nodes/go2_description/xacro/robot.xacro
+++ b/deploy/ros2_nodes/go2_description/xacro/robot.xacro
@@ -157,7 +157,7 @@
-
+
diff --git a/deploy/ros2_nodes/lidar_node/src/manager/source_driver_ros2.hpp b/deploy/ros2_nodes/lidar_node/src/manager/source_driver_ros2.hpp
index b6262ae..1baa3c2 100644
--- a/deploy/ros2_nodes/lidar_node/src/manager/source_driver_ros2.hpp
+++ b/deploy/ros2_nodes/lidar_node/src/manager/source_driver_ros2.hpp
@@ -144,7 +144,7 @@ inline void SourceDriver::Init(const YAML::Node& config)
YamlRead(config["ros"], "ros_send_packet_topic", ros_send_packet_topic, "hesai_packets");
pkt_pub_ = node_ptr_->create_publisher(ros_send_packet_topic, 1);
}
- laserscan_pub_ = node_ptr_->create_publisher("/go2/scan", rclcpp::SensorDataQoS());
+ laserscan_pub_ = node_ptr_->create_publisher("/go2/scan", 1);
if (driver_param.input_param.source_type == DATA_FROM_ROS_PACKET) {
std::string ros_recv_packet_topic;
YamlRead(config["ros"], "ros_recv_packet_topic", ros_recv_packet_topic, "hesai_packets");
diff --git a/deploy/ros2_nodes/sportmode_nav2/launch/localization.launch.py b/deploy/ros2_nodes/sportmode_nav2/launch/localization.launch.py
deleted file mode 100644
index c7b62de..0000000
--- a/deploy/ros2_nodes/sportmode_nav2/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,
- ])
diff --git a/deploy/ros2_nodes/sportmode_nav2/launch/mapping_async.launch.py b/deploy/ros2_nodes/sportmode_nav2/launch/mapping_async.launch.py
index b8c5cfb..79ca198 100644
--- a/deploy/ros2_nodes/sportmode_nav2/launch/mapping_async.launch.py
+++ b/deploy/ros2_nodes/sportmode_nav2/launch/mapping_async.launch.py
@@ -1,59 +1,89 @@
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
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, EmitEvent, LogInfo,
+ RegisterEventHandler)
+from launch.conditions import IfCondition
+from launch.events import matches_action
+from launch.substitutions import (AndSubstitution, LaunchConfiguration,
+ NotSubstitution)
+from launch_ros.actions import LifecycleNode
+from launch_ros.event_handlers import OnStateTransition
+from launch_ros.events.lifecycle import ChangeState
+from lifecycle_msgs.msg import Transition
def generate_launch_description():
+ autostart = LaunchConfiguration('autostart')
+ use_lifecycle_manager = LaunchConfiguration("use_lifecycle_manager")
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')
+ slam_params_file = LaunchConfiguration('slam_params_file')
+ declare_autostart_cmd = DeclareLaunchArgument(
+ 'autostart', default_value='true',
+ description='Automatically startup the slamtoolbox. '
+ 'Ignored when use_lifecycle_manager is true.')
+ declare_use_lifecycle_manager = DeclareLaunchArgument(
+ 'use_lifecycle_manager', default_value='false',
+ description='Enable bond connection during node activation')
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,
+ declare_slam_params_file_cmd = DeclareLaunchArgument(
+ 'slam_params_file',
+ default_value=os.path.join(get_package_share_directory("sportmode_nav2"),
+ 'params', 'mapper_params_online_async.yaml'),
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(
+ start_async_slam_toolbox_node = LifecycleNode(
parameters=[
- actual_params_file,
- {'use_sim_time': use_sim_time}
+ slam_params_file,
+ {
+ 'use_lifecycle_manager': use_lifecycle_manager,
+ 'use_sim_time': use_sim_time
+ }
],
package='slam_toolbox',
executable='async_slam_toolbox_node',
name='slam_toolbox',
- output='screen')
+ output='screen',
+ namespace=''
+ )
+
+ configure_event = EmitEvent(
+ event=ChangeState(
+ lifecycle_node_matcher=matches_action(start_async_slam_toolbox_node),
+ transition_id=Transition.TRANSITION_CONFIGURE
+ ),
+ condition=IfCondition(AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager)))
+ )
+
+ activate_event = RegisterEventHandler(
+ OnStateTransition(
+ target_lifecycle_node=start_async_slam_toolbox_node,
+ start_state="configuring",
+ goal_state="inactive",
+ entities=[
+ LogInfo(msg="[LifecycleLaunch] Slamtoolbox node is activating."),
+ EmitEvent(event=ChangeState(
+ lifecycle_node_matcher=matches_action(start_async_slam_toolbox_node),
+ transition_id=Transition.TRANSITION_ACTIVATE
+ ))
+ ]
+ ),
+ condition=IfCondition(AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager)))
+ )
ld = LaunchDescription()
+ ld.add_action(declare_autostart_cmd)
+ ld.add_action(declare_use_lifecycle_manager)
ld.add_action(declare_use_sim_time_argument)
- ld.add_action(declare_params_file_cmd)
- ld.add_action(log_param_change)
+ ld.add_action(declare_slam_params_file_cmd)
ld.add_action(start_async_slam_toolbox_node)
+ ld.add_action(configure_event)
+ ld.add_action(activate_event)
return ld
diff --git a/deploy/ros2_nodes/sportmode_nav2/launch/navigation_launch.py b/deploy/ros2_nodes/sportmode_nav2/launch/navigation_launch.py
index fa055b9..aab348b 100644
--- a/deploy/ros2_nodes/sportmode_nav2/launch/navigation_launch.py
+++ b/deploy/ros2_nodes/sportmode_nav2/launch/navigation_launch.py
@@ -20,7 +20,7 @@ from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PythonExpression
-from launch_ros.actions import LoadComposableNodes
+from launch_ros.actions import LoadComposableNodes, SetParameter
from launch_ros.actions import Node
from launch_ros.descriptions import ComposableNode, ParameterFile
from nav2_common.launch import RewrittenYaml
@@ -40,13 +40,17 @@ def generate_launch_description():
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']
+ lifecycle_nodes = [
+ 'controller_server',
+ 'smoother_server',
+ 'planner_server',
+ 'behavior_server',
+ 'velocity_smoother',
+ # 'collision_monitor',
+ 'bt_navigator',
+ 'waypoint_follower',
+ # 'docking_server',
+ ]
# Map fully qualified names to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
@@ -54,63 +58,73 @@ def generate_launch_description():
# https://github.com/ros/robot_state_publisher/pull/30
# TODO(orduno) Substitute with `PushNodeRemapping`
# https://github.com/ros2/launch_ros/issues/56
- remappings = [('/tf', 'tf'),
- ('/tf_static', 'tf_static'), ('/cmd_vel', '/go2/cmd_vel')]
+ remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static'), ('/cmd_vel', '/go2/cmd_vel')]
# Create our own temporary YAML files that include substitutions
- param_substitutions = {
- 'use_sim_time': use_sim_time,
- 'autostart': autostart}
+ param_substitutions = {'autostart': autostart}
configured_params = ParameterFile(
RewrittenYaml(
source_file=params_file,
root_key=namespace,
param_rewrites=param_substitutions,
- convert_types=True),
- allow_substs=True)
+ convert_types=True,
+ ),
+ allow_substs=True,
+ )
stdout_linebuf_envvar = SetEnvironmentVariable(
- 'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
+ 'RCUTILS_LOGGING_BUFFERED_STREAM', '1'
+ )
declare_namespace_cmd = DeclareLaunchArgument(
- 'namespace',
- default_value='',
- description='Top-level namespace')
+ 'namespace', default_value='', description='Top-level namespace'
+ )
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='false',
- description='Use simulation (Gazebo) clock if true')
+ description='Use simulation (Gazebo) clock if true',
+ )
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
- description='Full path to the ROS2 parameters file to use for all launched nodes')
+ description='Full path to the ROS2 parameters file to use for all launched nodes',
+ )
declare_autostart_cmd = DeclareLaunchArgument(
- 'autostart', default_value='true',
- description='Automatically startup the nav2 stack')
+ 'autostart',
+ default_value='true',
+ description='Automatically startup the nav2 stack',
+ )
declare_use_composition_cmd = DeclareLaunchArgument(
- 'use_composition', default_value='False',
- description='Use composed bringup if True')
+ 'use_composition',
+ default_value='False',
+ description='Use composed bringup if True',
+ )
declare_container_name_cmd = DeclareLaunchArgument(
- 'container_name', default_value='nav2_container',
- description='the name of conatiner that nodes will load in if use composition')
+ 'container_name',
+ default_value='nav2_container',
+ description='the name of conatiner that nodes will load in if use composition',
+ )
declare_use_respawn_cmd = DeclareLaunchArgument(
- 'use_respawn', default_value='False',
- description='Whether to respawn if a node crashes. Applied when composition is disabled.')
+ 'use_respawn',
+ default_value='False',
+ description='Whether to respawn if a node crashes. Applied when composition is disabled.',
+ )
declare_log_level_cmd = DeclareLaunchArgument(
- 'log_level', default_value='info',
- description='log level')
+ 'log_level', default_value='info', description='log level'
+ )
load_nodes = GroupAction(
condition=IfCondition(PythonExpression(['not ', use_composition])),
actions=[
+ SetParameter('use_sim_time', use_sim_time),
Node(
package='nav2_controller',
executable='controller_server',
@@ -119,7 +133,8 @@ def generate_launch_description():
respawn_delay=2.0,
parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level],
- remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]),
+ remappings=remappings + [('cmd_vel', 'cmd_vel_nav')],
+ ),
Node(
package='nav2_smoother',
executable='smoother_server',
@@ -129,7 +144,8 @@ def generate_launch_description():
respawn_delay=2.0,
parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level],
- remappings=remappings),
+ remappings=remappings,
+ ),
Node(
package='nav2_planner',
executable='planner_server',
@@ -139,7 +155,8 @@ def generate_launch_description():
respawn_delay=2.0,
parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level],
- remappings=remappings),
+ remappings=remappings,
+ ),
Node(
package='nav2_behaviors',
executable='behavior_server',
@@ -149,7 +166,8 @@ def generate_launch_description():
respawn_delay=2.0,
parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level],
- remappings=remappings),
+ remappings=remappings + [('cmd_vel', 'cmd_vel_nav')],
+ ),
Node(
package='nav2_bt_navigator',
executable='bt_navigator',
@@ -159,7 +177,8 @@ def generate_launch_description():
respawn_delay=2.0,
parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level],
- remappings=remappings),
+ remappings=remappings,
+ ),
Node(
package='nav2_waypoint_follower',
executable='waypoint_follower',
@@ -169,7 +188,8 @@ def generate_launch_description():
respawn_delay=2.0,
parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level],
- remappings=remappings),
+ remappings=remappings,
+ ),
Node(
package='nav2_velocity_smoother',
executable='velocity_smoother',
@@ -179,74 +199,123 @@ def generate_launch_description():
respawn_delay=2.0,
parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level],
- remappings=remappings +
- [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
+ remappings=remappings
+ + [('cmd_vel', 'cmd_vel_nav')],
+ ),
+ # Node(
+ # package='nav2_collision_monitor',
+ # executable='collision_monitor',
+ # name='collision_monitor',
+ # output='screen',
+ # respawn=use_respawn,
+ # respawn_delay=2.0,
+ # parameters=[configured_params],
+ # arguments=['--ros-args', '--log-level', log_level],
+ # remappings=remappings,
+ # ),
+ # Node(
+ # package='opennav_docking',
+ # executable='opennav_docking',
+ # name='docking_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_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}]),
- ]
+ parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}],
+ ),
+ ],
)
- load_composable_nodes = LoadComposableNodes(
+ load_composable_nodes = GroupAction(
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}]),
+ actions=[
+ SetParameter('use_sim_time', use_sim_time),
+ LoadComposableNodes(
+ target_container=container_name_full,
+ composable_node_descriptions=[
+ ComposableNode(
+ package='nav2_controller',
+ plugin='nav2_controller::ControllerServer',
+ name='controller_server',
+ parameters=[configured_params],
+ remappings=remappings + [('cmd_vel', 'cmd_vel_nav')],
+ ),
+ ComposableNode(
+ package='nav2_smoother',
+ plugin='nav2_smoother::SmootherServer',
+ name='smoother_server',
+ parameters=[configured_params],
+ remappings=remappings,
+ ),
+ ComposableNode(
+ package='nav2_planner',
+ plugin='nav2_planner::PlannerServer',
+ name='planner_server',
+ parameters=[configured_params],
+ remappings=remappings,
+ ),
+ ComposableNode(
+ package='nav2_behaviors',
+ plugin='behavior_server::BehaviorServer',
+ name='behavior_server',
+ parameters=[configured_params],
+ remappings=remappings + [('cmd_vel', 'cmd_vel_nav')],
+ ),
+ ComposableNode(
+ package='nav2_bt_navigator',
+ plugin='nav2_bt_navigator::BtNavigator',
+ name='bt_navigator',
+ parameters=[configured_params],
+ remappings=remappings,
+ ),
+ ComposableNode(
+ package='nav2_waypoint_follower',
+ plugin='nav2_waypoint_follower::WaypointFollower',
+ name='waypoint_follower',
+ parameters=[configured_params],
+ remappings=remappings,
+ ),
+ ComposableNode(
+ package='nav2_velocity_smoother',
+ plugin='nav2_velocity_smoother::VelocitySmoother',
+ name='velocity_smoother',
+ parameters=[configured_params],
+ remappings=remappings
+ + [('cmd_vel', 'cmd_vel_nav')],
+ ),
+ # ComposableNode(
+ # package='nav2_collision_monitor',
+ # plugin='nav2_collision_monitor::CollisionMonitor',
+ # name='collision_monitor',
+ # parameters=[configured_params],
+ # remappings=remappings,
+ # ),
+ # ComposableNode(
+ # package='opennav_docking',
+ # plugin='opennav_docking::DockingServer',
+ # name='docking_server',
+ # 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}
+ ],
+ ),
+ ],
+ ),
],
)
diff --git a/deploy/ros2_nodes/sportmode_nav2/package.xml b/deploy/ros2_nodes/sportmode_nav2/package.xml
index 6e7a413..f0618d8 100644
--- a/deploy/ros2_nodes/sportmode_nav2/package.xml
+++ b/deploy/ros2_nodes/sportmode_nav2/package.xml
@@ -13,9 +13,6 @@
ament_lint_auto
ament_lint_common
- robot_locoalization
- slam_toolbox
-
ament_cmake
diff --git a/deploy/ros2_nodes/sportmode_nav2/params/mapper_params_online_async.yaml b/deploy/ros2_nodes/sportmode_nav2/params/mapper_params_online_async.yaml
new file mode 100644
index 0000000..70504d5
--- /dev/null
+++ b/deploy/ros2_nodes/sportmode_nav2/params/mapper_params_online_async.yaml
@@ -0,0 +1,75 @@
+slam_toolbox:
+ ros__parameters:
+
+ # Plugin params
+ solver_plugin: solver_plugins::CeresSolver
+ ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+ ceres_preconditioner: SCHUR_JACOBI
+ ceres_trust_strategy: LEVENBERG_MARQUARDT
+ ceres_dogleg_type: TRADITIONAL_DOGLEG
+ ceres_loss_function: None
+
+ # ROS Parameters
+ odom_frame: odom
+ map_frame: map
+ base_frame: base_link
+ scan_topic: /go2/scan
+ use_map_saver: true
+ mode: mapping #localization
+
+ # if you'd like to immediately start continuing a map at a given pose
+ # or at the dock, but they are mutually exclusive, if pose is given
+ # will use pose
+ #map_file_name: test_steve
+ # map_start_pose: [0.0, 0.0, 0.0]
+ #map_start_at_dock: true
+
+ debug_logging: false
+ throttle_scans: 1
+ transform_publish_period: 0.02 #if 0 never publishes odometry
+ map_update_interval: 5.0
+ resolution: 0.05
+ min_laser_range: 0.0 #for rastering images
+ max_laser_range: 20.0 #for rastering images
+ minimum_time_interval: 0.5
+ transform_timeout: 0.2
+ tf_buffer_duration: 30.0
+ 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/ros2_nodes/sportmode_nav2/params/mapping_async.yaml b/deploy/ros2_nodes/sportmode_nav2/params/mapping_async.yaml
index ac18a0f..926356e 100644
--- a/deploy/ros2_nodes/sportmode_nav2/params/mapping_async.yaml
+++ b/deploy/ros2_nodes/sportmode_nav2/params/mapping_async.yaml
@@ -28,8 +28,8 @@ slam_toolbox:
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 5.0
resolution: 0.05
- max_laser_range: 8.0 #for rastering images
- minimum_time_interval: 0.1
+ max_laser_range: 5.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
diff --git a/deploy/ros2_nodes/sportmode_nav2/params/nav2_params.old.yaml b/deploy/ros2_nodes/sportmode_nav2/params/nav2_params.old.yaml
deleted file mode 100644
index 7203562..0000000
--- a/deploy/ros2_nodes/sportmode_nav2/params/nav2_params.old.yaml
+++ /dev/null
@@ -1,350 +0,0 @@
-amcl:
- ros__parameters:
- use_sim_time: False
- alpha1: 0.2
- alpha2: 0.2
- alpha3: 0.2
- alpha4: 0.2
- alpha5: 0.2
- base_frame_id: "base_link"
- beam_skip_distance: 0.5
- beam_skip_error_threshold: 0.9
- beam_skip_threshold: 0.3
- do_beamskip: false
- global_frame_id: "map"
- lambda_short: 0.1
- laser_likelihood_max_dist: 2.0
- laser_max_range: 100.0
- laser_min_range: -1.0
- laser_model_type: "likelihood_field"
- max_beams: 60
- max_particles: 2000
- min_particles: 500
- odom_frame_id: "odom"
- pf_err: 0.05
- pf_z: 0.99
- recovery_alpha_fast: 0.0
- recovery_alpha_slow: 0.0
- resample_interval: 1
- robot_model_type: "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: /go2/scan
-
-bt_navigator:
- ros__parameters:
- use_sim_time: False
- global_frame: map
- robot_base_frame: base_link
- odom_topic: /go2/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: False
-
-bt_navigator_navigate_to_pose_rclcpp_node:
- ros__parameters:
- use_sim_time: False
-
-controller_server:
- ros__parameters:
- use_sim_time: False
- controller_frequency: 20.0
- min_x_velocity_threshold: 0.001
- min_y_velocity_threshold: 0.5
- min_theta_velocity_threshold: 0.001
- 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.5
- yaw_goal_tolerance: 1.
- # 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: map
- robot_base_frame: base_link
- use_sim_time: False
- rolling_window: true
- width: 3
- height: 3
- resolution: 0.05
- # robot_radius: 0.22
- footprint: "[ [0.35, 0.195], [0.35, -0.195], [-0.35, -0.195], [-0.35, 0.195] ]"
- plugins: ["voxel_layer", "inflation_layer"]
- inflation_layer:
- plugin: "nav2_costmap_2d::InflationLayer"
- cost_scaling_factor: 3.0
- inflation_radius: 0.2
- 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: False
- robot_radius: 0.22
- resolution: 0.05
- track_unknown_space: true
- plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
- obstacle_layer:
- plugin: "nav2_costmap_2d::ObstacleLayer"
- enabled: True
- observation_sources: scan
- scan:
- topic: /go2/scan
- max_obstacle_height: 2.0
- clearing: True
- marking: True
- data_type: "LaserScan"
- 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.65
- always_send_full_costmap: True
-
-map_server:
- ros__parameters:
- use_sim_time: False
- # Overridden in launch by the "map" launch configuration or provided default value.
- # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below.
- yaml_filename: ""
-
-map_saver:
- ros__parameters:
- use_sim_time: False
- save_map_timeout: 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: False
- 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: False
- 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: False
- simulate_ahead_time: 2.0
- max_rotational_vel: 1.0
- min_rotational_vel: 0.4
- rotational_acc_lim: 3.2
-
-robot_state_publisher:
- ros__parameters:
- use_sim_time: False
-
-waypoint_follower:
- ros__parameters:
- use_sim_time: False
- 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: False
- 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: "/go2/odom"
- odom_duration: 0.1
- deadband_velocity: [0.0, 0.0, 0.0]
- velocity_timeout: 1.0
diff --git a/deploy/ros2_nodes/sportmode_nav2/params/nav2_params.yaml b/deploy/ros2_nodes/sportmode_nav2/params/nav2_params.yaml
index 5f48860..c362f0c 100644
--- a/deploy/ros2_nodes/sportmode_nav2/params/nav2_params.yaml
+++ b/deploy/ros2_nodes/sportmode_nav2/params/nav2_params.yaml
@@ -10,7 +10,7 @@ amcl:
beam_skip_distance: 0.5
beam_skip_error_threshold: 0.9
beam_skip_threshold: 0.3
- do_beamskip: false
+ do_beamskip: False
global_frame_id: "map"
lambda_short: 0.1
laser_likelihood_max_dist: 2.0
@@ -29,7 +29,7 @@ amcl:
robot_model_type: "nav2_amcl::DifferentialMotionModel"
save_pose_rate: 0.5
sigma_hit: 0.2
- tf_broadcast: true
+ tf_broadcast: True
transform_tolerance: 1.0
update_min_a: 0.2
update_min_d: 0.25
@@ -44,97 +44,88 @@ bt_navigator:
use_sim_time: False
global_frame: map
robot_base_frame: base_link
- odom_topic: /go2/odom
+ odom_topic: /odometry/filtered
bt_loop_duration: 10
default_server_timeout: 20
+ wait_for_service_timeout: 1000
+ action_server_result_timeout: 900.0
+ navigators: ["navigate_to_pose", "navigate_through_poses"]
+ navigate_to_pose:
+ plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
+ navigate_through_poses:
+ plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
# '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: False
+ # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
+ # Built-in plugins are added automatically
+ # plugin_lib_names: []
-bt_navigator_navigate_to_pose_rclcpp_node:
- ros__parameters:
- use_sim_time: False
+ error_code_names:
+ - compute_path_error_code
+ - follow_path_error_code
controller_server:
ros__parameters:
+ use_sim_time: False
controller_frequency: 20.0
+ min_x_velocity_threshold: 0.01
+ min_y_velocity_threshold: 0.01
+ min_theta_velocity_threshold: 0.01
+ failure_tolerance: 0.3
+ progress_checker_plugins: ["progress_checker"]
+ goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
+ controller_plugins: ["FollowPath"]
+ use_realtime_priority: False
+
+ # 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
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
time_steps: 56
model_dt: 0.05
batch_size: 2000
- vx_std: 0.2
+ vx_std: 0.5
vy_std: 0.2
wz_std: 0.4
- vx_max: 0.5
- vx_min: -0.35
- vy_max: 0.5
- wz_max: 1.9
+ vx_max: 1.5
+ vx_min: -0.8
+ vy_max: 1.5
+ wz_max: 3.9
+ ax_max: 1004.0
+ ax_min: -1004.0
+ ay_max: 1004.0
+ az_max: 1004.5
iteration_count: 1
prune_distance: 1.7
transform_tolerance: 0.1
temperature: 0.3
gamma: 0.015
motion_model: "DiffDrive"
- visualize: false
+ visualize: true
+ # reset_period: 1.0 # (only in Humble)
+ regenerate_noises: false
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
AckermannConstraints:
- min_turning_r: 0.05
+ min_turning_r: 0.2
critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
ConstraintCritic:
enabled: true
@@ -151,7 +142,7 @@ controller_server:
cost_weight: 3.0
threshold_to_consider: 0.5
PreferForwardCritic:
- enabled: true
+ enabled: false
cost_power: 1
cost_weight: 5.0
threshold_to_consider: 0.5
@@ -164,24 +155,26 @@ controller_server:
# collision_cost: 10000.0
# collision_margin_distance: 0.1
# near_goal_distance: 0.5
+ # inflation_radius: 0.55 # (only in Humble)
+ # cost_scaling_factor: 10.0 # (only in Humble)
CostCritic:
enabled: true
cost_power: 1
- cost_weight: 20.0
+ cost_weight: 3.81
critical_cost: 300.0
consider_footprint: true
collision_cost: 1000000.0
near_goal_distance: 1.0
- PathAlignCritic:
+ trajectory_point_step: 2
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 14.0
max_path_occupancy_ratio: 0.05
- trajectory_point_step: 3
+ trajectory_point_step: 4
threshold_to_consider: 0.5
offset_from_furthest: 20
- use_path_orientations: false
+ use_path_orientations: true
PathFollowCritic:
enabled: true
cost_power: 1
@@ -195,7 +188,7 @@ controller_server:
offset_from_furthest: 4
threshold_to_consider: 0.5
max_angle_to_furthest: 1.0
- forward_preference: true
+ mode: 0
# VelocityDeadbandCritic:
# enabled: true
# cost_power: 1
@@ -209,22 +202,22 @@ controller_server:
local_costmap:
local_costmap:
ros__parameters:
+ use_sim_time: False
update_frequency: 5.0
publish_frequency: 2.0
global_frame: odom
robot_base_frame: base_link
- use_sim_time: False
- rolling_window: true
+ rolling_window: True
width: 3
height: 3
- resolution: 0.05
+ resolution: 0.02
# robot_radius: 0.22
footprint: "[ [0.33, 0.2], [0.33 -0.2], [-0.48 -0.2], [-0.48, 0.2] ]"
plugins: ["voxel_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
- inflation_radius: 0.2
+ inflation_radius: 0.70
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
@@ -253,15 +246,15 @@ local_costmap:
global_costmap:
global_costmap:
ros__parameters:
+ use_sim_time: False
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
- use_sim_time: False
- robot_radius: 0.22
- # footprint: "[ [0.34, 0.2], [0.34 -0.2], [-0.34 -0.2], [-0.34, 0.2] ]"
+ # robot_radius: 0.22
+ footprint: "[ [0.33, 0.2], [0.33 -0.2], [-0.48 -0.2], [-0.48, 0.2] ]"
resolution: 0.05
- track_unknown_space: true
+ track_unknown_space: True
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
@@ -283,15 +276,15 @@ global_costmap:
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
- inflation_radius: 0.25
+ inflation_radius: 0.7
always_send_full_costmap: True
-map_server:
- ros__parameters:
- use_sim_time: False
- # 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: ""
+# The yaml_filename does not need to be specified since it going to be set by defaults in launch.
+# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py
+# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used.
+# map_server:
+# ros__parameters:
+# yaml_filename: ""
map_saver:
ros__parameters:
@@ -301,53 +294,49 @@ map_saver:
occupied_thresh_default: 0.65
map_subscribe_transient_local: True
-# planner_server:
-# ros__parameters:
-# expected_planner_frequency: 1.0
-# use_sim_time: False
-# planner_plugins: ["GridBased"]
-# GridBased:
-# plugin: "nav2_smac_planner/SmacPlannerHybrid"
-# tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters
-# downsample_costmap: false # whether or not to downsample the map
-# downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
-# allow_unknown: true # allow traveling in unknown space
-# max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
-# max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance
-# max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
-# motion_model_for_search: "REEDS_SHEPP" # For Hybrid Dubin, Redds-Shepp
-# cost_travel_multiplier: 1.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
-# angle_quantization_bins: 64 # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search)
-# analytic_expansion_ratio: 3.5 # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach.
-# analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius
-# minimum_turning_radius: 0.3 # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle
-# reverse_penalty: 1.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
-# change_penalty: 0.2 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0
-# non_straight_penalty: 1.20 # For Hybrid nodes: penalty to apply if motion is non-straight, must be => 1
-# cost_penalty: 50.0 # For Hybrid nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
-# retrospective_penalty: 0.025 # For Hybrid/Lattice nodes: penalty to prefer later maneuvers before earlier along the path. Saves search time since earlier nodes are not expanded until it is necessary. Must be >= 0.0 and <= 1.0
-# rotation_penalty: 2.0 # For Lattice node: Penalty to apply only to pure rotate in place commands when using minimum control sets containing rotate in place primitives. This should always be set sufficiently high to weight against this action unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where it requests the robot to rotate in place to short-cut an otherwise smooth path for marginal path distance savings.
-# lookup_table_size: 20.0 # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters.
-# cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
-# allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step.
-# smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes.
-# smoother:
-# max_iterations: 1000
-# w_smooth: 0.3
-# w_data: 0.2
-# tolerance: 1.0e-10
-# do_refinement: true # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further
-
planner_server:
ros__parameters:
- expected_planner_frequency: 20.0
use_sim_time: False
planner_plugins: ["GridBased"]
+
GridBased:
- plugin: "nav2_navfn_planner/NavfnPlanner"
- tolerance: 0.5
- use_astar: false
- allow_unknown: true
+ plugin: "nav2_smac_planner::SmacPlannerHybrid"
+ tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters
+ downsample_costmap: False # whether or not to downsample the map
+ downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
+ allow_unknown: False # allow traveling in unknown space
+ max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
+ max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance
+ terminal_checking_interval: 5000 # number of iterations between checking if the goal has been cancelled or planner timed out
+ max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
+ motion_model_for_search: "REEDS_SHEPP" # For Hybrid Dubin, Redds-Shepp
+ cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
+ angle_quantization_bins: 64 # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search)
+ analytic_expansion_ratio: 3.5 # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach.
+ analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius
+ # analytic_expansion_max_cost: True # For Hybrid/Lattice nodes: The maximum single cost for any part of an analytic expansion to contain and be valid (except when necessary on approach to goal)
+ # analytic_expansion_max_cost_override: False # For Hybrid/Lattice nodes: Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required)
+ minimum_turning_radius: 0.1 # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle
+ reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
+ change_penalty: 0.0 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0
+ non_straight_penalty: 1.20 # For Hybrid nodes: penalty to apply if motion is non-straight, must be => 1
+ cost_penalty: 2.0 # For Hybrid nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
+ retrospective_penalty: 0.025 # For Hybrid/Lattice nodes: penalty to prefer later maneuvers before earlier along the path. Saves search time since earlier nodes are not expanded until it is necessary. Must be >= 0.0 and <= 1.0
+ rotation_penalty: 5.0 # For Lattice node: Penalty to apply only to pure rotate in place commands when using minimum control sets containing rotate in place primitives. This should always be set sufficiently high to weight against this action unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where it requests the robot to rotate in place to short-cut an otherwise smooth path for marginal path distance savings.
+ lookup_table_size: 20.0 # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters.
+ cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
+ allow_reverse_expansion: True # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step.
+ smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always True for 2D nodes.
+ debug_visualizations: True # For Hybrid/Lattice nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance.
+ smoother:
+ max_iterations: 1000
+ w_smooth: 0.3
+ w_data: 0.2
+ tolerance: 1.0e-10
+ do_refinement: True # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further
+
+
+
smoother_server:
ros__parameters:
use_sim_time: False
@@ -360,38 +349,38 @@ smoother_server:
behavior_server:
ros__parameters:
- costmap_topic: local_costmap/costmap_raw
- footprint_topic: local_costmap/published_footprint
+ use_sim_time: False
+ local_costmap_topic: local_costmap/costmap_raw
+ global_costmap_topic: global_costmap/costmap_raw
+ local_footprint_topic: local_costmap/published_footprint
+ global_footprint_topic: global_costmap/published_footprint
cycle_frequency: 10.0
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
spin:
- plugin: "nav2_behaviors/Spin"
+ plugin: "nav2_behaviors::Spin"
backup:
- plugin: "nav2_behaviors/BackUp"
+ plugin: "nav2_behaviors::BackUp"
drive_on_heading:
- plugin: "nav2_behaviors/DriveOnHeading"
+ plugin: "nav2_behaviors::DriveOnHeading"
wait:
- plugin: "nav2_behaviors/Wait"
+ plugin: "nav2_behaviors::Wait"
assisted_teleop:
- plugin: "nav2_behaviors/AssistedTeleop"
- global_frame: odom
+ plugin: "nav2_behaviors::AssistedTeleop"
+ local_frame: odom
+ global_frame: map
robot_base_frame: base_link
transform_tolerance: 0.1
- use_sim_time: False
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
-robot_state_publisher:
- ros__parameters:
- use_sim_time: False
-
waypoint_follower:
ros__parameters:
use_sim_time: False
loop_rate: 20
- stop_on_failure: false
+ stop_on_failure: False
+ action_server_result_timeout: 900.0
waypoint_task_executor_plugin: "wait_at_waypoint"
wait_at_waypoint:
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
@@ -404,11 +393,11 @@ velocity_smoother:
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: "/go2/odom"
+ max_velocity: [1.5, 1.0, 3.0]
+ min_velocity: [-1.5, -1.0, -3.0]
+ max_accel: [2.5, 3.0, 3.2]
+ max_decel: [-2.5, -3.0, -3.2]
+ odom_topic: "/odometry/filtered"
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
- velocity_timeout: 1.0
+ velocity_timeout: 1.0
\ No newline at end of file
diff --git a/deploy/ros2_nodes/sportmode_nav2/params/ros_ekf.yaml b/deploy/ros2_nodes/sportmode_nav2/params/ros_ekf.yaml
index caede57..b5e49a1 100644
--- a/deploy/ros2_nodes/sportmode_nav2/params/ros_ekf.yaml
+++ b/deploy/ros2_nodes/sportmode_nav2/params/ros_ekf.yaml
@@ -4,7 +4,7 @@ ekf_filter_node:
# 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: 30.0
+ frequency: 25.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
diff --git a/scripts/run_nav2.sh b/scripts/run_nav2.sh
index 6ed9a32..aca7630 100755
--- a/scripts/run_nav2.sh
+++ b/scripts/run_nav2.sh
@@ -41,6 +41,6 @@ if [ "$(docker ps -q -f name=$CONTAINER_NAME)" ]; then
check_docker
else
echo "Starting new container: $CONTAINER_NAME"
- sudo docker run -it --rm --name $CONTAINER_NAME --privileged --network host --ipc=host -v $(pwd)/deploy/ros2_nodes/sportmode_nav2:/home/nav2_ws/src/sportmode_nav2 -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro -v /home/unitree/Go2Py/deploy/SGNav/llm_planner:/home/llm_planner --runtime nvidia --workdir /home/nav2_ws go2py_nav2:latest
+ sudo docker run -it --rm --name $CONTAINER_NAME --privileged --network host --ipc=host -v $(pwd)/deploy/ros2_nodes/sportmode_nav2:/home/nav2/src/sportmode_nav2 -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro -v /home/unitree/Go2Py/deploy/SGNav/llm_planner:/home/llm_planner --runtime nvidia --workdir /home/nav2_ws go2py_nav2:latest
check_docker
fi