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

add rcl lifecycle

Browse source 
* (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 2016-12-14 09:28:54 -08:00 • committed by GitHub
parent 4590fc5f77
commit a18ef97e5a
17 changed files with 2357 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

94
rcl_lifecycle/CMakeLists.txt Normal file
View file

@ -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)

89
rcl_lifecycle/cmake/get_rcl_lifecycle_information.cmake Normal file
View file

@ -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()

93
rcl_lifecycle/include/rcl_lifecycle/data_types.h Normal file
View file

@ -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 <rcl/rcl.h>
#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_

86
rcl_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h Normal file
View file

@ -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 <stdbool.h>
#include <rcl_lifecycle/visibility_control.h>
#include <rcl_lifecycle/data_types.h>
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_

55
rcl_lifecycle/include/rcl_lifecycle/transition_map.h Normal file
View file

@ -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 <rcl_lifecycle/data_types.h>
#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_

56
rcl_lifecycle/include/rcl_lifecycle/visibility_control.h Normal file
View file

@ -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_

31
rcl_lifecycle/package.xml Normal file
View file

@ -0,0 +1,31 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>rcl_lifecycle</name>
<version>0.0.0</version>
<description>Package containing a C-based lifecycle implementation</description>
<maintainer email="karsten@osrfoundation.org">Karsten Knese</maintainer>
<license>Apache License 2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<build_depend>lifecycle_msgs</build_depend>
<build_depend>rcl</build_depend>
<build_depend>rmw_implementation</build_depend>
<build_depend>rmw_implementation_cmake</build_depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>lifecycle_msgs</exec_depend>
<exec_depend>rcl</exec_depend>
<exec_depend>rmw_implementation</exec_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_lint_auto</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

17
rcl_lifecycle/rcl_lifecycle-extras.cmake Normal file
View file

@ -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")

305
rcl_lifecycle/src/com_interface.c Normal file
View 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
View 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
View 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
View 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
View 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 &current_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
View 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
View 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

714
rcl_lifecycle/test/test_default_state_machine.cpp Normal file
View file

@ -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 <gtest/gtest.h>
#include <lifecycle_msgs/msg/state.h>
#include <lifecycle_msgs/msg/transition.h>
#include <vector>
#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<rcl_lifecycle_ret_t> 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);
}
}

100
rcl_lifecycle/test/test_multiple_instances.cpp Normal file
View file

@ -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 <gtest/gtest.h>
#include <lifecycle_msgs/msg/state.h>
#include <lifecycle_msgs/msg/transition.h>
#include <vector>
#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);
}