add humble-navigation2

navigation2/nav2_waypoint_follower/CMakeLists.txt

@@ -0,0 +1,107 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_waypoint_follower)
+
+# Try for OpenCV 4.X, but settle for whatever is installed
+find_package(OpenCV 4 QUIET)
+if(NOT OpenCV_FOUND)
+  find_package(OpenCV REQUIRED)
+endif()
+message(STATUS "Found OpenCV version ${OpenCV_VERSION}")
+
+find_package(image_transport REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(ament_cmake REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(pluginlib REQUIRED)
+
+nav2_package()
+
+include_directories(
+  include
+)
+
+set(executable_name waypoint_follower)
+
+add_executable(${executable_name}
+  src/main.cpp
+)
+
+set(library_name ${executable_name}_core)
+
+add_library(${library_name} SHARED
+  src/waypoint_follower.cpp
+)
+
+set(dependencies
+  rclcpp
+  rclcpp_action
+  rclcpp_lifecycle
+  rclcpp_components
+  nav_msgs
+  nav2_msgs
+  nav2_util
+  tf2_ros
+  nav2_core
+  pluginlib
+  image_transport
+  cv_bridge
+  OpenCV
+)
+
+ament_target_dependencies(${executable_name}
+  ${dependencies}
+)
+
+target_link_libraries(${executable_name} ${library_name})
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+add_library(wait_at_waypoint SHARED plugins/wait_at_waypoint.cpp)
+ament_target_dependencies(wait_at_waypoint ${dependencies})
+
+add_library(photo_at_waypoint SHARED plugins/photo_at_waypoint.cpp)
+ament_target_dependencies(photo_at_waypoint ${dependencies})
+
+add_library(input_at_waypoint SHARED plugins/input_at_waypoint.cpp)
+ament_target_dependencies(input_at_waypoint ${dependencies})
+
+rclcpp_components_register_nodes(${library_name} "nav2_waypoint_follower::WaypointFollower")
+
+install(TARGETS ${library_name} wait_at_waypoint photo_at_waypoint input_at_waypoint
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(TARGETS ${executable_name}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  add_subdirectory(test)
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(wait_at_waypoint photo_at_waypoint input_at_waypoint ${library_name})
+ament_export_dependencies(${dependencies})
+pluginlib_export_plugin_description_file(nav2_waypoint_follower plugins.xml)
+
+ament_package()

navigation2/nav2_waypoint_follower/README.md

@@ -0,0 +1,29 @@
+# Nav2 Waypoint Follower
+
+The Nav2 waypoint follower is an example application of how to use the navigation action to complete some sort of orchestrated task. In this example, that task is to take a given set of waypoints and navigate to a set of positions in the order provided in the action request. The last waypoint in the waypoint array is the final position. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/).
+
+See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-waypoint-follower.html) for additional parameter descriptions.
+
+The package exposes the `follow_waypoints` action server of type `nav2_msgs/FollowWaypoints`.
+ It is given an array of waypoints to visit, gives feedback about the current index of waypoint it is processing, and returns a list of waypoints it was unable to complete.
+
+It also hosts a waypoint task executor plugin which can be used to perform custom behavior at a waypoint like waiting for user instruction, taking a picture, or picking up a box.
+
+There is a parameterization `stop_on_failure` whether to stop processing the waypoint following action on a single waypoint failure. When false, it will continue onto the next waypoint when the current waypoint fails. The action will exist when either all the waypoint navigation tasks have terminated or when `stop_on_failure`, a single waypoint as failed.
+
+## An aside on autonomy / waypoint following
+
+The ``nav2_waypoint_follower`` contains a waypoint following program with a plugin interface for specific **task executors**.
+This is useful if you need to go to a given location and complete a specific task like take a picture, pick up a box, or wait for user input.
+It is a nice demo application for how to use Nav2 in a sample application.
+
+However, it could be used for more than just a sample application.
+There are 2 schools of thoughts for fleet managers / dispatchers.
+- Dumb robot; smart centralized dispatcher
+- Smart robot; dumb centralized dispatcher
+
+In the first, the ``nav2_waypoint_follower`` is weakly sufficient to create a production-grade on-robot solution. Since the autonomy system / dispatcher is taking into account things like the robot's pose, battery level, current task, and more when assigning tasks, the application on the robot just needs to worry about the task at hand and not the other complexities of the system complete the requested task. In this situation, you should think of a request to the waypoint follower as 1 unit of work (e.g. 1 pick in a warehouse, 1 security patrole loop, 1 aisle, etc) to do a task and then return to the dispatcher for the next task or request to recharge. In this school of thought, the waypoint following application is just one step above navigation and below the system autonomy application.
+
+In the second, the ``nav2_waypoint_follower`` is a nice sample application / proof of concept, but you really need your waypoint following / autonomy system on the robot to carry more weight in making a robust solution. In this case, you should use the ``nav2_behavior_tree`` package to create a custom application-level behavior tree using navigation to complete the task. This can include subtrees like checking for the charge status mid-task for returning to dock or handling more than 1 unit of work in a more complex task. Soon, there will be a ``nav2_bt_waypoint_follower`` (name subject to adjustment) that will allow you to create this application more easily. In this school of thought, the waypoint following application is more closely tied to the system autonomy, or in many cases, is the system autonomy.
+
+Neither is better than the other, it highly depends on the tasks your robot(s) are completing, in what type of environment, and with what cloud resources available. Often this distinction is very clear for a given business case.

navigation2/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_

navigation2/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp

@@ -0,0 +1,116 @@
+// Copyright (c) 2020 Fetullah Atas
+//
+// 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__PHOTO_AT_WAYPOINT_HPP_
+#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_
+
+/**
+ * While C++17 isn't the project standard. We have to force LLVM/CLang
+ * to ignore deprecated declarations
+ */
+#define _LIBCPP_NO_EXPERIMENTAL_DEPRECATION_WARNING_FILESYSTEM
+
+
+#include <filesystem>
+#include <mutex>
+#include <string>
+#include <exception>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+
+#include "sensor_msgs/msg/image.hpp"
+#include "nav2_core/waypoint_task_executor.hpp"
+#include "opencv4/opencv2/core.hpp"
+#include "opencv4/opencv2/opencv.hpp"
+#include "cv_bridge/cv_bridge.h"
+#include "image_transport/image_transport.hpp"
+
+
+namespace nav2_waypoint_follower
+{
+
+class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor
+{
+public:
+  /**
+  * @brief Construct a new Photo At Waypoint object
+  *
+  */
+  PhotoAtWaypoint();
+
+  /**
+   * @brief Destroy the Photo At Waypoint object
+   *
+   */
+  ~PhotoAtWaypoint();
+
+  /**
+   * @brief declares and loads parameters used
+   *
+   * @param parent parent node that plugin will be created within
+   * @param plugin_name should be provided in nav2_params.yaml==> waypoint_follower
+   */
+  void initialize(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    const std::string & plugin_name);
+
+
+  /**
+   * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint
+   *
+   * @param curr_pose current pose of the robot
+   * @param curr_waypoint_index current waypoint, that robot just arrived
+   * @return true if task execution was successful
+   * @return false if task execution failed
+   */
+  bool processAtWaypoint(
+    const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index);
+
+  /**
+   * @brief
+   *
+   * @param msg
+   */
+  void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg);
+
+  /**
+   * @brief given a shared pointer to sensor::msg::Image type, make a deep copy to inputted cv Mat
+   *
+   * @param msg
+   * @param mat
+   */
+  static void deepCopyMsg2Mat(const sensor_msgs::msg::Image::SharedPtr & msg, cv::Mat & mat);
+
+protected:
+  // to ensure safety when accessing global var curr_frame_
+  std::mutex global_mutex_;
+  // the taken photos will be saved under this directory
+  std::filesystem::path save_dir_;
+  // .png ? .jpg ? or some other well known format
+  std::string image_format_;
+  // the topic to subscribe in order capture a frame
+  std::string image_topic_;
+  // whether plugin is enabled
+  bool is_enabled_;
+  // current frame;
+  sensor_msgs::msg::Image::SharedPtr curr_frame_msg_;
+  // global logger
+  rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")};
+  // ros subscriber to get camera image
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr camera_image_subscriber_;
+};
+}  // namespace nav2_waypoint_follower
+
+#endif  // NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_

navigation2/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp

@@ -0,0 +1,80 @@
+// Copyright (c) 2020 Fetullah Atas
+//
+// 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__WAIT_AT_WAYPOINT_HPP_
+#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_
+#pragma once
+
+#include <string>
+
+#include "rclcpp/rclcpp.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 sleep for a
+ *        specified amount of time at waypoint arrival. You can reference this class to define
+ *        your own task and rewrite the body for it.
+ *
+ */
+class WaitAtWaypoint : public nav2_core::WaypointTaskExecutor
+{
+public:
+/**
+ * @brief Construct a new Wait At Waypoint Arrival object
+ *
+ */
+  WaitAtWaypoint();
+
+  /**
+   * @brief Destroy the Wait At Waypoint Arrival object
+   *
+   */
+  ~WaitAtWaypoint();
+
+  /**
+   * @brief declares and loads parameters used (waypoint_pause_duration_)
+   *
+   * @param parent parent node that plugin will be created withing(waypoint_follower in this case)
+   * @param plugin_name
+   */
+  void initialize(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    const std::string & plugin_name);
+
+
+  /**
+   * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint
+   *
+   * @param curr_pose current pose of the robot
+   * @param curr_waypoint_index current waypoint, that robot just arrived
+   * @return true if task execution was successful
+   * @return false if task execution failed
+   */
+  bool processAtWaypoint(
+    const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index);
+
+protected:
+  // the robot will sleep waypoint_pause_duration_ milliseconds
+  int waypoint_pause_duration_;
+  bool is_enabled_;
+  rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")};
+  rclcpp::Clock::SharedPtr clock_;
+};
+
+}  // namespace nav2_waypoint_follower
+#endif  // NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_

navigation2/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp

@@ -0,0 +1,152 @@
+// 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.
+// 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__WAYPOINT_FOLLOWER_HPP_
+#define NAV2_WAYPOINT_FOLLOWER__WAYPOINT_FOLLOWER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <mutex>
+
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_msgs/action/navigate_to_pose.hpp"
+#include "nav2_msgs/action/follow_waypoints.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_util/simple_action_server.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+#include "nav2_util/node_utils.hpp"
+#include "nav2_core/waypoint_task_executor.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "pluginlib/class_list_macros.hpp"
+
+namespace nav2_waypoint_follower
+{
+
+enum class ActionStatus
+{
+  UNKNOWN = 0,
+  PROCESSING = 1,
+  FAILED = 2,
+  SUCCEEDED = 3
+};
+
+/**
+ * @class nav2_waypoint_follower::WaypointFollower
+ * @brief An action server that uses behavior tree for navigating a robot to its
+ * goal position.
+ */
+class WaypointFollower : public nav2_util::LifecycleNode
+{
+public:
+  using ActionT = nav2_msgs::action::FollowWaypoints;
+  using ClientT = nav2_msgs::action::NavigateToPose;
+  using ActionServer = nav2_util::SimpleActionServer<ActionT>;
+  using ActionClient = rclcpp_action::Client<ClientT>;
+
+  /**
+   * @brief A constructor for nav2_waypoint_follower::WaypointFollower class
+   * @param options Additional options to control creation of the node.
+   */
+  explicit WaypointFollower(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  /**
+   * @brief A destructor for nav2_waypoint_follower::WaypointFollower class
+   */
+  ~WaypointFollower();
+
+protected:
+  /**
+   * @brief Configures member variables
+   *
+   * Initializes action server for "follow_waypoints"
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Activates action server
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Deactivates action server
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Resets member variables
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called when in shutdown state
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Action server callbacks
+   */
+  void followWaypoints();
+
+  /**
+   * @brief Action client result callback
+   * @param result Result of action server updated asynchronously
+   */
+  void resultCallback(const rclcpp_action::ClientGoalHandle<ClientT>::WrappedResult & result);
+
+  /**
+   * @brief Action client goal response callback
+   * @param goal Response of action server updated asynchronously
+   */
+  void goalResponseCallback(const rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr & goal);
+
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+
+  // Our action server
+  std::unique_ptr<ActionServer> action_server_;
+  ActionClient::SharedPtr nav_to_pose_client_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
+  std::shared_future<rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr> future_goal_handle_;
+  bool stop_on_failure_;
+  ActionStatus current_goal_status_;
+  int loop_rate_;
+  std::vector<int> failed_ids_;
+
+  // Task Execution At Waypoint Plugin
+  pluginlib::ClassLoader<nav2_core::WaypointTaskExecutor>
+  waypoint_task_executor_loader_;
+  pluginlib::UniquePtr<nav2_core::WaypointTaskExecutor>
+  waypoint_task_executor_;
+  std::string waypoint_task_executor_id_;
+  std::string waypoint_task_executor_type_;
+};
+
+}  // namespace nav2_waypoint_follower
+
+#endif  // NAV2_WAYPOINT_FOLLOWER__WAYPOINT_FOLLOWER_HPP_

navigation2/nav2_waypoint_follower/package.xml

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_waypoint_follower</name>
+  <version>1.1.18</version>
+  <description>A waypoint follower navigation server</description>
+  <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>nav2_common</depend>
+  <depend>cv_bridge</depend>
+  <depend>pluginlib</depend>
+  <depend>image_transport</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>nav_msgs</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav2_util</depend>
+  <depend>nav2_core</depend>
+  <depend>tf2_ros</depend>
+
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <nav2_waypoint_follower plugin="${prefix}/plugins.xml" />
+  </export>
+</package>

navigation2/nav2_waypoint_follower/plugins.xml

@@ -0,0 +1,18 @@
+<class_libraries>
+  <library path="wait_at_waypoint">
+    <class type="nav2_waypoint_follower::WaitAtWaypoint" base_class_type="nav2_core::WaypointTaskExecutor">
+      <description>Lets robot sleep for a specified amount of time at waypoint arrival</description>
+    </class>
+  </library>
+  <library path="photo_at_waypoint">
+    <class type="nav2_waypoint_follower::PhotoAtWaypoint" base_class_type="nav2_core::WaypointTaskExecutor">
+      <description>Run-time plugin that takes photos at waypoint arrivals when using waypoint follower node. 
+        Saves the taken photos to specified directory.</description>
+    </class>
+  </library>
+  <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>

navigation2/nav2_waypoint_follower/plugins/input_at_waypoint.cpp

@@ -0,0 +1,116 @@
+// 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 "nav2_waypoint_follower/plugins/input_at_waypoint.hpp"
+
+#include <string>
+#include <exception>
+
+#include "pluginlib/class_list_macros.hpp"
+
+#include "nav2_util/node_utils.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)

navigation2/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp

@@ -0,0 +1,159 @@
+// Copyright (c) 2020 Fetullah Atas
+//
+// 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 "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp"
+
+#include <string>
+#include <memory>
+
+#include "pluginlib/class_list_macros.hpp"
+
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_waypoint_follower
+{
+PhotoAtWaypoint::PhotoAtWaypoint()
+{
+}
+
+PhotoAtWaypoint::~PhotoAtWaypoint()
+{
+}
+
+void PhotoAtWaypoint::initialize(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  const std::string & plugin_name)
+{
+  auto node = parent.lock();
+
+  curr_frame_msg_ = std::make_shared<sensor_msgs::msg::Image>();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name + ".enabled",
+    rclcpp::ParameterValue(true));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name + ".image_topic",
+    rclcpp::ParameterValue("/camera/color/image_raw"));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name + ".save_dir",
+    rclcpp::ParameterValue("/tmp/waypoint_images"));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name + ".image_format",
+    rclcpp::ParameterValue("png"));
+
+  std::string save_dir_as_string;
+  node->get_parameter(plugin_name + ".enabled", is_enabled_);
+  node->get_parameter(plugin_name + ".image_topic", image_topic_);
+  node->get_parameter(plugin_name + ".save_dir", save_dir_as_string);
+  node->get_parameter(plugin_name + ".image_format", image_format_);
+
+  // get inputted save directory and make sure it exists, if not log and create  it
+  save_dir_ = save_dir_as_string;
+  try {
+    if (!std::filesystem::exists(save_dir_)) {
+      RCLCPP_WARN(
+        logger_,
+        "Provided save directory for photo at waypoint plugin does not exist,"
+        "provided directory is: %s, the directory will be created automatically.",
+        save_dir_.c_str()
+      );
+      if (!std::filesystem::create_directory(save_dir_)) {
+        RCLCPP_ERROR(
+          logger_,
+          "Failed to create directory!: %s required by photo at waypoint plugin, "
+          "exiting the plugin with failure!",
+          save_dir_.c_str()
+        );
+        is_enabled_ = false;
+      }
+    }
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(
+      logger_, "Exception (%s) thrown while attempting to create image capture directory."
+      " This task executor is being disabled as it cannot save images.", e.what());
+    is_enabled_ = false;
+  }
+
+  if (!is_enabled_) {
+    RCLCPP_INFO(
+      logger_, "Photo at waypoint plugin is disabled.");
+  } else {
+    RCLCPP_INFO(
+      logger_, "Initializing photo at waypoint plugin, subscribing to camera topic named; %s",
+      image_topic_.c_str());
+    camera_image_subscriber_ = node->create_subscription<sensor_msgs::msg::Image>(
+      image_topic_, rclcpp::SystemDefaultsQoS(),
+      std::bind(&PhotoAtWaypoint::imageCallback, this, std::placeholders::_1));
+  }
+}
+
+bool PhotoAtWaypoint::processAtWaypoint(
+  const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index)
+{
+  if (!is_enabled_) {
+    RCLCPP_WARN(
+      logger_,
+      "Photo at waypoint plugin is disabled. Not performing anything"
+    );
+    return true;
+  }
+  try {
+    // construct the full path to image filename
+    std::filesystem::path file_name = std::to_string(
+      curr_waypoint_index) + "_" +
+      std::to_string(curr_pose.header.stamp.sec) + "." + image_format_;
+    std::filesystem::path full_path_image_path = save_dir_ / file_name;
+
+    // save the taken photo at this waypoint to given directory
+    std::lock_guard<std::mutex> guard(global_mutex_);
+    cv::Mat curr_frame_mat;
+    deepCopyMsg2Mat(curr_frame_msg_, curr_frame_mat);
+    cv::imwrite(full_path_image_path.c_str(), curr_frame_mat);
+    RCLCPP_INFO(
+      logger_,
+      "Photo has been taken sucessfully at waypoint %i", curr_waypoint_index);
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(
+      logger_,
+      "Couldn't take photo at waypoint %i! Caught exception: %s \n"
+      "Make sure that the image topic named: %s is valid and active!",
+      curr_waypoint_index,
+      e.what(), image_topic_.c_str());
+    return false;
+  }
+  return true;
+}
+
+void PhotoAtWaypoint::imageCallback(const sensor_msgs::msg::Image::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> guard(global_mutex_);
+  curr_frame_msg_ = msg;
+}
+
+void PhotoAtWaypoint::deepCopyMsg2Mat(
+  const sensor_msgs::msg::Image::SharedPtr & msg,
+  cv::Mat & mat)
+{
+  cv_bridge::CvImageConstPtr cv_bridge_ptr = cv_bridge::toCvShare(msg, msg->encoding);
+  cv::Mat frame = cv_bridge_ptr->image;
+  if (msg->encoding == "rgb8") {
+    cv::cvtColor(frame, frame, cv::COLOR_RGB2BGR);
+  }
+  frame.copyTo(mat);
+}
+
+}      // namespace nav2_waypoint_follower
+PLUGINLIB_EXPORT_CLASS(
+  nav2_waypoint_follower::PhotoAtWaypoint,
+  nav2_core::WaypointTaskExecutor)

