diff --git a/rcl_lifecycle/CMakeLists.txt b/rcl_lifecycle/CMakeLists.txt new file mode 100644 index 0000000..d082fca --- /dev/null +++ b/rcl_lifecycle/CMakeLists.txt @@ -0,0 +1,94 @@ +cmake_minimum_required(VERSION 3.5) + +project(rcl_lifecycle) + +find_package(ament_cmake REQUIRED) +find_package(rcl REQUIRED) +find_package(rmw REQUIRED) +find_package(rmw_implementation_cmake REQUIRED) +find_package(lifecycle_msgs REQUIRED) + +include_directories(include) + +set(rcl_lifecycle_sources + src/com_interface.c + src/default_state_machine.c + src/rcl_lifecycle.c + src/transition_map.c) +set_source_files_properties( + ${rcl_lifecycle_sources} + PROPERTIES language "C") + +macro(targets) + get_rcl_information("${rmw_implementation}" "rcl${target_suffix}") + + ### C-Library depending only on RCL + add_library( + rcl_lifecycle${target_suffix} + SHARED + ${rcl_lifecycle_sources}) + + ament_target_dependencies(rcl_lifecycle${target_suffix} + "rcl${target_suffix}" + "lifecycle_msgs") + + # Causes the visibility macros to use dllexport rather than dllimport, + # which is appropriate when building the dll but not consuming it. + target_compile_definitions(rcl_lifecycle${target_suffix} PRIVATE "RCL_LIFECYCLE_BUILDING_DLL") + + install(TARGETS rcl_lifecycle${target_suffix} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) +endmacro() + +call_for_each_rmw_implementation(targets GENERATE_DEFAULT) + +if(BUILD_TESTING) + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} "-std=c++14") + add_compile_options(-Wall -Wextra -Wpedantic) + endif() + + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(rcl REQUIRED) + ament_lint_auto_find_test_dependencies() + + # Gtests + ament_add_gtest(test_default_state_machine + test/test_default_state_machine.cpp + ) + if(TARGET test_default_state_machine) + target_include_directories(test_default_state_machine PUBLIC + ${rcl_INCLUDE_DIRS} + ${rcl_lifecycle_INCLUDE_DIRS} + ) + target_link_libraries(test_default_state_machine ${PROJECT_NAME}) + endif() + ament_add_gtest(test_multiple_instances + test/test_multiple_instances.cpp + ) + if(TARGET test_multiple_instances) + target_include_directories(test_multiple_instances PUBLIC + ${rcl_INCLUDE_DIRS} + ${rcl_lifecycle_INCLUDE_DIRS} + ) + target_link_libraries(test_multiple_instances ${PROJECT_NAME}) + endif() +endif() + +ament_export_dependencies(ament_cmake) +ament_export_dependencies(lifecycle_msgs) +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_package( + CONFIG_EXTRAS rcl_lifecycle-extras.cmake) + +install( + DIRECTORY cmake + DESTINATION share/${PROJECT_NAME}) + +install( + DIRECTORY include/ + DESTINATION include) diff --git a/rcl_lifecycle/cmake/get_rcl_lifecycle_information.cmake b/rcl_lifecycle/cmake/get_rcl_lifecycle_information.cmake new file mode 100644 index 0000000..280553d --- /dev/null +++ b/rcl_lifecycle/cmake/get_rcl_lifecycle_information.cmake @@ -0,0 +1,89 @@ +# 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. + +# +# Get all information about rcl_lifecycle for a specific RMW implementation. +# +# It sets the common variables _DEFINITIONS, _INCLUDE_DIRS and _LIBRARIES +# with the given prefix. +# +# :param rmw_implementation: the RMW implementation name +# :type target: string +# :param var_prefix: the prefix of all output variable names +# :type var_prefix: string +# +# TODO(karsten1987): Copy/pasted from rclcpp/cmake/get_rclcpp_information.cmake. +# Code duplication is evil. Don't do this at home, kids. + +macro(get_rcl_lifecycle_information rmw_implementation var_prefix) + # pretend to be a "package" + # so that the variables can be used by various functions / macros + set(${var_prefix}_FOUND TRUE) + + # include directories + normalize_path(${var_prefix}_INCLUDE_DIRS "${rcl_lifecycle_DIR}/../../../include") + + # libraries + set(_libs) + # search for library relative to this CMake file + set(_library_target "rcl_lifecycle") + get_available_rmw_implementations(_rmw_impls) + list(LENGTH _rmw_impls _rmw_impls_length) + if(_rmw_impls_length GREATER 1) + set(_library_target "${_library_target}__${rmw_implementation}") + endif() + set(_lib "NOTFOUND") + find_library( + _lib NAMES "${_library_target}" + PATHS "${rcl_lifecycle_DIR}/../../../lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH + ) + if(NOT _lib) + # warn about not existing library and ignore it + message(WARNING "Package 'rcl_lifecycle' doesn't contain the library '${_library_target}'") + elseif(NOT IS_ABSOLUTE "${_lib}") + # the found library must be an absolute path + message(FATAL_ERROR "Package 'rcl_lifecycle' found the library '${_library_target}' at '${_lib}' which is not an absolute path") + elseif(NOT EXISTS "${_lib}") + # the found library must exist + message(FATAL_ERROR "Package 'rcl_lifecycle' found the library '${_lib}' which doesn't exist") + else() + list(APPEND _libs "${_lib}") + endif() + + # dependencies + set(_exported_dependencies + "rmw" + "${rmw_implementation}" + "rosidl_generator_c") + set(${var_prefix}_DEFINITIONS) + foreach(_dep ${_exported_dependencies}) + if(NOT ${_dep}_FOUND) + find_package("${_dep}" QUIET REQUIRED) + endif() + if(${_dep}_DEFINITIONS) + list_append_unique(${var_prefix}_DEFINITIONS "${${_dep}_DEFINITIONS}") + endif() + if(${_dep}_INCLUDE_DIRS) + list_append_unique(${var_prefix}_INCLUDE_DIRS "${${_dep}_INCLUDE_DIRS}") + endif() + if(${_dep}_LIBRARIES) + list(APPEND _libs "${${_dep}_LIBRARIES}") + endif() + endforeach() + if(_libs) + ament_libraries_deduplicate(_libs "${_libs}") + endif() + set(${var_prefix}_LIBRARIES "${_libs}") +endmacro() diff --git a/rcl_lifecycle/include/rcl_lifecycle/data_types.h b/rcl_lifecycle/include/rcl_lifecycle/data_types.h new file mode 100644 index 0000000..226003c --- /dev/null +++ b/rcl_lifecycle/include/rcl_lifecycle/data_types.h @@ -0,0 +1,93 @@ +// 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 RCL_LIFECYCLE__DATA_TYPES_H_ +#define RCL_LIFECYCLE__DATA_TYPES_H_ + +#include +#include "rcl_lifecycle/visibility_control.h" + +#if __cplusplus +extern "C" +{ +#endif + +typedef int rcl_lifecycle_ret_t; +#define RCL_LIFECYCLE_RET_OK -1 +#define RCL_LIFECYCLE_RET_FAILURE -2 +#define RCL_LIFECYCLE_RET_ERROR -3 + +typedef struct rcl_lifecycle_transition_t rcl_lifecycle_transition_t; + +typedef struct rcl_lifecycle_state_t +{ + const char * label; + unsigned int id; + + // a valid key is a generic identifier for deciding + // which transitions to trigger. + // This serves the purpose of hiding a unique ID from + // users prospective. + // e.g. shutdown + // the concrete transition for the state "unconfigured" + // is "unconfigured_shutdown". However, the user only specifies + // "shutdown". So we register the "unconfigured_shutdown" + // transition with the impuls "shutdown". + rcl_lifecycle_ret_t * valid_transition_keys; + rcl_lifecycle_transition_t * valid_transitions; + unsigned int valid_transition_size; +} rcl_lifecycle_state_t; + +typedef struct rcl_lifecycle_transition_t +{ + const char * label; + unsigned int id; + const rcl_lifecycle_state_t * start; + const rcl_lifecycle_state_t * goal; +} rcl_lifecycle_transition_t; + +// TODO(karsten1987): Rename map as it is no +// associated array any longer +typedef struct rcl_lifecycle_transition_map_t +{ + rcl_lifecycle_state_t * states; + unsigned int states_size; + rcl_lifecycle_transition_t * transitions; + unsigned int transitions_size; +} rcl_lifecycle_transition_map_t; + +typedef struct rcl_lifecycle_com_interface_t +{ + rcl_node_t * node_handle; + rcl_publisher_t pub_transition_event; + rcl_service_t srv_change_state; + rcl_service_t srv_get_state; + rcl_service_t srv_get_available_states; + rcl_service_t srv_get_available_transitions; +} rcl_lifecycle_com_interface_t; + +typedef struct rcl_lifecycle_state_machine_t +{ + const rcl_lifecycle_state_t * current_state; + // Map/Associated array of registered states and transitions + rcl_lifecycle_transition_map_t transition_map; + // Communication interface into a ROS world + rcl_lifecycle_com_interface_t com_interface; +} rcl_lifecycle_state_machine_t; + +#if __cplusplus +} +#endif + +#endif // RCL_LIFECYCLE__DATA_TYPES_H_ diff --git a/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h b/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h new file mode 100644 index 0000000..a5ce128 --- /dev/null +++ b/rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h @@ -0,0 +1,86 @@ +// 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 RCL_LIFECYCLE__RCL_LIFECYCLE_H_ +#define RCL_LIFECYCLE__RCL_LIFECYCLE_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include + +#include +#include + +RCL_LIFECYCLE_PUBLIC +rcl_lifecycle_state_machine_t +rcl_lifecycle_get_zero_initialized_state_machine(); + +RCL_LIFECYCLE_PUBLIC +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); + +RCL_LIFECYCLE_PUBLIC +rcl_ret_t +rcl_lifecycle_state_machine_fini( + rcl_lifecycle_state_machine_t * state_machine, + rcl_node_t * node_handle); + +RCL_LIFECYCLE_PUBLIC +rcl_ret_t +rcl_lifecycle_state_machine_is_initialized( + const rcl_lifecycle_state_machine_t * state_machine); + +RCL_LIFECYCLE_PUBLIC +const rcl_lifecycle_transition_t * +rcl_lifecycle_is_valid_callback_transition( + rcl_lifecycle_state_machine_t * state_machine, + rcl_lifecycle_ret_t key); + +/// Execute a transition +/* + * Important note for \param key here: + * This is meant as feedback from the high level + * callback associated with this transition. + * The key is the index for the valid transitions + * associated with the current state. + * This key may either be a valid external stimuli + * such as "configure" or direct return codes from + * callbacks such as RCL_LIFECYCLE_RET_OK et. al. + */ +RCL_LIFECYCLE_PUBLIC +rcl_ret_t +rcl_lifecycle_trigger_transition( + rcl_lifecycle_state_machine_t * state_machine, + rcl_lifecycle_ret_t key, bool publish_notification); + +RCL_LIFECYCLE_PUBLIC +void +rcl_print_state_machine(const rcl_lifecycle_state_machine_t * state_machine); + +#if __cplusplus +} +#endif // extern "C" + +#endif // RCL_LIFECYCLE__RCL_LIFECYCLE_H_ diff --git a/rcl_lifecycle/include/rcl_lifecycle/transition_map.h b/rcl_lifecycle/include/rcl_lifecycle/transition_map.h new file mode 100644 index 0000000..c06addd --- /dev/null +++ b/rcl_lifecycle/include/rcl_lifecycle/transition_map.h @@ -0,0 +1,55 @@ +// 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 RCL_LIFECYCLE__TRANSITION_MAP_H_ +#define RCL_LIFECYCLE__TRANSITION_MAP_H_ + +#include + +#if __cplusplus +extern "C" +{ +#endif + +RCL_LIFECYCLE_PUBLIC +void +rcl_lifecycle_register_state( + rcl_lifecycle_transition_map_t * map, + rcl_lifecycle_state_t state); + +RCL_LIFECYCLE_PUBLIC +void +rcl_lifecycle_register_transition( + rcl_lifecycle_transition_map_t * transition_map, + rcl_lifecycle_transition_t transition, + rcl_lifecycle_ret_t key); + +RCL_LIFECYCLE_PUBLIC +rcl_lifecycle_state_t * +rcl_lifecycle_get_state( + rcl_lifecycle_transition_map_t * transition_map, + unsigned int state_id); + +RCL_LIFECYCLE_PUBLIC +rcl_lifecycle_transition_t * +rcl_lifecycle_get_transitions( + rcl_lifecycle_transition_map_t * transition_map, + unsigned int state_id); + +#if __cplusplus +} +#endif + +#endif // RCL_LIFECYCLE__TRANSITION_MAP_H_ diff --git a/rcl_lifecycle/include/rcl_lifecycle/visibility_control.h b/rcl_lifecycle/include/rcl_lifecycle/visibility_control.h new file mode 100644 index 0000000..be4b922 --- /dev/null +++ b/rcl_lifecycle/include/rcl_lifecycle/visibility_control.h @@ -0,0 +1,56 @@ +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef RCL_LIFECYCLE__VISIBILITY_CONTROL_H_ +#define RCL_LIFECYCLE__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RCL_LIFECYCLE_EXPORT __attribute__ ((dllexport)) + #define RCL_LIFECYCLE_IMPORT __attribute__ ((dllimport)) + #else + #define RCL_LIFECYCLE_EXPORT __declspec(dllexport) + #define RCL_LIFECYCLE_IMPORT __declspec(dllimport) + #endif + #ifdef RCL_LIFECYCLE_BUILDING_DLL + #define RCL_LIFECYCLE_PUBLIC RCL_LIFECYCLE_EXPORT + #else + #define RCL_LIFECYCLE_PUBLIC RCL_LIFECYCLE_IMPORT + #endif + #define RCL_LIFECYCLE_PUBLIC_TYPE RCL_LIFECYCLE_PUBLIC + #define RCL_LIFECYCLE_LOCAL +#else + #define RCL_LIFECYCLE_EXPORT __attribute__ ((visibility("default"))) + #define RCL_LIFECYCLE_IMPORT + #if __GNUC__ >= 4 + #define RCL_LIFECYCLE_PUBLIC __attribute__ ((visibility("default"))) + #define RCL_LIFECYCLE_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RCL_LIFECYCLE_PUBLIC + #define RCL_LIFECYCLE_LOCAL + #endif + #define RCL_LIFECYCLE_PUBLIC_TYPE +#endif + +#endif // RCL_LIFECYCLE__VISIBILITY_CONTROL_H_ diff --git a/rcl_lifecycle/package.xml b/rcl_lifecycle/package.xml new file mode 100644 index 0000000..0cc7b38 --- /dev/null +++ b/rcl_lifecycle/package.xml @@ -0,0 +1,31 @@ + + + + rcl_lifecycle + 0.0.0 + Package containing a C-based lifecycle implementation + Karsten Knese + Apache License 2.0 + + ament_cmake + rosidl_default_generators + + lifecycle_msgs + rcl + rmw_implementation + rmw_implementation_cmake + rosidl_default_generators + + lifecycle_msgs + rcl + rmw_implementation + rosidl_default_runtime + + ament_cmake_gtest + ament_lint_common + ament_lint_auto + + + ament_cmake + + diff --git a/rcl_lifecycle/rcl_lifecycle-extras.cmake b/rcl_lifecycle/rcl_lifecycle-extras.cmake new file mode 100644 index 0000000..4f2f4a5 --- /dev/null +++ b/rcl_lifecycle/rcl_lifecycle-extras.cmake @@ -0,0 +1,17 @@ +# 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. + +# copied from rcl/rcl-extras.cmake + +include("${rcl_lifecycle_DIR}/get_rcl_lifecycle_information.cmake") diff --git a/rcl_lifecycle/src/com_interface.c b/rcl_lifecycle/src/com_interface.c new file mode 100644 index 0000000..e7a89d1 --- /dev/null +++ b/rcl_lifecycle/src/com_interface.c @@ -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 +#include + +#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 diff --git a/rcl_lifecycle/src/com_interface.h b/rcl_lifecycle/src/com_interface.h new file mode 100644 index 0000000..2b5ec0f --- /dev/null +++ b/rcl_lifecycle/src/com_interface.h @@ -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_ diff --git a/rcl_lifecycle/src/default_state_machine.c b/rcl_lifecycle/src/default_state_machine.c new file mode 100644 index 0000000..e0a1594 --- /dev/null +++ b/rcl_lifecycle/src/default_state_machine.c @@ -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 +#include +#include + +#include +#include + +#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" diff --git a/rcl_lifecycle/src/default_state_machine.h b/rcl_lifecycle/src/default_state_machine.h new file mode 100644 index 0000000..1d629ea --- /dev/null +++ b/rcl_lifecycle/src/default_state_machine.h @@ -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 + +#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_ diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c new file mode 100644 index 0000000..32ca640 --- /dev/null +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -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 +#include +#include + +#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" diff --git a/rcl_lifecycle/src/states.h b/rcl_lifecycle/src/states.h new file mode 100644 index 0000000..ac82863 --- /dev/null +++ b/rcl_lifecycle/src/states.h @@ -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 +#include + +#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_ diff --git a/rcl_lifecycle/src/transition_map.c b/rcl_lifecycle/src/transition_map.c new file mode 100644 index 0000000..a44439e --- /dev/null +++ b/rcl_lifecycle/src/transition_map.c @@ -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 +#include +#include + +#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 diff --git a/rcl_lifecycle/test/test_default_state_machine.cpp b/rcl_lifecycle/test/test_default_state_machine.cpp new file mode 100644 index 0000000..1d2774c --- /dev/null +++ b/rcl_lifecycle/test/test_default_state_machine.cpp @@ -0,0 +1,714 @@ +// 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. + +// testing default transition sequence. +// This test requires that the transitions are set +// as depicted in design.ros2.org + +#include + +#include +#include + +#include + +#include "rcl/error_handling.h" +#include "rcl/rcl.h" + +#include "rcl_lifecycle/rcl_lifecycle.h" +#include "../src/default_state_machine.h" + +class TestDefaultStateMachine : public ::testing::Test +{ +protected: + rcl_node_t * node_ptr; + void SetUp() + { + rcl_ret_t ret; + ret = rcl_init(0, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "node_name"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + } + + void TearDown() + { + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_shutdown(); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + } +}; + +std::vector keys = +{ + lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, + lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE, + lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE, + lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, + lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN, + RCL_LIFECYCLE_RET_OK, + RCL_LIFECYCLE_RET_FAILURE, + RCL_LIFECYCLE_RET_ERROR +}; + +void +test_trigger_transition( + rcl_lifecycle_state_machine_t * state_machine, + int key, + unsigned int expected_current_state, + unsigned int expected_goal_state) +{ + EXPECT_EQ( + expected_current_state, state_machine->current_state->id); + EXPECT_EQ( + RCL_RET_OK, rcl_lifecycle_trigger_transition( + state_machine, key, false)); + EXPECT_EQ( + expected_goal_state, state_machine->current_state->id); +} +/* + * Test suite + */ +TEST_F(TestDefaultStateMachine, zero_init) { + rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); + ASSERT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR); + const rcl_lifecycle_transition_map_t * transition_map = &state_machine.transition_map; + ASSERT_EQ(transition_map->states_size, (unsigned int)0); + ASSERT_EQ(transition_map->states, nullptr); + ASSERT_EQ(transition_map->transitions_size, (unsigned int)0); + ASSERT_EQ(transition_map->transitions, nullptr); +} + +TEST_F(TestDefaultStateMachine, default_sequence) { + rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); + rcl_lifecycle_init_default_state_machine(&state_machine); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING, + lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE, + lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN, + lifecycle_msgs__msg__State__PRIMARY_STATE_FINALIZED); +} + +TEST_F(TestDefaultStateMachine, wrong_default_sequence) { + rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); + rcl_lifecycle_init_default_state_machine(&state_machine); + + { // supposed to stay unconfigured for all invalid + for (auto it = keys.begin(); it != keys.end(); ++it) { + if (*it == lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE || + *it == lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN) {continue;} + + EXPECT_EQ( + RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false)); + EXPECT_EQ( + state_machine.current_state->id, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED); + } + } + + { // supposed to stay configuring for all invalid + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING); + + for (auto it = keys.begin(); it != keys.end(); ++it) { + if (*it == RCL_LIFECYCLE_RET_OK || + *it == RCL_LIFECYCLE_RET_FAILURE || + *it == RCL_LIFECYCLE_RET_ERROR) {continue;} + + EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false)); + EXPECT_EQ(state_machine.current_state->id, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING); + } + } + + { // supposed to stay inactive for all invalid + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE); + + for (auto it = keys.begin(); it != keys.end(); ++it) { + if (*it == lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP || + *it == lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE || + *it == lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN) {continue;} + + fprintf(stderr, "applying key %u\n", *it); + EXPECT_EQ( + RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false)); + EXPECT_EQ(state_machine.current_state->id, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE); + } + } + + { // supposed to stay activating for all invalid + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING); + + for (auto it = keys.begin(); it != keys.end(); ++it) { + if (*it == RCL_LIFECYCLE_RET_OK || + *it == RCL_LIFECYCLE_RET_FAILURE || + *it == RCL_LIFECYCLE_RET_ERROR) {continue;} + + EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false)); + EXPECT_EQ(state_machine.current_state->id, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING); + } + } + + { // supposed to stay active for all invalid + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING, + lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE); + + for (auto it = keys.begin(); it != keys.end(); ++it) { + if (*it == lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE || + *it == lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN) + {continue;} + + EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false)); + EXPECT_EQ(state_machine.current_state->id, + lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE); + } + } + + { // supposed to stay deactivating for all invalid + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE, + lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING); + + for (auto it = keys.begin(); it != keys.end(); ++it) { + if (*it == RCL_LIFECYCLE_RET_OK || + *it == RCL_LIFECYCLE_RET_FAILURE || + *it == RCL_LIFECYCLE_RET_ERROR) {continue;} + + EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false)); + EXPECT_EQ(state_machine.current_state->id, + lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING); + } + } + + { // supposed to stay cleanup for all invalid + // skip inactive, we tested that already + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE); + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP); + + for (auto it = keys.begin(); it != keys.end(); ++it) { + if (*it == RCL_LIFECYCLE_RET_OK || + *it == RCL_LIFECYCLE_RET_FAILURE || + *it == RCL_LIFECYCLE_RET_ERROR) {continue;} + + EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false)); + EXPECT_EQ(state_machine.current_state->id, + lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP); + } + } + + { // supposed to stay shutting down for all invalid + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED); + // shutdown directly, since we tested already unconfigured + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN); + + for (auto it = keys.begin(); it != keys.end(); ++it) { + if (*it == RCL_LIFECYCLE_RET_OK || + *it == RCL_LIFECYCLE_RET_FAILURE || + *it == RCL_LIFECYCLE_RET_ERROR) {continue;} + + EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false)); + EXPECT_EQ(state_machine.current_state->id, + lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN); + } + } + + { // supposed to stay finalized for all invalid + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN, + lifecycle_msgs__msg__State__PRIMARY_STATE_FINALIZED); + + for (auto it = keys.begin(); it != keys.end(); ++it) { + EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false)); + EXPECT_EQ(state_machine.current_state->id, + lifecycle_msgs__msg__State__PRIMARY_STATE_FINALIZED); + } + } +} + +TEST_F(TestDefaultStateMachine, default_in_a_loop) { + rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); + rcl_lifecycle_init_default_state_machine(&state_machine); + + for (auto i = 0; i < 5; ++i) { + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING, + lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE, + lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED); + } + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN, + lifecycle_msgs__msg__State__PRIMARY_STATE_FINALIZED); +} + +TEST_F(TestDefaultStateMachine, default_sequence_failure) { + rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); + rcl_lifecycle_init_default_state_machine(&state_machine); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_FAILURE, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED); + + /////////////// + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_FAILURE, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE); + + ////////////////////////// + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING, + lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE); + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE, + lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_FAILURE, + lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING, + lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE); + + ////////////////////////////// + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE, + lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING); + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE); + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP); + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_FAILURE, + lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE); + + ///////////////////////////// + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP); + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED); + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN); + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_FAILURE, + lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN, + lifecycle_msgs__msg__State__PRIMARY_STATE_FINALIZED); +} + +TEST_F(TestDefaultStateMachine, default_sequence_error_resolved) { + rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); + rcl_lifecycle_init_default_state_machine(&state_machine); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_ERROR, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING, + lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED); + + /////////////// + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_ERROR, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING, + lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED); + + ////////////////////////// + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING, + lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE, + lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING); + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_ERROR, + lifecycle_msgs__msg__State__TRANSITION_STATE_DEACTIVATING, + lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED); + + ////////////////////////////// + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING); + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE); + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP); + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_ERROR, + lifecycle_msgs__msg__State__TRANSITION_STATE_CLEANINGUP, + lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED); + + ///////////////////////////// + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN); + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_ERROR, + lifecycle_msgs__msg__State__TRANSITION_STATE_SHUTTINGDOWN, + lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING); + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED); +} + +TEST_F(TestDefaultStateMachine, default_sequence_error_unresolved) { + { + rcl_lifecycle_state_machine_t state_machine = + rcl_lifecycle_get_zero_initialized_state_machine(); + rcl_lifecycle_init_default_state_machine(&state_machine); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_ERROR, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING, + lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_FAILURE, + lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING, + lifecycle_msgs__msg__State__PRIMARY_STATE_FINALIZED); + } + + { + rcl_lifecycle_state_machine_t state_machine = + rcl_lifecycle_get_zero_initialized_state_machine(); + rcl_lifecycle_init_default_state_machine(&state_machine); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_OK, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE); + + test_trigger_transition( + &state_machine, + lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE, + lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_ERROR, + lifecycle_msgs__msg__State__TRANSITION_STATE_ACTIVATING, + lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING); + + test_trigger_transition( + &state_machine, + RCL_LIFECYCLE_RET_ERROR, + lifecycle_msgs__msg__State__TRANSITION_STATE_ERRORPROCESSING, + lifecycle_msgs__msg__State__PRIMARY_STATE_FINALIZED); + } +} diff --git a/rcl_lifecycle/test/test_multiple_instances.cpp b/rcl_lifecycle/test/test_multiple_instances.cpp new file mode 100644 index 0000000..4a34020 --- /dev/null +++ b/rcl_lifecycle/test/test_multiple_instances.cpp @@ -0,0 +1,100 @@ +// 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. + +// testing default transition sequence. +// This test requires that the transitions are set +// as depicted in design.ros2.org + +#include + +#include +#include + +#include + +#include "rcl/error_handling.h" +#include "rcl/rcl.h" + +#include "rcl_lifecycle/rcl_lifecycle.h" +#include "../src/default_state_machine.h" + +class TestMultipleInstances : public ::testing::Test +{ +protected: + rcl_node_t * node_ptr; + void SetUp() + { + rcl_ret_t ret; + ret = rcl_init(0, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "node_name"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + } + + void TearDown() + { + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_shutdown(); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + } +}; + +void +test_trigger_transition( + rcl_lifecycle_state_machine_t * state_machine, + int key, + unsigned int expected_current_state, + unsigned int expected_goal_state) +{ + EXPECT_EQ( + expected_current_state, state_machine->current_state->id); + EXPECT_EQ( + RCL_RET_OK, rcl_lifecycle_trigger_transition( + state_machine, key, false)); + EXPECT_EQ( + expected_goal_state, state_machine->current_state->id); +} + +TEST_F(TestMultipleInstances, default_sequence_error_unresolved) { + rcl_lifecycle_state_machine_t state_machine1 = + rcl_lifecycle_get_zero_initialized_state_machine(); + rcl_lifecycle_init_default_state_machine(&state_machine1); + + rcl_lifecycle_state_machine_t state_machine2 = + rcl_lifecycle_get_zero_initialized_state_machine(); + rcl_lifecycle_init_default_state_machine(&state_machine2); + + rcl_lifecycle_state_machine_t state_machine3 = + rcl_lifecycle_get_zero_initialized_state_machine(); + rcl_lifecycle_init_default_state_machine(&state_machine3); + + test_trigger_transition( + &state_machine1, + lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING); + + EXPECT_EQ( + lifecycle_msgs__msg__State__TRANSITION_STATE_CONFIGURING, state_machine1.current_state->id); + EXPECT_EQ( + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, state_machine2.current_state->id); + EXPECT_EQ( + lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, state_machine3.current_state->id); +}