rclcpp_action Server implementation (#593)

* Commiting to back up work, does not function * Can call user callback when goal request received * fini action server in destructor * rename user callback virtual functions * handle_execute test passes * Remove out of date comment * Refactor execute into three functions * Remove unused file * Add failing cancel test * Cancel test passes * Remove out of date comments * Make sure server publishes status when accepting a goal * Send status when goals transition to cancelling * Refactored sending goal request to its own function * Refactor cancel request into it's own function * Comment with remaining tests * Executing and terminal state statuses * publish feedback works * server sends result to clients that request it * Remove out of date comment * Add ServerGoalHandle::is_active() * Cleanup when goals expire * Can pass in action server options * cpplint and uncrustify fixes * Fix clang warnings * Copy rcl goal handle * Fix clang warning * Use intermediate value to avoid left shift on 32bit integer * RCLCPP_ACTION_PUBLIC everwhere * Change callback parameter from C type to C++ * Add accessors for request and uuid * Feedback must include goal id * Document Server<> and ServerBase<> * handle_execute -> handle_accepted * Test deferred execution * only publish feedback if goal is executing * Documentation for ServerGoalHandle * document msg parameters * remove unnecessary fini * notify_goal_done only takes server * Use unique_indentifier_msgs * create_server accepts group and removes waitable * uncrustify * Use weak ptr to avoid crash if goal handle lives longer than server * Handle goal callback const message * Goal handle doesn't have server pointer anymore * Lock goal_handles_ on Server<> * rcl_action_server_t protected with mutex * ServerBase results protected with mutex * protect rcl goal handle with mutex * is_cancel_request -> is_canceling * Add missing include * use GoalID and change uuid -> goal_id * Keep rcl goal handle alive until it expires on server * uncrustify * Move UUID hash * Log messages in server * ACTION -> ActionT * Cancel abandoned goal handles * Add convert() for C and C++ goal id * Remove unused variable * Constant reference * Move variable declaration down * is_ready if goal expired * map[] default constructs if it doesn't exist * Use rcl_action_get_goal_status_array() * Array -> GoalID * Use reentrant mutex for everything * comment * scope exit to fini cancel response * using GoalID
2018-12-06 09:38:01 -08:00 · 2018-12-06 09:38:01 -08:00 · fe09d937b7
commit fe09d937b7
parent 91167393ea
10 changed files with 2017 additions and 137 deletions
--- a/rclcpp_action/src/server_goal_handle.cpp
+++ b/rclcpp_action/src/server_goal_handle.cpp
@ -12,4 +12,140 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.

+#include <rcl_action/action_server.h>
+#include <rcl_action/goal_handle.h>
+
 #include <rclcpp_action/server_goal_handle.hpp>
+#include <rclcpp/exceptions.hpp>
+
+#include <memory>
+
+namespace rclcpp_action
+{
+ServerGoalHandleBase::~ServerGoalHandleBase()
+{
+}
+
+bool
+ServerGoalHandleBase::is_canceling() const
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
+  rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state");
+  }
+  return GOAL_STATE_CANCELING == state;
+}
+
+bool
+ServerGoalHandleBase::is_active() const
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  return rcl_action_goal_handle_is_active(rcl_handle_.get());
+}
+
+bool
+ServerGoalHandleBase::is_executing() const
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
+  rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state");
+  }
+  return GOAL_STATE_EXECUTING == state;
+}
+
+void
+ServerGoalHandleBase::_set_aborted()
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_ABORTED);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerGoalHandleBase::_set_succeeded()
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_SUCCEEDED);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerGoalHandleBase::_set_canceling()
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerGoalHandleBase::_set_canceled()
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_CANCELED);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerGoalHandleBase::_set_executing()
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_EXECUTE);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+bool
+ServerGoalHandleBase::try_canceling() noexcept
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  // Check if the goal reached a terminal state already
+  const bool active = rcl_action_goal_handle_is_active(rcl_handle_.get());
+  if (!active) {
+    return false;
+  }
+
+  rcl_ret_t ret;
+
+  // Get the current state
+  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
+  ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
+  if (RCL_RET_OK != ret) {
+    return false;
+  }
+
+  // If it's not already canceling then transition to that state
+  if (GOAL_STATE_CANCELING != state) {
+    ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL);
+    if (RCL_RET_OK != ret) {
+      return false;
+    }
+  }
+
+  // Get the state again
+  ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
+  if (RCL_RET_OK != ret) {
+    return false;
+  }
+
+  // If it's canceling, cancel it
+  if (GOAL_STATE_CANCELING == state) {
+    ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_CANCELED);
+    return RCL_RET_OK == ret;
+  }
+
+  return false;
+}
+}  // namespace rclcpp_action