navigation2/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp

@@ -0,0 +1,87 @@
+// Copyright (c) 2020 Fetullah Atas
+//
+// 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 "nav2_waypoint_follower/plugins/wait_at_waypoint.hpp"
+
+#include <string>
+#include <exception>
+
+#include "pluginlib/class_list_macros.hpp"
+
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_waypoint_follower
+{
+WaitAtWaypoint::WaitAtWaypoint()
+: waypoint_pause_duration_(0),
+  is_enabled_(true)
+{
+}
+
+WaitAtWaypoint::~WaitAtWaypoint()
+{
+}
+
+void WaitAtWaypoint::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 wait at waypoint plugin!"};
+  }
+  logger_ = node->get_logger();
+  clock_ = node->get_clock();
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    plugin_name + ".waypoint_pause_duration",
+    rclcpp::ParameterValue(0));
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    plugin_name + ".enabled",
+    rclcpp::ParameterValue(true));
+  node->get_parameter(
+    plugin_name + ".waypoint_pause_duration",
+    waypoint_pause_duration_);
+  node->get_parameter(
+    plugin_name + ".enabled",
+    is_enabled_);
+  if (waypoint_pause_duration_ == 0) {
+    is_enabled_ = false;
+    RCLCPP_INFO(
+      logger_,
+      "Waypoint pause duration is set to zero, disabling task executor plugin.");
+  } else if (!is_enabled_) {
+    RCLCPP_INFO(
+      logger_, "Waypoint task executor plugin is disabled.");
+  }
+}
+
+bool WaitAtWaypoint::processAtWaypoint(
+  const geometry_msgs::msg::PoseStamped & /*curr_pose*/, const int & curr_waypoint_index)
+{
+  if (!is_enabled_) {
+    return true;
+  }
+  RCLCPP_INFO(
+    logger_, "Arrived at %i'th waypoint, sleeping for %i milliseconds",
+    curr_waypoint_index,
+    waypoint_pause_duration_);
+  clock_->sleep_for(std::chrono::milliseconds(waypoint_pause_duration_));
+  return true;
+}
+}  // namespace nav2_waypoint_follower
+PLUGINLIB_EXPORT_CLASS(
+  nav2_waypoint_follower::WaitAtWaypoint,
+  nav2_core::WaypointTaskExecutor)

