Skip to content

Conversation

jacobperron
Copy link
Member

Connects to #306

This is a proposal of structs and functions for the core C implementation of ROS actions.
This is not a comprehensive set of function declarations, but I think there is enough of the core here for a review. In particular, it's missing utilities for adding actions to the wait set and generating UUIDs for action goals.

Let me know if there's any other core features that I have missed.

@jacobperron jacobperron added the in progress Actively being worked on (Kanban column) label Oct 18, 2018
@jacobperron jacobperron self-assigned this Oct 18, 2018
@jacobperron jacobperron added in review Waiting for review (Kanban column) and removed in progress Actively being worked on (Kanban column) labels Oct 18, 2018
@jacobperron
Copy link
Member Author

@mkhansen-intel FYI

@jacobperron
Copy link
Member Author

jacobperron commented Oct 18, 2018

Something I didn't consider until later (and reflect in TODOs) is to use generated definitions for the cancel service and status topic instead of defining structs for them:

// TODO(jacobperron): This could be replaced by a generated message in rcl_interfaces
typedef struct rcl_action_cancel_response_t

// TODO(jacobperron): This could be replaced by a generated message in rcl_interfaces
typedef struct rcl_action_status_t

// TODO(jacobperron): This could be replaced by a generated message in rcl_interfaces
typedef struct rcl_action_status_array_t

The main benefit of using generated code is to not have multiple definitions of the same structures in two places. Unless someone proposes a reason not to, I will update the functions related to canceling goals and taking/sending statuses to use types from rcl_interfaces/srv and rcl_interfaces/msg.

Copy link
Contributor

@sloretz sloretz left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

How does one get the servers/clients/subscribers from an action client or action server so they can be added to a wait set? Maybe add a function here to return the services/clients/subscribes that are part of a server or client?


find_package(ament_cmake_ros REQUIRED)

# find_package(action_msgs REQUIRED)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

action messagess could live in rcl_interfaces since that's where parameter messages live already


# Causes the visibility macros to use dllexport rather than dllimport,
# which is appropriate when building the dll but not consuming it.
# target_compile_definitions(${PROJECT_NAME} PRIVATE "RCL_LIFECYCLE_BUILDING_DLL")
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

RCL_ACTIONS_BUILDING_DLL

if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_lint_auto REQUIRED)
find_package(rcl REQUIRED)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

already called find_package(rcl REQUIRED) above

/**
* The defaults are:
*
* - qos = TODO(jacobperron): RFC where to define default? and what should it be?
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm a bit unfamiliar with the qos settings, but if I had to guess

For goal submision, getting a result, and cancelling a goal

{
  // Client will probably only have one response from the server to deal with at a time
  RMW_QOS_POLICY_HISTORY_KEEP_LAST,
  1,
  // It's really important client knows whether goal was accepted or rejected
  RMW_QOS_POLICY_RELIABILITY_RELIABLE,
  // TODO what affect does durability have on service calls?
  RMW_QOS_POLICY_DURABILITY_VOLATILE,
  // Use ros namespace conventions for actions by default
  // If a user does `true` here the action client probably shouldn't begin the name with `/_action/` either
  false
};

For status and feedback messages client side

{
  RMW_QOS_POLICY_HISTORY_KEEP_LAST,
  // Store a few samples in case client is processing them slowly
  10,
  // Default QoS seems to be reliaible
  RMW_QOS_POLICY_RELIABILITY_RELIABLE,
  RMW_QOS_POLICY_DURABILITY_VOLATILE,
  false
};

* <i>[1] for unique pairs of clients and goals, see above for more</i>
*
* \param[in] action_client handle to the client that will make the goal request
* \param[in] goal_info pointer to a struct containing meta-data about the goal
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Might add a note that only the UUID needs to be populated, the time accepted won't be known until the service sends a response.

rcl_ret_t
rcl_action_goal_handle_fini(
rcl_action_goal_handle_t * goal_handle,
const rcl_action_server_t * action_server);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would expect goal_handle to have a reference to action_server already. That avoids accidentally passing the wrong action server in.

RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_action_update_goal_state(
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Does rcl_action_update_goal_state get called before or after rcl_action_send_result_response?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That's a good question. I was imagining that the implementation of rcl_action_send_result_response could call this function so the user doesn't have to think about it (ie. before the client receives the message). The same goes for processing cancel request; rcl_action_send_cancel_response or some utility function will handle calling rcl_action_update_goal_state accordingly. I should update the documentation to be explicit about this behavior (and remove some const qualifiers).

I thought it would be a nice way to enforce state changes on service 'events', but perhaps bundling state changes with the service functions adds too much complexity.

rcl_ret_t
rcl_action_goal_handle_get_message(
const rcl_action_goal_handle_t * goal_handle,
void * ros_goal);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Where is the original request message used later?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I thought this might be convenient for a client library. I was mimicking the API from ROS 1: https://github.com/ros/actionlib/blob/d317e63ebabc6c437f8b9f73ece06acf348700ea/include/actionlib/server/server_goal_handle.h#L137

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm removing this to simplify implementation since this is the only place where typesupport for goal handles comes into play and it does not seem necessary.

GOAL_STATE_CANCELED,
GOAL_STATE_ABORTED,
GOAL_STATE_NUM_STATES,
GOAL_STATE_INVALID
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would put invalid at 0, some compilers will zero initialize if building without optimizations, which could help catch bugs during development

@jacobperron
Copy link
Member Author

jacobperron commented Oct 19, 2018

Something I didn't consider until later (and reflect in TODOs) is to use generated definitions for the cancel service and status topic instead of defining structs for them:

Summarizing an offline discussion with @wjwwood:

  • Using rcl structs for message passing would not allow client libraries in other languages to use their native service and topic facilities. This means we definitely should pass around void * types for the generated types.
  • Passing around generated types means that rcl will not have access to the messages unless the client libraries first convert the messages to rcl types. Ultimately, we could provide conversion functions alongside message generation (in rosidl?), but as a stopgap write the conversions in the client libraries.
  • If we want to keep logic (e.g. the cancel policy) in rcl, then it would be best to provide some utility functions in rcl for operating on requests/responses after they have been converted and passed back from the client library.

Major refactor of action client and action server APIs:
  - Using void pointers for message passing to leverage client library type support
  - Provide functions for operating on C types in order to maintain some logic/state in rcl
Copy link
Contributor

@hidmic hidmic left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've left a few small comments. I apologize up front in case I've misunderstood a part of it.

BTW where is blocking usually implemented? I've seen the same TODOs about it in the current rcl API for services.

/**
* After calling this function on a rcl_action_server_t, it can be used to take
* goals of the given type for the given action name using rcl_action_take_goal_request()
* and take cancel requests with rcl_aciton_take_cancel_request().
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@jacobperron rcl_aciton_take_cancel_request() -> rcl_action_take_cancel_request()

rcl_action_goal_handle_t *
rcl_action_accept_new_goal(
const rcl_action_server_t * action_server,
const rcl_action_goal_info_t * goal_info,
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@jacobperron so, if I follow the logic correctly, moments after a successful rcl_action_take_goal_request() call, rcl_action caller must populate an rcl_action_goal_info_t instance appropriately to let the server know of the goal. Is that right? If so, it'd be nice to be explicit about it.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, this is correct. I will makes sure this is documented. And add an init/fini for the rcl_action_goal_info_t

rcl_action_process_cancel_request(
const rcl_action_server_t * action_server,
const rcl_action_cancel_request_t * cancel_request,
rcl_action_cancel_response_t * cancel_response);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@jacobperron same as above, if I'm right about these base action types then I think it'd be nice to have it documented.

* - If the goal ID is not zero and timestamp is zero, cancel the goal with the
* given ID regardless of the time it was accepted.
* - If the goal ID is not zero and timestamp is not zero, cancel the goal with the
* given ID and all goals accepted at or before the timestamp.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@jacobperron would it make sense to expose cancelling operations on a per goal handle basis too and build rcl_action_process_cancel_request() on top of them instead?

Copy link
Member Author

@jacobperron jacobperron Oct 22, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The goal handle's state can be updated with rcl_action_update_goal_state(), which I plan to call from this function (i.e. set the goal handles state to CANCELING).

rcl_ret_t
rcl_action_send_goal_request(
const rcl_action_client_t * action_client,
const void * ros_goal_request);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@jacobperron as @sloretz suggested for rcl_action_take_feedback() below, may be useful to expose rmw_request_id_t if multiple goal requests are sent one after the other as in:

rcl_send_response(
const rcl_service_t * service,
rmw_request_id_t * response_header,
void * ros_response);

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Perhaps, I was assuming that the goal ID would be present in the ROS message (ros_goal_request). The client could use the goal ID to distinguish between multiple responses.

RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_action_send_cancel_request(
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@jacobperron same about exposing rmw_request_id_t

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Assuming we have a goal ID as part of the ROS request message, I'm not sure that we need to expose a rmw_request_id_t.

@jacobperron
Copy link
Member Author

jacobperron commented Oct 22, 2018

BTW where is blocking usually implemented? I've seen the same TODOs about it in the current rcl API for services.

@hidmic
I'm not sure myself, I've copied the TODOs over from the services implementation since actions are using services under the hood. I guess it is dependent on QoS settings configured in rmw.

@jacobperron
Copy link
Member Author

jacobperron commented Oct 22, 2018

@sloretz @hidmic Thanks for the reviews. I believe I've addressed all of your comments and I think this is ready for another round of review.

Copy link
Contributor

@sloretz sloretz left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Partial review, submitting to start discussion about wait set sooner

RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_wait_set_add_action_server(
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

nit rcl_action_wait_set_add_action_server since the other API's start with the package name

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Every time a client library spin() is called it calls rcl_wait_set_init(ws, num_subs, num_gcs, num_timers, num_clients, num_services) to allocate or reallocate array's in the wait set. Afterwards calls rcl_wait_set_add_*() set values in the arrays. rcl_wait_set_add_action_server() is the second part, but how does the client library know how many services/subscribers/timers/guard conditions etc will be added to the wait set?

Also, how does the client library know which services/subscribers/... should result in which call to the action server API? If a service belonging to an action server is ready how does the client library know to call rcl_action_server_take_cancel_request or rcl_action_server_take_goal_request? It would be convenient if the client library could notify the action server that an entity is ready without knowing what the action server was going to do with it.

A method like ..._action_server_get_num_entities(as, size_t *num_subscribers, size_t *num_timers, ...) which returns how many entities belong to the action server would help the client library count how many entities it needs to resize the wait set only once. Another group of methods like ..._action_server_notify_subscription_ready(as, rcl_subscription_t * sub) to notify the action server when an entity becomes ready would allow the client library to be more ignorant of the server implementation.

// Client library populates these with the free subs, servers, clients, etc
size_t num_subscriptions = 0u;
size_t num_servers = 0u;
size_t num_clients = 0u;
size_t num_timers = 0u;
size_t num_guard_conditions = 0u;

// For each action server..
{
  // rcl_action_server_t as;
  size_t as_num_subscriptions = 0u;
  size_t as_num_servers = 0u;
  size_t as_num_clients = 0u;
  size_t as_num_timers = 0u;
  size_t as_num_guard_conditions = 0u;

  size_t as_start_subscriptions = 0u;
  size_t as_start_servers = 0u;
  size_t as_start_clients = 0u;
  size_t as_start_timers = 0u;
  size_t as_start_guard_conditions = 0u;

  if (RCL_OK == rcl_action_server_get_num_entities(&as, &as_num_subscriptions, &as_num_guard_conditions, &as_num_timers, &as_num_clients, &as_num_servers)
  {
  // Indices in the wait set where this action server starts, client library needs to store this info for each action server
    as_start_subscriptions = num_subscriptions;
    as_start_servers = num_servers;
    as_start_clients = num_clients;
    as_start_timers = num_timers;
    as_start_guard_conditions = num_guard_conditions;

    // Add action server entities to the total
    num_subscriptions += as_num_subscriptions;
    num_servers += as_num_servers;
    num_clients += as_num_clients;
    num_timers += as_num_timers;
    num_guard_conditions += as_num_guard_conditions;
  }
}

// Client library can now resize the wait set
ret = rcl_wait_set_init(&ws, num_subscriptions, num_guard_conditions, num_timers, num_clients, num_services, allocator);
// error handling

ret = rcl_wait_t(ws, timeout);
// error handling

// Client library iterates through the wait set
for (size_t i = 0; i < num_subscriptions; ++i)
{
  // For each action server ...
  {
    if (i >= as_start_subscriptions && i < as_start_subscriptions + as_num_subscriptions; ++i)
    {
      // Action server subscription became ready, tell the server to do something with it
      rcl_action_notify_subscription_ready(&as, ws.subscriptions[i]);
    }
    // ... repeat for remaining entities
  }
}

That's a pretty verbose example, but it gives the client library enough info to do what it needs to. Maybe a struct or two to manage groups of waitable entities that need to be added to a wait set could shorten it?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Idea for waitset groups in #308

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah, I can see your point about providing a method for getting the number of services and subscriptions from an action server or action client. It's better than clients assuming the number.

Regarding "which services/subscribers?", I'm not sure how the client libraries will look when it comes to actions and wait sets (right now it's not clear to me without digging further), but I'll add some notify functions as you suggest. It could be the case that we come back and refactor to help with client library implementation.

RCL_PUBLIC
RCL_WARN_UNUSED
rcl_action_goal_info_t
rcl_action_get_zero_initialized_goal_info(void);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would suggest putting the zero initialization functions in the same file as the finalization functions. Something that zero initializes a struct is likely going to need to finalize it later.

@sloretz sloretz mentioned this pull request Oct 23, 2018
…me file

Also replaced typesupport struct defintitions with forward declarations.
@jacobperron jacobperron mentioned this pull request Oct 25, 2018
10 tasks
@jacobperron
Copy link
Member Author

jacobperron commented Oct 26, 2018

@sloretz
I have updated the wait set related functions. Rather than "notify" functions, since the client library is expect to take (and in some cases convert messages for rcl), I've added rcl_action_server_wait_set_get_entities_ready() and rcl_action_client_wait_set_get_entities_ready(). The idea is that the client library can call these functions after waiting so they know what rcl_action_take_* calls to make.

@jacobperron
Copy link
Member Author

CI:

  • Linux Build Status
  • Linux-aarch64 Build Status
  • macOS Build Status
  • Windows Build Status

@jacobperron
Copy link
Member Author

Fixed windows build after adding an inline that should have been there:

  • Linux Build Status
  • Linux-aarch64 Build Status
  • macOS Build Status
  • Windows Build Status

Copy link
Contributor

@sloretz sloretz left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I haven't looked too closely at the new changes, but since it's just headers in a new package and merging will make collaborating on the implementation easier this LGTM.

<name>rcl_action</name>
<version>0.5.0</version>
<description>Package containing a C-based ROS action implementation</description>
<maintainer email="karsten@osrfoundation.org">Karsten Knese</maintainer>
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@jacobperron Your name / email should go here.

@jacobperron jacobperron merged commit 451bf4a into master Oct 26, 2018
@jacobperron jacobperron deleted the prototype_actions branch October 26, 2018 22:55
@jacobperron jacobperron removed the in review Waiting for review (Kanban column) label Oct 26, 2018
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants