add rcl lifecycle
* (refactor) add rcl_lifecycle package * (refactor) cleanup free calls * (fix) initialize state machine with external node handle * (dev) use external typesupport * (fix) use external typesupport * (fix) cleanup states * (fix) no pointer comparison in state machine * (test) refactor for rcl testing purpose * (test) test suite for lifecycle sequence * (dev) change to rcl_ret_t * (test) wrong transition test * (fix) dependency for isolated build * (clean) remove std_msgs as a dep * (fix) enable correct visibility control * (fix) correct test for initialization * (fix) correct visibility attributes * (dev) change default value to lifecycle_msgs * (clean) style and lifecycle prefix * (dev) cmake macro get_rcl_lifecycle_information * (cleanup) remove unused files' * (cleanup) remove callback pointer * (debug) add print state machine function * (review) address review comments * (bugfix) correct export in information.cmake * (fix) correct visibility control * (vcs) fix convertion from size_t to unsigned int * (typo) fix internal struct name * const correctness * get_available_states service * new service msgs * tes for multiple instances * (dev) return codes * initial refactor * test default sequence * refactor state machine * apply upstream changes * c++14 * disable state machine print * address review comments * uncrustify * fix comparison with unsigned warning
This commit is contained in:
Karsten Knese
GitHub
parent
4590fc5f77
commit
a18ef97e5a
17 changed files with 2357 additions and 0 deletions
305
rcl_lifecycle/src/com_interface.c
Normal file
305
rcl_lifecycle/src/com_interface.c
Normal file
|
|
@ -0,0 +1,305 @@
|
|||
// 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.
|
||||
|
||||
#if __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#include "com_interface.h" // NOLINT
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "lifecycle_msgs/msg/transition_event.h"
|
||||
|
||||
#include "rosidl_generator_c/message_type_support.h"
|
||||
#include "rosidl_generator_c/string_functions.h"
|
||||
|
||||
#include "rcl/error_handling.h"
|
||||
|
||||
#include "rcl_lifecycle/data_types.h"
|
||||
|
||||
static lifecycle_msgs__msg__TransitionEvent msg;
|
||||
|
||||
bool concatenate(const char ** prefix, const char ** suffix, char ** result)
|
||||
{
|
||||
size_t prefix_size = strlen(*prefix);
|
||||
size_t suffix_size = strlen(*suffix);
|
||||
if ((prefix_size + suffix_size) >= 255) {
|
||||
return false;
|
||||
}
|
||||
*result = malloc((prefix_size + suffix_size) * sizeof(char));
|
||||
memcpy(*result, *prefix, prefix_size);
|
||||
memcpy(*result + prefix_size, *suffix, suffix_size + 1);
|
||||
return true;
|
||||
}
|
||||
|
||||
rcl_lifecycle_com_interface_t
|
||||
rcl_lifecycle_get_zero_initialized_com_interface()
|
||||
{
|
||||
rcl_lifecycle_com_interface_t com_interface;
|
||||
com_interface.pub_transition_event = rcl_get_zero_initialized_publisher();
|
||||
com_interface.srv_change_state = rcl_get_zero_initialized_service();
|
||||
com_interface.srv_get_state = rcl_get_zero_initialized_service();
|
||||
com_interface.srv_get_available_states = rcl_get_zero_initialized_service();
|
||||
com_interface.srv_get_available_transitions = rcl_get_zero_initialized_service();
|
||||
return com_interface;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_lifecycle_com_interface_init(rcl_lifecycle_com_interface_t * com_interface,
|
||||
rcl_node_t * node_handle,
|
||||
const rosidl_message_type_support_t * ts_pub_notify,
|
||||
const rosidl_service_type_support_t * ts_srv_change_state,
|
||||
const rosidl_service_type_support_t * ts_srv_get_state,
|
||||
const rosidl_service_type_support_t * ts_srv_get_available_states,
|
||||
const rosidl_service_type_support_t * ts_srv_get_available_transitions)
|
||||
{
|
||||
if (!ts_pub_notify) {
|
||||
RCL_SET_ERROR_MSG("ts_pub_notify is NULL");
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
if (!ts_srv_change_state) {
|
||||
RCL_SET_ERROR_MSG("ts_srv_change_state is NULL");
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
if (!ts_srv_get_state) {
|
||||
RCL_SET_ERROR_MSG("ts_srv_get_state is NULL");
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
if (!ts_srv_get_available_states) {
|
||||
RCL_SET_ERROR_MSG("ts_srv_get_available_states is NULL");
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
if (!ts_srv_get_available_states) {
|
||||
RCL_SET_ERROR_MSG("ts_srv_get_available_transitions is NULL");
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
|
||||
const char * node_name = rcl_node_get_name(node_handle);
|
||||
|
||||
// initialize publisher
|
||||
{
|
||||
// Build topic, topic suffix hardcoded for now
|
||||
// and limited in length of 255
|
||||
const char * topic_prefix = "__transition_event";
|
||||
char * topic_name;
|
||||
if (!concatenate(&node_name, &topic_prefix, &topic_name)) {
|
||||
RCL_SET_ERROR_MSG("Topic name exceeds maximum size of 255");
|
||||
goto fail;
|
||||
}
|
||||
|
||||
rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options();
|
||||
rcl_ret_t ret = rcl_publisher_init(
|
||||
&com_interface->pub_transition_event, node_handle,
|
||||
ts_pub_notify, topic_name, &publisher_options);
|
||||
free(topic_name);
|
||||
|
||||
if (ret != RCL_RET_OK) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// initialize static message for notification
|
||||
lifecycle_msgs__msg__TransitionEvent__init(&msg);
|
||||
}
|
||||
|
||||
// initialize change state service
|
||||
{
|
||||
// Build topic, topic suffix hardcoded for now
|
||||
// and limited in length of 255
|
||||
const char * topic_prefix = "__change_state";
|
||||
char * topic_name;
|
||||
if (!concatenate(&node_name, &topic_prefix, &topic_name)) {
|
||||
RCL_SET_ERROR_MSG("Topic name exceeds maximum size of 255");
|
||||
goto fail;
|
||||
}
|
||||
|
||||
rcl_service_options_t service_options = rcl_service_get_default_options();
|
||||
rcl_ret_t ret = rcl_service_init(
|
||||
&com_interface->srv_change_state, node_handle,
|
||||
ts_srv_change_state, topic_name, &service_options);
|
||||
free(topic_name);
|
||||
|
||||
if (ret != RCL_RET_OK) {
|
||||
goto fail;
|
||||
}
|
||||
}
|
||||
|
||||
// initialize get state service
|
||||
{
|
||||
// Build topic, topic suffix hardcoded for now
|
||||
// and limited in length of 255
|
||||
const char * topic_prefix = "__get_state";
|
||||
char * topic_name;
|
||||
if (!concatenate(&node_name, &topic_prefix, &topic_name)) {
|
||||
RCL_SET_ERROR_MSG("Topic name exceeds maximum size of 255");
|
||||
goto fail;
|
||||
}
|
||||
|
||||
rcl_service_options_t service_options = rcl_service_get_default_options();
|
||||
rcl_ret_t ret = rcl_service_init(
|
||||
&com_interface->srv_get_state, node_handle,
|
||||
ts_srv_get_state, topic_name, &service_options);
|
||||
free(topic_name);
|
||||
|
||||
if (ret != RCL_RET_OK) {
|
||||
goto fail;
|
||||
}
|
||||
}
|
||||
|
||||
// initialize get available state service
|
||||
{
|
||||
// Build topic, topic suffix hardcoded for now
|
||||
// and limited in length of 255
|
||||
const char * topic_prefix = "__get_available_states";
|
||||
char * topic_name;
|
||||
if (!concatenate(&node_name, &topic_prefix, &topic_name)) {
|
||||
RCL_SET_ERROR_MSG("Topic name exceeds maximum size of 255");
|
||||
goto fail;
|
||||
}
|
||||
|
||||
rcl_service_options_t service_options = rcl_service_get_default_options();
|
||||
rcl_ret_t ret = rcl_service_init(
|
||||
&com_interface->srv_get_available_states, node_handle,
|
||||
ts_srv_get_available_states, topic_name, &service_options);
|
||||
free(topic_name);
|
||||
|
||||
if (ret != RCL_RET_OK) {
|
||||
goto fail;
|
||||
}
|
||||
}
|
||||
|
||||
// initialize get available state service
|
||||
{
|
||||
// Build topic, topic suffix hardcoded for now
|
||||
// and limited in length of 255
|
||||
const char * topic_prefix = "__get_available_transitions";
|
||||
char * topic_name;
|
||||
if (!concatenate(&node_name, &topic_prefix, &topic_name)) {
|
||||
RCL_SET_ERROR_MSG("Topic name exceeds maximum size of 255");
|
||||
goto fail;
|
||||
}
|
||||
|
||||
rcl_service_options_t service_options = rcl_service_get_default_options();
|
||||
rcl_ret_t ret = rcl_service_init(
|
||||
&com_interface->srv_get_available_transitions, node_handle,
|
||||
ts_srv_get_available_transitions, topic_name, &service_options);
|
||||
free(topic_name);
|
||||
|
||||
if (ret != RCL_RET_OK) {
|
||||
goto fail;
|
||||
}
|
||||
}
|
||||
|
||||
return RCL_RET_OK;
|
||||
|
||||
fail:
|
||||
if (RCL_RET_OK != rcl_publisher_fini(&com_interface->pub_transition_event, node_handle)) {
|
||||
fprintf(stderr, "%s:%u, Failed to destroy transition_event publisher\n",
|
||||
__FILE__, __LINE__);
|
||||
}
|
||||
if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_change_state, node_handle)) {
|
||||
fprintf(stderr, "%s:%u, Failed to destroy change_state service\n",
|
||||
__FILE__, __LINE__);
|
||||
}
|
||||
if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_state, node_handle)) {
|
||||
fprintf(stderr, "%s:%u, Failed to destroy get_state service\n",
|
||||
__FILE__, __LINE__);
|
||||
}
|
||||
if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_available_states, node_handle)) {
|
||||
fprintf(stderr, "%s:%u, Failed to destroy get_available_states service\n",
|
||||
__FILE__, __LINE__);
|
||||
}
|
||||
if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_available_transitions, node_handle)) {
|
||||
fprintf(stderr, "%s:%u, Failed to destroy get_available_transitions service\n",
|
||||
__FILE__, __LINE__);
|
||||
}
|
||||
com_interface = NULL;
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_lifecycle_com_interface_fini(
|
||||
rcl_lifecycle_com_interface_t * com_interface,
|
||||
rcl_node_t * node_handle)
|
||||
{
|
||||
rcl_ret_t fcn_ret = RCL_RET_OK;
|
||||
|
||||
// destroy get available transitions srv
|
||||
{
|
||||
rcl_ret_t ret = rcl_service_fini(
|
||||
&com_interface->srv_get_available_transitions, node_handle);
|
||||
if (ret != RCL_RET_OK) {
|
||||
fcn_ret = RCL_RET_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
// destroy get available states srv
|
||||
{
|
||||
rcl_ret_t ret = rcl_service_fini(
|
||||
&com_interface->srv_get_available_states, node_handle);
|
||||
if (ret != RCL_RET_OK) {
|
||||
fcn_ret = RCL_RET_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
// destroy get state srv
|
||||
{
|
||||
rcl_ret_t ret = rcl_service_fini(
|
||||
&com_interface->srv_get_state, node_handle);
|
||||
if (ret != RCL_RET_OK) {
|
||||
fcn_ret = RCL_RET_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
// destroy change state srv
|
||||
{
|
||||
rcl_ret_t ret = rcl_service_fini(
|
||||
&com_interface->srv_change_state, node_handle);
|
||||
if (ret != RCL_RET_OK) {
|
||||
fcn_ret = RCL_RET_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
// destroy the publisher
|
||||
{
|
||||
lifecycle_msgs__msg__TransitionEvent__fini(&msg);
|
||||
|
||||
rcl_ret_t ret = rcl_publisher_fini(
|
||||
&com_interface->pub_transition_event, node_handle);
|
||||
if (ret != RCL_RET_OK) {
|
||||
fcn_ret = RCL_RET_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
return fcn_ret;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_lifecycle_com_interface_publish_notification(
|
||||
rcl_lifecycle_com_interface_t * com_interface,
|
||||
const rcl_lifecycle_state_t * start, const rcl_lifecycle_state_t * goal)
|
||||
{
|
||||
msg.start_state.id = start->id;
|
||||
rosidl_generator_c__String__assign(&msg.start_state.label, start->label);
|
||||
msg.goal_state.id = goal->id;
|
||||
rosidl_generator_c__String__assign(&msg.goal_state.label, goal->label);
|
||||
|
||||
return rcl_publish(&com_interface->pub_transition_event, &msg);
|
||||
}
|
||||
|
||||
#if __cplusplus
|
||||
}
|
||||
#endif
|
||||
52
rcl_lifecycle/src/com_interface.h
Normal file
52
rcl_lifecycle/src/com_interface.h
Normal file
|
|
@ -0,0 +1,52 @@
|
|||
// 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.
|
||||
|
||||
#ifndef COM_INTERFACE_H_
|
||||
#define COM_INTERFACE_H_
|
||||
|
||||
#if __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#include "rcl_lifecycle/data_types.h"
|
||||
|
||||
rcl_lifecycle_com_interface_t
|
||||
rcl_lifecycle_get_zero_initialized_com_interface();
|
||||
|
||||
rcl_ret_t
|
||||
rcl_lifecycle_com_interface_init(
|
||||
rcl_lifecycle_com_interface_t * com_interface,
|
||||
rcl_node_t * node_handle,
|
||||
const rosidl_message_type_support_t * ts_pub_notify,
|
||||
const rosidl_service_type_support_t * ts_srv_change_state,
|
||||
const rosidl_service_type_support_t * ts_srv_get_state,
|
||||
const rosidl_service_type_support_t * ts_srv_get_available_states,
|
||||
const rosidl_service_type_support_t * ts_srv_get_available_transitions);
|
||||
|
||||
rcl_ret_t
|
||||
rcl_lifecycle_com_interface_fini(
|
||||
rcl_lifecycle_com_interface_t * com_interface,
|
||||
rcl_node_t * node_handle);
|
||||
|
||||
rcl_ret_t
|
||||
rcl_lifecycle_com_interface_publish_notification(
|
||||
rcl_lifecycle_com_interface_t * com_interface,
|
||||
const rcl_lifecycle_state_t * start, const rcl_lifecycle_state_t * goal);
|
||||
|
||||
#if __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // COM_INTERFACE_H_
|
||||
294
rcl_lifecycle/src/default_state_machine.c
Normal file
294
rcl_lifecycle/src/default_state_machine.c
Normal file
|
|
@ -0,0 +1,294 @@
|
|||
// 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.
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <lifecycle_msgs/msg/state.h>
|
||||
#include <lifecycle_msgs/msg/transition.h>
|
||||
|
||||
#include "rcl_lifecycle/transition_map.h"
|
||||
|
||||
#include "default_state_machine.h" // NOLINT
|
||||
#include "states.h" // NOLINT
|
||||
|
||||
#if __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
// Primary States
|
||||
const rcl_lifecycle_state_t rcl_state_unknown =
|
||||
{"unknown", lifecycle_msgs__msg__State__PRIMARY_STATE_UNKNOWN,
|
||||
NULL, NULL, 0};
|
||||
const rcl_lifecycle_state_t rcl_state_unconfigured =
|
||||
{"unconfigured", lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED,
|
||||
NULL, NULL, 0};
|
||||
const rcl_lifecycle_state_t rcl_state_inactive =
|
||||
{"inactive", lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE,
|
||||
NULL, NULL, 0};
|
||||
const rcl_lifecycle_state_t rcl_state_active =
|
||||
{"active", lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE,
|
||||
NULL, NULL, 0};
|
||||
const rcl_lifecycle_state_t rcl_state_finalized =
|
||||
{"finalized", lifecycle_msgs__msg__State__PRIMARY_STATE_FINALIZED,
|
||||
NULL, NULL, 0};
|
||||
// Transition States
|
||||
const rcl_lifecycle_state_t rcl_state_configuring =
|
||||
{"configuring", lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING,
|
||||
NULL, NULL, 0};
|
||||
const rcl_lifecycle_state_t rcl_state_cleaningup =
|
||||
{"cleaningup", lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP,
|
||||
NULL, NULL, 0};
|
||||
const rcl_lifecycle_state_t rcl_state_shuttingdown =
|
||||
{"shuttingdown", lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN,
|
||||
NULL, NULL, 0};
|
||||
const rcl_lifecycle_state_t rcl_state_activating =
|
||||
{"activating", lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING,
|
||||
NULL, NULL, 0};
|
||||
const rcl_lifecycle_state_t rcl_state_deactivating =
|
||||
{"deactivating", lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING,
|
||||
NULL, NULL, 0};
|
||||
const rcl_lifecycle_state_t rcl_state_errorprocessing =
|
||||
{"errorprocessing", lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING,
|
||||
NULL, NULL, 0};
|
||||
|
||||
// Transitions
|
||||
const rcl_lifecycle_transition_t rcl_transition_configure =
|
||||
{"configure", lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE,
|
||||
&rcl_state_unconfigured, &rcl_state_configuring};
|
||||
const rcl_lifecycle_transition_t rcl_transition_configure_success =
|
||||
{"configure_success", lifecycle_msgs__msg__Transition__TRANSITION_ON_CONFIGURE_SUCCESS,
|
||||
&rcl_state_configuring, &rcl_state_inactive};
|
||||
const rcl_lifecycle_transition_t rcl_transition_configure_failure =
|
||||
{"configure_failure", lifecycle_msgs__msg__Transition__TRANSITION_ON_CONFIGURE_FAILURE,
|
||||
&rcl_state_configuring, &rcl_state_unconfigured};
|
||||
const rcl_lifecycle_transition_t rcl_transition_configure_error =
|
||||
{"configure_error", lifecycle_msgs__msg__Transition__TRANSITION_ON_CLEANUP_ERROR,
|
||||
&rcl_state_configuring, &rcl_state_errorprocessing};
|
||||
|
||||
const rcl_lifecycle_transition_t rcl_transition_cleanup =
|
||||
{"cleanup", lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP,
|
||||
&rcl_state_inactive, &rcl_state_cleaningup};
|
||||
const rcl_lifecycle_transition_t rcl_transition_cleanup_success =
|
||||
{"cleanup_success", lifecycle_msgs__msg__Transition__TRANSITION_ON_CLEANUP_SUCCESS,
|
||||
&rcl_state_cleaningup, &rcl_state_unconfigured};
|
||||
const rcl_lifecycle_transition_t rcl_transition_cleanup_failure =
|
||||
{"cleanup_failure", lifecycle_msgs__msg__Transition__TRANSITION_ON_CLEANUP_FAILURE,
|
||||
&rcl_state_cleaningup, &rcl_state_inactive};
|
||||
const rcl_lifecycle_transition_t rcl_transition_cleanup_error =
|
||||
{"cleanup_error", lifecycle_msgs__msg__Transition__TRANSITION_ON_CLEANUP_ERROR,
|
||||
&rcl_state_cleaningup, &rcl_state_errorprocessing};
|
||||
|
||||
const rcl_lifecycle_transition_t rcl_transition_activate =
|
||||
{"activate", lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE,
|
||||
&rcl_state_inactive, &rcl_state_activating};
|
||||
const rcl_lifecycle_transition_t rcl_transition_activate_success =
|
||||
{"activate_success", lifecycle_msgs__msg__Transition__TRANSITION_ON_ACTIVATE_SUCCESS,
|
||||
&rcl_state_activating, &rcl_state_active};
|
||||
const rcl_lifecycle_transition_t rcl_transition_activate_failure =
|
||||
{"activate_failure", lifecycle_msgs__msg__Transition__TRANSITION_ON_ACTIVATE_FAILURE,
|
||||
&rcl_state_activating, &rcl_state_inactive};
|
||||
const rcl_lifecycle_transition_t rcl_transition_activate_error =
|
||||
{"activate_error", lifecycle_msgs__msg__Transition__TRANSITION_ON_ACTIVATE_ERROR,
|
||||
&rcl_state_activating, &rcl_state_errorprocessing};
|
||||
|
||||
const rcl_lifecycle_transition_t rcl_transition_deactivate =
|
||||
{"deactivate", lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE,
|
||||
&rcl_state_active, &rcl_state_deactivating};
|
||||
const rcl_lifecycle_transition_t rcl_transition_deactivate_success =
|
||||
{"deactivate_success", lifecycle_msgs__msg__Transition__TRANSITION_ON_DEACTIVATE_SUCCESS,
|
||||
&rcl_state_deactivating, &rcl_state_inactive};
|
||||
const rcl_lifecycle_transition_t rcl_transition_deactivate_failure =
|
||||
{"deactivate_failure", lifecycle_msgs__msg__Transition__TRANSITION_ON_DEACTIVATE_FAILURE,
|
||||
&rcl_state_deactivating, &rcl_state_active};
|
||||
const rcl_lifecycle_transition_t rcl_transition_deactivate_error =
|
||||
{"deactivate_error", lifecycle_msgs__msg__Transition__TRANSITION_ON_DEACTIVATE_ERROR,
|
||||
&rcl_state_deactivating, &rcl_state_errorprocessing};
|
||||
|
||||
const rcl_lifecycle_transition_t rcl_transition_unconfigured_shutdown =
|
||||
{"unconfigured_shutdown", lifecycle_msgs__msg__Transition__TRANSITION_UNCONFIGURED_SHUTDOWN,
|
||||
&rcl_state_unconfigured, &rcl_state_shuttingdown};
|
||||
const rcl_lifecycle_transition_t rcl_transition_inactive_shutdown =
|
||||
{"inactive_shutdown", lifecycle_msgs__msg__Transition__TRANSITION_INACTIVE_SHUTDOWN,
|
||||
&rcl_state_inactive, &rcl_state_shuttingdown};
|
||||
const rcl_lifecycle_transition_t rcl_transition_active_shutdown =
|
||||
{"active_shutdown", lifecycle_msgs__msg__Transition__TRANSITION_ACTIVE_SHUTDOWN,
|
||||
&rcl_state_active, &rcl_state_shuttingdown};
|
||||
const rcl_lifecycle_transition_t rcl_transition_shutdown_success =
|
||||
{"shutdown_success", lifecycle_msgs__msg__Transition__TRANSITION_ON_SHUTDOWN_SUCCESS,
|
||||
&rcl_state_shuttingdown, &rcl_state_finalized};
|
||||
const rcl_lifecycle_transition_t rcl_transition_shutdown_failure =
|
||||
{"shutdown_failure", lifecycle_msgs__msg__Transition__TRANSITION_ON_SHUTDOWN_FAILURE,
|
||||
&rcl_state_shuttingdown, &rcl_state_finalized};
|
||||
const rcl_lifecycle_transition_t rcl_transition_shutdown_error =
|
||||
{"shutdown_error", lifecycle_msgs__msg__Transition__TRANSITION_ON_SHUTDOWN_ERROR,
|
||||
&rcl_state_shuttingdown, &rcl_state_errorprocessing};
|
||||
|
||||
const rcl_lifecycle_transition_t rcl_transition_error_success =
|
||||
{"errorprocessing_success", lifecycle_msgs__msg__Transition__TRANSITION_ON_ERROR_SUCCESS,
|
||||
&rcl_state_errorprocessing, &rcl_state_unconfigured};
|
||||
const rcl_lifecycle_transition_t rcl_transition_error_failure =
|
||||
{"errorprocessing_failure", lifecycle_msgs__msg__Transition__TRANSITION_ON_ERROR_FAILURE,
|
||||
&rcl_state_errorprocessing, &rcl_state_finalized};
|
||||
const rcl_lifecycle_transition_t rcl_transition_error_error =
|
||||
{"errorprocessing_error", lifecycle_msgs__msg__Transition__TRANSITION_ON_ERROR_ERROR,
|
||||
&rcl_state_errorprocessing, &rcl_state_finalized};
|
||||
|
||||
// default implementation as despicted on
|
||||
// design.ros2.org
|
||||
rcl_ret_t
|
||||
rcl_lifecycle_init_default_state_machine(rcl_lifecycle_state_machine_t * state_machine)
|
||||
{
|
||||
// Maybe we can directly store only pointers to states
|
||||
rcl_lifecycle_register_state(
|
||||
&state_machine->transition_map, rcl_state_unknown);
|
||||
rcl_lifecycle_register_state(
|
||||
&state_machine->transition_map, rcl_state_unconfigured);
|
||||
rcl_lifecycle_register_state(
|
||||
&state_machine->transition_map, rcl_state_inactive);
|
||||
rcl_lifecycle_register_state(
|
||||
&state_machine->transition_map, rcl_state_active);
|
||||
rcl_lifecycle_register_state(
|
||||
&state_machine->transition_map, rcl_state_finalized);
|
||||
// transition states
|
||||
rcl_lifecycle_register_state(
|
||||
&state_machine->transition_map, rcl_state_configuring);
|
||||
rcl_lifecycle_register_state(
|
||||
&state_machine->transition_map, rcl_state_cleaningup);
|
||||
rcl_lifecycle_register_state(
|
||||
&state_machine->transition_map, rcl_state_shuttingdown);
|
||||
rcl_lifecycle_register_state(
|
||||
&state_machine->transition_map, rcl_state_activating);
|
||||
rcl_lifecycle_register_state(
|
||||
&state_machine->transition_map, rcl_state_deactivating);
|
||||
rcl_lifecycle_register_state(
|
||||
&state_machine->transition_map, rcl_state_errorprocessing);
|
||||
|
||||
// add transitions to map
|
||||
// TRANSITION_CONFIGURE
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_configure,
|
||||
lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE);
|
||||
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_configure_success,
|
||||
RCL_LIFECYCLE_RET_OK);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_configure_failure,
|
||||
RCL_LIFECYCLE_RET_FAILURE);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_configure_error,
|
||||
RCL_LIFECYCLE_RET_ERROR);
|
||||
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_cleanup,
|
||||
lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_cleanup_success,
|
||||
RCL_LIFECYCLE_RET_OK);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_cleanup_failure,
|
||||
RCL_LIFECYCLE_RET_FAILURE);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_cleanup_error,
|
||||
RCL_LIFECYCLE_RET_ERROR);
|
||||
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_activate,
|
||||
lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_activate_success,
|
||||
RCL_LIFECYCLE_RET_OK);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_activate_failure,
|
||||
RCL_LIFECYCLE_RET_FAILURE);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_activate_error,
|
||||
RCL_LIFECYCLE_RET_ERROR);
|
||||
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_deactivate,
|
||||
lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_deactivate_success,
|
||||
RCL_LIFECYCLE_RET_OK);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_deactivate_failure,
|
||||
RCL_LIFECYCLE_RET_FAILURE);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_deactivate_error,
|
||||
RCL_LIFECYCLE_RET_ERROR);
|
||||
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_unconfigured_shutdown,
|
||||
lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_inactive_shutdown,
|
||||
lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_active_shutdown,
|
||||
lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN);
|
||||
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_shutdown_success,
|
||||
RCL_LIFECYCLE_RET_OK);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_shutdown_failure,
|
||||
RCL_LIFECYCLE_RET_FAILURE);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_shutdown_error,
|
||||
RCL_LIFECYCLE_RET_ERROR);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_error_success,
|
||||
RCL_LIFECYCLE_RET_OK);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_error_failure,
|
||||
RCL_LIFECYCLE_RET_FAILURE);
|
||||
rcl_lifecycle_register_transition(
|
||||
&state_machine->transition_map,
|
||||
rcl_transition_error_error,
|
||||
RCL_LIFECYCLE_RET_ERROR);
|
||||
state_machine->current_state = &rcl_state_unconfigured;
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
#if __cplusplus
|
||||
}
|
||||
#endif // extern "C"
|
||||
36
rcl_lifecycle/src/default_state_machine.h
Normal file
36
rcl_lifecycle/src/default_state_machine.h
Normal file
|
|
@ -0,0 +1,36 @@
|
|||
// 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.
|
||||
|
||||
#ifndef DEFAULT_STATE_MACHINE_H_
|
||||
#define DEFAULT_STATE_MACHINE_H_
|
||||
|
||||
#include <rcl/types.h>
|
||||
|
||||
#include "rcl_lifecycle/data_types.h"
|
||||
#include "rcl_lifecycle/visibility_control.h"
|
||||
|
||||
#if __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
RCL_LIFECYCLE_PUBLIC
|
||||
rcl_ret_t
|
||||
rcl_lifecycle_init_default_state_machine(rcl_lifecycle_state_machine_t * state_machine);
|
||||
|
||||
#if __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // DEFAULT_STATE_MACHINE_H_
|
||||
178
rcl_lifecycle/src/rcl_lifecycle.c
Normal file
178
rcl_lifecycle/src/rcl_lifecycle.c
Normal file
|
|
@ -0,0 +1,178 @@
|
|||
// 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.
|
||||
|
||||
#if __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "rcl/rcl.h"
|
||||
#include "rcl/error_handling.h"
|
||||
|
||||
#include "rcl_lifecycle/rcl_lifecycle.h"
|
||||
#include "rcl_lifecycle/transition_map.h"
|
||||
|
||||
#include "com_interface.h" // NOLINT
|
||||
#include "default_state_machine.h" // NOLINT
|
||||
#include "states.h" // NOLINT
|
||||
|
||||
// get zero initialized state machine here
|
||||
rcl_lifecycle_state_machine_t
|
||||
rcl_lifecycle_get_zero_initialized_state_machine()
|
||||
{
|
||||
rcl_lifecycle_state_machine_t state_machine;
|
||||
state_machine.transition_map.states = NULL;
|
||||
state_machine.transition_map.states_size = 0;
|
||||
state_machine.transition_map.transitions = NULL;
|
||||
state_machine.transition_map.transitions_size = 0;
|
||||
state_machine.com_interface = rcl_lifecycle_get_zero_initialized_com_interface();
|
||||
return state_machine;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_lifecycle_state_machine_init(
|
||||
rcl_lifecycle_state_machine_t * state_machine,
|
||||
rcl_node_t * node_handle,
|
||||
const rosidl_message_type_support_t * ts_pub_notify,
|
||||
const rosidl_service_type_support_t * ts_srv_change_state,
|
||||
const rosidl_service_type_support_t * ts_srv_get_state,
|
||||
const rosidl_service_type_support_t * ts_srv_get_available_states,
|
||||
const rosidl_service_type_support_t * ts_srv_get_available_transitions,
|
||||
bool default_states)
|
||||
{
|
||||
if (rcl_lifecycle_com_interface_init(
|
||||
&state_machine->com_interface, node_handle,
|
||||
ts_pub_notify,
|
||||
ts_srv_change_state, ts_srv_get_state,
|
||||
ts_srv_get_available_states, ts_srv_get_available_transitions) != RCL_RET_OK)
|
||||
{
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
|
||||
if (default_states) {
|
||||
rcl_lifecycle_init_default_state_machine(state_machine);
|
||||
}
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_lifecycle_state_machine_fini(
|
||||
rcl_lifecycle_state_machine_t * state_machine,
|
||||
rcl_node_t * node_handle)
|
||||
{
|
||||
rcl_ret_t fcn_ret = RCL_RET_OK;
|
||||
|
||||
fcn_ret = rcl_lifecycle_com_interface_fini(&state_machine->com_interface, node_handle);
|
||||
|
||||
rcl_lifecycle_transition_map_t * transition_map = &state_machine->transition_map;
|
||||
|
||||
// for each state free the allocations for their keys/transitions
|
||||
for (unsigned int i = 0; i < transition_map->states_size; ++i) {
|
||||
free(transition_map->states[i].valid_transition_keys);
|
||||
free(transition_map->states[i].valid_transitions);
|
||||
}
|
||||
// free the primary states
|
||||
free(transition_map->states);
|
||||
transition_map->states = NULL;
|
||||
// free the tansitions
|
||||
free(transition_map->transitions);
|
||||
transition_map->transitions = NULL;
|
||||
|
||||
return fcn_ret;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_lifecycle_state_machine_is_initialized(const rcl_lifecycle_state_machine_t * state_machine)
|
||||
{
|
||||
if (!state_machine->com_interface.srv_get_state.impl) {
|
||||
RCL_SET_ERROR_MSG("get_state service is null");
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
if (!state_machine->com_interface.srv_change_state.impl) {
|
||||
RCL_SET_ERROR_MSG("change_state service is null");
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
const rcl_lifecycle_transition_t *
|
||||
rcl_lifecycle_is_valid_transition(
|
||||
rcl_lifecycle_state_machine_t * state_machine,
|
||||
rcl_lifecycle_ret_t key)
|
||||
{
|
||||
unsigned int current_id = state_machine->current_state->id;
|
||||
const rcl_lifecycle_state_t * current_state = rcl_lifecycle_get_state(
|
||||
&state_machine->transition_map, current_id);
|
||||
|
||||
for (unsigned int i = 0; i < current_state->valid_transition_size; ++i) {
|
||||
if (current_state->valid_transition_keys[i] == key) {
|
||||
return ¤t_state->valid_transitions[i];
|
||||
}
|
||||
}
|
||||
fprintf(stderr, "%s:%u, No callback transition matching %u found for current state %s\n",
|
||||
__FILE__, __LINE__, key, state_machine->current_state->label);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_lifecycle_trigger_transition(
|
||||
rcl_lifecycle_state_machine_t * state_machine,
|
||||
rcl_lifecycle_ret_t key, bool publish_notification)
|
||||
{
|
||||
const rcl_lifecycle_transition_t * transition =
|
||||
rcl_lifecycle_is_valid_transition(state_machine, key);
|
||||
|
||||
// If we have a faulty transition pointer
|
||||
if (!transition) {
|
||||
fprintf(stderr, "No transition found for node %s with key %d\n",
|
||||
state_machine->current_state->label, key);
|
||||
RCL_SET_ERROR_MSG("Transition is not registered.");
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
|
||||
if (!transition->goal) {
|
||||
fprintf(stderr, "No valid goal is set\n");
|
||||
}
|
||||
state_machine->current_state = transition->goal;
|
||||
if (publish_notification) {
|
||||
rcl_lifecycle_com_interface_publish_notification(
|
||||
&state_machine->com_interface, transition->start, state_machine->current_state);
|
||||
}
|
||||
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
rcl_print_state_machine(const rcl_lifecycle_state_machine_t * state_machine)
|
||||
{
|
||||
const rcl_lifecycle_transition_map_t * map = &state_machine->transition_map;
|
||||
for (size_t i = 0; i < map->states_size; ++i) {
|
||||
printf("Primary State: %s(%u)\n", map->states[i].label, map->states[i].id);
|
||||
printf("# of valid transitions: %u\n", map->states[i].valid_transition_size);
|
||||
for (size_t j = 0; j < map->states[i].valid_transition_size; ++j) {
|
||||
printf("\tNode %s: Key %d: Transition: %s\n",
|
||||
map->states[i].label,
|
||||
map->states[i].valid_transition_keys[j],
|
||||
map->states[i].valid_transitions[j].label);
|
||||
}
|
||||
}
|
||||
}
|
||||
#if __cplusplus
|
||||
}
|
||||
#endif // extern "C"
|
||||
51
rcl_lifecycle/src/states.h
Normal file
51
rcl_lifecycle/src/states.h
Normal file
|
|
@ -0,0 +1,51 @@
|
|||
// Copyright 2015 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.
|
||||
|
||||
#ifndef STATES_H_
|
||||
#define STATES_H_
|
||||
|
||||
#include <rcl_lifecycle/visibility_control.h>
|
||||
#include <rcl_lifecycle/data_types.h>
|
||||
|
||||
#if __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
// primary states based on
|
||||
// design.ros2.org/articles/node_lifecycle.html
|
||||
extern const rcl_lifecycle_state_t rcl_state_unknown;
|
||||
extern const rcl_lifecycle_state_t rcl_state_unconfigured;
|
||||
extern const rcl_lifecycle_state_t rcl_state_inactive;
|
||||
extern const rcl_lifecycle_state_t rcl_state_active;
|
||||
extern const rcl_lifecycle_state_t rcl_state_finalized;
|
||||
|
||||
extern const rcl_lifecycle_state_t rcl_state_configuring;
|
||||
extern const rcl_lifecycle_state_t rcl_state_cleaningup;
|
||||
extern const rcl_lifecycle_state_t rcl_state_shuttingdown;
|
||||
extern const rcl_lifecycle_state_t rcl_state_activating;
|
||||
extern const rcl_lifecycle_state_t rcl_state_deactivating;
|
||||
extern const rcl_lifecycle_state_t rcl_state_errorprocessing;
|
||||
|
||||
extern const rcl_lifecycle_transition_t rcl_transition_configure;
|
||||
extern const rcl_lifecycle_transition_t rcl_transition_cleanup;
|
||||
extern const rcl_lifecycle_transition_t rcl_transition_shutdown;
|
||||
extern const rcl_lifecycle_transition_t rcl_transition_activate;
|
||||
extern const rcl_lifecycle_transition_t rcl_transition_deactivate;
|
||||
|
||||
#if __cplusplus
|
||||
}
|
||||
#endif // extern "C"
|
||||
|
||||
#endif // STATES_H_
|
||||
106
rcl_lifecycle/src/transition_map.c
Normal file
106
rcl_lifecycle/src/transition_map.c
Normal file
|
|
@ -0,0 +1,106 @@
|
|||
// 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.
|
||||
|
||||
#if __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "rcl_lifecycle/transition_map.h"
|
||||
|
||||
void
|
||||
rcl_lifecycle_register_state(
|
||||
rcl_lifecycle_transition_map_t * transition_map,
|
||||
rcl_lifecycle_state_t state)
|
||||
{
|
||||
if (rcl_lifecycle_get_state(transition_map, state.id) != NULL) {
|
||||
// primary state is already registered
|
||||
fprintf(stderr, "%s:%u, State %u is already registered\n",
|
||||
__FILE__, __LINE__, state.id);
|
||||
return;
|
||||
}
|
||||
|
||||
// add new primary state memory
|
||||
// TODO(karsten1987): Add function for reallocf (see rcl)
|
||||
++transition_map->states_size;
|
||||
transition_map->states = realloc(
|
||||
transition_map->states,
|
||||
transition_map->states_size * sizeof(rcl_lifecycle_state_t));
|
||||
|
||||
transition_map->states[transition_map->states_size - 1] = state;
|
||||
}
|
||||
|
||||
void
|
||||
rcl_lifecycle_register_transition(
|
||||
rcl_lifecycle_transition_map_t * transition_map,
|
||||
rcl_lifecycle_transition_t transition,
|
||||
rcl_lifecycle_ret_t key)
|
||||
{
|
||||
// we add a new transition, so increase the size
|
||||
++transition_map->transitions_size;
|
||||
transition_map->transitions = realloc(
|
||||
transition_map->transitions,
|
||||
transition_map->transitions_size * sizeof(rcl_lifecycle_transition_t));
|
||||
// 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
|
||||
rcl_lifecycle_state_t * state = rcl_lifecycle_get_state(transition_map, transition.start->id);
|
||||
|
||||
++state->valid_transition_size;
|
||||
state->valid_transitions = realloc(
|
||||
state->valid_transitions,
|
||||
state->valid_transition_size * sizeof(rcl_lifecycle_transition_t));
|
||||
state->valid_transition_keys = realloc(
|
||||
state->valid_transition_keys,
|
||||
state->valid_transition_size * sizeof(rcl_lifecycle_ret_t));
|
||||
|
||||
// assign key
|
||||
state->valid_transition_keys[state->valid_transition_size - 1] = key;
|
||||
state->valid_transitions[state->valid_transition_size - 1] = transition;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
#if __cplusplus
|
||||
}
|
||||
#endif
|
||||
Loading…
Add table
Add a link
Reference in a new issue