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

navigation parameters and docker is updated

This commit is contained in:
Rooholla-KhorramBakht 2024-07-27 21:32:32 -04:00
parent b7d6503816
commit 30cff75c8f
15 changed files with 622 additions and 695 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Makefile
View File

@ -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
@rm /etc/systemd/system/go2py-robot-description.service

170
deploy/docker/Dockerfile.nav2
View File

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

32
deploy/launch_files/utlidar_to_laserscan.py Normal file
View File

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

6
deploy/ros2_nodes/go2_description/xacro/robot.xacro
View File

@ -157,7 +157,7 @@
</visual>
</link>
<!-- <link name="utlidar">
<link name="utlidar_lidar">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
@ -167,9 +167,9 @@
<joint name="utlidar_joint" type="fixed">
<origin rpy="0 2.8782 0" xyz="0.28945 0 -0.046825"/>
<parent link="base_link"/>
<child link="utlidar"/>
<child link="utlidar_lidar"/>
<axis xyz="0 0 0"/>
</joint> -->
</joint>
<link name="hesai">
<inertial>

2
deploy/ros2_nodes/lidar_node/src/manager/source_driver_ros2.hpp
View File

@ -144,7 +144,7 @@ inline void SourceDriver::Init(const YAML::Node& config)
YamlRead<std::string>(config["ros"], "ros_send_packet_topic", ros_send_packet_topic, "hesai_packets");
pkt_pub_ = node_ptr_->create_publisher<hesai_ros_driver::msg::UdpFrame>(ros_send_packet_topic, 1);
}
laserscan_pub_ = node_ptr_->create_publisher<sensor_msgs::msg::LaserScan>("/go2/scan", rclcpp::SensorDataQoS());
laserscan_pub_ = node_ptr_->create_publisher<sensor_msgs::msg::LaserScan>("/go2/scan", 1);
if (driver_param.input_param.source_type == DATA_FROM_ROS_PACKET) {
std::string ros_recv_packet_topic;
YamlRead<std::string>(config["ros"], "ros_recv_packet_topic", ros_recv_packet_topic, "hesai_packets");

27
deploy/ros2_nodes/sportmode_nav2/launch/localization.launch.py
View File

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

96
deploy/ros2_nodes/sportmode_nav2/launch/mapping_async.launch.py
View File

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

261
deploy/ros2_nodes/sportmode_nav2/launch/navigation_launch.py
View File

@ -20,7 +20,7 @@ from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
from launch.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}
],
),
],
),
],
)

3
deploy/ros2_nodes/sportmode_nav2/package.xml
View File

@ -13,9 +13,6 @@
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>robot_locoalization</test_depend>
<test_depend>slam_toolbox</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>

75
deploy/ros2_nodes/sportmode_nav2/params/mapper_params_online_async.yaml Normal file
View File

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

4
deploy/ros2_nodes/sportmode_nav2/params/mapping_async.yaml
View File

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

350
deploy/ros2_nodes/sportmode_nav2/params/nav2_params.old.yaml
View File

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

283
deploy/ros2_nodes/sportmode_nav2/params/nav2_params.yaml
View File

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

2
deploy/ros2_nodes/sportmode_nav2/params/ros_ekf.yaml
View File

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

2
scripts/run_nav2.sh
View File

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