diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index bf5e650a20203b7f2cd77620f76a0795cadef440..f212425ed60cb879f27eb4b722396e4a7cd8cb32 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -541,6 +541,16 @@ When `planner_plugins` parameter is not overridden, the following default plugin | `<waypoint task executor>`.enabled | true | Whether it is enabled | | `<waypoint task executor>`.waypoint_pause_duration | 0 | Amount of time in milliseconds, for robot to sleep/wait after each waypoint is reached. If zero, robot will directly continue to next waypoint. | +## InputAtWaypoint plugin + +* `<waypoint task executor>`: Name corresponding to the `nav2_waypoint_follower::WaitAtWaypoint` plugin. + +| Parameter | Default | Description | +| ----------| --------| ------------| +| `<waypoint task executor>`.enabled | true | Whether it is enabled | +| `<waypoint task executor>`.timeout | 10.0 | Amount of time in seconds to wait for user input before moving on to the next waypoint. | +| `<waypoint task executor>`.input_topic | "input_at_waypoint/input" | Topic input is published to to indicate to move to the next waypoint, in `std_msgs/Empty`. | + # recoveries ## recovery_server diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index e9b5b5f280b222a726c5a5d0fd4b70d7586efe56..a22cd4cd6cc3832fd0f017117bbe1bba09194720 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -58,7 +58,11 @@ ament_target_dependencies(simple_waypoint_task_executor ${dependencies}) # prevent pluginlib from using boost target_compile_definitions(simple_waypoint_task_executor PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -install(TARGETS ${library_name} simple_waypoint_task_executor +add_library(input_at_waypoint SHARED plugins/input_at_waypoint.cpp) +ament_target_dependencies(input_at_waypoint ${dependencies}) +target_compile_definitions(input_at_waypoint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +install(TARGETS ${library_name} simple_waypoint_task_executor input_at_waypoint ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -78,7 +82,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(simple_waypoint_task_executor) +ament_export_libraries(simple_waypoint_task_executor input_at_waypoint) pluginlib_export_plugin_description_file(nav2_waypoint_follower plugins.xml) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp new file mode 100644 index 0000000000000000000000000000000000000000..2a8537007e2edb0c7fb75149cbca1a6795db24d3 --- /dev/null +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp @@ -0,0 +1,85 @@ +// Copyright (c) 2020 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. + +#ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__INPUT_AT_WAYPOINT_HPP_ +#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__INPUT_AT_WAYPOINT_HPP_ +#pragma once + +#include <string> +#include <mutex> +#include <memory> + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/empty.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/waypoint_task_executor.hpp" + +namespace nav2_waypoint_follower +{ + +/** + * @brief Simple plugin based on WaypointTaskExecutor, lets robot to wait for a + * user input at waypoint arrival. + */ +class InputAtWaypoint : public nav2_core::WaypointTaskExecutor +{ +public: +/** + * @brief Construct a new Input At Waypoint Arrival object + * + */ + InputAtWaypoint(); + + /** + * @brief Destroy the Input At Waypoint Arrival object + * + */ + ~InputAtWaypoint(); + + /** + * @brief declares and loads parameters used + * @param parent parent node + * @param plugin_name name of plugin + */ + void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name); + + /** + * @brief Processor + * @param curr_pose current pose of the robot + * @param curr_waypoint_index current waypoint, that robot just arrived + * @return if task execution failed + */ + bool processAtWaypoint( + const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index); + +protected: + /** + * @brief Processor callback + * @param msg Empty message + */ + void Cb(const std_msgs::msg::Empty::SharedPtr msg); + + bool input_received_; + bool is_enabled_; + rclcpp::Duration timeout_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; + rclcpp::Clock::SharedPtr clock_; + std::mutex mutex_; + rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr subscription_; +}; + +} // namespace nav2_waypoint_follower + +#endif // NAV2_WAYPOINT_FOLLOWER__PLUGINS__INPUT_AT_WAYPOINT_HPP_ diff --git a/nav2_waypoint_follower/plugins.xml b/nav2_waypoint_follower/plugins.xml index aaf7cd191c6e8f8a30d7abf933cb5ade18963fed..2c38b6c5f2ca4960ec44d70e226b4784329a76cd 100644 --- a/nav2_waypoint_follower/plugins.xml +++ b/nav2_waypoint_follower/plugins.xml @@ -4,4 +4,9 @@ <description>Lets robot sleep for a specified amount of time at waypoint arrival</description> </class> </library> -</class_libraries> \ No newline at end of file + <library path="input_at_waypoint"> + <class type="nav2_waypoint_follower::InputAtWaypoint" base_class_type="nav2_core::WaypointTaskExecutor"> + <description>Lets robot wait for input at waypoint arrival</description> + </class> + </library> +</class_libraries> diff --git a/nav2_waypoint_follower/plugins/input_at_waypoint.cpp b/nav2_waypoint_follower/plugins/input_at_waypoint.cpp new file mode 100644 index 0000000000000000000000000000000000000000..98dbfeaa0cb2d989d05867e2769c89f12dc2b07e --- /dev/null +++ b/nav2_waypoint_follower/plugins/input_at_waypoint.cpp @@ -0,0 +1,114 @@ +// Copyright (c) 2020 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. + +#include <pluginlib/class_list_macros.hpp> +#include <string> +#include <exception> + +#include "nav2_util/node_utils.hpp" +#include "nav2_waypoint_follower/plugins/input_at_waypoint.hpp" + +namespace nav2_waypoint_follower +{ + +using std::placeholders::_1; + +InputAtWaypoint::InputAtWaypoint() +: input_received_(false), + is_enabled_(true), + timeout_(10.0, 0.0) +{ +} + +InputAtWaypoint::~InputAtWaypoint() +{ +} + +void InputAtWaypoint::initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name) +{ + auto node = parent.lock(); + + if (!node) { + throw std::runtime_error{"Failed to lock node in input at waypoint plugin!"}; + } + + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + double timeout; + std::string input_topic; + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".timeout", + rclcpp::ParameterValue(10.0)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".enabled", + rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".input_topic", + rclcpp::ParameterValue("input_at_waypoint/input")); + node->get_parameter(plugin_name + ".timeout", timeout); + node->get_parameter(plugin_name + ".enabled", is_enabled_); + node->get_parameter(plugin_name + ".input_topic", input_topic); + + timeout_ = rclcpp::Duration(timeout, 0.0); + + RCLCPP_INFO( + logger_, "InputAtWaypoint: Subscribing to input topic %s.", input_topic.c_str()); + subscription_ = node->create_subscription<std_msgs::msg::Empty>( + input_topic, 1, std::bind(&InputAtWaypoint::Cb, this, _1)); +} + +void InputAtWaypoint::Cb(const std_msgs::msg::Empty::SharedPtr /*msg*/) +{ + std::lock_guard<std::mutex> lock(mutex_); + input_received_ = true; +} + +bool InputAtWaypoint::processAtWaypoint( + const geometry_msgs::msg::PoseStamped & /*curr_pose*/, + const int & curr_waypoint_index) +{ + if (!is_enabled_) { + return true; + } + + input_received_ = false; + + rclcpp::Time start = clock_->now(); + rclcpp::Rate r(50); + bool input_received = false; + while (clock_->now() - start < timeout_) { + { + std::lock_guard<std::mutex> lock(mutex_); + input_received = input_received_; + } + + if (input_received) { + return true; + } + + r.sleep(); + } + + RCLCPP_WARN( + logger_, "Unable to get external input at wp %i. Moving on.", curr_waypoint_index); + return false; +} + +} // namespace nav2_waypoint_follower + +PLUGINLIB_EXPORT_CLASS( + nav2_waypoint_follower::InputAtWaypoint, + nav2_core::WaypointTaskExecutor)