diff --git a/rcl_action/include/rcl_action/types.h b/rcl_action/include/rcl_action/types.h index d87b067d1..783165b4c 100644 --- a/rcl_action/include/rcl_action/types.h +++ b/rcl_action/include/rcl_action/types.h @@ -97,10 +97,10 @@ typedef int8_t rcl_action_goal_state_t; typedef enum rcl_action_goal_event_t { GOAL_EVENT_EXECUTE = 0, - GOAL_EVENT_CANCEL, - GOAL_EVENT_SET_SUCCEEDED, - GOAL_EVENT_SET_ABORTED, - GOAL_EVENT_SET_CANCELED, + GOAL_EVENT_CANCEL_GOAL, + GOAL_EVENT_SUCCEED, + GOAL_EVENT_ABORT, + GOAL_EVENT_CANCELED, GOAL_EVENT_NUM_EVENTS } rcl_action_goal_event_t; diff --git a/rcl_action/src/rcl_action/goal_handle.c b/rcl_action/src/rcl_action/goal_handle.c index 5add061a7..e456042bf 100644 --- a/rcl_action/src/rcl_action/goal_handle.c +++ b/rcl_action/src/rcl_action/goal_handle.c @@ -147,9 +147,9 @@ rcl_action_goal_handle_is_cancelable(const rcl_action_goal_handle_t * goal_handl if (!rcl_action_goal_handle_is_valid(goal_handle)) { return false; // error message is set } - // Check if the state machine reports a cancel event is valid + // Check if the state machine reports a cancel goal event is valid rcl_action_goal_state_t state = rcl_action_transition_goal_state( - goal_handle->impl->state, GOAL_EVENT_CANCEL); + goal_handle->impl->state, GOAL_EVENT_CANCEL_GOAL); return GOAL_STATE_CANCELING == state; } diff --git a/rcl_action/src/rcl_action/goal_state_machine.c b/rcl_action/src/rcl_action/goal_state_machine.c index cfb38fd71..9bbc3c6d1 100644 --- a/rcl_action/src/rcl_action/goal_state_machine.c +++ b/rcl_action/src/rcl_action/goal_state_machine.c @@ -33,10 +33,10 @@ _execute_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t ev } rcl_action_goal_state_t -_cancel_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event) +_cancel_goal_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event) { if ((GOAL_STATE_ACCEPTED != state && GOAL_STATE_EXECUTING != state) || - GOAL_EVENT_CANCEL != event) + GOAL_EVENT_CANCEL_GOAL != event) { return GOAL_STATE_UNKNOWN; } @@ -44,10 +44,10 @@ _cancel_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t eve } rcl_action_goal_state_t -_set_succeeded_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event) +_succeed_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event) { if ((GOAL_STATE_EXECUTING != state && GOAL_STATE_CANCELING != state) || - GOAL_EVENT_SET_SUCCEEDED != event) + GOAL_EVENT_SUCCEED != event) { return GOAL_STATE_UNKNOWN; } @@ -55,10 +55,10 @@ _set_succeeded_event_handler(rcl_action_goal_state_t state, rcl_action_goal_even } rcl_action_goal_state_t -_set_aborted_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event) +_abort_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event) { if ((GOAL_STATE_EXECUTING != state && GOAL_STATE_CANCELING != state) || - GOAL_EVENT_SET_ABORTED != event) + GOAL_EVENT_ABORT != event) { return GOAL_STATE_UNKNOWN; } @@ -66,9 +66,9 @@ _set_aborted_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_ } rcl_action_goal_state_t -_set_canceled_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event) +_canceled_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event) { - if (GOAL_STATE_CANCELING != state || GOAL_EVENT_SET_CANCELED != event) { + if (GOAL_STATE_CANCELING != state || GOAL_EVENT_CANCELED != event) { return GOAL_STATE_UNKNOWN; } return GOAL_STATE_CANCELED; @@ -79,17 +79,17 @@ static rcl_action_goal_event_handler _goal_state_transition_map[GOAL_STATE_NUM_STATES][GOAL_EVENT_NUM_EVENTS] = { [GOAL_STATE_ACCEPTED] = { [GOAL_EVENT_EXECUTE] = _execute_event_handler, - [GOAL_EVENT_CANCEL] = _cancel_event_handler, + [GOAL_EVENT_CANCEL_GOAL] = _cancel_goal_event_handler, }, [GOAL_STATE_EXECUTING] = { - [GOAL_EVENT_CANCEL] = _cancel_event_handler, - [GOAL_EVENT_SET_SUCCEEDED] = _set_succeeded_event_handler, - [GOAL_EVENT_SET_ABORTED] = _set_aborted_event_handler, + [GOAL_EVENT_CANCEL_GOAL] = _cancel_goal_event_handler, + [GOAL_EVENT_SUCCEED] = _succeed_event_handler, + [GOAL_EVENT_ABORT] = _abort_event_handler, }, [GOAL_STATE_CANCELING] = { - [GOAL_EVENT_SET_SUCCEEDED] = _set_succeeded_event_handler, - [GOAL_EVENT_SET_ABORTED] = _set_aborted_event_handler, - [GOAL_EVENT_SET_CANCELED] = _set_canceled_event_handler, + [GOAL_EVENT_SUCCEED] = _succeed_event_handler, + [GOAL_EVENT_ABORT] = _abort_event_handler, + [GOAL_EVENT_CANCELED] = _canceled_event_handler, }, }; diff --git a/rcl_action/test/rcl_action/test_action_server.cpp b/rcl_action/test/rcl_action/test_action_server.cpp index f92c6feeb..0bd6c5487 100644 --- a/rcl_action/test/rcl_action/test_action_server.cpp +++ b/rcl_action/test/rcl_action/test_action_server.cpp @@ -329,7 +329,7 @@ TEST_F(TestActionServer, test_action_clear_expired_goals) handles.push_back(*goal_handle); // Transition executing to aborted ASSERT_EQ(RCL_RET_OK, rcl_action_update_goal_state(goal_handle, GOAL_EVENT_EXECUTE)); - ASSERT_EQ(RCL_RET_OK, rcl_action_update_goal_state(goal_handle, GOAL_EVENT_SET_ABORTED)); + ASSERT_EQ(RCL_RET_OK, rcl_action_update_goal_state(goal_handle, GOAL_EVENT_ABORT)); // Set time to something far in the future ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&this->clock, RCUTILS_S_TO_NS(99999))); // Clear with valid arguments diff --git a/rcl_action/test/rcl_action/test_goal_handle.cpp b/rcl_action/test/rcl_action/test_goal_handle.cpp index d0d265502..d28ee4fc6 100644 --- a/rcl_action/test/rcl_action/test_goal_handle.cpp +++ b/rcl_action/test/rcl_action/test_goal_handle.cpp @@ -161,7 +161,7 @@ using EventStateActiveCancelableTuple = std::tuple; using StateTransitionSequence = std::vector; const std::vector event_strs = { - "EXECUTE", "CANCEL", "SET_SUCCEEDED", "SET_ABORTED", "SET_CANCELED"}; + "EXECUTE", "CANCEL_GOAL", "SUCCEED", "ABORT", "CANCELED"}; class TestGoalHandleStateTransitionSequence : public ::testing::TestWithParam @@ -242,39 +242,39 @@ TEST_P(TestGoalHandleStateTransitionSequence, test_goal_handle_state_transitions const StateTransitionSequence valid_state_transition_sequences[] = { { std::make_tuple(GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING, true, true), - std::make_tuple(GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING, true, false), - std::make_tuple(GOAL_EVENT_SET_CANCELED, GOAL_STATE_CANCELED, 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), }, { std::make_tuple(GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING, true, true), - std::make_tuple(GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING, true, false), - std::make_tuple(GOAL_EVENT_SET_SUCCEEDED, GOAL_STATE_SUCCEEDED, false, false), + std::make_tuple(GOAL_EVENT_CANCEL_GOAL, GOAL_STATE_CANCELING, true, false), + std::make_tuple(GOAL_EVENT_SUCCEED, GOAL_STATE_SUCCEEDED, false, false), }, { std::make_tuple(GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING, true, true), - std::make_tuple(GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING, true, false), - std::make_tuple(GOAL_EVENT_SET_ABORTED, GOAL_STATE_ABORTED, false, false), + 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_SET_SUCCEEDED, GOAL_STATE_SUCCEEDED, false, false), + std::make_tuple(GOAL_EVENT_SUCCEED, GOAL_STATE_SUCCEEDED, false, false), }, { std::make_tuple(GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING, true, true), - std::make_tuple(GOAL_EVENT_SET_ABORTED, GOAL_STATE_ABORTED, false, false), + std::make_tuple(GOAL_EVENT_ABORT, GOAL_STATE_ABORTED, false, false), }, { - std::make_tuple(GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING, true, false), - std::make_tuple(GOAL_EVENT_SET_CANCELED, GOAL_STATE_CANCELED, 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), }, { - std::make_tuple(GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING, true, false), - std::make_tuple(GOAL_EVENT_SET_ABORTED, GOAL_STATE_ABORTED, false, false), + std::make_tuple(GOAL_EVENT_CANCEL_GOAL, GOAL_STATE_CANCELING, true, false), + std::make_tuple(GOAL_EVENT_ABORT, GOAL_STATE_ABORTED, false, false), }, // This is an odd case, but valid nonetheless { - std::make_tuple(GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING, true, false), - std::make_tuple(GOAL_EVENT_SET_SUCCEEDED, GOAL_STATE_SUCCEEDED, false, false), + std::make_tuple(GOAL_EVENT_CANCEL_GOAL, GOAL_STATE_CANCELING, true, false), + std::make_tuple(GOAL_EVENT_SUCCEED, GOAL_STATE_SUCCEEDED, false, false), }, }; @@ -287,26 +287,26 @@ INSTANTIATE_TEST_CASE_P( const StateTransitionSequence invalid_state_transition_sequences[] = { { std::make_tuple(GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING, true, true), - std::make_tuple(GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING, true, false), + std::make_tuple(GOAL_EVENT_CANCEL_GOAL, GOAL_STATE_CANCELING, true, false), std::make_tuple(GOAL_EVENT_EXECUTE, GOAL_STATE_UNKNOWN, false, false), }, { std::make_tuple(GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING, true, true), - std::make_tuple(GOAL_EVENT_CANCEL, GOAL_STATE_CANCELING, true, false), - std::make_tuple(GOAL_EVENT_CANCEL, GOAL_STATE_UNKNOWN, false, false), + std::make_tuple(GOAL_EVENT_CANCEL_GOAL, GOAL_STATE_CANCELING, true, false), + std::make_tuple(GOAL_EVENT_CANCEL_GOAL, GOAL_STATE_UNKNOWN, false, false), }, { std::make_tuple(GOAL_EVENT_EXECUTE, GOAL_STATE_EXECUTING, true, true), std::make_tuple(GOAL_EVENT_EXECUTE, GOAL_STATE_UNKNOWN, false, false), }, { - std::make_tuple(GOAL_EVENT_SET_CANCELED, GOAL_STATE_UNKNOWN, false, false), + std::make_tuple(GOAL_EVENT_CANCELED, GOAL_STATE_UNKNOWN, false, false), }, { - std::make_tuple(GOAL_EVENT_SET_SUCCEEDED, GOAL_STATE_UNKNOWN, false, false), + std::make_tuple(GOAL_EVENT_SUCCEED, GOAL_STATE_UNKNOWN, false, false), }, { - std::make_tuple(GOAL_EVENT_SET_ABORTED, GOAL_STATE_UNKNOWN, false, false), + std::make_tuple(GOAL_EVENT_ABORT, GOAL_STATE_UNKNOWN, false, false), }, }; 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 32d710204..ecfc20f54 100644 --- a/rcl_action/test/rcl_action/test_goal_state_machine.cpp +++ b/rcl_action/test/rcl_action/test_goal_state_machine.cpp @@ -24,27 +24,27 @@ TEST(TestGoalStateMachine, test_valid_transitions) EXPECT_EQ(GOAL_STATE_EXECUTING, state); state = rcl_action_transition_goal_state( GOAL_STATE_ACCEPTED, - GOAL_EVENT_CANCEL); + GOAL_EVENT_CANCEL_GOAL); EXPECT_EQ(GOAL_STATE_CANCELING, state); state = rcl_action_transition_goal_state( GOAL_STATE_EXECUTING, - GOAL_EVENT_CANCEL); + GOAL_EVENT_CANCEL_GOAL); EXPECT_EQ(GOAL_STATE_CANCELING, state); state = rcl_action_transition_goal_state( GOAL_STATE_EXECUTING, - GOAL_EVENT_SET_SUCCEEDED); + GOAL_EVENT_SUCCEED); EXPECT_EQ(GOAL_STATE_SUCCEEDED, state); state = rcl_action_transition_goal_state( GOAL_STATE_EXECUTING, - GOAL_EVENT_SET_ABORTED); + GOAL_EVENT_ABORT); EXPECT_EQ(GOAL_STATE_ABORTED, state); state = rcl_action_transition_goal_state( GOAL_STATE_CANCELING, - GOAL_EVENT_SET_CANCELED); + GOAL_EVENT_CANCELED); EXPECT_EQ(GOAL_STATE_CANCELED, state); state = rcl_action_transition_goal_state( GOAL_STATE_CANCELING, - GOAL_EVENT_SET_ABORTED); + GOAL_EVENT_ABORT); EXPECT_EQ(GOAL_STATE_ABORTED, state); } @@ -53,15 +53,15 @@ TEST(TestGoalStateMachine, test_invalid_transitions) // Invalid from ACCEPTED rcl_action_goal_state_t state = rcl_action_transition_goal_state( GOAL_STATE_ACCEPTED, - GOAL_EVENT_SET_SUCCEEDED); + GOAL_EVENT_SUCCEED); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_ACCEPTED, - GOAL_EVENT_SET_ABORTED); + GOAL_EVENT_ABORT); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_ACCEPTED, - GOAL_EVENT_SET_CANCELED); + GOAL_EVENT_CANCELED); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); // Invalid from EXECUTING @@ -71,7 +71,7 @@ TEST(TestGoalStateMachine, test_invalid_transitions) EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_EXECUTING, - GOAL_EVENT_SET_CANCELED); + GOAL_EVENT_CANCELED); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); // Invalid from CANCELING @@ -81,7 +81,7 @@ TEST(TestGoalStateMachine, test_invalid_transitions) EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_CANCELING, - GOAL_EVENT_CANCEL); + GOAL_EVENT_CANCEL_GOAL); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); // Invalid from SUCCEEDED @@ -91,19 +91,19 @@ TEST(TestGoalStateMachine, test_invalid_transitions) EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_SUCCEEDED, - GOAL_EVENT_CANCEL); + GOAL_EVENT_CANCEL_GOAL); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_SUCCEEDED, - GOAL_EVENT_SET_SUCCEEDED); + GOAL_EVENT_SUCCEED); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_SUCCEEDED, - GOAL_EVENT_SET_ABORTED); + GOAL_EVENT_ABORT); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_SUCCEEDED, - GOAL_EVENT_SET_CANCELED); + GOAL_EVENT_CANCELED); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); // Invalid from ABORTED @@ -113,19 +113,19 @@ TEST(TestGoalStateMachine, test_invalid_transitions) EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_ABORTED, - GOAL_EVENT_CANCEL); + GOAL_EVENT_CANCEL_GOAL); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_ABORTED, - GOAL_EVENT_SET_SUCCEEDED); + GOAL_EVENT_SUCCEED); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_ABORTED, - GOAL_EVENT_SET_ABORTED); + GOAL_EVENT_ABORT); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_ABORTED, - GOAL_EVENT_SET_CANCELED); + GOAL_EVENT_CANCELED); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); // Invalid from CANCELED @@ -135,18 +135,18 @@ TEST(TestGoalStateMachine, test_invalid_transitions) EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_CANCELED, - GOAL_EVENT_CANCEL); + GOAL_EVENT_CANCEL_GOAL); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_CANCELED, - GOAL_EVENT_SET_SUCCEEDED); + GOAL_EVENT_SUCCEED); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_CANCELED, - GOAL_EVENT_SET_ABORTED); + GOAL_EVENT_ABORT); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); state = rcl_action_transition_goal_state( GOAL_STATE_CANCELED, - GOAL_EVENT_SET_CANCELED); + GOAL_EVENT_CANCELED); EXPECT_EQ(GOAL_STATE_UNKNOWN, state); }