Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add preemption to rcl_action #679

Open
wants to merge 1 commit into
base: rolling
Choose a base branch
from
Open

Add preemption to rcl_action #679

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion rcl_action/include/rcl_action/types.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,8 @@ typedef int8_t rcl_action_goal_state_t;
#define GOAL_STATE_SUCCEEDED action_msgs__msg__GoalStatus__STATUS_SUCCEEDED
#define GOAL_STATE_CANCELED action_msgs__msg__GoalStatus__STATUS_CANCELED
#define GOAL_STATE_ABORTED action_msgs__msg__GoalStatus__STATUS_ABORTED
#define GOAL_STATE_NUM_STATES 7
#define GOAL_STATE_PREEMPTED action_msgs__msg__GoalStatus__STATUS_PREEMPTED
#define GOAL_STATE_NUM_STATES 8

/// User friendly error messages for invalid trasntions
// Description variables in types.c should be changed if enum values change
Expand All @@ -105,6 +106,7 @@ typedef enum rcl_action_goal_event_t
GOAL_EVENT_CANCEL_GOAL,
GOAL_EVENT_SUCCEED,
GOAL_EVENT_ABORT,
GOAL_EVENT_PREEMPT,
GOAL_EVENT_CANCELED,
GOAL_EVENT_NUM_EVENTS
} rcl_action_goal_event_t;
Expand Down
13 changes: 13 additions & 0 deletions rcl_action/src/rcl_action/goal_state_machine.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,17 @@ _abort_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t even
return GOAL_STATE_ABORTED;
}

rcl_action_goal_state_t
_preempt_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
{
assert(GOAL_STATE_EXECUTING == state || GOAL_STATE_CANCELING == state);
assert(GOAL_EVENT_PREEMPT == event);
// Avoid unused warnings, but keep asserts for debug purposes
(void)state;
(void)event;
return GOAL_STATE_PREEMPTED;
}

rcl_action_goal_state_t
_canceled_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
{
Expand All @@ -89,10 +100,12 @@ static rcl_action_goal_event_handler
[GOAL_EVENT_CANCEL_GOAL] = _cancel_goal_event_handler,
[GOAL_EVENT_SUCCEED] = _succeed_event_handler,
[GOAL_EVENT_ABORT] = _abort_event_handler,
[GOAL_EVENT_PREEMPT] = _preempt_event_handler,
},
[GOAL_STATE_CANCELING] = {
[GOAL_EVENT_SUCCEED] = _succeed_event_handler,
[GOAL_EVENT_ABORT] = _abort_event_handler,
[GOAL_EVENT_PREEMPT] = _preempt_event_handler,
Copy link
Collaborator

Choose a reason for hiding this comment

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

not sure canceling state can be preempted.

[GOAL_EVENT_CANCELED] = _canceled_event_handler,
},
};
Expand Down
4 changes: 2 additions & 2 deletions rcl_action/src/rcl_action/types.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -134,10 +134,10 @@ rcl_action_cancel_response_fini(rcl_action_cancel_response_t * cancel_response)

/// Values should be changed if enum values change
const char * goal_state_descriptions[] =
{"UNKNOWN", "ACCEPTED", "EXECUTING", "CANCELING", "SUCCEEDED", "CANCELED", "ABORTED"};
{"UNKNOWN", "ACCEPTED", "EXECUTING", "CANCELING", "SUCCEEDED", "CANCELED", "ABORTED", "PREEMPTED"};

const char * goal_event_descriptions[] =
{"EXECUTE", "CANCEL_GOAL", "SUCCEED", "ABORT", "CANCELED", "NUM_EVENTS"};
{"EXECUTE", "CANCEL_GOAL", "SUCCEED", "ABORT", "PREEMPT", "CANCELED", "NUM_EVENTS"};

#ifdef __cplusplus
}
Expand Down
18 changes: 17 additions & 1 deletion rcl_action/test/rcl_action/test_goal_handle.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -174,7 +174,7 @@ using EventStateActiveCancelableTuple =
std::tuple<rcl_action_goal_event_t, rcl_action_goal_state_t, bool, bool>;
using StateTransitionSequence = std::vector<EventStateActiveCancelableTuple>;
const std::vector<std::string> event_strs = {
"EXECUTE", "CANCEL_GOAL", "SUCCEED", "ABORT", "CANCELED"};
"EXECUTE", "CANCEL_GOAL", "SUCCEED", "ABORT", "PREEMPT", "CANCELED"};

class TestGoalHandleStateTransitionSequence
: public ::testing::TestWithParam<StateTransitionSequence>
Expand Down Expand Up @@ -292,6 +292,11 @@ const StateTransitionSequence valid_state_transition_sequences[] = {
std::make_tuple(GOAL_EVENT_CANCEL_GOAL, GOAL_STATE_CANCELING, true, false),
std::make_tuple(GOAL_EVENT_ABORT, GOAL_STATE_ABORTED, false, false),
},
{
std::make_tuple(GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING, true, true),
std::make_tuple(GOAL_EVENT_CANCEL_GOAL, GOAL_STATE_CANCELING, true, false),
std::make_tuple(GOAL_EVENT_PREEMPT, GOAL_STATE_PREEMPTED, false, false),
},
{
std::make_tuple(GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING, true, true),
std::make_tuple(GOAL_EVENT_SUCCEED, GOAL_STATE_SUCCEEDED, false, false),
Expand All @@ -300,6 +305,10 @@ const StateTransitionSequence valid_state_transition_sequences[] = {
std::make_tuple(GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING, true, true),
std::make_tuple(GOAL_EVENT_ABORT, GOAL_STATE_ABORTED, false, false),
},
{
std::make_tuple(GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING, true, true),
std::make_tuple(GOAL_EVENT_PREEMPT, GOAL_STATE_PREEMPTED, false, false),
},
{
std::make_tuple(GOAL_EVENT_CANCEL_GOAL, GOAL_STATE_CANCELING, true, false),
std::make_tuple(GOAL_EVENT_CANCELED, GOAL_STATE_CANCELED, false, false),
Expand All @@ -308,6 +317,10 @@ const StateTransitionSequence valid_state_transition_sequences[] = {
std::make_tuple(GOAL_EVENT_CANCEL_GOAL, GOAL_STATE_CANCELING, true, false),
std::make_tuple(GOAL_EVENT_ABORT, GOAL_STATE_ABORTED, false, false),
},
{
std::make_tuple(GOAL_EVENT_CANCEL_GOAL, GOAL_STATE_CANCELING, true, false),
std::make_tuple(GOAL_EVENT_PREEMPT, GOAL_STATE_PREEMPTED, false, false),
},
// This is an odd case, but valid nonetheless
{
std::make_tuple(GOAL_EVENT_CANCEL_GOAL, GOAL_STATE_CANCELING, true, false),
Expand Down Expand Up @@ -345,6 +358,9 @@ const StateTransitionSequence invalid_state_transition_sequences[] = {
{
std::make_tuple(GOAL_EVENT_ABORT, GOAL_STATE_UNKNOWN, false, false),
},
{
std::make_tuple(GOAL_EVENT_PREEMPT, GOAL_STATE_UNKNOWN, false, false),
},
};

INSTANTIATE_TEST_CASE_P(
Expand Down
50 changes: 50 additions & 0 deletions rcl_action/test/rcl_action/test_goal_state_machine.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,10 @@ TEST(TestGoalStateMachine, test_valid_transitions)
GOAL_STATE_EXECUTING,
GOAL_EVENT_ABORT);
EXPECT_EQ(GOAL_STATE_ABORTED, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_EXECUTING,
GOAL_EVENT_PREEMPT);
EXPECT_EQ(GOAL_STATE_PREEMPTED, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_CANCELING,
GOAL_EVENT_CANCELED);
Expand All @@ -46,6 +50,10 @@ TEST(TestGoalStateMachine, test_valid_transitions)
GOAL_STATE_CANCELING,
GOAL_EVENT_ABORT);
EXPECT_EQ(GOAL_STATE_ABORTED, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_CANCELING,
GOAL_EVENT_PREEMPT);
EXPECT_EQ(GOAL_STATE_PREEMPTED, state);
}

TEST(TestGoalStateMachine, test_invalid_transitions)
Expand All @@ -59,6 +67,10 @@ TEST(TestGoalStateMachine, test_invalid_transitions)
GOAL_STATE_ACCEPTED,
GOAL_EVENT_ABORT);
EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_ACCEPTED,
GOAL_EVENT_PREEMPT);
EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_ACCEPTED,
GOAL_EVENT_CANCELED);
Expand Down Expand Up @@ -101,6 +113,10 @@ TEST(TestGoalStateMachine, test_invalid_transitions)
GOAL_STATE_SUCCEEDED,
GOAL_EVENT_ABORT);
EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_SUCCEEDED,
GOAL_EVENT_PREEMPT);
EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_SUCCEEDED,
GOAL_EVENT_CANCELED);
Expand All @@ -125,6 +141,36 @@ TEST(TestGoalStateMachine, test_invalid_transitions)
EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_ABORTED,
GOAL_EVENT_PREEMPT);
EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_ABORTED,
GOAL_EVENT_CANCELED);
EXPECT_EQ(GOAL_STATE_UNKNOWN, state);

// Invalid from PREEMPTED
state = rcl_action_transition_goal_state(
GOAL_STATE_PREEMPTED,
GOAL_EVENT_EXECUTE);
EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_PREEMPTED,
GOAL_EVENT_CANCEL_GOAL);
EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_PREEMPTED,
GOAL_EVENT_SUCCEED);
EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_PREEMPTED,
GOAL_EVENT_ABORT);
EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_PREEMPTED,
GOAL_EVENT_PREEMPT);
EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_PREEMPTED,
GOAL_EVENT_CANCELED);
EXPECT_EQ(GOAL_STATE_UNKNOWN, state);

Expand All @@ -145,6 +191,10 @@ TEST(TestGoalStateMachine, test_invalid_transitions)
GOAL_STATE_CANCELED,
GOAL_EVENT_ABORT);
EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_CANCELED,
GOAL_EVENT_PREEMPT);
EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
state = rcl_action_transition_goal_state(
GOAL_STATE_CANCELED,
GOAL_EVENT_CANCELED);
Expand Down