niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Refactor goal state machine implementation and add unit tests (#311)

Browse source 
* Fix buggy if-conditions in transition functions.
* Bugfix: incease number of states by one
* Cleanup CMakeLists.txt and package.xml
* Move goal state machine implementation details from header to C file
This commit is contained in:
Jacob Perron 2018-10-30 16:15:10 -07:00 • committed by GitHub
parent 2c0e35d9d1
commit 29e7dbe156
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 317 additions and 162 deletions
Download patch file Download diff file Expand all files Collapse all files

91
rcl_action/include/rcl_action/goal_state_machine.h
View file

@ -24,83 +24,6 @@ extern "C"
#include "rcl_action/visibility_control.h"
typedef rcl_action_goal_state_t
(* rcl_action_goal_event_handler)(rcl_action_goal_state_t, rcl_action_goal_event_t);
// Transition event handlers
static inline rcl_action_goal_state_t
_execute_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
{
if (GOAL_STATE_ACCEPTED != state || GOAL_EVENT_EXECUTE != event) {
return GOAL_STATE_UNKNOWN;
}
return GOAL_STATE_EXECUTING;
}
static inline rcl_action_goal_state_t
_cancel_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)
{
return GOAL_STATE_UNKNOWN;
}
return GOAL_STATE_CANCELING;
}
static inline rcl_action_goal_state_t
_set_succeeded_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)
{
return GOAL_STATE_UNKNOWN;
}
return GOAL_STATE_SUCCEEDED;
}
static inline rcl_action_goal_state_t
_set_aborted_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)
{
return GOAL_STATE_UNKNOWN;
}
return GOAL_STATE_ABORTED;
}
static inline rcl_action_goal_state_t
_set_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) {
return GOAL_STATE_UNKNOWN;
}
return GOAL_STATE_CANCELED;
}
// Transition map
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_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_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,
},
};
/// Transition a goal from one state to the next.
/**
* Given a goal state and a goal event, return the next state.
@ -108,22 +31,14 @@ rcl_action_goal_event_handler
* \param[in] state the state to transition from
* \param[in] event the event triggering a transition
* \return the next goal state if the transition is valid, or
* \return `GOAl_STATE_UNKNOWN` if the transition is invalid or an error occured
* \return `GOAL_STATE_UNKNOWN` if the transition is invalid or an error occured
*/
RCL_ACTION_PUBLIC
RCL_WARN_UNUSED
inline rcl_action_goal_state_t
rcl_action_goal_state_t
rcl_action_transition_goal_state(
const rcl_action_goal_state_t state,
const rcl_action_goal_event_t event)
{
// rcl_action_goal_event_handler ** transition_map = get_state_transition_map();
rcl_action_goal_event_handler handler = _goal_state_transition_map[state][event];
if (NULL == handler) {
return GOAL_STATE_UNKNOWN;
}
return handler(state, event);
}
const rcl_action_goal_event_t event);
#ifdef __cplusplus
}

2
rcl_action/include/rcl_action/types.h
View file

@ -72,7 +72,7 @@ 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 6 // not counting `UNKNOWN`
#define GOAL_STATE_NUM_STATES 7
/// Goal state transition events
typedef enum rcl_action_goal_event_t