// Copyright 2016 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifdef __cplusplus extern "C" { #endif #include #include #include #include "rcl/error_handling.h" #include "rcutils/format_string.h" #include "rcl_lifecycle/transition_map.h" rcl_lifecycle_transition_map_t rcl_lifecycle_get_zero_initialized_transition_map() { static rcl_lifecycle_transition_map_t transition_map; transition_map.states = NULL; transition_map.states_size = 0; transition_map.transitions = NULL; transition_map.transitions_size = 0; return transition_map; } rcl_ret_t rcl_lifecycle_transition_map_is_initialized(const rcl_lifecycle_transition_map_t * transition_map) { rcl_ret_t is_initialized = RCL_RET_OK; if (!transition_map->states && !transition_map->transitions) { is_initialized = RCL_RET_ERROR; } return is_initialized; } rcl_ret_t rcl_lifecycle_transition_map_fini( rcl_lifecycle_transition_map_t * transition_map, const rcutils_allocator_t * allocator) { rcl_ret_t fcn_ret = RCL_RET_OK; // for each state free the allocations for their keys/transitions for (unsigned int i = 0; i < transition_map->states_size; ++i) { allocator->deallocate(transition_map->states[i].valid_transition_keys, allocator->state); allocator->deallocate(transition_map->states[i].valid_transitions, allocator->state); } // free the primary states allocator->deallocate(transition_map->states, allocator->state); transition_map->states = NULL; // free the tansitions allocator->deallocate(transition_map->transitions, allocator->state); transition_map->transitions = NULL; return fcn_ret; } rcl_ret_t rcl_lifecycle_register_state( rcl_lifecycle_transition_map_t * transition_map, rcl_lifecycle_state_t state, const rcutils_allocator_t * allocator) { if (rcl_lifecycle_get_state(transition_map, state.id) != NULL) { RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(rcutils_get_default_allocator(), "state %u is already registered\n", state.id); return RCL_RET_ERROR; } RCUTILS_CHECK_ALLOCATOR_WITH_MSG( allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT) // add new primary state memory transition_map->states_size += 1; rcl_lifecycle_state_t * new_states = allocator->reallocate( transition_map->states, transition_map->states_size * sizeof(rcl_lifecycle_state_t), allocator->state); if (!new_states) { RCL_SET_ERROR_MSG( "failed to reallocate memory for new states", rcl_get_default_allocator()); return RCL_RET_ERROR; } transition_map->states = new_states; transition_map->states[transition_map->states_size - 1] = state; return RCL_RET_OK; } rcl_ret_t rcl_lifecycle_register_transition( rcl_lifecycle_transition_map_t * transition_map, rcl_lifecycle_transition_t transition, rcl_lifecycle_transition_key_t key, const rcutils_allocator_t * allocator) { RCUTILS_CHECK_ALLOCATOR_WITH_MSG( allocator, "invalid allocator", return RCL_RET_ERROR) rcl_lifecycle_state_t * state = rcl_lifecycle_get_state(transition_map, transition.start->id); if (!state) { RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(rcl_get_default_allocator(), "state %u is not registered\n", transition.start->id); return RCL_RET_ERROR; } // we add a new transition, so increase the size transition_map->transitions_size += 1; rcl_lifecycle_transition_t * new_transitions = allocator->reallocate( transition_map->transitions, transition_map->transitions_size * sizeof(rcl_lifecycle_transition_t), allocator->state); if (!new_transitions) { RCL_SET_ERROR_MSG( "failed to reallocate memory for new transitions", rcl_get_default_allocator()); return RCL_RET_BAD_ALLOC; } transition_map->transitions = new_transitions; // finally set the new transition to the end of the array transition_map->transitions[transition_map->transitions_size - 1] = transition; // connect transition to state key // we have to copy the transitons here once more to the actual state // as we can't assign only the pointer. This pointer gets invalidated whenever // we add a new transition and re-shuffle/re-allocate new memory for it. state->valid_transition_size += 1; rcl_lifecycle_transition_t * new_valid_transitions = allocator->reallocate( state->valid_transitions, state->valid_transition_size * sizeof(rcl_lifecycle_transition_t), allocator->state); if (!new_valid_transitions) { RCL_SET_ERROR_MSG( "failed to reallocate memory for new transitions on state", rcl_get_default_allocator()); return RCL_RET_ERROR; } state->valid_transitions = new_valid_transitions; rcl_lifecycle_transition_key_t * new_valid_transition_keys = allocator->reallocate( state->valid_transition_keys, state->valid_transition_size * sizeof(rcl_lifecycle_transition_key_t), allocator->state); if (!new_valid_transitions) { RCL_SET_ERROR_MSG( "failed to reallocate memory for new transitions keys on state", rcl_get_default_allocator()); return RCL_RET_ERROR; } state->valid_transition_keys = new_valid_transition_keys; // assign key state->valid_transition_keys[state->valid_transition_size - 1] = key; state->valid_transitions[state->valid_transition_size - 1] = transition; return RCL_RET_OK; } rcl_lifecycle_state_t * rcl_lifecycle_get_state( rcl_lifecycle_transition_map_t * transition_map, unsigned int state_id) { for (unsigned int i = 0; i < transition_map->states_size; ++i) { if (transition_map->states[i].id == state_id) { return &transition_map->states[i]; } } return NULL; } rcl_lifecycle_transition_t * rcl_lifecycle_get_transitions( rcl_lifecycle_transition_map_t * transition_map, unsigned int transition_id) { for (unsigned int i = 0; i < transition_map->transitions_size; ++i) { if (transition_map->transitions[i].id == transition_id) { return &transition_map->transitions[i]; } } return NULL; } #ifdef __cplusplus } #endif