rclcpp_action Server implementation #593

sloretz · 2018-11-27T20:23:59Z

The example server in PR ros2/examples#216 has been updated to work with this PR https://github.com/ros2/examples/blob/75e3f7d4c1d4f26853fe5c894d3940e7cd7af1b1/rclcpp/minimal_action_server/not_composable.cpp

This PR requires #594 in a convoluted way: checkout that branch and merge this one into it.

requires ros2/rcl#343

sloretz · 2018-11-29T03:19:17Z

Implementation is almost complete, running CI to check other platforms.

Linux
Linux-aarch64
macOS
Windows

sloretz · 2018-11-29T20:55:54Z

Checking clang warnings on osx

sloretz · 2018-11-29T23:16:43Z

CI again

Linux
Linux-aarch64
macOS
Windows

rclcpp_action/src/server.cpp

jacobperron

Partial review. Just some minor comments, LGTM!

rclcpp_action/include/rclcpp_action/create_server.hpp

rclcpp_action/include/rclcpp_action/server.hpp

rclcpp_action/include/rclcpp_action/server_goal_handle.hpp

rclcpp_action/include/rclcpp_action/server.hpp

jacobperron · 2018-12-05T00:48:41Z

rclcpp_action/src/server.cpp

+      {
+        if (nullptr != ptr) {
+          rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr);
+          (void)fail_ret;  // TODO(sloretz) do something with error during cleanup


Logging an error message seems like the only thing to do here.

rclcpp_action/src/server.cpp

jacobperron · 2018-12-05T01:31:14Z

rclcpp_action/src/server.cpp

+}
+
+void
+ServerBase::publish_status()


Just a thought:

IIRC, there was discussion in the design doc about making the status (and feedback) publisher optional. I'm not sure if we had any ideas about how the implementation would look, but given time constraints it's probably good to capture this feature in a ticket to tackle at a later time.
It might make sense to instead have the option to disable subscribers in the action client so then CLI tools can always interact with action servers.

jacobperron · 2018-12-05T01:40:22Z

rclcpp_action/src/server.cpp