navigation2/nav2_waypoint_follower/src/main.cpp

@@ -0,0 +1,28 @@
+// 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.
+// 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 <memory>
+
+#include "nav2_waypoint_follower/waypoint_follower.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<nav2_waypoint_follower::WaypointFollower>();
+  rclcpp::spin(node->get_node_base_interface());
+  rclcpp::shutdown();
+
+  return 0;
+}

navigation2/nav2_waypoint_follower/src/waypoint_follower.cpp

@@ -0,0 +1,357 @@
+// 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.
+// 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 "nav2_waypoint_follower/waypoint_follower.hpp"
+
+#include <fstream>
+#include <memory>
+#include <streambuf>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace nav2_waypoint_follower
+{
+
+using rcl_interfaces::msg::ParameterType;
+using std::placeholders::_1;
+
+WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options)
+: nav2_util::LifecycleNode("waypoint_follower", "", options),
+  waypoint_task_executor_loader_("nav2_waypoint_follower",
+    "nav2_core::WaypointTaskExecutor")
+{
+  RCLCPP_INFO(get_logger(), "Creating");
+
+  declare_parameter("stop_on_failure", true);
+  declare_parameter("loop_rate", 20);
+  nav2_util::declare_parameter_if_not_declared(
+    this, std::string("waypoint_task_executor_plugin"),
+    rclcpp::ParameterValue(std::string("wait_at_waypoint")));
+  nav2_util::declare_parameter_if_not_declared(
+    this, std::string("wait_at_waypoint.plugin"),
+    rclcpp::ParameterValue(std::string("nav2_waypoint_follower::WaitAtWaypoint")));
+}
+
+WaypointFollower::~WaypointFollower()
+{
+}
+
+nav2_util::CallbackReturn
+WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Configuring");
+
+  auto node = shared_from_this();
+
+  stop_on_failure_ = get_parameter("stop_on_failure").as_bool();
+  loop_rate_ = get_parameter("loop_rate").as_int();
+  waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string();
+
+  callback_group_ = create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, get_node_base_interface());
+
+  nav_to_pose_client_ = rclcpp_action::create_client<ClientT>(
+    get_node_base_interface(),
+    get_node_graph_interface(),
+    get_node_logging_interface(),
+    get_node_waitables_interface(),
+    "navigate_to_pose", callback_group_);
+
+  action_server_ = std::make_unique<ActionServer>(
+    get_node_base_interface(),
+    get_node_clock_interface(),
+    get_node_logging_interface(),
+    get_node_waitables_interface(),
+    "follow_waypoints", std::bind(&WaypointFollower::followWaypoints, this));
+
+  try {
+    waypoint_task_executor_type_ = nav2_util::get_plugin_type_param(
+      this,
+      waypoint_task_executor_id_);
+    waypoint_task_executor_ = waypoint_task_executor_loader_.createUniqueInstance(
+      waypoint_task_executor_type_);
+    RCLCPP_INFO(
+      get_logger(), "Created waypoint_task_executor : %s of type %s",
+      waypoint_task_executor_id_.c_str(), waypoint_task_executor_type_.c_str());
+    waypoint_task_executor_->initialize(node, waypoint_task_executor_id_);
+  } catch (const pluginlib::PluginlibException & ex) {
+    RCLCPP_FATAL(
+      get_logger(),
+      "Failed to create waypoint_task_executor. Exception: %s", ex.what());
+  }
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+WaypointFollower::on_activate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Activating");
+
+  action_server_->activate();
+
+  auto node = shared_from_this();
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(&WaypointFollower::dynamicParametersCallback, this, _1));
+
+  // create bond connection
+  createBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+WaypointFollower::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Deactivating");
+
+  action_server_->deactivate();
+  dyn_params_handler_.reset();
+
+  // destroy bond connection
+  destroyBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+WaypointFollower::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Cleaning up");
+
+  action_server_.reset();
+  nav_to_pose_client_.reset();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+WaypointFollower::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Shutting down");
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+void
+WaypointFollower::followWaypoints()
+{
+  auto goal = action_server_->get_current_goal();
+  auto feedback = std::make_shared<ActionT::Feedback>();
+  auto result = std::make_shared<ActionT::Result>();
+
+  // Check if request is valid
+  if (!action_server_ || !action_server_->is_server_active()) {
+    RCLCPP_DEBUG(get_logger(), "Action server inactive. Stopping.");
+    return;
+  }
+
+  RCLCPP_INFO(
+    get_logger(), "Received follow waypoint request with %i waypoints.",
+    static_cast<int>(goal->poses.size()));
+
+  if (goal->poses.size() == 0) {
+    action_server_->succeeded_current(result);
+    return;
+  }
+
+  rclcpp::WallRate r(loop_rate_);
+  uint32_t goal_index = 0;
+  bool new_goal = true;
+
+  while (rclcpp::ok()) {
+    // Check if asked to stop processing action
+    if (action_server_->is_cancel_requested()) {
+      auto cancel_future = nav_to_pose_client_->async_cancel_all_goals();
+      callback_group_executor_.spin_until_future_complete(cancel_future);
+      // for result callback processing
+      callback_group_executor_.spin_some();
+      action_server_->terminate_all();
+      return;
+    }
+
+    // Check if asked to process another action
+    if (action_server_->is_preempt_requested()) {
+      RCLCPP_INFO(get_logger(), "Preempting the goal pose.");
+      goal = action_server_->accept_pending_goal();
+      goal_index = 0;
+      new_goal = true;
+    }
+
+    // Check if we need to send a new goal
+    if (new_goal) {
+      new_goal = false;
+      ClientT::Goal client_goal;
+      client_goal.pose = goal->poses[goal_index];
+
+      auto send_goal_options = rclcpp_action::Client<ClientT>::SendGoalOptions();
+      send_goal_options.result_callback =
+        std::bind(&WaypointFollower::resultCallback, this, std::placeholders::_1);
+      send_goal_options.goal_response_callback =
+        std::bind(&WaypointFollower::goalResponseCallback, this, std::placeholders::_1);
+      future_goal_handle_ =
+        nav_to_pose_client_->async_send_goal(client_goal, send_goal_options);
+      current_goal_status_ = ActionStatus::PROCESSING;
+    }
+
+    feedback->current_waypoint = goal_index;
+    action_server_->publish_feedback(feedback);
+
+    if (current_goal_status_ == ActionStatus::FAILED) {
+      failed_ids_.push_back(goal_index);
+
+      if (stop_on_failure_) {
+        RCLCPP_WARN(
+          get_logger(), "Failed to process waypoint %i in waypoint "
+          "list and stop on failure is enabled."
+          " Terminating action.", goal_index);
+        result->missed_waypoints = failed_ids_;
+        action_server_->terminate_current(result);
+        failed_ids_.clear();
+        return;
+      } else {
+        RCLCPP_INFO(
+          get_logger(), "Failed to process waypoint %i,"
+          " moving to next.", goal_index);
+      }
+    } else if (current_goal_status_ == ActionStatus::SUCCEEDED) {
+      RCLCPP_INFO(
+        get_logger(), "Succeeded processing waypoint %i, processing waypoint task execution",
+        goal_index);
+      bool is_task_executed = waypoint_task_executor_->processAtWaypoint(
+        goal->poses[goal_index], goal_index);
+      RCLCPP_INFO(
+        get_logger(), "Task execution at waypoint %i %s", goal_index,
+        is_task_executed ? "succeeded" : "failed!");
+      // if task execution was failed and stop_on_failure_ is on , terminate action
+      if (!is_task_executed && stop_on_failure_) {
+        failed_ids_.push_back(goal_index);
+        RCLCPP_WARN(
+          get_logger(), "Failed to execute task at waypoint %i "
+          " stop on failure is enabled."
+          " Terminating action.", goal_index);
+        result->missed_waypoints = failed_ids_;
+        action_server_->terminate_current(result);
+        failed_ids_.clear();
+        return;
+      } else {
+        RCLCPP_INFO(
+          get_logger(), "Handled task execution on waypoint %i,"
+          " moving to next.", goal_index);
+      }
+    }
+
+    if (current_goal_status_ != ActionStatus::PROCESSING &&
+      current_goal_status_ != ActionStatus::UNKNOWN)
+    {
+      // Update server state
+      goal_index++;
+      new_goal = true;
+      if (goal_index >= goal->poses.size()) {
+        RCLCPP_INFO(
+          get_logger(), "Completed all %zu waypoints requested.",
+          goal->poses.size());
+        result->missed_waypoints = failed_ids_;
+        action_server_->succeeded_current(result);
+        failed_ids_.clear();
+        return;
+      }
+    } else {
+      RCLCPP_INFO_EXPRESSION(
+        get_logger(),
+        (static_cast<int>(now().seconds()) % 30 == 0),
+        "Processing waypoint %i...", goal_index);
+    }
+
+    callback_group_executor_.spin_some();
+    r.sleep();
+  }
+}
+
+void
+WaypointFollower::resultCallback(
+  const rclcpp_action::ClientGoalHandle<ClientT>::WrappedResult & result)
+{
+  if (result.goal_id != future_goal_handle_.get()->get_goal_id()) {
+    RCLCPP_DEBUG(
+      get_logger(),
+      "Goal IDs do not match for the current goal handle and received result."
+      "Ignoring likely due to receiving result for an old goal.");
+    return;
+  }
+
+  switch (result.code) {
+    case rclcpp_action::ResultCode::SUCCEEDED:
+      current_goal_status_ = ActionStatus::SUCCEEDED;
+      return;
+    case rclcpp_action::ResultCode::ABORTED:
+      current_goal_status_ = ActionStatus::FAILED;
+      return;
+    case rclcpp_action::ResultCode::CANCELED:
+      current_goal_status_ = ActionStatus::FAILED;
+      return;
+    default:
+      current_goal_status_ = ActionStatus::UNKNOWN;
+      return;
+  }
+}
+
+void
+WaypointFollower::goalResponseCallback(
+  const rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr & goal)
+{
+  if (!goal) {
+    RCLCPP_ERROR(
+      get_logger(),
+      "navigate_to_pose action client failed to send goal to server.");
+    current_goal_status_ = ActionStatus::FAILED;
+  }
+}
+
+rcl_interfaces::msg::SetParametersResult
+WaypointFollower::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  // No locking required as action server is running on same single threaded executor
+  rcl_interfaces::msg::SetParametersResult result;
+
+  for (auto parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == ParameterType::PARAMETER_INTEGER) {
+      if (name == "loop_rate") {
+        loop_rate_ = parameter.as_int();
+      }
+    } else if (type == ParameterType::PARAMETER_BOOL) {
+      if (name == "stop_on_failure") {
+        stop_on_failure_ = parameter.as_bool();
+      }
+    }
+  }
+
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_waypoint_follower
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(nav2_waypoint_follower::WaypointFollower)

