diff --git a/README.md b/README.md index c5c4b2ce080..1b3bdfa8402 100644 --- a/README.md +++ b/README.md @@ -12,6 +12,26 @@ For detailed instructions on how to: Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://navigation2.slack.com). +## Citation + +If you use the navigation framework, an algorithm from this repository, or ideas from it +please cite this work in your papers! + + - S. Macenski, F. Martín, R. White, J. Clavero. [**The Marathon 2: A Navigation System**](https://arxiv.org/abs/2003.00368). IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020. + + ```bibtex + @InProceedings{macenski2020marathon2, + title = {The Marathon 2: A Navigation System}, + author = {Macenski, Steve and Martín, Francisco and White, Ruffin and Ginés Clavero, Jonatan}, + year = {2020}, + booktitle = {2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + url = {https://github.com/ros-planning/navigation2}, + pdf = {https://arxiv.org/abs/2003.00368} + } +``` + +## Build Status + | Service | Dashing | Eloquent | Foxy | Master | | :---: | :---: | :---: | :---: | :---: | | ROS Build Farm | [![Build Status](http://build.ros2.org/job/Ddev__navigation2__ubuntu_bionic_amd64/badge/icon)](http://build.ros2.org/job/Ddev__navigation2__ubuntu_bionic_amd64/) | [![Build Status](http://build.ros2.org/job/Edev__navigation2__ubuntu_bionic_amd64/badge/icon)](http://build.ros2.org/job/Edev__navigation2__ubuntu_bionic_amd64/) | [![Build Status](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/badge/icon)](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/) | N/A | diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index 531657aece3..dec94bf61a3 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -154,12 +154,15 @@ When `plugins` parameter is not overridden, the following default plugins are lo | Parameter | Default | Description | | ----------| --------| ------------| | controller_frequency | 20.0 | Frequency to run controller | +| progress_checker_plugin | "progress_checker" | Plugin used by the controller to check whether the robot has at least covered a set distance/displacement in a set amount of time, thus checking the progress of the robot. | +| `.plugin` | "nav2_controller::SimpleProgressChecker" | Default plugin | +| goal_checker_plugin | "goal_checker" | Check if the goal has been reached | +| `.plugin` | "nav2_controller::SimpleGoalChecker" | Default plugin | | controller_plugins | ["FollowPath"] | List of mapped names for controller plugins for processing requests and parameters | +| `.plugin` | "dwb_core::DWBLocalPlanner" | Default plugin | | min_x_velocity_threshold | 0.0001 | Minimum X velocity to use (m/s) | | min_y_velocity_threshold | 0.0001 | Minimum Y velocity to use (m/s) | | min_theta_velocity_threshold | 0.0001 | Minimum angular velocity to use (rad/s) | -| required_movement_radius | 0.5 | Minimum amount a robot must move to be progressing to goal (m) | -| movement_time_allowance | 10.0 | Maximum amount of time a robot has to move the minimum radius (s) | **NOTE:** When `controller_plugins` parameter is overridden, each plugin namespace defined in the list needs to have a `plugin` parameter defining the type of plugin to be loaded in the namespace. @@ -173,11 +176,36 @@ controller_server: plugin: "dwb_core::DWBLocalPlanner" ``` -When `controller_plugins` parameter is not overridden, the following default plugins are loaded: +When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parameters are not overridden, the following default plugins are loaded: | Namespace | Plugin | | ----------| --------| | "FollowPath" | "dwb_core::DWBLocalPlanner" | +| "progress_checker" | "nav2_controller::SimpleProgressChecker" | +| "goal_checker" | "nav2_controller::SimpleGoalChecker" | + +## simple_progress_checker plugin + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.required_movement_radius | 0.5 | Minimum distance to count as progress (m) | +| ``.movement_time_allowance | 10.0 | Maximum time allowence for progress to happen (s) | + + +## simple_goal_checker plugin + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.xy_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (m) | +| ``.yaw_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (rad) | +| ``.stateful | true | Whether to check for XY position tolerance after rotating to goal orientation in case of minor localization changes | + +## stopped_goal_checker plugin + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.rot_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (rad/s) | +| ``.trans_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (m/s) | # dwb_controller @@ -193,7 +221,6 @@ When `controller_plugins` parameter is not overridden, the following default plu | ``.prune_distance | 1.0 | Distance (m) to prune backward until | | ``.debug_trajectory_details | false | Publish debug information | | ``.trajectory_generator_name | "dwb_plugins::StandardTrajectoryGenerator" | Trajectory generator plugin name | -| ``.goal_checker_name | "dwb_plugins::SimpleGoalChecker" | Goal checker plugin name | | ``.transform_tolerance | 0.1 | TF transform tolerance | | ``.short_circuit_trajectory_evaluation | true | Stop evaluating scores after best score is found | | ``.path_distance_bias | N/A | Old version of `PathAlign.scale`, use that instead | @@ -350,14 +377,6 @@ When `controller_plugins` parameter is not overridden, the following default plu | ``.lookahead_time | -1 | If > 0, amount of time to look forward for a collision for. | | ``.``.scale | 1.0 | Weight scale | -## simple_goal_checker plugin - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.xy_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (m) | -| ``.yaw_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (rad) | -| ``.stateful | true | Whether to check for XY position tolerance after rotating to goal orientation in case of minor localization changes | - ## standard_traj_generator plugin | Parameter | Default | Description | @@ -375,13 +394,6 @@ When `controller_plugins` parameter is not overridden, the following default plu | ----------| --------| ------------| | ``.sim_time | N/A | Time to simulate ahead by (s) | -## stopped_goal_checker plugin - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.rot_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (rad/s) | -| ``.trans_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (m/s) | - # lifecycle_manager | Parameter | Default | Description | @@ -547,6 +559,7 @@ When `recovery_plugins` parameter is not overridden, the following default plugi | z_rand | 0.5 | Mixture weight for z_rand part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand. | | z_short | 0.05 | Mixture weight for z_short part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand. | | always_reset_initial_pose | false | Requires that AMCL is provided an initial pose either via topic or initial_pose* parameter (with parameter set_initial_pose: true) when reset. Otherwise, by default AMCL will use the last known pose to initialize | +| scan_topic | scan | Topic to subscribe to in order to receive the laser scan for localization | --- @@ -624,6 +637,14 @@ When `recovery_plugins` parameter is not overridden, the following default plugi | server_name | N/A | Action server name | | server_timeout | 10 | Action server timeout (ms) | +### BT Node TruncatePath + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| input_path | N/A | Path to be truncated | +| output_path | N/A | Path truncated | +| distance | 1.0 | Distance (m) to cut from last pose | + ## Conditions ### BT Node GoalReached @@ -674,3 +695,10 @@ When `recovery_plugins` parameter is not overridden, the following default plugi | min_speed | 0.0 | Minimum speed (m/s) | | max_speed | 0.5 | Maximum speed (m/s) | | filter_duration | 0.3 | Duration (secs) for velocity smoothing filter | + +### BT Node GoalUpdater + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| input_goal | N/A | The reference goal | +| output_goal | N/A | The reference goal, or a newer goal received by subscription | diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 669ea988359..be7cf77d8ca 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -167,7 +167,6 @@ class AmclNode : public nav2_util::LifecycleNode // Laser scan related void initLaserScan(); - const char * scan_topic_{"scan"}; nav2_amcl::Laser * createLaserObject(); int scan_error_count_{0}; std::vector lasers_; @@ -250,6 +249,7 @@ class AmclNode : public nav2_util::LifecycleNode double z_max_; double z_short_; double z_rand_; + std::string scan_topic_{"scan"}; }; } // namespace nav2_amcl diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index c0942603197..e90d48a4ffc 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -213,6 +213,10 @@ AmclNode::AmclNode() "Requires that AMCL is provided an initial pose either via topic or initial_pose* parameter " "(with parameter set_initial_pose: true) when reset. Otherwise, by default AMCL will use the" "last known pose to initialize"); + + add_parameter( + "scan_topic", rclcpp::ParameterValue("scan"), + "Topic to subscribe to in order to receive the laser scan for localization"); } AmclNode::~AmclNode() @@ -1098,6 +1102,7 @@ AmclNode::initParameters() get_parameter("z_short", z_short_); get_parameter("first_map_only_", first_map_only_); get_parameter("always_reset_initial_pose", always_reset_initial_pose_); + get_parameter("scan_topic", scan_topic_); save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate); transform_tolerance_ = tf2::durationFromSec(tmp_tol); diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 807c8030646..b062c777973 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -101,6 +101,12 @@ list(APPEND plugin_libs nav2_distance_controller_bt_node) add_library(nav2_speed_controller_bt_node SHARED plugins/decorator/speed_controller.cpp) list(APPEND plugin_libs nav2_speed_controller_bt_node) +add_library(nav2_truncate_path_action_bt_node SHARED plugins/action/truncate_path_action.cpp) +list(APPEND plugin_libs nav2_truncate_path_action_bt_node) + +add_library(nav2_goal_updater_node_bt_node SHARED plugins/decorator/goal_updater_node.cpp) +list(APPEND plugin_libs nav2_goal_updater_node_bt_node) + add_library(nav2_recovery_node_bt_node SHARED plugins/control/recovery_node.cpp) list(APPEND plugin_libs nav2_recovery_node_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp new file mode 100644 index 00000000000..f76b7df4d59 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp @@ -0,0 +1,55 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Francisco Martin Rico +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_ + +#include +#include + +#include "nav_msgs/msg/path.hpp" + +#include "behaviortree_cpp_v3/action_node.h" + +namespace nav2_behavior_tree +{ + +class TruncatePath : public BT::ActionNodeBase +{ +public: + TruncatePath( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("input_path", "Original Path"), + BT::OutputPort("output_path", "Path truncated to a certain distance"), + BT::InputPort("distance", 1.0, "distance"), + }; + } + +private: + void halt() override {} + BT::NodeStatus tick() override; + + double distance_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp new file mode 100644 index 00000000000..1aba0ccd890 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Francisco Martin Rico +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_ + +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "behaviortree_cpp_v3/decorator_node.h" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +class GoalUpdater : public BT::DecoratorNode +{ +public: + GoalUpdater( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("input_goal", "Original Goal"), + BT::OutputPort( + "output_goal", + "Received Goal by subscription"), + }; + } + +private: + BT::NodeStatus tick() override; + + void callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + + rclcpp::Subscription::SharedPtr goal_sub_; + + geometry_msgs::msg::PoseStamped last_goal_received_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_ diff --git a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp new file mode 100644 index 00000000000..7f919a30f3a --- /dev/null +++ b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp @@ -0,0 +1,89 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Francisco Martin Rico +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "behaviortree_cpp_v3/decorator_node.h" + +#include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp" + +namespace nav2_behavior_tree +{ + +TruncatePath::TruncatePath( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(name, conf), + distance_(1.0) +{ + getInput("distance", distance_); +} + +inline BT::NodeStatus TruncatePath::tick() +{ + setStatus(BT::NodeStatus::RUNNING); + + nav_msgs::msg::Path input_path; + + getInput("input_path", input_path); + + if (input_path.poses.empty()) { + setOutput("output_path", input_path); + return BT::NodeStatus::SUCCESS; + } + + geometry_msgs::msg::PoseStamped final_pose = input_path.poses.back(); + + double distance_to_goal = nav2_util::geometry_utils::euclidean_distance( + input_path.poses.back(), final_pose); + + while (distance_to_goal < distance_ && input_path.poses.size() > 2) { + input_path.poses.pop_back(); + distance_to_goal = nav2_util::geometry_utils::euclidean_distance( + input_path.poses.back(), final_pose); + } + + double dx = final_pose.pose.position.x - input_path.poses.back().pose.position.x; + double dy = final_pose.pose.position.y - input_path.poses.back().pose.position.y; + + double final_angle = atan2(dy, dx); + + if (std::isnan(final_angle) || std::isinf(final_angle)) { + RCLCPP_WARN( + config().blackboard->get("node")->get_logger(), + "Final angle is not valid while truncating path. Setting to 0.0"); + final_angle = 0.0; + } + + input_path.poses.back().pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + final_angle); + + setOutput("output_path", input_path); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("TruncatePath"); +} diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp new file mode 100644 index 00000000000..5279613498b --- /dev/null +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -0,0 +1,71 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Francisco Martin Rico +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "behaviortree_cpp_v3/decorator_node.h" + +#include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +using std::placeholders::_1; + +GoalUpdater::GoalUpdater( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::DecoratorNode(name, conf) +{ + auto node = config().blackboard->get("node"); + + std::string goal_updater_topic; + node->get_parameter_or("goal_updater_topic", goal_updater_topic, "goal_update"); + + goal_sub_ = node->create_subscription( + goal_updater_topic, 10, std::bind(&GoalUpdater::callback_updated_goal, this, _1)); +} + +inline BT::NodeStatus GoalUpdater::tick() +{ + geometry_msgs::msg::PoseStamped goal; + + getInput("input_goal", goal); + + if (rclcpp::Time(last_goal_received_.header.stamp) > rclcpp::Time(goal.header.stamp)) { + goal = last_goal_received_; + } + + setOutput("output_goal", goal); + return child_node_->executeTick(); +} + +void +GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg) +{ + last_goal_received_ = *msg; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("GoalUpdater"); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 165ab35540c..ec004fea2d3 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -29,3 +29,7 @@ ament_target_dependencies(test_action_follow_path_action ${dependencies}) ament_add_gtest(test_action_navigate_to_pose_action test_navigate_to_pose_action.cpp) target_link_libraries(test_action_navigate_to_pose_action nav2_navigate_to_pose_action_bt_node) ament_target_dependencies(test_action_navigate_to_pose_action ${dependencies}) + +ament_add_gtest(test_truncate_path_action test_truncate_path_action.cpp) +target_link_libraries(test_truncate_path_action nav2_truncate_path_action_bt_node) +ament_target_dependencies(test_truncate_path_action ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp new file mode 100644 index 00000000000..11c56ef082d --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp @@ -0,0 +1,156 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Francisco Martin Rico +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" + +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp" + + +class TruncatePathTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("change_goal_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "TruncatePath", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr TruncatePathTestFixture::node_ = nullptr; + +BT::NodeConfiguration * TruncatePathTestFixture::config_ = nullptr; +std::shared_ptr TruncatePathTestFixture::factory_ = nullptr; +std::shared_ptr TruncatePathTestFixture::tree_ = nullptr; + +TEST_F(TruncatePathTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new goal and set it on blackboard + nav_msgs::msg::Path path; + path.header.stamp = node_->now(); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 0; + pose.pose.position.y = 0; + pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0); + path.poses.push_back(pose); + + pose.pose.position.x = 0.5; + pose.pose.position.y = 0.0; + path.poses.push_back(pose); + + pose.pose.position.x = 0.9; + pose.pose.position.y = 0.0; + path.poses.push_back(pose); + + pose.pose.position.x = 1.5; + pose.pose.position.y = 0.5; + path.poses.push_back(pose); + + EXPECT_EQ(path.poses.size(), 4u); + + config_->blackboard->set("path", path); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + nav_msgs::msg::Path truncated_path; + config_->blackboard->get("truncated_path", truncated_path); + + EXPECT_NE(path, truncated_path); + EXPECT_EQ(truncated_path.poses.size(), 2u); + + double r, p, y; + tf2::Quaternion q; + tf2::fromMsg(truncated_path.poses.back().pose.orientation, q); + tf2::Matrix3x3(q).getRPY(r, p, y); + + EXPECT_NEAR(y, 0.463, 0.001); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt b/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt index a1b568a8a54..ad53c5d1d1a 100644 --- a/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt @@ -9,3 +9,7 @@ ament_target_dependencies(test_decorator_speed_controller ${dependencies}) ament_add_gtest(test_decorator_rate_controller test_rate_controller.cpp) target_link_libraries(test_decorator_rate_controller nav2_rate_controller_bt_node) ament_target_dependencies(test_decorator_rate_controller ${dependencies}) + +ament_add_gtest(test_goal_updater_node test_goal_updater_node.cpp) +target_link_libraries(test_goal_updater_node nav2_goal_updater_node_bt_node) +ament_target_dependencies(test_goal_updater_node ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp new file mode 100644 index 00000000000..341192757fb --- /dev/null +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -0,0 +1,149 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Francisco Martin Rico +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp" + + +class GoalUpdaterTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("goal_updater_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "GoalUpdater", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr GoalUpdaterTestFixture::node_ = nullptr; + +BT::NodeConfiguration * GoalUpdaterTestFixture::config_ = nullptr; +std::shared_ptr GoalUpdaterTestFixture::factory_ = nullptr; +std::shared_ptr GoalUpdaterTestFixture::tree_ = nullptr; + +TEST_F(GoalUpdaterTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + geometry_msgs::msg::PoseStamped updated_goal; + config_->blackboard->get("updated_goal", updated_goal); + + EXPECT_EQ(updated_goal, goal); + + geometry_msgs::msg::PoseStamped goal_to_update; + goal_to_update.header.stamp = node_->now(); + goal_to_update.pose.position.x = 2.0; + + auto goal_updater_pub = + node_->create_publisher("goal_update", 10); + + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + goal_updater_pub->publish(goal_to_update); + + rclcpp::spin_some(node_); + } + + config_->blackboard->get("updated_goal", updated_goal); + + EXPECT_NE(updated_goal, goal); + EXPECT_EQ(updated_goal, goal_to_update); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_bringup/bringup/launch/bringup_launch.py b/nav2_bringup/bringup/launch/bringup_launch.py index 61a048d0da9..4e200fcde25 100644 --- a/nav2_bringup/bringup/launch/bringup_launch.py +++ b/nav2_bringup/bringup/launch/bringup_launch.py @@ -41,7 +41,7 @@ def generate_launch_description(): autostart = LaunchConfiguration('autostart') stdout_linebuf_envvar = SetEnvironmentVariable( - 'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1') + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') declare_namespace_cmd = DeclareLaunchArgument( 'namespace', diff --git a/nav2_bringup/bringup/launch/localization_launch.py b/nav2_bringup/bringup/launch/localization_launch.py index 2791e0068cd..e058bba8c10 100644 --- a/nav2_bringup/bringup/launch/localization_launch.py +++ b/nav2_bringup/bringup/launch/localization_launch.py @@ -56,7 +56,7 @@ def generate_launch_description(): return LaunchDescription([ # Set env var to print messages to stdout immediately - SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), DeclareLaunchArgument( 'namespace', default_value='', diff --git a/nav2_bringup/bringup/launch/navigation_launch.py b/nav2_bringup/bringup/launch/navigation_launch.py index b837ac4d4eb..c421d709731 100644 --- a/nav2_bringup/bringup/launch/navigation_launch.py +++ b/nav2_bringup/bringup/launch/navigation_launch.py @@ -64,7 +64,7 @@ def generate_launch_description(): return LaunchDescription([ # Set env var to print messages to stdout immediately - SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), DeclareLaunchArgument( 'namespace', default_value='', diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index 0c9c0ffa7af..53ab496bebd 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -37,6 +37,7 @@ amcl: z_max: 0.05 z_rand: 0.5 z_short: 0.05 + scan_topic: scan amcl_map_client: ros__parameters: @@ -85,8 +86,21 @@ controller_server: min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" controller_plugins: ["FollowPath"] + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index 6f3cb4e31ca..a4a62d6b1a5 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -37,6 +37,7 @@ amcl: z_max: 0.05 z_rand: 0.5 z_short: 0.05 + scan_topic: scan amcl_map_client: ros__parameters: @@ -85,8 +86,21 @@ controller_server: min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" controller_plugins: ["FollowPath"] + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 137cec171c7..c92390f46d2 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -37,6 +37,7 @@ amcl: z_max: 0.05 z_rand: 0.5 z_short: 0.05 + scan_topic: scan amcl_map_client: ros__parameters: @@ -68,6 +69,8 @@ bt_navigator: - nav2_rate_controller_bt_node - nav2_distance_controller_bt_node - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_round_robin_node_bt_node @@ -86,8 +89,21 @@ controller_server: min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" controller_plugins: ["FollowPath"] + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" @@ -249,6 +265,7 @@ map_saver: save_map_timeout: 5000 free_thresh_default: 0.25 occupied_thresh_default: 0.65 + map_subscribe_transient_local: True planner_server: ros__parameters: diff --git a/nav2_bt_navigator/README.md b/nav2_bt_navigator/README.md index c0aa7ea66ca..f0864df97ad 100644 --- a/nav2_bt_navigator/README.md +++ b/nav2_bt_navigator/README.md @@ -24,8 +24,9 @@ A Behavior Tree consists of control flow nodes, such as fallback, sequence, para ## Navigation Behavior Trees -The BT Navigator package has three sample XML-based descriptions of BTs. -These trees are [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml), [navigate_w_replanning_distance.xml](behavior_trees/navigate_w_replanning_distance.xml) and [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml). +The BT Navigator package has four sample XML-based descriptions of BTs. +These trees are [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml), [navigate_w_replanning_distance.xml](behavior_trees/navigate_w_replanning_distance.xml), [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml) and +[followpoint.xml](behavior_trees/followpoint.xml). The user may use any of these sample trees or develop a more complex tree which could better suit the user's needs. ### Navigate with Replanning (time-based) @@ -66,6 +67,9 @@ returns FAILURE, all nodes are halted and this node returns FAILURE. * SpeedController: A custom control flow node, which controls the tick rate based on the current speed. This decorator offers the most flexibility as the user can set the minimum/maximum tick rate which is adjusted according to the current speed. +* GoalUpdater: A custom control node, which updates the goal pose. It subscribes to a topic in which it can receive an updated goal pose to use instead of the one commanded in action. It is useful for dynamic object following tasks. + + #### Condition Nodes * GoalReached: Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, which in that case the `ComputePathToPose` action node will not get ticked. @@ -81,6 +85,9 @@ The graphical version of this Behavior Tree: The navigate with replanning BT first ticks the `RateController` node which specifies how frequently the `GoalReached` and `ComputePathToPose` should be invoked. Then the `GoalReached` nodes check the distance to the goal to determine if the `ComputePathToPose` should be ticked or not. The `ComputePathToPose` gets the incoming goal pose from the blackboard, computes the path and puts the result back on the blackboard, where `FollowPath` picks it up. Each time a new path is computed, the blackboard gets updated and then `FollowPath` picks up the new goal to compute a control effort for. `controller_id` will specify the type of control effort you'd like to compute such as `FollowPath` `FollowPathSlow` `FollowPathExactly`, etc. + +* TruncatePath: A custom control node, which modifies a path making it shorter. It removes parts of the path closer than a distance to the goal pose. The resulting last pose of the path orientates the robot to the original goal pose. + ### Navigate with replanning and simple recovery actions With the recovery node, simple recoverable navigation with replanning can be implemented by utilizing the [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml) and a sequence of recovery actions. Our custom behavior actions for recovery are: `clearEntirelyCostmapServiceRequest` for both global and local costmaps, `spin` and `wait`. A graphical version of this simple recoverable Behavior Tree is depicted in the figure below. @@ -118,6 +125,13 @@ In the navigation stack, multi-scope recovery actions are implemented for each m This tree currently is not our default tree in the stack. The xml file is located here: [navigate_w_replanning_and_round_robin_recovery.xml](behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml). +### Navigate following a dynamic point to a certain distance +This tree is an example of how behavior trees can be used to make the robot do more than navigate the current position to the desired pose. In this case, the robot will follow a point that changes dynamically during the execution of the action. The robot will stop its advance and will be oriented towards the target position when it reaches a distance to the target established in the tree. The UpdateGoal decorator node will be used to update the target position. The TruncatePath node will modify the generated path by removing the end part of this path, to maintain a distance from the target, and changes the orientation of the last position. This tree never returns that the action has finished successfully, but must be canceled when you want to stop following the target position. + +![Navigate following a dynamic point to a certain distance](./doc/follow_point.png) + +This tree currently is not our default tree in the stack. The xml file is located here: [follow_point.xml](behavior_trees/follow_point.xml). +
## Open Issues diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml new file mode 100644 index 00000000000..ffae8d9b2ab --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/follow_point.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + diff --git a/nav2_bt_navigator/doc/follow_point.png b/nav2_bt_navigator/doc/follow_point.png new file mode 100644 index 00000000000..825bc4e16c8 Binary files /dev/null and b/nav2_bt_navigator/doc/follow_point.png differ diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index beee45ac91d..c660a25765f 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -51,6 +51,8 @@ BtNavigator::BtNavigator() "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_speed_controller_bt_node", + "nav2_truncate_path_action_bt_node", + "nav2_change_goal_node_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index b4cb1b50d03..5ca5a22605c 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -4,6 +4,7 @@ project(nav2_controller) find_package(ament_cmake REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_common REQUIRED) +find_package(angles REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(std_msgs REQUIRED) @@ -29,12 +30,12 @@ set(library_name ${executable_name}_core) add_library(${library_name} src/nav2_controller.cpp - src/progress_checker.cpp ) target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") set(dependencies + angles rclcpp rclcpp_action std_msgs @@ -46,6 +47,22 @@ set(dependencies pluginlib ) +add_library(simple_progress_checker SHARED plugins/simple_progress_checker.cpp) +ament_target_dependencies(simple_progress_checker ${dependencies}) +# prevent pluginlib from using boost +target_compile_definitions(simple_progress_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +add_library(simple_goal_checker SHARED plugins/simple_goal_checker.cpp) +ament_target_dependencies(simple_goal_checker ${dependencies}) +# prevent pluginlib from using boost +target_compile_definitions(simple_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +add_library(stopped_goal_checker SHARED plugins/stopped_goal_checker.cpp) +target_link_libraries(stopped_goal_checker simple_goal_checker) +ament_target_dependencies(stopped_goal_checker ${dependencies}) +# prevent pluginlib from using boost +target_compile_definitions(stopped_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + ament_target_dependencies(${library_name} ${dependencies} ) @@ -53,13 +70,23 @@ ament_target_dependencies(${library_name} # prevent pluginlib from using boost target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(plugins/test) +endif() + ament_target_dependencies(${executable_name} ${dependencies} ) target_link_libraries(${executable_name} ${library_name}) -install(TARGETS ${library_name} +install(TARGETS simple_progress_checker simple_goal_checker stopped_goal_checker ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -79,6 +106,10 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(${library_name}) +ament_export_libraries(simple_progress_checker + simple_goal_checker + stopped_goal_checker + ${library_name}) +pluginlib_export_plugin_description_file(nav2_core plugins.xml) ament_package() diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp index b0b28b462a1..3d4e7be277e 100644 --- a/nav2_controller/include/nav2_controller/nav2_controller.hpp +++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp @@ -22,6 +22,8 @@ #include #include "nav2_core/controller.hpp" +#include "nav2_core/progress_checker.hpp" +#include "nav2_core/goal_checker.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "tf2_ros/transform_listener.h" #include "nav2_msgs/action/follow_path.hpp" @@ -201,6 +203,22 @@ class ControllerServer : public nav2_util::LifecycleNode std::unique_ptr odom_sub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_publisher_; + // Progress Checker Plugin + pluginlib::ClassLoader progress_checker_loader_; + nav2_core::ProgressChecker::Ptr progress_checker_; + std::string default_progress_checker_id_; + std::string default_progress_checker_type_; + std::string progress_checker_id_; + std::string progress_checker_type_; + + // Goal Checker Plugin + pluginlib::ClassLoader goal_checker_loader_; + nav2_core::GoalChecker::Ptr goal_checker_; + std::string default_goal_checker_id_; + std::string default_goal_checker_type_; + std::string goal_checker_id_; + std::string goal_checker_type_; + // Controller Plugins pluginlib::ClassLoader lp_loader_; ControllerMap controllers_; @@ -210,8 +228,6 @@ class ControllerServer : public nav2_util::LifecycleNode std::vector controller_types_; std::string controller_ids_concat_, current_controller_; - std::unique_ptr progress_checker_; - double controller_frequency_; double min_x_velocity_threshold_; double min_y_velocity_threshold_; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp similarity index 92% rename from nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp rename to nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index 697a0dc10e4..b7e483c1611 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DWB_PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ -#define DWB_PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ +#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ #include #include @@ -42,7 +42,7 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_core/goal_checker.hpp" -namespace dwb_plugins +namespace nav2_controller { /** @@ -72,6 +72,6 @@ class SimpleGoalChecker : public nav2_core::GoalChecker double xy_goal_tolerance_sq_; }; -} // namespace dwb_plugins +} // namespace nav2_controller -#endif // DWB_PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ +#endif // NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ diff --git a/nav2_controller/include/nav2_controller/progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp similarity index 63% rename from nav2_controller/include/nav2_controller/progress_checker.hpp rename to nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp index 3d754ad7aa2..92a5374a5e0 100644 --- a/nav2_controller/include/nav2_controller/progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp @@ -12,40 +12,32 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_ -#define NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_ +#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_ +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/progress_checker.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" namespace nav2_controller { /** - * @class nav2_controller::ProgressChecker - * @brief This class is used to check the position of the robot to make sure - * that it is actually progressing towards a goal. - */ -class ProgressChecker +* @class SimpleProgressChecker +* @brief This plugin is used to check the position of the robot to make sure +* that it is actually progressing towards a goal. +*/ + +class SimpleProgressChecker : public nav2_core::ProgressChecker { public: - /** - * @brief Constructor of ProgressChecker - * @param node Node pointer - */ - explicit ProgressChecker(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node); - /** - * @brief Checks if the robot has moved compare to previous - * pose - * @param current_pose Current pose of the robot - * @throw nav2_core::PlannerException when failed to make progress - */ - void check(geometry_msgs::msg::PoseStamped & current_pose); - /** - * @brief Reset class state upon calling - */ - void reset() {baseline_pose_set_ = false;} + void initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_name) override; + bool check(geometry_msgs::msg::PoseStamped & current_pose) override; + void reset() override; protected: /** @@ -72,4 +64,4 @@ class ProgressChecker }; } // namespace nav2_controller -#endif // NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_ +#endif // NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_ diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp similarity index 88% rename from nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp rename to nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp index 23f727d9456..869edb6330b 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp @@ -32,17 +32,17 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DWB_PLUGINS__STOPPED_GOAL_CHECKER_HPP_ -#define DWB_PLUGINS__STOPPED_GOAL_CHECKER_HPP_ +#ifndef NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_ #include #include #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "dwb_plugins/simple_goal_checker.hpp" +#include "nav2_controller/plugins/simple_goal_checker.hpp" -namespace dwb_plugins +namespace nav2_controller { /** @@ -65,6 +65,6 @@ class StoppedGoalChecker : public SimpleGoalChecker double rot_stopped_velocity_, trans_stopped_velocity_; }; -} // namespace dwb_plugins +} // namespace nav2_controller -#endif // DWB_PLUGINS__STOPPED_GOAL_CHECKER_HPP_ +#endif // NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_ diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index c678de23c75..4e4dc40acde 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -9,6 +9,7 @@ ament_cmake nav2_common + angles rclcpp rclcpp_action std_msgs @@ -24,5 +25,6 @@ ament_cmake + diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml new file mode 100644 index 00000000000..ad27127c03d --- /dev/null +++ b/nav2_controller/plugins.xml @@ -0,0 +1,17 @@ + + + + Checks if distance between current and previous pose is above a threshold + + + + + Checks if current pose is within goal window for x,y and yaw + + + + + Checks linear and angular velocity after stopping + + + diff --git a/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp similarity index 94% rename from nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp rename to nav2_controller/plugins/simple_goal_checker.cpp index b108b551989..f1ec7dba6ca 100644 --- a/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -34,7 +34,7 @@ #include #include -#include "dwb_plugins/simple_goal_checker.hpp" +#include "nav2_controller/plugins/simple_goal_checker.hpp" #include "pluginlib/class_list_macros.hpp" #include "angles/angles.h" #include "nav2_util/node_utils.hpp" @@ -43,7 +43,7 @@ #include "tf2/utils.h" #pragma GCC diagnostic pop -namespace dwb_plugins +namespace nav2_controller { SimpleGoalChecker::SimpleGoalChecker() @@ -103,6 +103,6 @@ bool SimpleGoalChecker::isGoalReached( return fabs(dyaw) < yaw_goal_tolerance_; } -} // namespace dwb_plugins +} // namespace nav2_controller -PLUGINLIB_EXPORT_CLASS(dwb_plugins::SimpleGoalChecker, nav2_core::GoalChecker) +PLUGINLIB_EXPORT_CLASS(nav2_controller::SimpleGoalChecker, nav2_core::GoalChecker) diff --git a/nav2_controller/src/progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp similarity index 65% rename from nav2_controller/src/progress_checker.cpp rename to nav2_controller/plugins/simple_progress_checker.cpp index 68fe96de50c..498d13256cb 100644 --- a/nav2_controller/src/progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -12,33 +12,37 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nav2_controller/progress_checker.hpp" +#include "nav2_controller/plugins/simple_progress_checker.hpp" #include +#include #include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" #include "nav2_util/node_utils.hpp" +#include "pluginlib/class_list_macros.hpp" namespace nav2_controller { static double pose_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &); -ProgressChecker::ProgressChecker(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) -: nh_(node) +void SimpleProgressChecker::initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_name) { + nh_ = node; nav2_util::declare_parameter_if_not_declared( - nh_, "required_movement_radius", rclcpp::ParameterValue(0.5)); + nh_, plugin_name + ".required_movement_radius", rclcpp::ParameterValue(0.5)); nav2_util::declare_parameter_if_not_declared( - nh_, "movement_time_allowance", rclcpp::ParameterValue(10.0)); + nh_, plugin_name + ".movement_time_allowance", rclcpp::ParameterValue(10.0)); // Scale is set to 0 by default, so if it was not set otherwise, set to 0 - nh_->get_parameter_or("required_movement_radius", radius_, 0.5); + nh_->get_parameter_or(plugin_name + ".required_movement_radius", radius_, 0.5); double time_allowance_param = 0.0; - nh_->get_parameter_or("movement_time_allowance", time_allowance_param, 10.0); + nh_->get_parameter_or(plugin_name + ".movement_time_allowance", time_allowance_param, 10.0); time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param); } -void ProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) +bool SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) { // relies on short circuit evaluation to not call is_robot_moved_enough if // baseline_pose is not set. @@ -47,21 +51,27 @@ void ProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) if ((!baseline_pose_set_) || (is_robot_moved_enough(current_pose2d))) { reset_baseline_pose(current_pose2d); - return; + return true; } if ((nh_->now() - baseline_time_) > time_allowance_) { - throw nav2_core::PlannerException("Failed to make progress"); + return false; } + return true; +} + +void SimpleProgressChecker::reset() +{ + baseline_pose_set_ = false; } -void ProgressChecker::reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose) +void SimpleProgressChecker::reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose) { baseline_pose_ = pose; baseline_time_ = nh_->now(); baseline_pose_set_ = true; } -bool ProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose) +bool SimpleProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose) { if (pose_distance(pose, baseline_pose_) > radius_) { return true; @@ -81,3 +91,5 @@ static double pose_distance( } } // namespace nav2_controller + +PLUGINLIB_EXPORT_CLASS(nav2_controller::SimpleProgressChecker, nav2_core::ProgressChecker) diff --git a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp similarity index 93% rename from nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp rename to nav2_controller/plugins/stopped_goal_checker.cpp index c94948e8967..2e24ad181aa 100644 --- a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -35,14 +35,14 @@ #include #include #include -#include "dwb_plugins/stopped_goal_checker.hpp" +#include "nav2_controller/plugins/stopped_goal_checker.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav2_util/node_utils.hpp" using std::hypot; using std::fabs; -namespace dwb_plugins +namespace nav2_controller { StoppedGoalChecker::StoppedGoalChecker() @@ -80,6 +80,6 @@ bool StoppedGoalChecker::isGoalReached( hypot(velocity.linear.x, velocity.linear.y) <= trans_stopped_velocity_; } -} // namespace dwb_plugins +} // namespace nav2_controller -PLUGINLIB_EXPORT_CLASS(dwb_plugins::StoppedGoalChecker, nav2_core::GoalChecker) +PLUGINLIB_EXPORT_CLASS(nav2_controller::StoppedGoalChecker, nav2_core::GoalChecker) diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt new file mode 100644 index 00000000000..d4f34d3918f --- /dev/null +++ b/nav2_controller/plugins/test/CMakeLists.txt @@ -0,0 +1,4 @@ +ament_add_gtest(pctest progress_checker.cpp) +target_link_libraries(pctest simple_progress_checker) +ament_add_gtest(gctest goal_checker.cpp) +target_link_libraries(gctest simple_goal_checker stopped_goal_checker) diff --git a/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp similarity index 94% rename from nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp rename to nav2_controller/plugins/test/goal_checker.cpp index 3e5b5e0357b..fe175db4450 100644 --- a/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -36,13 +36,13 @@ #include #include "gtest/gtest.h" -#include "dwb_plugins/simple_goal_checker.hpp" -#include "dwb_plugins/stopped_goal_checker.hpp" +#include "nav2_controller/plugins/simple_goal_checker.hpp" +#include "nav2_controller/plugins/stopped_goal_checker.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav2_util/lifecycle_node.hpp" -using dwb_plugins::SimpleGoalChecker; -using dwb_plugins::StoppedGoalChecker; +using nav2_controller::SimpleGoalChecker; +using nav2_controller::StoppedGoalChecker; void checkMacro( nav2_core::GoalChecker & gc, @@ -151,8 +151,8 @@ TEST(VelocityIterator, two_checks) SimpleGoalChecker gc; StoppedGoalChecker sgc; - gc.initialize(x, "dwb"); - sgc.initialize(x, "dwb"); + gc.initialize(x, "nav2_controller"); + sgc.initialize(x, "nav2_controller"); sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true); sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false); sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false); diff --git a/nav2_controller/plugins/test/progress_checker.cpp b/nav2_controller/plugins/test/progress_checker.cpp new file mode 100644 index 00000000000..418ff1032ae --- /dev/null +++ b/nav2_controller/plugins/test/progress_checker.cpp @@ -0,0 +1,135 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include "gtest/gtest.h" +#include "nav2_controller/plugins/simple_progress_checker.hpp" +#include "nav_2d_utils/conversions.hpp" +#include "nav2_util/lifecycle_node.hpp" + +using nav2_controller::SimpleProgressChecker; + +class TestLifecycleNode : public nav2_util::LifecycleNode +{ +public: + explicit TestLifecycleNode(const std::string & name) + : nav2_util::LifecycleNode(name) + { + } + + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } +}; + +void checkMacro( + nav2_core::ProgressChecker & pc, + double x0, double y0, + double x1, double y1, + int delay, + bool expected_result) +{ + pc.reset(); + geometry_msgs::msg::PoseStamped pose0, pose1; + pose0.pose.position.x = x0; + pose0.pose.position.y = y0; + pose1.pose.position.x = x1; + pose1.pose.position.y = y1; + EXPECT_TRUE(pc.check(pose0)); + rclcpp::sleep_for(std::chrono::seconds(delay)); + if (expected_result) { + EXPECT_TRUE(pc.check(pose1)); + } else { + EXPECT_FALSE(pc.check(pose1)); + } +} + +TEST(SimpleProgressChecker, progress_checker_reset) +{ + auto x = std::make_shared("progress_checker"); + + nav2_core::ProgressChecker * pc = new SimpleProgressChecker; + pc->reset(); + delete pc; + EXPECT_TRUE(true); +} + +TEST(SimpleProgressChecker, unit_tests) +{ + auto x = std::make_shared("progress_checker"); + + SimpleProgressChecker pc; + pc.initialize(x, "nav2_controller"); + checkMacro(pc, 0, 0, 0, 0, 1, true); + checkMacro(pc, 0, 0, 1, 0, 1, true); + checkMacro(pc, 0, 0, 0, 1, 1, true); + checkMacro(pc, 0, 0, 1, 0, 11, true); + checkMacro(pc, 0, 0, 0, 1, 11, true); + checkMacro(pc, 0, 0, 0, 0, 11, false); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index fc7c36e2737..a01df102fad 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -22,7 +22,6 @@ #include "nav_2d_utils/conversions.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" -#include "nav2_controller/progress_checker.hpp" #include "nav2_controller/nav2_controller.hpp" using namespace std::chrono_literals; @@ -32,6 +31,12 @@ namespace nav2_controller ControllerServer::ControllerServer() : LifecycleNode("controller_server", "", true), + progress_checker_loader_("nav2_core", "nav2_core::ProgressChecker"), + default_progress_checker_id_{"progress_checker"}, + default_progress_checker_type_{"nav2_controller::SimpleProgressChecker"}, + goal_checker_loader_("nav2_core", "nav2_core::GoalChecker"), + default_goal_checker_id_{"goal_checker"}, + default_goal_checker_type_{"nav2_controller::SimpleGoalChecker"}, lp_loader_("nav2_core", "nav2_core::Controller"), default_ids_{"FollowPath"}, default_types_{"dwb_core::DWBLocalPlanner"} @@ -40,6 +45,8 @@ ControllerServer::ControllerServer() declare_parameter("controller_frequency", 20.0); + declare_parameter("progress_checker_plugin", default_progress_checker_id_); + declare_parameter("goal_checker_plugin", default_goal_checker_id_); declare_parameter("controller_plugins", default_ids_); declare_parameter("min_x_velocity_threshold", rclcpp::ParameterValue(0.0001)); declare_parameter("min_y_velocity_threshold", rclcpp::ParameterValue(0.0001)); @@ -61,12 +68,29 @@ ControllerServer::~ControllerServer() nav2_util::CallbackReturn ControllerServer::on_configure(const rclcpp_lifecycle::State & state) { + auto node = shared_from_this(); + RCLCPP_INFO(get_logger(), "Configuring controller interface"); + get_parameter("progress_checker_plugin", progress_checker_id_); + if (progress_checker_id_ == default_progress_checker_id_) { + nav2_util::declare_parameter_if_not_declared( + node, default_progress_checker_id_ + ".plugin", + rclcpp::ParameterValue(default_progress_checker_type_)); + } + get_parameter("goal_checker_plugin", goal_checker_id_); + if (goal_checker_id_ == default_goal_checker_id_) { + nav2_util::declare_parameter_if_not_declared( + node, default_goal_checker_id_ + ".plugin", + rclcpp::ParameterValue(default_goal_checker_type_)); + } + get_parameter("controller_plugins", controller_ids_); if (controller_ids_ == default_ids_) { for (size_t i = 0; i < default_ids_.size(); ++i) { - declare_parameter(default_ids_[i] + ".plugin", default_types_[i]); + nav2_util::declare_parameter_if_not_declared( + node, default_ids_[i] + ".plugin", + rclcpp::ParameterValue(default_types_[i])); } } controller_types_.resize(controller_ids_.size()); @@ -79,9 +103,28 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) costmap_ros_->on_configure(state); - auto node = shared_from_this(); - - progress_checker_ = std::make_unique(node); + try { + progress_checker_type_ = nav2_util::get_plugin_type_param(node, progress_checker_id_); + progress_checker_ = progress_checker_loader_.createUniqueInstance(progress_checker_type_); + RCLCPP_INFO( + get_logger(), "Created progress_checker : %s of type %s", + progress_checker_id_.c_str(), progress_checker_type_.c_str()); + progress_checker_->initialize(node, progress_checker_id_); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what()); + exit(-1); + } + try { + goal_checker_type_ = nav2_util::get_plugin_type_param(node, goal_checker_id_); + goal_checker_ = goal_checker_loader_.createUniqueInstance(goal_checker_type_); + RCLCPP_INFO( + get_logger(), "Created goal_checker : %s of type %s", + goal_checker_id_.c_str(), goal_checker_type_.c_str()); + goal_checker_->initialize(node, goal_checker_id_); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what()); + exit(-1); + } for (size_t i = 0; i != controller_ids_.size(); i++) { try { @@ -172,6 +215,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state) vel_publisher_.reset(); action_server_.reset(); + goal_checker_->reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -295,6 +339,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) controllers_[current_controller_]->setPlan(path); auto end_pose = *(path.poses.end() - 1); + goal_checker_->reset(); RCLCPP_DEBUG( get_logger(), "Path end point is (%.2f, %.2f)", @@ -310,7 +355,9 @@ void ControllerServer::computeAndPublishVelocity() throw nav2_core::PlannerException("Failed to obtain robot pose"); } - progress_checker_->check(pose); + if (!progress_checker_->check(pose)) { + throw nav2_core::PlannerException("Failed to make progress"); + } nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); @@ -375,7 +422,7 @@ bool ControllerServer::isGoalReached() nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(twist); - return controllers_[current_controller_]->isGoalReached(pose, velocity); + return goal_checker_->isGoalReached(pose.pose, end_pose_, velocity); } bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose) diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 1e6e98052c3..69aa10f25b0 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -112,20 +112,6 @@ class Controller virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity) = 0; - - /** - * @brief Controller isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. - * - * The pose that it checks against is the last pose in the current global plan. - * The calculation is delegated to the goal_checker plugin. - * - * @param pose Current pose - * @param velocity Current velocity - * @return True if the robot should be considered as having reached the goal. - */ - virtual bool isGoalReached( - const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) = 0; }; } // namespace nav2_core diff --git a/nav2_core/include/nav2_core/progress_checker.hpp b/nav2_core/include/nav2_core/progress_checker.hpp new file mode 100644 index 00000000000..041ae88fbdb --- /dev/null +++ b/nav2_core/include/nav2_core/progress_checker.hpp @@ -0,0 +1,62 @@ +// Copyright (c) 2019 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. + +#ifndef NAV2_CORE__PROGRESS_CHECKER_HPP_ +#define NAV2_CORE__PROGRESS_CHECKER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" + +namespace nav2_core +{ +/** + * @class nav2_core::ProgressChecker + * @brief This class defines the plugin interface used to check the + * position of the robot to make sure that it is actually progressing + * towards a goal. + */ +class ProgressChecker +{ +public: + typedef std::shared_ptr Ptr; + + virtual ~ProgressChecker() {} + + /** + * @brief Initialize parameters for ProgressChecker + * @param node Node pointer + */ + virtual void initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_name) = 0; + /** + * @brief Checks if the robot has moved compare to previous + * pose + * @param current_pose Current pose of the robot + * @return True if progress is made + */ + virtual bool check(geometry_msgs::msg::PoseStamped & current_pose) = 0; + /** + * @brief Reset class state upon calling + */ + virtual void reset() {} +}; +} // namespace nav2_core + +#endif // NAV2_CORE__PROGRESS_CHECKER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index f6f01e8c563..bbd5daa757e 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -111,6 +111,8 @@ class StaticLayer : public CostmapLayer bool trinary_costmap_; bool map_received_{false}; tf2::Duration transform_tolerance_; + std::atomic update_in_progress_; + nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 8180e3f6ba2..411981813f6 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -58,6 +58,7 @@ namespace nav2_costmap_2d { StaticLayer::StaticLayer() +: map_buffer_(nullptr) { } @@ -140,6 +141,7 @@ StaticLayer::getParameters() // Enforce bounds lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); map_received_ = false; + update_in_progress_.store(false); transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); } @@ -193,6 +195,9 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) unsigned int index = 0; + // we have a new map, update full size of map + std::lock_guard guard(*getMutex()); + // initialize the costmap with static data for (unsigned int i = 0; i < size_y; ++i) { for (unsigned int j = 0; j < size_x; ++j) { @@ -204,8 +209,6 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) map_frame_ = new_map.header.frame_id; - // we have a new map, update full size of map - std::lock_guard guard(*getMutex()); x_ = y_ = 0; width_ = size_x_; height_ = size_y_; @@ -249,9 +252,15 @@ void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map) { std::lock_guard guard(*getMutex()); - processMap(*new_map); if (!map_received_) { map_received_ = true; + processMap(*new_map); + } + if (update_in_progress_.load()) { + map_buffer_ = new_map; + } else { + processMap(*new_map); + map_buffer_ = nullptr; } } @@ -311,6 +320,14 @@ StaticLayer::updateBounds( } std::lock_guard guard(*getMutex()); + update_in_progress_.store(true); + + // If there is a new available map, load it. + if (map_buffer_) { + processMap(*map_buffer_); + map_buffer_ = nullptr; + } + if (!layered_costmap_->isRolling() ) { if (!(has_updated_data_ || has_extra_bounds_)) { return; @@ -338,6 +355,7 @@ StaticLayer::updateCosts( int min_i, int min_j, int max_i, int max_j) { if (!enabled_) { + update_in_progress_.store(false); return; } if (!map_received_) { @@ -347,6 +365,7 @@ StaticLayer::updateCosts( RCLCPP_WARN(node_->get_logger(), "Can't update static costmap layer, no map received"); count = 0; } + update_in_progress_.store(false); return; } @@ -369,6 +388,7 @@ StaticLayer::updateCosts( transform_tolerance_); } catch (tf2::TransformException & ex) { RCLCPP_ERROR(node_->get_logger(), "StaticLayer: %s", ex.what()); + update_in_progress_.store(false); return; } // Copy map data given proper transformations @@ -393,6 +413,7 @@ StaticLayer::updateCosts( } } } + update_in_progress_.store(false); } } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/observation_buffer.cpp b/nav2_costmap_2d/src/observation_buffer.cpp index f87d59bc25f..c0086ebb9bc 100644 --- a/nav2_costmap_2d/src/observation_buffer.cpp +++ b/nav2_costmap_2d/src/observation_buffer.cpp @@ -234,7 +234,7 @@ void ObservationBuffer::purgeStaleObservations() Observation & obs = *obs_it; // check if the observation is out of date... and if it is, // remove it and those that follow from the list - if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_) { + if ((nh_->now() - obs.cloud_->header.stamp) > observation_keep_time_) { observation_list_.erase(obs_it, observation_list_.end()); return; } diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 4ded71cb7eb..2da4b44a52c 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -99,20 +99,6 @@ class DWBLocalPlanner : public nav2_core::Controller const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity) override; - /** - * @brief nav2_core isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. - * - * The pose that it checks against is the last pose in the current global plan. - * The calculation is delegated to the goal_checker plugin. - * - * @param pose Current pose - * @param velocity Current velocity - * @return True if the robot should be considered as having reached the goal. - */ - bool isGoalReached( - const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) override; - /** * @brief Score a given command. Can be used for testing. * @@ -210,9 +196,6 @@ class DWBLocalPlanner : public nav2_core::Controller pluginlib::ClassLoader traj_gen_loader_; TrajectoryGenerator::Ptr traj_generator_; - pluginlib::ClassLoader goal_checker_loader_; - nav2_core::GoalChecker::Ptr goal_checker_; - pluginlib::ClassLoader critic_loader_; std::vector critics_; diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 011ea530422..8c7041bcb72 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -59,7 +59,6 @@ namespace dwb_core DWBLocalPlanner::DWBLocalPlanner() : traj_gen_loader_("dwb_core", "dwb_core::TrajectoryGenerator"), - goal_checker_loader_("dwb_core", "nav2_core::GoalChecker"), critic_loader_("dwb_core", "dwb_core::TrajectoryCritic") { } @@ -87,9 +86,6 @@ void DWBLocalPlanner::configure( declare_parameter_if_not_declared( node_, dwb_plugin_name_ + ".trajectory_generator_name", rclcpp::ParameterValue(std::string("dwb_plugins::StandardTrajectoryGenerator"))); - declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".goal_checker_name", - rclcpp::ParameterValue(std::string("dwb_plugins::SimpleGoalChecker"))); declare_parameter_if_not_declared( node_, dwb_plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); @@ -98,7 +94,6 @@ void DWBLocalPlanner::configure( rclcpp::ParameterValue(true)); std::string traj_generator_name; - std::string goal_checker_name; double transform_tolerance; node_->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance); @@ -109,7 +104,6 @@ void DWBLocalPlanner::configure( node_->get_parameter(dwb_plugin_name_ + ".prune_distance", prune_distance_); node_->get_parameter(dwb_plugin_name_ + ".debug_trajectory_details", debug_trajectory_details_); node_->get_parameter(dwb_plugin_name_ + ".trajectory_generator_name", traj_generator_name); - node_->get_parameter(dwb_plugin_name_ + ".goal_checker_name", goal_checker_name); node_->get_parameter( dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", short_circuit_trajectory_evaluation_); @@ -118,10 +112,8 @@ void DWBLocalPlanner::configure( pub_->on_configure(); traj_generator_ = traj_gen_loader_.createUniqueInstance(traj_generator_name); - goal_checker_ = goal_checker_loader_.createUniqueInstance(goal_checker_name); traj_generator_->initialize(node_, dwb_plugin_name_); - goal_checker_->initialize(node_, dwb_plugin_name_); try { loadCritics(); @@ -149,7 +141,6 @@ DWBLocalPlanner::cleanup() pub_->on_cleanup(); traj_generator_.reset(); - goal_checker_.reset(); } std::string @@ -263,42 +254,6 @@ DWBLocalPlanner::loadBackwardsCompatibleParameters() /* *INDENT-ON* */ } -bool -DWBLocalPlanner::isGoalReached( - const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) -{ - if (global_plan_.poses.size() == 0) { - RCLCPP_WARN( - rclcpp::get_logger( - "DWBLocalPlanner"), "Cannot check if the goal is reached without the goal being set!"); - return false; - } - nav_2d_msgs::msg::Pose2DStamped local_start_pose2d, goal_pose2d, local_goal_pose2d; - - nav_2d_utils::transformPose( - tf_, costmap_ros_->getGlobalFrameID(), - nav_2d_utils::poseStampedToPose2D(pose), - local_start_pose2d, transform_tolerance_); - - goal_pose2d.header.frame_id = global_plan_.header.frame_id; - goal_pose2d.pose = global_plan_.poses.back(); - - nav_2d_utils::transformPose( - tf_, costmap_ros_->getGlobalFrameID(), goal_pose2d, - local_goal_pose2d, transform_tolerance_); - - geometry_msgs::msg::PoseStamped local_start_pose, local_goal_pose; - local_start_pose = nav_2d_utils::pose2DToPoseStamped(local_start_pose2d); - local_goal_pose = nav_2d_utils::pose2DToPoseStamped(local_goal_pose2d); - - bool ret = goal_checker_->isGoalReached(local_start_pose.pose, local_goal_pose.pose, velocity); - if (ret) { - RCLCPP_INFO(rclcpp::get_logger("DWBLocalPlanner"), "Goal reached!"); - } - return ret; -} - void DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) { @@ -308,7 +263,6 @@ DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) } traj_generator_->reset(); - goal_checker_->reset(); pub_->publishGlobalPlan(path2d); global_plan_ = path2d; diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt index ea4d7868eaa..6e54ad061e3 100644 --- a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt @@ -27,19 +27,6 @@ include_directories( include ) -add_library(simple_goal_checker SHARED src/simple_goal_checker.cpp) -ament_target_dependencies(simple_goal_checker ${dependencies}) -# prevent pluginlib from using boost -target_compile_definitions(simple_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - - -add_library(stopped_goal_checker SHARED src/stopped_goal_checker.cpp) -target_link_libraries(stopped_goal_checker simple_goal_checker) -ament_target_dependencies(stopped_goal_checker ${dependencies}) -# prevent pluginlib from using boost -target_compile_definitions(stopped_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - - add_library(standard_traj_generator SHARED src/standard_traj_generator.cpp src/limited_accel_generator.cpp @@ -60,7 +47,7 @@ if(BUILD_TESTING) add_subdirectory(test) endif() -install(TARGETS simple_goal_checker stopped_goal_checker standard_traj_generator +install(TARGETS standard_traj_generator ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -73,8 +60,6 @@ install(FILES plugins.xml ) ament_export_include_directories(include) -ament_export_libraries(simple_goal_checker) -ament_export_libraries(stopped_goal_checker) ament_export_libraries(standard_traj_generator) pluginlib_export_plugin_description_file(dwb_core plugins.xml) diff --git a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt index 272f38cf4ca..29845a84998 100644 --- a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt @@ -1,7 +1,4 @@ ament_add_gtest(vtest velocity_iterator_test.cpp) -ament_add_gtest(goal_checker goal_checker.cpp) -target_link_libraries(goal_checker simple_goal_checker stopped_goal_checker) - ament_add_gtest(twist_gen_test twist_gen.cpp) target_link_libraries(twist_gen_test standard_traj_generator) diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index 8cb747d453a..618d318935c 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -88,7 +88,7 @@ LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout) auto future_result = is_active_client_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { return SystemStatus::TIMEOUT; } diff --git a/nav2_map_server/include/nav2_map_server/map_saver.hpp b/nav2_map_server/include/nav2_map_server/map_saver.hpp index 2c096e3dba4..ef3b7849603 100644 --- a/nav2_map_server/include/nav2_map_server/map_saver.hpp +++ b/nav2_map_server/include/nav2_map_server/map_saver.hpp @@ -109,6 +109,8 @@ class MapSaver : public nav2_util::LifecycleNode // Default values for map thresholds double free_thresh_default_; double occupied_thresh_default_; + // param for handling QoS configuration + bool map_subscribe_transient_local_; // The name of the service for saving a map from topic const std::string save_map_service_name_{"save_map"}; diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 001e2ed28b6..34107978afe 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -50,6 +50,7 @@ MapSaver::MapSaver() free_thresh_default_ = declare_parameter("free_thresh_default", 0.25), occupied_thresh_default_ = declare_parameter("occupied_thresh_default", 0.65); + map_subscribe_transient_local_ = declare_parameter("map_subscribe_transient_local", true); } MapSaver::~MapSaver() @@ -180,8 +181,14 @@ bool MapSaver::saveMapTopicToFile( // Add new subscription for incoming map topic. // Utilizing local rclcpp::Node (rclcpp_node_) from nav2_util::LifecycleNode // as a map listener. + rclcpp::QoS map_qos(10); // initialize to default + if (map_subscribe_transient_local_) { + map_qos.transient_local(); + map_qos.reliable(); + map_qos.keep_last(1); + } auto map_sub = rclcpp_node_->create_subscription( - map_topic_loc, rclcpp::SystemDefaultsQoS(), mapCallback); + map_topic_loc, map_qos, mapCallback); rclcpp::Time start_time = now(); while (rclcpp::ok()) { diff --git a/nav2_recoveries/test/test_recoveries.cpp b/nav2_recoveries/test/test_recoveries.cpp index a838803f87b..6f5c83912ed 100644 --- a/nav2_recoveries/test/test_recoveries.cpp +++ b/nav2_recoveries/test/test_recoveries.cpp @@ -157,7 +157,7 @@ class RecoveryTest : public ::testing::Test auto future_goal = client_->async_send_goal(goal); if (rclcpp::spin_until_future_complete(node_lifecycle_, future_goal) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { std::cout << "failed sending goal" << std::endl; // failed sending the goal diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 9d1b3156834..5d2b5147fc8 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -410,7 +410,7 @@ Nav2Panel::onCancelButtonPressed() waypoint_follower_action_client_->async_cancel_goal(waypoint_follower_goal_handle_); if (rclcpp::spin_until_future_complete(client_node_, future_cancel) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel waypoint follower"); return; @@ -419,7 +419,7 @@ Nav2Panel::onCancelButtonPressed() auto future_cancel = navigation_action_client_->async_cancel_goal(navigation_goal_handle_); if (rclcpp::spin_until_future_complete(client_node_, future_cancel) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); return; @@ -523,7 +523,7 @@ Nav2Panel::startWaypointFollowing(std::vector p auto future_goal_handle = waypoint_follower_action_client_->async_send_goal(waypoint_follower_goal_, send_goal_options); if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); return; @@ -563,7 +563,7 @@ Nav2Panel::startNavigation(geometry_msgs::msg::PoseStamped pose) auto future_goal_handle = navigation_action_client_->async_send_goal(navigation_goal_, send_goal_options); if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); return; diff --git a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp index 6d87fb1bd28..290deff0317 100644 --- a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp @@ -131,7 +131,7 @@ bool BackupRecoveryTester::defaultBackupRecoveryTest( auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); return false; @@ -148,7 +148,7 @@ bool BackupRecoveryTester::defaultBackupRecoveryTest( RCLCPP_INFO(node_->get_logger(), "Waiting for result"); if (rclcpp::spin_until_future_complete(node_, result_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); return false; diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp index 521c6759559..ee9f6b63bc2 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp @@ -129,7 +129,7 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); return false; @@ -146,7 +146,7 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( RCLCPP_INFO(node_->get_logger(), "Waiting for result"); if (rclcpp::spin_until_future_complete(node_, result_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); return false; diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp index b6bdd62148a..eb980e9a9d1 100644 --- a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp @@ -123,7 +123,7 @@ bool WaitRecoveryTester::recoveryTest( auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); return false; @@ -140,7 +140,7 @@ bool WaitRecoveryTester::recoveryTest( RCLCPP_INFO(node_->get_logger(), "Waiting for result"); if (rclcpp::spin_until_future_complete(node_, result_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); return false; diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 26f0eedf03d..e1eb6983817 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -70,7 +70,7 @@ class ServiceClient auto future_result = client_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { throw std::runtime_error(service_name_ + " service client: async_send_request failed"); } @@ -98,7 +98,7 @@ class ServiceClient auto future_result = client_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, future_result) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { return false; } diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index dff0a3bb1e5..4d3b5511dc8 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -44,10 +44,34 @@ target_link_libraries(test_robot_utils ${library_name}) # This test is disabled due to failing services # https://github.com/ros-planning/navigation2/issues/1836 -# add_launch_test( -# "test_dump_params/test_dump_params.test.py" -# TARGET "test_dump_params" -# TIMEOUT 30 -# ENV -# TEST_EXECUTABLE=$ -# ) +add_launch_test( + "test_dump_params/test_dump_params_default.test.py" + TARGET "test_dump_params_default" + TIMEOUT 10 + ENV + TEST_EXECUTABLE=$ +) + +add_launch_test( + "test_dump_params/test_dump_params_yaml.test.py" + TARGET "test_dump_params_yaml" + TIMEOUT 10 + ENV + TEST_EXECUTABLE=$ +) + +add_launch_test( + "test_dump_params/test_dump_params_md.test.py" + TARGET "test_dump_params_md" + TIMEOUT 10 + ENV + TEST_EXECUTABLE=$ +) + +add_launch_test( + "test_dump_params/test_dump_params_multiple.test.py" + TARGET "test_dump_params_multiple" + TIMEOUT 10 + ENV + TEST_EXECUTABLE=$ +) diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 11d5c54f118..5619d115ebd 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -245,7 +245,7 @@ TEST_F(ActionTest, test_simple_action) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); auto goal_handle = future_goal_handle.get(); @@ -253,7 +253,7 @@ TEST_F(ActionTest, test_simple_action) auto future_result = node_->action_client_->async_get_result(goal_handle); EXPECT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // The final result rclcpp_action::ClientGoalHandle::WrappedResult result = future_result.get(); @@ -293,7 +293,7 @@ TEST_F(ActionTest, test_simple_action_with_feedback) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); auto goal_handle = future_goal_handle.get(); @@ -302,7 +302,7 @@ TEST_F(ActionTest, test_simple_action_with_feedback) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_result), rclcpp::executor::FutureReturnCode::SUCCESS); + future_result), rclcpp::FutureReturnCode::SUCCESS); // The final result rclcpp_action::ClientGoalHandle::WrappedResult result = future_result.get(); @@ -334,7 +334,7 @@ TEST_F(ActionTest, test_simple_action_activation_cycling) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); // Deactivate while running node_->deactivate_server(); @@ -345,7 +345,7 @@ TEST_F(ActionTest, test_simple_action_activation_cycling) auto future_result = node_->action_client_->async_get_result(goal_handle); EXPECT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // The action should be reported as aborted. EXPECT_EQ(future_result.get().code, rclcpp_action::ResultCode::ABORTED); @@ -361,7 +361,7 @@ TEST_F(ActionTest, test_simple_action_activation_cycling) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); goal_handle = future_goal_handle.get(); @@ -370,7 +370,7 @@ TEST_F(ActionTest, test_simple_action_activation_cycling) std::cout << "Getting result, spinning til complete..." << std::endl; EXPECT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // Now the action should have been successfully executed. EXPECT_EQ(future_result.get().code, rclcpp_action::ResultCode::SUCCEEDED); @@ -391,7 +391,7 @@ TEST_F(ActionTest, test_simple_action_preemption) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); // Preempt the goal auto preemption_goal = Fibonacci::Goal(); @@ -403,7 +403,7 @@ TEST_F(ActionTest, test_simple_action_preemption) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); auto goal_handle = future_goal_handle.get(); @@ -412,7 +412,7 @@ TEST_F(ActionTest, test_simple_action_preemption) std::cout << "Getting result, spinning til complete..." << std::endl; EXPECT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // The final result rclcpp_action::ClientGoalHandle::WrappedResult result = future_result.get(); @@ -442,7 +442,7 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); node_->omit_server_preemptions(); @@ -450,7 +450,7 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); // Get the results auto goal_handle = future_goal_handle.get(); @@ -459,7 +459,7 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded) auto future_result = node_->action_client_->async_get_result(goal_handle); EXPECT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // The final result rclcpp_action::ClientGoalHandle::WrappedResult result = future_result.get(); @@ -480,7 +480,7 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded) future_result = node_->action_client_->async_get_result(goal_handle); ASSERT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // The final result result = future_result.get(); @@ -507,7 +507,7 @@ TEST_F(ActionTest, test_handle_goal_deactivated) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); node_->activate_server(); diff --git a/nav2_util/test/test_dump_params/test_dump_params.test.py b/nav2_util/test/test_dump_params/test_dump_params_default.test.py similarity index 68% rename from nav2_util/test/test_dump_params/test_dump_params.test.py rename to nav2_util/test/test_dump_params/test_dump_params_default.test.py index 55d28b5fac4..e0849b398fb 100644 --- a/nav2_util/test/test_dump_params/test_dump_params.test.py +++ b/nav2_util/test/test_dump_params/test_dump_params_default.test.py @@ -37,12 +37,6 @@ def generate_test_description(): cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], name='test_dump_params') ) - launch_description.add_action( - ExecuteProcess( - cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py'), - 'test_dump_params_copy'], - name='test_dump_params_copy') - ) processes_to_test = [ ExecuteProcess( cmd=[os.getenv('TEST_EXECUTABLE'), '-h'], @@ -52,30 +46,6 @@ def generate_test_description(): cmd=[os.getenv('TEST_EXECUTABLE')], name='test_dump_params_default', output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params'], - name='test_dump_params_yaml', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params'], - name='test_dump_params_markdown', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params', '-v'], - name='test_dump_params_yaml_verbose', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params', '-v'], - name='test_dump_params_markdown_verbose', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'error', '-n', 'test_dump_params'], - name='test_dump_params_bad_format', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params,test_dump_params_copy'], - name='test_dump_params_multiple', - output='screen'), ExecuteProcess( cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params_error'], name='test_dump_params_error', @@ -114,12 +84,6 @@ def test_processes_output(self, proc_output, processes_to_test): os.path.join(os.path.dirname(__file__), out) for out in ['dump_params_help', 'dump_params_default', - 'dump_params_yaml', - 'dump_params_md', - 'dump_params_yaml_verbose', - 'dump_params_md_verbose', - 'dump_params_yaml', - 'dump_params_multiple', 'dump_params_error'] ] for process, output_file in zip(processes_to_test[:-1], output_files[:-1]): diff --git a/nav2_util/test/test_dump_params/test_dump_params_md.test.py b/nav2_util/test/test_dump_params/test_dump_params_md.test.py new file mode 100644 index 00000000000..61a85c514cd --- /dev/null +++ b/nav2_util/test/test_dump_params/test_dump_params_md.test.py @@ -0,0 +1,90 @@ +#! /usr/bin/env python3 +# Copyright (c) 2020 Sarthak Mittal +# +# 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 + +import unittest + +from launch import LaunchDescription +from launch.actions import ExecuteProcess + +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.util +import launch_testing_ros + +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + launch_description = LaunchDescription() + launch_description.add_action( + ExecuteProcess( + cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], + name='test_dump_params') + ) + processes_to_test = [ + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params'], + name='test_dump_params_markdown', + output='screen'), + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params', '-v'], + name='test_dump_params_markdown_verbose', + output='screen') + ] + for process in processes_to_test: + launch_description.add_action(process) + launch_description.add_action( + launch_testing.actions.ReadyToTest() + ) + return launch_description, {'processes_to_test': processes_to_test} + + +# Tests without a unittest to run concurrently with +# the processes under test throw an exception +# The following is a dummy test to suppress the traceback +# https://github.com/ros2/launch/issues/380 + +class TestLoggingOutputFormat(unittest.TestCase): + + def test_logging_output(self, proc_info, processes_to_test): + for process_name in processes_to_test: + proc_info.assertWaitForShutdown(process=process_name, timeout=10) + + +@launch_testing.post_shutdown_test() +class TestDumpParams(unittest.TestCase): + + def test_processes_output(self, proc_output, processes_to_test): + """Test all processes output against expectations.""" + from launch_testing.tools.output import get_default_filtered_prefixes + output_filter = launch_testing_ros.tools.basic_output_filter( + filtered_prefixes=get_default_filtered_prefixes() + ) + output_files = [ + os.path.join(os.path.dirname(__file__), out) + for out in ['dump_params_md', + 'dump_params_md_verbose'] + ] + for process, output_file in zip(processes_to_test, output_files): + launch_testing.asserts.assertInStdout( + proc_output, + expected_output=launch_testing.tools.expected_output_from_file( + path=output_file), + process=process, output_filter=output_filter + ) diff --git a/nav2_util/test/test_dump_params/test_dump_params_multiple.test.py b/nav2_util/test/test_dump_params/test_dump_params_multiple.test.py new file mode 100644 index 00000000000..4733c19f584 --- /dev/null +++ b/nav2_util/test/test_dump_params/test_dump_params_multiple.test.py @@ -0,0 +1,96 @@ +#! /usr/bin/env python3 +# Copyright (c) 2020 Sarthak Mittal +# +# 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 + +import unittest + +from launch import LaunchDescription +from launch.actions import ExecuteProcess + +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.util +import launch_testing_ros + +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + launch_description = LaunchDescription() + launch_description.add_action( + ExecuteProcess( + cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], + name='test_dump_params') + ) + launch_description.add_action( + ExecuteProcess( + cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py'), + 'test_dump_params_copy'], + name='test_dump_params_copy') + ) + processes_to_test = [ + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'error', '-n', 'test_dump_params'], + name='test_dump_params_bad_format', + output='screen'), + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params,test_dump_params_copy'], + name='test_dump_params_multiple', + output='screen') + ] + for process in processes_to_test: + launch_description.add_action(process) + launch_description.add_action( + launch_testing.actions.ReadyToTest() + ) + return launch_description, {'processes_to_test': processes_to_test} + + +# Tests without a unittest to run concurrently with +# the processes under test throw an exception +# The following is a dummy test to suppress the traceback +# https://github.com/ros2/launch/issues/380 + +class TestLoggingOutputFormat(unittest.TestCase): + + def test_logging_output(self, proc_info, processes_to_test): + for process_name in processes_to_test: + proc_info.assertWaitForShutdown(process=process_name, timeout=10) + + +@launch_testing.post_shutdown_test() +class TestDumpParams(unittest.TestCase): + + def test_processes_output(self, proc_output, processes_to_test): + """Test all processes output against expectations.""" + from launch_testing.tools.output import get_default_filtered_prefixes + output_filter = launch_testing_ros.tools.basic_output_filter( + filtered_prefixes=get_default_filtered_prefixes() + ) + output_files = [ + os.path.join(os.path.dirname(__file__), out) + for out in ['dump_params_yaml', + 'dump_params_multiple'] + ] + for process, output_file in zip(processes_to_test[:-1], output_files[:-1]): + launch_testing.asserts.assertInStdout( + proc_output, + expected_output=launch_testing.tools.expected_output_from_file( + path=output_file), + process=process, output_filter=output_filter + ) diff --git a/nav2_util/test/test_dump_params/test_dump_params_yaml.test.py b/nav2_util/test/test_dump_params/test_dump_params_yaml.test.py new file mode 100644 index 00000000000..545bf4934cf --- /dev/null +++ b/nav2_util/test/test_dump_params/test_dump_params_yaml.test.py @@ -0,0 +1,90 @@ +#! /usr/bin/env python3 +# Copyright (c) 2020 Sarthak Mittal +# +# 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 + +import unittest + +from launch import LaunchDescription +from launch.actions import ExecuteProcess + +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.util +import launch_testing_ros + +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + launch_description = LaunchDescription() + launch_description.add_action( + ExecuteProcess( + cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], + name='test_dump_params') + ) + processes_to_test = [ + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params'], + name='test_dump_params_yaml', + output='screen'), + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params', '-v'], + name='test_dump_params_yaml_verbose', + output='screen') + ] + for process in processes_to_test: + launch_description.add_action(process) + launch_description.add_action( + launch_testing.actions.ReadyToTest() + ) + return launch_description, {'processes_to_test': processes_to_test} + + +# Tests without a unittest to run concurrently with +# the processes under test throw an exception +# The following is a dummy test to suppress the traceback +# https://github.com/ros2/launch/issues/380 + +class TestLoggingOutputFormat(unittest.TestCase): + + def test_logging_output(self, proc_info, processes_to_test): + for process_name in processes_to_test: + proc_info.assertWaitForShutdown(process=process_name, timeout=10) + + +@launch_testing.post_shutdown_test() +class TestDumpParams(unittest.TestCase): + + def test_processes_output(self, proc_output, processes_to_test): + """Test all processes output against expectations.""" + from launch_testing.tools.output import get_default_filtered_prefixes + output_filter = launch_testing_ros.tools.basic_output_filter( + filtered_prefixes=get_default_filtered_prefixes() + ) + output_files = [ + os.path.join(os.path.dirname(__file__), out) + for out in ['dump_params_yaml', + 'dump_params_yaml_verbose'] + ] + for process, output_file in zip(processes_to_test, output_files): + launch_testing.asserts.assertInStdout( + proc_output, + expected_output=launch_testing.tools.expected_output_from_file( + path=output_file), + process=process, output_filter=output_filter + )