diff --git a/Dockerfile.nav2 b/Dockerfile.nav2 index 0036845..eb4d3f8 100644 --- a/Dockerfile.nav2 +++ b/Dockerfile.nav2 @@ -8,12 +8,12 @@ RUN apt-get update && apt-get install -y \ ros-humble-realsense2-camera \ ros-humble-pointcloud-to-laserscan \ libyaml-cpp-dev \ - # ros-humble-isaac-ros-visual-slam \ - # ros-humble-isaac-ros-occupancy-grid-localizer\ libboost-all-dev\ build-essential \ cmake \ git \ + ros-humble-navigation2 \ + ros-humble-nav2-bringup \ && rm -rf /var/lib/apt/lists/* # Cheange the ROS2 RMW to CycloneDDS as instructed by Unitree @@ -21,23 +21,13 @@ RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /uni 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 -# RUN echo "source /opt/ros/humble/setup.bash" >> /usr/local/bin/scripts/workspace-entrypoint.sh -# RUN echo "source /unitree_ros2/cyclonedds_ws/install/setup.bash" >> /usr/local/bin/scripts/workspace-entrypoint.sh -# RUN echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> /usr/local/bin/scripts/workspace-entrypoint.sh +# RUN touch /workspace-entrypoint.sh +RUN echo "source /opt/ros/humble/setup.bash" > /workspace-entrypoint.sh +RUN echo "source /unitree_ros2/cyclonedds_ws/install/setup.bash" >> /workspace-entrypoint.sh +RUN echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> /workspace-entrypoint.sh +RUN echo "/bin/bash" >> /workspace-entrypoint.sh # RUN echo "export CYCLONEDDS_URI=' '" >> /usr/local/bin/scripts/workspace-entrypoint.sh -# copy the go2py ros2 nodes -COPY deploy/dock_ws/src /nav2_ws/src -# RUN cd /dock_ws && source /opt/ros/humble/setup.bash && colcon build --symlink-install - -# Compile the C++ hypervisor bridge -COPY deploy/dds_bridge /dds_bridge -WORKDIR /dds_bridge -# RUN ./install.sh && mkdir build && cd build && cmake .. && make - -# Copy the script to start the nodes -COPY deploy/scripts /root/scripts -COPY deploy/launch /root/launch # set the entrypoint to bash -ENTRYPOINT ["/bin/bash"] -# ENTRYPOINT ["/bin/bash", "/root/scripts/dock_hw_start.sh"] +# ENTRYPOINT ["/bin/bash"] +ENTRYPOINT ["/bin/bash", "/workspace-entrypoint.sh"] diff --git a/Makefile b/Makefile index 3fd65b1..2661d53 100644 --- a/Makefile +++ b/Makefile @@ -5,7 +5,8 @@ docker_robot: @docker build --no-cache --tag go2py_robot_hw:latest -f Dockerfile.robot . docker_nav: - @docker build --no-cache --tag go2py_nav:latest -f Dockerfile.nav2 . + # @docker build --no-cache --tag go2py_nav:latest -f Dockerfile.nav2 . + @docker build --tag go2py_nav:latest -f Dockerfile.nav2 . docker_dock_install: @cp deploy/scripts/go2py-dock-hw-nodes.service /etc/systemd/system/ diff --git a/deploy/nav2_ws/src/sportmode_nav2/launch/navigation_launch.py b/deploy/nav2_ws/src/sportmode_nav2/launch/navigation_launch.py new file mode 100644 index 0000000..e34b671 --- /dev/null +++ b/deploy/nav2_ws/src/sportmode_nav2/launch/navigation_launch.py @@ -0,0 +1,324 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes, SetParameter +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = [ + 'controller_server', + 'smoother_server', + 'planner_server', + 'behavior_server', + 'velocity_smoother', + 'collision_monitor', + 'bt_navigator', + 'waypoint_follower', + ] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', '/tf'), + ('/tf_static', '/tf_static'), + ('/cmd_vel', '/go2/cmd_vel')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = {'autostart': autostart} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True, + ), + allow_substs=True, + ) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' + ) + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true', + ) + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', + default_value='False', + description='Use composed bringup if True', + ) + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', + default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition', + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', description='log level' + ) + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + SetParameter('use_sim_time', use_sim_time), + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], + ), + Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + ), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + ), + Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], + ), + Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + ), + Node( + package='nav2_waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + ), + Node( + package='nav2_velocity_smoother', + executable='velocity_smoother', + name='velocity_smoother', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav')], + ), + 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='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + ), + ], + ) + + load_composable_nodes = GroupAction( + condition=IfCondition(use_composition), + actions=[ + SetParameter('use_sim_time', use_sim_time), + LoadComposableNodes( + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_controller', + plugin='nav2_controller::ControllerServer', + name='controller_server', + parameters=[configured_params], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], + ), + ComposableNode( + package='nav2_smoother', + plugin='nav2_smoother::SmootherServer', + name='smoother_server', + parameters=[configured_params], + remappings=remappings, + ), + ComposableNode( + package='nav2_planner', + plugin='nav2_planner::PlannerServer', + name='planner_server', + parameters=[configured_params], + remappings=remappings, + ), + ComposableNode( + package='nav2_behaviors', + plugin='behavior_server::BehaviorServer', + name='behavior_server', + parameters=[configured_params], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], + ), + ComposableNode( + package='nav2_bt_navigator', + plugin='nav2_bt_navigator::BtNavigator', + name='bt_navigator', + parameters=[configured_params], + remappings=remappings, + ), + ComposableNode( + package='nav2_waypoint_follower', + plugin='nav2_waypoint_follower::WaypointFollower', + name='waypoint_follower', + parameters=[configured_params], + remappings=remappings, + ), + ComposableNode( + package='nav2_velocity_smoother', + plugin='nav2_velocity_smoother::VelocitySmoother', + name='velocity_smoother', + parameters=[configured_params], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav')], + ), + ComposableNode( + package='nav2_collision_monitor', + plugin='nav2_collision_monitor::CollisionMonitor', + name='collision_monitor', + parameters=[configured_params], + remappings=remappings, + ), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_navigation', + parameters=[ + {'autostart': autostart, 'node_names': lifecycle_nodes} + ], + ), + ], + ), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/nav2_docker_run.sh b/nav2_docker_run.sh new file mode 100644 index 0000000..ca186cf --- /dev/null +++ b/nav2_docker_run.sh @@ -0,0 +1 @@ +docker run --rm -it --privileged --network host -v $(pwd)/deploy/nav2_ws:/home/ -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro --runtime nvidia go2py_nav:latest \ No newline at end of file