navigation2/nav2_waypoint_follower/test/CMakeLists.txt

@@ -0,0 +1,21 @@
+# Test test executors
+ament_add_gtest(test_task_executors
+  test_task_executors.cpp
+)
+ament_target_dependencies(test_task_executors
+  ${dependencies}
+)
+target_link_libraries(test_task_executors
+  ${library_name} wait_at_waypoint photo_at_waypoint input_at_waypoint
+)
+
+# Test dynamic parameters
+ament_add_gtest(test_dynamic_parameters
+  test_dynamic_parameters.cpp
+)
+ament_target_dependencies(test_dynamic_parameters
+  ${dependencies}
+)
+target_link_libraries(test_dynamic_parameters
+  ${library_name}
+)

navigation2/nav2_waypoint_follower/test/test_dynamic_parameters.cpp

@@ -0,0 +1,76 @@
+// Copyright (c) 2021, 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 <math.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_waypoint_follower/waypoint_follower.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+class WPShim : public nav2_waypoint_follower::WaypointFollower
+{
+public:
+  WPShim()
+  : nav2_waypoint_follower::WaypointFollower(rclcpp::NodeOptions())
+  {
+  }
+
+  void configure()
+  {
+    rclcpp_lifecycle::State state;
+    this->on_configure(state);
+  }
+
+  void activate()
+  {
+    rclcpp_lifecycle::State state;
+    this->on_activate(state);
+  }
+};
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(WPTest, test_dynamic_parameters)
+{
+  auto follower = std::make_shared<WPShim>();
+  follower->configure();
+  follower->activate();
+
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    follower->get_node_base_interface(), follower->get_node_topics_interface(),
+    follower->get_node_graph_interface(),
+    follower->get_node_services_interface());
+
+  auto results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("loop_rate", 100),
+      rclcpp::Parameter("stop_on_failure", false)});
+
+  rclcpp::spin_until_future_complete(
+    follower->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(follower->get_parameter("loop_rate").as_int(), 100);
+  EXPECT_EQ(follower->get_parameter("stop_on_failure").as_bool(), false);
+}

