Merge pull request #1 from AIResearchLab/nav-runner

added waypoint runner and modified action_runner

Author: mik-p

capabilities2_runner/include/capabilities2_runner/action_runner.hpp

@@ -1,7 +1,12 @@
 #pragma once
 
-#include <capabilities2_runner/runner_base.hpp>
+#include <thread>
+
+#include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
+#include <action_msgs/srv/cancel_goal.hpp>
+
+#include <capabilities2_runner/runner_base.hpp>
 
 namespace capabilities2_runner
 {
@@ -10,71 +15,136 @@ namespace capabilities2_runner
  * @brief action runner base class
  *
  * Create an action client to run an action based capability
- *
  */
 template <typename ActionT>
 class ActionRunner : public RunnerBase
 {
 public:
-  ActionRunner() : RunnerBase()
-  {
-  }
-
-  // helpers
-  // init action base members
-  void init_action(rclcpp::Node::SharedPtr node, const runner_opts& opts, const std::string& action_type)
-  {
-    // store node pointer and opts
-    init_base(node, opts);
-
-    // create an action client
-    action_client_ = rclcpp_action::create_client<ActionT>(node_, get_action_name_by_type(action_type));
-
-    // wait for action server
-    RCLCPP_INFO(node_->get_logger(), "%s waiting for action: %s", run_config_.interface.c_str(),
-                get_action_name_by_type(action_type).c_str());
-
-    if (!action_client_->wait_for_action_server())
-    {
-      RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to action server", run_config_.interface.c_str());
-      throw runner_exception("failed to connect to action server");
-    }
-  }
-
-  // find resource name by action type
-  std::string get_action_name_by_type(const std::string& action_type)
-  {
-    for (const auto& resource : run_config_.resources)
-    {
-      if (resource.resource_type == "action")
-      {
-        if (resource.msg_type == action_type)
-        {
-          return resource.name;
-        }
-      }
-    }
-
-    throw runner_exception("no action resource found: " + action_type);
-  }
-
-  // get first action resource
-  std::string get_first_action_name()
-  {
-    for (const auto& resource : run_config_.resources)
-    {
-      if (resource.resource_type == "action")
-      {
-        return resource.name;
-      }
-    }
-
-    throw runner_exception("no action resources found for interface: " + run_config_.interface);
-  }
+
+	/**
+	 * @brief Constructor which needs to be empty due to plugin semantics
+	 */
+	ActionRunner() : RunnerBase()
+	{}
+
+	/**
+	 * @brief Initializer function for initializing the action runner in place of constructor due to plugin semantics
+	 * 
+	 * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities
+   	 * @param run_config runner configuration loaded from the yaml file
+	 * @param action_name action name used in the yaml file, used to load specific configuration from the run_config
+	 * @param on_started function pointer to trigger at the start of the action client in the runner
+	 * @param on_terminated function pointer to trigger at the termination of the action client in the runner
+	 */
+	virtual void init_action(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& action_name, 
+					 std::function<void(const std::string&)> on_started = nullptr,
+                     std::function<void(const std::string&)> on_terminated = nullptr)
+	{
+		// initialize the runner base by storing node pointer and run config
+		init_base(node, run_config);
+
+		// initialize the used for message timestamps
+		clock_ = node_->get_clock();
+
+		// create an action client
+		action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name);
+
+		// wait for action server
+		RCLCPP_INFO(node_->get_logger(), "%s waiting for action: %s", run_config_.interface.c_str(), action_name.c_str());
+
+		if (!action_client_->wait_for_action_server(std::chrono::seconds(3)))
+		{
+			RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to action server", run_config_.interface.c_str());
+			throw runner_exception("failed to connect to action server");
+		}
+
+		// goal response callback
+		send_goal_options_.goal_response_callback = 
+		[this, on_started, on_terminated](const typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr& goal_handle) 
+		{
+			// store goal handle to be used with stop funtion
+			goal_handle_ = goal_handle;
+		};
+        
+		// result callback
+		send_goal_options_.result_callback =
+		[this, on_started, on_terminated](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult& wrapped_result) 
+		{
+			if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED)
+			{
+				// publish event
+				if (on_started)
+				{
+					on_started(run_config_.interface);
+				}
+			} else
+			{
+				// send terminated event
+				if (on_terminated)
+				{
+					on_terminated(run_config_.interface);
+				}
+			}
+		};
+	}
+
+	virtual void stop(std::function<void(const std::string&)> on_stopped = nullptr) override
+	{
+		// if the node pointer is empty then throw an error
+		// this means that the runner was not started and is being used out of order
+
+		if (!node_)  throw runner_exception("cannot stop runner that was not started");
+
+		// throw an error if the action client is null
+		// this can happen if the runner is not able to find the action resource
+
+		if (!action_client_) throw runner_exception("cannot stop runner action that was not started");
+
+		// stop runner using action client
+		if (goal_handle_)
+		{
+			try
+			{
+				auto cancel_future = action_client_->async_cancel_goal(
+					goal_handle_, 
+					[this, on_stopped](action_msgs::srv::CancelGoal_Response::SharedPtr response) 
+						{
+							if (response->return_code != action_msgs::srv::CancelGoal_Response::ERROR_NONE)
+							{
+								// throw runner_exception("failed to cancel runner");
+							}
+
+							// publish event
+							if (on_stopped)
+							{
+								on_stopped(run_config_.interface);
+							}
+						}
+				);
+
+				// wait for action to be stopped. hold the thread for 2 seconds to help keep callbacks in scope
+				rclcpp::spin_until_future_complete(node_, cancel_future, std::chrono::seconds(2));
+			}
+			catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e)
+			{
+				throw runner_exception(e.what());
+			}
+		}
+	}
 
 protected:
