diff --git a/rcl_action/include/rcl_action/types.h b/rcl_action/include/rcl_action/types.h index 47785fea2..f9cb6f981 100644 --- a/rcl_action/include/rcl_action/types.h +++ b/rcl_action/include/rcl_action/types.h @@ -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 @@ -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; diff --git a/rcl_action/src/rcl_action/goal_state_machine.c b/rcl_action/src/rcl_action/goal_state_machine.c index 4b9501f5d..de908bba1 100644 --- a/rcl_action/src/rcl_action/goal_state_machine.c +++ b/rcl_action/src/rcl_action/goal_state_machine.c @@ -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) { @@ -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, [GOAL_EVENT_CANCELED] = _canceled_event_handler, }, }; diff --git a/rcl_action/src/rcl_action/types.c b/rcl_action/src/rcl_action/types.c index 03c9cbeef..d09cb2d7d 100644 --- a/rcl_action/src/rcl_action/types.c +++ b/rcl_action/src/rcl_action/types.c @@ -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 } diff --git a/rcl_action/test/rcl_action/test_goal_handle.cpp b/rcl_action/test/rcl_action/test_goal_handle.cpp index e7659aa0a..c6c1b4f07 100644 --- a/rcl_action/test/rcl_action/test_goal_handle.cpp +++ b/rcl_action/test/rcl_action/test_goal_handle.cpp @@ -174,7 +174,7 @@ using EventStateActiveCancelableTuple = std::tuple; using StateTransitionSequence = std::vector; const std::vector event_strs = { - "EXECUTE", "CANCEL_GOAL", "SUCCEED", "ABORT", "CANCELED"}; + "EXECUTE", "CANCEL_GOAL", "SUCCEED", "ABORT", "PREEMPT", "CANCELED"}; class TestGoalHandleStateTransitionSequence : public ::testing::TestWithParam @@ -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), @@ -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), @@ -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), @@ -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( diff --git a/rcl_action/test/rcl_action/test_goal_state_machine.cpp b/rcl_action/test/rcl_action/test_goal_state_machine.cpp index ecfc20f54..f7b43f8d5 100644 --- a/rcl_action/test/rcl_action/test_goal_state_machine.cpp +++ b/rcl_action/test/rcl_action/test_goal_state_machine.cpp @@ -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); @@ -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) @@ -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); @@ -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); @@ -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); @@ -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);