navigation2/nav2_waypoint_follower/test/test_task_executors.cpp

@@ -0,0 +1,164 @@
+// Copyright (c) 2021, 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 <math.h>
+#include <condition_variable>
+#include <memory>
+#include <string>
+#include <vector>
+#include <utility>
+#include <thread>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp"
+#include "nav2_waypoint_follower/plugins/wait_at_waypoint.hpp"
+#include "nav2_waypoint_follower/plugins/input_at_waypoint.hpp"
+
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(WaypointFollowerTest, WaitAtWaypoint)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testWaypointNode");
+
+  node->declare_parameter("WAW.waypoint_pause_duration", 50);
+
+  std::unique_ptr<nav2_waypoint_follower::WaitAtWaypoint> waw(
+    new nav2_waypoint_follower::WaitAtWaypoint
+  );
+  waw->initialize(node, std::string("WAW"));
+
+  auto start_time = node->now();
+
+  // should wait 50ms
+  geometry_msgs::msg::PoseStamped pose;
+  waw->processAtWaypoint(pose, 0);
+
+  auto end_time = node->now();
+
+  EXPECT_NEAR((end_time - start_time).seconds(), 0.05, 0.01);
+
+  waw.reset(new nav2_waypoint_follower::WaitAtWaypoint);
+  node->set_parameter(rclcpp::Parameter("WAW.enabled", false));
+  waw->initialize(node, std::string("WAW"));
+
+  // plugin is not enabled, should exit
+  EXPECT_TRUE(waw->processAtWaypoint(pose, 0));
+}
+
+TEST(WaypointFollowerTest, InputAtWaypoint)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testWaypointNode");
+  auto pub = node->create_publisher<std_msgs::msg::Empty>("input_at_waypoint/input", 1);
+  pub->on_activate();
+  auto publish_message =
+    [&]() -> void
+    {
+      rclcpp::Rate(5).sleep();
+      auto msg = std::make_unique<std_msgs::msg::Empty>();
+      pub->publish(std::move(msg));
+      rclcpp::spin_some(node->shared_from_this()->get_node_base_interface());
+    };
+
+  std::unique_ptr<nav2_waypoint_follower::InputAtWaypoint> iaw(
+    new nav2_waypoint_follower::InputAtWaypoint
+  );
+  iaw->initialize(node, std::string("IAW"));
+
+  auto start_time = node->now();
+
+  // no input, should timeout
+  geometry_msgs::msg::PoseStamped pose;
+  EXPECT_FALSE(iaw->processAtWaypoint(pose, 0));
+
+  auto end_time = node->now();
+
+  EXPECT_NEAR((end_time - start_time).seconds(), 10.0, 0.1);
+
+  // has input now, should work
+  std::thread t1(publish_message);
+  EXPECT_TRUE(iaw->processAtWaypoint(pose, 0));
+  t1.join();
+
+  iaw.reset(new nav2_waypoint_follower::InputAtWaypoint);
+  node->set_parameter(rclcpp::Parameter("IAW.enabled", false));
+  iaw->initialize(node, std::string("IAW"));
+
+  // plugin is not enabled, should exit
+  EXPECT_TRUE(iaw->processAtWaypoint(pose, 0));
+}
+
+TEST(WaypointFollowerTest, PhotoAtWaypoint)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testWaypointNode");
+  auto pub = node->create_publisher<sensor_msgs::msg::Image>("/camera/color/image_raw", 1);
+  pub->on_activate();
+  std::condition_variable cv;
+  std::mutex mtx;
+  std::unique_lock<std::mutex> lck(mtx, std::defer_lock);
+  bool data_published = false;
+  auto publish_message =
+    [&]() -> void
+    {
+      rclcpp::Rate(5).sleep();
+      auto msg = std::make_unique<sensor_msgs::msg::Image>();
+      // fill image msg data.
+      msg->encoding = "rgb8";
+      msg->height = 240;
+      msg->width = 320;
+      msg->step = 960;
+      auto size = msg->height * msg->width * 3;
+      msg->data.reserve(size);
+      int fake_data = 0;
+      for (size_t i = 0; i < size; i++) {
+        msg->data.push_back(fake_data++);
+      }
+      pub->publish(std::move(msg));
+      rclcpp::spin_some(node->shared_from_this()->get_node_base_interface());
+      lck.lock();
+      data_published = true;
+      cv.notify_one();
+      lck.unlock();
+    };
+
+  std::unique_ptr<nav2_waypoint_follower::PhotoAtWaypoint> paw(
+    new nav2_waypoint_follower::PhotoAtWaypoint
+  );
+  paw->initialize(node, std::string("PAW"));
+
+  // no images, throws because can't write
+  geometry_msgs::msg::PoseStamped pose;
+  EXPECT_FALSE(paw->processAtWaypoint(pose, 0));
+
+  std::thread t1(publish_message);
+  cv.wait(lck);
+  // has image now, since we force waiting until image is published
+  EXPECT_TRUE(paw->processAtWaypoint(pose, 0));
+  t1.join();
+
+  paw.reset(new nav2_waypoint_follower::PhotoAtWaypoint);
+  node->set_parameter(rclcpp::Parameter("PAW.enabled", false));
+  paw->initialize(node, std::string("PAW"));
+
+  // plugin is not enabled, should exit
+  EXPECT_TRUE(paw->processAtWaypoint(pose, 0));
+}