+ServerBase::publish_result(const std::array<uint8_t, 16> & uuid, std::shared_ptr<void> result_msg)
+{
+  // Check that the goal exists
+  rcl_action_goal_info_t goal_info;


Since the uuid -> goal_info conversion is happening in several places, it would be nice to wrap in a utility function. Perhaps after we have the GoalID wrapper introduced by #594

One observation: we use GoalID everywhere, but the other fields of GoalInfo are in many places meaningless.

Added conversion function in f717e9b

jacobperron · 2018-12-05T01:44:55Z

rclcpp_action/test/test_server.cpp


  using GoalHandle = rclcpp_action::ServerGoalHandle<test_msgs::action::Fibonacci>;
-  auto as = rclcpp_action::create_server<test_msgs::action::Fibonacci>(node.get(), "fibonacci",
-      [](rcl_action_goal_info_t &, test_msgs::action::Fibonacci::Goal *) {},
+  auto as = rclcpp_action::create_server<test_msgs::action::Fibonacci>(node, "fibonacci",


We could add tests for invalid constructions (e.g. invalid node or action name).

rclcpp_action/include/rclcpp_action/server_goal_handle.hpp

hidmic

Leaving some comments, awesome work @sloretz !

hidmic · 2018-12-05T23:26:18Z

rclcpp_action/include/rclcpp_action/server.hpp

+  {
+    std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+    CancelResponse resp = CancelResponse::REJECT;
+    std::shared_ptr<rcl_action_goal_handle_t> rcl_handle;


@sloretz unused?

Removed in 03c9cf0

hidmic · 2018-12-05T23:39:11Z

rclcpp_action/include/rclcpp_action/server_goal_handle.hpp

+  }
+
+  /// Get the unique identifier of the goal
+  const GoalID


@sloretz why not const GoalID & instead?

Constant reference returned in ea33c88

hidmic · 2018-12-05T23:41:30Z

rclcpp_action/src/server.cpp

-  (void)node_base;
-  (void)name;
-  (void)type_support;
+  rcl_ret_t ret;


@sloretz consider not declaring ret until you can assign something to it (e.g. line 91 below).

Moved below in 8a21dec

hidmic · 2018-12-05T23:42:56Z

rclcpp_action/src/server.cpp

+
+  return pimpl_->goal_request_ready_ ||
+         pimpl_->cancel_request_ready_ ||
+         pimpl_->result_request_ready_;


@sloretz looks like pimpl_->goal_expired_ is missing.

Whoops, added in bf4797a

hidmic · 2018-12-05T23:43:47Z

rclcpp_action/src/server.cpp

+void
+ServerBase::execute_goal_request_received()
+{
+  rcl_ret_t ret;


@sloretz same suggestion about scoping ret

hidmic · 2018-12-06T00:14:49Z

rclcpp_action/src/types.cpp

+    stream << static_cast<int>(element);
+  }
+  return stream.str();
+}


hidmic · 2018-12-06T00:17:26Z

rclcpp_action/test/test_server.cpp

+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      [](const std::array<uint8_t, 16> &, std::shared_ptr<const Fibonacci::Goal>) {


@sloretz consider using GoalID instead of std::array<...>.

GoalID in 71c7100

hidmic · 2018-12-06T00:18:28Z

rclcpp_action/test/test_server.cpp

+
+  auto request = std::make_shared<Fibonacci::GoalRequestService::Request>();
+
+  const std::array<uint8_t, 16> uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};


@sloretz consider making this a constant member field in the fixture class.

hidmic · 2018-12-06T00:23:22Z

rclcpp_action/include/rclcpp_action/server.hpp

+   * Three callbacks must be provided:
+   *  - one to accept or reject goals sent to the server,
+   *  - one to accept or reject requests to cancel a goal,
+   *  - one to given a goal handle after a goal has been accepted.


Not sure I understand what the third bullet means.

Reworded in de1510b

hidmic · 2018-12-06T00:25:26Z

rclcpp_action/include/rclcpp_action/server.hpp

+    GoalID uuid, std::shared_ptr<void> goal_request_message) override
+  {
+    std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle;
+    std::weak_ptr<Server<ActionT>> weak_this = this->shared_from_this();


Hmm, so neither the server gets a hold of the goal handle, nor the handle gets a hold of the server. Is that right?

Yeah, this is making sure the server or server goal handle can be safely destructed in either order.

sloretz · 2018-12-06T01:45:51Z

All comments so far addressed, checking CI

Linux
Linux-aarch64
macOS
Windows

sloretz · 2018-12-06T02:36:30Z

Windows attempt 2

sloretz · 2018-12-06T03:01:42Z

Remaining CI jobs

Linux
Linux-aarch64
macOS

jacobperron · 2018-12-06T17:14:49Z

rclcpp_action/src/server_goal_handle.cpp

+{
+  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());


We can use a short-cut and check if the goal is cancelable:

const bool is_cancelable = rcl_action_goal_handle_is_cancelable(rcl_handle.get());

What's the short cut? IIUC it looks like the code below would be the same.

If the goal can be transitioned to canceling, then we don't need to get the state at L124 and check if the state is GOAL_STATE_CANCELING at L130. But in the case is_cancelable is false, then we still need to check I guess.

I think this is equivalent to the current implementation:

const bool is_cancelable = rcl_action_goal_handle_is_cancelable(rcl_handle_.get()); if (is_cancelable) { ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL); if (RCL_RET_OK != ret) { return false; } } // Get the state 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;

Mind making a follow up PR with this?

jacobperron

LGTM

* 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

Signed-off-by: Dan Rose <dan@digilabs.io>

…bject (ros2#593) Signed-off-by: Josh Langsfeld <josh.langsfeld@gmail.com>

sloretz added the in progress Actively being worked on (Kanban column) label Nov 27, 2018

sloretz added this to the crystal milestone Nov 27, 2018

sloretz self-assigned this Nov 27, 2018

sloretz mentioned this pull request Nov 29, 2018

Add timer to action server to check expired goals + asan fixes ros2/rcl#343

Merged

sloretz force-pushed the rclcpp_action_server branch from 784edbd to 6747d0a Compare November 30, 2018 16:54

sloretz commented Nov 30, 2018

View reviewed changes

rclcpp_action/src/server.cpp Outdated Show resolved Hide resolved

sloretz force-pushed the rclcpp_action_server branch from 7583828 to ca19322 Compare December 4, 2018 21:05

wjwwood mentioned this pull request Dec 4, 2018

add wait_for_action_server() for action clients #598

Merged

jacobperron self-requested a review December 4, 2018 23:07

jacobperron reviewed Dec 5, 2018

View reviewed changes

wjwwood reviewed Dec 5, 2018

View reviewed changes

rclcpp_action/include/rclcpp_action/server_goal_handle.hpp Outdated Show resolved Hide resolved

wjwwood reviewed Dec 5, 2018

View reviewed changes

rclcpp_action/include/rclcpp_action/server.hpp Outdated Show resolved Hide resolved

jacobperron reviewed Dec 5, 2018

View reviewed changes

wjwwood reviewed Dec 5, 2018

View reviewed changes

rclcpp_action/include/rclcpp_action/server_goal_handle.hpp Outdated Show resolved Hide resolved

wjwwood mentioned this pull request Dec 5, 2018

add Fibonacci test for actions ros2/system_tests#316

Merged

sloretz added 12 commits December 5, 2018 14:52

Commiting to back up work, does not function

5ffe3e8

Can call user callback when goal request received

0967eda

fini action server in destructor

c2338f0

rename user callback virtual functions

8854af3

handle_execute test passes

f752de0

Remove out of date comment

6962024

Refactor execute into three functions

5a4cc1b

Remove unused file

9e5d0d6

Add failing cancel test

36c3825

Cancel test passes

c508c6d

Remove out of date comments

6e603e9

Make sure server publishes status when accepting a goal

9565203

uncrustify

63eff6c

sloretz force-pushed the rclcpp_action_server branch from 70ccd21 to 63eff6c Compare December 5, 2018 22:52

sloretz added 4 commits December 5, 2018 14:58

Move UUID hash

054b818

Log messages in server

ac82e36

ACTION -> ActionT

ad570dc

Cancel abandoned goal handles

ec70b67

hidmic reviewed Dec 6, 2018

View reviewed changes

sloretz added 11 commits December 5, 2018 16:32

Add convert() for C and C++ goal id

f717e9b

Remove unused variable

03c9cf0

Constant reference

ea33c88

Move variable declaration down

8a21dec

is_ready if goal expired

bf4797a

map[] default constructs if it doesn't exist

ae676d8

Use rcl_action_get_goal_status_array()

039854d

Array -> GoalID

71c7100

Use reentrant mutex for everything

b7a3de1

comment

de1510b

scope exit to fini cancel response

1c8c86d

using GoalID

9ec3f39

jacobperron reviewed Dec 6, 2018

View reviewed changes

jacobperron approved these changes Dec 6, 2018

View reviewed changes

sloretz merged commit fe09d93 into master Dec 6, 2018

sloretz deleted the rclcpp_action_server branch December 6, 2018 17:38

jacobperron mentioned this pull request Dec 7, 2018

Refactor server goal handle's try_canceling() function #603

Merged

nnmm pushed a commit to ApexAI/rclcpp that referenced this pull request Jul 9, 2022

Don't check history depth if RMW_QOS_POLICY_HISTORY_KEEP_ALL (ros2#593)

4835a94

Signed-off-by: Dan Rose <dan@digilabs.io>

DensoADAS pushed a commit to DensoADAS/rclcpp that referenced this pull request Aug 5, 2022

Use std::filesystem compliant non-member exists function for path o…

64a39ab

…bject (ros2#593) Signed-off-by: Josh Langsfeld <josh.langsfeld@gmail.com>


		auto request = std::make_shared<Fibonacci::GoalRequestService::Request>();

		const std::array<uint8_t, 16> uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};

rclcpp_action Server implementation #593

rclcpp_action Server implementation #593

Conversation

sloretz commented Nov 27, 2018 • edited Loading

sloretz commented Nov 29, 2018

sloretz commented Nov 29, 2018

sloretz commented Nov 29, 2018

jacobperron left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hidmic left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sloretz commented Dec 6, 2018

sloretz commented Dec 6, 2018

sloretz commented Dec 6, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jacobperron Dec 6, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jacobperron left a comment

Choose a reason for hiding this comment

sloretz commented Nov 27, 2018 •

edited

Loading

jacobperron Dec 6, 2018 •

edited

Loading