diff --git a/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml index ce987eb118..16d172aff8 100644 --- a/.github/workflows/update_ci_image.yaml +++ b/.github/workflows/update_ci_image.yaml @@ -99,7 +99,7 @@ jobs: - name: Build and push ${{ github.ref_name }} if: steps.config.outputs.trigger == 'true' id: docker_build - uses: docker/build-push-action@v5 + uses: docker/build-push-action@v6 with: context: . pull: true diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 030a85f38c..514bba4c55 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -152,7 +152,8 @@ class AmclNode : public nav2_util::LifecycleNode std::recursive_mutex mutex_; rclcpp::Subscription::ConstSharedPtr map_sub_; #if NEW_UNIFORM_SAMPLING - static std::vector> free_space_indices; + struct Point2D { int32_t x; int32_t y; }; + static std::vector free_space_indices; #endif // Transforms @@ -391,6 +392,7 @@ class AmclNode : public nav2_util::LifecycleNode double z_rand_; std::string scan_topic_{"scan"}; std::string map_topic_{"map"}; + bool freespace_downsampling_ = false; }; } // namespace nav2_amcl diff --git a/nav2_amcl/include/nav2_amcl/map/map.hpp b/nav2_amcl/include/nav2_amcl/map/map.hpp index c9557efa0b..3cf6f0d597 100644 --- a/nav2_amcl/include/nav2_amcl/map/map.hpp +++ b/nav2_amcl/include/nav2_amcl/map/map.hpp @@ -41,20 +41,21 @@ struct _rtk_fig_t; // Limits #define MAP_WIFI_MAX_LEVELS 8 - +// make sure that the sizeof(map_cell_t) == 5 +#pragma pack(push, 1) // Description for a single map cell. typedef struct { // Occupancy state (-1 = free, 0 = unknown, +1 = occ) - int occ_state; + int8_t occ_state; // Distance to the nearest occupied cell - double occ_dist; + float occ_dist; // Wifi levels // int wifi_levels[MAP_WIFI_MAX_LEVELS]; } map_cell_t; - +#pragma pack(pop) // Description for a map typedef struct diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index cdd4c5adbd..1a10c7aa41 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -227,6 +227,11 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) add_parameter( "first_map_only", rclcpp::ParameterValue(false), "Set this to true, when you want to load a new map published from the map_server"); + + add_parameter( + "freespace_downsampling", rclcpp::ParameterValue( + false), + "Downsample the free space used by the Pose Generator. Use it with large maps to save memory"); } AmclNode::~AmclNode() @@ -404,7 +409,7 @@ AmclNode::checkElapsedTime(std::chrono::seconds check_interval, rclcpp::Time las } #if NEW_UNIFORM_SAMPLING -std::vector> AmclNode::free_space_indices; +std::vector AmclNode::free_space_indices; #endif bool @@ -446,10 +451,10 @@ AmclNode::uniformPoseGenerator(void * arg) #if NEW_UNIFORM_SAMPLING unsigned int rand_index = drand48() * free_space_indices.size(); - std::pair free_point = free_space_indices[rand_index]; + AmclNode::Point2D free_point = free_space_indices[rand_index]; pf_vector_t p; - p.v[0] = MAP_WXGX(map, free_point.first); - p.v[1] = MAP_WYGY(map, free_point.second); + p.v[0] = MAP_WXGX(map, free_point.x); + p.v[1] = MAP_WYGY(map, free_point.y); p.v[2] = drand48() * 2 * M_PI - M_PI; #else double min_x, max_x, min_y, max_y; @@ -1106,6 +1111,7 @@ AmclNode::initParameters() get_parameter("always_reset_initial_pose", always_reset_initial_pose_); get_parameter("scan_topic", scan_topic_); get_parameter("map_topic", map_topic_); + get_parameter("freespace_downsampling", freespace_downsampling_); save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate); transform_tolerance_ = tf2::durationFromSec(tmp_tol); @@ -1435,12 +1441,14 @@ AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg) void AmclNode::createFreeSpaceVector() { + int delta = freespace_downsampling_ ? 2 : 1; // Index of free space free_space_indices.resize(0); - for (int i = 0; i < map_->size_x; i++) { - for (int j = 0; j < map_->size_y; j++) { + for (int i = 0; i < map_->size_x; i += delta) { + for (int j = 0; j < map_->size_y; j += delta) { if (map_->cells[MAP_INDEX(map_, i, j)].occ_state == -1) { - free_space_indices.push_back(std::make_pair(i, j)); + AmclNode::Point2D point = {i, j}; + free_space_indices.push_back(point); } } } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 9ee903fd78..a0d86649c0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -47,7 +47,8 @@ class BehaviorTreeEngine * @param plugin_libraries vector of BT plugin library names to load */ explicit BehaviorTreeEngine( - const std::vector & plugin_libraries); + const std::vector & plugin_libraries, + rclcpp::Node::SharedPtr node); virtual ~BehaviorTreeEngine() {} /** @@ -93,6 +94,9 @@ class BehaviorTreeEngine protected: // The factory that will be used to dynamically construct the behavior tree BT::BehaviorTreeFactory factory_; + + // Clock + rclcpp::Clock::SharedPtr clock_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 75dfff5597..d2b120f44c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -173,7 +173,7 @@ bool BtActionServer::on_configure() error_code_names_ = node->get_parameter("error_code_names").as_string_array(); // Create the class that registers our custom nodes and executes the BT - bt_ = std::make_unique(plugin_lib_names_); + bt_ = std::make_unique(plugin_lib_names_, client_node_); // Create the blackboard that will be shared by all of the nodes in the tree blackboard_ = BT::Blackboard::create(); diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 8ed1cd4e71..70a192d719 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -26,13 +26,16 @@ namespace nav2_behavior_tree { BehaviorTreeEngine::BehaviorTreeEngine( - const std::vector & plugin_libraries) + const std::vector & plugin_libraries, rclcpp::Node::SharedPtr node) { BT::SharedLibrary loader; for (const auto & p : plugin_libraries) { factory_.registerFromPlugin(loader.getOSName(p)); } + // clock for throttled debug log + clock_ = node->get_clock(); + // FIXME: the next two line are needed for back-compatibility with BT.CPP 3.8.x // Note that the can be removed, once we migrate from BT.CPP 4.5.x to 4.6+ BT::ReactiveSequence::EnableException(false); @@ -62,8 +65,9 @@ BehaviorTreeEngine::run( onLoop(); if (!loopRate.sleep()) { - RCLCPP_WARN( + RCLCPP_DEBUG_THROTTLE( rclcpp::get_logger("BehaviorTreeEngine"), + *clock_, 1000, "Behavior Tree tick rate %0.2f was exceeded!", 1.0 / (loopRate.period().count() * 1.0e-9)); } diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index 1c008d6478..d495587955 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -59,13 +59,7 @@ TEST(PointPortTest, test_wrong_syntax) BT::BehaviorTreeFactory factory; factory.registerNodeType>("PointPort"); - auto tree = factory.createTreeFromText(xml_txt); - - geometry_msgs::msg::Point value; - tree.rootNode()->getInput("test", value); - EXPECT_EQ(value.x, 0.0); - EXPECT_EQ(value.y, 0.0); - EXPECT_EQ(value.z, 0.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); xml_txt = R"( @@ -75,11 +69,7 @@ TEST(PointPortTest, test_wrong_syntax) )"; - tree = factory.createTreeFromText(xml_txt); - tree.rootNode()->getInput("test", value); - EXPECT_EQ(value.x, 0.0); - EXPECT_EQ(value.y, 0.0); - EXPECT_EQ(value.z, 0.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); } TEST(PointPortTest, test_correct_syntax) @@ -115,14 +105,8 @@ TEST(QuaternionPortTest, test_wrong_syntax) BT::BehaviorTreeFactory factory; factory.registerNodeType>("QuaternionPort"); - auto tree = factory.createTreeFromText(xml_txt); - geometry_msgs::msg::Quaternion value; - tree.rootNode()->getInput("test", value); - EXPECT_EQ(value.x, 0.0); - EXPECT_EQ(value.y, 0.0); - EXPECT_EQ(value.z, 0.0); - EXPECT_EQ(value.w, 1.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); xml_txt = R"( @@ -132,12 +116,7 @@ TEST(QuaternionPortTest, test_wrong_syntax) )"; - tree = factory.createTreeFromText(xml_txt); - tree.rootNode()->getInput("test", value); - EXPECT_EQ(value.x, 0.0); - EXPECT_EQ(value.y, 0.0); - EXPECT_EQ(value.z, 0.0); - EXPECT_EQ(value.w, 1.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); } TEST(QuaternionPortTest, test_correct_syntax) @@ -174,19 +153,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax) BT::BehaviorTreeFactory factory; factory.registerNodeType>("PoseStampedPort"); - auto tree = factory.createTreeFromText(xml_txt); - - geometry_msgs::msg::PoseStamped value; - tree.rootNode()->getInput("test", value); - EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); - EXPECT_EQ(value.header.frame_id, ""); - EXPECT_EQ(value.pose.position.x, 0.0); - EXPECT_EQ(value.pose.position.y, 0.0); - EXPECT_EQ(value.pose.position.z, 0.0); - EXPECT_EQ(value.pose.orientation.x, 0.0); - EXPECT_EQ(value.pose.orientation.y, 0.0); - EXPECT_EQ(value.pose.orientation.z, 0.0); - EXPECT_EQ(value.pose.orientation.w, 1.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); xml_txt = R"( @@ -196,17 +163,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax) )"; - tree = factory.createTreeFromText(xml_txt); - tree.rootNode()->getInput("test", value); - EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); - EXPECT_EQ(value.header.frame_id, ""); - EXPECT_EQ(value.pose.position.x, 0.0); - EXPECT_EQ(value.pose.position.y, 0.0); - EXPECT_EQ(value.pose.position.z, 0.0); - EXPECT_EQ(value.pose.orientation.x, 0.0); - EXPECT_EQ(value.pose.orientation.y, 0.0); - EXPECT_EQ(value.pose.orientation.z, 0.0); - EXPECT_EQ(value.pose.orientation.w, 1.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); } TEST(PoseStampedPortTest, test_correct_syntax) diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py deleted file mode 100644 index 54f9d60f80..0000000000 --- a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ /dev/null @@ -1,233 +0,0 @@ -# Copyright (c) 2023 LG Electronics. -# Copyright (c) 2024 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. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import os -from pathlib import Path -import tempfile - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - GroupAction, - IncludeLaunchDescription, - LogInfo, - OpaqueFunction, - RegisterEventHandler, -) -from launch.conditions import IfCondition -from launch.event_handlers import OnShutdown -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, TextSubstitution -from nav2_common.launch import ParseMultiRobotPose - - -def generate_launch_description(): - """ - Bring up the multi-robots with given launch arguments. - - 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}' - """ - # Get the launch directory - bringup_dir = get_package_share_directory('nav2_bringup') - launch_dir = os.path.join(bringup_dir, 'launch') - sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') - - # Simulation settings - world = LaunchConfiguration('world') - - # On this example all robots are launched with the same settings - map_yaml_file = LaunchConfiguration('map') - params_file = LaunchConfiguration('params_file') - autostart = LaunchConfiguration('autostart') - rviz_config_file = LaunchConfiguration('rviz_config') - use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') - use_rviz = LaunchConfiguration('use_rviz') - log_settings = LaunchConfiguration('log_settings', default='true') - - # Declare the launch arguments - declare_world_cmd = DeclareLaunchArgument( - 'world', - default_value=os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro'), - description='Full path to world file to load', - ) - - declare_map_yaml_cmd = DeclareLaunchArgument( - 'map', - default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'), - 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', - ) - - declare_autostart_cmd = DeclareLaunchArgument( - '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.', - ) - - declare_use_robot_state_pub_cmd = DeclareLaunchArgument( - 'use_robot_state_pub', - default_value='True', - description='Whether to start the robot state publisher', - ) - - declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', default_value='True', description='Whether to start RVIZ' - ) - - # Start Gazebo with plugin providing the robot spawning service - world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') - world_sdf_xacro = ExecuteProcess( - cmd=['xacro', '-o', world_sdf, ['headless:=', 'False'], world]) - start_gazebo_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', - 'gz_sim.launch.py')), - launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items()) - - remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( - on_shutdown=[ - OpaqueFunction(function=lambda _: os.remove(world_sdf)) - ])) - - robots_list = ParseMultiRobotPose('robots').value() - - # Define commands for launching the navigation instances - 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(), - ), - ] - ) - - bringup_cmd_group.append(group) - - set_env_vars_resources = AppendEnvironmentVariable( - 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')) - set_env_vars_resources2 = AppendEnvironmentVariable( - 'GZ_SIM_RESOURCE_PATH', - str(Path(os.path.join(sim_dir)).parent.resolve())) - - # Create the launch description and populate - ld = LaunchDescription() - ld.add_action(set_env_vars_resources) - ld.add_action(set_env_vars_resources2) - - # Declare the launch options - ld.add_action(declare_world_cmd) - ld.add_action(declare_map_yaml_cmd) - ld.add_action(declare_params_file_cmd) - ld.add_action(declare_use_rviz_cmd) - ld.add_action(declare_autostart_cmd) - ld.add_action(declare_rviz_config_file_cmd) - ld.add_action(declare_use_robot_state_pub_cmd) - - # Add the actions to start gazebo, robots and simulations - ld.add_action(world_sdf_xacro) - ld.add_action(start_gazebo_cmd) - ld.add_action(remove_temp_sdf_file) - - 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]) - ) - - for cmd in bringup_cmd_group: - ld.add_action(cmd) - - return ld diff --git a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py deleted file mode 100644 index 7eaaf1a97d..0000000000 --- a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py +++ /dev/null @@ -1,256 +0,0 @@ -# Copyright (c) 2018 Intel Corporation -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -Example for spawning multiple robots in Gazebo. - -This is an example on how to create a launch file for spawning multiple robots into Gazebo -and launch multiple instances of the navigation stack, each controlling one robot. -The robots co-exist on a shared environment and are controlled by independent nav stacks. -""" - -import os -from pathlib import Path -import tempfile - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - GroupAction, - IncludeLaunchDescription, - LogInfo, - OpaqueFunction, - RegisterEventHandler, -) -from launch.conditions import IfCondition -from launch.event_handlers import OnShutdown -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, TextSubstitution - - -def generate_launch_description(): - # Get the launch directory - bringup_dir = get_package_share_directory('nav2_bringup') - launch_dir = os.path.join(bringup_dir, 'launch') - sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') - - # 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, - }, - ] - - # Simulation settings - world = LaunchConfiguration('world') - - # On this example all robots are launched with the same settings - map_yaml_file = LaunchConfiguration('map') - - autostart = LaunchConfiguration('autostart') - rviz_config_file = LaunchConfiguration('rviz_config') - use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') - use_rviz = LaunchConfiguration('use_rviz') - log_settings = LaunchConfiguration('log_settings', default='true') - - # Declare the launch arguments - declare_world_cmd = DeclareLaunchArgument( - 'world', - default_value=os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro'), - description='Full path to world file to load', - ) - - declare_map_yaml_cmd = DeclareLaunchArgument( - 'map', - default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'), - 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', - ) - - 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', - ) - - declare_autostart_cmd = DeclareLaunchArgument( - '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.', - ) - - declare_use_robot_state_pub_cmd = DeclareLaunchArgument( - 'use_robot_state_pub', - default_value='True', - description='Whether to start the robot state publisher', - ) - - declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', default_value='True', description='Whether to start RVIZ' - ) - - # Start Gazebo with plugin providing the robot spawning service - world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') - world_sdf_xacro = ExecuteProcess( - cmd=['xacro', '-o', world_sdf, ['headless:=', 'False'], world]) - start_gazebo_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', - 'gz_sim.launch.py')), - launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items()) - - remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( - on_shutdown=[ - OpaqueFunction(function=lambda _: os.remove(world_sdf)) - ])) - - # 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], - ), - ] - ) - - nav_instances_cmds.append(group) - - set_env_vars_resources = AppendEnvironmentVariable( - 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')) - set_env_vars_resources2 = AppendEnvironmentVariable( - 'GZ_SIM_RESOURCE_PATH', - str(Path(os.path.join(sim_dir)).parent.resolve())) - - # Create the launch description and populate - ld = LaunchDescription() - ld.add_action(set_env_vars_resources) - ld.add_action(set_env_vars_resources2) - - # Declare the launch options - ld.add_action(declare_world_cmd) - ld.add_action(declare_map_yaml_cmd) - ld.add_action(declare_robot1_params_file_cmd) - ld.add_action(declare_robot2_params_file_cmd) - ld.add_action(declare_use_rviz_cmd) - ld.add_action(declare_autostart_cmd) - ld.add_action(declare_rviz_config_file_cmd) - ld.add_action(declare_use_robot_state_pub_cmd) - - # Add the actions to start gazebo, robots and simulations - ld.add_action(world_sdf_xacro) - ld.add_action(start_gazebo_cmd) - ld.add_action(remove_temp_sdf_file) - - for simulation_instance_cmd in nav_instances_cmds: - ld.add_action(simulation_instance_cmd) - - return ld diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index b43b70318c..307eb2f37a 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -407,11 +407,12 @@ docking_server: filter_coef: 0.1 # Dock instances - docks: ['home_dock'] # Input your docks here - home_dock: - type: 'simple_charging_dock' - frame: map - pose: [0.0, 0.0, 0.0] + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] controller: k_phi: 3.0 diff --git a/nav2_bringup/rviz/nav2_namespaced_view.rviz b/nav2_bringup/rviz/nav2_namespaced_view.rviz deleted file mode 100644 index 9f70030775..0000000000 --- a/nav2_bringup/rviz/nav2_namespaced_view.rviz +++ /dev/null @@ -1,378 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 195 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /RobotModel1/Status1 - - /TF1/Frames1 - - /TF1/Tree1 - - /Global Planner1/Global Costmap1 - Splitter Ratio: 0.5833333134651184 - Tree Height: 464 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: nav2_rviz_plugins/Navigation 2 - Name: Navigation 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Name: RobotModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - {} - Update Interval: 0 - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/LaserScan - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: LaserScan - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /scan - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Bumper Hit - Position Transformer: "" - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /mobile_base/sensors/bumper_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: true - Enabled: true - Name: Map - Topic: - Depth: 1 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map - Use Timestamp: false - Value: true - - Alpha: 1 - Class: nav2_rviz_plugins/ParticleCloud - Color: 0; 180; 0 - Enabled: true - Max Arrow Length: 0.3 - Min Arrow Length: 0.02 - Name: Amcl Particle Swarm - Shape: Arrow (Flat) - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /particle_cloud - Value: true - - Class: rviz_common/Group - Displays: - - Alpha: 0.30000001192092896 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: false - Enabled: true - Name: Global Costmap - Topic: - Depth: 1 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_costmap/costmap - Use Timestamp: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 255; 0; 0 - Enabled: true - Head Diameter: 0.019999999552965164 - Head Length: 0.019999999552965164 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: Arrows - Radius: 0.029999999329447746 - Shaft Diameter: 0.004999999888241291 - Shaft Length: 0.019999999552965164 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /plan - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: Polygon - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_costmap/published_footprint - Value: false - Enabled: true - Name: Global Planner - - Class: rviz_common/Group - Displays: - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: false - Enabled: true - Name: Local Costmap - Topic: - Depth: 1 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /local_costmap/costmap - Use Timestamp: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 0; 12; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Local Plan - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /local_plan - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Trajectories - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /marker - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: true - Name: Polygon - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /local_costmap/published_footprint - Value: true - Enabled: true - Name: Controller - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MarkerArray - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /waypoints - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: nav2_rviz_plugins/GoalTool - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Angle: -0.0008007669821381569 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 54 - Target Frame: - Value: TopDownOrtho (rviz_default_plugins) - X: -5.41 - Y: 0 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 914 - Hide Left Dock: false - Hide Right Dock: true - Navigation 2: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000338fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000003070000006e0000006200ffffff000000010000010f00000338fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000338000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000033800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1545 - X: 186 - Y: 117 diff --git a/nav2_costmap_2d/cfg/Costmap2D.cfg b/nav2_costmap_2d/cfg/Costmap2D.cfg deleted file mode 100755 index 07c4a1628b..0000000000 --- a/nav2_costmap_2d/cfg/Costmap2D.cfg +++ /dev/null @@ -1,23 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, str_t - -gen = ParameterGenerator() - -gen.add("transform_tolerance", double_t, 0, "Specifies the delay in transform (tf) data that is tolerable in seconds.", 0.3, 0, 10) -gen.add("update_frequency", double_t, 0, "The frequency in Hz for the map to be updated.", 5, 0, 100) -gen.add("publish_frequency", double_t, 0, "The frequency in Hz for the map to be publish display information.", 0, 0, 100) - -#map params -gen.add("width", int_t, 0, "The width of the map in meters.", 10, 0) -gen.add("height", int_t, 0, "The height of the map in meters.", 10, 0) -gen.add("resolution", double_t, 0, "The resolution of the map in meters/cell.", 0.05, 0, 50) -gen.add("origin_x", double_t, 0, "The x origin of the map in the global frame in meters.", 0) -gen.add("origin_y", double_t, 0, "The y origin of the map in the global frame in meters.", 0) - -# robot footprint shape -gen.add("footprint", str_t, 0, "The footprint of the robot specified in the robot_base_frame coordinate frame as a list in the format: [ [x1, y1], [x2, y2], ...., [xn, yn] ].", "[]") -gen.add("robot_radius", double_t, 0, 'The radius of the robot in meters, this parameter should only be set for circular robots, all others should use the footprint parameter described above.', 0.46, 0, 10) -gen.add("footprint_padding", double_t, 0, "How much to pad (increase the size of) the footprint, in meters.", 0.01) - -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "Costmap2D")) diff --git a/nav2_costmap_2d/cfg/GenericPlugin.cfg b/nav2_costmap_2d/cfg/GenericPlugin.cfg deleted file mode 100755 index 555e2b5415..0000000000 --- a/nav2_costmap_2d/cfg/GenericPlugin.cfg +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t - -gen = ParameterGenerator() -gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "GenericPlugin")) diff --git a/nav2_costmap_2d/cfg/InflationPlugin.cfg b/nav2_costmap_2d/cfg/InflationPlugin.cfg deleted file mode 100755 index 5c11eaf791..0000000000 --- a/nav2_costmap_2d/cfg/InflationPlugin.cfg +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t - -gen = ParameterGenerator() - -gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -gen.add("cost_scaling_factor", double_t, 0, "A scaling factor to apply to cost values during inflation.", 10, 0, 100) -gen.add("inflation_radius", double_t, 0, "The radius in meters to which the map inflates obstacle cost values.", 0.55, 0, 50) -gen.add("inflate_unknown", bool_t, 0, "Whether to inflate unknown cells.", False) - -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "InflationPlugin")) diff --git a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg b/nav2_costmap_2d/cfg/ObstaclePlugin.cfg deleted file mode 100755 index a94e98fbbe..0000000000 --- a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t - -gen = ParameterGenerator() - -gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) -gen.add("max_obstacle_height", double_t, 0, "The maximum height of any obstacle to be inserted into the costmap in meters.", 2, 0, 50) - -combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), - gen.const("Maximum", int_t, 1, "Take the maximum of the values"), - gen.const("Nothing", int_t, 99, "Do nothing")], - "Method for combining layers enum") -gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, edit_method=combo_enum) - - -# gen.add("obstacle_max_range", double_t, 0, "The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 2.5, 0, 50) -# gen.add("obstacle_min_range", double_t, 0, "The default minimum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 0.0, 0, 50) -# gen.add("raytrace_max_range", double_t, 0, "The default maximum range in meters at which to raytrace out obstacles from the map using sensor data.", 3, 0, 50) -# gen.add("raytrace_min_range", double_t, 0, "The default minimum range in meters from which to raytrace out obstacles from the map using sensor data.", 0.0, 0, 50) -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "ObstaclePlugin")) diff --git a/nav2_costmap_2d/cfg/VoxelPlugin.cfg b/nav2_costmap_2d/cfg/VoxelPlugin.cfg deleted file mode 100755 index 977fcb99d4..0000000000 --- a/nav2_costmap_2d/cfg/VoxelPlugin.cfg +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t - -gen = ParameterGenerator() - -gen.add("enabled", bool_t, 0, "Whether to use this plugin or not", True) -gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) -gen.add("max_obstacle_height", double_t, 0, "Max Obstacle Height", 2.0, 0, 50) -gen.add("origin_z", double_t, 0, "The z origin of the map in meters.", 0, 0) -gen.add("z_resolution", double_t, 0, "The z resolution of the map in meters/cell.", 0.2, 0, 50) -gen.add("z_voxels", int_t, 0, "The number of voxels to in each vertical column.", 10, 0, 16) -gen.add("unknown_threshold", int_t, 0, 'The number of unknown cells allowed in a column considered to be known', 15, 0, 16) -gen.add("mark_threshold", int_t, 0, 'The maximum number of marked cells allowed in a column considered to be free', 0, 0, 16) - -combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), - gen.const("Maximum", int_t, 1, "Take the maximum of the values"), - gen.const("Nothing", int_t, 99, "Do nothing")], - "Method for combining layers enum") -gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, 0, 2, edit_method=combo_enum) - -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "VoxelPlugin")) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index b83010c62d..0ae010e94b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -60,18 +60,16 @@ class CellData public: /** * @brief Constructor for a CellData objects - * @param i The index of the cell in the cost map * @param x The x coordinate of the cell in the cost map * @param y The y coordinate of the cell in the cost map * @param sx The x coordinate of the closest obstacle cell in the costmap * @param sy The y coordinate of the closest obstacle cell in the costmap * @return */ - CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) - : index_(static_cast(i)), x_(x), y_(y), src_x_(sx), src_y_(sy) + CellData(unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) + : x_(x), y_(y), src_x_(sx), src_y_(sy) { } - unsigned int index_; unsigned int x_, y_; unsigned int src_x_, src_y_; }; diff --git a/nav2_costmap_2d/launch/example.launch b/nav2_costmap_2d/launch/example.launch deleted file mode 100644 index ac089abfba..0000000000 --- a/nav2_costmap_2d/launch/example.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/nav2_costmap_2d/launch/example_params.yaml b/nav2_costmap_2d/launch/example_params.yaml deleted file mode 100644 index 72bf695fad..0000000000 --- a/nav2_costmap_2d/launch/example_params.yaml +++ /dev/null @@ -1,43 +0,0 @@ -global_frame: map -robot_base_frame: base_link -update_frequency: 5.0 -publish_frequency: 1.0 - -#set if you want the voxel map published -publish_voxel_map: true - -#set to true if you want to initialize the costmap from a static map -static_map: false - -#begin - COMMENT these lines if you set static_map to true -rolling_window: true -width: 6.0 -height: 6.0 -resolution: 0.025 -#end - COMMENT these lines if you set static_map to true - -#START VOXEL STUFF -map_type: voxel -origin_z: 0.0 -z_resolution: 0.2 -z_voxels: 10 -unknown_threshold: 10 -mark_threshold: 0 -#END VOXEL STUFF - -transform_tolerance: 0.3 -obstacle_max_range: 2.5 -obstacle_min_range: 0.0 -max_obstacle_height: 2.0 -raytrace_max_range: 3.0 -raytrace_min_range: 0.0 - -footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] -#robot_radius: 0.46 -footprint_padding: 0.01 -inflation_radius: 0.55 -cost_scaling_factor: 10.0 -lethal_cost_threshold: 100 -observation_sources: base_scan -base_scan: {data_type: LaserScan, expected_update_rate: 0.4, - observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 067877e548..01b788a7cc 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -170,6 +170,16 @@ InflationLayer::onFootprintChanged() computeCaches(); need_reinflation_ = true; + if (inflation_radius_ < inscribed_radius_) { + RCLCPP_ERROR( + logger_, + "The configured inflation radius (%.3f) is smaller than " + "the computed inscribed radius (%.3f) of your footprint, " + "it is highly recommended to set inflation radius to be at " + "least as big as the inscribed radius to avoid collisions", + inflation_radius_, inscribed_radius_); + } + RCLCPP_DEBUG( logger_, "InflationLayer::onFootprintChanged(): num footprint points: %zu," " inscribed_radius_ = %.3f, inflation_radius_ = %.3f", @@ -230,12 +240,13 @@ InflationLayer::updateCosts( // Start with lethal obstacles: by definition distance is 0.0 auto & obs_bin = inflation_cells_[0]; + obs_bin.reserve(200); for (int j = min_j; j < max_j; j++) { for (int i = min_i; i < max_i; i++) { int index = static_cast(master_grid.getIndex(i, j)); unsigned char cost = master_array[index]; if (cost == LETHAL_OBSTACLE || (inflate_around_unknown_ && cost == NO_INFORMATION)) { - obs_bin.emplace_back(index, i, j, i, j); + obs_bin.emplace_back(i, j, i, j); } } } @@ -243,12 +254,18 @@ InflationLayer::updateCosts( // Process cells by increasing distance; new cells are appended to the // corresponding distance bin, so they // can overtake previously inserted but farther away cells - for (const auto & dist_bin : inflation_cells_) { + for (auto & dist_bin : inflation_cells_) { + dist_bin.reserve(200); for (std::size_t i = 0; i < dist_bin.size(); ++i) { // Do not use iterator or for-range based loops to // iterate though dist_bin, since it's size might // change when a new cell is enqueued, invalidating all iterators - unsigned int index = dist_bin[i].index_; + const CellData & cell = dist_bin[i]; + unsigned int mx = cell.x_; + unsigned int my = cell.y_; + unsigned int sx = cell.src_x_; + unsigned int sy = cell.src_y_; + unsigned int index = master_grid.getIndex(mx, my); // ignore if already visited if (seen_[index]) { @@ -257,11 +274,6 @@ InflationLayer::updateCosts( seen_[index] = true; - unsigned int mx = dist_bin[i].x_; - unsigned int my = dist_bin[i].y_; - unsigned int sx = dist_bin[i].src_x_; - unsigned int sy = dist_bin[i].src_y_; - // assign the cost associated with the distance from an obstacle to the cell unsigned char cost = costLookup(mx, my, sx, sy); unsigned char old_cost = master_array[index]; @@ -296,11 +308,9 @@ InflationLayer::updateCosts( enqueue(index + size_x, mx, my + 1, sx, sy); } } - } - - for (auto & dist : inflation_cells_) { - dist.clear(); - dist.reserve(200); + // This level of inflation_cells_ is not needed anymore. We can free the memory + // Note that dist_bin.clear() is not enough, because it won't free the memory + dist_bin = std::vector(); } current_ = true; @@ -334,8 +344,8 @@ InflationLayer::enqueue( const unsigned int r = cell_inflation_radius_ + 2; // push the cell data onto the inflation list and mark - inflation_cells_[distance_matrix_[mx - src_x + r][my - src_y + r]].emplace_back( - index, mx, my, src_x, src_y); + const auto dist = distance_matrix_[mx - src_x + r][my - src_y + r]; + inflation_cells_[dist].emplace_back(mx, my, src_x, src_y); } } @@ -372,9 +382,6 @@ InflationLayer::computeCaches() int max_dist = generateIntegerDistances(); inflation_cells_.clear(); inflation_cells_.resize(max_dist + 1); - for (auto & dist : inflation_cells_) { - dist.reserve(200); - } } int diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index ad867128b1..b88c373688 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -125,15 +125,16 @@ void TestNode::validatePointInflation( bool * seen = new bool[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()]; memset(seen, false, costmap->getSizeInCellsX() * costmap->getSizeInCellsY() * sizeof(bool)); std::map> m; - CellData initial(costmap->getIndex(mx, my), mx, my, mx, my); + CellData initial(mx, my, mx, my); m[0].push_back(initial); for (std::map>::iterator bin = m.begin(); bin != m.end(); ++bin) { for (unsigned int i = 0; i < bin->second.size(); ++i) { const CellData cell = bin->second[i]; - if (!seen[cell.index_]) { - seen[cell.index_] = true; + const auto index = costmap->getIndex(cell.x_, cell.y_); + if (!seen[index]) { + seen[index] = true; unsigned int dx = (cell.x_ > cell.src_x_) ? cell.x_ - cell.src_x_ : cell.src_x_ - cell.x_; unsigned int dy = (cell.y_ > cell.src_y_) ? cell.y_ - cell.src_y_ : cell.src_y_ - cell.y_; double dist = std::hypot(dx, dy); @@ -152,23 +153,19 @@ void TestNode::validatePointInflation( } if (cell.x_ > 0) { - CellData data(costmap->getIndex(cell.x_ - 1, cell.y_), - cell.x_ - 1, cell.y_, cell.src_x_, cell.src_y_); + CellData data(cell.x_ - 1, cell.y_, cell.src_x_, cell.src_y_); m[dist].push_back(data); } if (cell.y_ > 0) { - CellData data(costmap->getIndex(cell.x_, cell.y_ - 1), - cell.x_, cell.y_ - 1, cell.src_x_, cell.src_y_); + CellData data(cell.x_, cell.y_ - 1, cell.src_x_, cell.src_y_); m[dist].push_back(data); } if (cell.x_ < costmap->getSizeInCellsX() - 1) { - CellData data(costmap->getIndex(cell.x_ + 1, cell.y_), - cell.x_ + 1, cell.y_, cell.src_x_, cell.src_y_); + CellData data(cell.x_ + 1, cell.y_, cell.src_x_, cell.src_y_); m[dist].push_back(data); } if (cell.y_ < costmap->getSizeInCellsY() - 1) { - CellData data(costmap->getIndex(cell.x_, cell.y_ + 1), - cell.x_, cell.y_ + 1, cell.src_x_, cell.src_y_); + CellData data(cell.x_, cell.y_ + 1, cell.src_x_, cell.src_y_); m[dist].push_back(data); } } diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt index 38e9b79516..e50590d3e4 100644 --- a/nav2_docking/opennav_docking/CMakeLists.txt +++ b/nav2_docking/opennav_docking/CMakeLists.txt @@ -56,8 +56,15 @@ set(dependencies opennav_docking_core ) -add_library(${library_name} SHARED +add_library(controller SHARED src/controller.cpp +) + +ament_target_dependencies(controller + ${dependencies} +) + +add_library(${library_name} SHARED src/docking_server.cpp src/dock_database.cpp src/navigator.cpp @@ -68,7 +75,9 @@ ament_target_dependencies(${library_name} ) target_link_libraries(${library_name} - yaml-cpp::yaml-cpp) + yaml-cpp::yaml-cpp + controller +) add_library(pose_filter SHARED src/pose_filter.cpp @@ -100,7 +109,7 @@ target_link_libraries(simple_charging_dock pose_filter) pluginlib_export_plugin_description_file(opennav_docking_core plugins.xml) -install(TARGETS ${library_name} pose_filter simple_charging_dock +install(TARGETS ${library_name} controller pose_filter simple_charging_dock ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -127,6 +136,6 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(${library_name} pose_filter) +ament_export_libraries(${library_name} controller pose_filter) ament_export_dependencies(${dependencies} yaml-cpp) ament_package() diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp index 54472e720f..afae5965d2 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp @@ -16,6 +16,7 @@ #define OPENNAV_DOCKING__CONTROLLER_HPP_ #include +#include #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -47,8 +48,22 @@ class Controller const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool backward = false); + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::mutex dynamic_params_lock_; + protected: std::unique_ptr control_law_; + + double k_phi_, k_delta_, beta_, lambda_; + double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_; }; } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index 3060aaf8c8..8613f2d27f 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -40,25 +40,69 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) nav2_util::declare_parameter_if_not_declared( node, "controller.slowdown_radius", rclcpp::ParameterValue(0.25)); - double k_phi, k_delta, beta, lambda; - double slowdown_radius, v_linear_min, v_linear_max, v_angular_max; - node->get_parameter("controller.k_phi", k_phi); - node->get_parameter("controller.k_delta", k_delta); - node->get_parameter("controller.beta", beta); - node->get_parameter("controller.lambda", lambda); - node->get_parameter("controller.v_linear_min", v_linear_min); - node->get_parameter("controller.v_linear_max", v_linear_max); - node->get_parameter("controller.v_angular_max", v_angular_max); - node->get_parameter("controller.slowdown_radius", slowdown_radius); + node->get_parameter("controller.k_phi", k_phi_); + node->get_parameter("controller.k_delta", k_delta_); + node->get_parameter("controller.beta", beta_); + node->get_parameter("controller.lambda", lambda_); + node->get_parameter("controller.v_linear_min", v_linear_min_); + node->get_parameter("controller.v_linear_max", v_linear_max_); + node->get_parameter("controller.v_angular_max", v_angular_max_); + node->get_parameter("controller.slowdown_radius", slowdown_radius_); control_law_ = std::make_unique( - k_phi, k_delta, beta, lambda, slowdown_radius, v_linear_min, v_linear_max, v_angular_max); + k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, v_linear_min_, v_linear_max_, + v_angular_max_); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Controller::dynamicParametersCallback, this, std::placeholders::_1)); } bool Controller::computeVelocityCommand( const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool backward) { + std::lock_guard lock(dynamic_params_lock_); cmd = control_law_->calculateRegularVelocity(pose, backward); return true; } +rcl_interfaces::msg::SetParametersResult +Controller::dynamicParametersCallback(std::vector parameters) +{ + std::lock_guard lock(dynamic_params_lock_); + + rcl_interfaces::msg::SetParametersResult result; + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) { + if (name == "controller.k_phi") { + k_phi_ = parameter.as_double(); + } else if (name == "controller.k_delta") { + k_delta_ = parameter.as_double(); + } else if (name == "controller.beta") { + beta_ = parameter.as_double(); + } else if (name == "controller.lambda") { + lambda_ = parameter.as_double(); + } else if (name == "controller.v_linear_min") { + v_linear_min_ = parameter.as_double(); + } else if (name == "controller.v_linear_max") { + v_linear_max_ = parameter.as_double(); + } else if (name == "controller.v_angular_max") { + v_angular_max_ = parameter.as_double(); + } else if (name == "controller.slowdown_radius") { + slowdown_radius_ = parameter.as_double(); + } + + // Update the smooth control law with the new params + control_law_->setCurvatureConstants(k_phi_, k_delta_, beta_, lambda_); + control_law_->setSlowdownRadius(slowdown_radius_); + control_law_->setSpeedLimit(v_linear_min_, v_linear_max_, v_angular_max_); + } + } + + result.successful = true; + return result; +} + } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp index 6774e93620..83ef42cd67 100644 --- a/nav2_docking/opennav_docking/src/dock_database.cpp +++ b/nav2_docking/opennav_docking/src/dock_database.cpp @@ -34,21 +34,32 @@ bool DockDatabase::initialize( node_ = parent; auto node = node_.lock(); - if (getDockPlugins(node, tf) && getDockInstances(node)) { - RCLCPP_INFO( + if (!getDockPlugins(node, tf)) { + RCLCPP_ERROR( node->get_logger(), - "Docking Server has %u dock types and %u dock instances available.", - this->plugin_size(), this->instance_size()); - return true; + "An error occurred while getting the dock plugins!"); + return false; + } + + if (!getDockInstances(node)) { + RCLCPP_ERROR( + node->get_logger(), + "An error occurred while getting the dock instances!"); + return false; } + RCLCPP_INFO( + node->get_logger(), + "Docking Server has %u dock types and %u dock instances available.", + this->plugin_size(), this->instance_size()); + reload_db_service_ = node->create_service( "~/reload_database", std::bind( &DockDatabase::reloadDbCb, this, std::placeholders::_1, std::placeholders::_2)); - return false; + return true; } void DockDatabase::activate() @@ -71,10 +82,14 @@ void DockDatabase::reloadDbCb( const std::shared_ptr request, std::shared_ptr response) { + auto node = node_.lock(); DockMap dock_instances; - if (utils::parseDockFile(request->filepath, node_.lock(), dock_instances)) { + if (utils::parseDockFile(request->filepath, node, dock_instances)) { dock_instances_ = dock_instances; response->success = true; + RCLCPP_INFO( + node->get_logger(), + "Dock database reloaded from file %s.", request->filepath.c_str()); return; } response->success = false; @@ -194,10 +209,12 @@ bool DockDatabase::getDockInstances(const rclcpp_lifecycle::LifecycleNode::Share return utils::parseDockParams(docks_param, node, dock_instances_); } - RCLCPP_ERROR( + RCLCPP_WARN( node->get_logger(), - "Dock database filepath nor dock parameters set. Unable to perform docking actions."); - return false; + "Dock database filepath nor dock parameters set. " + "Docking actions can only be executed specifying the dock pose via the action request. " + "Or update the dock database via the reload_database service."); + return true; } unsigned int DockDatabase::plugin_size() const diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index c70e4b29aa..2d533c0871 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -235,6 +235,12 @@ void DockingServer::dockRobot() dock = generateGoalDock(goal); } + // Check if the robot is docked or charging before proceeding + if (dock->plugin->isDocked() || dock->plugin->isCharging()) { + RCLCPP_INFO(get_logger(), "Robot is already charging, no need to dock"); + return; + } + // Send robot to its staging pose publishDockingFeedback(DockRobot::Feedback::NAV_TO_STAGING_POSE); const auto initial_staging_pose = dock->getStagingPose(); @@ -569,6 +575,12 @@ void DockingServer::undockRobot() get_logger(), "Attempting to undock robot from charger of type %s.", dock->getName().c_str()); + // Check if the robot is docked before proceeding + if (!dock->isDocked()) { + RCLCPP_INFO(get_logger(), "Robot is not in the charger, no need to undock"); + return; + } + // Get "dock pose" by finding the robot pose geometry_msgs::msg::PoseStamped dock_pose = getRobotPoseInFrame(fixed_frame_); diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp index da8885d89d..c2f1e25f1c 100644 --- a/nav2_docking/opennav_docking/test/test_controller.cpp +++ b/nav2_docking/opennav_docking/test/test_controller.cpp @@ -45,4 +45,38 @@ TEST(ControllerTests, ObjectLifecycle) controller.reset(); } +TEST(ControllerTests, DynamicParameters) { + auto node = std::make_shared("test"); + auto controller = std::make_shared(node); + + auto params = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + // Set parameters + auto results = params->set_parameters_atomically( + {rclcpp::Parameter("controller.k_phi", 1.0), + rclcpp::Parameter("controller.k_delta", 2.0), + rclcpp::Parameter("controller.beta", 3.0), + rclcpp::Parameter("controller.lambda", 4.0), + rclcpp::Parameter("controller.v_linear_min", 5.0), + rclcpp::Parameter("controller.v_linear_max", 6.0), + rclcpp::Parameter("controller.v_angular_max", 7.0), + rclcpp::Parameter("controller.slowdown_radius", 8.0)}); + + // Spin + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); + + // Check parameters + EXPECT_EQ(node->get_parameter("controller.k_phi").as_double(), 1.0); + EXPECT_EQ(node->get_parameter("controller.k_delta").as_double(), 2.0); + EXPECT_EQ(node->get_parameter("controller.beta").as_double(), 3.0); + EXPECT_EQ(node->get_parameter("controller.lambda").as_double(), 4.0); + EXPECT_EQ(node->get_parameter("controller.v_linear_min").as_double(), 5.0); + EXPECT_EQ(node->get_parameter("controller.v_linear_max").as_double(), 6.0); + EXPECT_EQ(node->get_parameter("controller.v_angular_max").as_double(), 7.0); + EXPECT_EQ(node->get_parameter("controller.slowdown_radius").as_double(), 8.0); +} + } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/test/test_dock_database.cpp b/nav2_docking/opennav_docking/test/test_dock_database.cpp index 2753f34f1b..0285e30f4a 100644 --- a/nav2_docking/opennav_docking/test/test_dock_database.cpp +++ b/nav2_docking/opennav_docking/test/test_dock_database.cpp @@ -102,8 +102,11 @@ TEST(DatabaseTests, findTests) TEST(DatabaseTests, reloadDbService) { auto node = std::make_shared("test"); - std::vector plugins{"dockv1", "dockv2"}; + std::vector plugins{"dockv1"}; node->declare_parameter("dock_plugins", rclcpp::ParameterValue(plugins)); + node->declare_parameter( + "dockv1.plugin", + rclcpp::ParameterValue("opennav_docking::SimpleChargingDock")); opennav_docking::DockDatabase db; db.initialize(node, nullptr); diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp index 74da62205f..144a612f3b 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp +++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp @@ -86,9 +86,8 @@ TEST(DockingServerTests, testErrorExceptions) node->on_configure(rclcpp_lifecycle::State()); node->on_activate(rclcpp_lifecycle::State()); - node->declare_parameter( - "exception_to_throw", - rclcpp::ParameterValue("")); + node->declare_parameter("exception_to_throw", rclcpp::ParameterValue("")); + node->declare_parameter("dock_action_called", rclcpp::ParameterValue(false)); // Error codes docking std::vector error_ids{ @@ -142,6 +141,9 @@ TEST(DockingServerTests, testErrorExceptions) "DockingException", "exception"}; std::vector error_codes_undocking{999, 902, 905, 999, 999}; + // Set dock_action_called to true to simulate robot being docked in dock plugin + node->set_parameter(rclcpp::Parameter("dock_action_called", true)); + // Call action, check error code for (unsigned int i = 0; i != error_ids_undocking.size(); i++) { node->set_parameter( diff --git a/nav2_docking/opennav_docking/test/testing_dock.cpp b/nav2_docking/opennav_docking/test/testing_dock.cpp index a51924533e..4c17eb000c 100644 --- a/nav2_docking/opennav_docking/test/testing_dock.cpp +++ b/nav2_docking/opennav_docking/test/testing_dock.cpp @@ -80,7 +80,9 @@ class TestFailureDock : public opennav_docking_core::ChargingDock virtual bool isDocked() { - return false; + bool dock_action_called; + node_->get_parameter("dock_action_called", dock_action_called); + return dock_action_called; } virtual bool isCharging() diff --git a/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg b/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg deleted file mode 100644 index 51dab28aa4..0000000000 --- a/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/env python -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t - -gen = ParameterGenerator() - -# velocities -gen.add('min_vel_x', double_t, 0, 'The minimum x velocity for the robot in m/s', 0.0) -gen.add('max_vel_x', double_t, 0, 'The maximum x velocity for the robot in m/s', 0.55) -gen.add('min_vel_y', double_t, 0, 'The minimum y velocity for the robot in m/s', -0.1) -gen.add('max_vel_y', double_t, 0, 'The maximum y velocity for the robot in m/s', 0.1) -gen.add('max_vel_theta', double_t, 0, 'The absolute value of the maximum rotational velocity for the robot in rad/s. ' - 'The minimum rotational velocity is assumed to be -max_vel_theta', 1.0) - -# acceleration -gen.add('acc_lim_x', double_t, 0, 'The acceleration limit of the robot in the x direction in m/s^2', 2.5) -gen.add('acc_lim_y', double_t, 0, 'The acceleration limit of the robot in the y direction in m/s^2', 2.5) -gen.add('acc_lim_theta', double_t, 0, 'The acceleration limit of the robot in the theta direction in rad/s^2', 3.2) - -gen.add('decel_lim_x', double_t, 0, 'The deceleration limit of the robot in the x direction in m/s^2', -2.5) -gen.add('decel_lim_y', double_t, 0, 'The deceleration limit of the robot in the y direction in m/s^2', -2.5) -gen.add('decel_lim_theta', double_t, 0, 'The deceleration limit of the robot in the theta direction in rad/s^2', -3.2) - -gen.add('min_speed_xy', double_t, 0, 'The absolute value of the minimum translational/xy velocity in m/s. ' - 'If the value is negative, then the min speed will be arbitrarily close to 0.0. ' - 'Previously called min_trans_vel', 0.1) -gen.add('max_speed_xy', double_t, 0, 'The absolute value of the maximum translational/xy velocity in m/s. ' - 'If the value is negative, then the max speed is hypot(max_vel_x, max_vel_y). ' - 'Previously called max_trans_vel', 0.55) -gen.add('min_speed_theta', double_t, 0, 'The absolute value of the minimum rotational velocity in rad/s. ' - 'If the value is negative, then the min speed will be arbitrarily close to 0.0.' - ' Previously called min_rot_vel', 0.4) - -exit(gen.generate('dwb_plugins', 'dwb_plugins', 'KinematicParams')) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index d2258ceb80..b033289622 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -53,6 +53,7 @@ struct Parameters double curvature_lookahead_dist; bool use_rotate_to_heading; double max_angular_accel; + bool use_cancel_deceleration; double cancel_deceleration; double rotate_to_heading_min_angle; bool allow_reversing; diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index a47af13ef0..42876c248e 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -85,6 +85,8 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_cancel_deceleration", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, plugin_name_ + ".cancel_deceleration", rclcpp::ParameterValue(3.2)); declare_parameter_if_not_declared( @@ -153,6 +155,7 @@ ParameterHandler::ParameterHandler( node->get_parameter( plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle); node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel); + node->get_parameter(plugin_name_ + ".use_cancel_deceleration", params_.use_cancel_deceleration); node->get_parameter(plugin_name_ + ".cancel_deceleration", params_.cancel_deceleration); node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing); node->get_parameter( @@ -258,6 +261,8 @@ ParameterHandler::dynamicParametersCallback( params_.use_collision_detection = parameter.as_bool(); } else if (name == plugin_name_ + ".use_rotate_to_heading") { params_.use_rotate_to_heading = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_cancel_deceleration") { + params_.use_cancel_deceleration = parameter.as_bool(); } else if (name == plugin_name_ + ".allow_reversing") { if (params_.use_rotate_to_heading && parameter.as_bool()) { RCLCPP_WARN( diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 423d0d1a04..edcd6db558 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -290,6 +290,10 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity bool RegulatedPurePursuitController::cancel() { + // if false then publish zero velocity + if (!params_->use_cancel_deceleration) { + return true; + } cancelling_ = true; return finished_cancelling_; } diff --git a/nav2_simple_commander/nav2_simple_commander/utils.py b/nav2_simple_commander/nav2_simple_commander/utils.py new file mode 100644 index 0000000000..d363901020 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/utils.py @@ -0,0 +1,55 @@ +#! /usr/bin/env python3 +# Copyright 2024 Open Navigation LLC +# Copyright 2024 Stevedan Ogochukwu Omodolor Omodia +# +# 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. + +"""General utility functions.""" + +import os +import signal +import subprocess + + +def find_os_processes(name): + """Find all the processes that are running gz sim.""" + ps_output = subprocess.check_output(['ps', 'aux'], text=True) + ps_lines = ps_output.split('\n') + gz_sim_processes = [] + for line in ps_lines: + if name in line: + columns = line.split() + pid = columns[1] + command = ' '.join(columns[10:]) + if command.startswith(name): + gz_sim_processes.append((pid, command)) + return gz_sim_processes + + +def kill_process(pid): + """Kill a process with a given PID.""" + try: + os.kill(int(pid), signal.SIGKILL) + print(f'Successfully killed process with PID: {pid}') + except Exception as e: + print(f'Failed to kill process with PID: {pid}. Error: {e}') + + +def kill_os_processes(name): + """Kill all processes that are running with name.""" + processes = find_os_processes(name) + if processes: + for pid, _ in processes: + kill_process(pid) + else: + print(f'No processes found starting with {name}') diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 64c905d01c..c95d8c82cf 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -251,6 +251,7 @@ class AStarAlgorithm const NodePtr & node, std::vector> * expansions_log); bool _traverse_unknown; + bool _is_initialized; int _max_iterations; int _max_on_approach_iterations; int _terminal_checking_interval; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index a3cfbb4b4f..3c22ce450f 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -36,6 +36,7 @@ AStarAlgorithm::AStarAlgorithm( const MotionModel & motion_model, const SearchInfo & search_info) : _traverse_unknown(true), + _is_initialized(false), _max_iterations(0), _terminal_checking_interval(5000), _max_planning_time(0), @@ -70,7 +71,10 @@ void AStarAlgorithm::initialize( _max_on_approach_iterations = max_on_approach_iterations; _terminal_checking_interval = terminal_checking_interval; _max_planning_time = max_planning_time; - NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); + if(!_is_initialized) { + NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); + } + _is_initialized = true; _dim3_size = dim_3_size; _expander = std::make_unique>( _motion_model, _search_info, _traverse_unknown, _dim3_size); diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 45a8097b35..13576a6cdd 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(navigation2) find_package(angles REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(pluginlib REQUIRED) +find_package(nav2_minimal_tb3_sim REQUIRED) nav2_package() @@ -113,11 +114,11 @@ if(BUILD_TESTING) add_subdirectory(src/behavior_tree) add_subdirectory(src/planning) + add_subdirectory(src/localization) + add_subdirectory(src/system) + add_subdirectory(src/system_failure) + add_subdirectory(src/updown) # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634 - # add_subdirectory(src/localization) - # add_subdirectory(src/system) - # add_subdirectory(src/system_failure) - # add_subdirectory(src/updown) # add_subdirectory(src/waypoint_follower) # add_subdirectory(src/gps_navigation) # add_subdirectory(src/behaviors/spin) @@ -127,7 +128,7 @@ if(BUILD_TESTING) # add_subdirectory(src/behaviors/assisted_teleop) # add_subdirectory(src/costmap_filters) add_subdirectory(src/error_codes) - install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) + install(DIRECTORY maps models DESTINATION share/${PROJECT_NAME}) endif() diff --git a/nav2_system_tests/models/cardboard_box.sdf b/nav2_system_tests/models/cardboard_box.sdf new file mode 100644 index 0000000000..97a40603c2 --- /dev/null +++ b/nav2_system_tests/models/cardboard_box.sdf @@ -0,0 +1,9 @@ + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Cardboard box + + + + \ No newline at end of file diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 5fdbc230b7..2f3f45d191 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -30,6 +30,7 @@ launch_ros launch_testing nav2_planner + nav2_minimal_tb3_sim launch_ros launch_testing @@ -48,6 +49,7 @@ nav2_amcl std_msgs tf2_geometry_msgs + nav2_minimal_tb3_sim navigation2 lcov diff --git a/nav2_system_tests/src/localization/CMakeLists.txt b/nav2_system_tests/src/localization/CMakeLists.txt index 740d38f53e..7fa35172c3 100644 --- a/nav2_system_tests/src/localization/CMakeLists.txt +++ b/nav2_system_tests/src/localization/CMakeLists.txt @@ -11,8 +11,5 @@ ament_add_test(test_localization COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_localization_launch.py" TIMEOUT 180 ENV - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml TEST_EXECUTABLE=$ - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models ) diff --git a/nav2_system_tests/src/localization/test_localization_launch.py b/nav2_system_tests/src/localization/test_localization_launch.py index d638e02c4e..e56ca77986 100755 --- a/nav2_system_tests/src/localization/test_localization_launch.py +++ b/nav2_system_tests/src/localization/test_localization_launch.py @@ -15,43 +15,81 @@ # limitations under the License. import os +from pathlib import Path import sys +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch import LaunchService -import launch.actions -from launch.actions import ExecuteProcess +from launch.actions import AppendEnvironmentVariable, ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource import launch_ros.actions from launch_testing.legacy import LaunchTestService +from nav2_simple_commander.utils import kill_os_processes + def main(argv=sys.argv[1:]): - mapFile = os.getenv('TEST_MAP') testExecutable = os.getenv('TEST_EXECUTABLE') - world = os.getenv('TEST_WORLD') + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + ros_gz_sim_dir = get_package_share_directory('ros_gz_sim') - launch_gazebo = launch.actions.ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', '--minimal_comms', world], - output='screen', + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ) - link_footprint = launch_ros.actions.Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + set_env_vars_resources2 = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()) + ) + + start_gazebo_server = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(), + ) + + spawn_robot = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-2.0', + 'y_pose': '-0.5', + 'z_pose': '0.01', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), ) - footprint_scan = launch_ros.actions.Node( - package='tf2_ros', - executable='static_transform_publisher', + run_robot_state_publisher = launch_ros.actions.Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], ) run_map_server = launch_ros.actions.Node( package='nav2_map_server', executable='map_server', name='map_server', output='screen', - parameters=[{'yaml_filename': mapFile}], + parameters=[{'yaml_filename': map_yaml_file}], ) run_amcl = launch_ros.actions.Node( package='nav2_amcl', executable='amcl', output='screen' @@ -65,9 +103,11 @@ def main(argv=sys.argv[1:]): ) ld = LaunchDescription( [ - launch_gazebo, - link_footprint, - footprint_scan, + set_env_vars_resources, + set_env_vars_resources2, + start_gazebo_server, + spawn_robot, + run_robot_state_publisher, run_map_server, run_amcl, run_lifecycle_manager, @@ -82,7 +122,9 @@ def main(argv=sys.argv[1:]): lts.add_test_action(ld, test1_action) ls = LaunchService(argv=argv) ls.include_launch_description(ld) - return lts.run(ls) + return_code = lts.run(ls) + kill_os_processes('gz sim') + return return_code if __name__ == '__main__': diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index 39afa461c2..34ebf0e30e 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -5,9 +5,6 @@ ament_add_test(test_bt_navigator TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=True @@ -22,9 +19,6 @@ ament_add_test(test_bt_navigator_with_wrong_init_pose TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=True @@ -39,9 +33,6 @@ ament_add_test(test_bt_navigator_with_dijkstra TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=False @@ -56,9 +47,6 @@ ament_add_test(test_bt_navigator_2 TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=False @@ -68,14 +56,12 @@ ament_add_test(test_bt_navigator_2 ament_add_test(test_dynamic_obstacle GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_with_obstacle_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_obstacle.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + ADD_OBSTACLE=True BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=False @@ -85,14 +71,12 @@ ament_add_test(test_dynamic_obstacle ament_add_test(test_nav_through_poses GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_with_obstacle_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_obstacle.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + ADD_OBSTACLE=True BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml TESTER=nav_through_poses_tester_node.py ASTAR=False diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index a98d340f21..5ee29e6536 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -317,3 +317,49 @@ collision_monitor: min_height: 0.15 max_height: 2.0 enabled: True + +docking_server: + ros__parameters: + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + docks: ['home_dock'] # Input your docks here + home_dock: + type: 'simple_charging_dock' + frame: map + pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + \ No newline at end of file diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index 8fd1bc6034..0eca068039 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -59,6 +59,7 @@ def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): self.initial_pose_received = False self.initial_pose = initial_pose self.goal_pose = goal_pose + self.set_initial_pose_timeout = 15 self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') def info_msg(self, msg: str): @@ -212,11 +213,21 @@ def shutdown(self): def wait_for_initial_pose(self): self.initial_pose_received = False + # If the initial pose is not received within 100 seconds, return False + # this is because when setting a wrong initial pose, amcl_pose is not received + # and the test will hang indefinitely + start_time = time.time() + duration = 0 while not self.initial_pose_received: self.info_msg('Setting initial pose') self.setInitialPose() self.info_msg('Waiting for amcl_pose to be received') + duration = time.time() - start_time + if duration > self.set_initial_pose_timeout: + self.error_msg('Timeout waiting for initial pose to be set') + return False rclpy.spin_once(self, timeout_sec=1) + return True def test_RobotMovesToGoal(robot_tester): @@ -231,7 +242,8 @@ def run_all_tests(robot_tester): result = True if result: robot_tester.wait_for_node_active('amcl') - robot_tester.wait_for_initial_pose() + result = robot_tester.wait_for_initial_pose() + if result: robot_tester.wait_for_node_active('bt_navigator') result = robot_tester.runNavigateAction() diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 2f0546d156..a644072ebb 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -16,6 +16,7 @@ # limitations under the License. import os +from pathlib import Path import sys from ament_index_python.packages import get_package_share_directory @@ -23,6 +24,7 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import ( + AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable, @@ -33,11 +35,22 @@ from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml +from nav2_simple_commander.utils import kill_os_processes def generate_launch_description(): - map_yaml_file = os.getenv('TEST_MAP') - world = os.getenv('TEST_WORLD') + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + ros_gz_sim_dir = get_package_share_directory('ros_gz_sim') + + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') bt_navigator_xml = os.path.join( get_package_share_directory('nav2_bt_navigator'), @@ -45,8 +58,6 @@ def generate_launch_description(): os.getenv('BT_NAVIGATOR_XML'), ) - bringup_dir = get_package_share_directory('nav2_bringup') - # Use local param file launch_dir = os.path.dirname(os.path.realpath(__file__)) params_file = os.path.join(launch_dir, 'nav2_system_params.yaml') @@ -79,34 +90,46 @@ def generate_launch_description(): [ SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - # Launch gazebo server for simulation - ExecuteProcess( - cmd=[ - 'gzserver', - '-s', - 'libgazebo_ros_init.so', - '--minimal_comms', - world, - ], - output='screen', + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()) + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-2.0', + 'y_pose': '-0.5', + 'z_pose': '0.01', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), ), Node( - package='tf2_ros', - executable='static_transform_publisher', + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') ), launch_arguments={ 'namespace': '', @@ -145,7 +168,9 @@ def main(argv=sys.argv[1:]): lts.add_test_action(ld, test1_action) ls = LaunchService(argv=argv) ls.include_launch_description(ld) - return lts.run(ls) + return_code = lts.run(ls) + kill_os_processes('gz sim') + return return_code if __name__ == '__main__': diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py new file mode 100755 index 0000000000..fb4468afef --- /dev/null +++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py @@ -0,0 +1,190 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# Copyright (c) 2020 Florian Gramss +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from pathlib import Path +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ( + AppendEnvironmentVariable, + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml +from nav2_simple_commander.utils import kill_os_processes + + +def generate_launch_description(): + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + ros_gz_sim_dir = get_package_share_directory('ros_gz_sim') + nav2_system_tests_dir = get_package_share_directory('nav2_system_tests') + + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') + + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) + + # Use local param file + launch_dir = os.path.dirname(os.path.realpath(__file__)) + params_file = os.path.join(launch_dir, 'nav2_system_params.yaml') + + # Replace the default parameter values for testing special features + # without having multiple params_files inside the nav2 stack + context = LaunchContext() + param_substitutions = {} + + if os.getenv('ASTAR') == 'True': + param_substitutions.update({'use_astar': 'True'}) + + param_substitutions.update( + {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')} + ) + param_substitutions.update( + {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')} + ) + + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True, + ) + + new_yaml = configured_params.perform(context) + + cardbox_sdf = os.path.join(nav2_system_tests_dir, 'models', 'cardboard_box.sdf') + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') + ), + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()) + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-2.0', + 'y_pose': '-0.5', + 'z_pose': '0.01', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), + ), + Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'cardboard_box', + '-file', cardbox_sdf, + '-x', '-1.0', '-y', '0.6', '-z', '0.15', + '-R', '0.0', '-P', '0.0', '-Y', '0.0',] + ), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test1_action = ExecuteProcess( + cmd=[ + os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), + '-r', + '-2.0', + '-0.5', + '0.0', + '2.0', + '-e', + 'True', + ], + name='tester_node', + output='screen', + ) + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return_code = lts.run(ls) + kill_os_processes('gz sim') + return return_code + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index f42d276fcc..b0e2322692 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -16,13 +16,14 @@ # limitations under the License. import os +from pathlib import Path import sys from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription from launch import LaunchService from launch.actions import ( + AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable, @@ -33,11 +34,22 @@ from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml +from nav2_simple_commander.utils import kill_os_processes def generate_launch_description(): - map_yaml_file = os.getenv('TEST_MAP') - world = os.getenv('TEST_WORLD') + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + ros_gz_sim_dir = get_package_share_directory('ros_gz_sim') + + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') bt_navigator_xml = os.path.join( get_package_share_directory('nav2_bt_navigator'), @@ -45,8 +57,9 @@ def generate_launch_description(): os.getenv('BT_NAVIGATOR_XML'), ) - bringup_dir = get_package_share_directory('nav2_bringup') - params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') + # Use local param file + launch_dir = os.path.dirname(os.path.realpath(__file__)) + params_file = os.path.join(launch_dir, 'nav2_system_params.yaml') # Replace the default parameter values for testing special features # without having multiple params_files inside the nav2 stack @@ -76,34 +89,46 @@ def generate_launch_description(): [ SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - # Launch gazebo server for simulation - ExecuteProcess( - cmd=[ - 'gzserver', - '-s', - 'libgazebo_ros_init.so', - '--minimal_comms', - world, - ], - output='screen', + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()) + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-2.0', + 'y_pose': '-0.5', + 'z_pose': '0.01', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), ), Node( - package='tf2_ros', - executable='static_transform_publisher', + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') ), launch_arguments={ 'namespace': '', @@ -142,7 +167,9 @@ def main(argv=sys.argv[1:]): lts.add_test_action(ld, test1_action) ls = LaunchService(argv=argv) ls.include_launch_description(ld) - return lts.run(ls) + return_code = lts.run(ls) + kill_os_processes('gz sim') + return return_code if __name__ == '__main__': diff --git a/nav2_system_tests/src/system_failure/CMakeLists.txt b/nav2_system_tests/src/system_failure/CMakeLists.txt index 6bc4620535..babd49fd5b 100644 --- a/nav2_system_tests/src/system_failure/CMakeLists.txt +++ b/nav2_system_tests/src/system_failure/CMakeLists.txt @@ -5,8 +5,5 @@ ament_add_test(test_failure_navigator TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml ) diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py index 01a7d50bc9..052ec50b31 100755 --- a/nav2_system_tests/src/system_failure/test_system_failure_launch.py +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -16,6 +16,7 @@ # limitations under the License. import os +from pathlib import Path import sys from ament_index_python.packages import get_package_share_directory @@ -23,6 +24,7 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import ( + AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable, @@ -33,11 +35,22 @@ from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml +from nav2_simple_commander.utils import kill_os_processes def generate_launch_description(): - map_yaml_file = os.getenv('TEST_MAP') - world = os.getenv('TEST_WORLD') + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + ros_gz_sim_dir = get_package_share_directory('ros_gz_sim') + + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') bt_navigator_xml = os.path.join( get_package_share_directory('nav2_bt_navigator'), @@ -45,8 +58,7 @@ def generate_launch_description(): os.getenv('BT_NAVIGATOR_XML'), ) - bringup_dir = get_package_share_directory('nav2_bringup') - params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') + params_file = os.path.join(nav2_bringup_dir, 'params', 'nav2_params.yaml') # Replace the `use_astar` setting on the params file param_substitutions = { @@ -66,33 +78,46 @@ def generate_launch_description(): SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), # Launch gazebo server for simulation - ExecuteProcess( - cmd=[ - 'gzserver', - '-s', - 'libgazebo_ros_init.so', - '--minimal_comms', - world, - ], - output='screen', + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()) + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-2.0', + 'y_pose': '-0.5', + 'z_pose': '0.01', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), ), Node( - package='tf2_ros', - executable='static_transform_publisher', + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') ), launch_arguments={ 'namespace': '', @@ -129,7 +154,9 @@ def main(argv=sys.argv[1:]): lts.add_test_action(ld, test1_action) ls = LaunchService(argv=argv) ls.include_launch_description(ld) - return lts.run(ls) + return_code = lts.run(ls) + kill_os_processes('gz sim') + return return_code if __name__ == '__main__': diff --git a/nav2_system_tests/src/updown/README.md b/nav2_system_tests/src/updown/README.md index 6756d52465..ff84569c3a 100644 --- a/nav2_system_tests/src/updown/README.md +++ b/nav2_system_tests/src/updown/README.md @@ -14,7 +14,7 @@ If the test passes, you should see this comment in the output: To run the test in a loop 1000x, run the `test_updown_reliablity` script and log the output: ``` -./test_updown_reliablity |& tee /tmp/updown.log +./test_updown_reliability |& tee /tmp/updown.log ``` When the test is completed, pipe the log to the `updownresults.py` script to get a summary of the results: ``` diff --git a/nav2_system_tests/src/updown/test_updown_launch.py b/nav2_system_tests/src/updown/test_updown_launch.py index 458779411b..760f458d57 100755 --- a/nav2_system_tests/src/updown/test_updown_launch.py +++ b/nav2_system_tests/src/updown/test_updown_launch.py @@ -25,11 +25,9 @@ def generate_launch_description(): # Configuration parameters for the launch - launch_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') - map_yaml_file = os.path.join( - get_package_share_directory('nav2_system_tests'), 'maps/map_circular.yaml' - ) + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') # Specify the actions start_tf_cmd_1 = Node( @@ -61,7 +59,8 @@ def generate_launch_description(): ) nav2_bringup = launch.actions.IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), launch_arguments={ 'map': map_yaml_file, 'use_sim_time': 'True', @@ -77,7 +76,7 @@ def generate_launch_description(): 'lib/nav2_system_tests/test_updown', ) ], - cwd=[launch_dir], + cwd=[nav2_bringup_dir], output='screen', ) diff --git a/nav2_system_tests/urdf/common_properties.urdf b/nav2_system_tests/urdf/common_properties.urdf deleted file mode 100644 index 9d457ea1d6..0000000000 --- a/nav2_system_tests/urdf/common_properties.urdf +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/nav2_system_tests/urdf/turtlebot3_burger.urdf b/nav2_system_tests/urdf/turtlebot3_burger.urdf deleted file mode 100644 index 90f34dbbb8..0000000000 --- a/nav2_system_tests/urdf/turtlebot3_burger.urdf +++ /dev/null @@ -1,155 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/nav2_system_tests/urdf/turtlebot3_waffle.urdf b/nav2_system_tests/urdf/turtlebot3_waffle.urdf deleted file mode 100644 index 9a5bca1141..0000000000 --- a/nav2_system_tests/urdf/turtlebot3_waffle.urdf +++ /dev/null @@ -1,279 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo.world deleted file mode 100644 index 9a9e1bdbe8..0000000000 --- a/nav2_system_tests/worlds/turtlebot3_ros2_demo.world +++ /dev/null @@ -1,497 +0,0 @@ - - - - - - model://ground_plane - - - - model://sun - - - - false - - - - - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - orbit - perspective - - - - - 1000.0 - 0.001 - 1 - - - quick - 150 - 0 - 1.400000 - 1 - - - 0.00001 - 0.2 - 2000.000000 - 0.01000 - - - - - - 1 - - model://turtlebot3_world - - - - - -2.0 -0.5 0.01 0.0 0.0 0.0 - - - - - - - -0.064 0 0.048 0 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 1.0 - - - - -0.064 0 0.048 0 0 0 - - - 0.265 0.265 0.089 - - - - - - -0.064 0 0 0 0 0 - - - model://turtlebot3_waffle/meshes/waffle_base.dae - 0.001 0.001 0.001 - - - - - - - - true - 200 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - - false - - - ~/out:=imu - - - - - - - - -0.052 0 0.111 0 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.125 - - - - -0.064 0 0.121 0 0 0 - - - 0.0508 - 0.055 - - - - - - -0.032 0 0.171 0 0 0 - - - model://turtlebot3_waffle/meshes/lds.dae - 0.001 0.001 0.001 - - - - - - true - false - -0.064 0 0.121 0 0 0 - 5 - - - - 360 - 1.000000 - 0.000000 - 6.280000 - - - - 0.120000 - 3.5 - 0.015000 - - - gaussian - 0.0 - 0.01 - - - - - - ~/out:=scan - - sensor_msgs/LaserScan - - - - - - - - 0.0 0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - 0.0 0.144 0.023 0 0 0 - - - model://turtlebot3_waffle/meshes/left_tire.dae - 0.001 0.001 0.001 - - - - - - - - - 0.0 -0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 -0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - 0.0 -0.144 0.023 0 0 0 - - - model://turtlebot3_waffle/meshes/right_tire.dae - 0.001 0.001 0.001 - - - - - - - -0.177 -0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - - -0.177 0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - - base_footprint - base_link - 0.0 0.0 0.010 0 0 0 - - - - base_link - wheel_left_link - 0.0 0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - wheel_right_link - 0.0 -0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - caster_back_right_link - - - - base_link - caster_back_left_link - - - - base_link - imu_link - -0.032 0 0.068 0 0 0 - - 0 0 1 - - - - - base_link - base_scan - -0.064 0 0.121 0 0 0 - - 0 0 1 - - - - - - - - /tf:=tf - - - - wheel_left_joint - wheel_right_joint - - - 0.287 - 0.066 - - - 20 - 1.0 - - - true - true - false - - odom - base_footprint - - - - - - - ~/out:=joint_states - - 250 - wheel_left_joint - wheel_right_joint - - - - - - - diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world deleted file mode 100644 index 94b72499a2..0000000000 --- a/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world +++ /dev/null @@ -1,555 +0,0 @@ - - - - - - - EARTH_WGS84 - ENU - 55.944831 - -3.186998 - 0.0 - 180.0 - - - - model://ground_plane - - - - model://sun - - - - false - - - - - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - orbit - perspective - - - - - 1000.0 - 0.001 - 1 - - - quick - 150 - 0 - 1.400000 - 1 - - - 0.00001 - 0.2 - 2000.000000 - 0.01000 - - - - - - -2.0 -0.5 0.01 0.0 0.0 0.0 - - - - - - - -0.064 0 0.048 0 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 1.0 - - - - -0.064 0 0.048 0 0 0 - - - 0.265 0.265 0.089 - - - - - - -0.064 0 0 0 0 0 - - - model://turtlebot3_waffle/meshes/waffle_base.dae - 0.001 0.001 0.001 - - - - - - - - true - 200 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - - false - - - ~/out:=/imu/data - - - - - - - - - -0.052 0 0.111 0 0 0 - - 0.001 - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.125 - - - true - 1 - 0 0 0 0 0 0 - - - - - 0.0 - 0.1 - - - - - 0.0 - 0.0 - - - - - - - ~/out:=/gps/fix - - - - - - - - -0.052 0 0.111 0 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.125 - - - - -0.064 0 0.121 0 0 0 - - - 0.0508 - 0.055 - - - - - - -0.032 0 0.171 0 0 0 - - - model://turtlebot3_waffle/meshes/lds.dae - 0.001 0.001 0.001 - - - - - - true - false - -0.064 0 0.121 0 0 0 - 5 - - - - 360 - 1.000000 - 0.000000 - 6.280000 - - - - 0.120000 - 3.5 - 0.015000 - - - gaussian - 0.0 - 0.01 - - - - - - ~/out:=scan - - sensor_msgs/LaserScan - - - - - - - - 0.0 0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - 0.0 0.144 0.023 0 0 0 - - - model://turtlebot3_waffle/meshes/left_tire.dae - 0.001 0.001 0.001 - - - - - - - - - 0.0 -0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 -0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - 0.0 -0.144 0.023 0 0 0 - - - model://turtlebot3_waffle/meshes/right_tire.dae - 0.001 0.001 0.001 - - - - - - - -0.177 -0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - - -0.177 0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - - base_footprint - base_link - 0.0 0.0 0.010 0 0 0 - - - - base_link - wheel_left_link - 0.0 0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - wheel_right_link - 0.0 -0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - caster_back_right_link - - - - base_link - caster_back_left_link - - - - base_link - imu_link - -0.032 0 0.068 0 0 0 - - 0 0 1 - - - - - base_link - gps_link - -0.05 0 0.05 0 0 0 - - 0 0 1 - - - - - base_link - base_scan - -0.064 0 0.121 0 0 0 - - 0 0 1 - - - - - - - - /tf:=tf - - - - wheel_left_joint - wheel_right_joint - - - 0.287 - 0.066 - - - 20 - 1.0 - - - true - false - false - - odom - base_link - - - - - - - ~/out:=joint_states - - 250 - wheel_left_joint - wheel_right_joint - - - - - - - diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world deleted file mode 100644 index b8c0d0d128..0000000000 --- a/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world +++ /dev/null @@ -1,558 +0,0 @@ - - - - - - model://ground_plane - - - - model://sun - - - - false - - - - - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - orbit - perspective - - - - - 1000.0 - 0.001 - 1 - - - quick - 150 - 0 - 1.400000 - 1 - - - 0.00001 - 0.2 - 2000.000000 - 0.01000 - - - - - - 1 - - model://turtlebot3_world - - - - - -2.0 -0.5 0.01 0.0 0.0 0.0 - - - - - - - -0.064 0 0.048 0 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 1.0 - - - - -0.064 0 0.048 0 0 0 - - - 0.265 0.265 0.089 - - - - - - -0.064 0 0 0 0 0 - - - model://turtlebot3_waffle/meshes/waffle_base.dae - 0.001 0.001 0.001 - - - - - - - - true - 200 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - - false - - - ~/out:=imu - - - - - - - - -0.052 0 0.111 0 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.125 - - - - -0.020 0 0.161 0 0 0 - - - 0.0508 - 0.055 - - - - - - -0.064 0 0.121 0 0 0 - - - model://turtlebot3_waffle/meshes/lds.dae - 0.001 0.001 0.001 - - - - - - true - false - -0.064 0 0.121 0 0 0 - 5 - - - - 360 - 1.000000 - 0.000000 - 6.280000 - - - - 0.120000 - 3.5 - 0.015000 - - - gaussian - 0.0 - 0.01 - - - - - - ~/out:=scan - - sensor_msgs/LaserScan - - - - - - - - 0.0 0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - 0.0 0.144 0.023 0 0 0 - - - model://turtlebot3_waffle/meshes/left_tire.dae - 0.001 0.001 0.001 - - - - - - - - - 0.0 -0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 -0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - 0.0 -0.144 0.023 0 0 0 - - - model://turtlebot3_waffle/meshes/right_tire.dae - 0.001 0.001 0.001 - - - - - - - -0.177 -0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - - -0.177 0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - - base_footprint - base_link - 0.0 0.0 0.010 0 0 0 - - - - base_link - wheel_left_link - 0.0 0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - wheel_right_link - 0.0 -0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - caster_back_right_link - - - - base_link - caster_back_left_link - - - - base_link - imu_link - -0.032 0 0.068 0 0 0 - - 0 0 1 - - - - - base_link - base_scan - -0.064 0 0.121 0 0 0 - - 0 0 1 - - - - - - - - /tf:=tf - - - - wheel_left_joint - wheel_right_joint - - - 0.287 - 0.066 - - - 20 - 1.0 - - - true - true - false - - odom - base_footprint - - - - - - - ~/out:=joint_states - - 250 - wheel_left_joint - wheel_right_joint - - - - - - -1.0 0.6 0.15 0 -0 0 - - - - 20 - - 0.0416667 - 0 - 0 - 0.0566667 - 0 - 0.0683333 - - 0 0 0 0 -0 0 - - - - - - 0.5 0.4 0.3 - - - - - - 1 - 1 - - - - - - - - 1e+07 - 1 - 0.001 - 0.1 - - - - - 10 - - - - 0 0 -0.15 0 -0 0 - - - model://cardboard_box/meshes/cardboard_box.dae - 1.25932 1.00745 0.755591 - - - - - 0 - 0 - 0 - - - - - diff --git a/nav2_system_tests/worlds/world_only.model b/nav2_system_tests/worlds/world_only.model deleted file mode 100644 index 4c45d4e2f9..0000000000 --- a/nav2_system_tests/worlds/world_only.model +++ /dev/null @@ -1,54 +0,0 @@ - - - - - - model://ground_plane - - - - model://sun - - - - false - - - - - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - orbit - perspective - - - - - 1000.0 - 0.001 - 1 - - - quick - 150 - 0 - 1.400000 - 1 - - - 0.00001 - 0.2 - 2000.000000 - 0.01000 - - - - - - 1 - - model://turtlebot3_world - - - - - diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 1c8b9b1d64..6a82f01511 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -2,54 +2,36 @@ cmake_minimum_required(VERSION 3.5) project(nav2_util) find_package(ament_cmake REQUIRED) +find_package(bond REQUIRED) +find_package(bondcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(lifecycle_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_msgs REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(rcl_interfaces REQUIRED) find_package(rclcpp REQUIRED) -find_package(lifecycle_msgs REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -find_package(bondcpp REQUIRED) -find_package(bond REQUIRED) -find_package(action_msgs REQUIRED) - -set(dependencies - nav2_msgs - tf2_ros - tf2 - tf2_geometry_msgs - geometry_msgs - nav_msgs - rclcpp - lifecycle_msgs - rclcpp_action - rclcpp_lifecycle - bondcpp - bond - action_msgs - rcl_interfaces -) +find_package(tf2_msgs REQUIRED) +find_package(tf2_ros REQUIRED) nav2_package() -include_directories(include) - set(library_name ${PROJECT_NAME}_core) + add_subdirectory(src) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) 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() @@ -58,8 +40,23 @@ if(BUILD_TESTING) add_subdirectory(test) endif() -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(${library_name}) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + bondcpp + geometry_msgs + lifecycle_msgs + nav2_msgs + nav_msgs + rcl_interfaces + rclcpp + rclcpp_action + rclcpp_lifecycle + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros +) +ament_export_targets(${library_name}) ament_package() diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index 7f1ef07157..d652982aef 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -19,6 +19,7 @@ #include #include +#include "rcl_interfaces/msg/parameter_descriptor.hpp" #include "nav2_util/node_thread.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 9de53f5fd5..26274e10d6 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -10,37 +10,31 @@ BSD-3-Clause ament_cmake - libboost-program-options-dev + nav2_common + bond + bondcpp + builtin_interfaces geometry_msgs - rclcpp + lifecycle_msgs nav2_msgs nav_msgs - tf2 - tf2_ros - tf2_geometry_msgs - lifecycle_msgs - bondcpp - bond + rcl_interfaces + rclcpp rclcpp_action rclcpp_lifecycle - launch - launch_testing_ament_cmake - action_msgs - rcl_interfaces - - libboost-program-options + std_msgs + tf2 + tf2_geometry_msgs + tf2_msgs + tf2_ros ament_lint_common ament_lint_auto ament_cmake_gtest - launch - launch_testing_ament_cmake std_srvs test_msgs - action_msgs - launch_testing_ros ament_cmake_pytest diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index 6ddcdc6d32..96f69b5458 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -10,34 +10,41 @@ add_library(${library_name} SHARED odometry_utils.cpp array_parser.cpp ) - -ament_target_dependencies(${library_name} - rclcpp - nav2_msgs - tf2 - tf2_ros - nav_msgs - geometry_msgs - lifecycle_msgs - rclcpp_lifecycle - tf2_geometry_msgs - bondcpp +target_include_directories(${library_name} + PUBLIC + "$" + "$") +target_link_libraries(${library_name} PUBLIC + bondcpp::bondcpp + ${geometry_msgs_TARGETS} + ${lifecycle_msgs_TARGETS} + ${nav2_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${rcl_interfaces_TARGETS} + rclcpp::rclcpp + rclcpp_action::rclcpp_action + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros + tf2::tf2 + ${tf2_geometry_msgs_TARGETS} +) +target_link_libraries(${library_name} PRIVATE + ${bond_TARGETS} ) add_executable(lifecycle_bringup lifecycle_bringup_commandline.cpp ) -target_link_libraries(lifecycle_bringup ${library_name}) +target_link_libraries(lifecycle_bringup PRIVATE ${library_name} rclcpp::rclcpp) add_executable(base_footprint_publisher base_footprint_publisher.cpp ) -target_link_libraries(base_footprint_publisher ${library_name}) - -find_package(Boost REQUIRED COMPONENTS program_options) +target_link_libraries(base_footprint_publisher PRIVATE ${library_name} rclcpp::rclcpp ${tf2_msgs_TARGETS}) install(TARGETS ${library_name} + EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/nav2_util/src/base_footprint_publisher.cpp b/nav2_util/src/base_footprint_publisher.cpp index f3b6791db4..144ba14ab6 100644 --- a/nav2_util/src/base_footprint_publisher.cpp +++ b/nav2_util/src/base_footprint_publisher.cpp @@ -14,7 +14,7 @@ #include -#include "nav2_util/base_footprint_publisher.hpp" +#include "base_footprint_publisher.hpp" int main(int argc, char ** argv) { diff --git a/nav2_util/include/nav2_util/base_footprint_publisher.hpp b/nav2_util/src/base_footprint_publisher.hpp similarity index 96% rename from nav2_util/include/nav2_util/base_footprint_publisher.hpp rename to nav2_util/src/base_footprint_publisher.hpp index 52bcdb53eb..25d58d504b 100644 --- a/nav2_util/include/nav2_util/base_footprint_publisher.hpp +++ b/nav2_util/src/base_footprint_publisher.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ -#define NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ +#ifndef BASE_FOOTPRINT_PUBLISHER_HPP_ +#define BASE_FOOTPRINT_PUBLISHER_HPP_ #include #include @@ -126,4 +126,4 @@ class BaseFootprintPublisher : public rclcpp::Node } // end namespace nav2_util -#endif // NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ +#endif // BASE_FOOTPRINT_PUBLISHER_HPP_ diff --git a/nav2_util/src/lifecycle_utils.cpp b/nav2_util/src/lifecycle_utils.cpp index c778e2abdf..e84bbbc55b 100644 --- a/nav2_util/src/lifecycle_utils.cpp +++ b/nav2_util/src/lifecycle_utils.cpp @@ -13,16 +13,13 @@ // limitations under the License. #include +#include #include -#include #include -#include "lifecycle_msgs/srv/change_state.hpp" -#include "lifecycle_msgs/srv/get_state.hpp" -#include "nav2_util/lifecycle_service_client.hpp" +#include "lifecycle_msgs/msg/transition.hpp" -using std::string; -using lifecycle_msgs::msg::Transition; +#include "nav2_util/lifecycle_service_client.hpp" namespace nav2_util { @@ -34,7 +31,7 @@ namespace nav2_util try { \ fn; \ break; \ - } catch (std::runtime_error & e) { \ + } catch (const std::runtime_error & e) { \ ++count; \ if (count > (retries)) { \ throw e;} \ @@ -53,10 +50,10 @@ static void startupLifecycleNode( // service calls still frequently hang. To get reliable startup it's necessary // to timeout the service call and retry it when that happens. RETRY( - sc.change_state(Transition::TRANSITION_CONFIGURE, service_call_timeout), + sc.change_state(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, service_call_timeout), retries); RETRY( - sc.change_state(Transition::TRANSITION_ACTIVATE, service_call_timeout), + sc.change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, service_call_timeout), retries); } @@ -81,10 +78,10 @@ static void resetLifecycleNode( // service calls still frequently hang. To get reliable reset it's necessary // to timeout the service call and retry it when that happens. RETRY( - sc.change_state(Transition::TRANSITION_DEACTIVATE, service_call_timeout), + sc.change_state(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, service_call_timeout), retries); RETRY( - sc.change_state(Transition::TRANSITION_CLEANUP, service_call_timeout), + sc.change_state(lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, service_call_timeout), retries); } diff --git a/nav2_util/src/string_utils.cpp b/nav2_util/src/string_utils.cpp index 47d86aaf57..df447b5492 100644 --- a/nav2_util/src/string_utils.cpp +++ b/nav2_util/src/string_utils.cpp @@ -15,14 +15,12 @@ #include "nav2_util/string_utils.hpp" #include -using std::string; - namespace nav2_util { -std::string strip_leading_slash(const string & in) +std::string strip_leading_slash(const std::string & in) { - string out = in; + std::string out = in; if ((!in.empty()) && (in[0] == '/')) { out.erase(0, 1); @@ -31,13 +29,13 @@ std::string strip_leading_slash(const string & in) return out; } -Tokens split(const string & tokenstring, char delimiter) +Tokens split(const std::string & tokenstring, char delimiter) { Tokens tokens; size_t current_pos = 0; size_t pos = 0; - while ((pos = tokenstring.find(delimiter, current_pos)) != string::npos) { + while ((pos = tokenstring.find(delimiter, current_pos)) != std::string::npos) { tokens.push_back(tokenstring.substr(current_pos, pos - current_pos)); current_pos = pos + 1; } diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 4c0f274721..14d774c298 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -1,4 +1,5 @@ ament_add_gtest(test_execution_timer test_execution_timer.cpp) +target_link_libraries(test_execution_timer ${library_name}) ament_add_gtest(test_node_utils test_node_utils.cpp) target_link_libraries(test_node_utils ${library_name}) @@ -7,56 +8,46 @@ find_package(std_srvs REQUIRED) find_package(test_msgs REQUIRED) ament_add_gtest(test_service_client test_service_client.cpp) -ament_target_dependencies(test_service_client std_srvs) -target_link_libraries(test_service_client ${library_name}) +target_link_libraries(test_service_client ${library_name} ${std_srvs_TARGETS}) ament_add_gtest(test_string_utils test_string_utils.cpp) target_link_libraries(test_string_utils ${library_name}) find_package(rclcpp_lifecycle REQUIRED) ament_add_gtest(test_lifecycle_utils test_lifecycle_utils.cpp) -ament_target_dependencies(test_lifecycle_utils rclcpp_lifecycle) -target_link_libraries(test_lifecycle_utils ${library_name}) +target_link_libraries(test_lifecycle_utils ${library_name} rclcpp_lifecycle::rclcpp_lifecycle) ament_add_gtest(test_actions test_actions.cpp) -ament_target_dependencies(test_actions rclcpp_action test_msgs) -target_link_libraries(test_actions ${library_name}) +target_link_libraries(test_actions ${library_name} rclcpp_action::rclcpp_action ${test_msgs_TARGETS}) ament_add_gtest(test_lifecycle_node test_lifecycle_node.cpp) -ament_target_dependencies(test_lifecycle_node rclcpp_lifecycle) -target_link_libraries(test_lifecycle_node ${library_name}) +target_link_libraries(test_lifecycle_node ${library_name} rclcpp_lifecycle::rclcpp_lifecycle) ament_add_gtest(test_lifecycle_cli_node test_lifecycle_cli_node.cpp) -ament_target_dependencies(test_lifecycle_cli_node rclcpp_lifecycle) -target_link_libraries(test_lifecycle_cli_node ${library_name}) +target_link_libraries(test_lifecycle_cli_node ${library_name} rclcpp_lifecycle::rclcpp_lifecycle) ament_add_gtest(test_geometry_utils test_geometry_utils.cpp) -ament_target_dependencies(test_geometry_utils geometry_msgs) -target_link_libraries(test_geometry_utils ${library_name}) +target_link_libraries(test_geometry_utils ${library_name} ${geometry_msgs_TARGETS}) ament_add_gtest(test_odometry_utils test_odometry_utils.cpp) -ament_target_dependencies(test_odometry_utils nav_msgs geometry_msgs) -target_link_libraries(test_odometry_utils ${library_name}) +target_link_libraries(test_odometry_utils ${library_name} ${nav_msgs_TARGETS} ${geometry_msgs_TARGETS}) 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}) +target_link_libraries(test_robot_utils ${library_name} ${geometry_msgs_TARGETS}) ament_add_gtest(test_base_footprint_publisher test_base_footprint_publisher.cpp) -ament_target_dependencies(test_base_footprint_publisher geometry_msgs) -target_link_libraries(test_base_footprint_publisher ${library_name}) +target_include_directories(test_base_footprint_publisher PRIVATE "$") + +target_link_libraries(test_base_footprint_publisher ${library_name} tf2_ros::tf2_ros rclcpp::rclcpp ${geometry_msgs_TARGETS}) 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}) +target_link_libraries(test_twist_publisher ${library_name} rclcpp::rclcpp ${geometry_msgs_TARGETS}) 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}) +target_link_libraries(test_twist_subscriber ${library_name} rclcpp::rclcpp ${geometry_msgs_TARGETS}) ament_add_gtest(test_validation_messages test_validation_messages.cpp) -ament_target_dependencies(test_validation_messages rclcpp_lifecycle) -target_link_libraries(test_validation_messages ${library_name}) \ No newline at end of file +target_link_libraries(test_validation_messages ${library_name} ${builtin_interfaces_TARGETS} ${std_msgs_TARGETS} ${geometry_msgs_TARGETS}) diff --git a/nav2_util/test/test_base_footprint_publisher.cpp b/nav2_util/test/test_base_footprint_publisher.cpp index 47dc83c7f3..cd78883132 100644 --- a/nav2_util/test/test_base_footprint_publisher.cpp +++ b/nav2_util/test/test_base_footprint_publisher.cpp @@ -15,7 +15,7 @@ #include #include -#include "nav2_util/base_footprint_publisher.hpp" +#include "base_footprint_publisher.hpp" #include "gtest/gtest.h" #include "tf2/exceptions.h"