-  // action client
-  std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
+
+	/**< action client */
+	std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
+
+	/**< rclcpp clock to get time */
+	rclcpp::Clock::SharedPtr clock_;
+
+	/**< Send Goal Option struct to link result_callback, feedback_callback and goal_response_callback with action client */
+	typename rclcpp_action::Client<ActionT>::SendGoalOptions send_goal_options_;
+
+	/**< goal handle parameter to capture goal response from goal_response_callback */
+	typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr goal_handle_;
 };
 
 }  // namespace capabilities2_runner

capabilities2_runner/include/capabilities2_runner/launch_runner.hpp

@@ -14,7 +14,6 @@ namespace capabilities2_runner
  * @brief launch runner base class
  *
  * Create a launch file runner to run a launch file based capability
- *
  */
 class LaunchRunner : public ActionRunner<capabilities2_msgs::action::Launch>
 {
@@ -23,12 +22,12 @@ class LaunchRunner : public ActionRunner<capabilities2_msgs::action::Launch>
   {
   }
 
-  virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& opts,
+  virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config,
                      std::function<void(const std::string&)> on_started = nullptr,
                      std::function<void(const std::string&)> on_terminated = nullptr) override
   {
-    // store node pointer and opts
-    init_base(node, opts);
+    // store node pointer and run_config
+    init_base(node, run_config);
 
     // create an action client
     action_client_ = rclcpp_action::create_client<capabilities2_msgs::action::Launch>(node_, "/capabilities_launch_"
@@ -76,6 +75,7 @@ class LaunchRunner : public ActionRunner<capabilities2_msgs::action::Launch>
           // store goal handle
           goal_handle_ = goal_handle;
         };
+        
     // result callback
     send_goal_options.result_callback =
         [this, on_started, on_terminated](

capabilities2_runner/include/capabilities2_runner/runner_base.hpp

@@ -41,7 +41,8 @@ struct resource
 /**
  * @brief runner options
  *
- * Contains the options required to start and maintain a consistent runner
+ * Contains the options required to start and maintain a consistent runner. normally 
+ * loaded from the yaml file
  *
  */
 struct runner_opts
@@ -71,59 +72,86 @@ class RunnerBase
   /**
    * @brief start the runner
    *
-   * @param node
-   * @param opts
-   * @param on_started
-   * @param on_terminated
+	 * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities
+   * @param run_config runner configuration loaded from the yaml file
+   * @param on_started pointer to function to execute on starting the runner
+   * @param on_terminated pointer to function to execute on terminating the runner
    */
-  virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& opts,
+  virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config,
                      std::function<void(const std::string&)> on_started = nullptr,
                      std::function<void(const std::string&)> on_terminated = nullptr) = 0;
 
   /**
    * @brief stop the runner
    *
-   * @param on_stopped
+   * @param on_stopped pointer to function to execute on stopping the runner
    */
   virtual void stop(std::function<void(const std::string&)> on_stopped = nullptr) = 0;
 
-  // helpers
-  // init base members
-  void init_base(rclcpp::Node::SharedPtr node, const runner_opts& opts)
+
+	/**
+	 * @brief Initializer function for initializing the base runner in place of constructor due to plugin semantics
+	 * 
+	 * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities
+   * @param run_config runner configuration loaded from the yaml file
+	 */
+  void init_base(rclcpp::Node::SharedPtr node, const runner_opts& run_config)
   {
     // store node pointer and opts
     node_ = node;
-    run_config_ = opts;
+    run_config_ = run_config;
   }
 
+  /**
+   * @brief get the package name to which the runner belong to.
+   */
   const std::string get_package_name()
   {
     return run_config_.interface.substr(0, run_config_.interface.find("/"));
   }
 
-  // getters
+  /**
+   * @brief get the interface of the runner.
+   */
   const std::string& get_interface() const
   {
     return run_config_.interface;
   }
 
+  /**
+   * @brief get the provider of the runner.
+   */
   const std::string& get_provider() const
   {
     return run_config_.provider;
   }
 
+  /**
+   * @brief get the starter of the runner.
+   */
   const std::string& get_started_by() const
   {
     return run_config_.started_by;
   }
 
+  /**
+   * @brief get the pid of the runner.
+   */
   const std::string& get_pid() const
   {
     return run_config_.pid;
   }
 
 protected:
+
+	/** 
+	 * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities
+	 */
   rclcpp::Node::SharedPtr node_;
+
+	/** 
+	 * @param node runner configuration
+	 */
   runner_opts run_config_;
 };
 

capabilities2_runner/readme.md

@@ -8,6 +8,11 @@ provide runners for common capabilities like nav2, moveit2, etc. Plugins extend
 - `action runner` - runner for capabilities that are implemented as actions. This is also a base class for as most runners will be actions.
 - `launch runner` - runner for capabilities that are implemented as launch files.
 
+
+## Inheritance Diagram
+
+![inheritance diagram](/images/inheritance-diagram.png)
+
 ## creating a new runner
 
 New runners can be created to perform new capabilities. The runner can be specified in a capability provider as the `runner` tag: