diff --git a/.github/workflows/ros_build.yaml b/.github/workflows/ros_build.yaml index 792ab31..cf1f12a 100644 --- a/.github/workflows/ros_build.yaml +++ b/.github/workflows/ros_build.yaml @@ -1,4 +1,4 @@ -name: ROS2 CI +name: ROS2 Iron on: pull_request: @@ -15,26 +15,28 @@ jobs: fail-fast: false matrix: ros_distribution: - - humble - iron - - rolling include: - # Humble Hawksbill (May 2022 - May 2027) - - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest - ros_distribution: humble - ros_version: 2 - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-iron-ros-base-latest ros_distribution: iron ros_version: 2 - # Rolling Ridley (June 2020 - Present) - - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest - ros_distribution: rolling - ros_version: 2 container: image: ${{ matrix.docker_image }} steps: - name: checkout uses: actions/checkout@v2 + - name: Install Ceres dependencies + run: | + sudo apt-get update + sudo apt-get install -y \ + libunwind-dev \ + libgoogle-glog-dev \ + libatlas-base-dev \ + libsuitesparse-dev + - name: Install Ceres + run: | + sudo apt-get update + sudo apt-get install -y libceres-dev - name: build and test uses: ros-tooling/action-ros-ci@master with: diff --git a/.github/workflows/ros_build_humble.yaml b/.github/workflows/ros_build_humble.yaml new file mode 100644 index 0000000..e89461d --- /dev/null +++ b/.github/workflows/ros_build_humble.yaml @@ -0,0 +1,45 @@ +name: ROS2 Humble + +on: + pull_request: + branches: + - 'humble' + push: + branches: + - 'humble' + +jobs: + test_environment: + runs-on: [ubuntu-latest] + strategy: + fail-fast: false + matrix: + ros_distribution: + - humble + include: + - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest + ros_distribution: humble + ros_version: 2 + container: + image: ${{ matrix.docker_image }} + steps: + - name: checkout + uses: actions/checkout@v2 + - name: Install Ceres dependencies + run: | + sudo apt-get update + sudo apt-get install -y \ + libunwind-dev \ + libgoogle-glog-dev \ + libatlas-base-dev \ + libsuitesparse-dev + - name: Install Ceres + run: | + sudo apt-get update + sudo apt-get install -y libceres-dev + - name: build and test + uses: ros-tooling/action-ros-ci@master + with: + target-ros2-distro: ${{ matrix.ros_distribution }} + vcs-repo-file-url: "" + skip-tests: true \ No newline at end of file diff --git a/README.md b/README.md index b1b1cf7..c000f77 100644 --- a/README.md +++ b/README.md @@ -69,31 +69,69 @@ Real time Go2 Air/PRO ROS2 topics ## System requirements Tested systems and ROS2 distro -|systems|ROS2 distro| -|--|--| -|Ubuntu 22.04|humble| -|Ubuntu 22.04|iron| +|systems|ROS2 distro|Build status +|--|--|--| +|Ubuntu 22.04|iron|![example workflow](https://github.com/abizovnuralem/go2_ros2_sdk/actions/workflows/ros_build.yaml/badge.svg) +|Ubuntu 22.04|humble|![example workflow](https://github.com/abizovnuralem/go2_ros2_sdk/actions/workflows/ros_build_humble.yaml/badge.svg) + +A single workspace can contain as many packages as you want, each in their own folder. You can also have packages of different build types in one workspace (CMake, Python, etc.). You cannot have nested packages. + +Best practice is to have a src folder within your workspace, and to create your packages in there. This keeps the top level of the workspace “clean”. + +Your workspace should look like: + +``` +workspace_folder/ + src/ + py_package_1/ + package.xml + resource/py_package_1 + setup.cfg + setup.py + py_package_1/ + + py_package_2/ + package.xml + resource/py_package_2 + setup.cfg + setup.py + py_package_2/ +``` + +clone this repo to src folder of your own ros2_ws repo -clone this rep and build it (put go2_interfaces and go2_robot_sdk to src folder of your own ros2_ws repo) -install rust language support in your system (cargo build should work in terminal) ``` git clone https://github.com/abizovnuralem/go2_ros2_sdk.git +cd go2_ros2_sdk pip install -r requirements.txt sudo apt install ros-{ROS2_VER}-test-msgs sudo apt install ros-{ROS2_VER}-tf2-sensor-msgs -source /opt/ros//setup.bash -colcon build +cd .. +mkdir -p ros2_ws/src +copy all files inside go2_ros2_sdk folder to ros2_ws/src folder ``` -don't forget to setup your GO2-robot in Wifi-mode and get IP -then +install rust language support in your system https://www.rust-lang.org/tools/install +cargo should work in terminal ``` -export ROBOT_IP="Your robot ip" +cargo --version +``` + +build it + +``` +source /opt/ros//setup.bash +cd ros2_ws +colcon build ``` ## Usage +don't forget to setup your GO2-robot in Wifi-mode and get IP then + ``` +export ROBOT_IP="Your robot ip" +cd ros2_ws source install/setup.bash ros2 launch go2_robot_sdk robot.launch.py ``` diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 453fd17..cac7910 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.2.0 + 1.1.13

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 1921114..65df56d 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -328,12 +328,10 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) nomotion_update_srv_.reset(); initial_pose_sub_.reset(); laser_scan_connection_.disconnect(); - tf_listener_.reset(); // listener may access lase_scan_filter_, so it should be reset earlier laser_scan_filter_.reset(); laser_scan_sub_.reset(); // Map - map_sub_.reset(); // map_sub_ may access map_, so it should be reset earlier if (map_ != NULL) { map_free(map_); map_ = nullptr; @@ -343,6 +341,7 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // Transforms tf_broadcaster_.reset(); + tf_listener_.reset(); tf_buffer_.reset(); // PubSub @@ -809,15 +808,6 @@ bool AmclNode::updateFilter( get_logger(), "Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment); - // Check the validity of range_max, must > 0.0 - if (laser_scan->range_max <= 0.0) { - RCLCPP_WARN( - get_logger(), "wrong range_max of laser_scan data: %f. The message could be malformed." - " Ignore this message and stop updating.", - laser_scan->range_max); - return false; - } - // Apply range min/max thresholds, if the user supplied them if (laser_max_range_ > 0.0) { ldata.range_max = std::min(laser_scan->range_max, static_cast(laser_max_range_)); @@ -1168,7 +1158,7 @@ AmclNode::dynamicParametersCallback( if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == "alpha1") { alpha1_ = parameter.as_double(); - // alpha restricted to be non-negative + //alpha restricted to be non-negative if (alpha1_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha1 to be negative," @@ -1178,7 +1168,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha2") { alpha2_ = parameter.as_double(); - // alpha restricted to be non-negative + //alpha restricted to be non-negative if (alpha2_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha2 to be negative," @@ -1188,7 +1178,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha3") { alpha3_ = parameter.as_double(); - // alpha restricted to be non-negative + //alpha restricted to be non-negative if (alpha3_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha3 to be negative," @@ -1198,7 +1188,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha4") { alpha4_ = parameter.as_double(); - // alpha restricted to be non-negative + //alpha restricted to be non-negative if (alpha4_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha4 to be negative," @@ -1208,7 +1198,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha5") { alpha5_ = parameter.as_double(); - // alpha restricted to be non-negative + //alpha restricted to be non-negative if (alpha5_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha5 to be negative," diff --git a/nav2_amcl/src/pf/eig3.c b/nav2_amcl/src/pf/eig3.c index 1980475..a5a84ae 100644 --- a/nav2_amcl/src/pf/eig3.c +++ b/nav2_amcl/src/pf/eig3.c @@ -43,7 +43,7 @@ static void tred2(double V[n][n], double d[n], double e[n]) // Fortran subroutine in EISPACK. int i, j, k; - double f, g, hh; + double f, g, h, hh; for (j = 0; j < n; j++) { d[j] = V[n - 1][j]; } @@ -122,7 +122,7 @@ static void tred2(double V[n][n], double d[n], double e[n]) for (i = 0; i < n - 1; i++) { V[n - 1][i] = V[i][i]; V[i][i] = 1.0; - const double h = d[i + 1]; + h = d[i + 1]; if (h != 0.0) { for (k = 0; k <= i; k++) { d[k] = V[k][i + 1] / h; diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index f39a0b8..40cbd68 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -17,19 +17,15 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - GroupAction, - IncludeLaunchDescription, - SetEnvironmentVariable, -) +from launch.actions import (DeclareLaunchArgument, GroupAction, + IncludeLaunchDescription, SetEnvironmentVariable) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import Node -from launch_ros.actions import PushROSNamespace +from launch_ros.actions import PushRosNamespace from launch_ros.descriptions import ParameterFile -from nav2_common.launch import ReplaceString, RewrittenYaml +from nav2_common.launch import RewrittenYaml, ReplaceString def generate_launch_description(): @@ -55,7 +51,13 @@ 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')] + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'yaml_filename': map_yaml_file} # Only it applys when `use_namespace` is True. # '' keyword shall be replaced by 'namespace' launch argument @@ -64,134 +66,112 @@ def generate_launch_description(): params_file = ReplaceString( source_file=params_file, replacements={'': ('/', namespace)}, - condition=IfCondition(use_namespace), - ) + condition=IfCondition(use_namespace)) configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, - param_rewrites={}, - convert_types=True, - ), - allow_substs=True, - ) + param_rewrites=param_substitutions, + 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_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', - description='Whether to apply a namespace to the navigation stack', - ) + description='Whether to apply a namespace to the navigation stack') declare_slam_cmd = DeclareLaunchArgument( - 'slam', default_value='False', description='Whether run a SLAM' - ) + 'slam', + default_value='False', + description='Whether run a SLAM') declare_map_yaml_cmd = DeclareLaunchArgument( - 'map', default_value='', description='Full path to map yaml file to load' - ) + 'map', + description='Full path to map yaml file to load') 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='True', - description='Whether to use composed bringup', - ) + 'use_composition', default_value='True', + description='Whether to use composed bringup') 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') # Specify the actions - bringup_cmd_group = GroupAction( - [ - PushROSNamespace(condition=IfCondition(use_namespace), namespace=namespace), - Node( - condition=IfCondition(use_composition), - name='nav2_container', - package='rclcpp_components', - executable='component_container_isolated', - parameters=[configured_params, {'autostart': autostart}], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - output='screen', - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'slam_launch.py') - ), - condition=IfCondition(slam), - launch_arguments={ - 'namespace': namespace, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'use_respawn': use_respawn, - 'params_file': params_file, - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'localization_launch.py') - ), - condition=IfCondition(PythonExpression(['not ', slam])), - launch_arguments={ - 'namespace': namespace, - 'map': map_yaml_file, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'params_file': params_file, - 'use_composition': use_composition, - 'use_respawn': use_respawn, - 'container_name': 'nav2_container', - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'navigation_launch.py') - ), - launch_arguments={ - 'namespace': namespace, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'params_file': params_file, - 'use_composition': use_composition, - 'use_respawn': use_respawn, - 'container_name': 'nav2_container', - }.items(), - ), - ] - ) + bringup_cmd_group = GroupAction([ + PushRosNamespace( + condition=IfCondition(use_namespace), + namespace=namespace), + + Node( + condition=IfCondition(use_composition), + name='nav2_container', + package='rclcpp_components', + executable='component_container_isolated', + parameters=[configured_params, {'autostart': autostart}], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + output='screen'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'slam_launch.py')), + condition=IfCondition(slam), + launch_arguments={'namespace': namespace, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'use_respawn': use_respawn, + 'params_file': params_file}.items()), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, + 'localization_launch.py')), + condition=IfCondition(PythonExpression(['not ', slam])), + launch_arguments={'namespace': namespace, + 'map': map_yaml_file, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'params_file': params_file, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container'}.items()), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')), + launch_arguments={'namespace': namespace, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'params_file': params_file, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container'}.items()), + ]) # Create the launch description and populate ld = LaunchDescription() diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index dbff594..fc1499a 100644 --- a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -14,16 +14,10 @@ import os - from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - ExecuteProcess, - GroupAction, - IncludeLaunchDescription, - LogInfo, -) +from launch.actions import (DeclareLaunchArgument, ExecuteProcess, GroupAction, + IncludeLaunchDescription, LogInfo) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, TextSubstitution @@ -36,9 +30,9 @@ def generate_launch_description(): Launch arguments consist of robot name(which is namespace) and pose for initialization. Keep general yaml format for pose information. - ex) robots:='robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}' - ex) robots:='robot3={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}; - robot4={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}' + ex) robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}" + ex) robots:="robot3={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}; + robot4={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}" """ # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') @@ -61,64 +55,47 @@ def generate_launch_description(): declare_world_cmd = DeclareLaunchArgument( 'world', default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), - description='Full path to world file to load', - ) + description='Full path to world file to load') declare_simulator_cmd = DeclareLaunchArgument( 'simulator', default_value='gazebo', - description='The simulator to use (gazebo or gzserver)', - ) + description='The simulator to use (gazebo or gzserver)') declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), - description='Full path to map file to load', - ) + description='Full path to map file to load') declare_params_file_cmd = DeclareLaunchArgument( 'params_file', - default_value=os.path.join( - bringup_dir, 'params', 'nav2_multirobot_params_all.yaml' - ), - description='Full path to the ROS2 parameters file to use for all launched nodes', - ) + default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_all.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', - default_value='false', - description='Automatically startup the stacks', - ) + 'autostart', default_value='false', + description='Automatically startup the stacks') declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), - description='Full path to the RVIZ config file to use.', - ) + description='Full path to the RVIZ config file to use.') declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 'use_robot_state_pub', default_value='True', - description='Whether to start the robot state publisher', - ) + description='Whether to start the robot state publisher') declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', default_value='True', description='Whether to start RVIZ' - ) + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') # Start Gazebo with plugin providing the robot spawning service start_gazebo_cmd = ExecuteProcess( - cmd=[ - simulator, - '--verbose', - '-s', - 'libgazebo_ros_init.so', - '-s', - 'libgazebo_ros_factory.so', - world, - ], - output='screen', - ) + cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so', + '-s', 'libgazebo_ros_factory.so', world], + output='screen') robots_list = ParseMultiRobotPose('robots').value() @@ -126,53 +103,39 @@ def generate_launch_description(): bringup_cmd_group = [] for robot_name in robots_list: init_pose = robots_list[robot_name] - group = GroupAction( - [ - LogInfo( - msg=[ - 'Launching namespace=', - robot_name, - ' init_pose=', - str(init_pose), - ] - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'rviz_launch.py') - ), - condition=IfCondition(use_rviz), - launch_arguments={ - 'namespace': TextSubstitution(text=robot_name), - 'use_namespace': 'True', - 'rviz_config': rviz_config_file, - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py') - ), - launch_arguments={ - 'namespace': robot_name, - 'use_namespace': 'True', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': params_file, - 'autostart': autostart, - 'use_rviz': 'False', - 'use_simulator': 'False', - 'headless': 'False', - 'use_robot_state_pub': use_robot_state_pub, - 'x_pose': TextSubstitution(text=str(init_pose['x'])), - 'y_pose': TextSubstitution(text=str(init_pose['y'])), - 'z_pose': TextSubstitution(text=str(init_pose['z'])), - 'roll': TextSubstitution(text=str(init_pose['roll'])), - 'pitch': TextSubstitution(text=str(init_pose['pitch'])), - 'yaw': TextSubstitution(text=str(init_pose['yaw'])), - 'robot_name': TextSubstitution(text=robot_name), - }.items(), - ), - ] - ) + group = GroupAction([ + LogInfo(msg=['Launching namespace=', robot_name, ' init_pose=', str(init_pose)]), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': TextSubstitution(text=robot_name), + 'use_namespace': 'True', + 'rviz_config': rviz_config_file}.items()), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(bringup_dir, + 'launch', + 'tb3_simulation_launch.py')), + launch_arguments={'namespace': robot_name, + 'use_namespace': 'True', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'autostart': autostart, + 'use_rviz': 'False', + 'use_simulator': 'False', + 'headless': 'False', + 'use_robot_state_pub': use_robot_state_pub, + 'x_pose': TextSubstitution(text=str(init_pose['x'])), + 'y_pose': TextSubstitution(text=str(init_pose['y'])), + 'z_pose': TextSubstitution(text=str(init_pose['z'])), + 'roll': TextSubstitution(text=str(init_pose['roll'])), + 'pitch': TextSubstitution(text=str(init_pose['pitch'])), + 'yaw': TextSubstitution(text=str(init_pose['yaw'])), + 'robot_name':TextSubstitution(text=robot_name), }.items()) + ]) bringup_cmd_group.append(group) @@ -194,27 +157,16 @@ def generate_launch_description(): ld.add_action(LogInfo(msg=['number_of_robots=', str(len(robots_list))])) - ld.add_action( - LogInfo(condition=IfCondition(log_settings), msg=['map yaml: ', map_yaml_file]) - ) - ld.add_action( - LogInfo(condition=IfCondition(log_settings), msg=['params yaml: ', params_file]) - ) - ld.add_action( - LogInfo( - condition=IfCondition(log_settings), - msg=['rviz config file: ', rviz_config_file], - ) - ) - ld.add_action( - LogInfo( - condition=IfCondition(log_settings), - msg=['using robot state pub: ', use_robot_state_pub], - ) - ) - ld.add_action( - LogInfo(condition=IfCondition(log_settings), msg=['autostart: ', autostart]) - ) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['map yaml: ', map_yaml_file])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['params yaml: ', params_file])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['rviz config file: ', rviz_config_file])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['using robot state pub: ', use_robot_state_pub])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['autostart: ', autostart])) for cmd in bringup_cmd_group: ld.add_action(cmd) diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 42afb5c..f5b3e3f 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -17,13 +17,10 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction -from launch.actions import SetEnvironmentVariable +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition -from launch.substitutions import EqualsSubstitution from launch.substitutions import LaunchConfiguration, PythonExpression -from launch.substitutions import NotEqualsSubstitution -from launch_ros.actions import LoadComposableNodes, SetParameter +from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml @@ -52,78 +49,68 @@ 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')] + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'yaml_filename': map_yaml_file} configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, - param_rewrites={}, - convert_types=True, - ), - allow_substs=True, - ) + param_rewrites=param_substitutions, + 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_map_yaml_cmd = DeclareLaunchArgument( - 'map', default_value='', description='Full path to map yaml file to load' - ) + 'map', + description='Full path to map yaml file to load') 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( - condition=IfCondition( - EqualsSubstitution(LaunchConfiguration('map'), '') - ), package='nav2_map_server', executable='map_server', name='map_server', @@ -132,22 +119,7 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - ), - Node( - condition=IfCondition( - NotEqualsSubstitution(LaunchConfiguration('map'), '') - ), - package='nav2_map_server', - executable='map_server', - name='map_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params, {'yaml_filename': map_yaml_file}], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - ), + remappings=remappings), Node( package='nav2_amcl', executable='amcl', @@ -157,80 +129,42 @@ 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_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_localization', output='screen', arguments=['--ros-args', '--log-level', log_level], - parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], - ), - ], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + ] ) - # LoadComposableNode for map server twice depending if we should use the - # value of map from a CLI or launch default or user defined value in the - # yaml configuration file. They are separated since the conditions - # currently only work on the LoadComposableNodes commands and not on the - # ComposableNode node function itself - load_composable_nodes = GroupAction( + + load_composable_nodes = LoadComposableNodes( condition=IfCondition(use_composition), - actions=[ - SetParameter('use_sim_time', use_sim_time), - LoadComposableNodes( - target_container=container_name_full, - condition=IfCondition( - EqualsSubstitution(LaunchConfiguration('map'), '') - ), - composable_node_descriptions=[ - ComposableNode( - package='nav2_map_server', - plugin='nav2_map_server::MapServer', - name='map_server', - parameters=[configured_params], - remappings=remappings, - ), - ], - ), - LoadComposableNodes( - target_container=container_name_full, - condition=IfCondition( - NotEqualsSubstitution(LaunchConfiguration('map'), '') - ), - composable_node_descriptions=[ - ComposableNode( - package='nav2_map_server', - plugin='nav2_map_server::MapServer', - name='map_server', - parameters=[ - configured_params, - {'yaml_filename': map_yaml_file}, - ], - remappings=remappings, - ), - ], - ), - LoadComposableNodes( - target_container=container_name_full, - composable_node_descriptions=[ - ComposableNode( - package='nav2_amcl', - plugin='nav2_amcl::AmclNode', - name='amcl', - parameters=[configured_params], - remappings=remappings, - ), - ComposableNode( - package='nav2_lifecycle_manager', - plugin='nav2_lifecycle_manager::LifecycleManager', - name='lifecycle_manager_localization', - parameters=[ - {'autostart': autostart, 'node_names': lifecycle_nodes} - ], - ), - ], - ), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::MapServer', + name='map_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_amcl', + plugin='nav2_amcl::AmclNode', + name='amcl', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_localization', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), ], ) diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 84d63e0..adb5a92 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -20,7 +20,7 @@ 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 LoadComposableNodes from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml @@ -40,16 +40,13 @@ def generate_launch_description(): 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', - ] + lifecycle_nodes = ['controller_server', + 'smoother_server', + 'planner_server', + 'behavior_server', + 'bt_navigator', + 'waypoint_follower', + 'velocity_smoother'] # Map fully qualified names to relative ones so the node's namespace can be prepended. # In case of the transforms (tf), currently, there doesn't seem to be a better alternative @@ -57,73 +54,63 @@ 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')] + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] # Create our own temporary YAML files that include substitutions - param_substitutions = {'autostart': autostart} + param_substitutions = { + 'use_sim_time': use_sim_time, + 'autostart': autostart} configured_params = ParameterFile( 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', @@ -132,8 +119,7 @@ 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', @@ -143,8 +129,7 @@ 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', @@ -154,8 +139,7 @@ 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', @@ -165,8 +149,7 @@ 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), Node( package='nav2_bt_navigator', executable='bt_navigator', @@ -176,8 +159,7 @@ 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', @@ -187,8 +169,7 @@ 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', @@ -198,105 +179,74 @@ 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')], - ), - 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, - ), + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_navigation', output='screen', arguments=['--ros-args', '--log-level', log_level], - parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], - ), - ], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]), + ] ) - load_composable_nodes = GroupAction( + load_composable_nodes = LoadComposableNodes( condition=IfCondition(use_composition), - 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} - ], - ), - ], - ), + 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}]), ], ) diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index dee33f2..29194ac 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -39,23 +39,18 @@ def generate_launch_description(): declare_namespace_cmd = DeclareLaunchArgument( 'namespace', default_value='navigation', - description=( - 'Top-level namespace. The value will be used to replace the ' - ' keyword on the rviz config file.' - ), - ) + description=('Top-level namespace. The value will be used to replace the ' + ' keyword on the rviz config file.')) declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', - description='Whether to apply a namespace to the navigation stack', - ) + description='Whether to apply a namespace to the navigation stack') declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), - description='Full path to the RVIZ config file to use', - ) + description='Full path to the RVIZ config file to use') # Launch rviz start_rviz_cmd = Node( @@ -63,13 +58,11 @@ def generate_launch_description(): package='rviz2', executable='rviz2', arguments=['-d', rviz_config_file], - output='screen', - ) + output='screen') namespaced_rviz_config_file = ReplaceString( - source_file=rviz_config_file, - replacements={'': ('/', namespace)}, - ) + source_file=rviz_config_file, + replacements={'': ('/', namespace)}) start_namespaced_rviz_cmd = Node( condition=IfCondition(use_namespace), @@ -78,31 +71,24 @@ def generate_launch_description(): namespace=namespace, arguments=['-d', namespaced_rviz_config_file], output='screen', - remappings=[ - ('/map', 'map'), - ('/tf', 'tf'), - ('/tf_static', 'tf_static'), - ('/goal_pose', 'goal_pose'), - ('/clicked_point', 'clicked_point'), - ('/initialpose', 'initialpose'), - ], - ) + remappings=[('/map', 'map'), + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('/goal_pose', 'goal_pose'), + ('/clicked_point', 'clicked_point'), + ('/initialpose', 'initialpose')]) exit_event_handler = RegisterEventHandler( condition=UnlessCondition(use_namespace), event_handler=OnProcessExit( target_action=start_rviz_cmd, - on_exit=EmitEvent(event=Shutdown(reason='rviz exited')), - ), - ) + on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) exit_event_handler_namespaced = RegisterEventHandler( condition=IfCondition(use_namespace), event_handler=OnProcessExit( target_action=start_namespaced_rviz_cmd, - on_exit=EmitEvent(event=Shutdown(reason='rviz exited')), - ), - ) + on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) # Create the launch description and populate ld = LaunchDescription() diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py index a806d26..52c810a 100644 --- a/nav2_bringup/launch/slam_launch.py +++ b/nav2_bringup/launch/slam_launch.py @@ -17,11 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.conditions import IfCondition, UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node, SetParameter +from launch_ros.actions import Node from launch_ros.descriptions import ParameterFile from nav2_common.launch import HasNodeParams, RewrittenYaml @@ -44,95 +44,82 @@ def generate_launch_description(): slam_launch_file = os.path.join(slam_toolbox_dir, 'launch', 'online_sync_launch.py') # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time} + configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, - param_rewrites={}, - convert_types=True, - ), - allow_substs=True, - ) + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', default_value='', description='Top-level namespace' - ) + 'namespace', + default_value='', + description='Top-level namespace') 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_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='True', - description='Use simulation (Gazebo) clock if true', - ) + description='Use simulation (Gazebo) clock if true') 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_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') # Nodes launching commands - start_map_server = GroupAction( - actions=[ - SetParameter('use_sim_time', use_sim_time), - Node( - package='nav2_map_server', - executable='map_saver_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - arguments=['--ros-args', '--log-level', log_level], - parameters=[configured_params], - ), - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_slam', - output='screen', - arguments=['--ros-args', '--log-level', log_level], - parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], - ), - ] - ) + start_map_saver_server_cmd = Node( + package='nav2_map_server', + executable='map_saver_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + arguments=['--ros-args', '--log-level', log_level], + parameters=[configured_params]) + + start_lifecycle_manager_cmd = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_slam', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) # If the provided param file doesn't have slam_toolbox params, we must remove the 'params_file' # LaunchConfiguration, or it will be passed automatically to slam_toolbox and will not load # the default file - has_slam_toolbox_params = HasNodeParams( - source_file=params_file, node_name='slam_toolbox' - ) + has_slam_toolbox_params = HasNodeParams(source_file=params_file, + node_name='slam_toolbox') start_slam_toolbox_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource(slam_launch_file), launch_arguments={'use_sim_time': use_sim_time}.items(), - condition=UnlessCondition(has_slam_toolbox_params), - ) + condition=UnlessCondition(has_slam_toolbox_params)) start_slam_toolbox_cmd_with_params = IncludeLaunchDescription( PythonLaunchDescriptionSource(slam_launch_file), - launch_arguments={ - 'use_sim_time': use_sim_time, - 'slam_params_file': params_file, - }.items(), - condition=IfCondition(has_slam_toolbox_params), - ) + launch_arguments={'use_sim_time': use_sim_time, + 'slam_params_file': params_file}.items(), + condition=IfCondition(has_slam_toolbox_params)) ld = LaunchDescription() @@ -145,7 +132,8 @@ def generate_launch_description(): ld.add_action(declare_log_level_cmd) # Running Map Saver Server - ld.add_action(start_map_server) + ld.add_action(start_map_saver_server_cmd) + ld.add_action(start_lifecycle_manager_cmd) # Running SLAM Toolbox (Only one of them will be run) ld.add_action(start_slam_toolbox_cmd) diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index 6598ac9..b4dfd3b 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -19,11 +19,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, -) +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -34,8 +30,6 @@ def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') launch_dir = os.path.join(bringup_dir, 'launch') - # This checks that tb3 exists needed for the URDF. If not using TB3, its safe to remove. - _ = get_package_share_directory('turtlebot3_gazebo') # Create the launch configuration variables slam = LaunchConfiguration('slam') @@ -55,14 +49,12 @@ def generate_launch_description(): use_rviz = LaunchConfiguration('use_rviz') headless = LaunchConfiguration('headless') world = LaunchConfiguration('world') - pose = { - 'x': LaunchConfiguration('x_pose', default='-2.00'), - 'y': LaunchConfiguration('y_pose', default='-0.50'), - 'z': LaunchConfiguration('z_pose', default='0.01'), - 'R': LaunchConfiguration('roll', default='0.00'), - 'P': LaunchConfiguration('pitch', default='0.00'), - 'Y': LaunchConfiguration('yaw', default='0.00'), - } + pose = {'x': LaunchConfiguration('x_pose', default='-2.00'), + 'y': LaunchConfiguration('y_pose', default='-0.50'), + 'z': LaunchConfiguration('z_pose', default='0.01'), + 'R': LaunchConfiguration('roll', default='0.00'), + 'P': LaunchConfiguration('pitch', default='0.00'), + 'Y': LaunchConfiguration('yaw', default='0.00')} robot_name = LaunchConfiguration('robot_name') robot_sdf = LaunchConfiguration('robot_sdf') @@ -72,84 +64,78 @@ 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')] + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', default_value='', description='Top-level namespace' - ) + 'namespace', + default_value='', + description='Top-level namespace') declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', - description='Whether to apply a namespace to the navigation stack', - ) + description='Whether to apply a namespace to the navigation stack') declare_slam_cmd = DeclareLaunchArgument( - 'slam', default_value='False', description='Whether run a SLAM' - ) + 'slam', + default_value='False', + description='Whether run a SLAM') declare_map_yaml_cmd = DeclareLaunchArgument( 'map', - default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), - description='Full path to map file to load', - ) + default_value=os.path.join( + bringup_dir, 'maps', 'turtlebot3_world.yaml'), + description='Full path to map file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='true', - 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='True', - description='Whether to use composed bringup', - ) + 'use_composition', default_value='True', + description='Whether to use composed bringup') 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_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', - default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), - description='Full path to the RVIZ config file to use', - ) + default_value=os.path.join( + bringup_dir, 'rviz', 'nav2_default_view.rviz'), + description='Full path to the RVIZ config file to use') declare_use_simulator_cmd = DeclareLaunchArgument( 'use_simulator', default_value='True', - description='Whether to start the simulator', - ) + description='Whether to start the simulator') declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 'use_robot_state_pub', default_value='True', - description='Whether to start the robot state publisher', - ) + description='Whether to start the robot state publisher') declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', default_value='True', description='Whether to start RVIZ' - ) + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') declare_simulator_cmd = DeclareLaunchArgument( - 'headless', default_value='True', description='Whether to execute gzclient)' - ) + 'headless', + default_value='True', + description='Whether to execute gzclient)') declare_world_cmd = DeclareLaunchArgument( 'world', @@ -158,40 +144,30 @@ def generate_launch_description(): # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'), # worlds/turtlebot3_worlds/waffle.model') default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), - description='Full path to world model file to load', - ) + description='Full path to world model file to load') declare_robot_name_cmd = DeclareLaunchArgument( - 'robot_name', default_value='turtlebot3_waffle', description='name of the robot' - ) + 'robot_name', + default_value='turtlebot3_waffle', + description='name of the robot') declare_robot_sdf_cmd = DeclareLaunchArgument( 'robot_sdf', default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'), - description='Full path to robot sdf file to spawn the robot in gazebo', - ) + description='Full path to robot sdf file to spawn the robot in gazebo') # Specify the actions start_gazebo_server_cmd = ExecuteProcess( condition=IfCondition(use_simulator), - cmd=[ - 'gzserver', - '-s', - 'libgazebo_ros_init.so', - '-s', - 'libgazebo_ros_factory.so', - world, - ], - cwd=[launch_dir], - output='screen', - ) + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '-s', 'libgazebo_ros_factory.so', world], + cwd=[launch_dir], output='screen') start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])), + condition=IfCondition(PythonExpression( + [use_simulator, ' and not ', headless])), cmd=['gzclient'], - cwd=[launch_dir], - output='screen', - ) + cwd=[launch_dir], output='screen') urdf = os.path.join(bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') with open(urdf, 'r') as infp: @@ -204,62 +180,41 @@ def generate_launch_description(): name='robot_state_publisher', namespace=namespace, output='screen', - parameters=[ - {'use_sim_time': use_sim_time, 'robot_description': robot_description} - ], - remappings=remappings, - ) + parameters=[{'use_sim_time': use_sim_time, + 'robot_description': robot_description}], + remappings=remappings) start_gazebo_spawner_cmd = Node( package='gazebo_ros', executable='spawn_entity.py', output='screen', arguments=[ - '-entity', - robot_name, - '-file', - robot_sdf, - '-robot_namespace', - namespace, - '-x', - pose['x'], - '-y', - pose['y'], - '-z', - pose['z'], - '-R', - pose['R'], - '-P', - pose['P'], - '-Y', - pose['Y'], - ], - ) + '-entity', robot_name, + '-file', robot_sdf, + '-robot_namespace', namespace, + '-x', pose['x'], '-y', pose['y'], '-z', pose['z'], + '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']]) rviz_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')), + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'rviz_launch.py')), condition=IfCondition(use_rviz), - launch_arguments={ - 'namespace': namespace, - 'use_namespace': use_namespace, - 'rviz_config': rviz_config_file, - }.items(), - ) + launch_arguments={'namespace': namespace, + 'use_namespace': use_namespace, + 'rviz_config': rviz_config_file}.items()) bringup_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), - launch_arguments={ - 'namespace': namespace, - 'use_namespace': use_namespace, - 'slam': slam, - 'map': map_yaml_file, - 'use_sim_time': use_sim_time, - 'params_file': params_file, - 'autostart': autostart, - 'use_composition': use_composition, - 'use_respawn': use_respawn, - }.items(), - ) + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'bringup_launch.py')), + launch_arguments={'namespace': namespace, + 'use_namespace': use_namespace, + 'slam': slam, + 'map': map_yaml_file, + 'use_sim_time': use_sim_time, + 'params_file': params_file, + 'autostart': autostart, + 'use_composition': use_composition, + 'use_respawn': use_respawn}.items()) # Create the launch description and populate ld = LaunchDescription() diff --git a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py index 2319def..d91c5ce 100644 --- a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py @@ -25,13 +25,8 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - ExecuteProcess, - GroupAction, - IncludeLaunchDescription, - LogInfo, -) +from launch.actions import (DeclareLaunchArgument, ExecuteProcess, GroupAction, + IncludeLaunchDescription, LogInfo) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, TextSubstitution @@ -44,25 +39,10 @@ def generate_launch_description(): # Names and poses of the robots robots = [ - { - 'name': 'robot1', - 'x_pose': 0.0, - 'y_pose': 0.5, - 'z_pose': 0.01, - 'roll': 0.0, - 'pitch': 0.0, - 'yaw': 0.0, - }, - { - 'name': 'robot2', - 'x_pose': 0.0, - 'y_pose': -0.5, - 'z_pose': 0.01, - 'roll': 0.0, - 'pitch': 0.0, - 'yaw': 0.0, - }, - ] + {'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01, + 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}, + {'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01, + 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}] # Simulation settings world = LaunchConfiguration('world') @@ -81,145 +61,109 @@ def generate_launch_description(): declare_world_cmd = DeclareLaunchArgument( 'world', default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), - description='Full path to world file to load', - ) + description='Full path to world file to load') declare_simulator_cmd = DeclareLaunchArgument( 'simulator', default_value='gazebo', - description='The simulator to use (gazebo or gzserver)', - ) + description='The simulator to use (gazebo or gzserver)') declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), - description='Full path to map file to load', - ) + description='Full path to map file to load') declare_robot1_params_file_cmd = DeclareLaunchArgument( 'robot1_params_file', - default_value=os.path.join( - bringup_dir, 'params', 'nav2_multirobot_params_1.yaml' - ), - description='Full path to the ROS2 parameters file to use for robot1 launched nodes', - ) + default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_1.yaml'), + description='Full path to the ROS2 parameters file to use for robot1 launched nodes') declare_robot2_params_file_cmd = DeclareLaunchArgument( 'robot2_params_file', - default_value=os.path.join( - bringup_dir, 'params', 'nav2_multirobot_params_2.yaml' - ), - description='Full path to the ROS2 parameters file to use for robot2 launched nodes', - ) + default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_2.yaml'), + description='Full path to the ROS2 parameters file to use for robot2 launched nodes') declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', - default_value='false', - description='Automatically startup the stacks', - ) + 'autostart', default_value='false', + description='Automatically startup the stacks') declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), - description='Full path to the RVIZ config file to use.', - ) + description='Full path to the RVIZ config file to use.') declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 'use_robot_state_pub', default_value='True', - description='Whether to start the robot state publisher', - ) + description='Whether to start the robot state publisher') declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', default_value='True', description='Whether to start RVIZ' - ) + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') # Start Gazebo with plugin providing the robot spawning service start_gazebo_cmd = ExecuteProcess( - cmd=[ - simulator, - '--verbose', - '-s', - 'libgazebo_ros_init.so', - '-s', - 'libgazebo_ros_factory.so', - world, - ], - output='screen', - ) + cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so', + '-s', 'libgazebo_ros_factory.so', world], + output='screen') # Define commands for launching the navigation instances nav_instances_cmds = [] for robot in robots: params_file = LaunchConfiguration(f"{robot['name']}_params_file") - group = GroupAction( - [ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'rviz_launch.py') - ), - condition=IfCondition(use_rviz), - launch_arguments={ - 'namespace': TextSubstitution(text=robot['name']), - 'use_namespace': 'True', - 'rviz_config': rviz_config_file, - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py') - ), - launch_arguments={ - 'namespace': robot['name'], - 'use_namespace': 'True', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': params_file, - 'autostart': autostart, - 'use_rviz': 'False', - 'use_simulator': 'False', - 'headless': 'False', - 'use_robot_state_pub': use_robot_state_pub, - 'x_pose': TextSubstitution(text=str(robot['x_pose'])), - 'y_pose': TextSubstitution(text=str(robot['y_pose'])), - 'z_pose': TextSubstitution(text=str(robot['z_pose'])), - 'roll': TextSubstitution(text=str(robot['roll'])), - 'pitch': TextSubstitution(text=str(robot['pitch'])), - 'yaw': TextSubstitution(text=str(robot['yaw'])), - 'robot_name': TextSubstitution(text=robot['name']), - }.items(), - ), - LogInfo( - condition=IfCondition(log_settings), - msg=['Launching ', robot['name']], - ), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' map yaml: ', map_yaml_file], - ), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' params yaml: ', params_file], - ), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' rviz config file: ', rviz_config_file], - ), - LogInfo( - condition=IfCondition(log_settings), - msg=[ - robot['name'], - ' using robot state pub: ', - use_robot_state_pub, - ], - ), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' autostart: ', autostart], - ), - ] - ) + group = GroupAction([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={ + 'namespace': TextSubstitution(text=robot['name']), + 'use_namespace': 'True', + 'rviz_config': rviz_config_file}.items()), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(bringup_dir, + 'launch', + 'tb3_simulation_launch.py')), + launch_arguments={'namespace': robot['name'], + 'use_namespace': 'True', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'autostart': autostart, + 'use_rviz': 'False', + 'use_simulator': 'False', + 'headless': 'False', + 'use_robot_state_pub': use_robot_state_pub, + 'x_pose': TextSubstitution(text=str(robot['x_pose'])), + 'y_pose': TextSubstitution(text=str(robot['y_pose'])), + 'z_pose': TextSubstitution(text=str(robot['z_pose'])), + 'roll': TextSubstitution(text=str(robot['roll'])), + 'pitch': TextSubstitution(text=str(robot['pitch'])), + 'yaw': TextSubstitution(text=str(robot['yaw'])), + 'robot_name':TextSubstitution(text=robot['name']), }.items()), + + LogInfo( + condition=IfCondition(log_settings), + msg=['Launching ', robot['name']]), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' map yaml: ', map_yaml_file]), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' params yaml: ', params_file]), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' rviz config file: ', rviz_config_file]), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' using robot state pub: ', use_robot_state_pub]), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' autostart: ', autostart]) + ]) nav_instances_cmds.append(group) diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 7080f58..1e41f7a 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.2.0 + 1.1.13 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski @@ -19,7 +19,6 @@ navigation2 nav2_common slam_toolbox - turtlebot3_gazebo ament_lint_common ament_lint_auto diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index cae1b00..58d12bc 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -1,11 +1,12 @@ amcl: ros__parameters: + use_sim_time: True alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 alpha4: 0.2 alpha5: 0.2 - base_frame_id: "base_link" + base_frame_id: "base_footprint" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 @@ -40,38 +41,81 @@ amcl: bt_navigator: ros__parameters: + use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 - 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 is used to add custom BT plugins to the executor (vector of strings). - # Built-in plugins are added automatically - # plugin_lib_names: [] + 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 - error_code_names: - - compute_path_error_code - - follow_path_error_code +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True controller_server: ros__parameters: + use_sim_time: True controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 - progress_checker_plugins: ["progress_checker"] + progress_checker_plugin: "progress_checker" goal_checker_plugins: ["goal_checker"] controller_plugins: ["FollowPath"] @@ -129,6 +173,7 @@ controller_server: local_costmap: local_costmap: ros__parameters: + use_sim_time: True global_frame: odom rolling_window: true width: 3 @@ -167,6 +212,7 @@ local_costmap: global_costmap: global_costmap: ros__parameters: + use_sim_time: True robot_radius: 0.22 plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: @@ -194,11 +240,13 @@ global_costmap: map_server: ros__parameters: + use_sim_time: True yaml_filename: "turtlebot3_world.yaml" save_map_timeout: 5.0 planner_server: ros__parameters: + use_sim_time: True planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" @@ -206,6 +254,10 @@ planner_server: use_astar: false allow_unknown: true +smoother_server: + ros__parameters: + use_sim_time: True + behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw @@ -223,49 +275,22 @@ behavior_server: global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 + use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 +robot_state_publisher: + ros__parameters: + use_sim_time: True + waypoint_follower: ros__parameters: loop_rate: 20 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" enabled: True waypoint_pause_duration: 200 - -collision_monitor: - ros__parameters: - base_frame_id: "base_link" - odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_smoothed" - cmd_vel_out_topic: "cmd_vel" - state_topic: "collision_monitor_state" - transform_tolerance: 0.2 - source_timeout: 1.0 - base_shift_correction: True - stop_pub_timeout: 2.0 - # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, - # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: - type: "polygon" - action_type: "approach" - footprint_topic: "local_costmap/published_footprint" - time_before_collision: 2.0 - simulation_time_step: 0.1 - min_points: 6 - visualize: False - enabled: True - observation_sources: ["scan"] - scan: - type: "scan" - topic: "/robot1/scan" - min_height: 0.15 - max_height: 2.0 - enabled: True diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index cac04b0..f1704ed 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -1,11 +1,12 @@ amcl: ros__parameters: + use_sim_time: True alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 alpha4: 0.2 alpha5: 0.2 - base_frame_id: "base_link" + base_frame_id: "base_footprint" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 @@ -40,38 +41,81 @@ amcl: bt_navigator: ros__parameters: + use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 - 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 - # 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_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True - error_code_names: - - compute_path_error_code - - follow_path_error_code +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True controller_server: ros__parameters: + use_sim_time: True controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 - progress_checker_plugins: ["progress_checker"] + progress_checker_plugin: "progress_checker" goal_checker_plugins: ["goal_checker"] controller_plugins: ["FollowPath"] @@ -129,6 +173,7 @@ controller_server: local_costmap: local_costmap: ros__parameters: + use_sim_time: True global_frame: odom rolling_window: true width: 3 @@ -166,6 +211,7 @@ local_costmap: global_costmap: global_costmap: ros__parameters: + use_sim_time: True robot_radius: 0.22 plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: @@ -193,11 +239,13 @@ global_costmap: map_server: ros__parameters: + use_sim_time: True yaml_filename: "turtlebot3_world.yaml" save_map_timeout: 5.0 planner_server: ros__parameters: + use_sim_time: True planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" @@ -205,6 +253,10 @@ planner_server: use_astar: false allow_unknown: true +smoother_server: + ros__parameters: + use_sim_time: True + behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw @@ -222,49 +274,22 @@ behavior_server: global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 + use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 +robot_state_publisher: + ros__parameters: + use_sim_time: True + waypoint_follower: ros__parameters: loop_rate: 20 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" enabled: True waypoint_pause_duration: 200 - -collision_monitor: - ros__parameters: - base_frame_id: "base_link" - odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_smoothed" - cmd_vel_out_topic: "cmd_vel" - state_topic: "collision_monitor_state" - transform_tolerance: 0.2 - source_timeout: 1.0 - base_shift_correction: True - stop_pub_timeout: 2.0 - # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, - # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: - type: "polygon" - action_type: "approach" - footprint_topic: "local_costmap/published_footprint" - time_before_collision: 2.0 - simulation_time_step: 0.1 - min_points: 6 - visualize: False - enabled: True - observation_sources: ["scan"] - scan: - type: "scan" - topic: "/robot2/scan" - min_height: 0.15 - max_height: 2.0 - enabled: True diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index ffb24e2..372dcdd 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -5,7 +5,7 @@ amcl: alpha3: 0.2 alpha4: 0.2 alpha5: 0.2 - base_frame_id: "base_link" + base_frame_id: "base_footprint" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 @@ -45,8 +45,6 @@ bt_navigator: odom_topic: /odom 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" @@ -56,11 +54,58 @@ bt_navigator: # 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 is used to add custom BT plugins to the executor (vector of strings). - # Built-in plugins are added automatically - # plugin_lib_names: [] - + 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_are_error_codes_active_condition_bt_node + - nav2_would_a_controller_recovery_help_condition_bt_node + - nav2_would_a_planner_recovery_help_condition_bt_node + - nav2_would_a_smoother_recovery_help_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 error_code_names: - compute_path_error_code - follow_path_error_code @@ -279,7 +324,6 @@ waypoint_follower: ros__parameters: loop_rate: 20 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" @@ -299,34 +343,3 @@ velocity_smoother: odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 - -collision_monitor: - ros__parameters: - base_frame_id: "base_link" - odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_smoothed" - cmd_vel_out_topic: "cmd_vel" - state_topic: "collision_monitor_state" - transform_tolerance: 0.2 - source_timeout: 1.0 - base_shift_correction: True - stop_pub_timeout: 2.0 - # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, - # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: - type: "polygon" - action_type: "approach" - footprint_topic: "/local_costmap/published_footprint" - time_before_collision: 2.0 - simulation_time_step: 0.1 - min_points: 6 - visualize: False - enabled: True - observation_sources: ["scan"] - scan: - type: "scan" - topic: "/scan" - min_height: 0.15 - max_height: 2.0 - enabled: True diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index bb2956c..5f4b08f 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -1,11 +1,12 @@ amcl: ros__parameters: + use_sim_time: True alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 alpha4: 0.2 alpha5: 0.2 - base_frame_id: "base_link" + base_frame_id: "base_footprint" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 @@ -40,34 +41,84 @@ amcl: bt_navigator: ros__parameters: + use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 - 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 - error_code_names: - - compute_path_error_code - - follow_path_error_code +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True controller_server: ros__parameters: - controller_frequency: 1.0 - min_x_velocity_threshold: 10.5 - min_y_velocity_threshold: 10.5 - min_theta_velocity_threshold: 10.5 - failure_tolerance: 0.5 - progress_checker_plugins: ["progress_checker"] + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] - use_realtime_priority: false # Progress checker parameters progress_checker: @@ -91,11 +142,11 @@ controller_server: debug_trajectory_details: True min_vel_x: 0.0 min_vel_y: 0.0 - max_vel_x: 3.0 - max_vel_y: 3.0 - max_vel_theta: 3.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 min_speed_xy: 0.0 - max_speed_xy: 3.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 @@ -108,6 +159,7 @@ controller_server: 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 @@ -134,13 +186,17 @@ local_costmap: publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link - use_sim_time: False - rolling_window: True + use_sim_time: True + rolling_window: true width: 3 height: 3 resolution: 0.05 - footprint: "[ [0.35, 0.195], [0.35, -0.195], [-0.35, -0.195], [-0.35, 0.195] ]" + robot_radius: 0.22 plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -157,10 +213,13 @@ local_costmap: clearing: True marking: True data_type: "LaserScan" - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.1 + 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: @@ -170,14 +229,15 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - footprint: "[ [0.35, 0.195], [0.35, -0.195], [-0.35, -0.195], [-0.35, 0.195] ]" + use_sim_time: True + robot_radius: 0.22 resolution: 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True - observation_sources: scan pointcloud + observation_sources: scan scan: topic: /scan max_obstacle_height: 2.0 @@ -188,27 +248,25 @@ global_costmap: raytrace_min_range: 0.0 obstacle_max_range: 2.5 obstacle_min_range: 0.0 - pointcloud: - topic: /point_cloud2 - data_type: "PointCloud2" 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.1 + inflation_radius: 0.55 always_send_full_costmap: True -# 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_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" map_saver: ros__parameters: + use_sim_time: True save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 @@ -217,6 +275,7 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 + use_sim_time: True planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" @@ -224,34 +283,67 @@ planner_server: use_astar: false allow_unknown: true +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True -collision_monitor: +behavior_server: ros__parameters: - base_frame_id: "base_link" - odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_smoothed" - cmd_vel_out_topic: "cmd_vel" - state_topic: "collision_monitor_state" - transform_tolerance: 2.0 - source_timeout: 1.0 - base_shift_correction: True - stop_pub_timeout: 2.0 - # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, - # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: - type: "polygon" - action_type: "approach" - footprint_topic: "/local_costmap/published_footprint" - time_before_collision: 1.2 - simulation_time_step: 0.1 - min_points: 6 - visualize: True - enabled: True - observation_sources: ["scan"] - scan: - type: "scan" - topic: "scan" - min_height: 0.15 - max_height: 2.0 + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz index 28e4f10..d8bade7 100644 --- a/nav2_bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/rviz/nav2_default_view.rviz @@ -20,11 +20,9 @@ Panels: Expanded: - /Current View1 Name: Views - Splitter Ratio: 0.3333333432674408 + Splitter Ratio: 0.5 - Class: nav2_rviz_plugins/Navigation 2 Name: Navigation 2 - - Class: nav2_rviz_plugins/Selector - Name: Selector Visualization Manager: Class: "" Displays: diff --git a/nav2_bringup/worlds/waffle.model b/nav2_bringup/worlds/waffle.model index c0bdcac..3c5923d 100644 --- a/nav2_bringup/worlds/waffle.model +++ b/nav2_bringup/worlds/waffle.model @@ -145,7 +145,7 @@ - 0.00000 + 0.120000 20.0 0.015000 diff --git a/nav2_map_server/launch/map_saver_server.launch.py b/nav2_map_server/launch/map_saver_server.launch.py index aa3f2c4..5373156 100644 --- a/nav2_map_server/launch/map_saver_server.launch.py +++ b/nav2_map_server/launch/map_saver_server.launch.py @@ -29,29 +29,23 @@ def generate_launch_description(): # Nodes launching commands start_map_saver_server_cmd = launch_ros.actions.Node( - package='nav2_map_server', - executable='map_saver_server', - output='screen', - emulate_tty=True, # https://github.com/ros2/launch/issues/188 - parameters=[ - {'save_map_timeout': save_map_timeout}, - {'free_thresh_default': free_thresh_default}, - {'occupied_thresh_default': occupied_thresh_default}, - ], - ) + package='nav2_map_server', + executable='map_saver_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[{'save_map_timeout': save_map_timeout}, + {'free_thresh_default': free_thresh_default}, + {'occupied_thresh_default': occupied_thresh_default}]) start_lifecycle_manager_cmd = launch_ros.actions.Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager', - output='screen', - emulate_tty=True, # https://github.com/ros2/launch/issues/188 - parameters=[ - {'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}, - ], - ) + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) ld = LaunchDescription() diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 3f4e287..0852f14 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.2.0 + 1.1.13 Refactored map server for ROS2 Navigation diff --git a/nav2_map_server/test/component/test_map_saver_launch.py b/nav2_map_server/test/component/test_map_saver_launch.py index e33e03a..a491a2d 100755 --- a/nav2_map_server/test/component/test_map_saver_launch.py +++ b/nav2_map_server/test/component/test_map_saver_launch.py @@ -28,11 +28,9 @@ def main(argv=sys.argv[1:]): launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'map_saver_node.launch.py') testExecutable = os.getenv('TEST_EXECUTABLE') - ld = LaunchDescription( - [ - IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), - ] - ) + ld = LaunchDescription([ + IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), + ]) test1_action = ExecuteProcess( cmd=[testExecutable], name='test_map_saver_node', diff --git a/nav2_map_server/test/component/test_map_server_launch.py b/nav2_map_server/test/component/test_map_server_launch.py index 473feea..8164294 100755 --- a/nav2_map_server/test/component/test_map_server_launch.py +++ b/nav2_map_server/test/component/test_map_server_launch.py @@ -28,11 +28,9 @@ def main(argv=sys.argv[1:]): launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'map_server_node.launch.py') testExecutable = os.getenv('TEST_EXECUTABLE') - ld = LaunchDescription( - [ - IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), - ] - ) + ld = LaunchDescription([ + IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), + ]) test1_action = ExecuteProcess( cmd=[testExecutable], name='test_map_server_node', diff --git a/nav2_map_server/test/test_launch_files/map_saver_node.launch.py b/nav2_map_server/test/test_launch_files/map_saver_node.launch.py index a0ac0ae..7287f73 100644 --- a/nav2_map_server/test/test_launch_files/map_saver_node.launch.py +++ b/nav2_map_server/test/test_launch_files/map_saver_node.launch.py @@ -22,9 +22,7 @@ def generate_launch_description(): - map_publisher = ( - f"{os.path.dirname(os.getenv('TEST_EXECUTABLE'))}/test_map_saver_publisher" - ) + map_publisher = f"{os.path.dirname(os.getenv('TEST_EXECUTABLE'))}/test_map_saver_publisher" ld = LaunchDescription() @@ -32,10 +30,11 @@ def generate_launch_description(): package='nav2_map_server', executable='map_saver_server', output='screen', - parameters=[os.path.join(os.getenv('TEST_DIR'), 'map_saver_params.yaml')], - ) + parameters=[os.path.join(os.getenv('TEST_DIR'), + 'map_saver_params.yaml')]) - map_publisher_cmd = ExecuteProcess(cmd=[map_publisher]) + map_publisher_cmd = ExecuteProcess( + cmd=[map_publisher]) ld.add_action(map_saver_server_cmd) ld.add_action(map_publisher_cmd) diff --git a/nav2_map_server/test/test_launch_files/map_server_node.launch.py b/nav2_map_server/test/test_launch_files/map_server_node.launch.py index 813abf7..78d5e8b 100644 --- a/nav2_map_server/test/test_launch_files/map_server_node.launch.py +++ b/nav2_map_server/test/test_launch_files/map_server_node.launch.py @@ -21,15 +21,11 @@ def generate_launch_description(): - return LaunchDescription( - [ - launch_ros.actions.Node( - package='nav2_map_server', - executable='map_server', - output='screen', - parameters=[ - os.path.join(os.getenv('TEST_DIR'), 'map_server_params.yaml') - ], - ) - ] - ) + return LaunchDescription([ + launch_ros.actions.Node( + package='nav2_map_server', + executable='map_server', + output='screen', + parameters=[os.path.join(os.getenv('TEST_DIR'), + 'map_server_params.yaml')]) + ]) diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 1c8b9b1..4635bab 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -50,8 +50,6 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_pytest REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) - # skip copyright linting - set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/nav2_util/README.md b/nav2_util/README.md index adf71e6..8443709 100644 --- a/nav2_util/README.md +++ b/nav2_util/README.md @@ -7,25 +7,6 @@ The `nav2_util` package contains utilities abstracted from individual packages w - Simplified service clients - Simplified action servers - Transformation and robot pose helpers -- Twist Subscriber and Twist Publisher The long-term aim is for these utilities to find more permanent homes in other packages (within and outside of Nav2) or migrate to the raw tools made available in ROS 2. - -## Twist Publisher and Twist Subscriber for commanded velocities - -### Background - -The Twist Publisher and Twist Subscriber are utility classes to assist NAV2 transition from -[Twist](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/Twist.msg) to [TwistStamped](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/TwistStamped.msg). - -Details on the migration are found in [#1594](https://github.com/ros-planning/navigation2/issues/1594). - -Certain applications of NAV2, such as in ROS Aerial mandate the usage of `TwistStamped`, while many other applications still use `Twist`. - -The utility has the following effect: -* Allows use of either `Twist` or `TwistStamped`, controlled by ROS parameter `enable_stamped_cmd_vel` -* Preserves existing topic names without duplication of data - -Every node in `nav2` that subscribes or publishes velocity commands with `Twist` now supports this optional behavior. -The behavior up through ROS 2 Iron is preserved - using `Twist`. In a future ROS 2 version, when enough of the -ROS ecosystem has moved to `TwistStamped`, the default may change. + \ No newline at end of file diff --git a/nav2_util/include/nav2_util/array_parser.hpp b/nav2_util/include/nav2_util/array_parser.hpp deleted file mode 100644 index f232cf3..0000000 --- a/nav2_util/include/nav2_util/array_parser.hpp +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * author: Dave Hershberger - */ -#ifndef NAV2_UTIL__ARRAY_PARSER_HPP_ -#define NAV2_UTIL__ARRAY_PARSER_HPP_ - -#include -#include - -namespace nav2_util -{ - -/** @brief Parse a vector of vectors of floats from a string. - * @param error_return If no error, error_return is set to "". If - * error, error_return will describe the error. - * Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] - * - * On error, error_return is set and the return value could be - * anything, like part of a successful parse. */ -std::vector> parseVVF(const std::string & input, std::string & error_return); - -} // end namespace nav2_util - -#endif // NAV2_UTIL__ARRAY_PARSER_HPP_ diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 67cc872..884b7b3 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -132,7 +132,7 @@ inline Iter min_by(Iter begin, Iter end, Getter getCompareVal) Iter lowest_it = begin; for (Iter it = ++begin; it != end; ++it) { auto comp = getCompareVal(*it); - if (comp <= lowest) { + if (comp < lowest) { lowest = comp; lowest_it = it; } diff --git a/nav2_util/include/nav2_util/node_thread.hpp b/nav2_util/include/nav2_util/node_thread.hpp index e0c1692..78d84fb 100644 --- a/nav2_util/include/nav2_util/node_thread.hpp +++ b/nav2_util/include/nav2_util/node_thread.hpp @@ -32,15 +32,13 @@ class NodeThread * @brief A background thread to process node callbacks constructor * @param node_base Interface to Node to spin in thread */ - explicit NodeThread( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base); + explicit NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base); /** * @brief A background thread to process executor's callbacks constructor * @param executor Interface to executor to spin in thread */ - explicit NodeThread( - rclcpp::executors::SingleThreadedExecutor::SharedPtr executor); + explicit NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor); /** * @brief A background thread to process node callbacks constructor diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index 9957022..6220162 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -1,5 +1,4 @@ // Copyright (c) 2019 Intel Corporation -// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -143,22 +142,34 @@ std::string get_plugin_type_param( if (!node->get_parameter(plugin_name + ".plugin", plugin_type)) { RCLCPP_FATAL( node->get_logger(), "Can not get 'plugin' param value for %s", plugin_name.c_str()); - throw std::runtime_error("No 'plugin' param for param ns!"); + exit(-1); } } catch (rclcpp::exceptions::ParameterUninitializedException & ex) { RCLCPP_FATAL(node->get_logger(), "'plugin' param not defined for %s", plugin_name.c_str()); - throw std::runtime_error("No 'plugin' param for param ns!"); + exit(-1); } return plugin_type; } /** - * @brief Sets the caller thread to have a soft-realtime prioritization by - * increasing the priority level of the host thread. - * May throw exception if unable to set prioritization successfully + * @brief A method to copy all parameters from one node (parent) to another (child). + * May throw parameter exceptions in error conditions + * @param parent Node to copy parameters from + * @param child Node to copy parameters to */ -void setSoftRealTimePriority(); +template +void copy_all_parameters(const NodeT1 & parent, const NodeT2 & child) +{ + using Parameters = std::vector; + std::vector param_names = parent->list_parameters({}, 0).names; + Parameters params = parent->get_parameters(param_names); + for (Parameters::const_iterator iter = params.begin(); iter != params.end(); ++iter) { + if (!child->has_parameter(iter->get_name())) { + child->declare_parameter(iter->get_name(), iter->get_parameter_value()); + } + } +} } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index 3cc8f87..e653e39 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -18,11 +18,9 @@ #define NAV2_UTIL__ROBOT_UTILS_HPP_ #include -#include #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -60,6 +58,18 @@ bool transformPoseInTargetFrame( tf2_ros::Buffer & tf_buffer, const std::string target_frame, const double transform_timeout = 0.1); +/** + * @brief Obtains a transform from source_frame_id at source_time -> + * to target_frame_id at target_time time + * @param source_frame_id Source frame ID to convert from + * @param source_time Source timestamp to convert from + * @param target_frame_id Target frame ID to convert to + * @param target_time Target time to interpolate to + * @param transform_tolerance Transform tolerance + * @param tf_transform Output source->target transform + * @return True if got correct transform, otherwise false + */ + /** * @brief Obtains a transform from source_frame_id -> to target_frame_id * @param source_frame_id Source frame ID to convert from @@ -104,8 +114,7 @@ bool getTransform( * @param msg Twist message to validate * @return True if valid, false if contains unactionable values */ -[[nodiscard]] bool validateTwist(const geometry_msgs::msg::Twist & msg); -[[nodiscard]] bool validateTwist(const geometry_msgs::msg::TwistStamped & msg); +bool validateTwist(const geometry_msgs::msg::Twist & msg); } // end namespace nav2_util diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 6f25965..8f18c6c 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -26,7 +26,7 @@ namespace nav2_util * @class nav2_util::ServiceClient * @brief A simple wrapper on ROS2 services for invoke() and block-style calling */ -template +template class ServiceClient { public: @@ -37,16 +37,16 @@ class ServiceClient */ explicit ServiceClient( const std::string & service_name, - const NodeT & provided_node) + const rclcpp::Node::SharedPtr & provided_node) : service_name_(service_name), node_(provided_node) { callback_group_ = node_->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - client_ = node_->template create_client( + client_ = node_->create_client( service_name, - rclcpp::SystemDefaultsQoS(), + rclcpp::ServicesQoS().get_rmw_qos_profile(), callback_group_); } @@ -147,7 +147,7 @@ class ServiceClient protected: std::string service_name_; - NodeT node_; + rclcpp::Node::SharedPtr node_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; typename rclcpp::Client::SharedPtr client_; diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index eec9c2d..4ee84d3 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -25,7 +25,6 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_util/node_thread.hpp" -#include "nav2_util/node_utils.hpp" namespace nav2_util { @@ -58,8 +57,6 @@ class SimpleActionServer * @param server_timeout Timeout to to react to stop or preemption requests * @param spin_thread Whether to spin with a dedicated thread internally * @param options Options to pass to the underlying rcl_action_server_t - * @param realtime Whether the action server's worker thread should have elevated - * prioritization (soft realtime) */ template explicit SimpleActionServer( @@ -69,15 +66,13 @@ class SimpleActionServer CompletionCallback completion_callback = nullptr, std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500), bool spin_thread = false, - const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), - const bool realtime = false) + const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) : SimpleActionServer( node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), node->get_node_waitables_interface(), - action_name, execute_callback, completion_callback, - server_timeout, spin_thread, options, realtime) + action_name, execute_callback, completion_callback, server_timeout, spin_thread, options) {} /** @@ -88,8 +83,6 @@ class SimpleActionServer * @param server_timeout Timeout to to react to stop or preemption requests * @param spin_thread Whether to spin with a dedicated thread internally * @param options Options to pass to the underlying rcl_action_server_t - * @param realtime Whether the action server's worker thread should have elevated - * prioritization (soft realtime) */ explicit SimpleActionServer( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, @@ -101,8 +94,7 @@ class SimpleActionServer CompletionCallback completion_callback = nullptr, std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500), bool spin_thread = false, - const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), - const bool realtime = false) + const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) : node_base_interface_(node_base_interface), node_clock_interface_(node_clock_interface), node_logging_interface_(node_logging_interface), @@ -114,7 +106,6 @@ class SimpleActionServer spin_thread_(spin_thread) { using namespace std::placeholders; // NOLINT - use_realtime_prioritization_ = realtime; if (spin_thread_) { callback_group_ = node_base_interface->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); @@ -179,17 +170,6 @@ class SimpleActionServer return rclcpp_action::CancelResponse::ACCEPT; } - /** - * @brief Sets thread priority level - */ - void setSoftRealTimePriority() - { - if (use_realtime_prioritization_) { - nav2_util::setSoftRealTimePriority(); - debug_msg("Soft realtime prioritization successfully set!"); - } - } - /** * @brief Handles accepted goals and adds to preempted queue to switch to * @param Goal A server goal handle to cancel @@ -222,11 +202,7 @@ class SimpleActionServer // Return quickly to avoid blocking the executor, so spin up a new thread debug_msg("Executing goal asynchronously."); - execution_future_ = std::async( - std::launch::async, [this]() { - setSoftRealTimePriority(); - work(); - }); + execution_future_ = std::async(std::launch::async, [this]() {work();}); } } @@ -244,7 +220,7 @@ class SimpleActionServer node_logging_interface_->get_logger(), "Action server failed while executing action callback: \"%s\"", ex.what()); terminate_all(); - if (completion_callback_) {completion_callback_();} + completion_callback_(); return; } @@ -254,14 +230,14 @@ class SimpleActionServer if (stop_execution_) { warn_msg("Stopping the thread per request."); terminate_all(); - if (completion_callback_) {completion_callback_();} + completion_callback_(); break; } if (is_active(current_handle_)) { warn_msg("Current goal was not completed successfully."); terminate(current_handle_); - if (completion_callback_) {completion_callback_();} + completion_callback_(); } if (is_active(pending_handle_)) { @@ -314,7 +290,7 @@ class SimpleActionServer info_msg("Waiting for async process to finish."); if (steady_clock::now() - start_time >= server_timeout_) { terminate_all(); - if (completion_callback_) {completion_callback_();} + completion_callback_(); throw std::runtime_error("Action callback is still running and missed deadline to stop"); } } @@ -533,7 +509,6 @@ class SimpleActionServer CompletionCallback completion_callback_; std::future execution_future_; bool stop_execution_{false}; - bool use_realtime_prioritization_{false}; mutable std::recursive_mutex update_mutex_; bool server_active_{false}; diff --git a/nav2_util/include/nav2_util/twist_publisher.hpp b/nav2_util/include/nav2_util/twist_publisher.hpp deleted file mode 100644 index b86fdee..0000000 --- a/nav2_util/include/nav2_util/twist_publisher.hpp +++ /dev/null @@ -1,133 +0,0 @@ -// Copyright (C) 2023 Ryan Friedman -// -// 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. - - -#ifndef NAV2_UTIL__TWIST_PUBLISHER_HPP_ -#define NAV2_UTIL__TWIST_PUBLISHER_HPP_ - - -#include -#include -#include - -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "rclcpp/parameter_service.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/qos.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" - -#include "lifecycle_node.hpp" -#include "node_utils.hpp" - -namespace nav2_util -{ - -/** - * @class nav2_util::TwistPublisher - * @brief A simple wrapper on a Twist publisher that provides either Twist or TwistStamped - * - * The default is to publish Twist to preserve backwards compatibility, but it can be overridden - * using the "enable_stamped_cmd_vel" parameter to publish TwistStamped. - * - */ - -class TwistPublisher -{ -public: - /** - * @brief A constructor - * @param nh The node - * @param topic publisher topic name - * @param qos publisher quality of service - */ - explicit TwistPublisher( - nav2_util::LifecycleNode::SharedPtr node, - const std::string & topic, - const rclcpp::QoS & qos) - : topic_(topic) - { - using nav2_util::declare_parameter_if_not_declared; - declare_parameter_if_not_declared( - node, "enable_stamped_cmd_vel", - rclcpp::ParameterValue{false}); - node->get_parameter("enable_stamped_cmd_vel", is_stamped_); - if (is_stamped_) { - twist_stamped_pub_ = node->create_publisher( - topic_, - qos); - } else { - twist_pub_ = node->create_publisher( - topic_, - qos); - } - } - - void on_activate() - { - if (is_stamped_) { - twist_stamped_pub_->on_activate(); - } else { - twist_pub_->on_activate(); - } - } - - void on_deactivate() - { - if (is_stamped_) { - twist_stamped_pub_->on_deactivate(); - } else { - twist_pub_->on_deactivate(); - } - } - - [[nodiscard]] bool is_activated() const - { - if (is_stamped_) { - return twist_stamped_pub_->is_activated(); - } else { - return twist_pub_->is_activated(); - } - } - - void publish(std::unique_ptr velocity) - { - if (is_stamped_) { - twist_stamped_pub_->publish(std::move(velocity)); - } else { - auto twist_msg = std::make_unique(velocity->twist); - twist_pub_->publish(std::move(twist_msg)); - } - } - - [[nodiscard]] size_t get_subscription_count() const - { - if (is_stamped_) { - return twist_stamped_pub_->get_subscription_count(); - } else { - return twist_pub_->get_subscription_count(); - } - } - -protected: - std::string topic_; - bool is_stamped_; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr twist_pub_; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr - twist_stamped_pub_; -}; - -} // namespace nav2_util - -#endif // NAV2_UTIL__TWIST_PUBLISHER_HPP_ diff --git a/nav2_util/include/nav2_util/twist_subscriber.hpp b/nav2_util/include/nav2_util/twist_subscriber.hpp deleted file mode 100644 index 01f9ca2..0000000 --- a/nav2_util/include/nav2_util/twist_subscriber.hpp +++ /dev/null @@ -1,153 +0,0 @@ -// Copyright (C) 2023 Ryan Friedman -// -// 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. - - -#ifndef NAV2_UTIL__TWIST_SUBSCRIBER_HPP_ -#define NAV2_UTIL__TWIST_SUBSCRIBER_HPP_ - - -#include -#include -#include - -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "rclcpp/parameter_service.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/qos.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" - -#include "lifecycle_node.hpp" -#include "node_utils.hpp" - -namespace nav2_util -{ - -/** - * @class nav2_util::TwistSubscriber - * @brief A simple wrapper on a Twist subscriber that receives either - * geometry_msgs::msg::TwistStamped or geometry_msgs::msg::Twist - * - * @note Usage: - * The default behavior is to subscribe to Twist, which preserves backwards compatibility - * with ROS distributions up to Iron. - * The behavior can be overridden using the "enable_stamped_cmd_vel" parameter. - * By setting that to "True", the TwistSubscriber class would subscribe to TwistStamped. - * - * @note Why use Twist Stamped over Twist? - * Twist has been used widely in many ROS applications, typically for body-frame velocity control, - * and between ROS nodes on the same computer. Many ROS interfaces are moving to using TwistStamped - * because it is more robust for stale data protection. This protection is especially important - * when sending velocity control over lossy communication links. - * An example application where this matters is a drone with a Linux computer running a ROS - * controller that sends Twist commands to an embedded autopilot. If the autopilot failed to - * recognize a highly latent connection, it could result in instability or a crash because of the - * decreased phase margin for control. - * TwistStamped also has a frame ID, allowing explicit control for multiple frames, rather than - * relying on an assumption of body-frame control or having to create a different topic. - * Adding a header is low-cost for most ROS applications; the header can be set to an empty string - * if bandwidth is of concern. - * - * @note Implementation Design Notes: - * Compared to the naive approach of setting up one subscriber for each message type, - * only one subscriber is created at a time; the other is nullptr. - * This reduces RAM usage and ROS discovery traffic. - * This approach allows NAV2 libraries to be flexible in which Twist message they support, - * while maintaining a stable API in a ROS distribution. - * - */ - -class TwistSubscriber -{ -public: - /** - * @brief A constructor that supports either Twist and TwistStamped - * @param node The node to add the Twist subscriber to - * @param topic The subscriber topic name - * @param qos The subscriber quality of service - * @param TwistCallback The subscriber callback for Twist messages - * @param TwistStampedCallback The subscriber callback for TwistStamped messages - */ - template - explicit TwistSubscriber( - nav2_util::LifecycleNode::SharedPtr node, - const std::string & topic, - const rclcpp::QoS & qos, - TwistCallbackT && TwistCallback, - TwistStampedCallbackT && TwistStampedCallback - ) - { - nav2_util::declare_parameter_if_not_declared( - node, "enable_stamped_cmd_vel", - rclcpp::ParameterValue(false)); - node->get_parameter("enable_stamped_cmd_vel", is_stamped_); - if (is_stamped_) { - twist_stamped_sub_ = node->create_subscription( - topic, - qos, - std::forward(TwistStampedCallback)); - } else { - twist_sub_ = node->create_subscription( - topic, - qos, - std::forward(TwistCallback)); - } - } - - /** - * @brief A constructor that only supports TwistStamped - * @param node The node to add the TwistStamped subscriber to - * @param topic The subscriber topic name - * @param qos The subscriber quality of service - * @param TwistStampedCallback The subscriber callback for TwistStamped messages - * @throw std::invalid_argument When configured with an invalid ROS parameter - */ - template - explicit TwistSubscriber( - nav2_util::LifecycleNode::SharedPtr node, - const std::string & topic, - const rclcpp::QoS & qos, - TwistStampedCallbackT && TwistStampedCallback - ) - { - nav2_util::declare_parameter_if_not_declared( - node, "enable_stamped_cmd_vel", - rclcpp::ParameterValue(false)); - node->get_parameter("enable_stamped_cmd_vel", is_stamped_); - if (is_stamped_) { - twist_stamped_sub_ = node->create_subscription( - topic, - qos, - std::forward(TwistStampedCallback)); - } else { - throw std::invalid_argument( - "enable_stamped_cmd_vel must be true when using this constructor!"); - } - } - -protected: - //! @brief The user-configured value for ROS parameter enable_stamped_cmd_vel - bool is_stamped_{false}; - //! @brief The subscription when using Twist - rclcpp::Subscription::SharedPtr twist_sub_ {nullptr}; - //! @brief The subscription when using TwistStamped - rclcpp::Subscription::SharedPtr twist_stamped_sub_ {nullptr}; -}; - - -} // namespace nav2_util - -#endif // NAV2_UTIL__TWIST_SUBSCRIBER_HPP_ diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 9de53f5..86b4fab 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.2.0 + 1.1.13 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index 104966e..a639a0e 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -8,7 +8,6 @@ add_library(${library_name} SHARED robot_utils.cpp node_thread.cpp odometry_utils.cpp - array_parser.cpp ) ament_target_dependencies(${library_name} diff --git a/nav2_util/src/array_parser.cpp b/nav2_util/src/array_parser.cpp deleted file mode 100644 index cfe2f69..0000000 --- a/nav2_util/src/array_parser.cpp +++ /dev/null @@ -1,105 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * author: Dave Hershberger - */ - -#include // for EOF -#include -#include -#include - -namespace nav2_util -{ - -/** @brief Parse a vector of vector of floats from a string. - * @param input - * @param error_return - * Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] */ -std::vector> parseVVF(const std::string & input, std::string & error_return) -{ - std::vector> result; - - std::stringstream input_ss(input); - int depth = 0; - std::vector current_vector; - while (!!input_ss && !input_ss.eof()) { - switch (input_ss.peek()) { - case EOF: - break; - case '[': - depth++; - if (depth > 2) { - error_return = "Array depth greater than 2"; - return result; - } - input_ss.get(); - current_vector.clear(); - break; - case ']': - depth--; - if (depth < 0) { - error_return = "More close ] than open ["; - return result; - } - input_ss.get(); - if (depth == 1) { - result.push_back(current_vector); - } - break; - case ',': - case ' ': - case '\t': - input_ss.get(); - break; - default: // All other characters should be part of the numbers. - if (depth != 2) { - std::stringstream err_ss; - err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'."; - error_return = err_ss.str(); - return result; - } - float value; - input_ss >> value; - if (!!input_ss) { - current_vector.push_back(value); - } - break; - } - } - - if (depth != 0) { - error_return = "Unterminated vector string."; - } else { - error_return = ""; - } - - return result; -} - -} // end namespace nav2_util diff --git a/nav2_util/src/node_thread.cpp b/nav2_util/src/node_thread.cpp index adcbe50..c19fbd9 100644 --- a/nav2_util/src/node_thread.cpp +++ b/nav2_util/src/node_thread.cpp @@ -13,13 +13,13 @@ // limitations under the License. #include + #include "nav2_util/node_thread.hpp" namespace nav2_util { -NodeThread::NodeThread( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) +NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) : node_(node_base) { executor_ = std::make_shared(); @@ -35,10 +35,7 @@ NodeThread::NodeThread( NodeThread::NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor) : executor_(executor) { - thread_ = std::make_unique( - [&]() { - executor_->spin(); - }); + thread_ = std::make_unique([&]() {executor_->spin();}); } NodeThread::~NodeThread() diff --git a/nav2_util/src/node_utils.cpp b/nav2_util/src/node_utils.cpp index 993eaf5..e5415b8 100644 --- a/nav2_util/src/node_utils.cpp +++ b/nav2_util/src/node_utils.cpp @@ -1,5 +1,4 @@ // Copyright (c) 2019 Intel Corporation -// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -90,17 +89,4 @@ rclcpp::Node::SharedPtr generate_internal_node(const std::string & prefix) return rclcpp::Node::make_shared("_", options); } -void setSoftRealTimePriority() -{ - sched_param sch; - sch.sched_priority = 49; - if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) { - std::string errmsg( - "Cannot set as real-time thread. Users must set: hard rtprio 99 and " - " soft rtprio 99 in /etc/security/limits.conf to enable " - "realtime prioritization! Error: "); - throw std::runtime_error(errmsg + std::strerror(errno)); - } -} - } // namespace nav2_util diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 6933b71..20791e8 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -18,7 +18,6 @@ #include #include -#include "tf2/convert.h" #include "nav2_util/robot_utils.hpp" #include "rclcpp/logger.hpp" @@ -172,9 +171,4 @@ bool validateTwist(const geometry_msgs::msg::Twist & msg) return true; } -bool validateTwist(const geometry_msgs::msg::TwistStamped & msg) -{ - return validateTwist(msg.twist); -} - } // end namespace nav2_util diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index fb4b6c1..9f1ae99 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -41,14 +41,3 @@ target_link_libraries(test_odometry_utils ${library_name}) ament_add_gtest(test_robot_utils test_robot_utils.cpp) ament_target_dependencies(test_robot_utils geometry_msgs) target_link_libraries(test_robot_utils ${library_name}) - -ament_add_gtest(test_array_parser test_array_parser.cpp) -target_link_libraries(test_array_parser ${library_name}) - -ament_add_gtest(test_twist_publisher test_twist_publisher.cpp) -ament_target_dependencies(test_twist_publisher rclcpp_lifecycle) -target_link_libraries(test_twist_publisher ${library_name}) - -ament_add_gtest(test_twist_subscriber test_twist_subscriber.cpp) -ament_target_dependencies(test_twist_subscriber rclcpp_lifecycle) -target_link_libraries(test_twist_subscriber ${library_name}) diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 3af965d..26c64b1 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -548,5 +548,6 @@ int main(int argc, char ** argv) ::testing::InitGoogleTest(&argc, argv); auto result = RUN_ALL_TESTS(); rclcpp::shutdown(); + rclcpp::Rate(1).sleep(); return result; } diff --git a/nav2_util/test/test_array_parser.cpp b/nav2_util/test/test_array_parser.cpp deleted file mode 100644 index 76a691b..0000000 --- a/nav2_util/test/test_array_parser.cpp +++ /dev/null @@ -1,79 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -#include "gtest/gtest.h" -#include "nav2_util/array_parser.hpp" - -TEST(array_parser, basic_operation) -{ - std::string error; - std::vector> vvf; - vvf = nav2_util::parseVVF("[[1, 2.2], [.3, -4e4]]", error); - EXPECT_EQ(2u, vvf.size() ); - EXPECT_EQ(2u, vvf[0].size() ); - EXPECT_EQ(2u, vvf[1].size() ); - EXPECT_EQ(1.0f, vvf[0][0]); - EXPECT_EQ(2.2f, vvf[0][1]); - EXPECT_EQ(0.3f, vvf[1][0]); - EXPECT_EQ(-40000.0f, vvf[1][1]); - EXPECT_EQ("", error); -} - -TEST(array_parser, missing_open) -{ - std::string error; - std::vector> vvf; - vvf = nav2_util::parseVVF("[1, 2.2], [.3, -4e4]]", error); - EXPECT_NE(error, ""); -} - -TEST(array_parser, missing_close) -{ - std::string error; - std::vector> vvf; - vvf = nav2_util::parseVVF("[[1, 2.2], [.3, -4e4]", error); - EXPECT_NE(error, ""); -} - -TEST(array_parser, wrong_depth) -{ - std::string error; - std::vector> vvf; - vvf = nav2_util::parseVVF("[1, 2.2], [.3, -4e4]", error); - EXPECT_NE(error, ""); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/nav2_util/test/test_node_utils.cpp b/nav2_util/test/test_node_utils.cpp index bf94527..9d6d6ba 100644 --- a/nav2_util/test/test_node_utils.cpp +++ b/nav2_util/test/test_node_utils.cpp @@ -92,5 +92,37 @@ TEST(GetPluginTypeParam, GetPluginTypeParam) auto node = std::make_shared("test_node"); node->declare_parameter("Foo.plugin", "bar"); ASSERT_EQ(get_plugin_type_param(node, "Foo"), "bar"); - EXPECT_THROW(get_plugin_type_param(node, "Waldo"), std::runtime_error); + ASSERT_EXIT(get_plugin_type_param(node, "Waldo"), ::testing::ExitedWithCode(255), ".*"); +} + +TEST(TestParamCopying, TestParamCopying) +{ + auto node1 = std::make_shared("test_node1"); + auto node2 = std::make_shared("test_node2"); + + // Tests for (1) multiple types, (2) recursion, (3) overriding values + node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar1")))); + node1->declare_parameter("Foo2", rclcpp::ParameterValue(0.123)); + node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar")))); + node1->declare_parameter("Foo.bar", rclcpp::ParameterValue(std::string(("steve")))); + node2->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("barz2")))); + + // Show Node2 is empty of Node1's parameters, but contains its own + EXPECT_FALSE(node2->has_parameter("Foo1")); + EXPECT_FALSE(node2->has_parameter("Foo2")); + EXPECT_FALSE(node2->has_parameter("Foo.bar")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); + + nav2_util::copy_all_parameters(node1, node2); + + // Test new parameters exist, of expected value, and original param is not overridden + EXPECT_TRUE(node2->has_parameter("Foo1")); + EXPECT_EQ(node2->get_parameter("Foo1").as_string(), std::string("bar1")); + EXPECT_TRUE(node2->has_parameter("Foo2")); + EXPECT_EQ(node2->get_parameter("Foo2").as_double(), 0.123); + EXPECT_TRUE(node2->has_parameter("Foo.bar")); + EXPECT_EQ(node2->get_parameter("Foo.bar").as_string(), std::string("steve")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); } diff --git a/nav2_util/test/test_twist_publisher.cpp b/nav2_util/test/test_twist_publisher.cpp deleted file mode 100644 index 94361e9..0000000 --- a/nav2_util/test/test_twist_publisher.cpp +++ /dev/null @@ -1,101 +0,0 @@ -// Copyright (C) 2023 Ryan Friedman -// -// 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. - -#include -#include - -#include "nav2_util/twist_publisher.hpp" -#include "nav2_util/lifecycle_utils.hpp" -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" - -using nav2_util::startup_lifecycle_nodes; -using nav2_util::reset_lifecycle_nodes; - -TEST(TwistPublisher, Unstamped) -{ - rclcpp::init(0, nullptr); - auto pub_node = std::make_shared("pub_node", ""); - pub_node->configure(); - auto vel_publisher = std::make_unique(pub_node, "cmd_vel", 1); - ASSERT_EQ(vel_publisher->get_subscription_count(), 0); - EXPECT_FALSE(vel_publisher->is_activated()); - pub_node->activate(); - EXPECT_TRUE(vel_publisher->is_activated()); - vel_publisher->on_activate(); - auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node->get_node_base_interface());}); - - auto sub_node = std::make_shared("sub_node", ""); - sub_node->configure(); - sub_node->activate(); - - auto pub_msg = std::make_unique(); - pub_msg->twist.linear.x = 42.0; - auto pub_msg_copy = pub_msg->twist; - - geometry_msgs::msg::Twist sub_msg {}; - auto my_sub = sub_node->create_subscription( - "cmd_vel", 10, - [&](const geometry_msgs::msg::Twist msg) {sub_msg = msg;}); - - vel_publisher->publish(std::move(pub_msg)); - rclcpp::spin_some(sub_node->get_node_base_interface()); - - EXPECT_EQ(pub_msg_copy.linear.x, sub_msg.linear.x); - EXPECT_EQ(vel_publisher->get_subscription_count(), 1); - pub_node->deactivate(); - sub_node->deactivate(); - rclcpp::shutdown(); - // // Have to join thread after rclcpp is shut down otherwise test hangs. - pub_thread.join(); -} - -TEST(TwistPublisher, Stamped) -{ - rclcpp::init(0, nullptr); - auto pub_node = std::make_shared("pub_node", ""); - pub_node->declare_parameter("enable_stamped_cmd_vel", true); - pub_node->configure(); - auto vel_publisher = std::make_unique(pub_node, "cmd_vel", 1); - ASSERT_EQ(vel_publisher->get_subscription_count(), 0); - EXPECT_FALSE(vel_publisher->is_activated()); - pub_node->activate(); - EXPECT_TRUE(vel_publisher->is_activated()); - auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node->get_node_base_interface());}); - - auto sub_node = std::make_shared("sub_node", ""); - sub_node->configure(); - sub_node->activate(); - - auto pub_msg = std::make_unique(); - pub_msg->twist.linear.x = 42.0; - pub_msg->header.frame_id = "foo"; - auto pub_msg_copy = *pub_msg; - - geometry_msgs::msg::TwistStamped sub_msg {}; - auto my_sub = sub_node->create_subscription( - "cmd_vel", 10, - [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;}); - - vel_publisher->publish(std::move(pub_msg)); - rclcpp::spin_some(sub_node->get_node_base_interface()); - ASSERT_EQ(vel_publisher->get_subscription_count(), 1); - EXPECT_EQ(pub_msg_copy, sub_msg); - pub_node->deactivate(); - sub_node->deactivate(); - rclcpp::shutdown(); - // Have to join thread after rclcpp is shut down otherwise test hangs. - pub_thread.join(); -} diff --git a/nav2_util/test/test_twist_subscriber.cpp b/nav2_util/test/test_twist_subscriber.cpp deleted file mode 100644 index 5672887..0000000 --- a/nav2_util/test/test_twist_subscriber.cpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright (C) 2023 Ryan Friedman -// -// 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. - -#include -#include - -#include "nav2_util/twist_subscriber.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" - -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" - - -TEST(TwistSubscriber, Unstamped) -{ - rclcpp::init(0, nullptr); - auto sub_node = std::make_shared("sub_node", ""); - sub_node->configure(); - sub_node->activate(); - - geometry_msgs::msg::TwistStamped sub_msg {}; - auto vel_subscriber = std::make_unique( - sub_node, "cmd_vel", 1, - [&](const geometry_msgs::msg::Twist msg) {sub_msg.twist = msg;}, - [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;} - ); - - auto pub_node = std::make_shared("pub_node", ""); - pub_node->configure(); - - geometry_msgs::msg::TwistStamped pub_msg {}; - pub_msg.twist.linear.x = 42.0; - - auto vel_pub = - pub_node->create_publisher("cmd_vel", 1); - - pub_node->activate(); - vel_pub->on_activate(); - - vel_pub->publish(pub_msg.twist); - rclcpp::spin_some(sub_node->get_node_base_interface()); - ASSERT_EQ(vel_pub->get_subscription_count(), 1); - EXPECT_EQ(pub_msg, sub_msg); - - pub_node->deactivate(); - sub_node->deactivate(); - rclcpp::shutdown(); -} - -TEST(TwistSubscriber, Stamped) -{ - rclcpp::init(0, nullptr); - auto sub_node = std::make_shared("sub_node", ""); - sub_node->declare_parameter("enable_stamped_cmd_vel", true); - sub_node->configure(); - sub_node->activate(); - - geometry_msgs::msg::TwistStamped sub_msg {}; - auto vel_subscriber = std::make_unique( - sub_node, "cmd_vel", 1, - [&](const geometry_msgs::msg::Twist msg) {sub_msg.twist = msg;}, - [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;} - ); - - auto pub_node = std::make_shared("pub_node", ""); - pub_node->configure(); - - geometry_msgs::msg::TwistStamped pub_msg {}; - pub_msg.twist.linear.x = 42.0; - - auto vel_pub = - pub_node->create_publisher("cmd_vel", 1); - - pub_node->activate(); - vel_pub->on_activate(); - - vel_pub->publish(pub_msg); - rclcpp::spin_some(sub_node->get_node_base_interface()); - ASSERT_EQ(vel_pub->get_subscription_count(), 1); - EXPECT_EQ(pub_msg, sub_msg); - - pub_node->deactivate(); - sub_node->deactivate(); - rclcpp::shutdown(); -} diff --git a/navigation2/package.xml b/navigation2/package.xml index 7d8a186..8e47a24 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.2.0 + 1.1.13 ROS2 Navigation Stack @@ -22,7 +22,6 @@ nav2_core nav2_costmap_2d nav2_dwb_controller - nav2_graceful_controller nav2_lifecycle_manager nav2_map_server nav2_msgs diff --git a/pointcloud_to_laserscan/CHANGELOG.rst b/pointcloud_to_laserscan/CHANGELOG.rst index 64ed7c2..54374c3 100644 --- a/pointcloud_to_laserscan/CHANGELOG.rst +++ b/pointcloud_to_laserscan/CHANGELOG.rst @@ -2,16 +2,6 @@ Changelog for package pointcloud_to_laserscan ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.0.2 (2024-02-16) ------------------- -* feat: use exported targets (`#69 `_) -* Stop using the deprecated tf2_sensor_msgs.h header. (`#73 `_) -* Fix default parameters in README (`#65 `_) -* fix: update static transfrom publisher arguments (`#70 `_) -* Cleanup CMakeLists.txt with ament_cmake_auto (`#57 `_) -* Add BSD-3 clause LICENSE -* Contributors: Carlos Andrés Álvarez Restrepo, Chris Lalancette, Daisuke Nishimatsu, Michel Hidalgo - 2.0.1 (2021-10-19) ------------------ * Replace deprecated launch api (`#56 `_) diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt index d13488c..b96516b 100644 --- a/pointcloud_to_laserscan/CMakeLists.txt +++ b/pointcloud_to_laserscan/CMakeLists.txt @@ -1,102 +1,33 @@ cmake_minimum_required(VERSION 3.5) project(pointcloud_to_laserscan) -find_package(ament_cmake REQUIRED) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -find_package(laser_geometry REQUIRED) -find_package(message_filters REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_sensor_msgs REQUIRED) - -add_library(laserscan_to_pointcloud SHARED +ament_auto_add_library(laserscan_to_pointcloud SHARED src/laserscan_to_pointcloud_node.cpp) -target_compile_definitions(laserscan_to_pointcloud - PRIVATE "POINTCLOUD_TO_LASERSCAN_BUILDING_DLL") -target_include_directories(laserscan_to_pointcloud PUBLIC - "$" - "$" -) -target_link_libraries(laserscan_to_pointcloud - laser_geometry::laser_geometry - message_filters::message_filters - rclcpp::rclcpp - rclcpp_components::component - tf2::tf2 - tf2_ros::tf2_ros - tf2_sensor_msgs::tf2_sensor_msgs -) + rclcpp_components_register_node(laserscan_to_pointcloud PLUGIN "pointcloud_to_laserscan::LaserScanToPointCloudNode" EXECUTABLE laserscan_to_pointcloud_node) -add_library(pointcloud_to_laserscan SHARED +ament_auto_add_library(pointcloud_to_laserscan SHARED src/pointcloud_to_laserscan_node.cpp) -target_compile_definitions(pointcloud_to_laserscan - PRIVATE "POINTCLOUD_TO_LASERSCAN_BUILDING_DLL") -target_include_directories(pointcloud_to_laserscan PUBLIC - "$" - "$" -) -target_link_libraries(pointcloud_to_laserscan - laser_geometry::laser_geometry - message_filters::message_filters - rclcpp::rclcpp - rclcpp_components::component - tf2::tf2 - tf2_ros::tf2_ros - tf2_sensor_msgs::tf2_sensor_msgs -) + rclcpp_components_register_node(pointcloud_to_laserscan PLUGIN "pointcloud_to_laserscan::PointCloudToLaserScanNode" EXECUTABLE pointcloud_to_laserscan_node) -add_executable(dummy_pointcloud_publisher +ament_auto_add_executable(dummy_pointcloud_publisher src/dummy_pointcloud_publisher.cpp ) -target_link_libraries(dummy_pointcloud_publisher - rclcpp::rclcpp - "${sensor_msgs_TARGETS}" -) - -install(TARGETS - laserscan_to_pointcloud - pointcloud_to_laserscan - EXPORT export_${PROJECT_NAME} - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib) - -install(TARGETS - dummy_pointcloud_publisher - DESTINATION lib/${PROJECT_NAME} -) - -install(DIRECTORY include - DESTINATION include/${PROJECT_NAME} -) - -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() endif() -ament_export_dependencies( - laser_geometry - message_filters - rclcpp - rclcpp_components - sensor_msgs - tf2 - tf2_ros - tf2_sensor_msgs +ament_auto_package( + INSTALL_TO_SHARE + launch ) - -ament_package() diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index 0da9645..3d9ec2f 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -1,7 +1,7 @@ pointcloud_to_laserscan - 2.0.2 + 2.0.1 Converts a 3D Point Cloud into a 2D laser scan. This is useful for making devices like the Kinect appear like a laser scanner for 2D-based algorithms (e.g. laser-based SLAM). Paul Bovbel @@ -16,7 +16,7 @@ https://github.com/ros-perception/perception_pcl/issues https://github.com/ros-perception/perception_pcl - ament_cmake + ament_cmake_auto laser_geometry message_filters