diff --git a/nav2_bringup/bringup/launch/nav2_bringup_launch.py b/nav2_bringup/bringup/launch/nav2_bringup_launch.py index 156f6b7c42fdaaa9167d686e31aa6fb9be022168..e487788826c5080a6eb4401952d71d1ea78443d1 100644 --- a/nav2_bringup/bringup/launch/nav2_bringup_launch.py +++ b/nav2_bringup/bringup/launch/nav2_bringup_launch.py @@ -119,6 +119,7 @@ def generate_launch_description(): 'amcl', 'controller_server', 'planner_server', + 'recoveries_server', 'bt_navigator']}]) # Create the launch description and populate diff --git a/nav2_bringup/bringup/launch/nav2_navigation_launch.py b/nav2_bringup/bringup/launch/nav2_navigation_launch.py index 53c01f2ca97a81acf6a14f21a6a60dfe1833e6cf..4f8f2194b07d5ecc83af1895638f579819b5bd13 100644 --- a/nav2_bringup/bringup/launch/nav2_navigation_launch.py +++ b/nav2_bringup/bringup/launch/nav2_navigation_launch.py @@ -120,8 +120,8 @@ def generate_launch_description(): Node( package='nav2_recoveries', - node_executable='recoveries_node', - node_name='recoveries', + node_executable='recoveries_server', + node_name='recoveries_server', output='screen', parameters=[{'use_sim_time': use_sim_time}], use_remappings=IfCondition(use_remappings), @@ -145,7 +145,8 @@ def generate_launch_description(): parameters=[{'use_sim_time': use_sim_time}, {'autostart': autostart}, {'node_names': ['controller_server', - 'navfn_planner', + 'planner_server', + 'recoveries_server', 'bt_navigator']}]), ]) diff --git a/nav2_core/include/nav2_core/global_planner.hpp b/nav2_core/include/nav2_core/global_planner.hpp index c98d01a4be91487d1e219cbe8deec99302bdb7d5..7245e6aedc0eb7e9058825d2f80b9712d5b4a124 100644 --- a/nav2_core/include/nav2_core/global_planner.hpp +++ b/nav2_core/include/nav2_core/global_planner.hpp @@ -49,7 +49,7 @@ public: */ virtual void configure( rclcpp_lifecycle::LifecycleNode::SharedPtr parent, - std::string name, tf2_ros::Buffer * tf, + std::string name, std::shared_ptr<tf2_ros::Buffer> tf, std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) = 0; /** diff --git a/nav2_core/include/nav2_core/recovery.hpp b/nav2_core/include/nav2_core/recovery.hpp index 0b936ea95b015cd1248021e62d352749a2719939..b3a34bf2d355b48c6f9e7bf12b642de3f927a2ae 100644 --- a/nav2_core/include/nav2_core/recovery.hpp +++ b/nav2_core/include/nav2_core/recovery.hpp @@ -16,6 +16,8 @@ #define NAV2_CORE__RECOVERY_HPP_ #include <string> +#include <memory> + #include "rclcpp/rclcpp.hpp" #include "nav2_util/lifecycle_node.hpp" #include "tf2_ros/buffer.h" @@ -30,6 +32,8 @@ namespace nav2_core class Recovery { public: + using Ptr = std::shared_ptr<Recovery>; + /** * @brief Virtual destructor */ @@ -42,8 +46,8 @@ public: * @param costmap_ros A pointer to the costmap */ virtual void configure( - const nav2_util::LifecycleNode * parent, - const std::string & name, tf2_ros::Buffer * tf) = 0; + const rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf) = 0; /** * @brief Method to cleanup resources used on shutdown. @@ -65,7 +69,9 @@ public: * @param name The name of this planner * @return true if successful, false is failed to execute fully */ - virtual bool executeRecovery() = 0; + // TODO(stevemacenski) evaluate design for recoveries to not host + // their own servers and utilize a recovery server exposed action. + // virtual bool executeRecovery() = 0; }; } // namespace nav2_core diff --git a/nav2_navfn_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt index 57fb09bdc9fafb31974e9a14674d1670bb0bcd4c..a467c43879ca56456c6ee876ed4f56179bf0ad16 100644 --- a/nav2_navfn_planner/CMakeLists.txt +++ b/nav2_navfn_planner/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(geometry_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(tf2_ros REQUIRED) find_package(nav2_costmap_2d REQUIRED) +find_package(pluginlib REQUIRED) nav2_package() @@ -39,6 +40,7 @@ set(dependencies tf2_ros nav2_costmap_2d nav2_core + pluginlib ) add_library(${library_name} SHARED diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index a9a57d025988a6d238d85aa5ad05b1b06da96311..e6637ce4a369202984d8ebd94af1046ed8c77b1e 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -43,7 +43,7 @@ public: // plugin configure void configure( rclcpp_lifecycle::LifecycleNode::SharedPtr parent, - std::string name, tf2_ros::Buffer * tf, + std::string name, std::shared_ptr<tf2_ros::Buffer> tf, std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override; // plugin cleanup @@ -117,7 +117,7 @@ protected: std::unique_ptr<NavFn> planner_; // TF buffer - tf2_ros::Buffer * tf_; + std::shared_ptr<tf2_ros::Buffer> tf_; // node ptr nav2_util::LifecycleNode::SharedPtr node_; diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index d37ae7d3fd95e0568a1d3a16bc4947f1aefc28f3..a41e583847be568d3709af05ae25d684b33017a1 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -24,6 +24,7 @@ <depend>tf2_ros</depend> <depend>nav2_costmap_2d</depend> <depend>nav2_core</depend> + <depend>pluginlib</depend> <test_depend>ament_lint_common</test_depend> <test_depend>ament_lint_auto</test_depend> diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index fb5ab8137cb07bc1e6b63324ce7334c77c968f08..7bd951ec41244daeb633f06aafbc40b3b6defc12 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -55,7 +55,7 @@ NavfnPlanner::~NavfnPlanner() void NavfnPlanner::configure( rclcpp_lifecycle::LifecycleNode::SharedPtr parent, - std::string name, tf2_ros::Buffer * tf, + std::string name, std::shared_ptr<tf2_ros::Buffer> tf, std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) { node_ = parent; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 09796490c220e83dec389d24322c13d3843a54dc..3345aa7bf7fa77dfc01f619cbece98ece0c559d3 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -87,7 +87,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_INFO(get_logger(), "Created global planner plugin %s", planner_plugin_name_.c_str()); planner_->configure(node, - gp_loader_.getName(planner_plugin_name_), tf_.get(), costmap_ros_); + gp_loader_.getName(planner_plugin_name_), tf_, costmap_ros_); } catch (const pluginlib::PluginlibException & ex) { RCLCPP_FATAL(get_logger(), "Failed to create global planner. Exception: %s", ex.what()); diff --git a/nav2_recoveries/CMakeLists.txt b/nav2_recoveries/CMakeLists.txt index c70d9c5cc3bdf1bb2db10c6bb3e972d7f3e32ff9..7d85dcf70c2c974e3374a2e39d1dc8e948020793 100644 --- a/nav2_recoveries/CMakeLists.txt +++ b/nav2_recoveries/CMakeLists.txt @@ -15,6 +15,8 @@ find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_core REQUIRED) +find_package(pluginlib REQUIRED) nav2_package() @@ -22,8 +24,8 @@ include_directories( include ) -set(library_name recoveries) -set(executable_name ${library_name}_node) +set(library_name recoveries_server_core) +set(executable_name recoveries_server) set(dependencies rclcpp @@ -38,12 +40,32 @@ set(dependencies tf2_geometry_msgs geometry_msgs nav2_costmap_2d + nav2_core + pluginlib ) +# plugins +add_library(nav2_spin_recovery SHARED + plugins/spin.cpp +) + +ament_target_dependencies(nav2_spin_recovery + ${dependencies} +) + +add_library(nav2_backup_recovery SHARED + plugins/back_up.cpp +) + +ament_target_dependencies(nav2_backup_recovery + ${dependencies} +) + +pluginlib_export_plugin_description_file(nav2_core recovery_plugin.xml) + # Library add_library(${library_name} - src/spin.cpp - src/back_up.cpp + src/recovery_server.cpp ) ament_target_dependencies(${library_name} @@ -61,7 +83,10 @@ ament_target_dependencies(${executable_name} ${dependencies} ) -install(TARGETS ${executable_name} ${library_name} +install(TARGETS ${executable_name} + ${library_name} + nav2_backup_recovery + nav2_spin_recovery ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} @@ -71,6 +96,10 @@ install(DIRECTORY include/ DESTINATION include/ ) +install(FILES recovery_plugin.xml + DESTINATION share +) + if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) find_package(ament_cmake_gtest REQUIRED) @@ -80,6 +109,6 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(${library_name}) +ament_export_libraries(${library_name} nav2_backup_recovery nav2_spin_recovery) ament_package() diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 359903ab5f3087fe31c51cf45a7a8611404dc0cb..3ff71d29ce9b6161e19b99ef1084be2770a8eb58 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp @@ -29,6 +29,7 @@ #include "nav2_costmap_2d/collision_checker.hpp" #include "nav2_util/simple_action_server.hpp" #include "nav2_util/robot_utils.hpp" +#include "nav2_core/recovery.hpp" namespace nav2_recoveries { @@ -43,26 +44,20 @@ enum class Status : int8_t using namespace std::chrono_literals; //NOLINT template<typename ActionT> -class Recovery +class Recovery : public nav2_core::Recovery { public: - using ActionServer = nav2_util::SimpleActionServer<ActionT>; - - explicit Recovery( - rclcpp::Node::SharedPtr & node, const std::string & recovery_name, - std::shared_ptr<tf2_ros::Buffer> tf) - : node_(node), - recovery_name_(recovery_name), - tf_(*tf), - action_server_(nullptr), - cycle_frequency_(10) + using ActionServer = nav2_util::SimpleActionServer<ActionT, rclcpp_lifecycle::LifecycleNode>; + + Recovery() + : action_server_(nullptr), + cycle_frequency_(10), + enabled_(false) { - configure(); } virtual ~Recovery() { - cleanup(); } // Derived classes can override this method to catch the command and perform some checks @@ -78,21 +73,27 @@ public: // It's up to the derived class to define the final commanded velocity. virtual Status onCycleUpdate() = 0; -protected: - rclcpp::Node::SharedPtr node_; - std::string recovery_name_; - rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_; - tf2_ros::Buffer & tf_; - std::unique_ptr<ActionServer> action_server_; + // an opportunity for derived classes to do something on configuration + // if they chose + virtual void onConfigure() + { + } - std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_; - std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_; - std::unique_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_; - double cycle_frequency_; + // an opportunity for derived classes to do something on cleanup + // if they chose + virtual void onCleanup() + { + } - void configure() + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf) override { - RCLCPP_INFO(node_->get_logger(), "Configuring %s", recovery_name_.c_str()); + RCLCPP_INFO(parent->get_logger(), "Configuring %s", name.c_str()); + + node_ = parent; + tf_ = tf; + recovery_name_ = name; std::string costmap_topic; std::string footprint_topic; @@ -110,24 +111,55 @@ protected: node_, footprint_topic); collision_checker_ = std::make_unique<nav2_costmap_2d::CollisionChecker>( - *costmap_sub_, *footprint_sub_, tf_, node_->get_name(), "odom"); + *costmap_sub_, *footprint_sub_, *tf_, node_->get_name(), "odom"); vel_pub_ = node_->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1); + + onConfigure(); } - void cleanup() + void cleanup() override { action_server_.reset(); vel_pub_.reset(); footprint_sub_.reset(); costmap_sub_.reset(); collision_checker_.reset(); + onCleanup(); + } + + void activate() override + { + enabled_ = true; } + void deactivate() override + { + enabled_ = false; + } + +protected: + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + std::string recovery_name_; + rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_; + std::shared_ptr<tf2_ros::Buffer> tf_; + std::unique_ptr<ActionServer> action_server_; + + std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_; + std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_; + std::unique_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_; + double cycle_frequency_; + double enabled_; + void execute() { RCLCPP_INFO(node_->get_logger(), "Attempting %s", recovery_name_.c_str()); + if (!enabled_) { + RCLCPP_WARN(node_->get_logger(), "Called while inactive, ignoring request."); + return; + } + if (onRun(action_server_->get_current_goal()) != Status::SUCCEEDED) { RCLCPP_INFO(node_->get_logger(), "Initial checks failed for %s", recovery_name_.c_str()); action_server_->terminate_goals(); diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp new file mode 100644 index 0000000000000000000000000000000000000000..a8503ca202a48d9c9fe1a8d556354a9ce122028d --- /dev/null +++ b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp @@ -0,0 +1,62 @@ +// Copyright (c) 2018 Samsung Research America +// +// 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. Reserved. + + +#include <chrono> +#include <string> +#include <memory> +#include <vector> + +#include "nav2_util/lifecycle_node.hpp" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/create_timer_ros.h" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_core/recovery.hpp" + +#ifndef NAV2_RECOVERIES__RECOVERY_SERVER_HPP_ +#define NAV2_RECOVERIES__RECOVERY_SERVER_HPP_ + +namespace recovery_server +{ + +class RecoveryServer : public nav2_util::LifecycleNode +{ +public: + RecoveryServer(); + ~RecoveryServer(); + + void loadRecoveryPlugins(); + +protected: + // Implement the lifecycle interface + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; + + std::shared_ptr<tf2_ros::Buffer> tf_; + std::shared_ptr<tf2_ros::TransformListener> transform_listener_; + + // Plugins + std::vector<pluginlib::UniquePtr<nav2_core::Recovery>> recoveries_; + pluginlib::ClassLoader<nav2_core::Recovery> plugin_loader_; + std::vector<std::string> plugin_names_, plugin_types_; +}; + +} // namespace recovery_server + +#endif // NAV2_RECOVERIES__RECOVERY_SERVER_HPP_ diff --git a/nav2_recoveries/package.xml b/nav2_recoveries/package.xml index 442540188686ca301123f7b8c192d3d26906cbe7..30b13b5d2ab3ba3f3a0a3174e5c217179755a978 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_recoveries/package.xml @@ -22,6 +22,8 @@ <build_depend>tf2_geometry_msgs</build_depend> <build_depend>geometry_msgs</build_depend> <build_depend>nav2_costmap_2d</build_depend> + <build_depend>nav2_core</build_depend> + <build_depend>pluginlib</build_depend> <exec_depend>rclcpp</exec_depend> <exec_depend>rclcpp_action</exec_depend> @@ -32,6 +34,8 @@ <exec_depend>nav_msgs</exec_depend> <exec_depend>geometry_msgs</exec_depend> <exec_depend>nav2_costmap_2d</exec_depend> + <exec_depend>nav2_core</exec_depend> + <exec_depend>pluginlib</exec_depend> <test_depend>ament_lint_common</test_depend> <test_depend>ament_lint_auto</test_depend> @@ -42,5 +46,6 @@ <export> <build_type>ament_cmake</build_type> + <nav2_core plugin="${prefix}/recovery_plugin.xml" /> </export> </package> diff --git a/nav2_recoveries/src/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp similarity index 86% rename from nav2_recoveries/src/back_up.cpp rename to nav2_recoveries/plugins/back_up.cpp index ec1506b819782a7c47d9d3e0b16a9ae7ab815892..b66ad9b2ee15dfba8802dcd2000f0d4811d78ca3 100644 --- a/nav2_recoveries/src/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -16,25 +16,29 @@ #include <ctime> #include <memory> -#include "nav2_recoveries/back_up.hpp" +#include "back_up.hpp" using namespace std::chrono_literals; namespace nav2_recoveries { -BackUp::BackUp(rclcpp::Node::SharedPtr & node, std::shared_ptr<tf2_ros::Buffer> tf) -: Recovery<BackUpAction>(node, "BackUp", tf) +BackUp::BackUp() +: Recovery<BackUpAction>() { simulate_ahead_time_ = 2.0; - node_->declare_parameter("simulate_ahead_time"); - node_->get_parameter("simulate_ahead_time", simulate_ahead_time_); } BackUp::~BackUp() { } +void BackUp::onConfigure() +{ + node_->declare_parameter("simulate_ahead_time", simulate_ahead_time_); + simulate_ahead_time_ = node_->get_parameter("simulate_ahead_time").as_double(); +} + Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command) { if (command->target.y != 0.0 || command->target.z != 0.0) { @@ -44,7 +48,7 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command) command_x_ = command->target.x; - if (!nav2_util::getCurrentPose(initial_pose_, tf_, "odom")) { + if (!nav2_util::getCurrentPose(initial_pose_, *tf_, "odom")) { RCLCPP_ERROR(node_->get_logger(), "Initial robot pose is not available."); return Status::FAILED; } @@ -55,7 +59,7 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command) Status BackUp::onCycleUpdate() { geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, tf_, "odom")) { + if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom")) { RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); return Status::FAILED; } @@ -118,3 +122,6 @@ bool BackUp::isCollisionFree( } } // namespace nav2_recoveries + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_recoveries::BackUp, nav2_core::Recovery) diff --git a/nav2_recoveries/include/nav2_recoveries/back_up.hpp b/nav2_recoveries/plugins/back_up.hpp similarity index 86% rename from nav2_recoveries/include/nav2_recoveries/back_up.hpp rename to nav2_recoveries/plugins/back_up.hpp index 1a144ee24747325b3a701947d38092a6653ed430..bef5134304957ed92d41f98cb92c220076a41d35 100644 --- a/nav2_recoveries/include/nav2_recoveries/back_up.hpp +++ b/nav2_recoveries/plugins/back_up.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_RECOVERIES__BACK_UP_HPP_ -#define NAV2_RECOVERIES__BACK_UP_HPP_ +#ifndef NAV2_RECOVERIES__PLUGINS__BACK_UP_HPP_ +#define NAV2_RECOVERIES__PLUGINS__BACK_UP_HPP_ #include <chrono> #include <memory> @@ -28,7 +28,7 @@ using BackUpAction = nav2_msgs::action::BackUp; class BackUp : public Recovery<BackUpAction> { public: - explicit BackUp(rclcpp::Node::SharedPtr & node, std::shared_ptr<tf2_ros::Buffer> tf); + BackUp(); ~BackUp(); Status onRun(const std::shared_ptr<const BackUpAction::Goal> command) override; @@ -41,6 +41,8 @@ protected: const geometry_msgs::msg::Twist & cmd_vel, geometry_msgs::msg::Pose2D & pose2d); + void onConfigure() override; + double min_linear_vel_; double max_linear_vel_; double linear_acc_lim_; @@ -52,4 +54,4 @@ protected: } // namespace nav2_recoveries -#endif // NAV2_RECOVERIES__BACK_UP_HPP_ +#endif // NAV2_RECOVERIES__PLUGINS__BACK_UP_HPP_ diff --git a/nav2_recoveries/src/spin.cpp b/nav2_recoveries/plugins/spin.cpp similarity index 92% rename from nav2_recoveries/src/spin.cpp rename to nav2_recoveries/plugins/spin.cpp index 540fe21e222ad092b60a1655e304da9a158df53b..931376a6fa8cd8cfeec1748a18df946d58d720ad 100644 --- a/nav2_recoveries/src/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -19,7 +19,7 @@ #include <algorithm> #include <memory> -#include "nav2_recoveries/spin.hpp" +#include "spin.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" #include "tf2/utils.h" @@ -32,8 +32,8 @@ using namespace std::chrono_literals; namespace nav2_recoveries { -Spin::Spin(rclcpp::Node::SharedPtr & node, std::shared_ptr<tf2_ros::Buffer> tf) -: Recovery<SpinAction>(node, "Spin", tf) +Spin::Spin() +: Recovery<SpinAction>() { // TODO(orduno) #378 Pull values from the robot max_rotational_vel_ = 1.0; @@ -50,10 +50,11 @@ Spin::~Spin() Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> command) { geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, tf_, "odom")) { + if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom")) { RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); return Status::FAILED; } + initial_yaw_ = tf2::getYaw(current_pose.pose.orientation); cmd_yaw_ = -command->target_yaw; @@ -65,7 +66,7 @@ Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> command) Status Spin::onCycleUpdate() { geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, tf_, "odom")) { + if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom")) { RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); return Status::FAILED; } @@ -131,3 +132,6 @@ bool Spin::isCollisionFree( } } // namespace nav2_recoveries + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_recoveries::Spin, nav2_core::Recovery) diff --git a/nav2_recoveries/include/nav2_recoveries/spin.hpp b/nav2_recoveries/plugins/spin.hpp similarity index 87% rename from nav2_recoveries/include/nav2_recoveries/spin.hpp rename to nav2_recoveries/plugins/spin.hpp index 5249158eee9e841736ae44b6cdb131ca3e94d78d..a24d5cb3b495af3eb1d016083b3cf19d37c03c8e 100644 --- a/nav2_recoveries/include/nav2_recoveries/spin.hpp +++ b/nav2_recoveries/plugins/spin.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_RECOVERIES__SPIN_HPP_ -#define NAV2_RECOVERIES__SPIN_HPP_ +#ifndef NAV2_RECOVERIES__PLUGINS__SPIN_HPP_ +#define NAV2_RECOVERIES__PLUGINS__SPIN_HPP_ #include <chrono> #include <string> @@ -30,7 +30,7 @@ using SpinAction = nav2_msgs::action::Spin; class Spin : public Recovery<SpinAction> { public: - explicit Spin(rclcpp::Node::SharedPtr & node, std::shared_ptr<tf2_ros::Buffer> tf); + Spin(); ~Spin(); Status onRun(const std::shared_ptr<const SpinAction::Goal> command) override; @@ -53,4 +53,4 @@ protected: } // namespace nav2_recoveries -#endif // NAV2_RECOVERIES__SPIN_HPP_ +#endif // NAV2_RECOVERIES__PLUGINS__SPIN_HPP_ diff --git a/nav2_recoveries/recovery_plugin.xml b/nav2_recoveries/recovery_plugin.xml new file mode 100644 index 0000000000000000000000000000000000000000..12a9324145cd2448430614537f1f914034f07abb --- /dev/null +++ b/nav2_recoveries/recovery_plugin.xml @@ -0,0 +1,13 @@ +<class_libraries> + <library path="nav2_spin_recovery"> + <class name="nav2_recoveries/Spin" type="nav2_recoveries::Spin" base_class_type="nav2_core::Recovery"> + <description></description> + </class> + </library> + + <library path="nav2_backup_recovery"> + <class name="nav2_recoveries/BackUp" type="nav2_recoveries::BackUp" base_class_type="nav2_core::Recovery"> + <description></description> + </class> + </library> +</class_libraries> \ No newline at end of file diff --git a/nav2_recoveries/src/main.cpp b/nav2_recoveries/src/main.cpp index 00423773e098e8593c0bc3dd91acf27d67f6edd0..23dea6b90f71ef227d3e2979e74a298f363de2d0 100644 --- a/nav2_recoveries/src/main.cpp +++ b/nav2_recoveries/src/main.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2018 Intel Corporation +// Copyright (c) 2019 Samsung Research America // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,37 +16,15 @@ #include <string> #include <memory> +#include "nav2_recoveries/recovery_server.hpp" #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" -#include "nav2_recoveries/back_up.hpp" -#include "nav2_recoveries/spin.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); + auto recoveries_node = std::make_shared<recovery_server::RecoveryServer>(); - auto recoveries_node = rclcpp::Node::make_shared("recoveries"); - - auto tf_buffer = std::make_shared<tf2_ros::Buffer>(recoveries_node->get_clock()); - auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>( - recoveries_node->get_node_base_interface(), - recoveries_node->get_node_timers_interface()); - tf_buffer->setCreateTimerInterface(timer_interface); - auto tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer); - - recoveries_node->declare_parameter( - "costmap_topic", rclcpp::ParameterValue(std::string("local_costmap/costmap_raw"))); - recoveries_node->declare_parameter( - "footprint_topic", rclcpp::ParameterValue(std::string("local_costmap/published_footprint"))); - - auto spin = std::make_shared<nav2_recoveries::Spin>( - recoveries_node, tf_buffer); - - auto back_up = std::make_shared<nav2_recoveries::BackUp>( - recoveries_node, tf_buffer); - - rclcpp::spin(recoveries_node); + rclcpp::spin(recoveries_node->get_node_base_interface()); rclcpp::shutdown(); return 0; diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp new file mode 100644 index 0000000000000000000000000000000000000000..95d5d7a35b8ad5622386942aca3a76785e36170f --- /dev/null +++ b/nav2_recoveries/src/recovery_server.cpp @@ -0,0 +1,143 @@ +// Copyright (c) 2018 Samsung Research America +// +// 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. Reserved. + +#include <memory> +#include <string> +#include <vector> +#include <utility> +#include "nav2_recoveries/recovery_server.hpp" + +namespace recovery_server +{ + +RecoveryServer::RecoveryServer() +: nav2_util::LifecycleNode("nav2_recoveries", "", true), + plugin_loader_("nav2_core", "nav2_core::Recovery") +{ +} + + +RecoveryServer::~RecoveryServer() +{ +} + +nav2_util::CallbackReturn +RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + tf_ = std::make_shared<tf2_ros::Buffer>(get_clock()); + auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>( + get_node_base_interface(), + get_node_timers_interface()); + tf_->setCreateTimerInterface(timer_interface); + transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_); + + declare_parameter("costmap_topic", + rclcpp::ParameterValue(std::string("local_costmap/costmap_raw"))); + declare_parameter("footprint_topic", + rclcpp::ParameterValue(std::string("local_costmap/published_footprint"))); + + std::vector<std::string> plugin_names{std::string("Spin"), + std::string("BackUp")}; + std::vector<std::string> plugin_types{std::string("nav2_recoveries/Spin"), + std::string("nav2_recoveries/BackUp")}; + + declare_parameter("plugin_names", + rclcpp::ParameterValue(plugin_names)); + declare_parameter("plugin_types", + rclcpp::ParameterValue(plugin_types)); + + plugin_names_ = get_parameter("plugin_names").as_string_array(); + plugin_types_ = get_parameter("plugin_types").as_string_array(); + + loadRecoveryPlugins(); + + return nav2_util::CallbackReturn::SUCCESS; +} + + +void +RecoveryServer::loadRecoveryPlugins() +{ + auto node = shared_from_this(); + + for (uint i = 0; i != plugin_names_.size(); i++) { + try { + RCLCPP_INFO(get_logger(), "Creating recovery plugin %s of type %s", + plugin_names_[i].c_str(), plugin_types_[i].c_str()); + recoveries_.push_back(plugin_loader_.createUniqueInstance(plugin_types_[i])); + recoveries_.back()->configure(node, plugin_names_[i], tf_); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL(get_logger(), "Failed to create recovery %s of type %s." + " Exception: %s", plugin_names_[i].c_str(), plugin_types_[i].c_str(), + ex.what()); + exit(-1); + } + } +} + +nav2_util::CallbackReturn +RecoveryServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + std::vector<pluginlib::UniquePtr<nav2_core::Recovery>>::iterator iter; + for (iter = recoveries_.begin(); iter != recoveries_.end(); ++iter) { + (*iter)->activate(); + } + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +RecoveryServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + std::vector<pluginlib::UniquePtr<nav2_core::Recovery>>::iterator iter; + for (iter = recoveries_.begin(); iter != recoveries_.end(); ++iter) { + (*iter)->deactivate(); + } + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +RecoveryServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + std::vector<pluginlib::UniquePtr<nav2_core::Recovery>>::iterator iter; + for (iter = recoveries_.begin(); iter != recoveries_.end(); ++iter) { + (*iter)->cleanup(); + } + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +RecoveryServer::on_error(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +RecoveryServer::on_shutdown(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2_util::CallbackReturn::SUCCESS; +} + +} // end namespace recovery_server diff --git a/nav2_recoveries/test/test_recoveries.cpp b/nav2_recoveries/test/test_recoveries.cpp index 911f6120e07033b9d6aea36a20f492d635258797..c7e70fce95511ebd96c61034314a49bfef557e7d 100644 --- a/nav2_recoveries/test/test_recoveries.cpp +++ b/nav2_recoveries/test/test_recoveries.cpp @@ -37,8 +37,8 @@ using namespace std::chrono_literals; class DummyRecovery : public Recovery<RecoveryAction> { public: - explicit DummyRecovery(rclcpp::Node::SharedPtr & node, std::shared_ptr<tf2_ros::Buffer> & tf) - : Recovery<RecoveryAction>(node, "Recovery", tf), + DummyRecovery() + : Recovery<RecoveryAction>(), initialized_(false) {} ~DummyRecovery() {} @@ -97,25 +97,35 @@ private: class RecoveryTest : public ::testing::Test { protected: - RecoveryTest() {} + RecoveryTest() {SetUp();} ~RecoveryTest() {} - void SetUp() override + void SetUp() { - node_ = std::make_shared<rclcpp::Node>("RecoveryTestNode"); - auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node_->get_clock()); + node_lifecycle_ = + std::make_shared<rclcpp_lifecycle::LifecycleNode>( + "LifecycleRecoveryTestNode", rclcpp::NodeOptions()); + node_lifecycle_->declare_parameter("costmap_topic", + rclcpp::ParameterValue(std::string("local_costmap/costmap_raw"))); + node_lifecycle_->declare_parameter("footprint_topic", + rclcpp::ParameterValue(std::string("local_costmap/published_footprint"))); + + tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_lifecycle_->get_clock()); auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>( - node_->get_node_base_interface(), - node_->get_node_timers_interface()); - tf_buffer->setCreateTimerInterface(timer_interface); - auto tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer); - node_->declare_parameter( - "costmap_topic", rclcpp::ParameterValue(std::string("local_costmap/costmap_raw"))); - node_->declare_parameter( - "footprint_topic", rclcpp::ParameterValue(std::string("local_costmap/published_footprint"))); - recovery_ = std::make_unique<DummyRecovery>(node_, tf_buffer); - - client_ = rclcpp_action::create_client<RecoveryAction>(node_, "Recovery"); + node_lifecycle_->get_node_base_interface(), + node_lifecycle_->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_); + + recovery_ = std::make_unique<DummyRecovery>(); + recovery_->configure(node_lifecycle_, "Recovery", tf_buffer_); + recovery_->activate(); + + client_ = rclcpp_action::create_client<RecoveryAction>( + node_lifecycle_->get_node_base_interface(), + node_lifecycle_->get_node_graph_interface(), + node_lifecycle_->get_node_logging_interface(), + node_lifecycle_->get_node_waitables_interface(), "Recovery"); } void TearDown() override {} @@ -123,14 +133,16 @@ protected: bool sendCommand(const std::string & command) { if (!client_->wait_for_action_server(4s)) { + std::cout << "Server not up" << std::endl; return false; } auto future_goal = getGoal(command); - if (rclcpp::spin_until_future_complete(node_, future_goal) != + if (rclcpp::spin_until_future_complete(node_lifecycle_, future_goal) != rclcpp::executor::FutureReturnCode::SUCCESS) { + std::cout << "failed sending goal" << std::endl; // failed sending the goal return false; } @@ -138,6 +150,7 @@ protected: goal_handle_ = future_goal.get(); if (!goal_handle_) { + std::cout << "goal was rejected" << std::endl; // goal was rejected by the action server return false; } @@ -171,16 +184,18 @@ protected: rclcpp::executor::FutureReturnCode frc; do { - frc = rclcpp::spin_until_future_complete(node_, future_result); + frc = rclcpp::spin_until_future_complete(node_lifecycle_, future_result); } while (frc != rclcpp::executor::FutureReturnCode::SUCCESS); return future_result.get(); } - std::shared_ptr<rclcpp::Node> node_; + std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_lifecycle_; std::unique_ptr<DummyRecovery> recovery_; std::shared_ptr<rclcpp_action::Client<RecoveryAction>> client_; std::shared_ptr<rclcpp_action::ClientGoalHandle<RecoveryAction>> goal_handle_; + std::shared_ptr<tf2_ros::Buffer> tf_buffer_; + std::shared_ptr<tf2_ros::TransformListener> tf_listener_; }; // Define the tests diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 5453883744b216d1bdd242740af8e9270d7c41b4..766456c4a9a9f9ad931372877979c251947393a0 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -90,8 +90,8 @@ def generate_launch_description(): launch_ros.actions.Node( package='nav2_recoveries', - node_executable='recoveries_node', - node_name='recoveries', + node_executable='recoveries_server', + node_name='recoveries_server', output='screen', parameters=[{'use_sim_time': use_sim_time}]), @@ -108,8 +108,12 @@ def generate_launch_description(): node_name='lifecycle_manager', output='screen', parameters=[{'use_sim_time': use_sim_time}, - {'node_names': ['map_server', 'amcl', - 'controller_server', 'planner_server', 'bt_navigator']}, + {'node_names': ['map_server', + 'amcl', + 'controller_server', + 'planner_server', + 'recoveries_server', + 'bt_navigator']}, {'autostart': True}]), ]) diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index 0a9c0bbb71bcfd27240c28079ce5fda5cc4a2b71..eb1e118b461798e7860965c64ff5e14dd57f0c71 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -25,14 +25,14 @@ namespace nav2_util { -template<typename ActionT> +template<typename ActionT, typename nodeT = rclcpp::Node> class SimpleActionServer { public: typedef std::function<void ()> ExecuteCallback; explicit SimpleActionServer( - rclcpp::Node::SharedPtr node, + typename nodeT::SharedPtr node, const std::string & action_name, ExecuteCallback execute_callback, bool autostart = true) diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index c94764e9a458a306f7c935c2a490a8f3db814981..bc6341f4d062fe93140c03aeab2268edc1ae26b4 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -52,6 +52,9 @@ bool getCurrentPose( } catch (tf2::TimeoutException & ex) { RCLCPP_ERROR(logger, "Transform timeout with tolerance: %.4f", transform_timeout); + } catch (...) { + RCLCPP_ERROR(logger, "Failed to transform from %s to %s", + global_frame.c_str(), robot_frame.c_str()); } return false;