From bb26fbc01d1e5697222d9f8bd080be24d72ef73d Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 18 Nov 2016 15:47:48 -0800 Subject: [PATCH 01/63] (dev) construct service from existing rcl_service_t --- rclcpp/CMakeLists.txt | 2 +- rclcpp/include/rclcpp/service.hpp | 39 +++++++------------ rclcpp/src/rclcpp/service.cpp | 8 ++-- .../test/test_externally_defined_services.cpp | 24 ++---------- 4 files changed, 22 insertions(+), 51 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 30bad0f764..c4869ded61 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -143,7 +143,7 @@ if(BUILD_TESTING) endif() get_default_rmw_implementation(default_rmw) - find_package(${default_rmw} REQUIRED) + find_package(${default_rmw}) get_rmw_typesupport(typesupport_impls_cpp "${default_rmw}" LANGUAGE "cpp") get_rmw_typesupport(typesupport_impls_c "${default_rmw}" LANGUAGE "c") set(mock_msg_files diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 9d32870e5d..350ab6f2b6 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -42,13 +42,7 @@ class ServiceBase RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase) RCLCPP_PUBLIC - ServiceBase( - std::shared_ptr node_handle, - const std::string & service_name); - - RCLCPP_PUBLIC - explicit ServiceBase( - std::shared_ptr node_handle); + explicit ServiceBase(std::shared_ptr node_handle); RCLCPP_PUBLIC virtual ~ServiceBase(); @@ -72,9 +66,7 @@ class ServiceBase std::shared_ptr node_handle_; - rcl_service_t * service_handle_ = nullptr; - std::string service_name_; - bool owns_rcl_handle_ = true; + rcl_service_t* service_handle_ = nullptr; }; using any_service_callback::AnyServiceCallback; @@ -100,13 +92,12 @@ class Service : public ServiceBase const std::string & service_name, AnyServiceCallback any_callback, rcl_service_options_t & service_options) - : ServiceBase(node_handle, service_name), any_callback_(any_callback) + : ServiceBase(node_handle), any_callback_(any_callback) { using rosidl_typesupport_cpp::get_service_type_support_handle; auto service_type_support_handle = get_service_type_support_handle(); // rcl does the static memory allocation here - service_handle_ = new rcl_service_t; *service_handle_ = rcl_get_zero_initialized_service(); if (rcl_service_init( @@ -120,24 +111,19 @@ class Service : public ServiceBase Service( std::shared_ptr node_handle, - rcl_service_t * service_handle, + rcl_service_t* service_handle, AnyServiceCallback any_callback) - : ServiceBase(node_handle), + : ServiceBase(node_handle), defined_extern(true), any_callback_(any_callback) { // check if service handle was initialized - // TODO(karsten1987): Take this verification - // directly in rcl_*_t - // see: https://github.com/ros2/rcl/issues/81 - if (!service_handle->impl) { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) + // TODO(Karsten1987): Can this go directly in RCL? + if (service_handle->impl == NULL) + { throw std::runtime_error( - std::string("rcl_service_t in constructor argument must be initialized beforehand.")); - // *INDENT-ON* + std::string("rcl_service_t in constructor argument \ + has to be initialized beforehand")); } - service_handle_ = service_handle; - service_name_ = std::string(rcl_service_get_service_name(service_handle)); - owns_rcl_handle_ = false; } Service() = delete; @@ -145,7 +131,8 @@ class Service : public ServiceBase virtual ~Service() { // check if you have ownership of the handle - if (owns_rcl_handle_) { + if (!defined_extern) + { if (rcl_service_fini(service_handle_, node_handle_.get()) != RCL_RET_OK) { std::stringstream ss; ss << "Error in destruction of rcl service_handle_ handle: " << @@ -193,6 +180,8 @@ class Service : public ServiceBase private: RCLCPP_DISABLE_COPY(Service) + bool defined_extern = false; + AnyServiceCallback any_callback_; }; diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 136d6883a0..f3ee79d76b 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -27,10 +27,8 @@ using rclcpp::service::ServiceBase; -ServiceBase::ServiceBase( - std::shared_ptr node_handle, - const std::string & service_name) -: node_handle_(node_handle), service_name_(service_name) +ServiceBase::ServiceBase(std::shared_ptr node_handle) +: node_handle_(node_handle) {} ServiceBase::ServiceBase(std::shared_ptr node_handle) @@ -43,7 +41,7 @@ ServiceBase::~ServiceBase() std::string ServiceBase::get_service_name() { - return this->service_name_; + return rcl_service_get_service_name(service_handle_); } const rcl_service_t * diff --git a/rclcpp/test/test_externally_defined_services.cpp b/rclcpp/test/test_externally_defined_services.cpp index efd05db1f6..d1ea5ae808 100644 --- a/rclcpp/test/test_externally_defined_services.cpp +++ b/rclcpp/test/test_externally_defined_services.cpp @@ -15,7 +15,6 @@ #include #include -#include #include "rclcpp/node.hpp" #include "rclcpp/any_service_callback.hpp" @@ -37,23 +36,8 @@ class TestExternallyDefinedServices : public ::testing::Test }; void -callback(const std::shared_ptr/*req*/, - std::shared_ptr/*resp*/) -{} - -TEST_F(TestExternallyDefinedServices, default_behavior) { - auto node_handle = rclcpp::node::Node::make_shared("base_node"); - - try { - auto srv = node_handle->create_service("test", - callback); - } catch (const std::exception &) { - FAIL(); - return; - } - SUCCEED(); -} - +callback(const std::shared_ptr /*req*/, + std::shared_ptr /*resp*/); TEST_F(TestExternallyDefinedServices, extern_defined_uninitialized) { auto node_handle = rclcpp::node::Node::make_shared("base_node"); @@ -93,7 +77,6 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) { FAIL(); return; } - rclcpp::any_service_callback::AnyServiceCallback cb; try { @@ -134,7 +117,8 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) { // Call destructor } - if (!service_handle.impl) { + if (service_handle.impl == NULL) + { FAIL(); return; } From 6d36ce828d11b7e8185615a7df790b4097fbdce1 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Sat, 19 Nov 2016 19:35:54 -0800 Subject: [PATCH 02/63] service takes rcl_node_t* --- rclcpp/include/rclcpp/service.hpp | 27 ++++++++++--------- rclcpp/src/rclcpp/service.cpp | 2 +- .../test/test_externally_defined_services.cpp | 8 +++--- 3 files changed, 19 insertions(+), 18 deletions(-) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 350ab6f2b6..261f24267b 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -42,7 +42,10 @@ class ServiceBase RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase) RCLCPP_PUBLIC - explicit ServiceBase(std::shared_ptr node_handle); + explicit ServiceBase(rcl_node_t * node_handle); + + RCLCPP_PUBLIC + explicit ServiceBase(rcl_service_t * service_handle); RCLCPP_PUBLIC virtual ~ServiceBase(); @@ -64,9 +67,9 @@ class ServiceBase protected: RCLCPP_DISABLE_COPY(ServiceBase) - std::shared_ptr node_handle_; + rcl_node_t * node_handle_; - rcl_service_t* service_handle_ = nullptr; + rcl_service_t * service_handle_ = nullptr; }; using any_service_callback::AnyServiceCallback; @@ -92,7 +95,7 @@ class Service : public ServiceBase const std::string & service_name, AnyServiceCallback any_callback, rcl_service_options_t & service_options) - : ServiceBase(node_handle), any_callback_(any_callback) + : ServiceBase(node_handle.get()), any_callback_(any_callback) { using rosidl_typesupport_cpp::get_service_type_support_handle; auto service_type_support_handle = get_service_type_support_handle(); @@ -110,19 +113,18 @@ class Service : public ServiceBase } Service( - std::shared_ptr node_handle, - rcl_service_t* service_handle, + rcl_node_t * node_handle, + rcl_service_t * service_handle, AnyServiceCallback any_callback) : ServiceBase(node_handle), defined_extern(true), any_callback_(any_callback) { // check if service handle was initialized // TODO(Karsten1987): Can this go directly in RCL? - if (service_handle->impl == NULL) - { + if (service_handle->impl == NULL) { throw std::runtime_error( - std::string("rcl_service_t in constructor argument \ - has to be initialized beforehand")); + std::string("rcl_service_t in constructor argument" + + "has to be initialized beforehand")); } } @@ -131,9 +133,8 @@ class Service : public ServiceBase virtual ~Service() { // check if you have ownership of the handle - if (!defined_extern) - { - if (rcl_service_fini(service_handle_, node_handle_.get()) != RCL_RET_OK) { + if (!defined_extern) { + if (rcl_service_fini(service_handle_, node_handle_) != RCL_RET_OK) { std::stringstream ss; ss << "Error in destruction of rcl service_handle_ handle: " << rcl_get_error_string_safe() << '\n'; diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index f3ee79d76b..58de5aac5f 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -27,7 +27,7 @@ using rclcpp::service::ServiceBase; -ServiceBase::ServiceBase(std::shared_ptr node_handle) +ServiceBase::ServiceBase(rcl_node_t * node_handle) : node_handle_(node_handle) {} diff --git a/rclcpp/test/test_externally_defined_services.cpp b/rclcpp/test/test_externally_defined_services.cpp index d1ea5ae808..d1f5d84ff0 100644 --- a/rclcpp/test/test_externally_defined_services.cpp +++ b/rclcpp/test/test_externally_defined_services.cpp @@ -15,6 +15,7 @@ #include #include +#include #include "rclcpp/node.hpp" #include "rclcpp/any_service_callback.hpp" @@ -36,8 +37,8 @@ class TestExternallyDefinedServices : public ::testing::Test }; void -callback(const std::shared_ptr /*req*/, - std::shared_ptr /*resp*/); +callback(const std::shared_ptr/*req*/, + std::shared_ptr/*resp*/); TEST_F(TestExternallyDefinedServices, extern_defined_uninitialized) { auto node_handle = rclcpp::node::Node::make_shared("base_node"); @@ -117,8 +118,7 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) { // Call destructor } - if (service_handle.impl == NULL) - { + if (service_handle.impl == NULL) { FAIL(); return; } From f28a80e1bde5938fd9d9a7e85b56cf8a2c744e6b Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Sat, 19 Nov 2016 19:56:23 -0800 Subject: [PATCH 03/63] correct typo --- rclcpp/include/rclcpp/service.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 261f24267b..13ad937e18 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -123,8 +123,8 @@ class Service : public ServiceBase // TODO(Karsten1987): Can this go directly in RCL? if (service_handle->impl == NULL) { throw std::runtime_error( - std::string("rcl_service_t in constructor argument" + - "has to be initialized beforehand")); + std::string("rcl_service_t in constructor argument ") + + "has to be initialized beforehand"); } } From f1fc89c13ec915dbb6518fee9a6765af91f0a42f Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Sat, 19 Nov 2016 20:28:34 -0800 Subject: [PATCH 04/63] add_service has to be public --- rclcpp/include/rclcpp/service.hpp | 1 + .../test/test_externally_defined_services.cpp | 17 ++++++++++++++++- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 13ad937e18..90a9a05a80 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -101,6 +101,7 @@ class Service : public ServiceBase auto service_type_support_handle = get_service_type_support_handle(); // rcl does the static memory allocation here + service_handle_ = new rcl_service_t; *service_handle_ = rcl_get_zero_initialized_service(); if (rcl_service_init( diff --git a/rclcpp/test/test_externally_defined_services.cpp b/rclcpp/test/test_externally_defined_services.cpp index d1f5d84ff0..7cbcf7d37f 100644 --- a/rclcpp/test/test_externally_defined_services.cpp +++ b/rclcpp/test/test_externally_defined_services.cpp @@ -38,7 +38,22 @@ class TestExternallyDefinedServices : public ::testing::Test void callback(const std::shared_ptr/*req*/, - std::shared_ptr/*resp*/); + std::shared_ptr/*resp*/) +{} + +TEST_F(TestExternallyDefinedServices, default_behavior) { + auto node_handle = rclcpp::node::Node::make_shared("base_node"); + + try { + auto srv = node_handle->create_service("test", + callback); + } catch (const std::exception& e) { + FAIL(); + return; + } + SUCCEED(); +} + TEST_F(TestExternallyDefinedServices, extern_defined_uninitialized) { auto node_handle = rclcpp::node::Node::make_shared("base_node"); From 72320c37e3739bae5370ad67d8041ee82a9314c8 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 22 Nov 2016 08:38:17 -0800 Subject: [PATCH 05/63] uncrustify --- rclcpp/test/test_externally_defined_services.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp/test/test_externally_defined_services.cpp b/rclcpp/test/test_externally_defined_services.cpp index 7cbcf7d37f..1308972f05 100644 --- a/rclcpp/test/test_externally_defined_services.cpp +++ b/rclcpp/test/test_externally_defined_services.cpp @@ -45,9 +45,9 @@ TEST_F(TestExternallyDefinedServices, default_behavior) { auto node_handle = rclcpp::node::Node::make_shared("base_node"); try { - auto srv = node_handle->create_service("test", - callback); - } catch (const std::exception& e) { + auto srv = node_handle->create_service("test", + callback); + } catch (const std::exception & e) { FAIL(); return; } From 6a5abed29c57766577c605d737809b690f085acc Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 26 Oct 2016 17:12:03 -0700 Subject: [PATCH 06/63] initial state machine implementation (fix) correctly initialize default state machine uncrustify (dev) first high level api interface src/default_state_machine.c (fix) correctly initialize arrays in statemachine (dev) deactivate/activate publisher demo (dev) initial state machine implementation in rcl --- rclcpp/include/rclcpp/time.h | 40 ++++++ rclcpp_lifecycle/CMakeLists.txt | 56 ++++++++ .../rcl_lifecycle/default_state_machine.h | 35 +++++ .../include/rcl_lifecycle/lifecycle_state.h | 90 ++++++++++++ .../include/rcl_lifecycle/transition_map.h | 91 ++++++++++++ .../rclcpp_lifecycle/lifecycle_executor.hpp | 46 ++++++ .../rclcpp_lifecycle/lifecycle_node.hpp | 130 +++++++++++++++++ .../rclcpp_lifecycle/lifecycle_publisher.hpp | 127 +++++++++++++++++ .../type_traits/is_manageable_node.hpp | 54 ++++++++ rclcpp_lifecycle/package.xml | 27 ++++ rclcpp_lifecycle/src/default_state_machine.c | 90 ++++++++++++ rclcpp_lifecycle/src/lifecycle_state.c | 42 ++++++ rclcpp_lifecycle/src/lifecycle_talker.cpp | 81 +++++++++++ rclcpp_lifecycle/src/transition_map.c | 131 ++++++++++++++++++ 14 files changed, 1040 insertions(+) create mode 100644 rclcpp/include/rclcpp/time.h create mode 100644 rclcpp_lifecycle/CMakeLists.txt create mode 100644 rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h create mode 100644 rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h create mode 100644 rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h create mode 100644 rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_executor.hpp create mode 100644 rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp create mode 100644 rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp create mode 100644 rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp create mode 100644 rclcpp_lifecycle/package.xml create mode 100644 rclcpp_lifecycle/src/default_state_machine.c create mode 100644 rclcpp_lifecycle/src/lifecycle_state.c create mode 100644 rclcpp_lifecycle/src/lifecycle_talker.cpp create mode 100644 rclcpp_lifecycle/src/transition_map.c diff --git a/rclcpp/include/rclcpp/time.h b/rclcpp/include/rclcpp/time.h new file mode 100644 index 0000000000..814bd21b3f --- /dev/null +++ b/rclcpp/include/rclcpp/time.h @@ -0,0 +1,40 @@ +// Copyright 2014 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 RCLCPP__TIME_H_ +#define RCLCPP__TIME_H_ + +#include +#include + +namespace rclcpp +{ + +struct Time +{ + static builtin_interfaces::msg::Time now() + { + builtin_interfaces::msg::Time time; + std::chrono::nanoseconds now = std::chrono::high_resolution_clock::now().time_since_epoch(); + if (now > std::chrono::nanoseconds(0)) { + time.sec = static_cast(now.count() / 1000000000); + time.nanosec = now.count() % 1000000000; + } + return time; + } +}; + +} // namespace rclcpp + +#endif // RCLCPP__TIME_H_ diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt new file mode 100644 index 0000000000..2d1a0861ba --- /dev/null +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.5) + +project(rclcpp_lifecycle) + +if(NOT WIN32) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11 -Wall -Wextra") +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rmw REQUIRED) +find_package(rmw_implementation_cmake REQUIRED) +find_package(std_msgs REQUIRED) + +include_directories( + include + # ${rclcpp_INCLUDE_DIRS} + # ${rmw_INCLUDE_DIRS} +) + +set_source_files_properties( + src/lifecycle_state.c + src/default_state_machine.c + src/transition_map.c + PROPERTIES language "C" +) +add_library( + state_machine + SHARED + src/lifecycle_state.c + src/default_state_machine.c + src/transition_map.c +) +install(TARGETS state_machine + DESTINATION lib) + +macro(targets) + if(NOT target_suffix STREQUAL "") + get_rclcpp_information("${rmw_implementation}" "rclcpp${target_suffix}") + endif() + + add_executable(lifecycle_talker${target_suffix} + src/lifecycle_talker.cpp) + target_link_libraries(lifecycle_talker${target_suffix} state_machine) + ament_target_dependencies(lifecycle_talker${target_suffix} + "rclcpp${target_suffix}" + "std_msgs" + ) + install(TARGETS lifecycle_talker${target_suffix} + DESTINATION bin) +endmacro() + +call_for_each_rmw_implementation(targets GENERATE_DEFAULT) + +ament_package() diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h b/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h new file mode 100644 index 0000000000..495ad0cf2d --- /dev/null +++ b/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h @@ -0,0 +1,35 @@ +// 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 RCL__DEFAULT_STATE_MACHINE_H_ +#define RCL__DEFAULT_STATE_MACHINE_H_ + +#if __cplusplus +extern "C" +{ +#endif + +// primary states based on +// design.ros2.org/articles/node_lifecycle.html +static const rcl_state_t rcl_state_unconfigured = {.state = 0, .label = "unconfigured"}; +static const rcl_state_t rcl_state_inactive = {.state = 1, .label = "inactive"}; +static const rcl_state_t rcl_state_active = {.state = 2, .label = "active"}; +static const rcl_state_t rcl_state_finalized = {.state = 3, .label = "finalized"}; +static const rcl_state_t rcl_state_error = {.state = 4, .label = "error"}; + +#if __cplusplus +} +#endif // extern "C" + +#endif // RCL__DEFAULT_STATE_MACHINE_H_ diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h b/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h new file mode 100644 index 0000000000..bccc6b7827 --- /dev/null +++ b/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h @@ -0,0 +1,90 @@ +// 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_STATE_H_ +#define RCL__LIFECYCLE_STATE_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include +#include + +/** + * @brief simple definition of a state + * @param state: integer giving the state + * @param label: label for easy indexing + */ +typedef struct _rcl_state_t +{ + unsigned int state; + const char* label; +} rcl_state_t; + +/** + * @brief transition definition + * @param start: rcl_state_t as a start state + * @param goal: rcl_state_t as a goal state + * TODO: Maybe specify callback pointer here + * and call on_* functions directly + */ +typedef struct _rcl_state_transition_t +{ + rcl_state_t start; + // function callback + rcl_state_t goal; +} rcl_state_transition_t; + +/** + * @brief: statemachine object holding + * a variable state object as current state + * of the complete machine. + * @param transition_map: a map object of all + * possible transitions registered with this + * state machine. + */ +typedef struct _rcl_state_machine_t +{ + // current state of the lifecycle + rcl_state_t current_state; + + rcl_transition_map_t transition_map; +} rcl_state_machine_t; + + +// function definitions +/* + * @brief traverses the transition map of the given + * state machine to find if there is a transition from the + * current state to the specified goal state + */ +bool +rcl_is_valid_transition(rcl_state_machine_t* state_machine, const rcl_state_t* goal_state); + +rcl_state_t +rcl_create_state(unsigned int state, char* label); + +rcl_state_transition_t +rcl_create_transition(rcl_state_t start, rcl_state_t goal); + +rcl_state_machine_t +rcl_get_default_state_machine(); + +#if __cplusplus +} +#endif // extern "C" + +#endif // RCL__LIFECYCLE_STATE_H diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h b/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h new file mode 100644 index 0000000000..32ba392405 --- /dev/null +++ b/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h @@ -0,0 +1,91 @@ +// 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__TRANSITION_MAP_H_ +#define RCL__TRANSITION_MAP_H_ + +#include + +#if __cplusplus +extern "C" +{ +#endif + +typedef struct _rcl_state_t rcl_state_t; + +typedef struct _rcl_state_transition_t rcl_state_transition_t; + +/** + * @brief The actual data array + * within the map. One array belongs + * to one label within the map. + */ +typedef struct _data_array +{ + rcl_state_transition_t* transitions; + unsigned int size; +} rcl_transition_array_t; + +/** + * @brief helper index for keeping + * track of the map content + */ +typedef struct _index +{ + unsigned int index; + const char* label; +} rcl_transition_map_index_t; + +/** + * @brief stores an array of transitions + * index by a start state + */ +typedef struct _map +{ + //rcl_state_t* state_index; + rcl_transition_map_index_t* state_index; + rcl_transition_array_t* transition_arrays; + unsigned int size; +} rcl_transition_map_t; + +/** + * @brief appends a transition in a map + * the classification is based on the start state + * within the given transition + */ +void rcl_append_transition(rcl_transition_map_t* m, rcl_state_transition_t transition); + +/** + * @brief gets all transitions based on a label + * label is supposed to come from a rcl_state_t object + */ +rcl_transition_array_t* rcl_get_map_by_label(rcl_transition_map_t* m, const char* label); +/** + * @brief gets all transitions based on a state + * state is supposed to come from a rcl_state_t object + */ +rcl_transition_array_t* rcl_get_map_by_state(rcl_transition_map_t* m, const unsigned int state); + +/** + * @brief helper functions to print + */ +void rcl_print_transition_array(const rcl_transition_array_t* da); +void rcl_print_transition_map(const rcl_transition_map_t* m); + +#if __cplusplus +} +#endif + +#endif // RCL__TRANSITION_MAP_H_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_executor.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_executor.hpp new file mode 100644 index 0000000000..0a920aba63 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_executor.hpp @@ -0,0 +1,46 @@ +// Copyright 2014 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 RCLCPP__LIFECYCLE_EXECUTOR_HPP_ +#define RCLCPP__LIFECYCLE_EXECUTOR_HPP_ + +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/type_traits/is_manageable_node.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace executors +{ + +class LifecycleExecutor : public single_threaded_executor::SingleThreadedExecutor +{ +// using Node = rclcpp::node::Node; + +public: + + explicit LifecycleExecutor(const executor::ExecutorArgs & args = executor::create_default_executor_arguments()) + : SingleThreadedExecutor(args) + {} + + //RCLCPP_PUBLIC + template::value>::type> + void + add_node(std::shared_ptr node_ptr, bool notify = true); +}; + +} // namespace executor +} // namespace rclcpp + +#endif diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp new file mode 100644 index 0000000000..d1354b29d5 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -0,0 +1,130 @@ +// 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 RCLCPP__LIFECYCLE_NODE_HPP_ +#define RCLCPP__LIFECYCLE_NODE_HPP_ + +#include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" + +#include "rcl_lifecycle/lifecycle_state.h" + +namespace rclcpp +{ +namespace node +{ + +class LifecycleNode : public rclcpp::node::Node +{ +public: + + RCLCPP_PUBLIC + explicit LifecycleNode(const std::string & node_name, bool use_intra_process_comms = false) : + Node(node_name, use_intra_process_comms), + state_machine_(rcl_get_default_state_machine()) + { + printf("Hello Lifecycle construcotr\n"); + } + + ~LifecycleNode(){} + + template> + std::shared_ptr> + create_publisher( + const std::string & topic_name, + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, + std::shared_ptr allocator = nullptr) + { + auto pub = rclcpp::node::Node::create_publisher>( + topic_name, qos_profile, allocator); + + weak_pub_ = pub; + return pub; + } + + void + print_state_machine() + { + printf("Got primary states:\n"); + for (auto i=0; i active + if (!rcl_is_valid_transition(&state_machine_.current_state, &state_machine_.transitions[0])) + { + // TODO: go to error state here + fprintf(stderr, "Unable to change from current state %s from transition start %s\n", state_machine_.current_state.label, state_machine_.transitions[0].start.state); + return false; + } + if (weak_pub_.expired()) + { + // TODO: go to error state here + fprintf(stderr, "I have no publisher handle\n"); + return false; + } + + // TODO: does a return value make sense here? + weak_pub_.lock()->on_activate(); + + state_machine_.current_state.state = 1; // activated + state_machine_.current_state.label = "active"; // activated + return true; + } + + bool + deactivate() + { + // second transition is from active to deactive + if(!rcl_is_valid_transition(&state_machine_.current_state, &state_machine_.transitions[1])) + { + fprintf(stderr, "Unable to change from current state %s from transition start %s\n", state_machine_.current_state.label, state_machine_.transitions[1].start.state); + fprintf(stderr, "deactivate is not a valid transaction in current state %s\n", state_machine_.current_state.label); + // TODO: go to error state here + return false; + } + if (weak_pub_.expired()) + { + fprintf(stderr, "I have no publisher handle\n"); + // TODO: go to error state here + return false; + } + + // TODO: does a return value make sense here? + weak_pub_.lock()->on_deactivate(); + + state_machine_.current_state.state = 0; // activated + state_machine_.current_state.label = "deactive"; + return true; + } + +private: + + std::weak_ptr weak_pub_; + + rcl_state_machine_t state_machine_; +}; + +} // namespace node +} // namespace rclcpp + +#endif diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp new file mode 100644 index 0000000000..1171ad719d --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -0,0 +1,127 @@ +// Copyright 2014 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 RCLCPP__LIFECYCLE_PUBLISHER_HPP_ +#define RCLCPP__LIFECYCLE_PUBLISHER_HPP_ + +#include "rclcpp/publisher.hpp" + +namespace rclcpp +{ +namespace publisher +{ + +namespace lifecycle_interface +{ +class Publisher +{ +public: + virtual void on_activate() = 0; + virtual void on_deactivate() = 0; +}; +} // namespace lifecycle_interface + +template> +class LifecyclePublisher : public rclcpp::publisher::Publisher, + public lifecycle_interface::Publisher +{ +public: + using MessageAllocTraits = allocator::AllocRebind; + using MessageAlloc = typename MessageAllocTraits::allocator_type; + using MessageDeleter = allocator::Deleter; + using MessageUniquePtr = std::unique_ptr; + + LifecyclePublisher( + std::shared_ptr node_handle, + std::string topic, + const rcl_publisher_options_t & publisher_options, + std::shared_ptr allocator) + : ::rclcpp::publisher::Publisher(node_handle, topic, publisher_options, allocator), + enabled_(false) + { } + + ~LifecyclePublisher() {}; + + void + publish(std::unique_ptr & msg) + { + if (!enabled_) + { + printf("I push every message to /dev/null\n"); + } + rclcpp::publisher::Publisher::publish(msg); + } + + void + publish(const std::shared_ptr & msg) + { + if (!enabled_) + { + printf("I am every message to /dev/null\n"); + } + rclcpp::publisher::Publisher::publish(msg); + } + + virtual void + publish(std::shared_ptr msg) + { + if (!enabled_) + { + printf("I am every message to /dev/null\n"); + } + rclcpp::publisher::Publisher::publish(msg); + } + + virtual void + publish(const MessageT & msg) + { + if (!enabled_) + { + printf("I am every message to /dev/null\n"); + } + rclcpp::publisher::Publisher::publish(msg); + } + + virtual void + publish(std::shared_ptr& msg) + { + if (!enabled_) + { + printf("I am every message to /dev/null\n"); + } + rclcpp::publisher::Publisher::publish(msg); + } + + virtual void + on_activate() + { + printf("Lifecycle publisher is enabled\n"); + enabled_ = true; + } + + virtual void + on_deactivate() + { + printf("Lifecycle publisher is deactivated\n"); + enabled_ = false; + } + +private: + bool enabled_ = false; +}; + +} // namespace publisher +} // namespace rclcpp + +#endif // endif RCLCPP__LIFECYCLE_PUBLISHER_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp new file mode 100644 index 0000000000..e959f64ccd --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp @@ -0,0 +1,54 @@ +// Copyright 2014 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 RCLCPP__IS_MANAGEABLE_HPP_ +#define RCLCPP__IS_MANAGEABLE_HPP_ + +#include +#include + +template +struct has_on_activate +{ + static constexpr bool value = false; +}; + +template +struct has_on_activate().on_activate())>::value>::type> +{ + static constexpr bool value = true; +}; + +template +struct has_on_deactivate +{ + static constexpr bool value = false; +}; + +template +struct has_on_deactivate().on_deactivate())>::value>::type> +{ + static constexpr bool value = true; +}; + +template +struct is_manageable_node : std::false_type +{}; + +template +struct is_manageable_node::value + && has_on_deactivate::value >::type> : std::true_type +{}; + +#endif diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml new file mode 100644 index 0000000000..3aee7a9f66 --- /dev/null +++ b/rclcpp_lifecycle/package.xml @@ -0,0 +1,27 @@ + + + + rclcpp_lifecycle + 0.0.0 + Package containing a prototype for lifecycle implementation + Karsten Knese + Apache License 2.0 + + ament_cmake + + rosidl_default_generators + + rclcpp + rmw_implementation + rmw_implementation_cmake + std_msgs + + rclcpp + rmw_implementation + rosidl_default_runtime + std_msgs + + + ament_cmake + + diff --git a/rclcpp_lifecycle/src/default_state_machine.c b/rclcpp_lifecycle/src/default_state_machine.c new file mode 100644 index 0000000000..201319cdef --- /dev/null +++ b/rclcpp_lifecycle/src/default_state_machine.c @@ -0,0 +1,90 @@ +// 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 RCL__STATE_MACHINE_H_ +#define RCL__STATE_MACHINE_H_ + +#include +#include +#include + +#include "rcl_lifecycle/lifecycle_state.h" +#include "rcl_lifecycle/default_state_machine.h" + +#if __cplusplus +extern "C" +{ +#endif + +rcl_state_t +rcl_create_state(unsigned int state, char* label) +{ + rcl_state_t ret_state = {.state = state, .label = label}; + return ret_state; +} + +rcl_state_transition_t +rcl_create_state_transition(rcl_state_t start, rcl_state_t goal) +{ + rcl_state_transition_t ret_transition = {.start = start, .goal = goal}; + return ret_transition; +} + +// default implementation as despicted on +// design.ros2.org +rcl_state_machine_t +rcl_get_default_state_machine() +{ + rcl_state_machine_t state_machine; + state_machine.transition_map.state_index = NULL; + state_machine.transition_map.transition_arrays = NULL; + state_machine.transition_map.size = 0; + + // set default state as unconfigured + state_machine.current_state = rcl_state_unconfigured; + + // from unconfigured to inactive + rcl_state_transition_t unconfigured_to_inactive = {.start = rcl_state_unconfigured, .goal = rcl_state_inactive}; + // from unconfigured to error state + rcl_state_transition_t unconfigured_to_error = {.start = rcl_state_unconfigured, .goal = rcl_state_error}; + + // from inactive to active + rcl_state_transition_t inactive_to_active = {.start = rcl_state_inactive, .goal = rcl_state_active}; + // from inactive to error state + rcl_state_transition_t inactive_to_error = {.start = rcl_state_inactive, .goal = rcl_state_error}; + + // from active to finalized + rcl_state_transition_t active_to_finalized = {.start = rcl_state_active, .goal = rcl_state_finalized}; + // from active to inactive + rcl_state_transition_t active_to_inactive = {.start = rcl_state_active, .goal = rcl_state_inactive}; + // from active to error + rcl_state_transition_t active_to_error = {.start = rcl_state_active, .goal = rcl_state_error}; + + // add transitions to map + rcl_append_transition(&state_machine.transition_map, unconfigured_to_inactive); + rcl_append_transition(&state_machine.transition_map, unconfigured_to_error); + rcl_append_transition(&state_machine.transition_map, inactive_to_active); + rcl_append_transition(&state_machine.transition_map, inactive_to_error); + rcl_append_transition(&state_machine.transition_map, active_to_finalized); + rcl_append_transition(&state_machine.transition_map, active_to_inactive); + rcl_append_transition(&state_machine.transition_map, active_to_error); + + return state_machine; +} + +#if __cplusplus +} +#endif // extern "C" + +#endif // RCL__STATE_MACHINE_H_ diff --git a/rclcpp_lifecycle/src/lifecycle_state.c b/rclcpp_lifecycle/src/lifecycle_state.c new file mode 100644 index 0000000000..2fe6bd3d31 --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_state.c @@ -0,0 +1,42 @@ +// 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. + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl_lifecycle/lifecycle_state.h" + +typedef rcl_state_transition_t rcl_transition; +typedef rcl_state_t rcl_state; + +bool +rcl_is_valid_transition(rcl_state_machine_t* state_machine, const rcl_state_t* goal_state) +{ + rcl_transition_array_t* all_transitions = rcl_get_map_by_label(&state_machine->transition_map, state_machine->current_state.label); + + for (unsigned int i=0; isize; ++i) + { + if (all_transitions->transitions[i].goal.state == goal_state->state) + { + return true; + } + } + return false; +} + +#if __cplusplus +} +#endif // extern "C" diff --git a/rclcpp_lifecycle/src/lifecycle_talker.cpp b/rclcpp_lifecycle/src/lifecycle_talker.cpp new file mode 100644 index 0000000000..9c126fe29e --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_talker.cpp @@ -0,0 +1,81 @@ +// Copyright 2014 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/publisher.hpp" + +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" + +#include "std_msgs/msg/string.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + std::shared_ptr lc_node = std::make_shared("lc_talker"); + + rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; + custom_qos_profile.depth = 7; + + std::shared_ptr> chatter_pub = + lc_node->create_publisher("chatter", custom_qos_profile); + + lc_node->print_state_machine(); + + rclcpp::WallRate loop_rate(2); + + auto msg = std::make_shared(); + auto i = 1; + + while (rclcpp::ok() && i <= 10) { + msg->data = "Hello World: " + std::to_string(i++); + //std::cout << "Publishing: '" << msg->data << "'" << std::endl; + chatter_pub->publish(msg); + rclcpp::spin_some(lc_node); + loop_rate.sleep(); + } + + printf("Calling activate\n"); + if (!lc_node->activate()) + { + return -1; + } + + while (rclcpp::ok() && i <= 20) { + msg->data = "Hello World: " + std::to_string(i++); + //std::cout << "Publishing: '" << msg->data << "'" << std::endl; + chatter_pub->publish(msg); + rclcpp::spin_some(lc_node); + loop_rate.sleep(); + } + + printf("Calling deactivate\n"); + if (!lc_node->deactivate()) + { + return -1; + } + + while (rclcpp::ok() && i <= 30) { + msg->data = "Hello World: " + std::to_string(i++); + //std::cout << "Publishing: '" << msg->data << "'" << std::endl; + chatter_pub->publish(msg); + rclcpp::spin_some(lc_node); + loop_rate.sleep(); + } + return 0; +} diff --git a/rclcpp_lifecycle/src/transition_map.c b/rclcpp_lifecycle/src/transition_map.c new file mode 100644 index 0000000000..a9ff9ad5fc --- /dev/null +++ b/rclcpp_lifecycle/src/transition_map.c @@ -0,0 +1,131 @@ +// 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 +#include + +void append_to_transition_array(rcl_transition_array_t* transition_array, rcl_state_transition_t data) +{ + // There is no transition array !? + if (!transition_array) + { + return; + } + + // we add a new transition, so increase the size + ++transition_array->size; + + // TODO: Not sure if this is really necessary to differentiate + // between malloc and realloc + if (transition_array->size == 1) + { + transition_array->transitions = malloc(sizeof(rcl_state_transition_t)); + } + else + { + transition_array->transitions + = realloc(transition_array->transitions, transition_array->size*sizeof(rcl_state_transition_t)); + } + + // finally set the new transition to the end of the array + transition_array->transitions[transition_array->size-1] = data; +} + +void rcl_append_transition(rcl_transition_map_t* m, rcl_state_transition_t data) +{ + // the idea here is to add this transition based on the + // start label and classify them. + + // check wether the label exits already + for (unsigned int i=0; isize; ++i) + { + if (strcmp(m->state_index[i].label, data.start.label) == 0) + { + // label exists already + //printf("Found existing label %s. Will append new data\n", m->state_index[i].label); + // we will append data into rcl_transition_array_t with given data.start.label + append_to_transition_array(&(m->transition_arrays[i]), data); + return; + } + } + + //printf("Label %s does not exist yet. Will add a new one\n", data.start.label); + //printf("There are %u existing labels\n", m->size); + // when we reach this point, the label does not exist yet + // reallocate the index array and extend it for the new label + rcl_transition_map_index_t idx = {.index = m->size, .label = data.start.label}; + ++m->size; + m->state_index = realloc(m->state_index, m->size*sizeof(rcl_transition_map_index_t)); + m->state_index[idx.index] = idx; + + // reallocate the actual data array and extend it for the data + m->transition_arrays = realloc(m->transition_arrays, m->size*sizeof(rcl_transition_array_t)); + m->transition_arrays[idx.index].size = 0; + append_to_transition_array(&m->transition_arrays[idx.index], data); +} + +rcl_transition_array_t* rcl_get_map_by_label(rcl_transition_map_t* m, const char* label) +{ + for (unsigned int i=0; isize; ++i) + { + if (strcmp(m->state_index[i].label, label) == 0) + { + return &m->transition_arrays[i]; + } + } + return NULL; +} + +rcl_transition_array_t* rcl_get_map_by_state(rcl_transition_map_t* m, const unsigned int state) +{ + for (unsigned int i=0; isize; ++i) + { + if (m->state_index[i].index == state ) + { + return &m->transition_arrays[i]; + } + } + return NULL; +} + +void rcl_print_transition_array(const rcl_transition_array_t* da) +{ + for (unsigned int i=0; isize; ++i) + { + printf("%s\t", da->transitions[i].goal.label); + } + printf("\n"); +} + +void rcl_print_transition_map(const rcl_transition_map_t* m) +{ + printf("rcl_transition_map_t size %u\n", m->size); + for (unsigned int i=0; isize; ++i) + { + printf("Start state %s ::: ", m->state_index[i].label); + rcl_print_transition_array(&(m->transition_arrays[i])); + } +} +#if __cplusplus +} +#endif From 8456209ce9d28a8e80d9c94cfbbf68f57a9b9fb0 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 1 Nov 2016 11:28:00 -0700 Subject: [PATCH 07/63] (dev) demo application for a managed lifecycle node --- rclcpp/include/rclcpp/time.h | 40 ------ .../include/rcl_lifecycle/transition_map.h | 15 +- .../rclcpp_lifecycle/lifecycle_node.hpp | 133 ++++++++++++++---- .../rclcpp_lifecycle/lifecycle_publisher.hpp | 60 ++++++-- rclcpp_lifecycle/src/lifecycle_talker.cpp | 11 +- 5 files changed, 176 insertions(+), 83 deletions(-) delete mode 100644 rclcpp/include/rclcpp/time.h diff --git a/rclcpp/include/rclcpp/time.h b/rclcpp/include/rclcpp/time.h deleted file mode 100644 index 814bd21b3f..0000000000 --- a/rclcpp/include/rclcpp/time.h +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright 2014 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 RCLCPP__TIME_H_ -#define RCLCPP__TIME_H_ - -#include -#include - -namespace rclcpp -{ - -struct Time -{ - static builtin_interfaces::msg::Time now() - { - builtin_interfaces::msg::Time time; - std::chrono::nanoseconds now = std::chrono::high_resolution_clock::now().time_since_epoch(); - if (now > std::chrono::nanoseconds(0)) { - time.sec = static_cast(now.count() / 1000000000); - time.nanosec = now.count() % 1000000000; - } - return time; - } -}; - -} // namespace rclcpp - -#endif // RCLCPP__TIME_H_ diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h b/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h index 32ba392405..1fa683e746 100644 --- a/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h +++ b/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h @@ -65,24 +65,29 @@ typedef struct _map * the classification is based on the start state * within the given transition */ -void rcl_append_transition(rcl_transition_map_t* m, rcl_state_transition_t transition); +void +rcl_append_transition(rcl_transition_map_t* m, rcl_state_transition_t transition); /** * @brief gets all transitions based on a label * label is supposed to come from a rcl_state_t object */ -rcl_transition_array_t* rcl_get_map_by_label(rcl_transition_map_t* m, const char* label); +rcl_transition_array_t* +rcl_get_map_by_label(rcl_transition_map_t* m, const char* label); /** * @brief gets all transitions based on a state * state is supposed to come from a rcl_state_t object */ -rcl_transition_array_t* rcl_get_map_by_state(rcl_transition_map_t* m, const unsigned int state); +rcl_transition_array_t* +rcl_get_map_by_state(rcl_transition_map_t* m, const unsigned int state); /** * @brief helper functions to print */ -void rcl_print_transition_array(const rcl_transition_array_t* da); -void rcl_print_transition_map(const rcl_transition_map_t* m); +void +rcl_print_transition_array(const rcl_transition_array_t* da); +void +rcl_print_transition_map(const rcl_transition_map_t* m); #if __cplusplus } diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index d1354b29d5..b72bb38cb5 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2015 Open Source Robotics Foundation, Inc. +// 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. @@ -19,26 +19,63 @@ #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rcl_lifecycle/lifecycle_state.h" +#include "rcl_lifecycle/default_state_machine.h" +#include "rcl_lifecycle/transition_map.h" namespace rclcpp { namespace node { -class LifecycleNode : public rclcpp::node::Node +namespace lifecycle_interface +{ + +/** + * @brief Interface class for a managed node. + * Pure virtual functions as defined in + * http://design.ros2.org/articles/node_lifecycle.html + */ +class NodeInterface +{ + virtual bool on_configure() = 0; + virtual bool on_clean_up() = 0; + virtual bool on_shutdown() = 0; + virtual bool on_activate() = 0; + virtual bool on_deactivate() = 0; + virtual bool on_error() = 0; +}; +} // namespace lifecycle_interface + +/** + * @brief LifecycleNode as child class of rclcpp Node + * has lifecycle nodeinterface for configuring this node. + */ +class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::NodeInterface { public: RCLCPP_PUBLIC explicit LifecycleNode(const std::string & node_name, bool use_intra_process_comms = false) : - Node(node_name, use_intra_process_comms), - state_machine_(rcl_get_default_state_machine()) + Node(node_name, use_intra_process_comms) { - printf("Hello Lifecycle construcotr\n"); + setup_state_machine(); } ~LifecycleNode(){} + /** + * @brief get the default state machine + * as defined on design.ros.org + */ + virtual void + setup_state_machine() + { + state_machine_ = rcl_get_default_state_machine(); + } + + /** + * @brief same API for creating publisher as regular Node + */ template> std::shared_ptr> create_publisher( @@ -46,10 +83,12 @@ class LifecycleNode : public rclcpp::node::Node const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, std::shared_ptr allocator = nullptr) { + // create regular publisher in rclcpp::Node auto pub = rclcpp::node::Node::create_publisher>( topic_name, qos_profile, allocator); + // keep weak handle for this publisher to enable/disable afterwards weak_pub_ = pub; return pub; } @@ -57,48 +96,77 @@ class LifecycleNode : public rclcpp::node::Node void print_state_machine() { - printf("Got primary states:\n"); - for (auto i=0; i active - if (!rcl_is_valid_transition(&state_machine_.current_state, &state_machine_.transitions[0])) + return true; + } + + virtual bool + on_shutdown() + { + return true; + } + + virtual bool + on_activate() + { + if (!rcl_is_valid_transition(&state_machine_, &rcl_state_active)) { - // TODO: go to error state here - fprintf(stderr, "Unable to change from current state %s from transition start %s\n", state_machine_.current_state.label, state_machine_.transitions[0].start.state); + fprintf(stderr, "Unable to change from current state %s from transition start %s\n", state_machine_.current_state.label, rcl_state_active.label); + state_machine_.current_state = rcl_state_error; return false; } if (weak_pub_.expired()) { - // TODO: go to error state here + // Someone externally destroyed the publisher handle fprintf(stderr, "I have no publisher handle\n"); + state_machine_.current_state = rcl_state_error; return false; } + // Placeholder print for all configuring work to be done + // with each pub/sub/srv/client + printf("I am doing a lot of activation work\n"); // TODO: does a return value make sense here? weak_pub_.lock()->on_activate(); - state_machine_.current_state.state = 1; // activated - state_machine_.current_state.label = "active"; // activated + state_machine_.current_state = rcl_state_active; return true; } - bool - deactivate() + virtual bool + on_deactivate() { // second transition is from active to deactive - if(!rcl_is_valid_transition(&state_machine_.current_state, &state_machine_.transitions[1])) + if(!rcl_is_valid_transition(&state_machine_, &rcl_state_inactive)) { - fprintf(stderr, "Unable to change from current state %s from transition start %s\n", state_machine_.current_state.label, state_machine_.transitions[1].start.state); - fprintf(stderr, "deactivate is not a valid transaction in current state %s\n", state_machine_.current_state.label); + fprintf(stderr, "Unable to change from current state %s from transition start %s\n", state_machine_.current_state.label, rcl_state_inactive.label); + fprintf(stderr, "deactivate is not a valid transition in current state %s\n", state_machine_.current_state.label); // TODO: go to error state here return false; } @@ -109,17 +177,28 @@ class LifecycleNode : public rclcpp::node::Node return false; } + // Placeholder print for all configuring work to be done + // with each pub/sub/srv/client + printf("I am doing a lot of activation work\n"); // TODO: does a return value make sense here? weak_pub_.lock()->on_deactivate(); - state_machine_.current_state.state = 0; // activated - state_machine_.current_state.label = "deactive"; + state_machine_.current_state = rcl_state_inactive; + return true; + } + + virtual bool + on_error() + { return true; } private: - std::weak_ptr weak_pub_; + // weak handle for the managing publisher + // TODO: Has to be a vector of weak publishers. Does on_deactivate deactivate every publisher?! + // Placeholder for all pub/sub/srv/clients + std::weak_ptr weak_pub_; rcl_state_machine_t state_machine_; }; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index 1171ad719d..8e4785fa23 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// 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. @@ -24,7 +24,15 @@ namespace publisher namespace lifecycle_interface { -class Publisher +/** + * @brief base class with only + * pure virtual functions. A managed + * node can then deactivate or activate + * the publishing. + * It is more a convenient interface class + * than a necessary base class. + */ +class PublisherInterface { public: virtual void on_activate() = 0; @@ -32,9 +40,14 @@ class Publisher }; } // namespace lifecycle_interface +/** + * @brief child class of rclcpp Publisher class. + * Overrides all publisher functions to check for + * enabled/disabled state. + */ template> class LifecyclePublisher : public rclcpp::publisher::Publisher, - public lifecycle_interface::Publisher + public lifecycle_interface::PublisherInterface { public: using MessageAllocTraits = allocator::AllocRebind; @@ -46,59 +59,90 @@ class LifecyclePublisher : public rclcpp::publisher::Publisher, std::shared_ptr node_handle, std::string topic, const rcl_publisher_options_t & publisher_options, - std::shared_ptr allocator) - : ::rclcpp::publisher::Publisher(node_handle, topic, publisher_options, allocator), + std::shared_ptr allocator): + ::rclcpp::publisher::Publisher(node_handle, topic, publisher_options, allocator), enabled_(false) { } ~LifecyclePublisher() {}; - void + /** + * @brief briefly checks whether this publisher + * was enabled or disabled and forwards the message + * to the actual rclcp Publisher base class + */ + virtual void publish(std::unique_ptr & msg) { if (!enabled_) { printf("I push every message to /dev/null\n"); + return; } rclcpp::publisher::Publisher::publish(msg); } - void + /** + * @brief briefly checks whether this publisher + * was enabled or disabled and forwards the message + * to the actual rclcp Publisher base class + */ + virtual void publish(const std::shared_ptr & msg) { if (!enabled_) { - printf("I am every message to /dev/null\n"); + printf("I publish message %s to /dev/null\n", msg->data.c_str()); + return; } + printf("I publish message %s to DDS\n", msg->data.c_str()); rclcpp::publisher::Publisher::publish(msg); } + /** + * @brief briefly checks whether this publisher + * was enabled or disabled and forwards the message + * to the actual rclcp Publisher base class + */ virtual void publish(std::shared_ptr msg) { if (!enabled_) { printf("I am every message to /dev/null\n"); + return; } rclcpp::publisher::Publisher::publish(msg); } + /** + * @brief briefly checks whether this publisher + * was enabled or disabled and forwards the message + * to the actual rclcp Publisher base class + */ virtual void publish(const MessageT & msg) { if (!enabled_) { printf("I am every message to /dev/null\n"); + return; } rclcpp::publisher::Publisher::publish(msg); } + /** + * @brief briefly checks whether this publisher + * was enabled or disabled and forwards the message + * to the actual rclcp Publisher base class + */ virtual void publish(std::shared_ptr& msg) { if (!enabled_) { printf("I am every message to /dev/null\n"); + return; } rclcpp::publisher::Publisher::publish(msg); } diff --git a/rclcpp_lifecycle/src/lifecycle_talker.cpp b/rclcpp_lifecycle/src/lifecycle_talker.cpp index 9c126fe29e..dbea9952b8 100644 --- a/rclcpp_lifecycle/src/lifecycle_talker.cpp +++ b/rclcpp_lifecycle/src/lifecycle_talker.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// 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. @@ -37,6 +37,11 @@ int main(int argc, char * argv[]) lc_node->print_state_machine(); + if (!lc_node->on_configure()) + { + printf("Could not configure node. Going to error state.\n"); + } + rclcpp::WallRate loop_rate(2); auto msg = std::make_shared(); @@ -51,7 +56,7 @@ int main(int argc, char * argv[]) } printf("Calling activate\n"); - if (!lc_node->activate()) + if (!lc_node->on_activate()) { return -1; } @@ -65,7 +70,7 @@ int main(int argc, char * argv[]) } printf("Calling deactivate\n"); - if (!lc_node->deactivate()) + if (!lc_node->on_deactivate()) { return -1; } From fa573f14fb454cf572968fe794151d28e3910881 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 1 Nov 2016 17:41:28 -0700 Subject: [PATCH 08/63] add visibility control --- .../rcl_lifecycle/visibility_control.h | 29 +++++++++++++++++++ .../rclcpp_lifecycle/visibility_control.h | 29 +++++++++++++++++++ 2 files changed, 58 insertions(+) create mode 100644 rclcpp_lifecycle/include/rcl_lifecycle/visibility_control.h create mode 100644 rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/visibility_control.h b/rclcpp_lifecycle/include/rcl_lifecycle/visibility_control.h new file mode 100644 index 0000000000..721411d55d --- /dev/null +++ b/rclcpp_lifecycle/include/rcl_lifecycle/visibility_control.h @@ -0,0 +1,29 @@ +// 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_VISIBILITY_CONTROL_H_ +#define RCL__LIFECYCLE_VISIBILITY_CONTROL_H_ + +#ifdef _WIN32 + #define shared_EXPORTS 1 + #ifdef shared_EXPORTS + #define LIFECYCLE_EXPORT __declspec(dllexport) + #else + #define LIFECYCLE_EXPORT __declspec(dllimport) + #endif +#else + #define LIFECYCLE_EXPORT +#endif + +#endif \ No newline at end of file diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h b/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h new file mode 100644 index 0000000000..2f74e7a5c8 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h @@ -0,0 +1,29 @@ +// 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 RCLCPP__LIFECYCLE_VISIBILITY_CONTROL_H_ +#define RCLCPP__LIFECYCLE_VISIBILITY_CONTROL_H_ + +#ifdef _WIN32 + #define shared_EXPORTS 1 + #ifdef shared_EXPORTS + #define LIFECYCLE_EXPORT __declspec(dllexport) + #else + #define LIFECYCLE_EXPORT __declspec(dllimport) + #endif +#else + #define LIFECYCLE_EXPORT +#endif + +#endif \ No newline at end of file From b24d09091ac6ea159ad7b97699a839d37daccac1 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 1 Nov 2016 17:44:22 -0700 Subject: [PATCH 09/63] correct install of c-library --- rclcpp_lifecycle/CMakeLists.txt | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 2d1a0861ba..a02683f2bb 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -33,7 +33,9 @@ add_library( src/transition_map.c ) install(TARGETS state_machine - DESTINATION lib) + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) macro(targets) if(NOT target_suffix STREQUAL "") @@ -42,6 +44,7 @@ macro(targets) add_executable(lifecycle_talker${target_suffix} src/lifecycle_talker.cpp) + target_link_libraries(lifecycle_talker${target_suffix} state_machine) ament_target_dependencies(lifecycle_talker${target_suffix} "rclcpp${target_suffix}" From f14a23ead46d5571c4e7626ce362fd682c5fdbce Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 1 Nov 2016 17:45:13 -0700 Subject: [PATCH 10/63] fix compilation on windows --- .../rcl_lifecycle/default_state_machine.h | 12 +++++---- .../include/rcl_lifecycle/lifecycle_state.h | 27 +++++++++++++------ .../include/rcl_lifecycle/transition_map.h | 11 +++++--- .../rclcpp_lifecycle/lifecycle_node.hpp | 25 ++++++++++++----- rclcpp_lifecycle/src/lifecycle_talker.cpp | 3 ++- 5 files changed, 54 insertions(+), 24 deletions(-) diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h b/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h index 495ad0cf2d..f7c4bc3746 100644 --- a/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h +++ b/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h @@ -15,6 +15,8 @@ #ifndef RCL__DEFAULT_STATE_MACHINE_H_ #define RCL__DEFAULT_STATE_MACHINE_H_ +#include + #if __cplusplus extern "C" { @@ -22,11 +24,11 @@ extern "C" // primary states based on // design.ros2.org/articles/node_lifecycle.html -static const rcl_state_t rcl_state_unconfigured = {.state = 0, .label = "unconfigured"}; -static const rcl_state_t rcl_state_inactive = {.state = 1, .label = "inactive"}; -static const rcl_state_t rcl_state_active = {.state = 2, .label = "active"}; -static const rcl_state_t rcl_state_finalized = {.state = 3, .label = "finalized"}; -static const rcl_state_t rcl_state_error = {.state = 4, .label = "error"}; +/*static*/ const rcl_state_t LIFECYCLE_EXPORT rcl_state_unconfigured = {/*.state = */0, /*.label = */"unconfigured"}; +/*static*/ const rcl_state_t LIFECYCLE_EXPORT rcl_state_inactive = {/*.state = */1, /*.label = */"inactive"}; +/*static*/ const rcl_state_t LIFECYCLE_EXPORT rcl_state_active = {/*.state = */2, /*.label = */"active"}; +/*static*/ const rcl_state_t LIFECYCLE_EXPORT rcl_state_finalized = {/*.state = */3, /*.label = */"finalized"}; +/*static*/ const rcl_state_t LIFECYCLE_EXPORT rcl_state_error = {/*.state = */4, /*.label = */"error"}; #if __cplusplus } diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h b/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h index bccc6b7827..04ca1c9388 100644 --- a/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h +++ b/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h @@ -20,7 +20,18 @@ extern "C" { #endif -#include +//#include +//#define bool int; +// #ifdef __cplusplus +// #error WRONG COMPILER +// #endif +#ifndef __cplusplus +typedef int bool; +#define true 1 +#define false 0 +#endif + +#include #include /** @@ -28,7 +39,7 @@ extern "C" * @param state: integer giving the state * @param label: label for easy indexing */ -typedef struct _rcl_state_t +typedef struct LIFECYCLE_EXPORT _rcl_state_t { unsigned int state; const char* label; @@ -41,7 +52,7 @@ typedef struct _rcl_state_t * TODO: Maybe specify callback pointer here * and call on_* functions directly */ -typedef struct _rcl_state_transition_t +typedef struct LIFECYCLE_EXPORT _rcl_state_transition_t { rcl_state_t start; // function callback @@ -56,7 +67,7 @@ typedef struct _rcl_state_transition_t * possible transitions registered with this * state machine. */ -typedef struct _rcl_state_machine_t +typedef struct LIFECYCLE_EXPORT _rcl_state_machine_t { // current state of the lifecycle rcl_state_t current_state; @@ -72,16 +83,16 @@ typedef struct _rcl_state_machine_t * current state to the specified goal state */ bool -rcl_is_valid_transition(rcl_state_machine_t* state_machine, const rcl_state_t* goal_state); +LIFECYCLE_EXPORT rcl_is_valid_transition(rcl_state_machine_t* state_machine, const rcl_state_t* goal_state); rcl_state_t -rcl_create_state(unsigned int state, char* label); +LIFECYCLE_EXPORT rcl_create_state(unsigned int state, char* label); rcl_state_transition_t -rcl_create_transition(rcl_state_t start, rcl_state_t goal); +LIFECYCLE_EXPORT rcl_create_transition(rcl_state_t start, rcl_state_t goal); rcl_state_machine_t -rcl_get_default_state_machine(); +LIFECYCLE_EXPORT rcl_get_default_state_machine(); #if __cplusplus } diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h b/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h index 1fa683e746..92e8a8a70d 100644 --- a/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h +++ b/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h @@ -32,7 +32,7 @@ typedef struct _rcl_state_transition_t rcl_state_transition_t; * within the map. One array belongs * to one label within the map. */ -typedef struct _data_array +typedef struct LIFECYCLE_EXPORT _data_array { rcl_state_transition_t* transitions; unsigned int size; @@ -42,7 +42,7 @@ typedef struct _data_array * @brief helper index for keeping * track of the map content */ -typedef struct _index +typedef struct LIFECYCLE_EXPORT _index { unsigned int index; const char* label; @@ -52,7 +52,7 @@ typedef struct _index * @brief stores an array of transitions * index by a start state */ -typedef struct _map +typedef struct LIFECYCLE_EXPORT _map { //rcl_state_t* state_index; rcl_transition_map_index_t* state_index; @@ -65,6 +65,7 @@ typedef struct _map * the classification is based on the start state * within the given transition */ +LIFECYCLE_EXPORT void rcl_append_transition(rcl_transition_map_t* m, rcl_state_transition_t transition); @@ -72,20 +73,24 @@ rcl_append_transition(rcl_transition_map_t* m, rcl_state_transition_t transition * @brief gets all transitions based on a label * label is supposed to come from a rcl_state_t object */ +LIFECYCLE_EXPORT rcl_transition_array_t* rcl_get_map_by_label(rcl_transition_map_t* m, const char* label); /** * @brief gets all transitions based on a state * state is supposed to come from a rcl_state_t object */ +LIFECYCLE_EXPORT rcl_transition_array_t* rcl_get_map_by_state(rcl_transition_map_t* m, const unsigned int state); /** * @brief helper functions to print */ +LIFECYCLE_EXPORT void rcl_print_transition_array(const rcl_transition_array_t* da); +LIFECYCLE_EXPORT void rcl_print_transition_map(const rcl_transition_map_t* m); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index b72bb38cb5..c1866681ac 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -46,6 +46,8 @@ class NodeInterface }; } // namespace lifecycle_interface +#include + /** * @brief LifecycleNode as child class of rclcpp Node * has lifecycle nodeinterface for configuring this node. @@ -54,19 +56,21 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod { public: - RCLCPP_PUBLIC + LIFECYCLE_EXPORT explicit LifecycleNode(const std::string & node_name, bool use_intra_process_comms = false) : Node(node_name, use_intra_process_comms) { setup_state_machine(); } + LIFECYCLE_EXPORT ~LifecycleNode(){} /** * @brief get the default state machine * as defined on design.ros.org */ + LIFECYCLE_EXPORT virtual void setup_state_machine() { @@ -93,6 +97,7 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod return pub; } + LIFECYCLE_EXPORT void print_state_machine() { @@ -100,16 +105,17 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod rcl_print_transition_map(&state_machine_.transition_map); } + LIFECYCLE_EXPORT virtual bool on_configure() { // check on every function whether we are in the correct state - if (!rcl_is_valid_transition(&state_machine_, &rcl_state_inactive)) + /*if (!rcl_is_valid_transition(&state_machine_, &rcl_state_inactive)) { // if not, go to error state state_machine_.current_state = rcl_state_error; return false; - } + }*/ // Placeholder print for all configuring work to be done // with each pub/sub/srv/client @@ -120,27 +126,30 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod return true; } + LIFECYCLE_EXPORT virtual bool on_clean_up() { return true; } + LIFECYCLE_EXPORT virtual bool on_shutdown() { return true; } + LIFECYCLE_EXPORT virtual bool on_activate() { - if (!rcl_is_valid_transition(&state_machine_, &rcl_state_active)) + /*if (!rcl_is_valid_transition(&state_machine_, &rcl_state_active)) { fprintf(stderr, "Unable to change from current state %s from transition start %s\n", state_machine_.current_state.label, rcl_state_active.label); state_machine_.current_state = rcl_state_error; return false; - } + }*/ if (weak_pub_.expired()) { // Someone externally destroyed the publisher handle @@ -159,17 +168,18 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod return true; } + LIFECYCLE_EXPORT virtual bool on_deactivate() { // second transition is from active to deactive - if(!rcl_is_valid_transition(&state_machine_, &rcl_state_inactive)) + /*if(!rcl_is_valid_transition(&state_machine_, &rcl_state_inactive)) { fprintf(stderr, "Unable to change from current state %s from transition start %s\n", state_machine_.current_state.label, rcl_state_inactive.label); fprintf(stderr, "deactivate is not a valid transition in current state %s\n", state_machine_.current_state.label); // TODO: go to error state here return false; - } + }*/ if (weak_pub_.expired()) { fprintf(stderr, "I have no publisher handle\n"); @@ -187,6 +197,7 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod return true; } + LIFECYCLE_EXPORT virtual bool on_error() { diff --git a/rclcpp_lifecycle/src/lifecycle_talker.cpp b/rclcpp_lifecycle/src/lifecycle_talker.cpp index dbea9952b8..1b61e7f76b 100644 --- a/rclcpp_lifecycle/src/lifecycle_talker.cpp +++ b/rclcpp_lifecycle/src/lifecycle_talker.cpp @@ -28,7 +28,7 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); std::shared_ptr lc_node = std::make_shared("lc_talker"); - + rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; custom_qos_profile.depth = 7; @@ -83,4 +83,5 @@ int main(int argc, char * argv[]) loop_rate.sleep(); } return 0; + } From 989178ce1e7e7820bb36b3081cb850529da7ba71 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 2 Nov 2016 23:07:47 -0700 Subject: [PATCH 11/63] refactoring of external/internal api --- .../rcl_lifecycle/default_state_machine.h | 26 ++- .../include/rcl_lifecycle/lifecycle_state.h | 12 +- .../include/rcl_lifecycle/transition_map.h | 6 +- .../rclcpp_lifecycle/lifecycle_node.hpp | 158 +++++++++++------- rclcpp_lifecycle/src/default_state_machine.c | 52 +++--- rclcpp_lifecycle/src/lifecycle_state.c | 18 +- rclcpp_lifecycle/src/lifecycle_talker.cpp | 8 +- rclcpp_lifecycle/src/transition_map.c | 15 +- 8 files changed, 188 insertions(+), 107 deletions(-) diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h b/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h index f7c4bc3746..1818b9e5ab 100644 --- a/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h +++ b/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h @@ -24,11 +24,27 @@ extern "C" // primary states based on // design.ros2.org/articles/node_lifecycle.html -/*static*/ const rcl_state_t LIFECYCLE_EXPORT rcl_state_unconfigured = {/*.state = */0, /*.label = */"unconfigured"}; -/*static*/ const rcl_state_t LIFECYCLE_EXPORT rcl_state_inactive = {/*.state = */1, /*.label = */"inactive"}; -/*static*/ const rcl_state_t LIFECYCLE_EXPORT rcl_state_active = {/*.state = */2, /*.label = */"active"}; -/*static*/ const rcl_state_t LIFECYCLE_EXPORT rcl_state_finalized = {/*.state = */3, /*.label = */"finalized"}; -/*static*/ const rcl_state_t LIFECYCLE_EXPORT rcl_state_error = {/*.state = */4, /*.label = */"error"}; +rcl_state_t LIFECYCLE_EXPORT rcl_state_unconfigured + = {"unconfigured", 0}; +rcl_state_t LIFECYCLE_EXPORT rcl_state_inactive + = {"inactive", 1}; +rcl_state_t LIFECYCLE_EXPORT rcl_state_active + = {"active", 2}; +rcl_state_t LIFECYCLE_EXPORT rcl_state_finalized + = {"finalized", 3}; + +rcl_state_t LIFECYCLE_EXPORT rcl_state_configuring + = {"configuring", 4}; +rcl_state_t LIFECYCLE_EXPORT rcl_state_cleaningup + = {"cleaningup", 5}; +rcl_state_t LIFECYCLE_EXPORT rcl_state_shuttingdown + = {"shuttingdown", 6}; +rcl_state_t LIFECYCLE_EXPORT rcl_state_activating + = {"activating", 7}; +rcl_state_t LIFECYCLE_EXPORT rcl_state_deactivating + = {"deactivating", 8}; +rcl_state_t LIFECYCLE_EXPORT rcl_state_errorprocessing + = {"errorprocessing", 9}; #if __cplusplus } diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h b/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h index 04ca1c9388..2bdf8e931c 100644 --- a/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h +++ b/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h @@ -41,8 +41,8 @@ typedef int bool; */ typedef struct LIFECYCLE_EXPORT _rcl_state_t { - unsigned int state; const char* label; + unsigned int index; } rcl_state_t; /** @@ -54,8 +54,10 @@ typedef struct LIFECYCLE_EXPORT _rcl_state_t */ typedef struct LIFECYCLE_EXPORT _rcl_state_transition_t { - rcl_state_t start; + rcl_state_t transition_index; + //rcl_state_t transition_state; // function callback + void* callback; rcl_state_t goal; } rcl_state_transition_t; @@ -70,8 +72,7 @@ typedef struct LIFECYCLE_EXPORT _rcl_state_transition_t typedef struct LIFECYCLE_EXPORT _rcl_state_machine_t { // current state of the lifecycle - rcl_state_t current_state; - + rcl_state_t* current_state; rcl_transition_map_t transition_map; } rcl_state_machine_t; @@ -94,6 +95,9 @@ LIFECYCLE_EXPORT rcl_create_transition(rcl_state_t start, rcl_state_t goal); rcl_state_machine_t LIFECYCLE_EXPORT rcl_get_default_state_machine(); +void +LIFECYCLE_EXPORT rcl_register_callback(rcl_state_machine_t* state_machine, unsigned int state_index, unsigned int transition_index, bool(*fcn)(void)); + #if __cplusplus } #endif // extern "C" diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h b/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h index 92e8a8a70d..af3e7ec005 100644 --- a/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h +++ b/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h @@ -67,7 +67,7 @@ typedef struct LIFECYCLE_EXPORT _map */ LIFECYCLE_EXPORT void -rcl_append_transition(rcl_transition_map_t* m, rcl_state_transition_t transition); +rcl_append_transition(rcl_transition_map_t* m, rcl_state_t state, rcl_state_transition_t transition); /** * @brief gets all transitions based on a label @@ -75,14 +75,14 @@ rcl_append_transition(rcl_transition_map_t* m, rcl_state_transition_t transition */ LIFECYCLE_EXPORT rcl_transition_array_t* -rcl_get_map_by_label(rcl_transition_map_t* m, const char* label); +rcl_get_transitions_by_label(rcl_transition_map_t* m, const char* label); /** * @brief gets all transitions based on a state * state is supposed to come from a rcl_state_t object */ LIFECYCLE_EXPORT rcl_transition_array_t* -rcl_get_map_by_state(rcl_transition_map_t* m, const unsigned int state); +rcl_get_transitions_by_index(rcl_transition_map_t* m, unsigned int index); /** * @brief helper functions to print diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index c1866681ac..f1935be277 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -29,7 +29,6 @@ namespace node namespace lifecycle_interface { - /** * @brief Interface class for a managed node. * Pure virtual functions as defined in @@ -37,12 +36,90 @@ namespace lifecycle_interface */ class NodeInterface { - virtual bool on_configure() = 0; - virtual bool on_clean_up() = 0; - virtual bool on_shutdown() = 0; - virtual bool on_activate() = 0; - virtual bool on_deactivate() = 0; - virtual bool on_error() = 0; +protected: + virtual bool on_configure() { return true; }; + virtual bool on_cleanup() { return true; }; + virtual bool on_shutdown() { return true; }; + virtual bool on_activate() { return true; }; + virtual bool on_deactivate() { return true; }; + virtual bool on_error() { return true; }; + + /** + * @brief get the default state machine + * as defined on design.ros.org + */ + LIFECYCLE_EXPORT + virtual void + setup_state_machine() + { + state_machine_ = rcl_get_default_state_machine(); + // register callback functions here so that a generic + // move_to function in rcl can actually makes the change + // register_callback(&state_machine, state.label, fcn) + } + + rcl_state_machine_t state_machine_; + +public: + //virtual bool create() = 0; + virtual bool configure() + { + if (state_machine_.current_state->index == rcl_state_unconfigured.index) + { + // change state here to "Configuring" + if (on_configure()) + { + state_machine_.current_state = &rcl_state_inactive; + return true; + } + } + // everything else is considered wrong + //state_machine_.current_state = rcl_state_error; + return false; + } + + virtual bool cleanup() + { + return on_cleanup(); + } + + virtual bool shutdown() + { + return on_shutdown(); + } + + virtual bool activate() + { + if (state_machine_.current_state->index == rcl_state_inactive.index) + { + // change state here to "Activating" + if (on_activate()) + { + state_machine_.current_state = &rcl_state_active; + return true; + } + } + // everything else is considered wrong + //state_machine_.current_state = rcl_state_error; + return false; + } + + virtual bool deactivate() + { + if (state_machine_.current_state->index == rcl_state_active.index) + { + // change state here to "Activating" + if (on_deactivate()) + { + state_machine_.current_state = &rcl_state_inactive; + return true; + } + } + // everything else is considered wrong + //state_machine_.current_state = rcl_state_error; + return false; + } + //virtual bool destroy() = 0; }; } // namespace lifecycle_interface @@ -74,9 +151,11 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod virtual void setup_state_machine() { - state_machine_ = rcl_get_default_state_machine(); - } + lifecycle_interface::NodeInterface::setup_state_machine(); + // FAAAAAAIL !!!!! + //rcl_register_callback(&state_machine_, rcl_state_unconfigured.index, rcl_state_configuring.index, &LifecycleNode::on_configure); + } /** * @brief same API for creating publisher as regular Node */ @@ -101,7 +180,7 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod void print_state_machine() { - printf("current state is %s\n", state_machine_.current_state.label); + printf("current state is %s\n", state_machine_.current_state->label); rcl_print_transition_map(&state_machine_.transition_map); } @@ -109,34 +188,10 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod virtual bool on_configure() { - // check on every function whether we are in the correct state - /*if (!rcl_is_valid_transition(&state_machine_, &rcl_state_inactive)) - { - // if not, go to error state - state_machine_.current_state = rcl_state_error; - return false; - }*/ - // Placeholder print for all configuring work to be done // with each pub/sub/srv/client printf("I am doing some important configuration work\n"); - // work was done correctly, so change the current state - state_machine_.current_state = rcl_state_inactive; - return true; - } - - LIFECYCLE_EXPORT - virtual bool - on_clean_up() - { - return true; - } - - LIFECYCLE_EXPORT - virtual bool - on_shutdown() - { return true; } @@ -144,17 +199,10 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod virtual bool on_activate() { - /*if (!rcl_is_valid_transition(&state_machine_, &rcl_state_active)) - { - fprintf(stderr, "Unable to change from current state %s from transition start %s\n", state_machine_.current_state.label, rcl_state_active.label); - state_machine_.current_state = rcl_state_error; - return false; - }*/ if (weak_pub_.expired()) { // Someone externally destroyed the publisher handle fprintf(stderr, "I have no publisher handle\n"); - state_machine_.current_state = rcl_state_error; return false; } @@ -162,9 +210,13 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod // with each pub/sub/srv/client printf("I am doing a lot of activation work\n"); // TODO: does a return value make sense here? - weak_pub_.lock()->on_activate(); + auto pub = weak_pub_.lock(); + if (!pub) + { + return false; + } + pub->on_activate(); - state_machine_.current_state = rcl_state_active; return true; } @@ -172,28 +224,22 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod virtual bool on_deactivate() { - // second transition is from active to deactive - /*if(!rcl_is_valid_transition(&state_machine_, &rcl_state_inactive)) - { - fprintf(stderr, "Unable to change from current state %s from transition start %s\n", state_machine_.current_state.label, rcl_state_inactive.label); - fprintf(stderr, "deactivate is not a valid transition in current state %s\n", state_machine_.current_state.label); - // TODO: go to error state here - return false; - }*/ if (weak_pub_.expired()) { fprintf(stderr, "I have no publisher handle\n"); - // TODO: go to error state here return false; } // Placeholder print for all configuring work to be done // with each pub/sub/srv/client - printf("I am doing a lot of activation work\n"); + printf("I am doing a lot of deactivation work\n"); // TODO: does a return value make sense here? - weak_pub_.lock()->on_deactivate(); + auto pub = weak_pub_.lock(); + if (!pub){ + return false; + } + pub->on_deactivate(); - state_machine_.current_state = rcl_state_inactive; return true; } @@ -201,6 +247,7 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod virtual bool on_error() { + fprintf(stderr, "Something went wrong here\n"); return true; } @@ -211,7 +258,6 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod // Placeholder for all pub/sub/srv/clients std::weak_ptr weak_pub_; - rcl_state_machine_t state_machine_; }; } // namespace node diff --git a/rclcpp_lifecycle/src/default_state_machine.c b/rclcpp_lifecycle/src/default_state_machine.c index 201319cdef..6090d1b9f3 100644 --- a/rclcpp_lifecycle/src/default_state_machine.c +++ b/rclcpp_lifecycle/src/default_state_machine.c @@ -28,16 +28,16 @@ extern "C" #endif rcl_state_t -rcl_create_state(unsigned int state, char* label) +rcl_create_state(unsigned int index, char* label) { - rcl_state_t ret_state = {.state = state, .label = label}; + rcl_state_t ret_state = {.index = index, .label = label}; return ret_state; } rcl_state_transition_t -rcl_create_state_transition(rcl_state_t start, rcl_state_t goal) +rcl_create_state_transition(rcl_state_t index, rcl_state_t goal) { - rcl_state_transition_t ret_transition = {.start = start, .goal = goal}; + rcl_state_transition_t ret_transition = {.transition_index = index, NULL, .goal = goal}; return ret_transition; } @@ -52,33 +52,33 @@ rcl_get_default_state_machine() state_machine.transition_map.size = 0; // set default state as unconfigured - state_machine.current_state = rcl_state_unconfigured; + state_machine.current_state = &rcl_state_unconfigured; - // from unconfigured to inactive - rcl_state_transition_t unconfigured_to_inactive = {.start = rcl_state_unconfigured, .goal = rcl_state_inactive}; - // from unconfigured to error state - rcl_state_transition_t unconfigured_to_error = {.start = rcl_state_unconfigured, .goal = rcl_state_error}; + rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_unconfigured_inactive + = {rcl_state_configuring, NULL, rcl_state_inactive}; + rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_unconfigured_finalized + = {rcl_state_shuttingdown, NULL, rcl_state_finalized}; - // from inactive to active - rcl_state_transition_t inactive_to_active = {.start = rcl_state_inactive, .goal = rcl_state_active}; - // from inactive to error state - rcl_state_transition_t inactive_to_error = {.start = rcl_state_inactive, .goal = rcl_state_error}; + rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_inactive_unconfigured + = {rcl_state_cleaningup, NULL, rcl_state_unconfigured}; + rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_inactive_finalized + = {rcl_state_shuttingdown, NULL, rcl_state_finalized}; + rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_inactive_active + = {rcl_state_activating, NULL, rcl_state_active}; - // from active to finalized - rcl_state_transition_t active_to_finalized = {.start = rcl_state_active, .goal = rcl_state_finalized}; - // from active to inactive - rcl_state_transition_t active_to_inactive = {.start = rcl_state_active, .goal = rcl_state_inactive}; - // from active to error - rcl_state_transition_t active_to_error = {.start = rcl_state_active, .goal = rcl_state_error}; + rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_active_inactive + = {rcl_state_deactivating, NULL, rcl_state_inactive}; + rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_active_finalized + = {rcl_state_shuttingdown, NULL, rcl_state_finalized}; // add transitions to map - rcl_append_transition(&state_machine.transition_map, unconfigured_to_inactive); - rcl_append_transition(&state_machine.transition_map, unconfigured_to_error); - rcl_append_transition(&state_machine.transition_map, inactive_to_active); - rcl_append_transition(&state_machine.transition_map, inactive_to_error); - rcl_append_transition(&state_machine.transition_map, active_to_finalized); - rcl_append_transition(&state_machine.transition_map, active_to_inactive); - rcl_append_transition(&state_machine.transition_map, active_to_error); + rcl_append_transition(&state_machine.transition_map, rcl_state_unconfigured, rcl_transition_unconfigured_inactive); + rcl_append_transition(&state_machine.transition_map, rcl_state_unconfigured, rcl_transition_unconfigured_finalized); + rcl_append_transition(&state_machine.transition_map, rcl_state_inactive, rcl_transition_inactive_unconfigured); + rcl_append_transition(&state_machine.transition_map, rcl_state_inactive, rcl_transition_inactive_finalized); + rcl_append_transition(&state_machine.transition_map, rcl_state_inactive, rcl_transition_inactive_active); + rcl_append_transition(&state_machine.transition_map, rcl_state_active, rcl_transition_active_inactive); + rcl_append_transition(&state_machine.transition_map, rcl_state_active, rcl_transition_active_finalized); return state_machine; } diff --git a/rclcpp_lifecycle/src/lifecycle_state.c b/rclcpp_lifecycle/src/lifecycle_state.c index 2fe6bd3d31..005e47adb0 100644 --- a/rclcpp_lifecycle/src/lifecycle_state.c +++ b/rclcpp_lifecycle/src/lifecycle_state.c @@ -25,11 +25,11 @@ typedef rcl_state_t rcl_state; bool rcl_is_valid_transition(rcl_state_machine_t* state_machine, const rcl_state_t* goal_state) { - rcl_transition_array_t* all_transitions = rcl_get_map_by_label(&state_machine->transition_map, state_machine->current_state.label); + rcl_transition_array_t* all_transitions = rcl_get_transitions_by_label(&state_machine->transition_map, state_machine->current_state->label); for (unsigned int i=0; isize; ++i) { - if (all_transitions->transitions[i].goal.state == goal_state->state) + if (all_transitions->transitions[i].goal.index == goal_state->index) { return true; } @@ -37,6 +37,20 @@ rcl_is_valid_transition(rcl_state_machine_t* state_machine, const rcl_state_t* g return false; } +void +rcL_register_callback(rcl_state_machine_t* state_machine, unsigned int state_index, unsigned int transition_index, bool(*fcn)(void)) +{ + rcl_transition_array_t* all_transitions = rcl_get_transitions_by_index(&state_machine->transition_map, state_index); + + for (unsigned int i=0; isize; ++i) + { + if (all_transitions->transitions[i].transition_index.index == transition_index) + { + all_transitions->transitions[i].callback = fcn; + } + } +} + #if __cplusplus } #endif // extern "C" diff --git a/rclcpp_lifecycle/src/lifecycle_talker.cpp b/rclcpp_lifecycle/src/lifecycle_talker.cpp index 1b61e7f76b..5a6ba5415d 100644 --- a/rclcpp_lifecycle/src/lifecycle_talker.cpp +++ b/rclcpp_lifecycle/src/lifecycle_talker.cpp @@ -37,7 +37,7 @@ int main(int argc, char * argv[]) lc_node->print_state_machine(); - if (!lc_node->on_configure()) + if (!lc_node->configure()) { printf("Could not configure node. Going to error state.\n"); } @@ -55,8 +55,8 @@ int main(int argc, char * argv[]) loop_rate.sleep(); } - printf("Calling activate\n"); - if (!lc_node->on_activate()) + printf("Calling new activate\n"); + if (!lc_node->activate()) { return -1; } @@ -70,7 +70,7 @@ int main(int argc, char * argv[]) } printf("Calling deactivate\n"); - if (!lc_node->on_deactivate()) + if (!lc_node->deactivate()) { return -1; } diff --git a/rclcpp_lifecycle/src/transition_map.c b/rclcpp_lifecycle/src/transition_map.c index a9ff9ad5fc..e62c79090d 100644 --- a/rclcpp_lifecycle/src/transition_map.c +++ b/rclcpp_lifecycle/src/transition_map.c @@ -51,7 +51,7 @@ void append_to_transition_array(rcl_transition_array_t* transition_array, rcl_st transition_array->transitions[transition_array->size-1] = data; } -void rcl_append_transition(rcl_transition_map_t* m, rcl_state_transition_t data) +void rcl_append_transition(rcl_transition_map_t* m, rcl_state_t state, rcl_state_transition_t data) { // the idea here is to add this transition based on the // start label and classify them. @@ -59,7 +59,7 @@ void rcl_append_transition(rcl_transition_map_t* m, rcl_state_transition_t data) // check wether the label exits already for (unsigned int i=0; isize; ++i) { - if (strcmp(m->state_index[i].label, data.start.label) == 0) + if (strcmp(m->state_index[i].label, state.label) == 0) { // label exists already //printf("Found existing label %s. Will append new data\n", m->state_index[i].label); @@ -73,7 +73,7 @@ void rcl_append_transition(rcl_transition_map_t* m, rcl_state_transition_t data) //printf("There are %u existing labels\n", m->size); // when we reach this point, the label does not exist yet // reallocate the index array and extend it for the new label - rcl_transition_map_index_t idx = {.index = m->size, .label = data.start.label}; + rcl_transition_map_index_t idx = {.index = m->size, .label = state.label}; ++m->size; m->state_index = realloc(m->state_index, m->size*sizeof(rcl_transition_map_index_t)); m->state_index[idx.index] = idx; @@ -84,7 +84,7 @@ void rcl_append_transition(rcl_transition_map_t* m, rcl_state_transition_t data) append_to_transition_array(&m->transition_arrays[idx.index], data); } -rcl_transition_array_t* rcl_get_map_by_label(rcl_transition_map_t* m, const char* label) +rcl_transition_array_t* rcl_get_transitions_by_label(rcl_transition_map_t* m, const char* label) { for (unsigned int i=0; isize; ++i) { @@ -96,11 +96,11 @@ rcl_transition_array_t* rcl_get_map_by_label(rcl_transition_map_t* m, const char return NULL; } -rcl_transition_array_t* rcl_get_map_by_state(rcl_transition_map_t* m, const unsigned int state) +rcl_transition_array_t* rcl_get_transitions_by_index(rcl_transition_map_t* m, unsigned int index) { for (unsigned int i=0; isize; ++i) { - if (m->state_index[i].index == state ) + if (m->state_index[i].index == index ) { return &m->transition_arrays[i]; } @@ -112,7 +112,7 @@ void rcl_print_transition_array(const rcl_transition_array_t* da) { for (unsigned int i=0; isize; ++i) { - printf("%s\t", da->transitions[i].goal.label); + printf("%s(%s)\t", da->transitions[i].goal.label, (da->transitions[i].callback==NULL)?" ":"x"); } printf("\n"); } @@ -126,6 +126,7 @@ void rcl_print_transition_map(const rcl_transition_map_t* m) rcl_print_transition_array(&(m->transition_arrays[i])); } } + #if __cplusplus } #endif From 61404f4d91dc119b92bf54265d4bc48f2fc7c049 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 4 Nov 2016 07:58:33 -0700 Subject: [PATCH 12/63] (dev) generate static functions for c-callback --- .../include/rcl_lifecycle/lifecycle_state.h | 3 ++ .../rclcpp_lifecycle/lifecycle_node.hpp | 40 ++++++++++++++++--- rclcpp_lifecycle/src/lifecycle_state.c | 24 +++++++++++ 3 files changed, 62 insertions(+), 5 deletions(-) diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h b/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h index 2bdf8e931c..aa6df76b2f 100644 --- a/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h +++ b/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h @@ -98,6 +98,9 @@ LIFECYCLE_EXPORT rcl_get_default_state_machine(); void LIFECYCLE_EXPORT rcl_register_callback(rcl_state_machine_t* state_machine, unsigned int state_index, unsigned int transition_index, bool(*fcn)(void)); +bool +LIFECYCLE_EXPORT rcl_invoke_transition(rcl_state_machine_t* state_machine, rcl_state_t transition_index); + #if __cplusplus } #endif // extern "C" diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index f1935be277..1643107e23 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -29,6 +29,21 @@ namespace node namespace lifecycle_interface { +template +struct Callback; + +template +struct Callback +{ + template + static Ret callback(Args... args) { return func(args...); } + static std::function func; +}; + +// Initialize the static member. +template +std::function Callback::func; + /** * @brief Interface class for a managed node. * Pure virtual functions as defined in @@ -58,6 +73,16 @@ class NodeInterface // register_callback(&state_machine, state.label, fcn) } + LIFECYCLE_EXPORT + template + void + register_state_callback(bool (T::*method)(), T* instance) + { + Callback::func = std::bind(method, instance); + bool(*c_function_pointer)(void) = static_cast(Callback::callback); + rcl_register_callback(&state_machine_, (unsigned int)State_Index, (unsigned int)Transition_Index, c_function_pointer); + } + rcl_state_machine_t state_machine_; public: @@ -66,12 +91,15 @@ class NodeInterface { if (state_machine_.current_state->index == rcl_state_unconfigured.index) { + // given the current state machine, specify a transition and go for it + auto ret = rcl_invoke_transition(&state_machine_, rcl_state_configuring); + printf("%s\n", (ret)?"Callback was successful":"Callback unsuccessful"); // change state here to "Configuring" - if (on_configure()) - { - state_machine_.current_state = &rcl_state_inactive; - return true; - } + //if (on_configure()) + //{ + // state_machine_.current_state = &rcl_state_inactive; + // return true; + //} } // everything else is considered wrong //state_machine_.current_state = rcl_state_error; @@ -153,6 +181,8 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod { lifecycle_interface::NodeInterface::setup_state_machine(); + register_state_callback( + &LifecycleNode::on_configure, this); // FAAAAAAIL !!!!! //rcl_register_callback(&state_machine_, rcl_state_unconfigured.index, rcl_state_configuring.index, &LifecycleNode::on_configure); } diff --git a/rclcpp_lifecycle/src/lifecycle_state.c b/rclcpp_lifecycle/src/lifecycle_state.c index 005e47adb0..6c3247d926 100644 --- a/rclcpp_lifecycle/src/lifecycle_state.c +++ b/rclcpp_lifecycle/src/lifecycle_state.c @@ -51,6 +51,30 @@ rcL_register_callback(rcl_state_machine_t* state_machine, unsigned int state_ind } } +// maybe change directly the current state here, +// no need to that all the time inside high level language +bool +rcl_invoke_transition(rcl_state_machine_t* state_machine, rcl_state_t transition_index) +{ + rcl_transition_array_t* all_transitions = rcl_get_transitions_by_index(&state_machine->transition_map, state_machine->current_state->index); + + if (!all_transitions) + { + return false; + } + bool success = false; + for (unsigned int i=0; isize; ++i) + { + if (all_transitions->transitions[i].transition_index.index == transition_index.index) + { + ((bool(*)(void))all_transitions->transitions[i].callback)(); + success = true; + // break here ?! would allow only one to one transitions + } + } + return success; +} + #if __cplusplus } #endif // extern "C" From 4af9891ad9f3325f1e4135bfe84e1a08960bb44f Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 4 Nov 2016 08:02:22 -0700 Subject: [PATCH 13/63] (fix) correct typo --- rclcpp_lifecycle/src/lifecycle_state.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_lifecycle/src/lifecycle_state.c b/rclcpp_lifecycle/src/lifecycle_state.c index 6c3247d926..c67de3d1f1 100644 --- a/rclcpp_lifecycle/src/lifecycle_state.c +++ b/rclcpp_lifecycle/src/lifecycle_state.c @@ -38,7 +38,7 @@ rcl_is_valid_transition(rcl_state_machine_t* state_machine, const rcl_state_t* g } void -rcL_register_callback(rcl_state_machine_t* state_machine, unsigned int state_index, unsigned int transition_index, bool(*fcn)(void)) +rcl_register_callback(rcl_state_machine_t* state_machine, unsigned int state_index, unsigned int transition_index, bool(*fcn)(void)) { rcl_transition_array_t* all_transitions = rcl_get_transitions_by_index(&state_machine->transition_map, state_index); From d8a30e7017aecef6aa3d95b2f7b1993f889f9a97 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 4 Nov 2016 11:20:13 -0700 Subject: [PATCH 14/63] (dev) cleanup for c-statemachine (dev) cleanup for c-statemachine --- .../include/rcl_lifecycle/lifecycle_state.h | 17 +- .../include/rcl_lifecycle/transition_map.h | 65 +++++--- rclcpp_lifecycle/src/default_state_machine.c | 52 +++--- rclcpp_lifecycle/src/lifecycle_state.c | 76 ++++++--- rclcpp_lifecycle/src/transition_map.c | 153 +++++++++++++----- 5 files changed, 257 insertions(+), 106 deletions(-) diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h b/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h index aa6df76b2f..2f45bb5d2d 100644 --- a/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h +++ b/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h @@ -54,11 +54,10 @@ typedef struct LIFECYCLE_EXPORT _rcl_state_t */ typedef struct LIFECYCLE_EXPORT _rcl_state_transition_t { - rcl_state_t transition_index; - //rcl_state_t transition_state; - // function callback + rcl_state_t transition_state; void* callback; - rcl_state_t goal; + rcl_state_t* start; + rcl_state_t* goal; } rcl_state_transition_t; /** @@ -71,7 +70,6 @@ typedef struct LIFECYCLE_EXPORT _rcl_state_transition_t */ typedef struct LIFECYCLE_EXPORT _rcl_state_machine_t { - // current state of the lifecycle rcl_state_t* current_state; rcl_transition_map_t transition_map; } rcl_state_machine_t; @@ -82,10 +80,17 @@ typedef struct LIFECYCLE_EXPORT _rcl_state_machine_t * @brief traverses the transition map of the given * state machine to find if there is a transition from the * current state to the specified goal state + * @return the transition state which is valid + * NULL if not available */ -bool +const rcl_state_transition_t* LIFECYCLE_EXPORT rcl_is_valid_transition(rcl_state_machine_t* state_machine, const rcl_state_t* goal_state); +const rcl_state_transition_t* +LIFECYCLE_EXPORT rcl_get_registered_transition_by_index(rcl_state_machine_t* state_machine, unsigned int transition_state_index); +const rcl_state_transition_t* +LIFECYCLE_EXPORT rcl_get_registered_transition_by_label(rcl_state_machine_t* state_machine, const char* transition_state_label); + rcl_state_t LIFECYCLE_EXPORT rcl_create_state(unsigned int state, char* label); diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h b/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h index af3e7ec005..49e6f4d160 100644 --- a/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h +++ b/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h @@ -24,30 +24,20 @@ extern "C" #endif typedef struct _rcl_state_t rcl_state_t; - typedef struct _rcl_state_transition_t rcl_state_transition_t; /** - * @brief The actual data array - * within the map. One array belongs - * to one label within the map. + * @brief All transitions which are + * valid associations for a primary state. + * One array belongs to one primary state + * within the map. */ -typedef struct LIFECYCLE_EXPORT _data_array +typedef struct LIFECYCLE_EXPORT _transition_array { rcl_state_transition_t* transitions; unsigned int size; } rcl_transition_array_t; -/** - * @brief helper index for keeping - * track of the map content - */ -typedef struct LIFECYCLE_EXPORT _index -{ - unsigned int index; - const char* label; -} rcl_transition_map_index_t; - /** * @brief stores an array of transitions * index by a start state @@ -55,11 +45,26 @@ typedef struct LIFECYCLE_EXPORT _index typedef struct LIFECYCLE_EXPORT _map { //rcl_state_t* state_index; - rcl_transition_map_index_t* state_index; + // associative array between primary state and their respective transitions + // 1 ps -> 1 transition_array + rcl_state_t* primary_states; rcl_transition_array_t* transition_arrays; unsigned int size; } rcl_transition_map_t; +LIFECYCLE_EXPORT +void +rcl_register_primary_state(rcl_transition_map_t* m, rcl_state_t primary_state); + +/** + * @brief appends a transition in a map + * the classification is based on the start state + * within the given transition + */ +LIFECYCLE_EXPORT +void +rcl_register_transition_by_state(rcl_transition_map_t* m, rcl_state_t start, rcl_state_t goal, rcl_state_transition_t transition); + /** * @brief appends a transition in a map * the classification is based on the start state @@ -67,7 +72,31 @@ typedef struct LIFECYCLE_EXPORT _map */ LIFECYCLE_EXPORT void -rcl_append_transition(rcl_transition_map_t* m, rcl_state_t state, rcl_state_transition_t transition); +rcl_register_transition_by_label(rcl_transition_map_t* m, const char* start_label, const char* goal_label, rcl_state_transition_t transition); + +/** + * @brief appends a transition in a map + * the classification is based on the start state + * within the given transition + */ +LIFECYCLE_EXPORT +void +rcl_register_transition_by_index(rcl_transition_map_t* m, unsigned int start_index, unsigned int goal_index, rcl_state_transition_t transition); + +/** + * @brief gets the registered primary state based on a label + * @return primary state pointer, NULL if not found + */ +LIFECYCLE_EXPORT +rcl_state_t* +rcl_get_primary_state_by_label(rcl_transition_map_t* m, const char* label); +/** + * @brief gets the registered primary state based on a index + * @return primary state pointer, NULL if not found + */ +LIFECYCLE_EXPORT +rcl_state_t* +rcl_get_primary_state_by_index(rcl_transition_map_t* m, unsigned int index); /** * @brief gets all transitions based on a label @@ -89,7 +118,7 @@ rcl_get_transitions_by_index(rcl_transition_map_t* m, unsigned int index); */ LIFECYCLE_EXPORT void -rcl_print_transition_array(const rcl_transition_array_t* da); +rcl_print_transition_array(const rcl_transition_array_t* transition_array); LIFECYCLE_EXPORT void rcl_print_transition_map(const rcl_transition_map_t* m); diff --git a/rclcpp_lifecycle/src/default_state_machine.c b/rclcpp_lifecycle/src/default_state_machine.c index 6090d1b9f3..d5bd9f68ca 100644 --- a/rclcpp_lifecycle/src/default_state_machine.c +++ b/rclcpp_lifecycle/src/default_state_machine.c @@ -35,9 +35,9 @@ rcl_create_state(unsigned int index, char* label) } rcl_state_transition_t -rcl_create_state_transition(rcl_state_t index, rcl_state_t goal) +rcl_create_state_transition(unsigned int index, const char* label) { - rcl_state_transition_t ret_transition = {.transition_index = index, NULL, .goal = goal}; + rcl_state_transition_t ret_transition = {{.index = index, .label = label}, NULL, NULL, NULL}; return ret_transition; } @@ -47,39 +47,39 @@ rcl_state_machine_t rcl_get_default_state_machine() { rcl_state_machine_t state_machine; - state_machine.transition_map.state_index = NULL; + state_machine.transition_map.primary_states = NULL; state_machine.transition_map.transition_arrays = NULL; state_machine.transition_map.size = 0; // set default state as unconfigured - state_machine.current_state = &rcl_state_unconfigured; + //state_machine.current_state = &rcl_state_unconfigured; - rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_unconfigured_inactive - = {rcl_state_configuring, NULL, rcl_state_inactive}; - rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_unconfigured_finalized - = {rcl_state_shuttingdown, NULL, rcl_state_finalized}; + rcl_state_transition_t rcl_transition_configuring + = rcl_create_state_transition(rcl_state_configuring.index, rcl_state_configuring.label); + rcl_state_transition_t rcl_transition_shuttingdown + = rcl_create_state_transition(rcl_state_shuttingdown.index, rcl_state_shuttingdown.label); + rcl_state_transition_t rcl_transition_cleaningup + = rcl_create_state_transition(rcl_state_cleaningup.index, rcl_state_cleaningup.label); + rcl_state_transition_t rcl_transition_activating + = rcl_create_state_transition(rcl_state_activating.index, rcl_state_activating.label); + rcl_state_transition_t rcl_transition_deactivating + = rcl_create_state_transition(rcl_state_deactivating.index, rcl_state_deactivating.label);; - rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_inactive_unconfigured - = {rcl_state_cleaningup, NULL, rcl_state_unconfigured}; - rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_inactive_finalized - = {rcl_state_shuttingdown, NULL, rcl_state_finalized}; - rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_inactive_active - = {rcl_state_activating, NULL, rcl_state_active}; - - rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_active_inactive - = {rcl_state_deactivating, NULL, rcl_state_inactive}; - rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_active_finalized - = {rcl_state_shuttingdown, NULL, rcl_state_finalized}; + rcl_register_primary_state(&state_machine.transition_map, rcl_state_unconfigured); + rcl_register_primary_state(&state_machine.transition_map, rcl_state_inactive); + rcl_register_primary_state(&state_machine.transition_map, rcl_state_active); + rcl_register_primary_state(&state_machine.transition_map, rcl_state_finalized); // add transitions to map - rcl_append_transition(&state_machine.transition_map, rcl_state_unconfigured, rcl_transition_unconfigured_inactive); - rcl_append_transition(&state_machine.transition_map, rcl_state_unconfigured, rcl_transition_unconfigured_finalized); - rcl_append_transition(&state_machine.transition_map, rcl_state_inactive, rcl_transition_inactive_unconfigured); - rcl_append_transition(&state_machine.transition_map, rcl_state_inactive, rcl_transition_inactive_finalized); - rcl_append_transition(&state_machine.transition_map, rcl_state_inactive, rcl_transition_inactive_active); - rcl_append_transition(&state_machine.transition_map, rcl_state_active, rcl_transition_active_inactive); - rcl_append_transition(&state_machine.transition_map, rcl_state_active, rcl_transition_active_finalized); + rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_unconfigured, rcl_state_inactive, rcl_transition_configuring); + rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_inactive, rcl_state_unconfigured, rcl_transition_cleaningup); + rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_unconfigured, rcl_state_finalized, rcl_transition_shuttingdown); + rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_inactive, rcl_state_finalized, rcl_transition_shuttingdown); + rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_inactive, rcl_state_active, rcl_transition_activating); + rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_active, rcl_state_inactive, rcl_transition_deactivating); + rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_active, rcl_state_finalized, rcl_transition_shuttingdown); + state_machine.current_state = &state_machine.transition_map.primary_states[0]; // set to first entry return state_machine; } diff --git a/rclcpp_lifecycle/src/lifecycle_state.c b/rclcpp_lifecycle/src/lifecycle_state.c index c67de3d1f1..a56444613d 100644 --- a/rclcpp_lifecycle/src/lifecycle_state.c +++ b/rclcpp_lifecycle/src/lifecycle_state.c @@ -17,36 +17,73 @@ extern "C" { #endif -#include "rcl_lifecycle/lifecycle_state.h" +#include +#include -typedef rcl_state_transition_t rcl_transition; -typedef rcl_state_t rcl_state; +#include "rcl_lifecycle/lifecycle_state.h" -bool +const rcl_state_transition_t* rcl_is_valid_transition(rcl_state_machine_t* state_machine, const rcl_state_t* goal_state) { - rcl_transition_array_t* all_transitions = rcl_get_transitions_by_label(&state_machine->transition_map, state_machine->current_state->label); + unsigned int current_index = state_machine->current_state->index; + const rcl_transition_array_t* valid_transitions = rcl_get_transitions_by_index(&state_machine->transition_map, current_index); + for (unsigned int i=0; isize; ++i) + { + if (valid_transitions->transitions[i].goal->index == goal_state->index) + { + return &valid_transitions->transitions[i]; + } + } + return NULL; +} - for (unsigned int i=0; isize; ++i) +const rcl_state_transition_t* +rcl_get_registered_transition_by_index(rcl_state_machine_t* state_machine, unsigned int transition_state_index) +{ + // extensive search approach + // TODO(karsten1987) can be improved by having a separate array for "registered transition" + const rcl_transition_map_t* map = &state_machine->transition_map; + for (unsigned int i=0; isize; ++i) { - if (all_transitions->transitions[i].goal.index == goal_state->index) + for (unsigned int j=0; jtransition_arrays[i].size; ++j) { - return true; + if (map->transition_arrays[i].transitions[j].transition_state.index == transition_state_index) + { + return &map->transition_arrays[i].transitions[j]; + } } } - return false; + return NULL; +} + +const rcl_state_transition_t* +rcl_get_registered_transition_by_label(rcl_state_machine_t* state_machine, const char* transition_state_label) +{ + // extensive search approach + // TODO(karsten1987) can be improved by having a separate array for "registered transition" + const rcl_transition_map_t* map = &state_machine->transition_map; + for (unsigned int i=0; isize; ++i) + { + for (unsigned int j=0; jtransition_arrays[i].size; ++j) + { + if (strcmp(map->transition_arrays[i].transitions[j].transition_state.label, transition_state_label) == 0) + { + return &map->transition_arrays[i].transitions[j]; + } + } + } + return NULL; } void rcl_register_callback(rcl_state_machine_t* state_machine, unsigned int state_index, unsigned int transition_index, bool(*fcn)(void)) { - rcl_transition_array_t* all_transitions = rcl_get_transitions_by_index(&state_machine->transition_map, state_index); - - for (unsigned int i=0; isize; ++i) + rcl_transition_array_t* transitions = rcl_get_transitions_by_index(&state_machine->transition_map, state_index); + for (unsigned int i=0; isize; ++i) { - if (all_transitions->transitions[i].transition_index.index == transition_index) + if (transitions->transitions[i].transition_state.index == transition_index) { - all_transitions->transitions[i].callback = fcn; + transitions->transitions[i].callback = fcn; } } } @@ -56,18 +93,19 @@ rcl_register_callback(rcl_state_machine_t* state_machine, unsigned int state_ind bool rcl_invoke_transition(rcl_state_machine_t* state_machine, rcl_state_t transition_index) { - rcl_transition_array_t* all_transitions = rcl_get_transitions_by_index(&state_machine->transition_map, state_machine->current_state->index); + unsigned int current_index = state_machine->current_state->index; + rcl_transition_array_t* transitions = rcl_get_transitions_by_index(&state_machine->transition_map, current_index); - if (!all_transitions) + if (transitions == NULL) { return false; } bool success = false; - for (unsigned int i=0; isize; ++i) + for (unsigned int i=0; isize; ++i) { - if (all_transitions->transitions[i].transition_index.index == transition_index.index) + if (transitions->transitions[i].transition_state.index == transition_index.index) { - ((bool(*)(void))all_transitions->transitions[i].callback)(); + ((bool(*)(void))transitions->transitions[i].callback)(); success = true; // break here ?! would allow only one to one transitions } diff --git a/rclcpp_lifecycle/src/transition_map.c b/rclcpp_lifecycle/src/transition_map.c index e62c79090d..beecb8330c 100644 --- a/rclcpp_lifecycle/src/transition_map.c +++ b/rclcpp_lifecycle/src/transition_map.c @@ -24,9 +24,52 @@ extern "C" #include #include -void append_to_transition_array(rcl_transition_array_t* transition_array, rcl_state_transition_t data) +void rcl_register_primary_state(rcl_transition_map_t* m, rcl_state_t primary_state) { - // There is no transition array !? + if (rcl_get_primary_state_by_index(m, primary_state.index) != NULL) + { + // primary state is already registered + return; + } + + // add new primary state memory + //rcl_transition_map_index_t idx = {.index = m->size, .label = state.label}; + ++m->size; + m->primary_states = realloc(m->primary_states, m->size*sizeof(rcl_state_t)); + m->primary_states[m->size-1] = primary_state; + + // allocate empty pointer for transition_array_t + m->transition_arrays = realloc(m->transition_arrays, m->size*sizeof(rcl_transition_array_t)); + m->transition_arrays[m->size-1].transitions = NULL; + m->transition_arrays[m->size-1].size = 0; // initialize array to size 0 +} + +void rcl_register_transition_by_state(rcl_transition_map_t* m, rcl_state_t start, rcl_state_t goal, rcl_state_transition_t transition) +{ + rcl_register_transition_by_index(m, start.index, goal.index, transition); +} + +void rcl_register_transition_by_label(rcl_transition_map_t* m, const char* start_label, const char* goal_label, rcl_state_transition_t transition) +{ + // the idea here is to add this transition based on the + // start label and classify them. + rcl_state_t* start_state = rcl_get_primary_state_by_label(m, start_label); + if (start_state == NULL) + { + // return false here? + return; + } + rcl_state_t* goal_state = rcl_get_primary_state_by_label(m, goal_label); + if (goal_state == NULL) + { + // return false here? + return; + } + + transition.start = start_state; + transition.goal = goal_state; + + rcl_transition_array_t* transition_array = rcl_get_transitions_by_index(m, transition.start->index); if (!transition_array) { return; @@ -37,58 +80,94 @@ void append_to_transition_array(rcl_transition_array_t* transition_array, rcl_st // TODO: Not sure if this is really necessary to differentiate // between malloc and realloc - if (transition_array->size == 1) - { - transition_array->transitions = malloc(sizeof(rcl_state_transition_t)); - } - else - { + //if (transition_array->size == 1) + //{ + // transition_array->transitions = malloc(sizeof(rcl_state_transition_t)); + //} + //else + //{ transition_array->transitions = realloc(transition_array->transitions, transition_array->size*sizeof(rcl_state_transition_t)); - } + //} // finally set the new transition to the end of the array - transition_array->transitions[transition_array->size-1] = data; + transition_array->transitions[transition_array->size-1] = transition; } -void rcl_append_transition(rcl_transition_map_t* m, rcl_state_t state, rcl_state_transition_t data) +void rcl_register_transition_by_index(rcl_transition_map_t* m, unsigned int start_index, unsigned int goal_index, rcl_state_transition_t transition) { // the idea here is to add this transition based on the // start label and classify them. + rcl_state_t* start_state = rcl_get_primary_state_by_index(m, start_index); + if (start_state == NULL) + { + // return false here? + return; + } + rcl_state_t* goal_state = rcl_get_primary_state_by_index(m, goal_index); + if (goal_state == NULL) + { + // return false here? + return; + } + + transition.start = start_state; + transition.goal = goal_state; + + rcl_transition_array_t* transition_array = rcl_get_transitions_by_index(m, transition.start->index); + if (!transition_array) + { + return; + } + + // we add a new transition, so increase the size + ++transition_array->size; + + // TODO: Not sure if this is really necessary to differentiate + // between malloc and realloc + //if (transition_array->size == 1) + //{ + // transition_array->transitions = malloc(sizeof(rcl_state_transition_t)); + //} + //else + //{ + transition_array->transitions + = realloc(transition_array->transitions, transition_array->size*sizeof(rcl_state_transition_t)); + //} - // check wether the label exits already + // finally set the new transition to the end of the array + transition_array->transitions[transition_array->size-1] = transition; +} + +rcl_state_t* rcl_get_primary_state_by_label(rcl_transition_map_t* m, const char* label) +{ for (unsigned int i=0; isize; ++i) { - if (strcmp(m->state_index[i].label, state.label) == 0) + if (m->primary_states[i].label == label) { - // label exists already - //printf("Found existing label %s. Will append new data\n", m->state_index[i].label); - // we will append data into rcl_transition_array_t with given data.start.label - append_to_transition_array(&(m->transition_arrays[i]), data); - return; + return &m->primary_states[i]; } } + return NULL; +} - //printf("Label %s does not exist yet. Will add a new one\n", data.start.label); - //printf("There are %u existing labels\n", m->size); - // when we reach this point, the label does not exist yet - // reallocate the index array and extend it for the new label - rcl_transition_map_index_t idx = {.index = m->size, .label = state.label}; - ++m->size; - m->state_index = realloc(m->state_index, m->size*sizeof(rcl_transition_map_index_t)); - m->state_index[idx.index] = idx; - - // reallocate the actual data array and extend it for the data - m->transition_arrays = realloc(m->transition_arrays, m->size*sizeof(rcl_transition_array_t)); - m->transition_arrays[idx.index].size = 0; - append_to_transition_array(&m->transition_arrays[idx.index], data); +rcl_state_t* rcl_get_primary_state_by_index(rcl_transition_map_t* m, unsigned int index) +{ + for (unsigned int i=0; isize; ++i) + { + if (m->primary_states[i].index == index) + { + return &m->primary_states[i]; + } + } + return NULL; } rcl_transition_array_t* rcl_get_transitions_by_label(rcl_transition_map_t* m, const char* label) { for (unsigned int i=0; isize; ++i) { - if (strcmp(m->state_index[i].label, label) == 0) + if (strcmp(m->primary_states[i].label, label) == 0) { return &m->transition_arrays[i]; } @@ -100,7 +179,7 @@ rcl_transition_array_t* rcl_get_transitions_by_index(rcl_transition_map_t* m, un { for (unsigned int i=0; isize; ++i) { - if (m->state_index[i].index == index ) + if (m->primary_states[i].index == index ) { return &m->transition_arrays[i]; } @@ -108,11 +187,11 @@ rcl_transition_array_t* rcl_get_transitions_by_index(rcl_transition_map_t* m, un return NULL; } -void rcl_print_transition_array(const rcl_transition_array_t* da) +void rcl_print_transition_array(const rcl_transition_array_t* ta) { - for (unsigned int i=0; isize; ++i) + for (unsigned int i=0; isize; ++i) { - printf("%s(%s)\t", da->transitions[i].goal.label, (da->transitions[i].callback==NULL)?" ":"x"); + printf("%s(%u)(%s)\t", ta->transitions[i].transition_state.label, ta->transitions[i].transition_state.index, (ta->transitions[i].callback==NULL)?" ":"x"); } printf("\n"); } @@ -122,7 +201,7 @@ void rcl_print_transition_map(const rcl_transition_map_t* m) printf("rcl_transition_map_t size %u\n", m->size); for (unsigned int i=0; isize; ++i) { - printf("Start state %s ::: ", m->state_index[i].label); + printf("Start state %s(%u) ::: ", m->primary_states[i].label, m->primary_states[i].index); rcl_print_transition_array(&(m->transition_arrays[i])); } } From b2e0803d15fcc5955e8cba64166ad8c1c595923f Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 4 Nov 2016 14:53:39 -0700 Subject: [PATCH 15/63] (dev) cpp callback map --- .../rclcpp_lifecycle/lifecycle_node.hpp | 74 +++++++++++++------ 1 file changed, 53 insertions(+), 21 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 1643107e23..22d94af5e0 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -35,9 +35,9 @@ struct Callback; template struct Callback { - template - static Ret callback(Args... args) { return func(args...); } - static std::function func; + template + static Ret callback(Args... args) { return func(args...); } + static std::function func; }; // Initialize the static member. @@ -51,7 +51,10 @@ std::function Callback> callback_map; + rcl_state_machine_t state_machine_; + + protected: virtual bool on_configure() { return true; }; virtual bool on_cleanup() { return true; }; virtual bool on_shutdown() { return true; }; @@ -68,11 +71,10 @@ class NodeInterface setup_state_machine() { state_machine_ = rcl_get_default_state_machine(); - // register callback functions here so that a generic - // move_to function in rcl can actually makes the change - // register_callback(&state_machine, state.label, fcn) } + // In case with want to register the callbacks directly in c + /* LIFECYCLE_EXPORT template void @@ -82,8 +84,46 @@ class NodeInterface bool(*c_function_pointer)(void) = static_cast(Callback::callback); rcl_register_callback(&state_machine_, (unsigned int)State_Index, (unsigned int)Transition_Index, c_function_pointer); } + */ - rcl_state_machine_t state_machine_; + LIFECYCLE_EXPORT + template + void + register_transition_callback(bool (T::*method)(), T* instance, size_t transition_state_index) + { + const rcl_state_transition_t* transition_state + = rcl_get_registered_transition_by_index(&state_machine_, transition_state_index); + if (!transition_state) + { + // TODO do something smarter here + throw std::runtime_error("Transition is not valid"); + } + callback_map[transition_state_index] = std::bind(method, instance); + } + + LIFECYCLE_EXPORT + template + void + register_transition_callback(bool (T::*method)(), T* instance, const std::string& transition_state_label) + { + const rcl_state_transition_t* transition_state + = rcl_get_registered_transition_by_label(&state_machine_, transition_state_label.c_str()); + if (!transition_state) + { + // TODO do something smarter here + throw std::runtime_error("Transition is not valid"); + } + callback_map[transition_state->transition_state.index] = std::bind(method, instance); + } + + public: + LIFECYCLE_EXPORT + void + print_state_machine() + { + printf("current state is %s\n", state_machine_.current_state->label); + rcl_print_transition_map(&state_machine_.transition_map); + } public: //virtual bool create() = 0; @@ -92,7 +132,9 @@ class NodeInterface if (state_machine_.current_state->index == rcl_state_unconfigured.index) { // given the current state machine, specify a transition and go for it - auto ret = rcl_invoke_transition(&state_machine_, rcl_state_configuring); + //auto ret = rcl_invoke_transition(&state_machine_, rcl_state_configuring); + auto cb = callback_map[rcl_state_configuring.index]; + auto ret = cb(); printf("%s\n", (ret)?"Callback was successful":"Callback unsuccessful"); // change state here to "Configuring" //if (on_configure()) @@ -181,11 +223,9 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod { lifecycle_interface::NodeInterface::setup_state_machine(); - register_state_callback( - &LifecycleNode::on_configure, this); - // FAAAAAAIL !!!!! - //rcl_register_callback(&state_machine_, rcl_state_unconfigured.index, rcl_state_configuring.index, &LifecycleNode::on_configure); + register_transition_callback(&LifecycleNode::on_configure, this, rcl_state_configuring.index); } + /** * @brief same API for creating publisher as regular Node */ @@ -206,14 +246,6 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod return pub; } - LIFECYCLE_EXPORT - void - print_state_machine() - { - printf("current state is %s\n", state_machine_.current_state->label); - rcl_print_transition_map(&state_machine_.transition_map); - } - LIFECYCLE_EXPORT virtual bool on_configure() From ec52ad594c69794256fed635a84745b2c007f96f Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 7 Nov 2016 17:24:30 -0800 Subject: [PATCH 16/63] (dev) mv source file into project folders --- rclcpp_lifecycle/CMakeLists.txt | 33 ++- rclcpp_lifecycle/src/default_state_machine.c | 90 -------- rclcpp_lifecycle/src/lifecycle_state.c | 118 ----------- rclcpp_lifecycle/src/lifecycle_talker.cpp | 87 -------- rclcpp_lifecycle/src/transition_map.c | 211 ------------------- 5 files changed, 22 insertions(+), 517 deletions(-) delete mode 100644 rclcpp_lifecycle/src/default_state_machine.c delete mode 100644 rclcpp_lifecycle/src/lifecycle_state.c delete mode 100644 rclcpp_lifecycle/src/lifecycle_talker.cpp delete mode 100644 rclcpp_lifecycle/src/transition_map.c diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index a02683f2bb..78d3f034c7 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -19,20 +19,21 @@ include_directories( # ${rmw_INCLUDE_DIRS} ) +set(rcl_lifecycle_sources + src/rcl_lifecycle/lifecycle_state.c + src/rcl_lifecycle/default_state_machine.c + src/rcl_lifecycle/transition_map.c +) set_source_files_properties( - src/lifecycle_state.c - src/default_state_machine.c - src/transition_map.c + ${rcl_lifecycle_sources} PROPERTIES language "C" ) add_library( - state_machine + rcl_lifecycle SHARED - src/lifecycle_state.c - src/default_state_machine.c - src/transition_map.c + ${rcl_lifecycle_sources} ) -install(TARGETS state_machine +install(TARGETS rcl_lifecycle ARCHIVE DESTINATION lib LIBRARY DESTINATION lib ) @@ -42,10 +43,20 @@ macro(targets) get_rclcpp_information("${rmw_implementation}" "rclcpp${target_suffix}") endif() + add_library(rclcpp_lifecycle${target_suffix} + src/rclcpp_lifecycle/lifecycle_manager.cpp) + target_link_libraries(rclcpp_lifecycle${target_suffix} + rcl_lifecycle + ) + ament_target_dependencies(rclcpp_lifecycle${target_suffix} + "rclcpp${target_suffix}" + ) + add_executable(lifecycle_talker${target_suffix} - src/lifecycle_talker.cpp) - - target_link_libraries(lifecycle_talker${target_suffix} state_machine) + src/rclcpp_lifecycle/lifecycle_talker.cpp) + target_link_libraries(lifecycle_talker${target_suffix} + rclcpp_lifecycle + ) ament_target_dependencies(lifecycle_talker${target_suffix} "rclcpp${target_suffix}" "std_msgs" diff --git a/rclcpp_lifecycle/src/default_state_machine.c b/rclcpp_lifecycle/src/default_state_machine.c deleted file mode 100644 index d5bd9f68ca..0000000000 --- a/rclcpp_lifecycle/src/default_state_machine.c +++ /dev/null @@ -1,90 +0,0 @@ -// 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 RCL__STATE_MACHINE_H_ -#define RCL__STATE_MACHINE_H_ - -#include -#include -#include - -#include "rcl_lifecycle/lifecycle_state.h" -#include "rcl_lifecycle/default_state_machine.h" - -#if __cplusplus -extern "C" -{ -#endif - -rcl_state_t -rcl_create_state(unsigned int index, char* label) -{ - rcl_state_t ret_state = {.index = index, .label = label}; - return ret_state; -} - -rcl_state_transition_t -rcl_create_state_transition(unsigned int index, const char* label) -{ - rcl_state_transition_t ret_transition = {{.index = index, .label = label}, NULL, NULL, NULL}; - return ret_transition; -} - -// default implementation as despicted on -// design.ros2.org -rcl_state_machine_t -rcl_get_default_state_machine() -{ - rcl_state_machine_t state_machine; - state_machine.transition_map.primary_states = NULL; - state_machine.transition_map.transition_arrays = NULL; - state_machine.transition_map.size = 0; - - // set default state as unconfigured - //state_machine.current_state = &rcl_state_unconfigured; - - rcl_state_transition_t rcl_transition_configuring - = rcl_create_state_transition(rcl_state_configuring.index, rcl_state_configuring.label); - rcl_state_transition_t rcl_transition_shuttingdown - = rcl_create_state_transition(rcl_state_shuttingdown.index, rcl_state_shuttingdown.label); - rcl_state_transition_t rcl_transition_cleaningup - = rcl_create_state_transition(rcl_state_cleaningup.index, rcl_state_cleaningup.label); - rcl_state_transition_t rcl_transition_activating - = rcl_create_state_transition(rcl_state_activating.index, rcl_state_activating.label); - rcl_state_transition_t rcl_transition_deactivating - = rcl_create_state_transition(rcl_state_deactivating.index, rcl_state_deactivating.label);; - - rcl_register_primary_state(&state_machine.transition_map, rcl_state_unconfigured); - rcl_register_primary_state(&state_machine.transition_map, rcl_state_inactive); - rcl_register_primary_state(&state_machine.transition_map, rcl_state_active); - rcl_register_primary_state(&state_machine.transition_map, rcl_state_finalized); - - // add transitions to map - rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_unconfigured, rcl_state_inactive, rcl_transition_configuring); - rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_inactive, rcl_state_unconfigured, rcl_transition_cleaningup); - rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_unconfigured, rcl_state_finalized, rcl_transition_shuttingdown); - rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_inactive, rcl_state_finalized, rcl_transition_shuttingdown); - rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_inactive, rcl_state_active, rcl_transition_activating); - rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_active, rcl_state_inactive, rcl_transition_deactivating); - rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_active, rcl_state_finalized, rcl_transition_shuttingdown); - - state_machine.current_state = &state_machine.transition_map.primary_states[0]; // set to first entry - return state_machine; -} - -#if __cplusplus -} -#endif // extern "C" - -#endif // RCL__STATE_MACHINE_H_ diff --git a/rclcpp_lifecycle/src/lifecycle_state.c b/rclcpp_lifecycle/src/lifecycle_state.c deleted file mode 100644 index a56444613d..0000000000 --- a/rclcpp_lifecycle/src/lifecycle_state.c +++ /dev/null @@ -1,118 +0,0 @@ -// 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. - -#if __cplusplus -extern "C" -{ -#endif - -#include -#include - -#include "rcl_lifecycle/lifecycle_state.h" - -const rcl_state_transition_t* -rcl_is_valid_transition(rcl_state_machine_t* state_machine, const rcl_state_t* goal_state) -{ - unsigned int current_index = state_machine->current_state->index; - const rcl_transition_array_t* valid_transitions = rcl_get_transitions_by_index(&state_machine->transition_map, current_index); - for (unsigned int i=0; isize; ++i) - { - if (valid_transitions->transitions[i].goal->index == goal_state->index) - { - return &valid_transitions->transitions[i]; - } - } - return NULL; -} - -const rcl_state_transition_t* -rcl_get_registered_transition_by_index(rcl_state_machine_t* state_machine, unsigned int transition_state_index) -{ - // extensive search approach - // TODO(karsten1987) can be improved by having a separate array for "registered transition" - const rcl_transition_map_t* map = &state_machine->transition_map; - for (unsigned int i=0; isize; ++i) - { - for (unsigned int j=0; jtransition_arrays[i].size; ++j) - { - if (map->transition_arrays[i].transitions[j].transition_state.index == transition_state_index) - { - return &map->transition_arrays[i].transitions[j]; - } - } - } - return NULL; -} - -const rcl_state_transition_t* -rcl_get_registered_transition_by_label(rcl_state_machine_t* state_machine, const char* transition_state_label) -{ - // extensive search approach - // TODO(karsten1987) can be improved by having a separate array for "registered transition" - const rcl_transition_map_t* map = &state_machine->transition_map; - for (unsigned int i=0; isize; ++i) - { - for (unsigned int j=0; jtransition_arrays[i].size; ++j) - { - if (strcmp(map->transition_arrays[i].transitions[j].transition_state.label, transition_state_label) == 0) - { - return &map->transition_arrays[i].transitions[j]; - } - } - } - return NULL; -} - -void -rcl_register_callback(rcl_state_machine_t* state_machine, unsigned int state_index, unsigned int transition_index, bool(*fcn)(void)) -{ - rcl_transition_array_t* transitions = rcl_get_transitions_by_index(&state_machine->transition_map, state_index); - for (unsigned int i=0; isize; ++i) - { - if (transitions->transitions[i].transition_state.index == transition_index) - { - transitions->transitions[i].callback = fcn; - } - } -} - -// maybe change directly the current state here, -// no need to that all the time inside high level language -bool -rcl_invoke_transition(rcl_state_machine_t* state_machine, rcl_state_t transition_index) -{ - unsigned int current_index = state_machine->current_state->index; - rcl_transition_array_t* transitions = rcl_get_transitions_by_index(&state_machine->transition_map, current_index); - - if (transitions == NULL) - { - return false; - } - bool success = false; - for (unsigned int i=0; isize; ++i) - { - if (transitions->transitions[i].transition_state.index == transition_index.index) - { - ((bool(*)(void))transitions->transitions[i].callback)(); - success = true; - // break here ?! would allow only one to one transitions - } - } - return success; -} - -#if __cplusplus -} -#endif // extern "C" diff --git a/rclcpp_lifecycle/src/lifecycle_talker.cpp b/rclcpp_lifecycle/src/lifecycle_talker.cpp deleted file mode 100644 index 5a6ba5415d..0000000000 --- a/rclcpp_lifecycle/src/lifecycle_talker.cpp +++ /dev/null @@ -1,87 +0,0 @@ -// 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 "rclcpp/rclcpp.hpp" -#include "rclcpp/publisher.hpp" - -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" - -#include "std_msgs/msg/string.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - std::shared_ptr lc_node = std::make_shared("lc_talker"); - - rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; - custom_qos_profile.depth = 7; - - std::shared_ptr> chatter_pub = - lc_node->create_publisher("chatter", custom_qos_profile); - - lc_node->print_state_machine(); - - if (!lc_node->configure()) - { - printf("Could not configure node. Going to error state.\n"); - } - - rclcpp::WallRate loop_rate(2); - - auto msg = std::make_shared(); - auto i = 1; - - while (rclcpp::ok() && i <= 10) { - msg->data = "Hello World: " + std::to_string(i++); - //std::cout << "Publishing: '" << msg->data << "'" << std::endl; - chatter_pub->publish(msg); - rclcpp::spin_some(lc_node); - loop_rate.sleep(); - } - - printf("Calling new activate\n"); - if (!lc_node->activate()) - { - return -1; - } - - while (rclcpp::ok() && i <= 20) { - msg->data = "Hello World: " + std::to_string(i++); - //std::cout << "Publishing: '" << msg->data << "'" << std::endl; - chatter_pub->publish(msg); - rclcpp::spin_some(lc_node); - loop_rate.sleep(); - } - - printf("Calling deactivate\n"); - if (!lc_node->deactivate()) - { - return -1; - } - - while (rclcpp::ok() && i <= 30) { - msg->data = "Hello World: " + std::to_string(i++); - //std::cout << "Publishing: '" << msg->data << "'" << std::endl; - chatter_pub->publish(msg); - rclcpp::spin_some(lc_node); - loop_rate.sleep(); - } - return 0; - -} diff --git a/rclcpp_lifecycle/src/transition_map.c b/rclcpp_lifecycle/src/transition_map.c deleted file mode 100644 index beecb8330c..0000000000 --- a/rclcpp_lifecycle/src/transition_map.c +++ /dev/null @@ -1,211 +0,0 @@ -// 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 -#include - -void rcl_register_primary_state(rcl_transition_map_t* m, rcl_state_t primary_state) -{ - if (rcl_get_primary_state_by_index(m, primary_state.index) != NULL) - { - // primary state is already registered - return; - } - - // add new primary state memory - //rcl_transition_map_index_t idx = {.index = m->size, .label = state.label}; - ++m->size; - m->primary_states = realloc(m->primary_states, m->size*sizeof(rcl_state_t)); - m->primary_states[m->size-1] = primary_state; - - // allocate empty pointer for transition_array_t - m->transition_arrays = realloc(m->transition_arrays, m->size*sizeof(rcl_transition_array_t)); - m->transition_arrays[m->size-1].transitions = NULL; - m->transition_arrays[m->size-1].size = 0; // initialize array to size 0 -} - -void rcl_register_transition_by_state(rcl_transition_map_t* m, rcl_state_t start, rcl_state_t goal, rcl_state_transition_t transition) -{ - rcl_register_transition_by_index(m, start.index, goal.index, transition); -} - -void rcl_register_transition_by_label(rcl_transition_map_t* m, const char* start_label, const char* goal_label, rcl_state_transition_t transition) -{ - // the idea here is to add this transition based on the - // start label and classify them. - rcl_state_t* start_state = rcl_get_primary_state_by_label(m, start_label); - if (start_state == NULL) - { - // return false here? - return; - } - rcl_state_t* goal_state = rcl_get_primary_state_by_label(m, goal_label); - if (goal_state == NULL) - { - // return false here? - return; - } - - transition.start = start_state; - transition.goal = goal_state; - - rcl_transition_array_t* transition_array = rcl_get_transitions_by_index(m, transition.start->index); - if (!transition_array) - { - return; - } - - // we add a new transition, so increase the size - ++transition_array->size; - - // TODO: Not sure if this is really necessary to differentiate - // between malloc and realloc - //if (transition_array->size == 1) - //{ - // transition_array->transitions = malloc(sizeof(rcl_state_transition_t)); - //} - //else - //{ - transition_array->transitions - = realloc(transition_array->transitions, transition_array->size*sizeof(rcl_state_transition_t)); - //} - - // finally set the new transition to the end of the array - transition_array->transitions[transition_array->size-1] = transition; -} - -void rcl_register_transition_by_index(rcl_transition_map_t* m, unsigned int start_index, unsigned int goal_index, rcl_state_transition_t transition) -{ - // the idea here is to add this transition based on the - // start label and classify them. - rcl_state_t* start_state = rcl_get_primary_state_by_index(m, start_index); - if (start_state == NULL) - { - // return false here? - return; - } - rcl_state_t* goal_state = rcl_get_primary_state_by_index(m, goal_index); - if (goal_state == NULL) - { - // return false here? - return; - } - - transition.start = start_state; - transition.goal = goal_state; - - rcl_transition_array_t* transition_array = rcl_get_transitions_by_index(m, transition.start->index); - if (!transition_array) - { - return; - } - - // we add a new transition, so increase the size - ++transition_array->size; - - // TODO: Not sure if this is really necessary to differentiate - // between malloc and realloc - //if (transition_array->size == 1) - //{ - // transition_array->transitions = malloc(sizeof(rcl_state_transition_t)); - //} - //else - //{ - transition_array->transitions - = realloc(transition_array->transitions, transition_array->size*sizeof(rcl_state_transition_t)); - //} - - // finally set the new transition to the end of the array - transition_array->transitions[transition_array->size-1] = transition; -} - -rcl_state_t* rcl_get_primary_state_by_label(rcl_transition_map_t* m, const char* label) -{ - for (unsigned int i=0; isize; ++i) - { - if (m->primary_states[i].label == label) - { - return &m->primary_states[i]; - } - } - return NULL; -} - -rcl_state_t* rcl_get_primary_state_by_index(rcl_transition_map_t* m, unsigned int index) -{ - for (unsigned int i=0; isize; ++i) - { - if (m->primary_states[i].index == index) - { - return &m->primary_states[i]; - } - } - return NULL; -} - -rcl_transition_array_t* rcl_get_transitions_by_label(rcl_transition_map_t* m, const char* label) -{ - for (unsigned int i=0; isize; ++i) - { - if (strcmp(m->primary_states[i].label, label) == 0) - { - return &m->transition_arrays[i]; - } - } - return NULL; -} - -rcl_transition_array_t* rcl_get_transitions_by_index(rcl_transition_map_t* m, unsigned int index) -{ - for (unsigned int i=0; isize; ++i) - { - if (m->primary_states[i].index == index ) - { - return &m->transition_arrays[i]; - } - } - return NULL; -} - -void rcl_print_transition_array(const rcl_transition_array_t* ta) -{ - for (unsigned int i=0; isize; ++i) - { - printf("%s(%u)(%s)\t", ta->transitions[i].transition_state.label, ta->transitions[i].transition_state.index, (ta->transitions[i].callback==NULL)?" ":"x"); - } - printf("\n"); -} - -void rcl_print_transition_map(const rcl_transition_map_t* m) -{ - printf("rcl_transition_map_t size %u\n", m->size); - for (unsigned int i=0; isize; ++i) - { - printf("Start state %s(%u) ::: ", m->primary_states[i].label, m->primary_states[i].index); - rcl_print_transition_array(&(m->transition_arrays[i])); - } -} - -#if __cplusplus -} -#endif From 9a5d1c0f5522357d3cad3f49bb2938cd4d19127e Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 7 Nov 2016 17:27:06 -0800 Subject: [PATCH 17/63] (dev) more helper functions for valid transition --- .../rcl_lifecycle/default_state_machine.h | 31 +-- .../include/rcl_lifecycle/lifecycle_state.h | 6 +- .../include/rcl_lifecycle/transition_map.h | 2 - .../src/rcl_lifecycle/default_state_machine.c | 90 ++++++++ .../src/rcl_lifecycle/lifecycle_state.c | 133 +++++++++++ .../src/rcl_lifecycle/transition_map.c | 211 ++++++++++++++++++ 6 files changed, 448 insertions(+), 25 deletions(-) create mode 100644 rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.c create mode 100644 rclcpp_lifecycle/src/rcl_lifecycle/lifecycle_state.c create mode 100644 rclcpp_lifecycle/src/rcl_lifecycle/transition_map.c diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h b/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h index 1818b9e5ab..cf43769adc 100644 --- a/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h +++ b/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h @@ -24,27 +24,16 @@ extern "C" // primary states based on // design.ros2.org/articles/node_lifecycle.html -rcl_state_t LIFECYCLE_EXPORT rcl_state_unconfigured - = {"unconfigured", 0}; -rcl_state_t LIFECYCLE_EXPORT rcl_state_inactive - = {"inactive", 1}; -rcl_state_t LIFECYCLE_EXPORT rcl_state_active - = {"active", 2}; -rcl_state_t LIFECYCLE_EXPORT rcl_state_finalized - = {"finalized", 3}; - -rcl_state_t LIFECYCLE_EXPORT rcl_state_configuring - = {"configuring", 4}; -rcl_state_t LIFECYCLE_EXPORT rcl_state_cleaningup - = {"cleaningup", 5}; -rcl_state_t LIFECYCLE_EXPORT rcl_state_shuttingdown - = {"shuttingdown", 6}; -rcl_state_t LIFECYCLE_EXPORT rcl_state_activating - = {"activating", 7}; -rcl_state_t LIFECYCLE_EXPORT rcl_state_deactivating - = {"deactivating", 8}; -rcl_state_t LIFECYCLE_EXPORT rcl_state_errorprocessing - = {"errorprocessing", 9}; +const rcl_state_t LIFECYCLE_EXPORT rcl_state_unconfigured = {"unconfigured", 0}; +const rcl_state_t LIFECYCLE_EXPORT rcl_state_inactive = {"inactive", 1}; +const rcl_state_t LIFECYCLE_EXPORT rcl_state_active = {"active", 2}; +const rcl_state_t LIFECYCLE_EXPORT rcl_state_finalized = {"finalized", 3}; +const rcl_state_t LIFECYCLE_EXPORT rcl_state_configuring = {"configuring", 4}; +const rcl_state_t LIFECYCLE_EXPORT rcl_state_cleaningup = {"cleaningup", 5}; +const rcl_state_t LIFECYCLE_EXPORT rcl_state_shuttingdown = {"shuttingdown", 6}; +const rcl_state_t LIFECYCLE_EXPORT rcl_state_activating = {"activating", 7}; +const rcl_state_t LIFECYCLE_EXPORT rcl_state_deactivating = {"deactivating", 8}; +const rcl_state_t LIFECYCLE_EXPORT rcl_state_errorprocessing = {"errorprocessing", 9}; #if __cplusplus } diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h b/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h index 2f45bb5d2d..e62cf4f065 100644 --- a/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h +++ b/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h @@ -70,7 +70,7 @@ typedef struct LIFECYCLE_EXPORT _rcl_state_transition_t */ typedef struct LIFECYCLE_EXPORT _rcl_state_machine_t { - rcl_state_t* current_state; + const rcl_state_t* current_state; rcl_transition_map_t transition_map; } rcl_state_machine_t; @@ -84,7 +84,9 @@ typedef struct LIFECYCLE_EXPORT _rcl_state_machine_t * NULL if not available */ const rcl_state_transition_t* -LIFECYCLE_EXPORT rcl_is_valid_transition(rcl_state_machine_t* state_machine, const rcl_state_t* goal_state); +LIFECYCLE_EXPORT rcl_is_valid_transition_by_index(rcl_state_machine_t* state_machine, unsigned int transition_index); +const rcl_state_transition_t* +LIFECYCLE_EXPORT rcl_is_valid_transition_by_label(rcl_state_machine_t* state_machine, const char* transition_label); const rcl_state_transition_t* LIFECYCLE_EXPORT rcl_get_registered_transition_by_index(rcl_state_machine_t* state_machine, unsigned int transition_state_index); diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h b/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h index 49e6f4d160..841a634424 100644 --- a/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h +++ b/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h @@ -16,8 +16,6 @@ #ifndef RCL__TRANSITION_MAP_H_ #define RCL__TRANSITION_MAP_H_ -#include - #if __cplusplus extern "C" { diff --git a/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.c b/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.c new file mode 100644 index 0000000000..d5bd9f68ca --- /dev/null +++ b/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.c @@ -0,0 +1,90 @@ +// 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 RCL__STATE_MACHINE_H_ +#define RCL__STATE_MACHINE_H_ + +#include +#include +#include + +#include "rcl_lifecycle/lifecycle_state.h" +#include "rcl_lifecycle/default_state_machine.h" + +#if __cplusplus +extern "C" +{ +#endif + +rcl_state_t +rcl_create_state(unsigned int index, char* label) +{ + rcl_state_t ret_state = {.index = index, .label = label}; + return ret_state; +} + +rcl_state_transition_t +rcl_create_state_transition(unsigned int index, const char* label) +{ + rcl_state_transition_t ret_transition = {{.index = index, .label = label}, NULL, NULL, NULL}; + return ret_transition; +} + +// default implementation as despicted on +// design.ros2.org +rcl_state_machine_t +rcl_get_default_state_machine() +{ + rcl_state_machine_t state_machine; + state_machine.transition_map.primary_states = NULL; + state_machine.transition_map.transition_arrays = NULL; + state_machine.transition_map.size = 0; + + // set default state as unconfigured + //state_machine.current_state = &rcl_state_unconfigured; + + rcl_state_transition_t rcl_transition_configuring + = rcl_create_state_transition(rcl_state_configuring.index, rcl_state_configuring.label); + rcl_state_transition_t rcl_transition_shuttingdown + = rcl_create_state_transition(rcl_state_shuttingdown.index, rcl_state_shuttingdown.label); + rcl_state_transition_t rcl_transition_cleaningup + = rcl_create_state_transition(rcl_state_cleaningup.index, rcl_state_cleaningup.label); + rcl_state_transition_t rcl_transition_activating + = rcl_create_state_transition(rcl_state_activating.index, rcl_state_activating.label); + rcl_state_transition_t rcl_transition_deactivating + = rcl_create_state_transition(rcl_state_deactivating.index, rcl_state_deactivating.label);; + + rcl_register_primary_state(&state_machine.transition_map, rcl_state_unconfigured); + rcl_register_primary_state(&state_machine.transition_map, rcl_state_inactive); + rcl_register_primary_state(&state_machine.transition_map, rcl_state_active); + rcl_register_primary_state(&state_machine.transition_map, rcl_state_finalized); + + // add transitions to map + rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_unconfigured, rcl_state_inactive, rcl_transition_configuring); + rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_inactive, rcl_state_unconfigured, rcl_transition_cleaningup); + rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_unconfigured, rcl_state_finalized, rcl_transition_shuttingdown); + rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_inactive, rcl_state_finalized, rcl_transition_shuttingdown); + rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_inactive, rcl_state_active, rcl_transition_activating); + rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_active, rcl_state_inactive, rcl_transition_deactivating); + rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_active, rcl_state_finalized, rcl_transition_shuttingdown); + + state_machine.current_state = &state_machine.transition_map.primary_states[0]; // set to first entry + return state_machine; +} + +#if __cplusplus +} +#endif // extern "C" + +#endif // RCL__STATE_MACHINE_H_ diff --git a/rclcpp_lifecycle/src/rcl_lifecycle/lifecycle_state.c b/rclcpp_lifecycle/src/rcl_lifecycle/lifecycle_state.c new file mode 100644 index 0000000000..279b8a5ca7 --- /dev/null +++ b/rclcpp_lifecycle/src/rcl_lifecycle/lifecycle_state.c @@ -0,0 +1,133 @@ +// 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. + +#if __cplusplus +extern "C" +{ +#endif + +#include +#include + +#include "rcl_lifecycle/lifecycle_state.h" + +const rcl_state_transition_t* +rcl_is_valid_transition_by_index(rcl_state_machine_t* state_machine, unsigned int transition_index) +{ + unsigned int current_index = state_machine->current_state->index; + const rcl_transition_array_t* valid_transitions = rcl_get_transitions_by_index(&state_machine->transition_map, current_index); + for (unsigned int i=0; isize; ++i) + { + if (valid_transitions->transitions[i].transition_state.index == transition_index) + { + return &valid_transitions->transitions[i]; + } + } + return NULL; +} + +const rcl_state_transition_t* +rcl_is_valid_transition_by_label(rcl_state_machine_t* state_machine, const char* transition_label) +{ + unsigned int current_index = state_machine->current_state->index; + const rcl_transition_array_t* valid_transitions = rcl_get_transitions_by_index(&state_machine->transition_map, current_index); + for (unsigned int i=0; isize; ++i) + { + if (valid_transitions->transitions[i].transition_state.label == transition_label) + { + return &valid_transitions->transitions[i]; + } + } + return NULL; +} + +const rcl_state_transition_t* +rcl_get_registered_transition_by_index(rcl_state_machine_t* state_machine, unsigned int transition_state_index) +{ + // extensive search approach + // TODO(karsten1987) can be improved by having a separate array for "registered transition" + const rcl_transition_map_t* map = &state_machine->transition_map; + for (unsigned int i=0; isize; ++i) + { + for (unsigned int j=0; jtransition_arrays[i].size; ++j) + { + if (map->transition_arrays[i].transitions[j].transition_state.index == transition_state_index) + { + return &map->transition_arrays[i].transitions[j]; + } + } + } + return NULL; +} + +const rcl_state_transition_t* +rcl_get_registered_transition_by_label(rcl_state_machine_t* state_machine, const char* transition_state_label) +{ + // extensive search approach + // TODO(karsten1987) can be improved by having a separate array for "registered transition" + const rcl_transition_map_t* map = &state_machine->transition_map; + for (unsigned int i=0; isize; ++i) + { + for (unsigned int j=0; jtransition_arrays[i].size; ++j) + { + if (strcmp(map->transition_arrays[i].transitions[j].transition_state.label, transition_state_label) == 0) + { + return &map->transition_arrays[i].transitions[j]; + } + } + } + return NULL; +} + +void +rcl_register_callback(rcl_state_machine_t* state_machine, unsigned int state_index, unsigned int transition_index, bool(*fcn)(void)) +{ + rcl_transition_array_t* transitions = rcl_get_transitions_by_index(&state_machine->transition_map, state_index); + for (unsigned int i=0; isize; ++i) + { + if (transitions->transitions[i].transition_state.index == transition_index) + { + transitions->transitions[i].callback = fcn; + } + } +} + +// maybe change directly the current state here, +// no need to that all the time inside high level language +bool +rcl_invoke_transition(rcl_state_machine_t* state_machine, rcl_state_t transition_index) +{ + unsigned int current_index = state_machine->current_state->index; + rcl_transition_array_t* transitions = rcl_get_transitions_by_index(&state_machine->transition_map, current_index); + + if (transitions == NULL) + { + return false; + } + bool success = false; + for (unsigned int i=0; isize; ++i) + { + if (transitions->transitions[i].transition_state.index == transition_index.index) + { + ((bool(*)(void))transitions->transitions[i].callback)(); + success = true; + // break here ?! would allow only one to one transitions + } + } + return success; +} + +#if __cplusplus +} +#endif // extern "C" diff --git a/rclcpp_lifecycle/src/rcl_lifecycle/transition_map.c b/rclcpp_lifecycle/src/rcl_lifecycle/transition_map.c new file mode 100644 index 0000000000..beecb8330c --- /dev/null +++ b/rclcpp_lifecycle/src/rcl_lifecycle/transition_map.c @@ -0,0 +1,211 @@ +// 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 +#include + +void rcl_register_primary_state(rcl_transition_map_t* m, rcl_state_t primary_state) +{ + if (rcl_get_primary_state_by_index(m, primary_state.index) != NULL) + { + // primary state is already registered + return; + } + + // add new primary state memory + //rcl_transition_map_index_t idx = {.index = m->size, .label = state.label}; + ++m->size; + m->primary_states = realloc(m->primary_states, m->size*sizeof(rcl_state_t)); + m->primary_states[m->size-1] = primary_state; + + // allocate empty pointer for transition_array_t + m->transition_arrays = realloc(m->transition_arrays, m->size*sizeof(rcl_transition_array_t)); + m->transition_arrays[m->size-1].transitions = NULL; + m->transition_arrays[m->size-1].size = 0; // initialize array to size 0 +} + +void rcl_register_transition_by_state(rcl_transition_map_t* m, rcl_state_t start, rcl_state_t goal, rcl_state_transition_t transition) +{ + rcl_register_transition_by_index(m, start.index, goal.index, transition); +} + +void rcl_register_transition_by_label(rcl_transition_map_t* m, const char* start_label, const char* goal_label, rcl_state_transition_t transition) +{ + // the idea here is to add this transition based on the + // start label and classify them. + rcl_state_t* start_state = rcl_get_primary_state_by_label(m, start_label); + if (start_state == NULL) + { + // return false here? + return; + } + rcl_state_t* goal_state = rcl_get_primary_state_by_label(m, goal_label); + if (goal_state == NULL) + { + // return false here? + return; + } + + transition.start = start_state; + transition.goal = goal_state; + + rcl_transition_array_t* transition_array = rcl_get_transitions_by_index(m, transition.start->index); + if (!transition_array) + { + return; + } + + // we add a new transition, so increase the size + ++transition_array->size; + + // TODO: Not sure if this is really necessary to differentiate + // between malloc and realloc + //if (transition_array->size == 1) + //{ + // transition_array->transitions = malloc(sizeof(rcl_state_transition_t)); + //} + //else + //{ + transition_array->transitions + = realloc(transition_array->transitions, transition_array->size*sizeof(rcl_state_transition_t)); + //} + + // finally set the new transition to the end of the array + transition_array->transitions[transition_array->size-1] = transition; +} + +void rcl_register_transition_by_index(rcl_transition_map_t* m, unsigned int start_index, unsigned int goal_index, rcl_state_transition_t transition) +{ + // the idea here is to add this transition based on the + // start label and classify them. + rcl_state_t* start_state = rcl_get_primary_state_by_index(m, start_index); + if (start_state == NULL) + { + // return false here? + return; + } + rcl_state_t* goal_state = rcl_get_primary_state_by_index(m, goal_index); + if (goal_state == NULL) + { + // return false here? + return; + } + + transition.start = start_state; + transition.goal = goal_state; + + rcl_transition_array_t* transition_array = rcl_get_transitions_by_index(m, transition.start->index); + if (!transition_array) + { + return; + } + + // we add a new transition, so increase the size + ++transition_array->size; + + // TODO: Not sure if this is really necessary to differentiate + // between malloc and realloc + //if (transition_array->size == 1) + //{ + // transition_array->transitions = malloc(sizeof(rcl_state_transition_t)); + //} + //else + //{ + transition_array->transitions + = realloc(transition_array->transitions, transition_array->size*sizeof(rcl_state_transition_t)); + //} + + // finally set the new transition to the end of the array + transition_array->transitions[transition_array->size-1] = transition; +} + +rcl_state_t* rcl_get_primary_state_by_label(rcl_transition_map_t* m, const char* label) +{ + for (unsigned int i=0; isize; ++i) + { + if (m->primary_states[i].label == label) + { + return &m->primary_states[i]; + } + } + return NULL; +} + +rcl_state_t* rcl_get_primary_state_by_index(rcl_transition_map_t* m, unsigned int index) +{ + for (unsigned int i=0; isize; ++i) + { + if (m->primary_states[i].index == index) + { + return &m->primary_states[i]; + } + } + return NULL; +} + +rcl_transition_array_t* rcl_get_transitions_by_label(rcl_transition_map_t* m, const char* label) +{ + for (unsigned int i=0; isize; ++i) + { + if (strcmp(m->primary_states[i].label, label) == 0) + { + return &m->transition_arrays[i]; + } + } + return NULL; +} + +rcl_transition_array_t* rcl_get_transitions_by_index(rcl_transition_map_t* m, unsigned int index) +{ + for (unsigned int i=0; isize; ++i) + { + if (m->primary_states[i].index == index ) + { + return &m->transition_arrays[i]; + } + } + return NULL; +} + +void rcl_print_transition_array(const rcl_transition_array_t* ta) +{ + for (unsigned int i=0; isize; ++i) + { + printf("%s(%u)(%s)\t", ta->transitions[i].transition_state.label, ta->transitions[i].transition_state.index, (ta->transitions[i].callback==NULL)?" ":"x"); + } + printf("\n"); +} + +void rcl_print_transition_map(const rcl_transition_map_t* m) +{ + printf("rcl_transition_map_t size %u\n", m->size); + for (unsigned int i=0; isize; ++i) + { + printf("Start state %s(%u) ::: ", m->primary_states[i].label, m->primary_states[i].index); + rcl_print_transition_array(&(m->transition_arrays[i])); + } +} + +#if __cplusplus +} +#endif From d647125ee820d86609f11f13e72f078af8b12892 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 7 Nov 2016 17:27:24 -0800 Subject: [PATCH 18/63] (dev) pimpl implementation for cpp lifecyclemanager --- .../rclcpp_lifecycle/lifecycle_manager.hpp | 103 +++++++++++ .../rclcpp_lifecycle/lifecycle_node.hpp | 170 ++---------------- .../rclcpp_lifecycle/lifecycle_manager.cpp | 76 ++++++++ .../lifecycle_manager_impl.hpp | 139 ++++++++++++++ .../src/rclcpp_lifecycle/lifecycle_talker.cpp | 86 +++++++++ 5 files changed, 418 insertions(+), 156 deletions(-) create mode 100644 rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp create mode 100644 rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp create mode 100644 rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp create mode 100644 rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_talker.cpp diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp new file mode 100644 index 0000000000..6e1edb7139 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp @@ -0,0 +1,103 @@ +// 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 RCLCPP__LIFECYCLE_MANAGER_H_ +#define RCLCPP__LIFECYCLE_MANAGER_H_ + +#include +#include + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +// forward declaration for c-struct +struct _rcl_state_machine_t; +typedef _rcl_state_machine_t rcl_state_machine_t; + +namespace rclcpp +{ +namespace lifecycle +{ + +enum class LifecyclePrimaryStatesT : std::uint8_t +{ + UNCONFIGURED = 0, + INACTIVE = 1, + ACTIVE = 2, + FINALIZED = 3 +}; + +enum class LifecycleTransitionsT : std::uint8_t +{ + CONFIGURING = 4, + CLEANINGUP = 5, + SHUTTINGDOWN = 6, + ACTIVATING = 7, + DEACTIVATING = 8, + ERRORPROCESSING = 9 +}; + +class LifecycleManagerImpl; + +class LifecycleManagerInterface +{ + virtual bool configure(const std::string& node_name = "") = 0; + virtual bool cleanup(const std::string& node_name = "") = 0; + virtual bool shutdown(const std::string& node_name = "") = 0; + virtual bool activate(const std::string& node_name = "") = 0; + virtual bool deactivate(const std::string& node_name = "") = 0; +}; + +class LifecycleManager : public LifecycleManagerInterface +{ +public: + LifecycleManager(); + ~LifecycleManager(); + + LIFECYCLE_EXPORT + void + add_node_interface(const std::shared_ptr& node_interface); + + LIFECYCLE_EXPORT + void + add_node_interface(const std::shared_ptr& node_interface, rcl_state_machine_t custom_state_machine); + + LIFECYCLE_EXPORT + bool + configure(const std::string& node_name = ""); + + LIFECYCLE_EXPORT + bool + cleanup(const std::string& node_name = ""); + + LIFECYCLE_EXPORT + bool + shutdown(const std::string& node_name = ""); + + LIFECYCLE_EXPORT + bool + activate(const std::string& node_name = ""); + + LIFECYCLE_EXPORT + bool + deactivate(const std::string& node_name = ""); + +private: + class LifecycleManagerImpl; + std::unique_ptr impl_; + //LifecycleManagerImpl* impl_; +}; + +} // namespace lifecycle +} // namespace rclcpp +#endif diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 22d94af5e0..2732776b03 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -18,16 +18,14 @@ #include "rclcpp/node.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" -#include "rcl_lifecycle/lifecycle_state.h" -#include "rcl_lifecycle/default_state_machine.h" -#include "rcl_lifecycle/transition_map.h" +#include namespace rclcpp { namespace node { -namespace lifecycle_interface +namespace lifecycle { template struct Callback; @@ -49,157 +47,30 @@ std::function Callback> callback_map; - rcl_state_machine_t state_machine_; - - protected: +public: virtual bool on_configure() { return true; }; virtual bool on_cleanup() { return true; }; virtual bool on_shutdown() { return true; }; virtual bool on_activate() { return true; }; virtual bool on_deactivate() { return true; }; virtual bool on_error() { return true; }; - - /** - * @brief get the default state machine - * as defined on design.ros.org - */ - LIFECYCLE_EXPORT - virtual void - setup_state_machine() - { - state_machine_ = rcl_get_default_state_machine(); - } - - // In case with want to register the callbacks directly in c - /* - LIFECYCLE_EXPORT - template - void - register_state_callback(bool (T::*method)(), T* instance) - { - Callback::func = std::bind(method, instance); - bool(*c_function_pointer)(void) = static_cast(Callback::callback); - rcl_register_callback(&state_machine_, (unsigned int)State_Index, (unsigned int)Transition_Index, c_function_pointer); - } - */ - - LIFECYCLE_EXPORT - template - void - register_transition_callback(bool (T::*method)(), T* instance, size_t transition_state_index) - { - const rcl_state_transition_t* transition_state - = rcl_get_registered_transition_by_index(&state_machine_, transition_state_index); - if (!transition_state) - { - // TODO do something smarter here - throw std::runtime_error("Transition is not valid"); - } - callback_map[transition_state_index] = std::bind(method, instance); - } - - LIFECYCLE_EXPORT - template - void - register_transition_callback(bool (T::*method)(), T* instance, const std::string& transition_state_label) - { - const rcl_state_transition_t* transition_state - = rcl_get_registered_transition_by_label(&state_machine_, transition_state_label.c_str()); - if (!transition_state) - { - // TODO do something smarter here - throw std::runtime_error("Transition is not valid"); - } - callback_map[transition_state->transition_state.index] = std::bind(method, instance); - } - - public: - LIFECYCLE_EXPORT - void - print_state_machine() - { - printf("current state is %s\n", state_machine_.current_state->label); - rcl_print_transition_map(&state_machine_.transition_map); - } - -public: - //virtual bool create() = 0; - virtual bool configure() - { - if (state_machine_.current_state->index == rcl_state_unconfigured.index) - { - // given the current state machine, specify a transition and go for it - //auto ret = rcl_invoke_transition(&state_machine_, rcl_state_configuring); - auto cb = callback_map[rcl_state_configuring.index]; - auto ret = cb(); - printf("%s\n", (ret)?"Callback was successful":"Callback unsuccessful"); - // change state here to "Configuring" - //if (on_configure()) - //{ - // state_machine_.current_state = &rcl_state_inactive; - // return true; - //} - } - // everything else is considered wrong - //state_machine_.current_state = rcl_state_error; - return false; - } - - virtual bool cleanup() - { - return on_cleanup(); - } - - virtual bool shutdown() - { - return on_shutdown(); - } - - virtual bool activate() - { - if (state_machine_.current_state->index == rcl_state_inactive.index) - { - // change state here to "Activating" - if (on_activate()) - { - state_machine_.current_state = &rcl_state_active; - return true; - } - } - // everything else is considered wrong - //state_machine_.current_state = rcl_state_error; - return false; - } - - virtual bool deactivate() + // hardcoded mock + // as we don't have a node base class yet + virtual std::string get_name() { - if (state_machine_.current_state->index == rcl_state_active.index) - { - // change state here to "Activating" - if (on_deactivate()) - { - state_machine_.current_state = &rcl_state_inactive; - return true; - } - } - // everything else is considered wrong - //state_machine_.current_state = rcl_state_error; - return false; + static auto counter = 0; + std::string tmp_name = "my_node"+std::to_string(++counter); + return tmp_name; } - //virtual bool destroy() = 0; }; -} // namespace lifecycle_interface - -#include /** * @brief LifecycleNode as child class of rclcpp Node * has lifecycle nodeinterface for configuring this node. */ -class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::NodeInterface +class LifecycleNode : public rclcpp::node::Node, public lifecycle::LifecycleNodeInterface { public: @@ -207,25 +78,11 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod explicit LifecycleNode(const std::string & node_name, bool use_intra_process_comms = false) : Node(node_name, use_intra_process_comms) { - setup_state_machine(); } LIFECYCLE_EXPORT ~LifecycleNode(){} - /** - * @brief get the default state machine - * as defined on design.ros.org - */ - LIFECYCLE_EXPORT - virtual void - setup_state_machine() - { - lifecycle_interface::NodeInterface::setup_state_machine(); - - register_transition_callback(&LifecycleNode::on_configure, this, rcl_state_configuring.index); - } - /** * @brief same API for creating publisher as regular Node */ @@ -322,7 +179,8 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle_interface::Nod }; -} // namespace node -} // namespace rclcpp +} // namespace lifecycle_interface +} // namespace node +} // namespace rclcpp #endif diff --git a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp new file mode 100644 index 0000000000..9d3def39e0 --- /dev/null +++ b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp @@ -0,0 +1,76 @@ +// 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 "lifecycle_manager_impl.hpp" // implementation + +namespace rclcpp +{ +namespace lifecycle +{ + +using NodeInterfacePtr = std::shared_ptr; + +LifecycleManager::LifecycleManager(): + impl_(new LifecycleManagerImpl()) +{} + + +LifecycleManager::~LifecycleManager() = default; + +void +LifecycleManager::add_node_interface(const NodeInterfacePtr& node_interface) +{ + impl_->add_node_interface(node_interface); +} + +void +LifecycleManager::add_node_interface(const NodeInterfacePtr& node_interface, rcl_state_machine_t custom_state_machine) +{ + impl_->add_node_interface(node_interface, custom_state_machine); +} + +bool +LifecycleManager::configure(const std::string& node_name) +{ + return impl_->change_state(node_name); +} + +bool +LifecycleManager::cleanup(const std::string& node_name) +{ + return impl_->change_state(node_name); +} + +bool +LifecycleManager::shutdown(const std::string& node_name) +{ + return impl_->change_state(node_name); +} + +bool +LifecycleManager::activate(const std::string& node_name) +{ + return impl_->change_state(node_name); +} + +bool +LifecycleManager::deactivate(const std::string& node_name) +{ + return impl_->change_state(node_name); +} + +} // namespace lifecycle +} // namespace rclcpp diff --git a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp new file mode 100644 index 0000000000..6651bc9e76 --- /dev/null +++ b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp @@ -0,0 +1,139 @@ +// 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 RCLCPP__LIFECYCLE_MANAGER_IMPL_H_ +#define RCLCPP__LIFECYCLE_MANAGER_IMPL_H_ + +#include +#include +#include + +#include +#include + +namespace rclcpp +{ +namespace lifecycle +{ + +using NodeInterfacePtr = std::shared_ptr; +using NodeInterfaceWeakPtr = std::weak_ptr; + +struct NodeStateMachine +{ + NodeInterfaceWeakPtr weak_node_handle; + rcl_state_machine_t state_machine; + std::map> cb_map; +}; + +namespace +{ + template + typename MapT::iterator is_registered(MapT& state_machine_map, const std::string& node_name) + { + for (auto map_iter=state_machine_map.begin(); map_iter!=state_machine_map.end(); ++map_iter) + { + if (map_iter->first == node_name) + return map_iter; + } + return state_machine_map.end(); + } +} // namespace + +class LifecycleManager::LifecycleManagerImpl +{ +public: + LifecycleManagerImpl() = default; + ~LifecycleManagerImpl() = default; + + LIFECYCLE_EXPORT + void + add_node_interface(const NodeInterfacePtr& node_interface) + { + rcl_state_machine_t state_machine = rcl_get_default_state_machine(); + add_node_interface(node_interface, state_machine); + } + + LIFECYCLE_EXPORT + void + add_node_interface(const NodeInterfacePtr& node_interface, rcl_state_machine_t custom_state_machine) + { + NodeStateMachine node_state_machine; + node_state_machine.weak_node_handle = node_interface; + // TODO(karsten1987): Find a way to make this generic to an enduser + node_state_machine.state_machine = custom_state_machine; + + // register default callbacks + // maybe optional + using NodeInterface = node::lifecycle::LifecycleNodeInterface; + std::function cb_configuring = std::bind(&NodeInterface::on_configure, node_interface); + std::function cb_cleaningup = std::bind(&NodeInterface::on_cleanup, node_interface); + std::function cb_shuttingdown = std::bind(&NodeInterface::on_shutdown, node_interface); + std::function cb_activating = std::bind(&NodeInterface::on_activate, node_interface); + std::function cb_deactivating = std::bind(&NodeInterface::on_deactivate, node_interface); + std::function cb_error = std::bind(&NodeInterface::on_error, node_interface); + node_state_machine.cb_map[LifecycleTransitionsT::CONFIGURING] = cb_configuring; + node_state_machine.cb_map[LifecycleTransitionsT::CLEANINGUP] = cb_cleaningup; + node_state_machine.cb_map[LifecycleTransitionsT::SHUTTINGDOWN] = cb_shuttingdown; + node_state_machine.cb_map[LifecycleTransitionsT::ACTIVATING] = cb_activating; + node_state_machine.cb_map[LifecycleTransitionsT::DEACTIVATING] = cb_deactivating; + node_state_machine.cb_map[LifecycleTransitionsT::ERRORPROCESSING] = cb_error; + + // TODO(karsten1987): clarify what do if node already exists + auto node_name = node_interface->get_name(); + node_handle_map_[node_name] = node_state_machine; + } + + template + bool + change_state(const std::string& node_name = "") + { + if (node_name.empty()) + return false; + + auto node_handle_iter = is_registered(node_handle_map_, node_name); + if (node_handle_iter == node_handle_map_.end()) + { + fprintf(stderr, "Node with name %s is not registered\n", node_name.c_str()); + return false; + } + + auto node_handle = node_handle_iter->second.weak_node_handle.lock(); + if (!node_handle) + return false; + + // ask RCL if this is a valid state + const rcl_state_transition_t* transition + = rcl_is_valid_transition_by_index(&node_handle_iter->second.state_machine, static_cast(lifecycle_transition)); + if (transition == NULL) + return false; + + std::function callback = node_handle_iter->second.cb_map[lifecycle_transition]; + if(!callback()) + { + node_handle->on_error(); + node_handle_iter->second.state_machine.current_state = &rcl_state_errorprocessing; + return false; + } + node_handle_iter->second.state_machine.current_state = transition->goal; + return true; + } + +private: + std::map node_handle_map_; +}; + +} // namespace lifecycle +} // namespace rclcpp +#endif diff --git a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_talker.cpp b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_talker.cpp new file mode 100644 index 0000000000..382b7d39fe --- /dev/null +++ b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_talker.cpp @@ -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. + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/publisher.hpp" + +#include "rclcpp_lifecycle/lifecycle_manager.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" + +#include "std_msgs/msg/string.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + std::shared_ptr lc_node = std::make_shared("lc_talker"); + + rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; + custom_qos_profile.depth = 7; + + std::shared_ptr> chatter_pub = + lc_node->create_publisher("chatter", custom_qos_profile); + + rclcpp::lifecycle::LifecycleManager lm; + lm.add_node_interface(lc_node); + + // configure + lm.configure("my_node1"); + + rclcpp::WallRate loop_rate(2); + + auto msg = std::make_shared(); + auto i = 1; + + while (rclcpp::ok() && i <= 10) { + msg->data = "Hello World: " + std::to_string(i++); + //std::cout << "Publishing: '" << msg->data << "'" << std::endl; + chatter_pub->publish(msg); + rclcpp::spin_some(lc_node); + loop_rate.sleep(); + } + + printf("Calling new activate\n"); + if (!lm.activate("my_node1")) + { + return -1; + } + + while (rclcpp::ok() && i <= 20) { + msg->data = "Hello World: " + std::to_string(i++); + //std::cout << "Publishing: '" << msg->data << "'" << std::endl; + chatter_pub->publish(msg); + rclcpp::spin_some(lc_node); + loop_rate.sleep(); + } + + printf("Calling deactivate\n"); + if (!lm.deactivate("my_node1")) + { + return -1; + } + + while (rclcpp::ok() && i <= 30) { + msg->data = "Hello World: " + std::to_string(i++); + //std::cout << "Publishing: '" << msg->data << "'" << std::endl; + chatter_pub->publish(msg); + rclcpp::spin_some(lc_node); + loop_rate.sleep(); + } + return 0; +} From 1fac6c664e3c0c4b6cade8a95a482d35b76ffbec Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 8 Nov 2016 11:05:20 -0800 Subject: [PATCH 19/63] (dev) register non-default callback functions --- .../rclcpp_lifecycle/lifecycle_manager.hpp | 65 +++++++++++++++++++ .../rclcpp_lifecycle/lifecycle_manager.cpp | 30 +++++++++ .../lifecycle_manager_impl.hpp | 32 ++++----- 3 files changed, 112 insertions(+), 15 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp index 6e1edb7139..f6eda75cb3 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp @@ -72,22 +72,87 @@ class LifecycleManager : public LifecycleManagerInterface void add_node_interface(const std::shared_ptr& node_interface, rcl_state_machine_t custom_state_machine); + LIFECYCLE_EXPORT + template + bool + register_on_configure(const std::string& node_name, bool(T::*method)(void), T* instance) + { + auto cb = std::bind(method, instance); + return register_on_configure(node_name, cb); + } + + LIFECYCLE_EXPORT + bool + register_on_configure(const std::string& node_name, std::function& fcn); + LIFECYCLE_EXPORT bool configure(const std::string& node_name = ""); + LIFECYCLE_EXPORT + template + bool + register_on_cleanup(const std::string& node_name, bool(T::*method)(void), T* instance) + { + auto cb = std::bind(method, instance); + return register_on_cleanup(node_name, cb); + } + + LIFECYCLE_EXPORT + bool + register_on_cleanup(const std::string& node_name, std::function& fcn); + LIFECYCLE_EXPORT bool cleanup(const std::string& node_name = ""); + LIFECYCLE_EXPORT + template + bool + register_on_shutdown(const std::string& node_name, bool(T::*method)(void), T* instance) + { + auto cb = std::bind(method, instance); + return register_on_shutdown(node_name, cb); + } + + LIFECYCLE_EXPORT + bool + register_on_shutdown(const std::string& node_name, std::function& fcn); + LIFECYCLE_EXPORT bool shutdown(const std::string& node_name = ""); + LIFECYCLE_EXPORT + template + bool + register_on_activate(const std::string& node_name, bool(T::*method)(void), T* instance) + { + auto cb = std::bind(method, instance); + return register_on_activate(node_name, cb); + } + + LIFECYCLE_EXPORT + bool + register_on_activate(const std::string& node_name, std::function& fcn); + LIFECYCLE_EXPORT bool activate(const std::string& node_name = ""); + LIFECYCLE_EXPORT + template + bool + register_on_deactivate(const std::string& node_name, bool(T::*method)(void), T* instance) + { + auto cb = std::bind(method, instance); + return register_on_deactivate(node_name, cb); + } + + LIFECYCLE_EXPORT + bool + register_on_deactivate(const std::string& node_name, std::function& fcn); + LIFECYCLE_EXPORT bool deactivate(const std::string& node_name = ""); diff --git a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp index 9d3def39e0..34d61407e2 100644 --- a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp +++ b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp @@ -42,30 +42,60 @@ LifecycleManager::add_node_interface(const NodeInterfacePtr& node_interface, rcl impl_->add_node_interface(node_interface, custom_state_machine); } +bool +LifecycleManager::register_on_configure(const std::string& node_name, std::function& fcn) +{ + return impl_->register_callback(node_name, fcn); +} + bool LifecycleManager::configure(const std::string& node_name) { return impl_->change_state(node_name); } +bool +LifecycleManager::register_on_cleanup(const std::string& node_name, std::function& fcn) +{ + return impl_->register_callback(node_name, fcn); +} + bool LifecycleManager::cleanup(const std::string& node_name) { return impl_->change_state(node_name); } +bool +LifecycleManager::register_on_shutdown(const std::string& node_name, std::function& fcn) +{ + return impl_->register_callback(node_name, fcn); +} + bool LifecycleManager::shutdown(const std::string& node_name) { return impl_->change_state(node_name); } +bool +LifecycleManager::register_on_activate(const std::string& node_name, std::function& fcn) +{ + return impl_->register_callback(node_name, fcn); +} + bool LifecycleManager::activate(const std::string& node_name) { return impl_->change_state(node_name); } +bool +LifecycleManager::register_on_deactivate(const std::string& node_name, std::function& fcn) +{ + return impl_->register_callback(node_name, fcn); +} + bool LifecycleManager::deactivate(const std::string& node_name) { diff --git a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp index 6651bc9e76..0e5e9e5ee2 100644 --- a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp +++ b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp @@ -37,20 +37,6 @@ struct NodeStateMachine std::map> cb_map; }; -namespace -{ - template - typename MapT::iterator is_registered(MapT& state_machine_map, const std::string& node_name) - { - for (auto map_iter=state_machine_map.begin(); map_iter!=state_machine_map.end(); ++map_iter) - { - if (map_iter->first == node_name) - return map_iter; - } - return state_machine_map.end(); - } -} // namespace - class LifecycleManager::LifecycleManagerImpl { public: @@ -95,6 +81,22 @@ class LifecycleManager::LifecycleManagerImpl node_handle_map_[node_name] = node_state_machine; } + template + bool + register_callback(const std::string& node_name, std::function& cb) + { + if (node_name.empty()) + return false; + + auto node_handle_iter = node_handle_map_.find(node_name); + if (node_handle_iter == node_handle_map_.end()) + { + fprintf(stderr, "Node with name %s is not registered\n", node_name.c_str()); + } + node_handle_iter->second.cb_map[lifecycle_transition] = cb; + return true; + } + template bool change_state(const std::string& node_name = "") @@ -102,7 +104,7 @@ class LifecycleManager::LifecycleManagerImpl if (node_name.empty()) return false; - auto node_handle_iter = is_registered(node_handle_map_, node_name); + auto node_handle_iter = node_handle_map_.find(node_name); if (node_handle_iter == node_handle_map_.end()) { fprintf(stderr, "Node with name %s is not registered\n", node_name.c_str()); From 794a131c55405d26e9dd30c8caaa5b31bd2ed290 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 8 Nov 2016 15:01:03 -0800 Subject: [PATCH 20/63] (dev) cleanup lifecycle node to serve as base class --- .../rclcpp_lifecycle/lifecycle_node.hpp | 75 +++++-------------- 1 file changed, 18 insertions(+), 57 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 2732776b03..e936854113 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -73,6 +73,7 @@ class LifecycleNodeInterface class LifecycleNode : public rclcpp::node::Node, public lifecycle::LifecycleNodeInterface { public: + using LifecyclePublisherWeakPtr = std::weak_ptr; LIFECYCLE_EXPORT explicit LifecycleNode(const std::string & node_name, bool use_intra_process_comms = false) : @@ -88,7 +89,7 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle::LifecycleNode */ template> std::shared_ptr> - create_publisher( + create_publisher ( const std::string & topic_name, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, std::shared_ptr allocator = nullptr) @@ -99,84 +100,44 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle::LifecycleNode topic_name, qos_profile, allocator); // keep weak handle for this publisher to enable/disable afterwards - weak_pub_ = pub; + weak_pubs_.push_back(pub); return pub; } LIFECYCLE_EXPORT virtual bool - on_configure() + disable_communication() { - // Placeholder print for all configuring work to be done - // with each pub/sub/srv/client - printf("I am doing some important configuration work\n"); - - return true; - } - - LIFECYCLE_EXPORT - virtual bool - on_activate() - { - if (weak_pub_.expired()) + for (auto weak_pub : weak_pubs_) { - // Someone externally destroyed the publisher handle - fprintf(stderr, "I have no publisher handle\n"); - return false; + auto pub = weak_pub.lock(); + if (!pub){ + return false; + } + pub->on_deactivate(); } - - // Placeholder print for all configuring work to be done - // with each pub/sub/srv/client - printf("I am doing a lot of activation work\n"); - // TODO: does a return value make sense here? - auto pub = weak_pub_.lock(); - if (!pub) - { - return false; - } - pub->on_activate(); - return true; } LIFECYCLE_EXPORT virtual bool - on_deactivate() + enable_communication() { - if (weak_pub_.expired()) + for (auto weak_pub : weak_pubs_) { - fprintf(stderr, "I have no publisher handle\n"); - return false; + auto pub = weak_pub.lock(); + if (!pub){ + return false; + } + pub->on_activate(); } - - // Placeholder print for all configuring work to be done - // with each pub/sub/srv/client - printf("I am doing a lot of deactivation work\n"); - // TODO: does a return value make sense here? - auto pub = weak_pub_.lock(); - if (!pub){ - return false; - } - pub->on_deactivate(); - - return true; - } - - LIFECYCLE_EXPORT - virtual bool - on_error() - { - fprintf(stderr, "Something went wrong here\n"); return true; } private: - // weak handle for the managing publisher - // TODO: Has to be a vector of weak publishers. Does on_deactivate deactivate every publisher?! // Placeholder for all pub/sub/srv/clients - std::weak_ptr weak_pub_; - + std::vector weak_pubs_; }; } // namespace lifecycle_interface From 546e252025ab1b2587f89a7f973742f9e342e7d7 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 8 Nov 2016 15:01:25 -0800 Subject: [PATCH 21/63] (dev) new my_node child of lifecyclenode for demo purpose update stuff (cleanup) remove unused comments (fix) correct dllexport in windows (fix) correctly install libraries (fix) uncrustify (dev) composition over inheritance (dev) publish notification in state_machine transition (dev) lifecycle talker + listener demo for notification (dev) custom transition message generation (dev) publish transition message on state change (dev) correctly malloc/free c data structures (fix) use single thread executor (dev) use services for get state (fix) set freed pointer to NULL (dev) add change state service (dev) introduce services: get_state and change_state in LM (dev) asynchronous caller script for service client (fix) correct dllexport for pimpl (dev) correct constness (dev) concatenate function for topic (fix) uncrustify prepare new service api (tmp) refactor stash (fix) correctly concatenate topics (fix) correctly initialize Service wo/ copy (dev) call both service types extract demo files (fix) remove debug prints (dev) change to lifecycle_msgs (refactor) extract rcl_lifecycle package (refactor) extract lifecycle demos (fix) address review comments (fix) address review comments (fix) pass shared_ptr by value (fix) make find_package(rmw) required (fix) return to shared node handle pointer (refactor) attach sm to lifecycle node and disable lc_manager (dev) construct service from existing rcl_service_t (refactor) extract method for adding a service to a node (fix) stop mock msgs from being installed service takes rcl_node_t* correct typo add_service has to be public uncrustify initial state machine implementation (fix) correctly initialize default state machine uncrustify (dev) first high level api interface src/default_state_machine.c (fix) correctly initialize arrays in statemachine (dev) deactivate/activate publisher demo (dev) initial state machine implementation in rcl (dev) demo application for a managed lifecycle node add visibility control correct install of c-library fix compilation on windows refactoring of external/internal api (dev) generate static functions for c-callback (fix) correct typo (dev) cleanup for c-statemachine (dev) cleanup for c-statemachine (dev) cpp callback map (dev) mv source file into project folders (dev) more helper functions for valid transition (dev) pimpl implementation for cpp lifecyclemanager (dev) register non-default callback functions (dev) cleanup lifecycle node to serve as base class (dev) new my_node child of lifecyclenode for demo purpose update stuff (cleanup) remove unused comments (fix) correct dllexport in windows (fix) correctly install libraries (fix) uncrustify (dev) composition over inheritance (dev) publish notification in state_machine transition (dev) lifecycle talker + listener demo for notification (dev) custom transition message generation (dev) publish transition message on state change (dev) correctly malloc/free c data structures (fix) use single thread executor (dev) use services for get state (fix) set freed pointer to NULL (dev) add change state service (dev) introduce services: get_state and change_state in LM (dev) asynchronous caller script for service client (fix) correct dllexport for pimpl (dev) correct constness (dev) concatenate function for topic (fix) uncrustify prepare new service api (tmp) refactor stash --- rclcpp/include/rclcpp/service.hpp | 21 +- .../strategies/allocator_memory_strategy.hpp | 5 + rclcpp/src/rclcpp/callback_group.cpp | 16 + rclcpp/src/rclcpp/executor.cpp | 1 + rclcpp/src/rclcpp/service.cpp | 4 + .../test/test_externally_defined_services.cpp | 1 + rclcpp_lifecycle/CMakeLists.txt | 102 +++-- .../include/rcl_lifecycle/data_types.h | 116 +++++ .../rcl_lifecycle/default_state_machine.h | 42 -- .../include/rcl_lifecycle/lifecycle_state.h | 115 ----- .../include/rcl_lifecycle/rcl_lifecycle.h | 101 +++++ .../include/rcl_lifecycle/states.h | 45 ++ .../include/rcl_lifecycle/transition_map.h | 73 ++-- .../rcl_lifecycle/visibility_control.h | 20 +- .../rclcpp_lifecycle/lifecycle_executor.hpp | 21 +- .../rclcpp_lifecycle/lifecycle_manager.hpp | 90 ++-- .../rclcpp_lifecycle/lifecycle_node.hpp | 120 ++++-- .../rclcpp_lifecycle/lifecycle_publisher.hpp | 43 +- .../type_traits/is_manageable_node.hpp | 44 +- .../rclcpp_lifecycle/visibility_control.h | 20 +- rclcpp_lifecycle/msg/Transition.msg | 2 + rclcpp_lifecycle/package.xml | 6 +- rclcpp_lifecycle/src/lifecycle_manager.cpp | 120 ++++++ .../src/lifecycle_manager_impl.hpp | 291 +++++++++++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 96 +++++ rclcpp_lifecycle/src/lifecycle_node_impl.hpp | 213 +++++++++ .../src/rcl_lifecycle/default_state_machine.c | 107 +++-- .../rcl_lifecycle/default_state_machine.hxx | 36 ++ .../src/rcl_lifecycle/lifecycle_state.c | 133 ------ .../src/rcl_lifecycle/rcl_lifecycle.c | 406 ++++++++++++++++++ .../src/rcl_lifecycle/transition_map.c | 207 ++++----- .../rclcpp_lifecycle/lifecycle_manager.cpp | 66 +-- .../lifecycle_manager_impl.hpp | 239 ++++++++--- .../src/rclcpp_lifecycle/lifecycle_talker.cpp | 281 ++++++++++-- rclcpp_lifecycle/srv/ChangeState.srv | 4 + rclcpp_lifecycle/srv/GetState.srv | 3 + 36 files changed, 2399 insertions(+), 811 deletions(-) create mode 100644 rclcpp_lifecycle/include/rcl_lifecycle/data_types.h delete mode 100644 rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h delete mode 100644 rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h create mode 100644 rclcpp_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h create mode 100644 rclcpp_lifecycle/include/rcl_lifecycle/states.h create mode 100644 rclcpp_lifecycle/msg/Transition.msg create mode 100644 rclcpp_lifecycle/src/lifecycle_manager.cpp create mode 100644 rclcpp_lifecycle/src/lifecycle_manager_impl.hpp create mode 100644 rclcpp_lifecycle/src/lifecycle_node.cpp create mode 100644 rclcpp_lifecycle/src/lifecycle_node_impl.hpp create mode 100644 rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.hxx delete mode 100644 rclcpp_lifecycle/src/rcl_lifecycle/lifecycle_state.c create mode 100644 rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c create mode 100644 rclcpp_lifecycle/srv/ChangeState.srv create mode 100644 rclcpp_lifecycle/srv/GetState.srv diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 90a9a05a80..6e226c6cc9 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -44,9 +44,6 @@ class ServiceBase RCLCPP_PUBLIC explicit ServiceBase(rcl_node_t * node_handle); - RCLCPP_PUBLIC - explicit ServiceBase(rcl_service_t * service_handle); - RCLCPP_PUBLIC virtual ~ServiceBase(); @@ -70,6 +67,8 @@ class ServiceBase rcl_node_t * node_handle_; rcl_service_t * service_handle_ = nullptr; + + bool defined_extern_ = false; }; using any_service_callback::AnyServiceCallback; @@ -117,16 +116,19 @@ class Service : public ServiceBase rcl_node_t * node_handle, rcl_service_t * service_handle, AnyServiceCallback any_callback) - : ServiceBase(node_handle), defined_extern(true), + : ServiceBase(node_handle), any_callback_(any_callback) { // check if service handle was initialized // TODO(Karsten1987): Can this go directly in RCL? if (service_handle->impl == NULL) { throw std::runtime_error( - std::string("rcl_service_t in constructor argument ") + - "has to be initialized beforehand"); + std::string("rcl_service_t in constructor argument ") + + "has to be initialized beforehand"); } + service_handle_ = service_handle; + defined_extern_ = true; + fprintf(stderr, "Created Extern Service around servicehandle %p\n", service_handle_); } Service() = delete; @@ -134,13 +136,16 @@ class Service : public ServiceBase virtual ~Service() { // check if you have ownership of the handle - if (!defined_extern) { + if (!defined_extern_) { + fprintf(stderr, "CPP Service Handle address %p\n", service_handle_); if (rcl_service_fini(service_handle_, node_handle_) != RCL_RET_OK) { std::stringstream ss; ss << "Error in destruction of rcl service_handle_ handle: " << rcl_get_error_string_safe() << '\n'; (std::cerr << ss.str()).flush(); } + } else { + fprintf(stderr, "Extern Service Handle address %p\n", service_handle_); } } @@ -182,8 +187,6 @@ class Service : public ServiceBase private: RCLCPP_DISABLE_COPY(Service) - bool defined_extern = false; - AnyServiceCallback any_callback_; }; diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 1cac9a5905..8c476a39eb 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -101,6 +101,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } for (size_t i = 0; i < wait_set->size_of_services; ++i) { if (!wait_set->services[i]) { + fprintf(stderr, "Going to remove a invalid service\n"); service_handles_[i] = nullptr; } } @@ -163,6 +164,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy for (auto & weak_service : group->get_service_ptrs()) { auto service = weak_service.lock(); if (service) { + fprintf(stderr, "Going to add %s to the list of service handles\n", service->get_service_name().c_str()); service_handles_.push_back(service->get_service_handle()); } } @@ -200,10 +202,13 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } for (auto service : service_handles_) { + fprintf(stderr, "Adding %s to waitset\n", rcl_service_get_service_name(service)); + fprintf(stderr, "I won't see that\n"); if (rcl_wait_set_add_service(wait_set, service) != RCL_RET_OK) { fprintf(stderr, "Couldn't add service to waitset: %s\n", rcl_get_error_string_safe()); return false; } + fprintf(stderr, "Successfully added %s to waitset\n", rcl_service_get_service_name(service)); } for (auto timer : timer_handles_) { diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 6c6fa281da..a62e12b92e 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -41,6 +41,13 @@ const std::vector & CallbackGroup::get_service_ptrs() const { std::lock_guard lock(mutex_); + for(auto& service : service_ptrs_) + { + auto srv = service.lock(); + if (srv) + fprintf(stderr, "Getting: Service in Group %s\n", srv->get_service_name().c_str()); + } + fprintf(stderr, "**Getting***\n"); return service_ptrs_; } @@ -81,8 +88,17 @@ CallbackGroup::add_timer(const rclcpp::timer::TimerBase::SharedPtr timer_ptr) void CallbackGroup::add_service(const rclcpp::service::ServiceBase::SharedPtr service_ptr) { + { std::lock_guard lock(mutex_); service_ptrs_.push_back(service_ptr); + for(auto& service : service_ptrs_) + { + auto srv = service.lock(); + if (srv) + fprintf(stderr, "Adding: Service in Group %s\n", srv->get_service_name().c_str()); + } + fprintf(stderr, "**Additing***\n"); + } } void diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index a22abfa840..6faa0d4d39 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -402,6 +402,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) rcl_get_error_string_safe()); } + fprintf(stderr, "I got %u services in waitset\n", memory_strategy_->number_of_ready_services()); if (!memory_strategy_->add_handles_to_waitset(&waitset_)) { throw std::runtime_error("Couldn't fill waitset"); } diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 58de5aac5f..a3978a19e0 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -35,6 +35,10 @@ ServiceBase::ServiceBase(std::shared_ptr node_handle) : node_handle_(node_handle) {} +ServiceBase::ServiceBase(std::shared_ptr node_handle) +: node_handle_(node_handle) +{} + ServiceBase::~ServiceBase() {} diff --git a/rclcpp/test/test_externally_defined_services.cpp b/rclcpp/test/test_externally_defined_services.cpp index 1308972f05..6295f96323 100644 --- a/rclcpp/test/test_externally_defined_services.cpp +++ b/rclcpp/test/test_externally_defined_services.cpp @@ -47,6 +47,7 @@ TEST_F(TestExternallyDefinedServices, default_behavior) { try { auto srv = node_handle->create_service("test", callback); + EXPECT_STREQ(srv->get_service_name().c_str(), "test"); } catch (const std::exception & e) { FAIL(); return; diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 78d3f034c7..c0ba793da0 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -3,68 +3,106 @@ cmake_minimum_required(VERSION 3.5) project(rclcpp_lifecycle) if(NOT WIN32) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11 -Wall -Wextra") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra") endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rcl REQUIRED) find_package(rmw REQUIRED) find_package(rmw_implementation_cmake REQUIRED) find_package(std_msgs REQUIRED) +# get the rmw implementations ahead of time +find_package(rmw_implementation_cmake REQUIRED) +get_available_rmw_implementations(rmw_implementations2) +foreach(rmw_implementation ${rmw_implementations2}) + find_package("${rmw_implementation}" REQUIRED) +endforeach() + +set(lifecycle_msg_files + "msg/Transition.msg") +set(lifecycle_srv_files + "srv/GetState.srv" + "srv/ChangeState.srv") +rosidl_generate_interfaces(lifecycle_msgs + ${lifecycle_msg_files} + ${lifecycle_srv_files}) + include_directories( - include - # ${rclcpp_INCLUDE_DIRS} - # ${rmw_INCLUDE_DIRS} -) + include) set(rcl_lifecycle_sources - src/rcl_lifecycle/lifecycle_state.c src/rcl_lifecycle/default_state_machine.c - src/rcl_lifecycle/transition_map.c -) + src/rcl_lifecycle/rcl_lifecycle.c + src/rcl_lifecycle/transition_map.c) set_source_files_properties( ${rcl_lifecycle_sources} - PROPERTIES language "C" -) -add_library( - rcl_lifecycle - SHARED - ${rcl_lifecycle_sources} -) -install(TARGETS rcl_lifecycle - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) + PROPERTIES language "C") macro(targets) - if(NOT target_suffix STREQUAL "") - get_rclcpp_information("${rmw_implementation}" "rclcpp${target_suffix}") - endif() + get_rclcpp_information("${rmw_implementation}" "rclcpp${target_suffix}") + get_rcl_information("${rmw_implementation}" "rcl${target_suffix}") + get_rmw_typesupport(typesupport_impls_c "${rmw_implementation}" LANGUAGE "c") + get_rmw_typesupport(typesupport_impls_cpp "${rmw_implementation}" LANGUAGE "cpp") + + ### C-Library depending only on RCL + add_library( + rcl_lifecycle${target_suffix} + SHARED + ${rcl_lifecycle_sources}) + + foreach(typesupport_impl ${typesupport_impls_c}) + rosidl_target_interfaces(rcl_lifecycle${target_suffix} + lifecycle_msgs + ${typesupport_impl}) + endforeach() + ament_target_dependencies(rcl_lifecycle${target_suffix} + "rcl${target_suffix}" + "std_msgs") + + install(TARGETS rcl_lifecycle${target_suffix} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + + ### CPP High level library+exe add_library(rclcpp_lifecycle${target_suffix} + SHARED src/rclcpp_lifecycle/lifecycle_manager.cpp) target_link_libraries(rclcpp_lifecycle${target_suffix} - rcl_lifecycle - ) + rcl_lifecycle${target_suffix}) + foreach(typesupport_impl ${typesupport_impls_cpp}) + rosidl_target_interfaces(rclcpp_lifecycle${target_suffix} + lifecycle_msgs + ${typesupport_impl}) + endforeach() ament_target_dependencies(rclcpp_lifecycle${target_suffix} - "rclcpp${target_suffix}" - ) + "rclcpp${target_suffix}") add_executable(lifecycle_talker${target_suffix} src/rclcpp_lifecycle/lifecycle_talker.cpp) target_link_libraries(lifecycle_talker${target_suffix} - rclcpp_lifecycle - ) + rclcpp_lifecycle${target_suffix}) ament_target_dependencies(lifecycle_talker${target_suffix} "rclcpp${target_suffix}" - "std_msgs" - ) - install(TARGETS lifecycle_talker${target_suffix} - DESTINATION bin) + "std_msgs") + + install(TARGETS + rclcpp_lifecycle${target_suffix} + lifecycle_talker${target_suffix} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) endmacro() call_for_each_rmw_implementation(targets GENERATE_DEFAULT) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + ament_package() diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/data_types.h b/rclcpp_lifecycle/include/rcl_lifecycle/data_types.h new file mode 100644 index 0000000000..354297951a --- /dev/null +++ b/rclcpp_lifecycle/include/rcl_lifecycle/data_types.h @@ -0,0 +1,116 @@ +// 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 + +/** + * @brief simple definition of a state + * @param state: integer giving the state + * @param label: label for easy indexing + */ +typedef struct LIFECYCLE_EXPORT _rcl_state_t +{ + const char * label; + unsigned int index; +} rcl_state_t; + +/** + * @brief transition definition + * @param start: rcl_state_t as a start state + * @param goal: rcl_state_t as a goal state + * TODO: Maybe specify callback pointer here + * and call on_* functions directly + */ +typedef struct LIFECYCLE_EXPORT _rcl_state_transition_t +{ + rcl_state_t transition_state; + void * callback; + const rcl_state_t * start; + const rcl_state_t * goal; + const rcl_state_t * error; +} rcl_state_transition_t; + +/** + * @brief All transitions which are + * valid associations for a primary state. + * One array belongs to one primary state + * within the map. + */ +typedef struct LIFECYCLE_EXPORT _rcl_transition_array_t +{ + rcl_state_transition_t * transitions; + unsigned int size; +} rcl_transition_array_t; + +/** + * @brief stores an array of transitions + * index by a start state + */ +typedef struct LIFECYCLE_EXPORT _rcl_transition_map_t +{ + // associative array between primary state + // and their respective transitions + // 1 ps -> 1 transition_array + rcl_state_t * primary_states; + rcl_transition_array_t * transition_arrays; + unsigned int size; +} rcl_transition_map_t; + +/** + * @brief: object holding all necessary + * ROS communication interfaces for the statemachine. + * node_handle pointer for instantiating + * state_publisher for publishing state changes + * srv_get_state for getting current state + * srv_change_state for requesting a state change + */ +typedef struct LIFECYCLE_EXPORT _rcl_state_comm_interface_t +{ + rcl_node_t * node_handle; + rcl_publisher_t state_publisher; + rcl_service_t srv_get_state; + rcl_service_t srv_change_state; +} rcl_state_comm_interface_t; + +/** + * @brief: statemachine object holding + * a variable state object as current state + * of the complete machine. + * @param transition_map: a map object of all + * possible transitions registered with this + * state machine. + */ +typedef struct LIFECYCLE_EXPORT _rcl_state_machine_t +{ + const rcl_state_t * current_state; + // Map/Associated array of registered states and transitions + rcl_transition_map_t transition_map; + // Communication interface into a ROS world + rcl_state_comm_interface_t comm_interface; +} rcl_state_machine_t; + +#if __cplusplus +} +#endif + +#endif // RCL_LIFECYCLE__DATA_TYPES_H_ diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h b/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h deleted file mode 100644 index cf43769adc..0000000000 --- a/rclcpp_lifecycle/include/rcl_lifecycle/default_state_machine.h +++ /dev/null @@ -1,42 +0,0 @@ -// 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 RCL__DEFAULT_STATE_MACHINE_H_ -#define RCL__DEFAULT_STATE_MACHINE_H_ - -#include - -#if __cplusplus -extern "C" -{ -#endif - -// primary states based on -// design.ros2.org/articles/node_lifecycle.html -const rcl_state_t LIFECYCLE_EXPORT rcl_state_unconfigured = {"unconfigured", 0}; -const rcl_state_t LIFECYCLE_EXPORT rcl_state_inactive = {"inactive", 1}; -const rcl_state_t LIFECYCLE_EXPORT rcl_state_active = {"active", 2}; -const rcl_state_t LIFECYCLE_EXPORT rcl_state_finalized = {"finalized", 3}; -const rcl_state_t LIFECYCLE_EXPORT rcl_state_configuring = {"configuring", 4}; -const rcl_state_t LIFECYCLE_EXPORT rcl_state_cleaningup = {"cleaningup", 5}; -const rcl_state_t LIFECYCLE_EXPORT rcl_state_shuttingdown = {"shuttingdown", 6}; -const rcl_state_t LIFECYCLE_EXPORT rcl_state_activating = {"activating", 7}; -const rcl_state_t LIFECYCLE_EXPORT rcl_state_deactivating = {"deactivating", 8}; -const rcl_state_t LIFECYCLE_EXPORT rcl_state_errorprocessing = {"errorprocessing", 9}; - -#if __cplusplus -} -#endif // extern "C" - -#endif // RCL__DEFAULT_STATE_MACHINE_H_ diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h b/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h deleted file mode 100644 index e62cf4f065..0000000000 --- a/rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h +++ /dev/null @@ -1,115 +0,0 @@ -// 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_STATE_H_ -#define RCL__LIFECYCLE_STATE_H_ - -#if __cplusplus -extern "C" -{ -#endif - -//#include -//#define bool int; -// #ifdef __cplusplus -// #error WRONG COMPILER -// #endif -#ifndef __cplusplus -typedef int bool; -#define true 1 -#define false 0 -#endif - -#include -#include - -/** - * @brief simple definition of a state - * @param state: integer giving the state - * @param label: label for easy indexing - */ -typedef struct LIFECYCLE_EXPORT _rcl_state_t -{ - const char* label; - unsigned int index; -} rcl_state_t; - -/** - * @brief transition definition - * @param start: rcl_state_t as a start state - * @param goal: rcl_state_t as a goal state - * TODO: Maybe specify callback pointer here - * and call on_* functions directly - */ -typedef struct LIFECYCLE_EXPORT _rcl_state_transition_t -{ - rcl_state_t transition_state; - void* callback; - rcl_state_t* start; - rcl_state_t* goal; -} rcl_state_transition_t; - -/** - * @brief: statemachine object holding - * a variable state object as current state - * of the complete machine. - * @param transition_map: a map object of all - * possible transitions registered with this - * state machine. - */ -typedef struct LIFECYCLE_EXPORT _rcl_state_machine_t -{ - const rcl_state_t* current_state; - rcl_transition_map_t transition_map; -} rcl_state_machine_t; - - -// function definitions -/* - * @brief traverses the transition map of the given - * state machine to find if there is a transition from the - * current state to the specified goal state - * @return the transition state which is valid - * NULL if not available - */ -const rcl_state_transition_t* -LIFECYCLE_EXPORT rcl_is_valid_transition_by_index(rcl_state_machine_t* state_machine, unsigned int transition_index); -const rcl_state_transition_t* -LIFECYCLE_EXPORT rcl_is_valid_transition_by_label(rcl_state_machine_t* state_machine, const char* transition_label); - -const rcl_state_transition_t* -LIFECYCLE_EXPORT rcl_get_registered_transition_by_index(rcl_state_machine_t* state_machine, unsigned int transition_state_index); -const rcl_state_transition_t* -LIFECYCLE_EXPORT rcl_get_registered_transition_by_label(rcl_state_machine_t* state_machine, const char* transition_state_label); - -rcl_state_t -LIFECYCLE_EXPORT rcl_create_state(unsigned int state, char* label); - -rcl_state_transition_t -LIFECYCLE_EXPORT rcl_create_transition(rcl_state_t start, rcl_state_t goal); - -rcl_state_machine_t -LIFECYCLE_EXPORT rcl_get_default_state_machine(); - -void -LIFECYCLE_EXPORT rcl_register_callback(rcl_state_machine_t* state_machine, unsigned int state_index, unsigned int transition_index, bool(*fcn)(void)); - -bool -LIFECYCLE_EXPORT rcl_invoke_transition(rcl_state_machine_t* state_machine, rcl_state_t transition_index); - -#if __cplusplus -} -#endif // extern "C" - -#endif // RCL__LIFECYCLE_STATE_H diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h b/rclcpp_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h new file mode 100644 index 0000000000..1a86421093 --- /dev/null +++ b/rclcpp_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h @@ -0,0 +1,101 @@ +// 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 + +#ifndef __cplusplus + #ifndef bool + #define bool int + #define true 1 + #define false 0 + #endif +#endif + +#include +#include +#include + +LIFECYCLE_EXPORT +rcl_state_machine_t +rcl_get_zero_initialized_state_machine(rcl_node_t * node_handle); + +LIFECYCLE_EXPORT +rcl_ret_t +rcl_state_machine_init(rcl_state_machine_t * state_machine, + const char* node_name, bool default_states); + +LIFECYCLE_EXPORT +rcl_ret_t +rcl_state_machine_fini(rcl_state_machine_t * state_machine); + +// function definitions +/* + * @brief traverses the transition map of the given + * state machine to find if there is a transition from the + * current state to the specified goal state + * @return the transition state which is valid + * NULL if not available + */ +LIFECYCLE_EXPORT +const rcl_state_transition_t * +rcl_is_valid_transition_by_index(rcl_state_machine_t * state_machine, + unsigned int transition_index); +LIFECYCLE_EXPORT +const rcl_state_transition_t * +rcl_is_valid_transition_by_label(rcl_state_machine_t * state_machine, + const char * transition_label); + +LIFECYCLE_EXPORT +const rcl_state_transition_t * +rcl_get_registered_transition_by_index(rcl_state_machine_t * state_machine, + unsigned int transition_state_index); +LIFECYCLE_EXPORT +const rcl_state_transition_t * +rcl_get_registered_transition_by_label(rcl_state_machine_t * state_machine, + const char * transition_state_label); + +LIFECYCLE_EXPORT +rcl_state_t +rcl_create_state(unsigned int state, char * label); + +LIFECYCLE_EXPORT +rcl_state_transition_t +rcl_create_transition(rcl_state_t start, rcl_state_t goal); + +LIFECYCLE_EXPORT +void +rcl_register_callback(rcl_state_machine_t * state_machine, + unsigned int state_index, unsigned int transition_index, bool (* fcn)(void)); + +LIFECYCLE_EXPORT +bool +rcl_start_transition_by_index(rcl_state_machine_t * state_machine, + unsigned int transition_index); + +LIFECYCLE_EXPORT +bool +rcl_finish_transition_by_index(rcl_state_machine_t * state_machine, + unsigned int transition_index, bool success); + +#if __cplusplus +} +#endif // extern "C" + +#endif // RCL_LIFECYCLE__RCL_LIFECYCLE_H_ diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/states.h b/rclcpp_lifecycle/include/rcl_lifecycle/states.h new file mode 100644 index 0000000000..62ed5fd49f --- /dev/null +++ b/rclcpp_lifecycle/include/rcl_lifecycle/states.h @@ -0,0 +1,45 @@ +// 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 RCL_LIFECYCLE__STATES_H_ +#define RCL_LIFECYCLE__STATES_H_ + +#include +#include + +#if __cplusplus +extern "C" +{ +#endif + +// primary states based on +// design.ros2.org/articles/node_lifecycle.html +LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_unknown; +LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_unconfigured; +LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_inactive; +LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_active; +LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_finalized; + +LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_configuring; +LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_cleaningup; +LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_shuttingdown; +LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_activating; +LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_deactivating; +LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_errorprocessing; + +#if __cplusplus +} +#endif // extern "C" + +#endif // RCL_LIFECYCLE__STATES_H_ diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h b/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h index 841a634424..ed4ef309f6 100644 --- a/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h +++ b/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h @@ -13,46 +13,20 @@ // limitations under the License. -#ifndef RCL__TRANSITION_MAP_H_ -#define RCL__TRANSITION_MAP_H_ +#ifndef RCL_LIFECYCLE__TRANSITION_MAP_H_ +#define RCL_LIFECYCLE__TRANSITION_MAP_H_ + +#include #if __cplusplus extern "C" { #endif -typedef struct _rcl_state_t rcl_state_t; -typedef struct _rcl_state_transition_t rcl_state_transition_t; - -/** - * @brief All transitions which are - * valid associations for a primary state. - * One array belongs to one primary state - * within the map. - */ -typedef struct LIFECYCLE_EXPORT _transition_array -{ - rcl_state_transition_t* transitions; - unsigned int size; -} rcl_transition_array_t; - -/** - * @brief stores an array of transitions - * index by a start state - */ -typedef struct LIFECYCLE_EXPORT _map -{ - //rcl_state_t* state_index; - // associative array between primary state and their respective transitions - // 1 ps -> 1 transition_array - rcl_state_t* primary_states; - rcl_transition_array_t* transition_arrays; - unsigned int size; -} rcl_transition_map_t; - LIFECYCLE_EXPORT void -rcl_register_primary_state(rcl_transition_map_t* m, rcl_state_t primary_state); +rcl_register_primary_state(rcl_transition_map_t * m, + rcl_state_t primary_state); /** * @brief appends a transition in a map @@ -61,7 +35,8 @@ rcl_register_primary_state(rcl_transition_map_t* m, rcl_state_t primary_state); */ LIFECYCLE_EXPORT void -rcl_register_transition_by_state(rcl_transition_map_t* m, rcl_state_t start, rcl_state_t goal, rcl_state_transition_t transition); +rcl_register_transition_by_state(rcl_transition_map_t * m, + const rcl_state_t * start, const rcl_state_t * goal, rcl_state_transition_t transition); /** * @brief appends a transition in a map @@ -70,7 +45,8 @@ rcl_register_transition_by_state(rcl_transition_map_t* m, rcl_state_t start, rcl */ LIFECYCLE_EXPORT void -rcl_register_transition_by_label(rcl_transition_map_t* m, const char* start_label, const char* goal_label, rcl_state_transition_t transition); +rcl_register_transition_by_label(rcl_transition_map_t * m, + const char * start_label, const char * goal_label, rcl_state_transition_t transition); /** * @brief appends a transition in a map @@ -79,50 +55,55 @@ rcl_register_transition_by_label(rcl_transition_map_t* m, const char* start_labe */ LIFECYCLE_EXPORT void -rcl_register_transition_by_index(rcl_transition_map_t* m, unsigned int start_index, unsigned int goal_index, rcl_state_transition_t transition); +rcl_register_transition_by_index(rcl_transition_map_t * m, + unsigned int start_index, unsigned int goal_index, rcl_state_transition_t transition); /** * @brief gets the registered primary state based on a label * @return primary state pointer, NULL if not found */ LIFECYCLE_EXPORT -rcl_state_t* -rcl_get_primary_state_by_label(rcl_transition_map_t* m, const char* label); +rcl_state_t * +rcl_get_primary_state_by_label(rcl_transition_map_t * m, + const char * label); /** * @brief gets the registered primary state based on a index * @return primary state pointer, NULL if not found */ LIFECYCLE_EXPORT -rcl_state_t* -rcl_get_primary_state_by_index(rcl_transition_map_t* m, unsigned int index); +rcl_state_t * +rcl_get_primary_state_by_index(rcl_transition_map_t * m, + unsigned int index); /** * @brief gets all transitions based on a label * label is supposed to come from a rcl_state_t object */ LIFECYCLE_EXPORT -rcl_transition_array_t* -rcl_get_transitions_by_label(rcl_transition_map_t* m, const char* label); +rcl_transition_array_t * +rcl_get_transitions_by_label(rcl_transition_map_t * m, + const char * label); /** * @brief gets all transitions based on a state * state is supposed to come from a rcl_state_t object */ LIFECYCLE_EXPORT -rcl_transition_array_t* -rcl_get_transitions_by_index(rcl_transition_map_t* m, unsigned int index); +rcl_transition_array_t * +rcl_get_transitions_by_index(rcl_transition_map_t * m, + unsigned int index); /** * @brief helper functions to print */ LIFECYCLE_EXPORT void -rcl_print_transition_array(const rcl_transition_array_t* transition_array); +rcl_print_transition_array(const rcl_transition_array_t * transition_array); LIFECYCLE_EXPORT void -rcl_print_transition_map(const rcl_transition_map_t* m); +rcl_print_transition_map(const rcl_transition_map_t * m); #if __cplusplus } #endif -#endif // RCL__TRANSITION_MAP_H_ +#endif // RCL_LIFECYCLE__TRANSITION_MAP_H_ diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/visibility_control.h b/rclcpp_lifecycle/include/rcl_lifecycle/visibility_control.h index 721411d55d..07fb89ced1 100644 --- a/rclcpp_lifecycle/include/rcl_lifecycle/visibility_control.h +++ b/rclcpp_lifecycle/include/rcl_lifecycle/visibility_control.h @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCL__LIFECYCLE_VISIBILITY_CONTROL_H_ -#define RCL__LIFECYCLE_VISIBILITY_CONTROL_H_ +#ifndef RCL_LIFECYCLE__VISIBILITY_CONTROL_H_ +#define RCL_LIFECYCLE__VISIBILITY_CONTROL_H_ #ifdef _WIN32 - #define shared_EXPORTS 1 - #ifdef shared_EXPORTS - #define LIFECYCLE_EXPORT __declspec(dllexport) - #else - #define LIFECYCLE_EXPORT __declspec(dllimport) - #endif + #define shared_EXPORTS 1 + #ifdef shared_EXPORTS + #define LIFECYCLE_EXPORT __declspec(dllexport) + #else + #define LIFECYCLE_EXPORT __declspec(dllimport) + #endif #else - #define LIFECYCLE_EXPORT + #define LIFECYCLE_EXPORT #endif -#endif \ No newline at end of file +#endif // RCL_LIFECYCLE__VISIBILITY_CONTROL_H_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_executor.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_executor.hpp index 0a920aba63..c59c2d2669 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_executor.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_executor.hpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__LIFECYCLE_EXECUTOR_HPP_ -#define RCLCPP__LIFECYCLE_EXECUTOR_HPP_ +#ifndef RCLCPP_LIFECYCLE__LIFECYCLE_EXECUTOR_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_EXECUTOR_HPP_ + +#include #include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/type_traits/is_manageable_node.hpp" @@ -26,21 +28,18 @@ namespace executors class LifecycleExecutor : public single_threaded_executor::SingleThreadedExecutor { -// using Node = rclcpp::node::Node; - public: - - explicit LifecycleExecutor(const executor::ExecutorArgs & args = executor::create_default_executor_arguments()) - : SingleThreadedExecutor(args) + explicit LifecycleExecutor(const executor::ExecutorArgs & args = + executor::create_default_executor_arguments()) + : SingleThreadedExecutor(args) {} - //RCLCPP_PUBLIC template::value>::type> void add_node(std::shared_ptr node_ptr, bool notify = true); }; -} // namespace executor -} // namespace rclcpp +} // namespace executors +} // namespace rclcpp -#endif +#endif // RCLCPP_LIFECYCLE__LIFECYCLE_EXECUTOR_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp index f6eda75cb3..d8efbbf9a6 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp @@ -12,43 +12,40 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__LIFECYCLE_MANAGER_H_ -#define RCLCPP__LIFECYCLE_MANAGER_H_ +#ifndef RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_HPP_ +#include #include #include #include "rclcpp_lifecycle/lifecycle_node.hpp" -// forward declaration for c-struct -struct _rcl_state_machine_t; -typedef _rcl_state_machine_t rcl_state_machine_t; - namespace rclcpp { namespace lifecycle { -enum class LifecyclePrimaryStatesT : std::uint8_t +// *INDENT-OFF* +enum class LifecyclePrimaryStatesT : unsigned int { - UNCONFIGURED = 0, - INACTIVE = 1, - ACTIVE = 2, - FINALIZED = 3 + UNKNOWN = 0, + UNCONFIGURED = 1, + INACTIVE = 2, + ACTIVE = 3, + FINALIZED = 4 }; -enum class LifecycleTransitionsT : std::uint8_t +enum class LifecycleTransitionsT : unsigned int { - CONFIGURING = 4, - CLEANINGUP = 5, - SHUTTINGDOWN = 6, - ACTIVATING = 7, - DEACTIVATING = 8, - ERRORPROCESSING = 9 + CONFIGURING = 10, + CLEANINGUP = 11, + SHUTTINGDOWN = 12, + ACTIVATING = 13, + DEACTIVATING = 14, + ERRORPROCESSING = 15 }; -class LifecycleManagerImpl; - class LifecycleManagerInterface { virtual bool configure(const std::string& node_name = "") = 0; @@ -57,25 +54,35 @@ class LifecycleManagerInterface virtual bool activate(const std::string& node_name = "") = 0; virtual bool deactivate(const std::string& node_name = "") = 0; }; +// *INDENT-ON* class LifecycleManager : public LifecycleManagerInterface { public: + using NodeInterfacePtr = std::shared_ptr; + using NodePtr = std::shared_ptr; + + LIFECYCLE_EXPORT LifecycleManager(); + + LIFECYCLE_EXPORT ~LifecycleManager(); LIFECYCLE_EXPORT - void - add_node_interface(const std::shared_ptr& node_interface); + std::shared_ptr + get_node_base_interface(); LIFECYCLE_EXPORT void - add_node_interface(const std::shared_ptr& node_interface, rcl_state_machine_t custom_state_machine); + add_node_interface(const NodePtr & node); LIFECYCLE_EXPORT + void + add_node_interface(const std::string & node_name, const NodeInterfacePtr & node_interface); + template bool - register_on_configure(const std::string& node_name, bool(T::*method)(void), T* instance) + register_on_configure(const std::string & node_name, bool (T::* method)(void), T * instance) { auto cb = std::bind(method, instance); return register_on_configure(node_name, cb); @@ -83,16 +90,15 @@ class LifecycleManager : public LifecycleManagerInterface LIFECYCLE_EXPORT bool - register_on_configure(const std::string& node_name, std::function& fcn); + register_on_configure(const std::string & node_name, std::function & fcn); LIFECYCLE_EXPORT bool - configure(const std::string& node_name = ""); + configure(const std::string & node_name = ""); - LIFECYCLE_EXPORT template bool - register_on_cleanup(const std::string& node_name, bool(T::*method)(void), T* instance) + register_on_cleanup(const std::string & node_name, bool (T::* method)(void), T * instance) { auto cb = std::bind(method, instance); return register_on_cleanup(node_name, cb); @@ -100,16 +106,15 @@ class LifecycleManager : public LifecycleManagerInterface LIFECYCLE_EXPORT bool - register_on_cleanup(const std::string& node_name, std::function& fcn); + register_on_cleanup(const std::string & node_name, std::function & fcn); LIFECYCLE_EXPORT bool - cleanup(const std::string& node_name = ""); + cleanup(const std::string & node_name = ""); - LIFECYCLE_EXPORT template bool - register_on_shutdown(const std::string& node_name, bool(T::*method)(void), T* instance) + register_on_shutdown(const std::string & node_name, bool (T::* method)(void), T * instance) { auto cb = std::bind(method, instance); return register_on_shutdown(node_name, cb); @@ -117,16 +122,15 @@ class LifecycleManager : public LifecycleManagerInterface LIFECYCLE_EXPORT bool - register_on_shutdown(const std::string& node_name, std::function& fcn); + register_on_shutdown(const std::string & node_name, std::function & fcn); LIFECYCLE_EXPORT bool - shutdown(const std::string& node_name = ""); + shutdown(const std::string & node_name = ""); - LIFECYCLE_EXPORT template bool - register_on_activate(const std::string& node_name, bool(T::*method)(void), T* instance) + register_on_activate(const std::string & node_name, bool (T::* method)(void), T * instance) { auto cb = std::bind(method, instance); return register_on_activate(node_name, cb); @@ -134,16 +138,15 @@ class LifecycleManager : public LifecycleManagerInterface LIFECYCLE_EXPORT bool - register_on_activate(const std::string& node_name, std::function& fcn); + register_on_activate(const std::string & node_name, std::function & fcn); LIFECYCLE_EXPORT bool - activate(const std::string& node_name = ""); + activate(const std::string & node_name = ""); - LIFECYCLE_EXPORT template bool - register_on_deactivate(const std::string& node_name, bool(T::*method)(void), T* instance) + register_on_deactivate(const std::string & node_name, bool (T::* method)(void), T * instance) { auto cb = std::bind(method, instance); return register_on_deactivate(node_name, cb); @@ -151,18 +154,17 @@ class LifecycleManager : public LifecycleManagerInterface LIFECYCLE_EXPORT bool - register_on_deactivate(const std::string& node_name, std::function& fcn); + register_on_deactivate(const std::string & node_name, std::function & fcn); LIFECYCLE_EXPORT bool - deactivate(const std::string& node_name = ""); + deactivate(const std::string & node_name = ""); private: class LifecycleManagerImpl; std::unique_ptr impl_; - //LifecycleManagerImpl* impl_; }; } // namespace lifecycle } // namespace rclcpp -#endif +#endif // RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index e936854113..da31a3857f 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -12,13 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__LIFECYCLE_NODE_HPP_ -#define RCLCPP__LIFECYCLE_NODE_HPP_ +#ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ + +#include +#include +#include #include "rclcpp/node.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" -#include +#include "rclcpp_lifecycle/visibility_control.h" namespace rclcpp { @@ -27,20 +31,20 @@ namespace node namespace lifecycle { -template +template struct Callback; -template -struct Callback +template +struct Callback { - template - static Ret callback(Args... args) { return func(args...); } - static std::function func; + template + static Ret callback(Args ... args) {return func(args ...); } + static std::function func; }; // Initialize the static member. -template -std::function Callback::func; +template +std::function Callback::func; /** * @brief Interface class for a managed node. @@ -50,68 +54,92 @@ std::function Callback; + using LifecyclePublisherWeakPtr = + std::weak_ptr; + + LIFECYCLE_EXPORT + explicit LifecycleNode(const std::string & node_name, bool use_intra_process_comms = false) + : base_interface_(std::make_shared(node_name, use_intra_process_comms)), + communication_interface_(base_interface_) // MOCK as base/comms interface not done yet + {} LIFECYCLE_EXPORT - explicit LifecycleNode(const std::string & node_name, bool use_intra_process_comms = false) : - Node(node_name, use_intra_process_comms) + ~LifecycleNode() {} + + // MOCK typedefs as node refactor not done yet + using BaseInterface = rclcpp::node::Node; + std::shared_ptr + get_base_interface() { + return base_interface_; } - LIFECYCLE_EXPORT - ~LifecycleNode(){} + // MOCK typedefs as node refactor not done yet + using CommunicationInterface = rclcpp::node::Node; + std::shared_ptr + get_communication_interface() + { + return communication_interface_; + } + + std::string + get_name() + { + return base_interface_->get_name(); + } /** * @brief same API for creating publisher as regular Node */ template> std::shared_ptr> - create_publisher ( - const std::string & topic_name, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, - std::shared_ptr allocator = nullptr) + create_publisher( + const std::string & topic_name, + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, + std::shared_ptr allocator = nullptr) { // create regular publisher in rclcpp::Node - auto pub = rclcpp::node::Node::create_publishercreate_publisher>( - topic_name, qos_profile, allocator); + topic_name, qos_profile, allocator); // keep weak handle for this publisher to enable/disable afterwards weak_pubs_.push_back(pub); return pub; } + template + typename rclcpp::timer::WallTimer::SharedPtr + create_wall_timer( + std::chrono::nanoseconds period, + CallbackType callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) + { + return communication_interface_->create_wall_timer(period, callback, group); + } + LIFECYCLE_EXPORT virtual bool disable_communication() { - for (auto weak_pub : weak_pubs_) - { + for (auto weak_pub : weak_pubs_) { auto pub = weak_pub.lock(); - if (!pub){ + if (!pub) { return false; } pub->on_deactivate(); @@ -123,10 +151,9 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle::LifecycleNode virtual bool enable_communication() { - for (auto weak_pub : weak_pubs_) - { + for (auto weak_pub : weak_pubs_) { auto pub = weak_pub.lock(); - if (!pub){ + if (!pub) { return false; } pub->on_activate(); @@ -135,13 +162,14 @@ class LifecycleNode : public rclcpp::node::Node, public lifecycle::LifecycleNode } private: - + std::shared_ptr base_interface_; + std::shared_ptr communication_interface_; // Placeholder for all pub/sub/srv/clients std::vector weak_pubs_; }; -} // namespace lifecycle_interface +} // namespace lifecycle } // namespace node } // namespace rclcpp -#endif +#endif // RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index 8e4785fa23..af7ba712be 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -12,8 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__LIFECYCLE_PUBLISHER_HPP_ -#define RCLCPP__LIFECYCLE_PUBLISHER_HPP_ +#ifndef RCLCPP_LIFECYCLE__LIFECYCLE_PUBLISHER_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_PUBLISHER_HPP_ + +#include +#include #include "rclcpp/publisher.hpp" @@ -56,15 +59,16 @@ class LifecyclePublisher : public rclcpp::publisher::Publisher, using MessageUniquePtr = std::unique_ptr; LifecyclePublisher( - std::shared_ptr node_handle, - std::string topic, - const rcl_publisher_options_t & publisher_options, - std::shared_ptr allocator): - ::rclcpp::publisher::Publisher(node_handle, topic, publisher_options, allocator), - enabled_(false) - { } + std::shared_ptr node_handle, + std::string topic, + const rcl_publisher_options_t & publisher_options, + std::shared_ptr allocator) + : ::rclcpp::publisher::Publisher( + node_handle, topic, publisher_options, allocator), + enabled_(false) + {} - ~LifecyclePublisher() {}; + ~LifecyclePublisher() {} /** * @brief briefly checks whether this publisher @@ -74,8 +78,7 @@ class LifecyclePublisher : public rclcpp::publisher::Publisher, virtual void publish(std::unique_ptr & msg) { - if (!enabled_) - { + if (!enabled_) { printf("I push every message to /dev/null\n"); return; } @@ -90,8 +93,7 @@ class LifecyclePublisher : public rclcpp::publisher::Publisher, virtual void publish(const std::shared_ptr & msg) { - if (!enabled_) - { + if (!enabled_) { printf("I publish message %s to /dev/null\n", msg->data.c_str()); return; } @@ -107,8 +109,7 @@ class LifecyclePublisher : public rclcpp::publisher::Publisher, virtual void publish(std::shared_ptr msg) { - if (!enabled_) - { + if (!enabled_) { printf("I am every message to /dev/null\n"); return; } @@ -123,8 +124,7 @@ class LifecyclePublisher : public rclcpp::publisher::Publisher, virtual void publish(const MessageT & msg) { - if (!enabled_) - { + if (!enabled_) { printf("I am every message to /dev/null\n"); return; } @@ -137,10 +137,9 @@ class LifecyclePublisher : public rclcpp::publisher::Publisher, * to the actual rclcp Publisher base class */ virtual void - publish(std::shared_ptr& msg) + publish(std::shared_ptr & msg) { - if (!enabled_) - { + if (!enabled_) { printf("I am every message to /dev/null\n"); return; } @@ -168,4 +167,4 @@ class LifecyclePublisher : public rclcpp::publisher::Publisher, } // namespace publisher } // namespace rclcpp -#endif // endif RCLCPP__LIFECYCLE_PUBLISHER_HPP_ +#endif // RCLCPP_LIFECYCLE__LIFECYCLE_PUBLISHER_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp index e959f64ccd..c8eeb42185 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp @@ -1,19 +1,19 @@ -// Copyright 2014 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 RCLCPP__IS_MANAGEABLE_HPP_ -#define RCLCPP__IS_MANAGEABLE_HPP_ +// 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 RCLCPP_LIFECYCLE__TYPE_TRAITS__IS_MANAGEABLE_NODE_HPP_ +#define RCLCPP_LIFECYCLE__TYPE_TRAITS__IS_MANAGEABLE_NODE_HPP_ #include #include @@ -25,7 +25,8 @@ struct has_on_activate }; template -struct has_on_activate().on_activate())>::value>::type> +struct has_on_activate().on_activate())>::value>::type> { static constexpr bool value = true; }; @@ -37,7 +38,8 @@ struct has_on_deactivate }; template -struct has_on_deactivate().on_deactivate())>::value>::type> +struct has_on_deactivate().on_deactivate())>::value>::type> { static constexpr bool value = true; }; @@ -47,8 +49,8 @@ struct is_manageable_node : std::false_type {}; template -struct is_manageable_node::value - && has_on_deactivate::value >::type> : std::true_type +struct is_manageable_node::value && has_on_deactivate::value>::type>: std::true_type {}; -#endif +#endif // RCLCPP_LIFECYCLE__TYPE_TRAITS__IS_MANAGEABLE_NODE_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h b/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h index 2f74e7a5c8..69e43cd708 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__LIFECYCLE_VISIBILITY_CONTROL_H_ -#define RCLCPP__LIFECYCLE_VISIBILITY_CONTROL_H_ +#ifndef RCLCPP_LIFECYCLE__VISIBILITY_CONTROL_H_ +#define RCLCPP_LIFECYCLE__VISIBILITY_CONTROL_H_ #ifdef _WIN32 - #define shared_EXPORTS 1 - #ifdef shared_EXPORTS - #define LIFECYCLE_EXPORT __declspec(dllexport) - #else - #define LIFECYCLE_EXPORT __declspec(dllimport) - #endif + #define shared_EXPORTS 1 + #ifdef shared_EXPORTS + #define LIFECYCLE_EXPORT __declspec(dllexport) + #else + #define LIFECYCLE_EXPORT __declspec(dllimport) + #endif #else - #define LIFECYCLE_EXPORT + #define LIFECYCLE_EXPORT #endif -#endif \ No newline at end of file +#endif // RCLCPP_LIFECYCLE__VISIBILITY_CONTROL_H_ diff --git a/rclcpp_lifecycle/msg/Transition.msg b/rclcpp_lifecycle/msg/Transition.msg new file mode 100644 index 0000000000..6f6c283fce --- /dev/null +++ b/rclcpp_lifecycle/msg/Transition.msg @@ -0,0 +1,2 @@ +uint8 start_state +uint8 goal_state diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 3aee7a9f66..03c4f6a55a 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -8,12 +8,13 @@ Apache License 2.0 ament_cmake - rosidl_default_generators rclcpp rmw_implementation rmw_implementation_cmake + builtin_interfaces + rosidl_default_generators std_msgs rclcpp @@ -21,6 +22,9 @@ rosidl_default_runtime std_msgs + ament_lint_auto + ament_lint_common + ament_cmake diff --git a/rclcpp_lifecycle/src/lifecycle_manager.cpp b/rclcpp_lifecycle/src/lifecycle_manager.cpp new file mode 100644 index 0000000000..7ca1b7885d --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_manager.cpp @@ -0,0 +1,120 @@ +// 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 "rclcpp_lifecycle/lifecycle_manager.hpp" + +#include "lifecycle_manager_impl.hpp" // implementation + +namespace rclcpp +{ +namespace lifecycle +{ + +LifecycleManager::LifecycleManager() +: impl_(new LifecycleManagerImpl()) +{ + impl_->init(); +} + +LifecycleManager::~LifecycleManager() = default; + +std::shared_ptr +LifecycleManager::get_node_base_interface() +{ + return impl_->node_base_handle_; +} + +void +LifecycleManager::add_node_interface(const NodePtr & node) +{ + add_node_interface(node->get_base_interface()->get_name(), node); +} + +void +LifecycleManager::add_node_interface(const std::string & node_name, + const NodeInterfacePtr & node_interface) +{ + impl_->add_node_interface(node_name, node_interface); +} + +bool +LifecycleManager::register_on_configure(const std::string & node_name, + std::function & fcn) +{ + return impl_->register_callback(node_name, fcn); +} + +bool +LifecycleManager::configure(const std::string & node_name) +{ + return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::CONFIGURING); +} + +bool +LifecycleManager::register_on_cleanup(const std::string & node_name, + std::function & fcn) +{ + return impl_->register_callback(node_name, fcn); +} + +bool +LifecycleManager::cleanup(const std::string & node_name) +{ + return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::CLEANINGUP); +} + +bool +LifecycleManager::register_on_shutdown(const std::string & node_name, + std::function & fcn) +{ + return impl_->register_callback(node_name, fcn); +} + +bool +LifecycleManager::shutdown(const std::string & node_name) +{ + return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::SHUTTINGDOWN); +} + +bool +LifecycleManager::register_on_activate(const std::string & node_name, + std::function & fcn) +{ + return impl_->register_callback(node_name, fcn); +} + +bool +LifecycleManager::activate(const std::string & node_name) +{ + return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::ACTIVATING); +} + +bool +LifecycleManager::register_on_deactivate(const std::string & node_name, + std::function & fcn) +{ + return impl_->register_callback(node_name, fcn); +} + +bool +LifecycleManager::deactivate(const std::string & node_name) +{ + return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::DEACTIVATING); +} + +} // namespace lifecycle +} // namespace rclcpp diff --git a/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp b/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp new file mode 100644 index 0000000000..08f218c0ee --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp @@ -0,0 +1,291 @@ +// 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 RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_IMPL_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_IMPL_HPP_ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_manager.hpp" + + +namespace rclcpp +{ +namespace lifecycle +{ + +using LifecycleInterface = rclcpp::node::lifecycle::LifecycleNodeInterface; +using LifecycleInterfacePtr = std::shared_ptr; +using LifecycleInterfaceWeakPtr = std::weak_ptr; + +struct NodeStateMachine +{ + LifecycleInterfaceWeakPtr weak_node_handle; + rcl_state_machine_t state_machine; + std::map> cb_map; + std::shared_ptr srv_get_state; + std::shared_ptr srv_change_state; +}; + +class LifecycleManager::LifecycleManagerImpl +{ +public: + LifecycleManagerImpl() + : node_base_handle_(std::make_shared("lifecycle_manager")) + {} + + ~LifecycleManagerImpl() + { + for (auto it = node_handle_map_.begin(); it != node_handle_map_.end(); ++it) { + rcl_state_machine_t * rcl_state_machine = &it->second.state_machine; + if (!rcl_state_machine) + { + fprintf(stderr, "%s:%u, FATAL: rcl_state_machine got destroyed externally.\n", + __FILE__, __LINE__); + } else { + rcl_state_machine_fini(rcl_state_machine, node_base_handle_->get_rcl_node_handle()); + } + } + } + + void + init() + { + srv_get_state_ = node_base_handle_->create_service( + node_base_handle_->get_name() + "__get_state", + std::bind(&LifecycleManagerImpl::on_get_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + srv_change_state_ = node_base_handle_->create_service( + node_base_handle_->get_name() + "__change_state", + std::bind(&LifecycleManagerImpl::on_change_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + } + + void + on_get_state(const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) + { + on_get_single_state(header, req, resp, req->node_name); + } + + void + on_get_single_state(const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp, + std::string node_name) + { + (void)header; + (void)req; + auto node_handle_iter = node_handle_map_.find(node_name); + if (node_handle_iter == node_handle_map_.end()) { + resp->current_state = static_cast(LifecyclePrimaryStatesT::UNKNOWN); + return; + } + resp->current_state = + static_cast(node_handle_iter->second.state_machine.current_state->index); + } + + void + on_change_state(const std::shared_ptrheader, + const std::shared_ptr req, + std::shared_ptr resp) + { + on_change_single_state(header, req, resp, req->node_name); + } + + void + on_change_single_state(const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp, + std::string node_name) + { + (void)header; + auto node_handle_iter = node_handle_map_.find(node_name); + if (node_handle_iter == node_handle_map_.end()) { + resp->success = false; + return; + } + auto transition = static_cast(req->transition); + resp->success = change_state(node_name, transition); + } + + void + add_node_interface(const std::string & node_name, + const LifecycleInterfacePtr & lifecycle_interface) + { + // TODO(karsten1987): clarify what do if node already exists; + NodeStateMachine& node_state_machine = node_handle_map_[node_name]; + node_state_machine.state_machine = rcl_get_zero_initialized_state_machine(); + rcl_ret_t ret = rcl_state_machine_init(&node_state_machine.state_machine, node_base_handle_->get_rcl_node_handle(), + rosidl_generator_cpp::get_message_type_support_handle(), + rosidl_generator_cpp::get_service_type_support_handle(), + rosidl_generator_cpp::get_service_type_support_handle(), + true); + if (ret != RCL_RET_OK) + { + fprintf(stderr, "Error adding %s: %s\n", + node_name.c_str(), rcl_get_error_string_safe()); + return; + } + + // srv objects may get destroyed directly here + { // get_state + std::function, + const std::shared_ptr, + std::shared_ptr)> cb = + std::bind(&LifecycleManagerImpl::on_get_single_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + node_name); + + rclcpp::any_service_callback::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + auto srv_get_state = + rclcpp::service::Service::make_shared( + node_base_handle_->get_shared_node_handle(), + &node_state_machine.state_machine.comm_interface.srv_get_state, + any_cb); + auto srv_get_state_base = std::dynamic_pointer_cast(srv_get_state); + node_base_handle_->add_service(srv_get_state_base); + node_state_machine.srv_get_state = srv_get_state_base; + } + + { // change_state + std::function, + const std::shared_ptr, + std::shared_ptr)> cb = + std::bind(&LifecycleManagerImpl::on_change_single_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + node_name); + + rclcpp::any_service_callback::AnyServiceCallback any_cb; + any_cb.set(cb); + auto srv_change_state = + std::make_shared>( + node_base_handle_->get_shared_node_handle(), + &node_state_machine.state_machine.comm_interface.srv_change_state, + any_cb); + auto srv_change_state_base = std::dynamic_pointer_cast(srv_change_state); + node_base_handle_->add_service(srv_change_state_base); + node_state_machine.srv_change_state = srv_change_state_base; + } + + node_state_machine.weak_node_handle = lifecycle_interface; + // register default callbacks + // maybe optional + std::function cb_configuring = std::bind( + &LifecycleInterface::on_configure, lifecycle_interface); + std::function cb_cleaningup = std::bind( + &LifecycleInterface::on_cleanup, lifecycle_interface); + std::function cb_shuttingdown = std::bind( + &LifecycleInterface::on_shutdown, lifecycle_interface); + std::function cb_activating = std::bind( + &LifecycleInterface::on_activate, lifecycle_interface); + std::function cb_deactivating = std::bind( + &LifecycleInterface::on_deactivate, lifecycle_interface); + std::function cb_error = std::bind( + &LifecycleInterface::on_error, lifecycle_interface); + node_state_machine.cb_map[LifecycleTransitionsT::CONFIGURING] = cb_configuring; + node_state_machine.cb_map[LifecycleTransitionsT::CLEANINGUP] = cb_cleaningup; + node_state_machine.cb_map[LifecycleTransitionsT::SHUTTINGDOWN] = cb_shuttingdown; + node_state_machine.cb_map[LifecycleTransitionsT::ACTIVATING] = cb_activating; + node_state_machine.cb_map[LifecycleTransitionsT::DEACTIVATING] = cb_deactivating; + node_state_machine.cb_map[LifecycleTransitionsT::ERRORPROCESSING] = cb_error; + } + + template + bool + register_callback(const std::string & node_name, std::function & cb) + { + if (node_name.empty()) { + return false; + } + + auto node_handle_iter = node_handle_map_.find(node_name); + if (node_handle_iter == node_handle_map_.end()) { + fprintf(stderr, "Node with name %s is not registered\n", node_name.c_str()); + } + node_handle_iter->second.cb_map[lifecycle_transition] = cb; + return true; + } + + bool + change_state(const std::string & node_name, LifecycleTransitionsT lifecycle_transition) + { + if (node_name.empty()) { + return false; + } + + auto node_handle_iter = node_handle_map_.find(node_name); + if (node_handle_iter == node_handle_map_.end()) { + fprintf(stderr, "%s:%d, Node with name %s is not registered\n", + __FILE__, __LINE__, node_name.c_str()); + return false; + } + + auto node_handle = node_handle_iter->second.weak_node_handle.lock(); + if (!node_handle) { + fprintf(stderr, + "%s:%d, Nodehandle is not available. Was it destroyed outside the lifecycle manager?\n", + __FILE__, __LINE__); + return false; + } + + unsigned int transition_index = static_cast(lifecycle_transition); + if (!rcl_start_transition_by_index(&node_handle_iter->second.state_machine, transition_index)) { + fprintf(stderr, "%s:%d, Unable to start transition %u from current state %s\n", + __FILE__, __LINE__, transition_index, + node_handle_iter->second.state_machine.current_state->label); + return false; + } + + // Since we set always set a default callback, + // we don't have to check for nullptr here + std::function callback = node_handle_iter->second.cb_map[lifecycle_transition]; + auto success = callback(); + + if (!rcl_finish_transition_by_index(&node_handle_iter->second.state_machine, + transition_index, success)) + { + fprintf(stderr, "Failed to finish transition %u. Current state is now: %s\n", + transition_index, node_handle_iter->second.state_machine.current_state->label); + return false; + } + // This true holds in both cases where the actual callback + // was successful or not, since at this point we have a valid transistion + // to either a new primary state or error state + return true; + } + + std::shared_ptr node_base_handle_; + std::shared_ptr> srv_get_state_; + std::shared_ptr> srv_change_state_; + std::map node_handle_map_; +}; + +} // namespace lifecycle +} // namespace rclcpp +#endif // RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_IMPL_HPP_ diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp new file mode 100644 index 0000000000..61a40b73f8 --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -0,0 +1,96 @@ +// 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 "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "lifecycle_node_impl.hpp" // implementation + +namespace rclcpp +{ +namespace lifecycle +{ + +LifecycleNode::LifecycleNode(const std::string & node_name, bool use_intra_process_comms) +: base_node_handle_(std::make_shared(node_name, use_intra_process_comms)), + communication_interface_(base_node_handle_), + impl_(new LifecycleNodeImpl(base_node_handle_)) +{ + impl_->init(); + + register_on_configure(std::bind(&LifecycleNodeInterface::on_configure, this)); + register_on_cleanup(std::bind(&LifecycleNodeInterface::on_cleanup, this)); + register_on_shutdown(std::bind(&LifecycleNodeInterface::on_shutdown, this)); + register_on_activate(std::bind(&LifecycleNodeInterface::on_activate, this)); + register_on_deactivate(std::bind(&LifecycleNodeInterface::on_deactivate, this)); + register_on_error(std::bind(&LifecycleNodeInterface::on_error, this)); +} + +LifecycleNode::~LifecycleNode() +{}; + +bool +LifecycleNode::register_on_configure(std::function fcn) +{ + return impl_->register_callback(rcl_state_configuring.index, fcn); +} + +bool +LifecycleNode::register_on_cleanup(std::function fcn) +{ + return impl_->register_callback(rcl_state_cleaningup.index, fcn); +} + +bool +LifecycleNode::register_on_shutdown(std::function fcn) +{ + return impl_->register_callback(rcl_state_shuttingdown.index, fcn); +} + +bool +LifecycleNode::register_on_activate(std::function fcn) +{ + return impl_->register_callback(rcl_state_activating.index, fcn); +} + +bool +LifecycleNode::register_on_deactivate(std::function fcn) +{ + return impl_->register_callback(rcl_state_deactivating.index, fcn); +} + +bool +LifecycleNode::register_on_error(std::function fcn) +{ + return impl_->register_callback(rcl_state_errorprocessing.index, fcn); +} + +void +LifecycleNode::add_publisher_handle(std::shared_ptr pub) +{ + impl_->add_publisher_handle(pub); +} + +void +LifecycleNode::add_timer_handle(std::shared_ptr timer) +{ + impl_->add_timer_handle(timer); +} + +} // namespace lifecycle +} // namespace rclcpp diff --git a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp new file mode 100644 index 0000000000..43d09ae98c --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp @@ -0,0 +1,213 @@ +// 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 RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ + +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include + +#include + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace rclcpp +{ +namespace lifecycle +{ + +class LifecycleNode::LifecycleNodeImpl +{ + +using TransitionMsg = lifecycle_msgs::msg::Transition; +using GetStateSrv = lifecycle_msgs::srv::GetState; +using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; + +public: + LifecycleNodeImpl(std::shared_ptr base_node_handle) + : base_node_handle_(base_node_handle) + {} + + ~LifecycleNodeImpl() + { + if (rcl_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) + { + fprintf(stderr, "%s:%u, FATAL: rcl_state_machine got destroyed externally.\n", + __FILE__, __LINE__); + } else { + rcl_state_machine_fini(&state_machine_, base_node_handle_->get_rcl_node_handle()); + } + } + + void + init() + { + state_machine_ = rcl_get_zero_initialized_state_machine(); + rcl_ret_t ret = rcl_state_machine_init(&state_machine_, base_node_handle_->get_rcl_node_handle(), + rosidl_generator_cpp::get_message_type_support_handle(), + rosidl_generator_cpp::get_service_type_support_handle(), + rosidl_generator_cpp::get_service_type_support_handle(), + true); + if (ret != RCL_RET_OK) + { + fprintf(stderr, "Error adding %s: %s\n", + base_node_handle_->get_name().c_str(), rcl_get_error_string_safe()); + return; + } + + // srv objects may get destroyed directly here + { // get_state + auto cb = std::bind(&LifecycleNodeImpl::on_get_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::any_service_callback::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_state_ = std::make_shared>( + base_node_handle_->get_shared_node_handle(), &state_machine_.comm_interface.srv_get_state, + any_cb); + base_node_handle_->add_service( + std::dynamic_pointer_cast(srv_get_state_)); + } + + { // change_state + auto cb = std::bind(&LifecycleNodeImpl::on_change_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::any_service_callback::AnyServiceCallback any_cb; + any_cb.set(cb); + + srv_change_state_ = std::make_shared>( + base_node_handle_->get_shared_node_handle(), + &state_machine_.comm_interface.srv_change_state, + any_cb); + base_node_handle_->add_service( + std::dynamic_pointer_cast(srv_change_state_)); + } + } + + void + on_get_state(const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) + { + (void)header; + (void)req; + if (rcl_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + resp->current_state = static_cast(rcl_state_unknown.index); + return; + } + resp->current_state = + static_cast(state_machine_.current_state->index); + } + + void + on_change_state(const std::shared_ptrheader, + const std::shared_ptr req, + std::shared_ptr resp) + { + (void)header; + if (rcl_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + resp->success = false; + return; + } + resp->success = change_state(req->transition); + } + + bool + register_callback(std::uint8_t lifecycle_transition, std::function & cb) + { + cb_map_[lifecycle_transition] = cb; + return true; + } + + bool + change_state(std::uint8_t lifecycle_transition) + { + if (rcl_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + fprintf(stderr, "%s:%d, Unable to change state for state machine for %s: %s \n", + __FILE__, __LINE__, base_node_handle_->get_name().c_str(), rcl_get_error_string_safe()); + return false; + } + + unsigned int transition_index = static_cast(lifecycle_transition); + if (!rcl_start_transition_by_index(&state_machine_, transition_index)) { + fprintf(stderr, "%s:%d, Unable to start transition %u from current state %s\n", + __FILE__, __LINE__, transition_index, state_machine_.current_state->label); + return false; + } + + fprintf(stderr, "Started Transition %u. SM is now in state %u\n", + lifecycle_transition, state_machine_.current_state->index); + // Since we set always set a default callback, + // we don't have to check for nullptr here + auto it = cb_map_.find(lifecycle_transition); + if (it == cb_map_.end()) + { + fprintf(stderr, "%s:%d, No callback is registered for transition %u.\n", + __FILE__, __LINE__, lifecycle_transition); + } + std::function callback = it->second; + auto success = callback(); + + if (!rcl_finish_transition_by_index(&state_machine_, transition_index, success)) + { + fprintf(stderr, "Failed to finish transition %u. Current state is now: %s\n", + transition_index, state_machine_.current_state->label); + return false; + } + fprintf(stderr, "Finished Transition %u. SM is now in state %u\n", + lifecycle_transition, state_machine_.current_state->index); + // This true holds in both cases where the actual callback + // was successful or not, since at this point we have a valid transistion + // to either a new primary state or error state + return true; + } + + void + add_publisher_handle(std::shared_ptr pub) + { + weak_pubs_.push_back(pub); + } + + void + add_timer_handle(std::shared_ptr timer) + { + weak_timers_.push_back(timer); + } + + rcl_state_machine_t state_machine_; + std::map> cb_map_; + + std::shared_ptr base_node_handle_; + std::shared_ptr> srv_change_state_; + std::shared_ptr> srv_get_state_; + + // to controllable things + std::vector> weak_pubs_; + std::vector> weak_timers_; +}; + +} // namespace lifecycle +} // namespace rclcpp +#endif // RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_IMPL_HPP_ diff --git a/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.c b/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.c index d5bd9f68ca..4055c1e3dc 100644 --- a/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.c +++ b/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.c @@ -1,4 +1,4 @@ -// Copyright 2015 Open Source Robotics Foundation, Inc. +// 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. @@ -12,79 +12,98 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCL__STATE_MACHINE_H_ -#define RCL__STATE_MACHINE_H_ - #include #include #include -#include "rcl_lifecycle/lifecycle_state.h" -#include "rcl_lifecycle/default_state_machine.h" +#include "rosidl_generator_c/message_type_support.h" +#include "rosidl_generator_c/string_functions.h" +#include "std_msgs/msg/string.h" + +#include "rcl_lifecycle/states.h" +#include "rcl_lifecycle/transition_map.h" + +#include "default_state_machine.hxx" #if __cplusplus extern "C" { #endif +// *INDENT-OFF* +const rcl_state_t rcl_state_unknown = {"unknown", 0}; +const rcl_state_t rcl_state_unconfigured = {"unconfigured", 1}; +const rcl_state_t rcl_state_inactive = {"my_inactive", 2}; +const rcl_state_t rcl_state_active = {"active", 3}; +const rcl_state_t rcl_state_finalized = {"finalized", 4}; + +const rcl_state_t rcl_state_configuring = {"configuring", 10}; +const rcl_state_t rcl_state_cleaningup = {"cleaningup", 11}; +const rcl_state_t rcl_state_shuttingdown = {"shuttingdown", 12}; +const rcl_state_t rcl_state_activating = {"activating", 13}; +const rcl_state_t rcl_state_deactivating = {"deactivating", 14}; +const rcl_state_t rcl_state_errorprocessing = {"errorprocessing", 15}; +// *INDENT-ON* + rcl_state_t -rcl_create_state(unsigned int index, char* label) +rcl_create_state(unsigned int index, char * label) { rcl_state_t ret_state = {.index = index, .label = label}; return ret_state; } rcl_state_transition_t -rcl_create_state_transition(unsigned int index, const char* label) +rcl_create_state_transition(unsigned int index, const char * label) { - rcl_state_transition_t ret_transition = {{.index = index, .label = label}, NULL, NULL, NULL}; + rcl_state_transition_t ret_transition = {{.index = index, .label = label}, + NULL, NULL, NULL, &rcl_state_errorprocessing}; return ret_transition; } // default implementation as despicted on // design.ros2.org -rcl_state_machine_t -rcl_get_default_state_machine() +rcl_ret_t +rcl_init_default_state_machine(rcl_state_machine_t * state_machine) { - rcl_state_machine_t state_machine; - state_machine.transition_map.primary_states = NULL; - state_machine.transition_map.transition_arrays = NULL; - state_machine.transition_map.size = 0; - - // set default state as unconfigured - //state_machine.current_state = &rcl_state_unconfigured; + // Maybe we can directly store only pointers to states + rcl_register_primary_state(&state_machine->transition_map, rcl_state_unconfigured); + rcl_register_primary_state(&state_machine->transition_map, rcl_state_inactive); + rcl_register_primary_state(&state_machine->transition_map, rcl_state_active); + rcl_register_primary_state(&state_machine->transition_map, rcl_state_finalized); - rcl_state_transition_t rcl_transition_configuring - = rcl_create_state_transition(rcl_state_configuring.index, rcl_state_configuring.label); - rcl_state_transition_t rcl_transition_shuttingdown - = rcl_create_state_transition(rcl_state_shuttingdown.index, rcl_state_shuttingdown.label); - rcl_state_transition_t rcl_transition_cleaningup - = rcl_create_state_transition(rcl_state_cleaningup.index, rcl_state_cleaningup.label); - rcl_state_transition_t rcl_transition_activating - = rcl_create_state_transition(rcl_state_activating.index, rcl_state_activating.label); - rcl_state_transition_t rcl_transition_deactivating - = rcl_create_state_transition(rcl_state_deactivating.index, rcl_state_deactivating.label);; - - rcl_register_primary_state(&state_machine.transition_map, rcl_state_unconfigured); - rcl_register_primary_state(&state_machine.transition_map, rcl_state_inactive); - rcl_register_primary_state(&state_machine.transition_map, rcl_state_active); - rcl_register_primary_state(&state_machine.transition_map, rcl_state_finalized); + rcl_state_transition_t rcl_transition_configuring = rcl_create_state_transition( + rcl_state_configuring.index, rcl_state_configuring.label); + rcl_state_transition_t rcl_transition_shuttingdown = rcl_create_state_transition( + rcl_state_shuttingdown.index, rcl_state_shuttingdown.label); + rcl_state_transition_t rcl_transition_cleaningup = rcl_create_state_transition( + rcl_state_cleaningup.index, rcl_state_cleaningup.label); + rcl_state_transition_t rcl_transition_activating = rcl_create_state_transition( + rcl_state_activating.index, rcl_state_activating.label); + rcl_state_transition_t rcl_transition_deactivating = rcl_create_state_transition( + rcl_state_deactivating.index, rcl_state_deactivating.label); // add transitions to map - rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_unconfigured, rcl_state_inactive, rcl_transition_configuring); - rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_inactive, rcl_state_unconfigured, rcl_transition_cleaningup); - rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_unconfigured, rcl_state_finalized, rcl_transition_shuttingdown); - rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_inactive, rcl_state_finalized, rcl_transition_shuttingdown); - rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_inactive, rcl_state_active, rcl_transition_activating); - rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_active, rcl_state_inactive, rcl_transition_deactivating); - rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_active, rcl_state_finalized, rcl_transition_shuttingdown); + rcl_register_transition_by_state(&state_machine->transition_map, + &rcl_state_unconfigured, &rcl_state_inactive, rcl_transition_configuring); + rcl_register_transition_by_state(&state_machine->transition_map, + &rcl_state_inactive, &rcl_state_unconfigured, rcl_transition_cleaningup); + rcl_register_transition_by_state(&state_machine->transition_map, + &rcl_state_unconfigured, &rcl_state_finalized, rcl_transition_shuttingdown); + rcl_register_transition_by_state(&state_machine->transition_map, + &rcl_state_inactive, &rcl_state_finalized, rcl_transition_shuttingdown); + rcl_register_transition_by_state(&state_machine->transition_map, + &rcl_state_inactive, &rcl_state_active, rcl_transition_activating); + rcl_register_transition_by_state(&state_machine->transition_map, + &rcl_state_active, &rcl_state_inactive, rcl_transition_deactivating); + rcl_register_transition_by_state(&state_machine->transition_map, + &rcl_state_active, &rcl_state_finalized, rcl_transition_shuttingdown); - state_machine.current_state = &state_machine.transition_map.primary_states[0]; // set to first entry - return state_machine; + // set to first entry + // state_machine->current_state = &state_machine->transition_map.primary_states[0]; + state_machine->current_state = &rcl_state_unconfigured; + return RCL_RET_OK; } #if __cplusplus } #endif // extern "C" - -#endif // RCL__STATE_MACHINE_H_ diff --git a/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.hxx b/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.hxx new file mode 100644 index 0000000000..031893272f --- /dev/null +++ b/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.hxx @@ -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 RCL_LIFECYCLE__DEFAULT_STATE_MACHINE_HXX_ +#define RCL_LIFECYCLE__DEFAULT_STATE_MACHINE_HXX_ + +#include + +#include "rcl_lifecycle/data_types.h" +#include "rcl_lifecycle/visibility_control.h" + +#if __cplusplus +extern "C" +{ +#endif + +LIFECYCLE_EXPORT +rcl_ret_t +rcl_init_default_state_machine(rcl_state_machine_t * state_machine); + +#if __cplusplus +} +#endif + +#endif // RCL_LIFECYCLE__DEFAULT_STATE_MACHINE_HXX_ diff --git a/rclcpp_lifecycle/src/rcl_lifecycle/lifecycle_state.c b/rclcpp_lifecycle/src/rcl_lifecycle/lifecycle_state.c deleted file mode 100644 index 279b8a5ca7..0000000000 --- a/rclcpp_lifecycle/src/rcl_lifecycle/lifecycle_state.c +++ /dev/null @@ -1,133 +0,0 @@ -// 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. - -#if __cplusplus -extern "C" -{ -#endif - -#include -#include - -#include "rcl_lifecycle/lifecycle_state.h" - -const rcl_state_transition_t* -rcl_is_valid_transition_by_index(rcl_state_machine_t* state_machine, unsigned int transition_index) -{ - unsigned int current_index = state_machine->current_state->index; - const rcl_transition_array_t* valid_transitions = rcl_get_transitions_by_index(&state_machine->transition_map, current_index); - for (unsigned int i=0; isize; ++i) - { - if (valid_transitions->transitions[i].transition_state.index == transition_index) - { - return &valid_transitions->transitions[i]; - } - } - return NULL; -} - -const rcl_state_transition_t* -rcl_is_valid_transition_by_label(rcl_state_machine_t* state_machine, const char* transition_label) -{ - unsigned int current_index = state_machine->current_state->index; - const rcl_transition_array_t* valid_transitions = rcl_get_transitions_by_index(&state_machine->transition_map, current_index); - for (unsigned int i=0; isize; ++i) - { - if (valid_transitions->transitions[i].transition_state.label == transition_label) - { - return &valid_transitions->transitions[i]; - } - } - return NULL; -} - -const rcl_state_transition_t* -rcl_get_registered_transition_by_index(rcl_state_machine_t* state_machine, unsigned int transition_state_index) -{ - // extensive search approach - // TODO(karsten1987) can be improved by having a separate array for "registered transition" - const rcl_transition_map_t* map = &state_machine->transition_map; - for (unsigned int i=0; isize; ++i) - { - for (unsigned int j=0; jtransition_arrays[i].size; ++j) - { - if (map->transition_arrays[i].transitions[j].transition_state.index == transition_state_index) - { - return &map->transition_arrays[i].transitions[j]; - } - } - } - return NULL; -} - -const rcl_state_transition_t* -rcl_get_registered_transition_by_label(rcl_state_machine_t* state_machine, const char* transition_state_label) -{ - // extensive search approach - // TODO(karsten1987) can be improved by having a separate array for "registered transition" - const rcl_transition_map_t* map = &state_machine->transition_map; - for (unsigned int i=0; isize; ++i) - { - for (unsigned int j=0; jtransition_arrays[i].size; ++j) - { - if (strcmp(map->transition_arrays[i].transitions[j].transition_state.label, transition_state_label) == 0) - { - return &map->transition_arrays[i].transitions[j]; - } - } - } - return NULL; -} - -void -rcl_register_callback(rcl_state_machine_t* state_machine, unsigned int state_index, unsigned int transition_index, bool(*fcn)(void)) -{ - rcl_transition_array_t* transitions = rcl_get_transitions_by_index(&state_machine->transition_map, state_index); - for (unsigned int i=0; isize; ++i) - { - if (transitions->transitions[i].transition_state.index == transition_index) - { - transitions->transitions[i].callback = fcn; - } - } -} - -// maybe change directly the current state here, -// no need to that all the time inside high level language -bool -rcl_invoke_transition(rcl_state_machine_t* state_machine, rcl_state_t transition_index) -{ - unsigned int current_index = state_machine->current_state->index; - rcl_transition_array_t* transitions = rcl_get_transitions_by_index(&state_machine->transition_map, current_index); - - if (transitions == NULL) - { - return false; - } - bool success = false; - for (unsigned int i=0; isize; ++i) - { - if (transitions->transitions[i].transition_state.index == transition_index.index) - { - ((bool(*)(void))transitions->transitions[i].callback)(); - success = true; - // break here ?! would allow only one to one transitions - } - } - return success; -} - -#if __cplusplus -} -#endif // extern "C" diff --git a/rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c b/rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c new file mode 100644 index 0000000000..5ca46eecb1 --- /dev/null +++ b/rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c @@ -0,0 +1,406 @@ +// 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 "rosidl_generator_c/message_type_support.h" +#include "rosidl_generator_c/string_functions.h" + +#include "rclcpp_lifecycle/msg/transition.h" +#include "rclcpp_lifecycle/srv/get_state.h" +#include "rclcpp_lifecycle/srv/change_state.h" + +#include "rcl_lifecycle/rcl_lifecycle.h" +#include "rcl_lifecycle/transition_map.h" + +#include "default_state_machine.hxx" + +static rclcpp_lifecycle__msg__Transition 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); + return true; +} + +// get zero initialized state machine here +rcl_state_machine_t +rcl_get_zero_initialized_state_machine(rcl_node_t * node_handle) +{ + rcl_state_machine_t state_machine; + state_machine.transition_map.size = 0; + state_machine.transition_map.primary_states = NULL; + state_machine.transition_map.transition_arrays = NULL; + state_machine.comm_interface.node_handle = node_handle; + state_machine.comm_interface.state_publisher = rcl_get_zero_initialized_publisher(); + state_machine.comm_interface.srv_get_state = rcl_get_zero_initialized_service(); + state_machine.comm_interface.srv_change_state = rcl_get_zero_initialized_service(); + + return state_machine; +} + +rcl_ret_t +rcl_state_machine_init(rcl_state_machine_t * state_machine, const char* node_name, bool default_states) +{ + // TODO(karsten1987): fail when state machine not zero initialized + // { // initialize node handle for notification + // rcl_node_options_t node_options = rcl_node_get_default_options(); + // { + // rcl_ret_t ret = rcl_node_init(&state_machine->notification_node_handle, + // node_name, &node_options); + // if (ret != RCL_RET_OK) { + // fprintf(stderr, "%s:%u, Unable to initialize node handle for state machine\n", + // __FILE__, __LINE__); + // state_machine = NULL; + // return ret; + // } + // } + // } + //const char * node_name = rcl_node_get_name(state_machine->notification_node_handle); + + { // initialize publisher + // Build topic, topic suffix hardcoded for now + // and limited in length of 255 + const char * topic_prefix = "__states"; + char * topic_name; + if (concatenate(&node_name, &topic_prefix, &topic_name) != true) { + fprintf(stderr, "%s:%u, Topic name exceeds maximum size of 255\n", + __FILE__, __LINE__); + state_machine = NULL; + return RCL_RET_ERROR; + } + + const rosidl_message_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT( + rclcpp_lifecycle, msg, Transition); + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_ret_t ret = rcl_publisher_init(&state_machine->comm_interface.state_publisher, + state_machine->comm_interface.node_handle, ts, topic_name, &publisher_options); + + if (ret != RCL_RET_OK) { + state_machine = NULL; + free(topic_name); + return ret; + } + //free(topic_name); + } + + { // 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) != true) { + fprintf(stderr, "%s:%u, Topic name exceeds maximum size of 255\n", + __FILE__, __LINE__); + state_machine = NULL; + return RCL_RET_ERROR; + } + + const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT( + rclcpp_lifecycle, srv, GetState); + rcl_service_options_t service_options = rcl_service_get_default_options(); + rcl_ret_t ret = rcl_service_init(&state_machine->comm_interface.srv_get_state, + state_machine->comm_interface.node_handle, ts, topic_name, &service_options); + if (ret != RCL_RET_OK) { + state_machine = NULL; + free(topic_name); + return ret; + } + //free(topic_name); + } + + { // 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) != true) { + fprintf(stderr, "%s:%u, Topic name exceeds maximum size of 255\n", + __FILE__, __LINE__); + state_machine = NULL; + return RCL_RET_ERROR; + } + + const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT( + rclcpp_lifecycle, srv, ChangeState); + rcl_service_options_t service_options = rcl_service_get_default_options(); + rcl_ret_t ret = rcl_service_init(&state_machine->comm_interface.srv_change_state, + state_machine->comm_interface.node_handle, ts, topic_name, &service_options); + if (ret != RCL_RET_OK) { + state_machine = NULL; + free(topic_name); + return ret; + } + //free(topic_name); + } + + if (default_states) { + rcl_init_default_state_machine(state_machine); + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_state_machine_fini(rcl_state_machine_t * state_machine) +{ + { // destroy get state srv + rcl_ret_t ret = rcl_service_fini(&state_machine->comm_interface.srv_get_state, + state_machine->comm_interface.node_handle); + if (ret != RCL_RET_OK) { + fprintf(stderr, "%s:%u, Failed to destroy get_state_srv service\n", + __FILE__, __LINE__); + } + } + + { // destroy change state srv + rcl_ret_t ret = rcl_service_fini(&state_machine->comm_interface.srv_change_state, + state_machine->comm_interface.node_handle); + if (ret != RCL_RET_OK) { + fprintf(stderr, "%s:%u, Failed to destroy change_state_srv service\n", + __FILE__, __LINE__); + } + } + + { // destroy the publisher + rcl_ret_t ret = rcl_publisher_fini(&state_machine->comm_interface.state_publisher, + state_machine->comm_interface.node_handle); + if (ret != RCL_RET_OK) { + fprintf(stderr, "%s:%u, Failed to destroy state publisher publisher\n", + __FILE__, __LINE__); + } + } + // { // destroy the node handle + // rcl_ret_t ret = rcl_node_fini(&state_machine->notification_node_handle); + // if (ret != RCL_RET_OK) { + // fprintf(stderr, "%s:%u, Failed to destroy lifecycle notification node handle\n", + // __FILE__, __LINE__); + // } + // } + + rcl_transition_map_t * transition_map = &state_machine->transition_map; + + // free the primary states array + free(transition_map->primary_states); + transition_map->primary_states = NULL; + for (unsigned int i = 0; i < transition_map->size; ++i) { + // free each transition array associated to a primary state + free(transition_map->transition_arrays[i].transitions); + transition_map->transition_arrays[i].transitions = NULL; + } + // free the top level transition array + free(transition_map->transition_arrays); + transition_map->transition_arrays = NULL; + + return RCL_RET_OK; +} + +const rcl_state_transition_t * +rcl_is_valid_transition_by_index(rcl_state_machine_t * state_machine, + unsigned int transition_index) +{ + unsigned int current_index = state_machine->current_state->index; + const rcl_transition_array_t * valid_transitions = rcl_get_transitions_by_index( + &state_machine->transition_map, current_index); + if (valid_transitions == NULL) { + fprintf(stderr, "%s:%u, No transitions registered for current state %s\n", + __FILE__, __LINE__, state_machine->current_state->label); + return NULL; + } + for (unsigned int i = 0; i < valid_transitions->size; ++i) { + if (valid_transitions->transitions[i].transition_state.index == transition_index) { + return &valid_transitions->transitions[i]; + } + } + fprintf(stderr, "%s:%u, No transition matching %u found for current state %s\n", + __FILE__, __LINE__, transition_index, state_machine->current_state->label); + return NULL; +} + +const rcl_state_transition_t * +rcl_is_valid_transition_by_label(rcl_state_machine_t * state_machine, + const char * transition_label) +{ + unsigned int current_index = state_machine->current_state->index; + const rcl_transition_array_t * valid_transitions = rcl_get_transitions_by_index( + &state_machine->transition_map, current_index); + for (unsigned int i = 0; i < valid_transitions->size; ++i) { + if (valid_transitions->transitions[i].transition_state.label == transition_label) { + return &valid_transitions->transitions[i]; + } + } + return NULL; +} + +const rcl_state_transition_t * +rcl_get_registered_transition_by_index(rcl_state_machine_t * state_machine, + unsigned int transition_state_index) +{ + // extensive search approach + // TODO(karsten1987) can be improved by having a separate array + // for "registered transition" + const rcl_transition_map_t * map = &state_machine->transition_map; + for (unsigned int i = 0; i < map->size; ++i) { + for (unsigned int j = 0; j < map->transition_arrays[i].size; ++j) { + if (map->transition_arrays[i].transitions[j].transition_state.index == + transition_state_index) + { + return &map->transition_arrays[i].transitions[j]; + } + } + } + return NULL; +} + +const rcl_state_transition_t * +rcl_get_registered_transition_by_label(rcl_state_machine_t * state_machine, + const char * transition_state_label) +{ + // extensive search approach + // TODO(karsten1987) can be improved by having a separate array + // for "registered transition" + const rcl_transition_map_t * map = &state_machine->transition_map; + for (unsigned int i = 0; i < map->size; ++i) { + for (unsigned int j = 0; j < map->transition_arrays[i].size; ++j) { + if (strcmp(map->transition_arrays[i].transitions[j].transition_state.label, + transition_state_label) == 0) + { + return &map->transition_arrays[i].transitions[j]; + } + } + } + return NULL; +} + +void +rcl_register_callback(rcl_state_machine_t * state_machine, + unsigned int state_index, unsigned int transition_index, bool (* fcn)(void)) +{ + rcl_transition_array_t * transitions = rcl_get_transitions_by_index( + &state_machine->transition_map, state_index); + for (unsigned int i = 0; i < transitions->size; ++i) { + if (transitions->transitions[i].transition_state.index == transition_index) { + transitions->transitions[i].callback = fcn; + } + } +} + +// maybe change directly the current state here, +// no need to that all the time inside high level language +bool +rcl_start_transition_by_index(rcl_state_machine_t * state_machine, + unsigned int transition_index) +{ + const rcl_state_transition_t * transition = + rcl_is_valid_transition_by_index(state_machine, transition_index); + + // If we have a faulty transition pointer + if (transition == NULL) { + fprintf(stderr, "%s:%d, Could not find registered transition %u\n", + __FILE__, __LINE__, transition_index); + return false; + } + + // If we have a transition which is semantically not correct + // we may have to set the current state to something intermediate + // or simply ignore it + if (transition->start != state_machine->current_state) { + fprintf(stderr, "%s:%d, Wrong transition index %s. State machine is in primary state %s\n", + __FILE__, __LINE__, transition->start->label, state_machine->current_state->label); + return false; + } + + // do the initialization + rclcpp_lifecycle__msg__Transition__init(&msg); + msg.start_state = state_machine->current_state->index; + msg.goal_state = transition->transition_state.index; + + if (rcl_publish(&state_machine->comm_interface.state_publisher, &msg) != RCL_RET_OK) { + fprintf(stderr, "%s:%d, Couldn't publish the notification message.\n", + __FILE__, __LINE__); + } + rclcpp_lifecycle__msg__Transition__fini(&msg); + + // Apply a transition state + state_machine->current_state = &transition->transition_state; + + return true; +} + +bool +rcl_finish_transition_by_index(rcl_state_machine_t * state_machine, + unsigned int transition_index, bool success) +{ + const rcl_state_transition_t * transition = + rcl_get_registered_transition_by_index(state_machine, transition_index); + + // If we have a faulty transition pointer + if (transition == NULL) { + fprintf(stderr, "%s:%d, Could not find registered transition %u\n", + __FILE__, __LINE__, transition_index); + return false; + } + + // If we have a transition which is semantically not correct + // we may have to set the current state to something intermediate + // or simply ignore it + if (&transition->transition_state != state_machine->current_state) { + fprintf(stderr, "%s:%d, Wrong transition state. State machine is in primary state %s\n", + __FILE__, __LINE__, state_machine->current_state->label); + return false; + } + + // high level transition(callback) was executed correctly + if (success == true) { + rclcpp_lifecycle__msg__Transition__init(&msg); + msg.start_state = transition->transition_state.index; + msg.goal_state = transition->goal->index; + if (rcl_publish(&state_machine->comm_interface.state_publisher, &msg) != RCL_RET_OK) { + fprintf(stderr, "%s:%d, Couldn't publish the notification message.\n", + __FILE__, __LINE__); + } + rclcpp_lifecycle__msg__Transition__fini(&msg); + state_machine->current_state = transition->goal; + return true; + } + + rclcpp_lifecycle__msg__Transition__init(&msg); + msg.start_state = transition->transition_state.index; + msg.goal_state = transition->error->index; + if (rcl_publish(&state_machine->comm_interface.state_publisher, &msg) != RCL_RET_OK) { + fprintf(stderr, "%s:%d, Couldn't publish the notification message.\n", + __FILE__, __LINE__); + } + rclcpp_lifecycle__msg__Transition__fini(&msg); + state_machine->current_state = transition->error; + return true; +} + +#if __cplusplus +} +#endif // extern "C" diff --git a/rclcpp_lifecycle/src/rcl_lifecycle/transition_map.c b/rclcpp_lifecycle/src/rcl_lifecycle/transition_map.c index beecb8330c..1dca057f90 100644 --- a/rclcpp_lifecycle/src/rcl_lifecycle/transition_map.c +++ b/rclcpp_lifecycle/src/rcl_lifecycle/transition_map.c @@ -21,187 +21,168 @@ extern "C" #include #include -#include -#include +#include "rcl_lifecycle/transition_map.h" -void rcl_register_primary_state(rcl_transition_map_t* m, rcl_state_t primary_state) +void +rcl_register_primary_state(rcl_transition_map_t * m, + rcl_state_t primary_state) { - if (rcl_get_primary_state_by_index(m, primary_state.index) != NULL) - { + if (rcl_get_primary_state_by_index(m, primary_state.index) != NULL) { // primary state is already registered + fprintf(stderr, "%s:%u, Primary state %u is already registered\n", + __FILE__, __LINE__, primary_state.index); return; } // add new primary state memory - //rcl_transition_map_index_t idx = {.index = m->size, .label = state.label}; ++m->size; - m->primary_states = realloc(m->primary_states, m->size*sizeof(rcl_state_t)); - m->primary_states[m->size-1] = primary_state; - - // allocate empty pointer for transition_array_t - m->transition_arrays = realloc(m->transition_arrays, m->size*sizeof(rcl_transition_array_t)); - m->transition_arrays[m->size-1].transitions = NULL; - m->transition_arrays[m->size-1].size = 0; // initialize array to size 0 + if (m->size == 1) { + m->primary_states = malloc(m->size * sizeof(rcl_state_t)); + m->transition_arrays = malloc(m->size * sizeof(rcl_transition_array_t)); + } else { + m->primary_states = realloc( + m->primary_states, m->size * sizeof(rcl_state_t)); + m->transition_arrays = realloc( + m->transition_arrays, m->size * sizeof(rcl_transition_array_t)); + } + m->primary_states[m->size - 1] = primary_state; + m->transition_arrays[m->size - 1].transitions = NULL; + m->transition_arrays[m->size - 1].size = 0; // initialize array to size 0 } -void rcl_register_transition_by_state(rcl_transition_map_t* m, rcl_state_t start, rcl_state_t goal, rcl_state_transition_t transition) +void +rcl_register_transition_by_state(rcl_transition_map_t * m, + const rcl_state_t * start, const rcl_state_t * goal, rcl_state_transition_t transition) { - rcl_register_transition_by_index(m, start.index, goal.index, transition); + transition.start = start; + transition.goal = goal; + + // TODO(karsten1987): check whether we can improve that + rcl_transition_array_t * transition_array = rcl_get_transitions_by_index( + m, transition.start->index); + if (!transition_array) { + fprintf(stderr, "%s:%u, Unable to find transition array registered for start index %u", + __FILE__, __LINE__, transition.start->index); + return; + } + + // we add a new transition, so increase the size + ++transition_array->size; + if (transition_array->size == 1) { + transition_array->transitions = malloc( + transition_array->size * sizeof(rcl_state_transition_t)); + } else { + transition_array->transitions = realloc( + transition_array->transitions, transition_array->size * sizeof(rcl_state_transition_t)); + } + // finally set the new transition to the end of the array + transition_array->transitions[transition_array->size - 1] = transition; } -void rcl_register_transition_by_label(rcl_transition_map_t* m, const char* start_label, const char* goal_label, rcl_state_transition_t transition) +void +rcl_register_transition_by_label(rcl_transition_map_t * m, + const char * start_label, const char * goal_label, rcl_state_transition_t transition) { // the idea here is to add this transition based on the // start label and classify them. - rcl_state_t* start_state = rcl_get_primary_state_by_label(m, start_label); - if (start_state == NULL) - { + const rcl_state_t * start_state = rcl_get_primary_state_by_label(m, start_label); + if (start_state == NULL) { // return false here? return; } - rcl_state_t* goal_state = rcl_get_primary_state_by_label(m, goal_label); - if (goal_state == NULL) - { + const rcl_state_t * goal_state = rcl_get_primary_state_by_label(m, goal_label); + if (goal_state == NULL) { // return false here? return; } - - transition.start = start_state; - transition.goal = goal_state; - - rcl_transition_array_t* transition_array = rcl_get_transitions_by_index(m, transition.start->index); - if (!transition_array) - { - return; - } - - // we add a new transition, so increase the size - ++transition_array->size; - - // TODO: Not sure if this is really necessary to differentiate - // between malloc and realloc - //if (transition_array->size == 1) - //{ - // transition_array->transitions = malloc(sizeof(rcl_state_transition_t)); - //} - //else - //{ - transition_array->transitions - = realloc(transition_array->transitions, transition_array->size*sizeof(rcl_state_transition_t)); - //} - - // finally set the new transition to the end of the array - transition_array->transitions[transition_array->size-1] = transition; + rcl_register_transition_by_state(m, start_state, goal_state, transition); } -void rcl_register_transition_by_index(rcl_transition_map_t* m, unsigned int start_index, unsigned int goal_index, rcl_state_transition_t transition) +void +rcl_register_transition_by_index(rcl_transition_map_t * m, + unsigned int start_index, unsigned int goal_index, rcl_state_transition_t transition) { // the idea here is to add this transition based on the // start label and classify them. - rcl_state_t* start_state = rcl_get_primary_state_by_index(m, start_index); - if (start_state == NULL) - { + const rcl_state_t * start_state = rcl_get_primary_state_by_index(m, start_index); + if (start_state == NULL) { // return false here? return; } - rcl_state_t* goal_state = rcl_get_primary_state_by_index(m, goal_index); - if (goal_state == NULL) - { + const rcl_state_t * goal_state = rcl_get_primary_state_by_index(m, goal_index); + if (goal_state == NULL) { // return false here? return; } - - transition.start = start_state; - transition.goal = goal_state; - - rcl_transition_array_t* transition_array = rcl_get_transitions_by_index(m, transition.start->index); - if (!transition_array) - { - return; - } - - // we add a new transition, so increase the size - ++transition_array->size; - - // TODO: Not sure if this is really necessary to differentiate - // between malloc and realloc - //if (transition_array->size == 1) - //{ - // transition_array->transitions = malloc(sizeof(rcl_state_transition_t)); - //} - //else - //{ - transition_array->transitions - = realloc(transition_array->transitions, transition_array->size*sizeof(rcl_state_transition_t)); - //} - - // finally set the new transition to the end of the array - transition_array->transitions[transition_array->size-1] = transition; + rcl_register_transition_by_state(m, start_state, goal_state, transition); } -rcl_state_t* rcl_get_primary_state_by_label(rcl_transition_map_t* m, const char* label) +rcl_state_t * +rcl_get_primary_state_by_label(rcl_transition_map_t * m, + const char * label) { - for (unsigned int i=0; isize; ++i) - { - if (m->primary_states[i].label == label) - { + for (unsigned int i = 0; i < m->size; ++i) { + if (m->primary_states[i].label == label) { return &m->primary_states[i]; } } return NULL; } -rcl_state_t* rcl_get_primary_state_by_index(rcl_transition_map_t* m, unsigned int index) +rcl_state_t * +rcl_get_primary_state_by_index(rcl_transition_map_t * m, + unsigned int index) { - for (unsigned int i=0; isize; ++i) - { - if (m->primary_states[i].index == index) - { + for (unsigned int i = 0; i < m->size; ++i) { + if (m->primary_states[i].index == index) { return &m->primary_states[i]; } } return NULL; } -rcl_transition_array_t* rcl_get_transitions_by_label(rcl_transition_map_t* m, const char* label) +rcl_transition_array_t * +rcl_get_transitions_by_label(rcl_transition_map_t * m, + const char * label) { - for (unsigned int i=0; isize; ++i) - { - if (strcmp(m->primary_states[i].label, label) == 0) - { + for (unsigned int i = 0; i < m->size; ++i) { + if (strcmp(m->primary_states[i].label, label) == 0) { return &m->transition_arrays[i]; } } return NULL; } -rcl_transition_array_t* rcl_get_transitions_by_index(rcl_transition_map_t* m, unsigned int index) +rcl_transition_array_t * +rcl_get_transitions_by_index(rcl_transition_map_t * m, + unsigned int index) { - for (unsigned int i=0; isize; ++i) - { - if (m->primary_states[i].index == index ) - { + for (unsigned int i = 0; i < m->size; ++i) { + if (m->primary_states[i].index == index) { return &m->transition_arrays[i]; } } return NULL; } -void rcl_print_transition_array(const rcl_transition_array_t* ta) +void +rcl_print_transition_array(const rcl_transition_array_t * ta) { - for (unsigned int i=0; isize; ++i) - { - printf("%s(%u)(%s)\t", ta->transitions[i].transition_state.label, ta->transitions[i].transition_state.index, (ta->transitions[i].callback==NULL)?" ":"x"); - } - printf("\n"); + for (unsigned int i = 0; i < ta->size; ++i) { + printf("%s(%u)(%s)\t", ta->transitions[i].transition_state.label, + ta->transitions[i].transition_state.index, + (ta->transitions[i].callback == NULL) ? " " : "x"); + } + printf("\n"); } -void rcl_print_transition_map(const rcl_transition_map_t* m) +void +rcl_print_transition_map(const rcl_transition_map_t * m) { printf("rcl_transition_map_t size %u\n", m->size); - for (unsigned int i=0; isize; ++i) - { - printf("Start state %s(%u) ::: ", m->primary_states[i].label, m->primary_states[i].index); + for (unsigned int i = 0; i < m->size; ++i) { + printf("Start state %s(%u) ::: ", m->primary_states[i].label, + m->primary_states[i].index); rcl_print_transition_array(&(m->transition_arrays[i])); } } diff --git a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp index 34d61407e2..7ca1b7885d 100644 --- a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp +++ b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include +#include + +#include "rclcpp_lifecycle/lifecycle_manager.hpp" #include "lifecycle_manager_impl.hpp" // implementation @@ -21,85 +24,96 @@ namespace rclcpp namespace lifecycle { -using NodeInterfacePtr = std::shared_ptr; - -LifecycleManager::LifecycleManager(): - impl_(new LifecycleManagerImpl()) -{} - +LifecycleManager::LifecycleManager() +: impl_(new LifecycleManagerImpl()) +{ + impl_->init(); +} LifecycleManager::~LifecycleManager() = default; +std::shared_ptr +LifecycleManager::get_node_base_interface() +{ + return impl_->node_base_handle_; +} + void -LifecycleManager::add_node_interface(const NodeInterfacePtr& node_interface) +LifecycleManager::add_node_interface(const NodePtr & node) { - impl_->add_node_interface(node_interface); + add_node_interface(node->get_base_interface()->get_name(), node); } void -LifecycleManager::add_node_interface(const NodeInterfacePtr& node_interface, rcl_state_machine_t custom_state_machine) +LifecycleManager::add_node_interface(const std::string & node_name, + const NodeInterfacePtr & node_interface) { - impl_->add_node_interface(node_interface, custom_state_machine); + impl_->add_node_interface(node_name, node_interface); } bool -LifecycleManager::register_on_configure(const std::string& node_name, std::function& fcn) +LifecycleManager::register_on_configure(const std::string & node_name, + std::function & fcn) { return impl_->register_callback(node_name, fcn); } bool -LifecycleManager::configure(const std::string& node_name) +LifecycleManager::configure(const std::string & node_name) { - return impl_->change_state(node_name); + return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::CONFIGURING); } bool -LifecycleManager::register_on_cleanup(const std::string& node_name, std::function& fcn) +LifecycleManager::register_on_cleanup(const std::string & node_name, + std::function & fcn) { return impl_->register_callback(node_name, fcn); } bool -LifecycleManager::cleanup(const std::string& node_name) +LifecycleManager::cleanup(const std::string & node_name) { - return impl_->change_state(node_name); + return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::CLEANINGUP); } bool -LifecycleManager::register_on_shutdown(const std::string& node_name, std::function& fcn) +LifecycleManager::register_on_shutdown(const std::string & node_name, + std::function & fcn) { return impl_->register_callback(node_name, fcn); } bool -LifecycleManager::shutdown(const std::string& node_name) +LifecycleManager::shutdown(const std::string & node_name) { - return impl_->change_state(node_name); + return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::SHUTTINGDOWN); } bool -LifecycleManager::register_on_activate(const std::string& node_name, std::function& fcn) +LifecycleManager::register_on_activate(const std::string & node_name, + std::function & fcn) { return impl_->register_callback(node_name, fcn); } bool -LifecycleManager::activate(const std::string& node_name) +LifecycleManager::activate(const std::string & node_name) { - return impl_->change_state(node_name); + return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::ACTIVATING); } bool -LifecycleManager::register_on_deactivate(const std::string& node_name, std::function& fcn) +LifecycleManager::register_on_deactivate(const std::string & node_name, + std::function & fcn) { return impl_->register_callback(node_name, fcn); } bool -LifecycleManager::deactivate(const std::string& node_name) +LifecycleManager::deactivate(const std::string & node_name) { - return impl_->change_state(node_name); + return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::DEACTIVATING); } } // namespace lifecycle diff --git a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp index 0e5e9e5ee2..557eb105c1 100644 --- a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp +++ b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp @@ -12,63 +12,191 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__LIFECYCLE_MANAGER_IMPL_H_ -#define RCLCPP__LIFECYCLE_MANAGER_IMPL_H_ +#ifndef RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_IMPL_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_IMPL_HPP_ +#include #include #include #include +#include -#include -#include +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_manager.hpp" +#include "rclcpp_lifecycle/srv/get_state.hpp" +#include "rclcpp_lifecycle/srv/change_state.hpp" + +#include "rcl_lifecycle/rcl_lifecycle.h" namespace rclcpp { namespace lifecycle { -using NodeInterfacePtr = std::shared_ptr; -using NodeInterfaceWeakPtr = std::weak_ptr; +using LifecycleInterface = rclcpp::node::lifecycle::LifecycleNode; +using LifecycleInterfacePtr = std::shared_ptr; +using LifecycleInterfaceWeakPtr = std::weak_ptr; struct NodeStateMachine { - NodeInterfaceWeakPtr weak_node_handle; + LifecycleInterfaceWeakPtr weak_node_handle; rcl_state_machine_t state_machine; std::map> cb_map; + // std::shared_ptr> srv_get_state; + // std::shared_ptr> srv_change_state; + std::shared_ptr srv_get_state; + std::shared_ptr srv_change_state; }; class LifecycleManager::LifecycleManagerImpl { public: - LifecycleManagerImpl() = default; - ~LifecycleManagerImpl() = default; + LifecycleManagerImpl() + : node_base_handle_(std::make_shared("lifecycle_manager")) + { + } + + ~LifecycleManagerImpl() + { + fprintf(stderr, "Going to detroy my lifecycle manager\n"); + for (auto it = node_handle_map_.begin(); it != node_handle_map_.end(); ++it) { + rcl_state_machine_t * rcl_state_machine = &it->second.state_machine; + if (!rcl_state_machine) + fprintf(stderr, "Hell is burning \n"); + rcl_state_machine_fini(rcl_state_machine); + } + } + + void + init() + { + //srv_get_state_ = node_base_handle_->create_service( + // node_base_handle_->get_name() + "__get_state", + // std::bind(&LifecycleManagerImpl::on_get_state, this, + // std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + //srv_change_state_ = node_base_handle_->create_service( + // node_base_handle_->get_name() + "__change_state", + // std::bind(&LifecycleManagerImpl::on_change_state, this, + // std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + } + + void + on_get_state(const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) + { + on_get_single_state(header, req, resp, req->node_name); + } + + void + on_get_single_state(const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp, + std::string node_name) + { + (void)header; + (void)req; + auto node_handle_iter = node_handle_map_.find(node_name); + if (node_handle_iter == node_handle_map_.end()) { + resp->current_state = static_cast(LifecyclePrimaryStatesT::UNKNOWN); + return; + } + resp->current_state = + static_cast(node_handle_iter->second.state_machine.current_state->index); + } - LIFECYCLE_EXPORT void - add_node_interface(const NodeInterfacePtr& node_interface) + on_change_state(const std::shared_ptrheader, + const std::shared_ptr req, + std::shared_ptr resp) { - rcl_state_machine_t state_machine = rcl_get_default_state_machine(); - add_node_interface(node_interface, state_machine); + on_change_single_state(header, req, resp, req->node_name); } - LIFECYCLE_EXPORT void - add_node_interface(const NodeInterfacePtr& node_interface, rcl_state_machine_t custom_state_machine) + on_change_single_state(const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp, + std::string node_name) + { + (void)header; + auto node_handle_iter = node_handle_map_.find(node_name); + if (node_handle_iter == node_handle_map_.end()) { + resp->success = false; + return; + } + auto transition = static_cast(req->transition); + resp->success = change_state(node_name, transition); + } + + void + add_node_interface(const std::string & node_name, + const LifecycleInterfacePtr & lifecycle_interface) { NodeStateMachine node_state_machine; - node_state_machine.weak_node_handle = node_interface; - // TODO(karsten1987): Find a way to make this generic to an enduser - node_state_machine.state_machine = custom_state_machine; + node_state_machine.state_machine = rcl_get_zero_initialized_state_machine( + node_base_handle_->get_rcl_node_handle()); + fprintf(stderr, "Created get service address %p\n", &node_state_machine.state_machine.comm_interface.srv_get_state); + fprintf(stderr, "Created change service address %p\n", &node_state_machine.state_machine.comm_interface.srv_change_state); + rcl_state_machine_init(&node_state_machine.state_machine, node_name.c_str(), true); + + // srv objects may get destroyed directly here + { // get_state + std::function, + const std::shared_ptr, + std::shared_ptr)> cb = + std::bind(&LifecycleManagerImpl::on_get_single_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + node_name); + + rclcpp::any_service_callback::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + auto srv_get_state = + rclcpp::service::Service::make_shared( + //node_state_machine.state_machine.comm_interface.node_handle, + node_base_handle_->get_rcl_node_handle(), + &node_state_machine.state_machine.comm_interface.srv_get_state, + any_cb); + auto srv_get_state_base = std::dynamic_pointer_cast(srv_get_state); + node_base_handle_->add_service(srv_get_state_base); + node_state_machine.srv_get_state = srv_get_state_base; + } + { // change_state + std::function, + const std::shared_ptr, + std::shared_ptr)> cb = + std::bind(&LifecycleManagerImpl::on_change_single_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + node_name); + + rclcpp::any_service_callback::AnyServiceCallback any_cb; + any_cb.set(cb); + auto srv_change_state = + std::make_shared>( + //node_state_machine.state_machine.comm_interface.node_handle, + node_base_handle_->get_rcl_node_handle(), + &node_state_machine.state_machine.comm_interface.srv_change_state, + any_cb); + auto srv_change_state_base = std::dynamic_pointer_cast(srv_change_state); + //node_base_handle_->add_service(srv_change_state_base); + node_state_machine.srv_change_state = srv_change_state_base; + } + node_state_machine.weak_node_handle = lifecycle_interface; // register default callbacks // maybe optional - using NodeInterface = node::lifecycle::LifecycleNodeInterface; - std::function cb_configuring = std::bind(&NodeInterface::on_configure, node_interface); - std::function cb_cleaningup = std::bind(&NodeInterface::on_cleanup, node_interface); - std::function cb_shuttingdown = std::bind(&NodeInterface::on_shutdown, node_interface); - std::function cb_activating = std::bind(&NodeInterface::on_activate, node_interface); - std::function cb_deactivating = std::bind(&NodeInterface::on_deactivate, node_interface); - std::function cb_error = std::bind(&NodeInterface::on_error, node_interface); + std::function cb_configuring = std::bind( + &LifecycleInterface::on_configure, lifecycle_interface); + std::function cb_cleaningup = std::bind( + &LifecycleInterface::on_cleanup, lifecycle_interface); + std::function cb_shuttingdown = std::bind( + &LifecycleInterface::on_shutdown, lifecycle_interface); + std::function cb_activating = std::bind( + &LifecycleInterface::on_activate, lifecycle_interface); + std::function cb_deactivating = std::bind( + &LifecycleInterface::on_deactivate, lifecycle_interface); + std::function cb_error = std::bind( + &LifecycleInterface::on_error, lifecycle_interface); node_state_machine.cb_map[LifecycleTransitionsT::CONFIGURING] = cb_configuring; node_state_machine.cb_map[LifecycleTransitionsT::CLEANINGUP] = cb_cleaningup; node_state_machine.cb_map[LifecycleTransitionsT::SHUTTINGDOWN] = cb_shuttingdown; @@ -76,66 +204,81 @@ class LifecycleManager::LifecycleManagerImpl node_state_machine.cb_map[LifecycleTransitionsT::DEACTIVATING] = cb_deactivating; node_state_machine.cb_map[LifecycleTransitionsT::ERRORPROCESSING] = cb_error; - // TODO(karsten1987): clarify what do if node already exists - auto node_name = node_interface->get_name(); + // TODO(karsten1987): clarify what do if node already exists; node_handle_map_[node_name] = node_state_machine; } template bool - register_callback(const std::string& node_name, std::function& cb) + register_callback(const std::string & node_name, std::function & cb) { - if (node_name.empty()) + if (node_name.empty()) { return false; + } auto node_handle_iter = node_handle_map_.find(node_name); - if (node_handle_iter == node_handle_map_.end()) - { + if (node_handle_iter == node_handle_map_.end()) { fprintf(stderr, "Node with name %s is not registered\n", node_name.c_str()); } node_handle_iter->second.cb_map[lifecycle_transition] = cb; return true; } - template bool - change_state(const std::string& node_name = "") + change_state(const std::string & node_name, LifecycleTransitionsT lifecycle_transition) { - if (node_name.empty()) + if (node_name.empty()) { return false; + } auto node_handle_iter = node_handle_map_.find(node_name); - if (node_handle_iter == node_handle_map_.end()) - { - fprintf(stderr, "Node with name %s is not registered\n", node_name.c_str()); + if (node_handle_iter == node_handle_map_.end()) { + fprintf(stderr, "%s:%d, Node with name %s is not registered\n", + __FILE__, __LINE__, node_name.c_str()); return false; } auto node_handle = node_handle_iter->second.weak_node_handle.lock(); - if (!node_handle) + if (!node_handle) { + fprintf(stderr, + "%s:%d, Nodehandle is not available. Was it destroyed outside the lifecycle manager?\n", + __FILE__, __LINE__); return false; + } - // ask RCL if this is a valid state - const rcl_state_transition_t* transition - = rcl_is_valid_transition_by_index(&node_handle_iter->second.state_machine, static_cast(lifecycle_transition)); - if (transition == NULL) + unsigned int transition_index = static_cast(lifecycle_transition); + if (!rcl_start_transition_by_index(&node_handle_iter->second.state_machine, transition_index)) { + fprintf(stderr, "%s:%d, Unable to start transition %u from current state %s\n", + __FILE__, __LINE__, transition_index, + node_handle_iter->second.state_machine.current_state->label); return false; + } + // Since we set always set a default callback, + // we don't have to check for nullptr here std::function callback = node_handle_iter->second.cb_map[lifecycle_transition]; - if(!callback()) + auto success = callback(); + + if (!rcl_finish_transition_by_index(&node_handle_iter->second.state_machine, + transition_index, success)) { - node_handle->on_error(); - node_handle_iter->second.state_machine.current_state = &rcl_state_errorprocessing; + fprintf(stderr, "Failed to finish transition %u. Current state is now: %s\n", + transition_index, node_handle_iter->second.state_machine.current_state->label); return false; } - node_handle_iter->second.state_machine.current_state = transition->goal; + // This true holds in both cases where the actual callback + // was successful or not, since at this point we have a valid transistion + // to either a new primary state or error state return true; } -private: +// private: + std::shared_ptr node_base_handle_; + std::shared_ptr> srv_get_state_; + std::shared_ptr> srv_change_state_; std::map node_handle_map_; }; } // namespace lifecycle } // namespace rclcpp -#endif +#endif // RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_IMPL_HPP_ diff --git a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_talker.cpp b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_talker.cpp index 382b7d39fe..a4f611f1ee 100644 --- a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_talker.cpp +++ b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_talker.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp/publisher.hpp" @@ -23,64 +24,268 @@ #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "std_msgs/msg/string.hpp" +#include "rclcpp_lifecycle/msg/transition.hpp" +#include "rclcpp_lifecycle/srv/get_state.hpp" +#include "rclcpp_lifecycle/srv/change_state.hpp" -int main(int argc, char * argv[]) +class LifecycleTalker : public rclcpp::node::lifecycle::LifecycleNode { - rclcpp::init(argc, argv); +public: + explicit LifecycleTalker(const std::string & node_name, bool intra_process_comms = false) + : rclcpp::node::lifecycle::LifecycleNode(node_name, intra_process_comms) + { + msg_ = std::make_shared(); + + pub_ = this->create_publisher("lifecycle_chatter"); + timer_ = this->get_communication_interface()->create_wall_timer( + 1_s, std::bind(&LifecycleTalker::publish, this)); + } - std::shared_ptr lc_node = std::make_shared("lc_talker"); + void publish() + { + static size_t count = 0; + msg_->data = "Lifecycle HelloWorld #" + std::to_string(++count); + pub_->publish(msg_); + } - rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; - custom_qos_profile.depth = 7; + bool on_configure() + { + printf("%s is going to configure its node\n", get_name().c_str()); + return true; + } - std::shared_ptr> chatter_pub = - lc_node->create_publisher("chatter", custom_qos_profile); + bool on_activate() + { + rclcpp::node::lifecycle::LifecycleNode::enable_communication(); + printf("%s is going to activate its node\n", get_name().c_str()); + return true; + } - rclcpp::lifecycle::LifecycleManager lm; - lm.add_node_interface(lc_node); + bool on_deactivate() + { + rclcpp::node::lifecycle::LifecycleNode::disable_communication(); + printf("%s is going to deactivate its node\n", get_name().c_str()); + return true; + } - // configure - lm.configure("my_node1"); +private: + std::shared_ptr msg_; + std::shared_ptr> pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; - rclcpp::WallRate loop_rate(2); +class LifecycleListener : public rclcpp::node::Node +{ +public: + explicit LifecycleListener(const std::string & node_name) + : rclcpp::node::Node(node_name) + { + sub_data_ = this->create_subscription("lifecycle_chatter", + std::bind(&LifecycleListener::data_callback, this, std::placeholders::_1)); + sub_notification_ = this->create_subscription( + "lifecycle_manager__lc_talker", std::bind(&LifecycleListener::notification_callback, this, + std::placeholders::_1)); + } - auto msg = std::make_shared(); - auto i = 1; + void data_callback(const std_msgs::msg::String::SharedPtr msg) + { + std::cout << "I heard data: [" << msg->data << "]" << std::endl; + } - while (rclcpp::ok() && i <= 10) { - msg->data = "Hello World: " + std::to_string(i++); - //std::cout << "Publishing: '" << msg->data << "'" << std::endl; - chatter_pub->publish(msg); - rclcpp::spin_some(lc_node); - loop_rate.sleep(); + void notification_callback(const rclcpp_lifecycle::msg::Transition::SharedPtr msg) + { + std::cout << "Transition triggered:: [ Going from state " << + static_cast(msg->start_state) << " to state " << + static_cast(msg->goal_state) << " ] " << std::endl; } - printf("Calling new activate\n"); - if (!lm.activate("my_node1")) +private: + std::shared_ptr> sub_data_; + std::shared_ptr> + sub_notification_; +}; + +class LifecycleServiceClient : public rclcpp::node::Node +{ +public: + explicit LifecycleServiceClient(const std::string & node_name) + : rclcpp::node::Node(node_name) + {} + + void + init() { - return -1; + client_get_state_ = this->create_client( + "lifecycle_manager__get_state"); + client_get_single_state_ = this->create_client( + "lc_talker__get_state"); + client_change_state_ = this->create_client( + "lifecycle_manager__change_state"); + client_change_single_state_ = this->create_client( + "lc_talker__change_state"); } - while (rclcpp::ok() && i <= 20) { - msg->data = "Hello World: " + std::to_string(i++); - //std::cout << "Publishing: '" << msg->data << "'" << std::endl; - chatter_pub->publish(msg); - rclcpp::spin_some(lc_node); - loop_rate.sleep(); + unsigned int + get_state(const std::string & node_name) + { + auto request = std::make_shared(); + request->node_name = node_name; + + if (!client_get_state_->wait_for_service(3_s)) { + fprintf(stderr, "Service %s is not available.\n", client_get_state_->get_service_name().c_str()); + return static_cast(rclcpp::lifecycle::LifecyclePrimaryStatesT::UNKNOWN); + } + + auto result = client_get_state_->async_send_request(request); + fprintf(stderr, "Asking current state of node %s. Let's wait!\n", node_name.c_str()); + // Kind of a hack for making a async request a synchronous one. + while (result.wait_for(std::chrono::milliseconds(500)) != std::future_status::ready) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + return result.get()->current_state; } - printf("Calling deactivate\n"); - if (!lm.deactivate("my_node1")) + unsigned int + get_single_state() { - return -1; + auto request = std::make_shared(); + + if (!client_get_single_state_->wait_for_service(3_s)) { + fprintf(stderr, "Service %s is not available.\n", client_get_single_state_->get_service_name().c_str()); + return static_cast(rclcpp::lifecycle::LifecyclePrimaryStatesT::UNKNOWN); + } + + auto result = client_get_single_state_->async_send_request(request); + fprintf(stderr, "Asking single state of node %s. Let's wait!\n", "lc_talker"); + // Kind of a hack for making a async request a synchronous one. + while (result.wait_for(std::chrono::milliseconds(500)) != std::future_status::ready) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + return result.get()->current_state; } - while (rclcpp::ok() && i <= 30) { - msg->data = "Hello World: " + std::to_string(i++); - //std::cout << "Publishing: '" << msg->data << "'" << std::endl; - chatter_pub->publish(msg); - rclcpp::spin_some(lc_node); - loop_rate.sleep(); + bool + change_state(const std::string & node_name, rclcpp::lifecycle::LifecycleTransitionsT transition) + { + auto request = std::make_shared(); + request->node_name = node_name; + request->transition = static_cast(transition); + + if (!client_change_state_->wait_for_service(3_s)) { + fprintf(stderr, "Service %s is not available.\n", client_change_state_->get_service_name().c_str()); + return false; + } + + auto result = client_change_state_->async_send_request(request); + fprintf(stderr, "Going to trigger transition %u for node %s\n", request->transition, + node_name.c_str()); + while (result.wait_for(std::chrono::milliseconds(500)) != std::future_status::ready) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + return result.get()->success; } + + bool + change_single_state(rclcpp::lifecycle::LifecycleTransitionsT transition) + { + auto request = std::make_shared(); + request->transition = static_cast(transition); + + if (!client_change_single_state_->wait_for_service(3_s)) { + fprintf(stderr, "Service %s is not available.\n", client_change_single_state_->get_service_name().c_str()); + return false; + } + + auto result = client_change_single_state_->async_send_request(request); + fprintf(stderr, "Going to trigger transition %u for node %s\n", request->transition, + "lc_talker"); + while (result.wait_for(std::chrono::milliseconds(500)) != std::future_status::ready) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + return result.get()->success; + } + +private: + std::shared_ptr> client_get_state_; + std::shared_ptr> client_change_state_; + std::shared_ptr> client_get_single_state_; + std::shared_ptr> client_change_single_state_; +}; + +void +callee_script(std::shared_ptr lc_client, + const std::string & node_name) +{ + auto sleep_time = 2_s; + + std::this_thread::sleep_for(sleep_time); + /* + { // configure + std::this_thread::sleep_for(sleep_time); + lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::CONFIGURING); + //lc_client->change_single_state(rclcpp::lifecycle::LifecycleTransitionsT::CONFIGURING); + //auto current_state = lc_client->get_single_state(); + auto current_state = lc_client->get_state(node_name); + printf("Node %s is in state %u\n", "lc_talker", current_state); + } + { // activate + std::this_thread::sleep_for(sleep_time); + lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::ACTIVATING); + auto current_state = lc_client->get_state(node_name); + printf("Node %s is in state %u\n", node_name.c_str(), current_state); + } + { // deactivate + std::this_thread::sleep_for(sleep_time); + lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::DEACTIVATING); + auto current_state = lc_client->get_state(node_name); + printf("Node %s is in state %u\n", node_name.c_str(), current_state); + } + { // activate againn + std::this_thread::sleep_for(sleep_time); + lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::ACTIVATING); + auto current_state = lc_client->get_state(node_name); + printf("Node %s is in state %u\n", node_name.c_str(), current_state); + } + { // deactivate again + std::this_thread::sleep_for(sleep_time); + lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::DEACTIVATING); + auto current_state = lc_client->get_state(node_name); + printf("Node %s is in state %u\n", node_name.c_str(), current_state); + } + */ +} + +int main(int argc, char * argv[]) +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); + + rclcpp::executors::SingleThreadedExecutor exe; + + std::shared_ptr lc_node = + std::make_shared("lc_talker"); + + std::shared_ptr lc_listener = + std::make_shared("lc_listener"); + + std::shared_ptr lc_client = + std::make_shared("lc_client"); + lc_client->init(); + + rclcpp::lifecycle::LifecycleManager lm; + lm.add_node_interface(lc_node); + + exe.add_node(lc_node->get_communication_interface()); + //exe.add_node(lc_listener); + //exe.add_node(lc_client); + exe.add_node(lm.get_node_base_interface()); + + auto node_name = lc_node->get_base_interface()->get_name(); + + std::shared_future script = std::async(std::launch::async, + std::bind(callee_script, lc_client, node_name)); + + exe.spin_until_future_complete(script); + return 0; } diff --git a/rclcpp_lifecycle/srv/ChangeState.srv b/rclcpp_lifecycle/srv/ChangeState.srv new file mode 100644 index 0000000000..b6b4e88710 --- /dev/null +++ b/rclcpp_lifecycle/srv/ChangeState.srv @@ -0,0 +1,4 @@ +string node_name +uint8 transition +--- +bool success diff --git a/rclcpp_lifecycle/srv/GetState.srv b/rclcpp_lifecycle/srv/GetState.srv new file mode 100644 index 0000000000..b66e0b8e1e --- /dev/null +++ b/rclcpp_lifecycle/srv/GetState.srv @@ -0,0 +1,3 @@ +string node_name +--- +uint8 current_state From cc67375beb9e792d3245c97dacdd9efbb39042e3 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 23 Nov 2016 16:56:02 -0800 Subject: [PATCH 22/63] (fix) correctly concatenate topics --- rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c b/rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c index 5ca46eecb1..8204aecb0c 100644 --- a/rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c +++ b/rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c @@ -45,7 +45,7 @@ bool concatenate(const char ** prefix, const char ** suffix, char ** result) } *result = malloc((prefix_size + suffix_size) * sizeof(char)); memcpy(*result, *prefix, prefix_size); - memcpy(*result + prefix_size, *suffix, suffix_size); + memcpy(*result + prefix_size, *suffix, suffix_size+1); return true; } @@ -87,7 +87,7 @@ rcl_state_machine_init(rcl_state_machine_t * state_machine, const char* node_nam { // initialize publisher // Build topic, topic suffix hardcoded for now // and limited in length of 255 - const char * topic_prefix = "__states"; + const char * topic_prefix = "__transition_notify"; char * topic_name; if (concatenate(&node_name, &topic_prefix, &topic_name) != true) { fprintf(stderr, "%s:%u, Topic name exceeds maximum size of 255\n", @@ -107,7 +107,7 @@ rcl_state_machine_init(rcl_state_machine_t * state_machine, const char* node_nam free(topic_name); return ret; } - //free(topic_name); + free(topic_name); } { // initialize get state service @@ -122,8 +122,8 @@ rcl_state_machine_init(rcl_state_machine_t * state_machine, const char* node_nam return RCL_RET_ERROR; } - const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT( - rclcpp_lifecycle, srv, GetState); + const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT_FUNCTION( + rclcpp_lifecycle, srv, GetState)(); rcl_service_options_t service_options = rcl_service_get_default_options(); rcl_ret_t ret = rcl_service_init(&state_machine->comm_interface.srv_get_state, state_machine->comm_interface.node_handle, ts, topic_name, &service_options); @@ -132,7 +132,7 @@ rcl_state_machine_init(rcl_state_machine_t * state_machine, const char* node_nam free(topic_name); return ret; } - //free(topic_name); + free(topic_name); } { // initialize change state service @@ -157,7 +157,7 @@ rcl_state_machine_init(rcl_state_machine_t * state_machine, const char* node_nam free(topic_name); return ret; } - //free(topic_name); + free(topic_name); } if (default_states) { From 97e4273fb508ac018d11fc05f4f8151a10fe6b04 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 23 Nov 2016 16:56:47 -0800 Subject: [PATCH 23/63] (fix) correctly initialize Service wo/ copy --- .../lifecycle_manager_impl.hpp | 40 +++++++++---------- 1 file changed, 19 insertions(+), 21 deletions(-) diff --git a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp index 557eb105c1..cd55bc1dae 100644 --- a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp +++ b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp @@ -53,31 +53,33 @@ class LifecycleManager::LifecycleManagerImpl public: LifecycleManagerImpl() : node_base_handle_(std::make_shared("lifecycle_manager")) - { - } + {} ~LifecycleManagerImpl() { - fprintf(stderr, "Going to detroy my lifecycle manager\n"); for (auto it = node_handle_map_.begin(); it != node_handle_map_.end(); ++it) { rcl_state_machine_t * rcl_state_machine = &it->second.state_machine; if (!rcl_state_machine) - fprintf(stderr, "Hell is burning \n"); - rcl_state_machine_fini(rcl_state_machine); + { + fprintf(stderr, "%s:%u, FATAL: rcl_state_machine got destroyed externally.\n", + __FILE__, __LINE__); + } else { + rcl_state_machine_fini(rcl_state_machine); + } } } void init() { - //srv_get_state_ = node_base_handle_->create_service( - // node_base_handle_->get_name() + "__get_state", - // std::bind(&LifecycleManagerImpl::on_get_state, this, - // std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - //srv_change_state_ = node_base_handle_->create_service( - // node_base_handle_->get_name() + "__change_state", - // std::bind(&LifecycleManagerImpl::on_change_state, this, - // std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + srv_get_state_ = node_base_handle_->create_service( + node_base_handle_->get_name() + "__get_state", + std::bind(&LifecycleManagerImpl::on_get_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + srv_change_state_ = node_base_handle_->create_service( + node_base_handle_->get_name() + "__change_state", + std::bind(&LifecycleManagerImpl::on_change_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } void @@ -133,11 +135,10 @@ class LifecycleManager::LifecycleManagerImpl add_node_interface(const std::string & node_name, const LifecycleInterfacePtr & lifecycle_interface) { - NodeStateMachine node_state_machine; + // TODO(karsten1987): clarify what do if node already exists; + NodeStateMachine& node_state_machine = node_handle_map_[node_name]; node_state_machine.state_machine = rcl_get_zero_initialized_state_machine( node_base_handle_->get_rcl_node_handle()); - fprintf(stderr, "Created get service address %p\n", &node_state_machine.state_machine.comm_interface.srv_get_state); - fprintf(stderr, "Created change service address %p\n", &node_state_machine.state_machine.comm_interface.srv_change_state); rcl_state_machine_init(&node_state_machine.state_machine, node_name.c_str(), true); // srv objects may get destroyed directly here @@ -161,6 +162,7 @@ class LifecycleManager::LifecycleManagerImpl node_base_handle_->add_service(srv_get_state_base); node_state_machine.srv_get_state = srv_get_state_base; } + { // change_state std::function, const std::shared_ptr, @@ -178,7 +180,7 @@ class LifecycleManager::LifecycleManagerImpl &node_state_machine.state_machine.comm_interface.srv_change_state, any_cb); auto srv_change_state_base = std::dynamic_pointer_cast(srv_change_state); - //node_base_handle_->add_service(srv_change_state_base); + node_base_handle_->add_service(srv_change_state_base); node_state_machine.srv_change_state = srv_change_state_base; } @@ -203,9 +205,6 @@ class LifecycleManager::LifecycleManagerImpl node_state_machine.cb_map[LifecycleTransitionsT::ACTIVATING] = cb_activating; node_state_machine.cb_map[LifecycleTransitionsT::DEACTIVATING] = cb_deactivating; node_state_machine.cb_map[LifecycleTransitionsT::ERRORPROCESSING] = cb_error; - - // TODO(karsten1987): clarify what do if node already exists; - node_handle_map_[node_name] = node_state_machine; } template @@ -272,7 +271,6 @@ class LifecycleManager::LifecycleManagerImpl return true; } -// private: std::shared_ptr node_base_handle_; std::shared_ptr> srv_get_state_; std::shared_ptr> srv_change_state_; From 328801f64e9c8ce28e47555330f48bd51fd9ac8c Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 23 Nov 2016 16:57:21 -0800 Subject: [PATCH 24/63] (dev) call both service types --- .../src/rclcpp_lifecycle/lifecycle_talker.cpp | 139 +++++++++++------- 1 file changed, 87 insertions(+), 52 deletions(-) diff --git a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_talker.cpp b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_talker.cpp index a4f611f1ee..1016a8fbf0 100644 --- a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_talker.cpp +++ b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_talker.cpp @@ -50,21 +50,21 @@ class LifecycleTalker : public rclcpp::node::lifecycle::LifecycleNode bool on_configure() { - printf("%s is going to configure its node\n", get_name().c_str()); + printf("[%s] on_configure() is called.\n", get_name().c_str()); return true; } bool on_activate() { rclcpp::node::lifecycle::LifecycleNode::enable_communication(); - printf("%s is going to activate its node\n", get_name().c_str()); + printf("[%s] on_activate() is called.\n", get_name().c_str()); return true; } bool on_deactivate() { rclcpp::node::lifecycle::LifecycleNode::disable_communication(); - printf("%s is going to deactivate its node\n", get_name().c_str()); + printf("[%s] on_deactivate() is called.\n", get_name().c_str()); return true; } @@ -81,22 +81,21 @@ class LifecycleListener : public rclcpp::node::Node : rclcpp::node::Node(node_name) { sub_data_ = this->create_subscription("lifecycle_chatter", - std::bind(&LifecycleListener::data_callback, this, std::placeholders::_1)); + std::bind(&LifecycleListener::data_callback, this, std::placeholders::_1)); sub_notification_ = this->create_subscription( - "lifecycle_manager__lc_talker", std::bind(&LifecycleListener::notification_callback, this, + "lc_talker__transition_notify", std::bind(&LifecycleListener::notification_callback, this, std::placeholders::_1)); } void data_callback(const std_msgs::msg::String::SharedPtr msg) { - std::cout << "I heard data: [" << msg->data << "]" << std::endl; + printf("[%s] data_callback: %s\n", get_name().c_str(), msg->data.c_str()); } void notification_callback(const rclcpp_lifecycle::msg::Transition::SharedPtr msg) { - std::cout << "Transition triggered:: [ Going from state " << - static_cast(msg->start_state) << " to state " << - static_cast(msg->goal_state) << " ] " << std::endl; + printf("[%s] notify callback: Transition from state %d to %d\n", + get_name().c_str(), static_cast(msg->start_state), static_cast(msg->goal_state)); } private: @@ -115,94 +114,138 @@ class LifecycleServiceClient : public rclcpp::node::Node void init() { + // Service clients pointing to a global lifecycle manager client_get_state_ = this->create_client( "lifecycle_manager__get_state"); - client_get_single_state_ = this->create_client( - "lc_talker__get_state"); client_change_state_ = this->create_client( "lifecycle_manager__change_state"); + + // Service client pointing to each individual service + client_get_single_state_ = this->create_client( + "lc_talker__get_state"); client_change_single_state_ = this->create_client( "lc_talker__change_state"); } unsigned int - get_state(const std::string & node_name) + get_state(const std::string & node_name, std::chrono::seconds time_out = 3_s) { auto request = std::make_shared(); request->node_name = node_name; - if (!client_get_state_->wait_for_service(3_s)) { - fprintf(stderr, "Service %s is not available.\n", client_get_state_->get_service_name().c_str()); + if (!client_get_state_->wait_for_service(time_out)) { + fprintf(stderr, "Service %s is not available.\n", + client_get_state_->get_service_name().c_str()); return static_cast(rclcpp::lifecycle::LifecyclePrimaryStatesT::UNKNOWN); } auto result = client_get_state_->async_send_request(request); - fprintf(stderr, "Asking current state of node %s. Let's wait!\n", node_name.c_str()); // Kind of a hack for making a async request a synchronous one. while (result.wait_for(std::chrono::milliseconds(500)) != std::future_status::ready) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - return result.get()->current_state; + + if(result.get()) + { + printf("[%s] Node %s has current state %d.\n", + get_name().c_str(), node_name.c_str(), result.get()->current_state); + return result.get()->current_state; + } else { + fprintf(stderr, "[%s] Failed to get current state for node %s\n", + get_name().c_str(), node_name.c_str()); + return static_cast(rclcpp::lifecycle::LifecyclePrimaryStatesT::UNKNOWN); + } } unsigned int - get_single_state() + get_single_state(std::chrono::seconds time_out = 3_s) { auto request = std::make_shared(); - if (!client_get_single_state_->wait_for_service(3_s)) { - fprintf(stderr, "Service %s is not available.\n", client_get_single_state_->get_service_name().c_str()); + if (!client_get_single_state_->wait_for_service(time_out)) { + fprintf(stderr, "Service %s is not available.\n", + client_get_single_state_->get_service_name().c_str()); return static_cast(rclcpp::lifecycle::LifecyclePrimaryStatesT::UNKNOWN); } auto result = client_get_single_state_->async_send_request(request); - fprintf(stderr, "Asking single state of node %s. Let's wait!\n", "lc_talker"); // Kind of a hack for making a async request a synchronous one. while (result.wait_for(std::chrono::milliseconds(500)) != std::future_status::ready) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - return result.get()->current_state; + + if(result.get()) + { + printf("[%s] Node %s has current state %d\n", + get_name().c_str(), "lc_talker", result.get()->current_state); + return result.get()->current_state; + } else { + fprintf(stderr, "[%s] Failed to get current state for node %s\n", + get_name().c_str(), "lc_talker"); + return static_cast(rclcpp::lifecycle::LifecyclePrimaryStatesT::UNKNOWN); + } } bool - change_state(const std::string & node_name, rclcpp::lifecycle::LifecycleTransitionsT transition) + change_state(const std::string & node_name, rclcpp::lifecycle::LifecycleTransitionsT transition, + std::chrono::seconds time_out = 3_s) { auto request = std::make_shared(); request->node_name = node_name; request->transition = static_cast(transition); - if (!client_change_state_->wait_for_service(3_s)) { - fprintf(stderr, "Service %s is not available.\n", client_change_state_->get_service_name().c_str()); + if (!client_change_state_->wait_for_service(time_out)) { + fprintf(stderr, "Service %s is not available.\n", + client_change_state_->get_service_name().c_str()); return false; } auto result = client_change_state_->async_send_request(request); - fprintf(stderr, "Going to trigger transition %u for node %s\n", request->transition, - node_name.c_str()); while (result.wait_for(std::chrono::milliseconds(500)) != std::future_status::ready) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - return result.get()->success; + + if(result.get()->success) + { + printf("[%s] Transition %d successfully triggered.\n", + get_name().c_str(), static_cast(transition)); + return true; + } else { + fprintf(stderr, "[%s] Failed to trigger transition %d\n", + get_name().c_str(), static_cast(transition)); + return false; + } } bool - change_single_state(rclcpp::lifecycle::LifecycleTransitionsT transition) + change_single_state(rclcpp::lifecycle::LifecycleTransitionsT transition, + std::chrono::seconds time_out = 3_s) { auto request = std::make_shared(); + request->node_name = "lc_talker"; request->transition = static_cast(transition); - if (!client_change_single_state_->wait_for_service(3_s)) { - fprintf(stderr, "Service %s is not available.\n", client_change_single_state_->get_service_name().c_str()); + if (!client_change_single_state_->wait_for_service(time_out)) { + fprintf(stderr, "Service %s is not available.\n", + client_change_single_state_->get_service_name().c_str()); return false; } auto result = client_change_single_state_->async_send_request(request); - fprintf(stderr, "Going to trigger transition %u for node %s\n", request->transition, - "lc_talker"); while (result.wait_for(std::chrono::milliseconds(500)) != std::future_status::ready) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - return result.get()->success; + + if(result.get()->success) + { + printf("[%s] Transition %d successfully triggered.\n", + get_name().c_str(), static_cast(transition)); + return true; + } else { + fprintf(stderr, "[%s] Failed to trigger transition %d\n", + get_name().c_str(), static_cast(transition)); + return false; + } } private: @@ -216,43 +259,35 @@ void callee_script(std::shared_ptr lc_client, const std::string & node_name) { - auto sleep_time = 2_s; + auto sleep_time = 10_s; - std::this_thread::sleep_for(sleep_time); - /* { // configure std::this_thread::sleep_for(sleep_time); - lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::CONFIGURING); - //lc_client->change_single_state(rclcpp::lifecycle::LifecycleTransitionsT::CONFIGURING); - //auto current_state = lc_client->get_single_state(); - auto current_state = lc_client->get_state(node_name); - printf("Node %s is in state %u\n", "lc_talker", current_state); + //lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::CONFIGURING); + //lc_client->get_state(node_name); + lc_client->change_single_state(rclcpp::lifecycle::LifecycleTransitionsT::CONFIGURING); + lc_client->get_single_state(); } { // activate std::this_thread::sleep_for(sleep_time); lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::ACTIVATING); - auto current_state = lc_client->get_state(node_name); - printf("Node %s is in state %u\n", node_name.c_str(), current_state); + lc_client->get_state(node_name); } { // deactivate std::this_thread::sleep_for(sleep_time); lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::DEACTIVATING); - auto current_state = lc_client->get_state(node_name); - printf("Node %s is in state %u\n", node_name.c_str(), current_state); + lc_client->get_state(node_name); } { // activate againn std::this_thread::sleep_for(sleep_time); lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::ACTIVATING); - auto current_state = lc_client->get_state(node_name); - printf("Node %s is in state %u\n", node_name.c_str(), current_state); + lc_client->get_state(node_name); } { // deactivate again std::this_thread::sleep_for(sleep_time); lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::DEACTIVATING); - auto current_state = lc_client->get_state(node_name); - printf("Node %s is in state %u\n", node_name.c_str(), current_state); + lc_client->get_state(node_name); } - */ } int main(int argc, char * argv[]) @@ -276,8 +311,8 @@ int main(int argc, char * argv[]) lm.add_node_interface(lc_node); exe.add_node(lc_node->get_communication_interface()); - //exe.add_node(lc_listener); - //exe.add_node(lc_client); + exe.add_node(lc_listener); + exe.add_node(lc_client); exe.add_node(lm.get_node_base_interface()); auto node_name = lc_node->get_base_interface()->get_name(); From df582ae6bbe8feb955026f9e0061d407b6f87411 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Sun, 27 Nov 2016 10:47:51 -0800 Subject: [PATCH 25/63] extract demo files --- rclcpp_lifecycle/CMakeLists.txt | 21 ++- .../src/demos/lifecycle_listener.cpp | 63 ++++++++ .../lifecycle_service_client.cpp} | 148 +++--------------- .../src/demos/lifecycle_talker.cpp | 95 +++++++++++ 4 files changed, 203 insertions(+), 124 deletions(-) create mode 100644 rclcpp_lifecycle/src/demos/lifecycle_listener.cpp rename rclcpp_lifecycle/src/{rclcpp_lifecycle/lifecycle_talker.cpp => demos/lifecycle_service_client.cpp} (63%) create mode 100644 rclcpp_lifecycle/src/demos/lifecycle_talker.cpp diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index c0ba793da0..7722af943a 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -68,7 +68,7 @@ macro(targets) LIBRARY DESTINATION lib RUNTIME DESTINATION bin) - ### CPP High level library+exe + ### CPP High level library add_library(rclcpp_lifecycle${target_suffix} SHARED src/rclcpp_lifecycle/lifecycle_manager.cpp) @@ -82,17 +82,34 @@ macro(targets) ament_target_dependencies(rclcpp_lifecycle${target_suffix} "rclcpp${target_suffix}") + ### demos add_executable(lifecycle_talker${target_suffix} - src/rclcpp_lifecycle/lifecycle_talker.cpp) + src/demos/lifecycle_talker.cpp) target_link_libraries(lifecycle_talker${target_suffix} rclcpp_lifecycle${target_suffix}) ament_target_dependencies(lifecycle_talker${target_suffix} "rclcpp${target_suffix}" "std_msgs") + add_executable(lifecycle_listener${target_suffix} + src/demos/lifecycle_listener.cpp) + target_link_libraries(lifecycle_listener${target_suffix} + rclcpp_lifecycle${target_suffix}) + ament_target_dependencies(lifecycle_listener${target_suffix} + "rclcpp${target_suffix}" + "std_msgs") + add_executable(lifecycle_service_client${target_suffix} + src/demos/lifecycle_service_client.cpp) + target_link_libraries(lifecycle_service_client${target_suffix} + rclcpp_lifecycle${target_suffix}) + ament_target_dependencies(lifecycle_service_client${target_suffix} + "rclcpp${target_suffix}" + "std_msgs") install(TARGETS rclcpp_lifecycle${target_suffix} lifecycle_talker${target_suffix} + lifecycle_listener${target_suffix} + lifecycle_service_client${target_suffix} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) diff --git a/rclcpp_lifecycle/src/demos/lifecycle_listener.cpp b/rclcpp_lifecycle/src/demos/lifecycle_listener.cpp new file mode 100644 index 0000000000..11b2ec4119 --- /dev/null +++ b/rclcpp_lifecycle/src/demos/lifecycle_listener.cpp @@ -0,0 +1,63 @@ +// 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 "rclcpp_lifecycle/msg/transition.hpp" + +static constexpr auto chatter_topic = "lifecycle_chatter"; +static constexpr auto notification_topic = "lc_talker__transition_notify"; + +class LifecycleListener : public rclcpp::node::Node +{ +public: + explicit LifecycleListener(const std::string & node_name) + : rclcpp::node::Node(node_name) + { + sub_data_ = this->create_subscription(chatter_topic, + std::bind(&LifecycleListener::data_callback, this, std::placeholders::_1)); + + sub_notification_ = this->create_subscription( + notification_topic, std::bind(&LifecycleListener::notification_callback, this, + std::placeholders::_1)); + } + + void data_callback(const std_msgs::msg::String::SharedPtr msg) + { + printf("[%s] data_callback: %s\n", get_name().c_str(), msg->data.c_str()); + } + + void notification_callback(const rclcpp_lifecycle::msg::Transition::SharedPtr msg) + { + printf("[%s] notify callback: Transition from state %d to %d\n", + get_name().c_str(), static_cast(msg->start_state), static_cast(msg->goal_state)); + } + +private: + std::shared_ptr> sub_data_; + std::shared_ptr> + sub_notification_; +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + auto lc_listener = std::make_shared("lc_listener"); + rclcpp::spin(lc_listener); + + return 0; +} diff --git a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_talker.cpp b/rclcpp_lifecycle/src/demos/lifecycle_service_client.cpp similarity index 63% rename from rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_talker.cpp rename to rclcpp_lifecycle/src/demos/lifecycle_service_client.cpp index 1016a8fbf0..e3eced98a8 100644 --- a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_talker.cpp +++ b/rclcpp_lifecycle/src/demos/lifecycle_service_client.cpp @@ -12,97 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/publisher.hpp" +#include +#include +#include +#include -#include "rclcpp_lifecycle/lifecycle_manager.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" +// service topics for general lifecycle manager +static constexpr auto manager_get_state_topic = "lifecycle_manager__get_state"; +static constexpr auto manager_change_state_topic = "lifecycle_manager__change_state"; -#include "std_msgs/msg/string.hpp" -#include "rclcpp_lifecycle/msg/transition.hpp" -#include "rclcpp_lifecycle/srv/get_state.hpp" -#include "rclcpp_lifecycle/srv/change_state.hpp" - -class LifecycleTalker : public rclcpp::node::lifecycle::LifecycleNode -{ -public: - explicit LifecycleTalker(const std::string & node_name, bool intra_process_comms = false) - : rclcpp::node::lifecycle::LifecycleNode(node_name, intra_process_comms) - { - msg_ = std::make_shared(); - - pub_ = this->create_publisher("lifecycle_chatter"); - timer_ = this->get_communication_interface()->create_wall_timer( - 1_s, std::bind(&LifecycleTalker::publish, this)); - } - - void publish() - { - static size_t count = 0; - msg_->data = "Lifecycle HelloWorld #" + std::to_string(++count); - pub_->publish(msg_); - } - - bool on_configure() - { - printf("[%s] on_configure() is called.\n", get_name().c_str()); - return true; - } - - bool on_activate() - { - rclcpp::node::lifecycle::LifecycleNode::enable_communication(); - printf("[%s] on_activate() is called.\n", get_name().c_str()); - return true; - } - - bool on_deactivate() - { - rclcpp::node::lifecycle::LifecycleNode::disable_communication(); - printf("[%s] on_deactivate() is called.\n", get_name().c_str()); - return true; - } - -private: - std::shared_ptr msg_; - std::shared_ptr> pub_; - rclcpp::TimerBase::SharedPtr timer_; -}; - -class LifecycleListener : public rclcpp::node::Node -{ -public: - explicit LifecycleListener(const std::string & node_name) - : rclcpp::node::Node(node_name) - { - sub_data_ = this->create_subscription("lifecycle_chatter", - std::bind(&LifecycleListener::data_callback, this, std::placeholders::_1)); - sub_notification_ = this->create_subscription( - "lc_talker__transition_notify", std::bind(&LifecycleListener::notification_callback, this, - std::placeholders::_1)); - } - - void data_callback(const std_msgs::msg::String::SharedPtr msg) - { - printf("[%s] data_callback: %s\n", get_name().c_str(), msg->data.c_str()); - } - - void notification_callback(const rclcpp_lifecycle::msg::Transition::SharedPtr msg) - { - printf("[%s] notify callback: Transition from state %d to %d\n", - get_name().c_str(), static_cast(msg->start_state), static_cast(msg->goal_state)); - } - -private: - std::shared_ptr> sub_data_; - std::shared_ptr> - sub_notification_; -}; +// service topics for node-attached services +static const std::string lifecycle_node = "lc_talker"; +static const std::string node_get_state_topic = lifecycle_node+"__get_state"; +static const std::string node_change_state_topic = lifecycle_node+"__change_state"; class LifecycleServiceClient : public rclcpp::node::Node { @@ -116,15 +41,15 @@ class LifecycleServiceClient : public rclcpp::node::Node { // Service clients pointing to a global lifecycle manager client_get_state_ = this->create_client( - "lifecycle_manager__get_state"); + manager_get_state_topic); client_change_state_ = this->create_client( - "lifecycle_manager__change_state"); + manager_change_state_topic); // Service client pointing to each individual service client_get_single_state_ = this->create_client( - "lc_talker__get_state"); + node_get_state_topic); client_change_single_state_ = this->create_client( - "lc_talker__change_state"); + node_change_state_topic); } unsigned int @@ -177,11 +102,11 @@ class LifecycleServiceClient : public rclcpp::node::Node if(result.get()) { printf("[%s] Node %s has current state %d\n", - get_name().c_str(), "lc_talker", result.get()->current_state); + get_name().c_str(), lifecycle_node.c_str(), result.get()->current_state); return result.get()->current_state; } else { fprintf(stderr, "[%s] Failed to get current state for node %s\n", - get_name().c_str(), "lc_talker"); + get_name().c_str(), lifecycle_node.c_str()); return static_cast(rclcpp::lifecycle::LifecyclePrimaryStatesT::UNKNOWN); } } @@ -222,7 +147,7 @@ class LifecycleServiceClient : public rclcpp::node::Node std::chrono::seconds time_out = 3_s) { auto request = std::make_shared(); - request->node_name = "lc_talker"; + request->node_name = lifecycle_node; request->transition = static_cast(transition); if (!client_change_single_state_->wait_for_service(time_out)) { @@ -262,64 +187,43 @@ callee_script(std::shared_ptr lc_client, auto sleep_time = 10_s; { // configure - std::this_thread::sleep_for(sleep_time); - //lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::CONFIGURING); - //lc_client->get_state(node_name); lc_client->change_single_state(rclcpp::lifecycle::LifecycleTransitionsT::CONFIGURING); lc_client->get_single_state(); } - { // activate + { // activate single node std::this_thread::sleep_for(sleep_time); lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::ACTIVATING); lc_client->get_state(node_name); } - { // deactivate + { // deactivate single node std::this_thread::sleep_for(sleep_time); lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::DEACTIVATING); lc_client->get_state(node_name); } - { // activate againn + { // activate lifecycle_manager std::this_thread::sleep_for(sleep_time); lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::ACTIVATING); lc_client->get_state(node_name); } - { // deactivate again + { // deactivate lifecycle_manager std::this_thread::sleep_for(sleep_time); lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::DEACTIVATING); lc_client->get_state(node_name); } } -int main(int argc, char * argv[]) +int main(int argc, char** argv) { - setvbuf(stdout, NULL, _IONBF, BUFSIZ); rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor exe; - - std::shared_ptr lc_node = - std::make_shared("lc_talker"); - - std::shared_ptr lc_listener = - std::make_shared("lc_listener"); - - std::shared_ptr lc_client = - std::make_shared("lc_client"); + auto lc_client = std::make_shared("lc_client"); lc_client->init(); - rclcpp::lifecycle::LifecycleManager lm; - lm.add_node_interface(lc_node); - - exe.add_node(lc_node->get_communication_interface()); - exe.add_node(lc_listener); + rclcpp::executors::SingleThreadedExecutor exe; exe.add_node(lc_client); - exe.add_node(lm.get_node_base_interface()); - - auto node_name = lc_node->get_base_interface()->get_name(); std::shared_future script = std::async(std::launch::async, - std::bind(callee_script, lc_client, node_name)); - + std::bind(callee_script, lc_client, lifecycle_node)); exe.spin_until_future_complete(script); return 0; diff --git a/rclcpp_lifecycle/src/demos/lifecycle_talker.cpp b/rclcpp_lifecycle/src/demos/lifecycle_talker.cpp new file mode 100644 index 0000000000..88f5a4a263 --- /dev/null +++ b/rclcpp_lifecycle/src/demos/lifecycle_talker.cpp @@ -0,0 +1,95 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/publisher.hpp" + +#include "rclcpp_lifecycle/lifecycle_manager.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" + +#include "std_msgs/msg/string.hpp" + +static constexpr auto chatter_topic = "lifecycle_chatter"; + +class LifecycleTalker : public rclcpp::node::lifecycle::LifecycleNode +{ +public: + explicit LifecycleTalker(const std::string & node_name, bool intra_process_comms = false) + : rclcpp::node::lifecycle::LifecycleNode(node_name, intra_process_comms) + { + msg_ = std::make_shared(); + + pub_ = this->create_publisher(chatter_topic); + timer_ = this->get_communication_interface()->create_wall_timer( + 1_s, std::bind(&LifecycleTalker::publish, this)); + } + + void publish() + { + static size_t count = 0; + msg_->data = "Lifecycle HelloWorld #" + std::to_string(++count); + pub_->publish(msg_); + } + + bool on_configure() + { + printf("[%s] on_configure() is called.\n", get_name().c_str()); + return true; + } + + bool on_activate() + { + rclcpp::node::lifecycle::LifecycleNode::enable_communication(); + printf("[%s] on_activate() is called.\n", get_name().c_str()); + return true; + } + + bool on_deactivate() + { + rclcpp::node::lifecycle::LifecycleNode::disable_communication(); + printf("[%s] on_deactivate() is called.\n", get_name().c_str()); + return true; + } + +private: + std::shared_ptr msg_; + std::shared_ptr> pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char * argv[]) +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); + + rclcpp::executors::SingleThreadedExecutor exe; + + std::shared_ptr lc_node = + std::make_shared("lc_talker"); + + rclcpp::lifecycle::LifecycleManager lm; + lm.add_node_interface(lc_node); + + exe.add_node(lc_node->get_communication_interface()); + exe.add_node(lm.get_node_base_interface()); + + exe.spin(); + + return 0; +} From b52658c445f03417058ef8931de11ef9f35632c4 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Sun, 27 Nov 2016 12:08:42 -0800 Subject: [PATCH 26/63] (fix) remove debug prints --- rclcpp/include/rclcpp/service.hpp | 4 ---- .../strategies/allocator_memory_strategy.hpp | 5 ----- rclcpp/src/rclcpp/callback_group.cpp | 16 ---------------- rclcpp/src/rclcpp/executor.cpp | 1 - .../rclcpp_lifecycle/lifecycle_manager_impl.hpp | 4 +--- 5 files changed, 1 insertion(+), 29 deletions(-) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 6e226c6cc9..9d0d4c0bee 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -128,7 +128,6 @@ class Service : public ServiceBase } service_handle_ = service_handle; defined_extern_ = true; - fprintf(stderr, "Created Extern Service around servicehandle %p\n", service_handle_); } Service() = delete; @@ -137,15 +136,12 @@ class Service : public ServiceBase { // check if you have ownership of the handle if (!defined_extern_) { - fprintf(stderr, "CPP Service Handle address %p\n", service_handle_); if (rcl_service_fini(service_handle_, node_handle_) != RCL_RET_OK) { std::stringstream ss; ss << "Error in destruction of rcl service_handle_ handle: " << rcl_get_error_string_safe() << '\n'; (std::cerr << ss.str()).flush(); } - } else { - fprintf(stderr, "Extern Service Handle address %p\n", service_handle_); } } diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 8c476a39eb..1cac9a5905 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -101,7 +101,6 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } for (size_t i = 0; i < wait_set->size_of_services; ++i) { if (!wait_set->services[i]) { - fprintf(stderr, "Going to remove a invalid service\n"); service_handles_[i] = nullptr; } } @@ -164,7 +163,6 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy for (auto & weak_service : group->get_service_ptrs()) { auto service = weak_service.lock(); if (service) { - fprintf(stderr, "Going to add %s to the list of service handles\n", service->get_service_name().c_str()); service_handles_.push_back(service->get_service_handle()); } } @@ -202,13 +200,10 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } for (auto service : service_handles_) { - fprintf(stderr, "Adding %s to waitset\n", rcl_service_get_service_name(service)); - fprintf(stderr, "I won't see that\n"); if (rcl_wait_set_add_service(wait_set, service) != RCL_RET_OK) { fprintf(stderr, "Couldn't add service to waitset: %s\n", rcl_get_error_string_safe()); return false; } - fprintf(stderr, "Successfully added %s to waitset\n", rcl_service_get_service_name(service)); } for (auto timer : timer_handles_) { diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index a62e12b92e..6c6fa281da 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -41,13 +41,6 @@ const std::vector & CallbackGroup::get_service_ptrs() const { std::lock_guard lock(mutex_); - for(auto& service : service_ptrs_) - { - auto srv = service.lock(); - if (srv) - fprintf(stderr, "Getting: Service in Group %s\n", srv->get_service_name().c_str()); - } - fprintf(stderr, "**Getting***\n"); return service_ptrs_; } @@ -88,17 +81,8 @@ CallbackGroup::add_timer(const rclcpp::timer::TimerBase::SharedPtr timer_ptr) void CallbackGroup::add_service(const rclcpp::service::ServiceBase::SharedPtr service_ptr) { - { std::lock_guard lock(mutex_); service_ptrs_.push_back(service_ptr); - for(auto& service : service_ptrs_) - { - auto srv = service.lock(); - if (srv) - fprintf(stderr, "Adding: Service in Group %s\n", srv->get_service_name().c_str()); - } - fprintf(stderr, "**Additing***\n"); - } } void diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 6faa0d4d39..a22abfa840 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -402,7 +402,6 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) rcl_get_error_string_safe()); } - fprintf(stderr, "I got %u services in waitset\n", memory_strategy_->number_of_ready_services()); if (!memory_strategy_->add_handles_to_waitset(&waitset_)) { throw std::runtime_error("Couldn't fill waitset"); } diff --git a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp index cd55bc1dae..79be0acb5e 100644 --- a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp +++ b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp @@ -33,7 +33,7 @@ namespace rclcpp namespace lifecycle { -using LifecycleInterface = rclcpp::node::lifecycle::LifecycleNode; +using LifecycleInterface = rclcpp::node::lifecycle::LifecycleNodeInterface; using LifecycleInterfacePtr = std::shared_ptr; using LifecycleInterfaceWeakPtr = std::weak_ptr; @@ -42,8 +42,6 @@ struct NodeStateMachine LifecycleInterfaceWeakPtr weak_node_handle; rcl_state_machine_t state_machine; std::map> cb_map; - // std::shared_ptr> srv_get_state; - // std::shared_ptr> srv_change_state; std::shared_ptr srv_get_state; std::shared_ptr srv_change_state; }; From 339dbd585b17a9f17f37ad2b3bd8f9a22386dd3e Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 28 Nov 2016 16:20:44 -0800 Subject: [PATCH 27/63] (dev) change to lifecycle_msgs --- rclcpp_lifecycle/CMakeLists.txt | 40 ++++------------- rclcpp_lifecycle/msg/Transition.msg | 2 - rclcpp_lifecycle/package.xml | 5 ++- .../src/demos/lifecycle_listener.cpp | 9 ++-- .../src/demos/lifecycle_service_client.cpp | 28 ++++++------ .../src/rcl_lifecycle/rcl_lifecycle.c | 26 +++++------ .../lifecycle_manager_impl.hpp | 44 +++++++++---------- rclcpp_lifecycle/srv/ChangeState.srv | 4 -- rclcpp_lifecycle/srv/GetState.srv | 3 -- 9 files changed, 66 insertions(+), 95 deletions(-) delete mode 100644 rclcpp_lifecycle/msg/Transition.msg delete mode 100644 rclcpp_lifecycle/srv/ChangeState.srv delete mode 100644 rclcpp_lifecycle/srv/GetState.srv diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 7722af943a..5eed06807c 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -13,22 +13,7 @@ find_package(rcl REQUIRED) find_package(rmw REQUIRED) find_package(rmw_implementation_cmake REQUIRED) find_package(std_msgs REQUIRED) - -# get the rmw implementations ahead of time -find_package(rmw_implementation_cmake REQUIRED) -get_available_rmw_implementations(rmw_implementations2) -foreach(rmw_implementation ${rmw_implementations2}) - find_package("${rmw_implementation}" REQUIRED) -endforeach() - -set(lifecycle_msg_files - "msg/Transition.msg") -set(lifecycle_srv_files - "srv/GetState.srv" - "srv/ChangeState.srv") -rosidl_generate_interfaces(lifecycle_msgs - ${lifecycle_msg_files} - ${lifecycle_srv_files}) +find_package(lifecycle_msgs REQUIRED) include_directories( include) @@ -53,15 +38,10 @@ macro(targets) SHARED ${rcl_lifecycle_sources}) - foreach(typesupport_impl ${typesupport_impls_c}) - rosidl_target_interfaces(rcl_lifecycle${target_suffix} - lifecycle_msgs - ${typesupport_impl}) - endforeach() - ament_target_dependencies(rcl_lifecycle${target_suffix} "rcl${target_suffix}" - "std_msgs") + "std_msgs" + "lifecycle_msgs") install(TARGETS rcl_lifecycle${target_suffix} ARCHIVE DESTINATION lib @@ -74,13 +54,9 @@ macro(targets) src/rclcpp_lifecycle/lifecycle_manager.cpp) target_link_libraries(rclcpp_lifecycle${target_suffix} rcl_lifecycle${target_suffix}) - foreach(typesupport_impl ${typesupport_impls_cpp}) - rosidl_target_interfaces(rclcpp_lifecycle${target_suffix} - lifecycle_msgs - ${typesupport_impl}) - endforeach() ament_target_dependencies(rclcpp_lifecycle${target_suffix} - "rclcpp${target_suffix}") + "rclcpp${target_suffix}" + "lifecycle_msgs") ### demos add_executable(lifecycle_talker${target_suffix} @@ -96,14 +72,16 @@ macro(targets) rclcpp_lifecycle${target_suffix}) ament_target_dependencies(lifecycle_listener${target_suffix} "rclcpp${target_suffix}" - "std_msgs") + "std_msgs" + "lifecycle_msgs") add_executable(lifecycle_service_client${target_suffix} src/demos/lifecycle_service_client.cpp) target_link_libraries(lifecycle_service_client${target_suffix} rclcpp_lifecycle${target_suffix}) ament_target_dependencies(lifecycle_service_client${target_suffix} "rclcpp${target_suffix}" - "std_msgs") + "std_msgs" + "lifecycle_msgs") install(TARGETS rclcpp_lifecycle${target_suffix} diff --git a/rclcpp_lifecycle/msg/Transition.msg b/rclcpp_lifecycle/msg/Transition.msg deleted file mode 100644 index 6f6c283fce..0000000000 --- a/rclcpp_lifecycle/msg/Transition.msg +++ /dev/null @@ -1,2 +0,0 @@ -uint8 start_state -uint8 goal_state diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 03c4f6a55a..033085b996 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -13,19 +13,22 @@ rclcpp rmw_implementation rmw_implementation_cmake - builtin_interfaces rosidl_default_generators std_msgs + lifecycle_msgs rclcpp + rclpy rmw_implementation rosidl_default_runtime std_msgs + lifecycle_msgs ament_lint_auto ament_lint_common ament_cmake + ament_python diff --git a/rclcpp_lifecycle/src/demos/lifecycle_listener.cpp b/rclcpp_lifecycle/src/demos/lifecycle_listener.cpp index 11b2ec4119..96fab8016d 100644 --- a/rclcpp_lifecycle/src/demos/lifecycle_listener.cpp +++ b/rclcpp_lifecycle/src/demos/lifecycle_listener.cpp @@ -15,8 +15,7 @@ #include #include #include - -#include "rclcpp_lifecycle/msg/transition.hpp" +#include static constexpr auto chatter_topic = "lifecycle_chatter"; static constexpr auto notification_topic = "lc_talker__transition_notify"; @@ -30,7 +29,7 @@ class LifecycleListener : public rclcpp::node::Node sub_data_ = this->create_subscription(chatter_topic, std::bind(&LifecycleListener::data_callback, this, std::placeholders::_1)); - sub_notification_ = this->create_subscription( + sub_notification_ = this->create_subscription( notification_topic, std::bind(&LifecycleListener::notification_callback, this, std::placeholders::_1)); } @@ -40,7 +39,7 @@ class LifecycleListener : public rclcpp::node::Node printf("[%s] data_callback: %s\n", get_name().c_str(), msg->data.c_str()); } - void notification_callback(const rclcpp_lifecycle::msg::Transition::SharedPtr msg) + void notification_callback(const lifecycle_msgs::msg::Transition::SharedPtr msg) { printf("[%s] notify callback: Transition from state %d to %d\n", get_name().c_str(), static_cast(msg->start_state), static_cast(msg->goal_state)); @@ -48,7 +47,7 @@ class LifecycleListener : public rclcpp::node::Node private: std::shared_ptr> sub_data_; - std::shared_ptr> + std::shared_ptr> sub_notification_; }; diff --git a/rclcpp_lifecycle/src/demos/lifecycle_service_client.cpp b/rclcpp_lifecycle/src/demos/lifecycle_service_client.cpp index e3eced98a8..ddcc5423db 100644 --- a/rclcpp_lifecycle/src/demos/lifecycle_service_client.cpp +++ b/rclcpp_lifecycle/src/demos/lifecycle_service_client.cpp @@ -17,8 +17,8 @@ #include #include -#include -#include +#include +#include // service topics for general lifecycle manager static constexpr auto manager_get_state_topic = "lifecycle_manager__get_state"; @@ -40,22 +40,22 @@ class LifecycleServiceClient : public rclcpp::node::Node init() { // Service clients pointing to a global lifecycle manager - client_get_state_ = this->create_client( + client_get_state_ = this->create_client( manager_get_state_topic); - client_change_state_ = this->create_client( + client_change_state_ = this->create_client( manager_change_state_topic); // Service client pointing to each individual service - client_get_single_state_ = this->create_client( + client_get_single_state_ = this->create_client( node_get_state_topic); - client_change_single_state_ = this->create_client( + client_change_single_state_ = this->create_client( node_change_state_topic); } unsigned int get_state(const std::string & node_name, std::chrono::seconds time_out = 3_s) { - auto request = std::make_shared(); + auto request = std::make_shared(); request->node_name = node_name; if (!client_get_state_->wait_for_service(time_out)) { @@ -85,7 +85,7 @@ class LifecycleServiceClient : public rclcpp::node::Node unsigned int get_single_state(std::chrono::seconds time_out = 3_s) { - auto request = std::make_shared(); + auto request = std::make_shared(); if (!client_get_single_state_->wait_for_service(time_out)) { fprintf(stderr, "Service %s is not available.\n", @@ -115,7 +115,7 @@ class LifecycleServiceClient : public rclcpp::node::Node change_state(const std::string & node_name, rclcpp::lifecycle::LifecycleTransitionsT transition, std::chrono::seconds time_out = 3_s) { - auto request = std::make_shared(); + auto request = std::make_shared(); request->node_name = node_name; request->transition = static_cast(transition); @@ -146,7 +146,7 @@ class LifecycleServiceClient : public rclcpp::node::Node change_single_state(rclcpp::lifecycle::LifecycleTransitionsT transition, std::chrono::seconds time_out = 3_s) { - auto request = std::make_shared(); + auto request = std::make_shared(); request->node_name = lifecycle_node; request->transition = static_cast(transition); @@ -174,10 +174,10 @@ class LifecycleServiceClient : public rclcpp::node::Node } private: - std::shared_ptr> client_get_state_; - std::shared_ptr> client_change_state_; - std::shared_ptr> client_get_single_state_; - std::shared_ptr> client_change_single_state_; + std::shared_ptr> client_get_state_; + std::shared_ptr> client_change_state_; + std::shared_ptr> client_get_single_state_; + std::shared_ptr> client_change_single_state_; }; void diff --git a/rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c b/rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c index 8204aecb0c..b61ea89d2a 100644 --- a/rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c +++ b/rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c @@ -25,16 +25,16 @@ extern "C" #include "rosidl_generator_c/message_type_support.h" #include "rosidl_generator_c/string_functions.h" -#include "rclcpp_lifecycle/msg/transition.h" -#include "rclcpp_lifecycle/srv/get_state.h" -#include "rclcpp_lifecycle/srv/change_state.h" +#include "lifecycle_msgs/msg/transition.h" +#include "lifecycle_msgs/srv/get_state.h" +#include "lifecycle_msgs/srv/change_state.h" #include "rcl_lifecycle/rcl_lifecycle.h" #include "rcl_lifecycle/transition_map.h" #include "default_state_machine.hxx" -static rclcpp_lifecycle__msg__Transition msg; +static lifecycle_msgs__msg__Transition msg; bool concatenate(const char ** prefix, const char ** suffix, char ** result) { @@ -97,7 +97,7 @@ rcl_state_machine_init(rcl_state_machine_t * state_machine, const char* node_nam } const rosidl_message_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT( - rclcpp_lifecycle, msg, Transition); + lifecycle_msgs, msg, Transition); rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); rcl_ret_t ret = rcl_publisher_init(&state_machine->comm_interface.state_publisher, state_machine->comm_interface.node_handle, ts, topic_name, &publisher_options); @@ -123,7 +123,7 @@ rcl_state_machine_init(rcl_state_machine_t * state_machine, const char* node_nam } const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT_FUNCTION( - rclcpp_lifecycle, srv, GetState)(); + lifecycle_msgs, srv, GetState)(); rcl_service_options_t service_options = rcl_service_get_default_options(); rcl_ret_t ret = rcl_service_init(&state_machine->comm_interface.srv_get_state, state_machine->comm_interface.node_handle, ts, topic_name, &service_options); @@ -148,7 +148,7 @@ rcl_state_machine_init(rcl_state_machine_t * state_machine, const char* node_nam } const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT( - rclcpp_lifecycle, srv, ChangeState); + lifecycle_msgs, srv, ChangeState); rcl_service_options_t service_options = rcl_service_get_default_options(); rcl_ret_t ret = rcl_service_init(&state_machine->comm_interface.srv_change_state, state_machine->comm_interface.node_handle, ts, topic_name, &service_options); @@ -336,7 +336,7 @@ rcl_start_transition_by_index(rcl_state_machine_t * state_machine, } // do the initialization - rclcpp_lifecycle__msg__Transition__init(&msg); + lifecycle_msgs__msg__Transition__init(&msg); msg.start_state = state_machine->current_state->index; msg.goal_state = transition->transition_state.index; @@ -344,7 +344,7 @@ rcl_start_transition_by_index(rcl_state_machine_t * state_machine, fprintf(stderr, "%s:%d, Couldn't publish the notification message.\n", __FILE__, __LINE__); } - rclcpp_lifecycle__msg__Transition__fini(&msg); + lifecycle_msgs__msg__Transition__fini(&msg); // Apply a transition state state_machine->current_state = &transition->transition_state; @@ -377,26 +377,26 @@ rcl_finish_transition_by_index(rcl_state_machine_t * state_machine, // high level transition(callback) was executed correctly if (success == true) { - rclcpp_lifecycle__msg__Transition__init(&msg); + lifecycle_msgs__msg__Transition__init(&msg); msg.start_state = transition->transition_state.index; msg.goal_state = transition->goal->index; if (rcl_publish(&state_machine->comm_interface.state_publisher, &msg) != RCL_RET_OK) { fprintf(stderr, "%s:%d, Couldn't publish the notification message.\n", __FILE__, __LINE__); } - rclcpp_lifecycle__msg__Transition__fini(&msg); + lifecycle_msgs__msg__Transition__fini(&msg); state_machine->current_state = transition->goal; return true; } - rclcpp_lifecycle__msg__Transition__init(&msg); + lifecycle_msgs__msg__Transition__init(&msg); msg.start_state = transition->transition_state.index; msg.goal_state = transition->error->index; if (rcl_publish(&state_machine->comm_interface.state_publisher, &msg) != RCL_RET_OK) { fprintf(stderr, "%s:%d, Couldn't publish the notification message.\n", __FILE__, __LINE__); } - rclcpp_lifecycle__msg__Transition__fini(&msg); + lifecycle_msgs__msg__Transition__fini(&msg); state_machine->current_state = transition->error; return true; } diff --git a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp index 79be0acb5e..877a1e344b 100644 --- a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp +++ b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp @@ -23,8 +23,8 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/lifecycle_manager.hpp" -#include "rclcpp_lifecycle/srv/get_state.hpp" -#include "rclcpp_lifecycle/srv/change_state.hpp" +#include "lifecycle_msgs/srv/get_state.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" #include "rcl_lifecycle/rcl_lifecycle.h" @@ -70,11 +70,11 @@ class LifecycleManager::LifecycleManagerImpl void init() { - srv_get_state_ = node_base_handle_->create_service( + srv_get_state_ = node_base_handle_->create_service( node_base_handle_->get_name() + "__get_state", std::bind(&LifecycleManagerImpl::on_get_state, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - srv_change_state_ = node_base_handle_->create_service( + srv_change_state_ = node_base_handle_->create_service( node_base_handle_->get_name() + "__change_state", std::bind(&LifecycleManagerImpl::on_change_state, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -82,16 +82,16 @@ class LifecycleManager::LifecycleManagerImpl void on_get_state(const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) + const std::shared_ptr req, + std::shared_ptr resp) { on_get_single_state(header, req, resp, req->node_name); } void on_get_single_state(const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp, + const std::shared_ptr req, + std::shared_ptr resp, std::string node_name) { (void)header; @@ -107,16 +107,16 @@ class LifecycleManager::LifecycleManagerImpl void on_change_state(const std::shared_ptrheader, - const std::shared_ptr req, - std::shared_ptr resp) + const std::shared_ptr req, + std::shared_ptr resp) { on_change_single_state(header, req, resp, req->node_name); } void on_change_single_state(const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp, + const std::shared_ptr req, + std::shared_ptr resp, std::string node_name) { (void)header; @@ -142,16 +142,16 @@ class LifecycleManager::LifecycleManagerImpl // srv objects may get destroyed directly here { // get_state std::function, - const std::shared_ptr, - std::shared_ptr)> cb = + const std::shared_ptr, + std::shared_ptr)> cb = std::bind(&LifecycleManagerImpl::on_get_single_state, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, node_name); - rclcpp::any_service_callback::AnyServiceCallback any_cb; + rclcpp::any_service_callback::AnyServiceCallback any_cb; any_cb.set(std::move(cb)); auto srv_get_state = - rclcpp::service::Service::make_shared( + rclcpp::service::Service::make_shared( //node_state_machine.state_machine.comm_interface.node_handle, node_base_handle_->get_rcl_node_handle(), &node_state_machine.state_machine.comm_interface.srv_get_state, @@ -163,16 +163,16 @@ class LifecycleManager::LifecycleManagerImpl { // change_state std::function, - const std::shared_ptr, - std::shared_ptr)> cb = + const std::shared_ptr, + std::shared_ptr)> cb = std::bind(&LifecycleManagerImpl::on_change_single_state, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, node_name); - rclcpp::any_service_callback::AnyServiceCallback any_cb; + rclcpp::any_service_callback::AnyServiceCallback any_cb; any_cb.set(cb); auto srv_change_state = - std::make_shared>( + std::make_shared>( //node_state_machine.state_machine.comm_interface.node_handle, node_base_handle_->get_rcl_node_handle(), &node_state_machine.state_machine.comm_interface.srv_change_state, @@ -270,8 +270,8 @@ class LifecycleManager::LifecycleManagerImpl } std::shared_ptr node_base_handle_; - std::shared_ptr> srv_get_state_; - std::shared_ptr> srv_change_state_; + std::shared_ptr> srv_get_state_; + std::shared_ptr> srv_change_state_; std::map node_handle_map_; }; diff --git a/rclcpp_lifecycle/srv/ChangeState.srv b/rclcpp_lifecycle/srv/ChangeState.srv deleted file mode 100644 index b6b4e88710..0000000000 --- a/rclcpp_lifecycle/srv/ChangeState.srv +++ /dev/null @@ -1,4 +0,0 @@ -string node_name -uint8 transition ---- -bool success diff --git a/rclcpp_lifecycle/srv/GetState.srv b/rclcpp_lifecycle/srv/GetState.srv deleted file mode 100644 index b66e0b8e1e..0000000000 --- a/rclcpp_lifecycle/srv/GetState.srv +++ /dev/null @@ -1,3 +0,0 @@ -string node_name ---- -uint8 current_state From 8159e44ee09af459015e629d8a0f2c7f99d0969d Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 28 Nov 2016 17:44:11 -0800 Subject: [PATCH 28/63] (refactor) extract rcl_lifecycle package --- rclcpp_lifecycle/CMakeLists.txt | 47 +- .../include/rcl_lifecycle/data_types.h | 116 ----- .../include/rcl_lifecycle/rcl_lifecycle.h | 101 ----- .../include/rcl_lifecycle/states.h | 45 -- .../include/rcl_lifecycle/transition_map.h | 109 ----- .../rcl_lifecycle/visibility_control.h | 29 -- rclcpp_lifecycle/package.xml | 3 +- .../src/lifecycle_manager_impl.hpp | 26 +- .../src/rcl_lifecycle/default_state_machine.c | 109 ----- .../rcl_lifecycle/default_state_machine.hxx | 36 -- .../src/rcl_lifecycle/rcl_lifecycle.c | 406 ------------------ .../src/rcl_lifecycle/transition_map.c | 192 --------- .../rclcpp_lifecycle/lifecycle_manager.cpp | 120 ------ .../lifecycle_manager_impl.hpp | 280 ------------ 14 files changed, 14 insertions(+), 1605 deletions(-) delete mode 100644 rclcpp_lifecycle/include/rcl_lifecycle/data_types.h delete mode 100644 rclcpp_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h delete mode 100644 rclcpp_lifecycle/include/rcl_lifecycle/states.h delete mode 100644 rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h delete mode 100644 rclcpp_lifecycle/include/rcl_lifecycle/visibility_control.h delete mode 100644 rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.c delete mode 100644 rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.hxx delete mode 100644 rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c delete mode 100644 rclcpp_lifecycle/src/rcl_lifecycle/transition_map.c delete mode 100644 rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp delete mode 100644 rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 5eed06807c..762a25e9d8 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -3,14 +3,12 @@ cmake_minimum_required(VERSION 3.5) project(rclcpp_lifecycle) if(NOT WIN32) - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11 -Wall -Wextra") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra") endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(rcl REQUIRED) -find_package(rmw REQUIRED) +find_package(rcl_lifecycle REQUIRED) find_package(rmw_implementation_cmake REQUIRED) find_package(std_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) @@ -18,44 +16,18 @@ find_package(lifecycle_msgs REQUIRED) include_directories( include) -set(rcl_lifecycle_sources - src/rcl_lifecycle/default_state_machine.c - src/rcl_lifecycle/rcl_lifecycle.c - src/rcl_lifecycle/transition_map.c) -set_source_files_properties( - ${rcl_lifecycle_sources} - PROPERTIES language "C") - macro(targets) get_rclcpp_information("${rmw_implementation}" "rclcpp${target_suffix}") - get_rcl_information("${rmw_implementation}" "rcl${target_suffix}") - get_rmw_typesupport(typesupport_impls_c "${rmw_implementation}" LANGUAGE "c") - get_rmw_typesupport(typesupport_impls_cpp "${rmw_implementation}" LANGUAGE "cpp") - - ### 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}" - "std_msgs" - "lifecycle_msgs") - - install(TARGETS rcl_lifecycle${target_suffix} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) ### CPP High level library add_library(rclcpp_lifecycle${target_suffix} SHARED - src/rclcpp_lifecycle/lifecycle_manager.cpp) + src/lifecycle_manager.cpp) target_link_libraries(rclcpp_lifecycle${target_suffix} - rcl_lifecycle${target_suffix}) + ${rcl_lifecycle_LIBRARIES}) ament_target_dependencies(rclcpp_lifecycle${target_suffix} "rclcpp${target_suffix}" + #"rcl_lifecycle${target_suffix}" This doesn't compile? "lifecycle_msgs") ### demos @@ -63,25 +35,14 @@ macro(targets) src/demos/lifecycle_talker.cpp) target_link_libraries(lifecycle_talker${target_suffix} rclcpp_lifecycle${target_suffix}) - ament_target_dependencies(lifecycle_talker${target_suffix} - "rclcpp${target_suffix}" - "std_msgs") add_executable(lifecycle_listener${target_suffix} src/demos/lifecycle_listener.cpp) target_link_libraries(lifecycle_listener${target_suffix} rclcpp_lifecycle${target_suffix}) - ament_target_dependencies(lifecycle_listener${target_suffix} - "rclcpp${target_suffix}" - "std_msgs" - "lifecycle_msgs") add_executable(lifecycle_service_client${target_suffix} src/demos/lifecycle_service_client.cpp) target_link_libraries(lifecycle_service_client${target_suffix} rclcpp_lifecycle${target_suffix}) - ament_target_dependencies(lifecycle_service_client${target_suffix} - "rclcpp${target_suffix}" - "std_msgs" - "lifecycle_msgs") install(TARGETS rclcpp_lifecycle${target_suffix} diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/data_types.h b/rclcpp_lifecycle/include/rcl_lifecycle/data_types.h deleted file mode 100644 index 354297951a..0000000000 --- a/rclcpp_lifecycle/include/rcl_lifecycle/data_types.h +++ /dev/null @@ -1,116 +0,0 @@ -// 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 - -/** - * @brief simple definition of a state - * @param state: integer giving the state - * @param label: label for easy indexing - */ -typedef struct LIFECYCLE_EXPORT _rcl_state_t -{ - const char * label; - unsigned int index; -} rcl_state_t; - -/** - * @brief transition definition - * @param start: rcl_state_t as a start state - * @param goal: rcl_state_t as a goal state - * TODO: Maybe specify callback pointer here - * and call on_* functions directly - */ -typedef struct LIFECYCLE_EXPORT _rcl_state_transition_t -{ - rcl_state_t transition_state; - void * callback; - const rcl_state_t * start; - const rcl_state_t * goal; - const rcl_state_t * error; -} rcl_state_transition_t; - -/** - * @brief All transitions which are - * valid associations for a primary state. - * One array belongs to one primary state - * within the map. - */ -typedef struct LIFECYCLE_EXPORT _rcl_transition_array_t -{ - rcl_state_transition_t * transitions; - unsigned int size; -} rcl_transition_array_t; - -/** - * @brief stores an array of transitions - * index by a start state - */ -typedef struct LIFECYCLE_EXPORT _rcl_transition_map_t -{ - // associative array between primary state - // and their respective transitions - // 1 ps -> 1 transition_array - rcl_state_t * primary_states; - rcl_transition_array_t * transition_arrays; - unsigned int size; -} rcl_transition_map_t; - -/** - * @brief: object holding all necessary - * ROS communication interfaces for the statemachine. - * node_handle pointer for instantiating - * state_publisher for publishing state changes - * srv_get_state for getting current state - * srv_change_state for requesting a state change - */ -typedef struct LIFECYCLE_EXPORT _rcl_state_comm_interface_t -{ - rcl_node_t * node_handle; - rcl_publisher_t state_publisher; - rcl_service_t srv_get_state; - rcl_service_t srv_change_state; -} rcl_state_comm_interface_t; - -/** - * @brief: statemachine object holding - * a variable state object as current state - * of the complete machine. - * @param transition_map: a map object of all - * possible transitions registered with this - * state machine. - */ -typedef struct LIFECYCLE_EXPORT _rcl_state_machine_t -{ - const rcl_state_t * current_state; - // Map/Associated array of registered states and transitions - rcl_transition_map_t transition_map; - // Communication interface into a ROS world - rcl_state_comm_interface_t comm_interface; -} rcl_state_machine_t; - -#if __cplusplus -} -#endif - -#endif // RCL_LIFECYCLE__DATA_TYPES_H_ diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h b/rclcpp_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h deleted file mode 100644 index 1a86421093..0000000000 --- a/rclcpp_lifecycle/include/rcl_lifecycle/rcl_lifecycle.h +++ /dev/null @@ -1,101 +0,0 @@ -// 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 - -#ifndef __cplusplus - #ifndef bool - #define bool int - #define true 1 - #define false 0 - #endif -#endif - -#include -#include -#include - -LIFECYCLE_EXPORT -rcl_state_machine_t -rcl_get_zero_initialized_state_machine(rcl_node_t * node_handle); - -LIFECYCLE_EXPORT -rcl_ret_t -rcl_state_machine_init(rcl_state_machine_t * state_machine, - const char* node_name, bool default_states); - -LIFECYCLE_EXPORT -rcl_ret_t -rcl_state_machine_fini(rcl_state_machine_t * state_machine); - -// function definitions -/* - * @brief traverses the transition map of the given - * state machine to find if there is a transition from the - * current state to the specified goal state - * @return the transition state which is valid - * NULL if not available - */ -LIFECYCLE_EXPORT -const rcl_state_transition_t * -rcl_is_valid_transition_by_index(rcl_state_machine_t * state_machine, - unsigned int transition_index); -LIFECYCLE_EXPORT -const rcl_state_transition_t * -rcl_is_valid_transition_by_label(rcl_state_machine_t * state_machine, - const char * transition_label); - -LIFECYCLE_EXPORT -const rcl_state_transition_t * -rcl_get_registered_transition_by_index(rcl_state_machine_t * state_machine, - unsigned int transition_state_index); -LIFECYCLE_EXPORT -const rcl_state_transition_t * -rcl_get_registered_transition_by_label(rcl_state_machine_t * state_machine, - const char * transition_state_label); - -LIFECYCLE_EXPORT -rcl_state_t -rcl_create_state(unsigned int state, char * label); - -LIFECYCLE_EXPORT -rcl_state_transition_t -rcl_create_transition(rcl_state_t start, rcl_state_t goal); - -LIFECYCLE_EXPORT -void -rcl_register_callback(rcl_state_machine_t * state_machine, - unsigned int state_index, unsigned int transition_index, bool (* fcn)(void)); - -LIFECYCLE_EXPORT -bool -rcl_start_transition_by_index(rcl_state_machine_t * state_machine, - unsigned int transition_index); - -LIFECYCLE_EXPORT -bool -rcl_finish_transition_by_index(rcl_state_machine_t * state_machine, - unsigned int transition_index, bool success); - -#if __cplusplus -} -#endif // extern "C" - -#endif // RCL_LIFECYCLE__RCL_LIFECYCLE_H_ diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/states.h b/rclcpp_lifecycle/include/rcl_lifecycle/states.h deleted file mode 100644 index 62ed5fd49f..0000000000 --- a/rclcpp_lifecycle/include/rcl_lifecycle/states.h +++ /dev/null @@ -1,45 +0,0 @@ -// 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 RCL_LIFECYCLE__STATES_H_ -#define RCL_LIFECYCLE__STATES_H_ - -#include -#include - -#if __cplusplus -extern "C" -{ -#endif - -// primary states based on -// design.ros2.org/articles/node_lifecycle.html -LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_unknown; -LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_unconfigured; -LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_inactive; -LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_active; -LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_finalized; - -LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_configuring; -LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_cleaningup; -LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_shuttingdown; -LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_activating; -LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_deactivating; -LIFECYCLE_EXPORT extern const rcl_state_t rcl_state_errorprocessing; - -#if __cplusplus -} -#endif // extern "C" - -#endif // RCL_LIFECYCLE__STATES_H_ diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h b/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h deleted file mode 100644 index ed4ef309f6..0000000000 --- a/rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h +++ /dev/null @@ -1,109 +0,0 @@ -// 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 - -LIFECYCLE_EXPORT -void -rcl_register_primary_state(rcl_transition_map_t * m, - rcl_state_t primary_state); - -/** - * @brief appends a transition in a map - * the classification is based on the start state - * within the given transition - */ -LIFECYCLE_EXPORT -void -rcl_register_transition_by_state(rcl_transition_map_t * m, - const rcl_state_t * start, const rcl_state_t * goal, rcl_state_transition_t transition); - -/** - * @brief appends a transition in a map - * the classification is based on the start state - * within the given transition - */ -LIFECYCLE_EXPORT -void -rcl_register_transition_by_label(rcl_transition_map_t * m, - const char * start_label, const char * goal_label, rcl_state_transition_t transition); - -/** - * @brief appends a transition in a map - * the classification is based on the start state - * within the given transition - */ -LIFECYCLE_EXPORT -void -rcl_register_transition_by_index(rcl_transition_map_t * m, - unsigned int start_index, unsigned int goal_index, rcl_state_transition_t transition); - -/** - * @brief gets the registered primary state based on a label - * @return primary state pointer, NULL if not found - */ -LIFECYCLE_EXPORT -rcl_state_t * -rcl_get_primary_state_by_label(rcl_transition_map_t * m, - const char * label); -/** - * @brief gets the registered primary state based on a index - * @return primary state pointer, NULL if not found - */ -LIFECYCLE_EXPORT -rcl_state_t * -rcl_get_primary_state_by_index(rcl_transition_map_t * m, - unsigned int index); - -/** - * @brief gets all transitions based on a label - * label is supposed to come from a rcl_state_t object - */ -LIFECYCLE_EXPORT -rcl_transition_array_t * -rcl_get_transitions_by_label(rcl_transition_map_t * m, - const char * label); -/** - * @brief gets all transitions based on a state - * state is supposed to come from a rcl_state_t object - */ -LIFECYCLE_EXPORT -rcl_transition_array_t * -rcl_get_transitions_by_index(rcl_transition_map_t * m, - unsigned int index); - -/** - * @brief helper functions to print - */ -LIFECYCLE_EXPORT -void -rcl_print_transition_array(const rcl_transition_array_t * transition_array); -LIFECYCLE_EXPORT -void -rcl_print_transition_map(const rcl_transition_map_t * m); - -#if __cplusplus -} -#endif - -#endif // RCL_LIFECYCLE__TRANSITION_MAP_H_ diff --git a/rclcpp_lifecycle/include/rcl_lifecycle/visibility_control.h b/rclcpp_lifecycle/include/rcl_lifecycle/visibility_control.h deleted file mode 100644 index 07fb89ced1..0000000000 --- a/rclcpp_lifecycle/include/rcl_lifecycle/visibility_control.h +++ /dev/null @@ -1,29 +0,0 @@ -// 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__VISIBILITY_CONTROL_H_ -#define RCL_LIFECYCLE__VISIBILITY_CONTROL_H_ - -#ifdef _WIN32 - #define shared_EXPORTS 1 - #ifdef shared_EXPORTS - #define LIFECYCLE_EXPORT __declspec(dllexport) - #else - #define LIFECYCLE_EXPORT __declspec(dllimport) - #endif -#else - #define LIFECYCLE_EXPORT -#endif - -#endif // RCL_LIFECYCLE__VISIBILITY_CONTROL_H_ diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 033085b996..fb7f9fdfb3 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -11,6 +11,7 @@ rosidl_default_generators rclcpp + rcl_lifecycle rmw_implementation rmw_implementation_cmake rosidl_default_generators @@ -18,6 +19,7 @@ lifecycle_msgs rclcpp + rcl_lifecycle rclpy rmw_implementation rosidl_default_runtime @@ -29,6 +31,5 @@ ament_cmake - ament_python diff --git a/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp b/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp index 08f218c0ee..ff2a55ffc3 100644 --- a/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp @@ -21,10 +21,7 @@ #include #include -#include - #include -#include #include #include @@ -66,7 +63,7 @@ class LifecycleManager::LifecycleManagerImpl fprintf(stderr, "%s:%u, FATAL: rcl_state_machine got destroyed externally.\n", __FILE__, __LINE__); } else { - rcl_state_machine_fini(rcl_state_machine, node_base_handle_->get_rcl_node_handle()); + rcl_state_machine_fini(rcl_state_machine); } } } @@ -139,18 +136,9 @@ class LifecycleManager::LifecycleManagerImpl { // TODO(karsten1987): clarify what do if node already exists; NodeStateMachine& node_state_machine = node_handle_map_[node_name]; - node_state_machine.state_machine = rcl_get_zero_initialized_state_machine(); - rcl_ret_t ret = rcl_state_machine_init(&node_state_machine.state_machine, node_base_handle_->get_rcl_node_handle(), - rosidl_generator_cpp::get_message_type_support_handle(), - rosidl_generator_cpp::get_service_type_support_handle(), - rosidl_generator_cpp::get_service_type_support_handle(), - true); - if (ret != RCL_RET_OK) - { - fprintf(stderr, "Error adding %s: %s\n", - node_name.c_str(), rcl_get_error_string_safe()); - return; - } + node_state_machine.state_machine = rcl_get_zero_initialized_state_machine( + node_base_handle_->get_rcl_node_handle()); + rcl_state_machine_init(&node_state_machine.state_machine, node_name.c_str(), true); // srv objects may get destroyed directly here { // get_state @@ -165,7 +153,8 @@ class LifecycleManager::LifecycleManagerImpl any_cb.set(std::move(cb)); auto srv_get_state = rclcpp::service::Service::make_shared( - node_base_handle_->get_shared_node_handle(), + //node_state_machine.state_machine.comm_interface.node_handle, + node_base_handle_->get_rcl_node_handle(), &node_state_machine.state_machine.comm_interface.srv_get_state, any_cb); auto srv_get_state_base = std::dynamic_pointer_cast(srv_get_state); @@ -185,7 +174,8 @@ class LifecycleManager::LifecycleManagerImpl any_cb.set(cb); auto srv_change_state = std::make_shared>( - node_base_handle_->get_shared_node_handle(), + //node_state_machine.state_machine.comm_interface.node_handle, + node_base_handle_->get_rcl_node_handle(), &node_state_machine.state_machine.comm_interface.srv_change_state, any_cb); auto srv_change_state_base = std::dynamic_pointer_cast(srv_change_state); diff --git a/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.c b/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.c deleted file mode 100644 index 4055c1e3dc..0000000000 --- a/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.c +++ /dev/null @@ -1,109 +0,0 @@ -// 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 "rosidl_generator_c/message_type_support.h" -#include "rosidl_generator_c/string_functions.h" -#include "std_msgs/msg/string.h" - -#include "rcl_lifecycle/states.h" -#include "rcl_lifecycle/transition_map.h" - -#include "default_state_machine.hxx" - -#if __cplusplus -extern "C" -{ -#endif - -// *INDENT-OFF* -const rcl_state_t rcl_state_unknown = {"unknown", 0}; -const rcl_state_t rcl_state_unconfigured = {"unconfigured", 1}; -const rcl_state_t rcl_state_inactive = {"my_inactive", 2}; -const rcl_state_t rcl_state_active = {"active", 3}; -const rcl_state_t rcl_state_finalized = {"finalized", 4}; - -const rcl_state_t rcl_state_configuring = {"configuring", 10}; -const rcl_state_t rcl_state_cleaningup = {"cleaningup", 11}; -const rcl_state_t rcl_state_shuttingdown = {"shuttingdown", 12}; -const rcl_state_t rcl_state_activating = {"activating", 13}; -const rcl_state_t rcl_state_deactivating = {"deactivating", 14}; -const rcl_state_t rcl_state_errorprocessing = {"errorprocessing", 15}; -// *INDENT-ON* - -rcl_state_t -rcl_create_state(unsigned int index, char * label) -{ - rcl_state_t ret_state = {.index = index, .label = label}; - return ret_state; -} - -rcl_state_transition_t -rcl_create_state_transition(unsigned int index, const char * label) -{ - rcl_state_transition_t ret_transition = {{.index = index, .label = label}, - NULL, NULL, NULL, &rcl_state_errorprocessing}; - return ret_transition; -} - -// default implementation as despicted on -// design.ros2.org -rcl_ret_t -rcl_init_default_state_machine(rcl_state_machine_t * state_machine) -{ - // Maybe we can directly store only pointers to states - rcl_register_primary_state(&state_machine->transition_map, rcl_state_unconfigured); - rcl_register_primary_state(&state_machine->transition_map, rcl_state_inactive); - rcl_register_primary_state(&state_machine->transition_map, rcl_state_active); - rcl_register_primary_state(&state_machine->transition_map, rcl_state_finalized); - - rcl_state_transition_t rcl_transition_configuring = rcl_create_state_transition( - rcl_state_configuring.index, rcl_state_configuring.label); - rcl_state_transition_t rcl_transition_shuttingdown = rcl_create_state_transition( - rcl_state_shuttingdown.index, rcl_state_shuttingdown.label); - rcl_state_transition_t rcl_transition_cleaningup = rcl_create_state_transition( - rcl_state_cleaningup.index, rcl_state_cleaningup.label); - rcl_state_transition_t rcl_transition_activating = rcl_create_state_transition( - rcl_state_activating.index, rcl_state_activating.label); - rcl_state_transition_t rcl_transition_deactivating = rcl_create_state_transition( - rcl_state_deactivating.index, rcl_state_deactivating.label); - - // add transitions to map - rcl_register_transition_by_state(&state_machine->transition_map, - &rcl_state_unconfigured, &rcl_state_inactive, rcl_transition_configuring); - rcl_register_transition_by_state(&state_machine->transition_map, - &rcl_state_inactive, &rcl_state_unconfigured, rcl_transition_cleaningup); - rcl_register_transition_by_state(&state_machine->transition_map, - &rcl_state_unconfigured, &rcl_state_finalized, rcl_transition_shuttingdown); - rcl_register_transition_by_state(&state_machine->transition_map, - &rcl_state_inactive, &rcl_state_finalized, rcl_transition_shuttingdown); - rcl_register_transition_by_state(&state_machine->transition_map, - &rcl_state_inactive, &rcl_state_active, rcl_transition_activating); - rcl_register_transition_by_state(&state_machine->transition_map, - &rcl_state_active, &rcl_state_inactive, rcl_transition_deactivating); - rcl_register_transition_by_state(&state_machine->transition_map, - &rcl_state_active, &rcl_state_finalized, rcl_transition_shuttingdown); - - // set to first entry - // state_machine->current_state = &state_machine->transition_map.primary_states[0]; - state_machine->current_state = &rcl_state_unconfigured; - return RCL_RET_OK; -} - -#if __cplusplus -} -#endif // extern "C" diff --git a/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.hxx b/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.hxx deleted file mode 100644 index 031893272f..0000000000 --- a/rclcpp_lifecycle/src/rcl_lifecycle/default_state_machine.hxx +++ /dev/null @@ -1,36 +0,0 @@ -// 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__DEFAULT_STATE_MACHINE_HXX_ -#define RCL_LIFECYCLE__DEFAULT_STATE_MACHINE_HXX_ - -#include - -#include "rcl_lifecycle/data_types.h" -#include "rcl_lifecycle/visibility_control.h" - -#if __cplusplus -extern "C" -{ -#endif - -LIFECYCLE_EXPORT -rcl_ret_t -rcl_init_default_state_machine(rcl_state_machine_t * state_machine); - -#if __cplusplus -} -#endif - -#endif // RCL_LIFECYCLE__DEFAULT_STATE_MACHINE_HXX_ diff --git a/rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c b/rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c deleted file mode 100644 index b61ea89d2a..0000000000 --- a/rclcpp_lifecycle/src/rcl_lifecycle/rcl_lifecycle.c +++ /dev/null @@ -1,406 +0,0 @@ -// 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 "rosidl_generator_c/message_type_support.h" -#include "rosidl_generator_c/string_functions.h" - -#include "lifecycle_msgs/msg/transition.h" -#include "lifecycle_msgs/srv/get_state.h" -#include "lifecycle_msgs/srv/change_state.h" - -#include "rcl_lifecycle/rcl_lifecycle.h" -#include "rcl_lifecycle/transition_map.h" - -#include "default_state_machine.hxx" - -static lifecycle_msgs__msg__Transition 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; -} - -// get zero initialized state machine here -rcl_state_machine_t -rcl_get_zero_initialized_state_machine(rcl_node_t * node_handle) -{ - rcl_state_machine_t state_machine; - state_machine.transition_map.size = 0; - state_machine.transition_map.primary_states = NULL; - state_machine.transition_map.transition_arrays = NULL; - state_machine.comm_interface.node_handle = node_handle; - state_machine.comm_interface.state_publisher = rcl_get_zero_initialized_publisher(); - state_machine.comm_interface.srv_get_state = rcl_get_zero_initialized_service(); - state_machine.comm_interface.srv_change_state = rcl_get_zero_initialized_service(); - - return state_machine; -} - -rcl_ret_t -rcl_state_machine_init(rcl_state_machine_t * state_machine, const char* node_name, bool default_states) -{ - // TODO(karsten1987): fail when state machine not zero initialized - // { // initialize node handle for notification - // rcl_node_options_t node_options = rcl_node_get_default_options(); - // { - // rcl_ret_t ret = rcl_node_init(&state_machine->notification_node_handle, - // node_name, &node_options); - // if (ret != RCL_RET_OK) { - // fprintf(stderr, "%s:%u, Unable to initialize node handle for state machine\n", - // __FILE__, __LINE__); - // state_machine = NULL; - // return ret; - // } - // } - // } - //const char * node_name = rcl_node_get_name(state_machine->notification_node_handle); - - { // initialize publisher - // Build topic, topic suffix hardcoded for now - // and limited in length of 255 - const char * topic_prefix = "__transition_notify"; - char * topic_name; - if (concatenate(&node_name, &topic_prefix, &topic_name) != true) { - fprintf(stderr, "%s:%u, Topic name exceeds maximum size of 255\n", - __FILE__, __LINE__); - state_machine = NULL; - return RCL_RET_ERROR; - } - - const rosidl_message_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT( - lifecycle_msgs, msg, Transition); - rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); - rcl_ret_t ret = rcl_publisher_init(&state_machine->comm_interface.state_publisher, - state_machine->comm_interface.node_handle, ts, topic_name, &publisher_options); - - if (ret != RCL_RET_OK) { - state_machine = NULL; - free(topic_name); - return ret; - } - free(topic_name); - } - - { // 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) != true) { - fprintf(stderr, "%s:%u, Topic name exceeds maximum size of 255\n", - __FILE__, __LINE__); - state_machine = NULL; - return RCL_RET_ERROR; - } - - const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT_FUNCTION( - lifecycle_msgs, srv, GetState)(); - rcl_service_options_t service_options = rcl_service_get_default_options(); - rcl_ret_t ret = rcl_service_init(&state_machine->comm_interface.srv_get_state, - state_machine->comm_interface.node_handle, ts, topic_name, &service_options); - if (ret != RCL_RET_OK) { - state_machine = NULL; - free(topic_name); - return ret; - } - free(topic_name); - } - - { // 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) != true) { - fprintf(stderr, "%s:%u, Topic name exceeds maximum size of 255\n", - __FILE__, __LINE__); - state_machine = NULL; - return RCL_RET_ERROR; - } - - const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT( - lifecycle_msgs, srv, ChangeState); - rcl_service_options_t service_options = rcl_service_get_default_options(); - rcl_ret_t ret = rcl_service_init(&state_machine->comm_interface.srv_change_state, - state_machine->comm_interface.node_handle, ts, topic_name, &service_options); - if (ret != RCL_RET_OK) { - state_machine = NULL; - free(topic_name); - return ret; - } - free(topic_name); - } - - if (default_states) { - rcl_init_default_state_machine(state_machine); - } - return RCL_RET_OK; -} - -rcl_ret_t -rcl_state_machine_fini(rcl_state_machine_t * state_machine) -{ - { // destroy get state srv - rcl_ret_t ret = rcl_service_fini(&state_machine->comm_interface.srv_get_state, - state_machine->comm_interface.node_handle); - if (ret != RCL_RET_OK) { - fprintf(stderr, "%s:%u, Failed to destroy get_state_srv service\n", - __FILE__, __LINE__); - } - } - - { // destroy change state srv - rcl_ret_t ret = rcl_service_fini(&state_machine->comm_interface.srv_change_state, - state_machine->comm_interface.node_handle); - if (ret != RCL_RET_OK) { - fprintf(stderr, "%s:%u, Failed to destroy change_state_srv service\n", - __FILE__, __LINE__); - } - } - - { // destroy the publisher - rcl_ret_t ret = rcl_publisher_fini(&state_machine->comm_interface.state_publisher, - state_machine->comm_interface.node_handle); - if (ret != RCL_RET_OK) { - fprintf(stderr, "%s:%u, Failed to destroy state publisher publisher\n", - __FILE__, __LINE__); - } - } - // { // destroy the node handle - // rcl_ret_t ret = rcl_node_fini(&state_machine->notification_node_handle); - // if (ret != RCL_RET_OK) { - // fprintf(stderr, "%s:%u, Failed to destroy lifecycle notification node handle\n", - // __FILE__, __LINE__); - // } - // } - - rcl_transition_map_t * transition_map = &state_machine->transition_map; - - // free the primary states array - free(transition_map->primary_states); - transition_map->primary_states = NULL; - for (unsigned int i = 0; i < transition_map->size; ++i) { - // free each transition array associated to a primary state - free(transition_map->transition_arrays[i].transitions); - transition_map->transition_arrays[i].transitions = NULL; - } - // free the top level transition array - free(transition_map->transition_arrays); - transition_map->transition_arrays = NULL; - - return RCL_RET_OK; -} - -const rcl_state_transition_t * -rcl_is_valid_transition_by_index(rcl_state_machine_t * state_machine, - unsigned int transition_index) -{ - unsigned int current_index = state_machine->current_state->index; - const rcl_transition_array_t * valid_transitions = rcl_get_transitions_by_index( - &state_machine->transition_map, current_index); - if (valid_transitions == NULL) { - fprintf(stderr, "%s:%u, No transitions registered for current state %s\n", - __FILE__, __LINE__, state_machine->current_state->label); - return NULL; - } - for (unsigned int i = 0; i < valid_transitions->size; ++i) { - if (valid_transitions->transitions[i].transition_state.index == transition_index) { - return &valid_transitions->transitions[i]; - } - } - fprintf(stderr, "%s:%u, No transition matching %u found for current state %s\n", - __FILE__, __LINE__, transition_index, state_machine->current_state->label); - return NULL; -} - -const rcl_state_transition_t * -rcl_is_valid_transition_by_label(rcl_state_machine_t * state_machine, - const char * transition_label) -{ - unsigned int current_index = state_machine->current_state->index; - const rcl_transition_array_t * valid_transitions = rcl_get_transitions_by_index( - &state_machine->transition_map, current_index); - for (unsigned int i = 0; i < valid_transitions->size; ++i) { - if (valid_transitions->transitions[i].transition_state.label == transition_label) { - return &valid_transitions->transitions[i]; - } - } - return NULL; -} - -const rcl_state_transition_t * -rcl_get_registered_transition_by_index(rcl_state_machine_t * state_machine, - unsigned int transition_state_index) -{ - // extensive search approach - // TODO(karsten1987) can be improved by having a separate array - // for "registered transition" - const rcl_transition_map_t * map = &state_machine->transition_map; - for (unsigned int i = 0; i < map->size; ++i) { - for (unsigned int j = 0; j < map->transition_arrays[i].size; ++j) { - if (map->transition_arrays[i].transitions[j].transition_state.index == - transition_state_index) - { - return &map->transition_arrays[i].transitions[j]; - } - } - } - return NULL; -} - -const rcl_state_transition_t * -rcl_get_registered_transition_by_label(rcl_state_machine_t * state_machine, - const char * transition_state_label) -{ - // extensive search approach - // TODO(karsten1987) can be improved by having a separate array - // for "registered transition" - const rcl_transition_map_t * map = &state_machine->transition_map; - for (unsigned int i = 0; i < map->size; ++i) { - for (unsigned int j = 0; j < map->transition_arrays[i].size; ++j) { - if (strcmp(map->transition_arrays[i].transitions[j].transition_state.label, - transition_state_label) == 0) - { - return &map->transition_arrays[i].transitions[j]; - } - } - } - return NULL; -} - -void -rcl_register_callback(rcl_state_machine_t * state_machine, - unsigned int state_index, unsigned int transition_index, bool (* fcn)(void)) -{ - rcl_transition_array_t * transitions = rcl_get_transitions_by_index( - &state_machine->transition_map, state_index); - for (unsigned int i = 0; i < transitions->size; ++i) { - if (transitions->transitions[i].transition_state.index == transition_index) { - transitions->transitions[i].callback = fcn; - } - } -} - -// maybe change directly the current state here, -// no need to that all the time inside high level language -bool -rcl_start_transition_by_index(rcl_state_machine_t * state_machine, - unsigned int transition_index) -{ - const rcl_state_transition_t * transition = - rcl_is_valid_transition_by_index(state_machine, transition_index); - - // If we have a faulty transition pointer - if (transition == NULL) { - fprintf(stderr, "%s:%d, Could not find registered transition %u\n", - __FILE__, __LINE__, transition_index); - return false; - } - - // If we have a transition which is semantically not correct - // we may have to set the current state to something intermediate - // or simply ignore it - if (transition->start != state_machine->current_state) { - fprintf(stderr, "%s:%d, Wrong transition index %s. State machine is in primary state %s\n", - __FILE__, __LINE__, transition->start->label, state_machine->current_state->label); - return false; - } - - // do the initialization - lifecycle_msgs__msg__Transition__init(&msg); - msg.start_state = state_machine->current_state->index; - msg.goal_state = transition->transition_state.index; - - if (rcl_publish(&state_machine->comm_interface.state_publisher, &msg) != RCL_RET_OK) { - fprintf(stderr, "%s:%d, Couldn't publish the notification message.\n", - __FILE__, __LINE__); - } - lifecycle_msgs__msg__Transition__fini(&msg); - - // Apply a transition state - state_machine->current_state = &transition->transition_state; - - return true; -} - -bool -rcl_finish_transition_by_index(rcl_state_machine_t * state_machine, - unsigned int transition_index, bool success) -{ - const rcl_state_transition_t * transition = - rcl_get_registered_transition_by_index(state_machine, transition_index); - - // If we have a faulty transition pointer - if (transition == NULL) { - fprintf(stderr, "%s:%d, Could not find registered transition %u\n", - __FILE__, __LINE__, transition_index); - return false; - } - - // If we have a transition which is semantically not correct - // we may have to set the current state to something intermediate - // or simply ignore it - if (&transition->transition_state != state_machine->current_state) { - fprintf(stderr, "%s:%d, Wrong transition state. State machine is in primary state %s\n", - __FILE__, __LINE__, state_machine->current_state->label); - return false; - } - - // high level transition(callback) was executed correctly - if (success == true) { - lifecycle_msgs__msg__Transition__init(&msg); - msg.start_state = transition->transition_state.index; - msg.goal_state = transition->goal->index; - if (rcl_publish(&state_machine->comm_interface.state_publisher, &msg) != RCL_RET_OK) { - fprintf(stderr, "%s:%d, Couldn't publish the notification message.\n", - __FILE__, __LINE__); - } - lifecycle_msgs__msg__Transition__fini(&msg); - state_machine->current_state = transition->goal; - return true; - } - - lifecycle_msgs__msg__Transition__init(&msg); - msg.start_state = transition->transition_state.index; - msg.goal_state = transition->error->index; - if (rcl_publish(&state_machine->comm_interface.state_publisher, &msg) != RCL_RET_OK) { - fprintf(stderr, "%s:%d, Couldn't publish the notification message.\n", - __FILE__, __LINE__); - } - lifecycle_msgs__msg__Transition__fini(&msg); - state_machine->current_state = transition->error; - return true; -} - -#if __cplusplus -} -#endif // extern "C" diff --git a/rclcpp_lifecycle/src/rcl_lifecycle/transition_map.c b/rclcpp_lifecycle/src/rcl_lifecycle/transition_map.c deleted file mode 100644 index 1dca057f90..0000000000 --- a/rclcpp_lifecycle/src/rcl_lifecycle/transition_map.c +++ /dev/null @@ -1,192 +0,0 @@ -// 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_register_primary_state(rcl_transition_map_t * m, - rcl_state_t primary_state) -{ - if (rcl_get_primary_state_by_index(m, primary_state.index) != NULL) { - // primary state is already registered - fprintf(stderr, "%s:%u, Primary state %u is already registered\n", - __FILE__, __LINE__, primary_state.index); - return; - } - - // add new primary state memory - ++m->size; - if (m->size == 1) { - m->primary_states = malloc(m->size * sizeof(rcl_state_t)); - m->transition_arrays = malloc(m->size * sizeof(rcl_transition_array_t)); - } else { - m->primary_states = realloc( - m->primary_states, m->size * sizeof(rcl_state_t)); - m->transition_arrays = realloc( - m->transition_arrays, m->size * sizeof(rcl_transition_array_t)); - } - m->primary_states[m->size - 1] = primary_state; - m->transition_arrays[m->size - 1].transitions = NULL; - m->transition_arrays[m->size - 1].size = 0; // initialize array to size 0 -} - -void -rcl_register_transition_by_state(rcl_transition_map_t * m, - const rcl_state_t * start, const rcl_state_t * goal, rcl_state_transition_t transition) -{ - transition.start = start; - transition.goal = goal; - - // TODO(karsten1987): check whether we can improve that - rcl_transition_array_t * transition_array = rcl_get_transitions_by_index( - m, transition.start->index); - if (!transition_array) { - fprintf(stderr, "%s:%u, Unable to find transition array registered for start index %u", - __FILE__, __LINE__, transition.start->index); - return; - } - - // we add a new transition, so increase the size - ++transition_array->size; - if (transition_array->size == 1) { - transition_array->transitions = malloc( - transition_array->size * sizeof(rcl_state_transition_t)); - } else { - transition_array->transitions = realloc( - transition_array->transitions, transition_array->size * sizeof(rcl_state_transition_t)); - } - // finally set the new transition to the end of the array - transition_array->transitions[transition_array->size - 1] = transition; -} - -void -rcl_register_transition_by_label(rcl_transition_map_t * m, - const char * start_label, const char * goal_label, rcl_state_transition_t transition) -{ - // the idea here is to add this transition based on the - // start label and classify them. - const rcl_state_t * start_state = rcl_get_primary_state_by_label(m, start_label); - if (start_state == NULL) { - // return false here? - return; - } - const rcl_state_t * goal_state = rcl_get_primary_state_by_label(m, goal_label); - if (goal_state == NULL) { - // return false here? - return; - } - rcl_register_transition_by_state(m, start_state, goal_state, transition); -} - -void -rcl_register_transition_by_index(rcl_transition_map_t * m, - unsigned int start_index, unsigned int goal_index, rcl_state_transition_t transition) -{ - // the idea here is to add this transition based on the - // start label and classify them. - const rcl_state_t * start_state = rcl_get_primary_state_by_index(m, start_index); - if (start_state == NULL) { - // return false here? - return; - } - const rcl_state_t * goal_state = rcl_get_primary_state_by_index(m, goal_index); - if (goal_state == NULL) { - // return false here? - return; - } - rcl_register_transition_by_state(m, start_state, goal_state, transition); -} - -rcl_state_t * -rcl_get_primary_state_by_label(rcl_transition_map_t * m, - const char * label) -{ - for (unsigned int i = 0; i < m->size; ++i) { - if (m->primary_states[i].label == label) { - return &m->primary_states[i]; - } - } - return NULL; -} - -rcl_state_t * -rcl_get_primary_state_by_index(rcl_transition_map_t * m, - unsigned int index) -{ - for (unsigned int i = 0; i < m->size; ++i) { - if (m->primary_states[i].index == index) { - return &m->primary_states[i]; - } - } - return NULL; -} - -rcl_transition_array_t * -rcl_get_transitions_by_label(rcl_transition_map_t * m, - const char * label) -{ - for (unsigned int i = 0; i < m->size; ++i) { - if (strcmp(m->primary_states[i].label, label) == 0) { - return &m->transition_arrays[i]; - } - } - return NULL; -} - -rcl_transition_array_t * -rcl_get_transitions_by_index(rcl_transition_map_t * m, - unsigned int index) -{ - for (unsigned int i = 0; i < m->size; ++i) { - if (m->primary_states[i].index == index) { - return &m->transition_arrays[i]; - } - } - return NULL; -} - -void -rcl_print_transition_array(const rcl_transition_array_t * ta) -{ - for (unsigned int i = 0; i < ta->size; ++i) { - printf("%s(%u)(%s)\t", ta->transitions[i].transition_state.label, - ta->transitions[i].transition_state.index, - (ta->transitions[i].callback == NULL) ? " " : "x"); - } - printf("\n"); -} - -void -rcl_print_transition_map(const rcl_transition_map_t * m) -{ - printf("rcl_transition_map_t size %u\n", m->size); - for (unsigned int i = 0; i < m->size; ++i) { - printf("Start state %s(%u) ::: ", m->primary_states[i].label, - m->primary_states[i].index); - rcl_print_transition_array(&(m->transition_arrays[i])); - } -} - -#if __cplusplus -} -#endif diff --git a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp deleted file mode 100644 index 7ca1b7885d..0000000000 --- a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager.cpp +++ /dev/null @@ -1,120 +0,0 @@ -// 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 "rclcpp_lifecycle/lifecycle_manager.hpp" - -#include "lifecycle_manager_impl.hpp" // implementation - -namespace rclcpp -{ -namespace lifecycle -{ - -LifecycleManager::LifecycleManager() -: impl_(new LifecycleManagerImpl()) -{ - impl_->init(); -} - -LifecycleManager::~LifecycleManager() = default; - -std::shared_ptr -LifecycleManager::get_node_base_interface() -{ - return impl_->node_base_handle_; -} - -void -LifecycleManager::add_node_interface(const NodePtr & node) -{ - add_node_interface(node->get_base_interface()->get_name(), node); -} - -void -LifecycleManager::add_node_interface(const std::string & node_name, - const NodeInterfacePtr & node_interface) -{ - impl_->add_node_interface(node_name, node_interface); -} - -bool -LifecycleManager::register_on_configure(const std::string & node_name, - std::function & fcn) -{ - return impl_->register_callback(node_name, fcn); -} - -bool -LifecycleManager::configure(const std::string & node_name) -{ - return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::CONFIGURING); -} - -bool -LifecycleManager::register_on_cleanup(const std::string & node_name, - std::function & fcn) -{ - return impl_->register_callback(node_name, fcn); -} - -bool -LifecycleManager::cleanup(const std::string & node_name) -{ - return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::CLEANINGUP); -} - -bool -LifecycleManager::register_on_shutdown(const std::string & node_name, - std::function & fcn) -{ - return impl_->register_callback(node_name, fcn); -} - -bool -LifecycleManager::shutdown(const std::string & node_name) -{ - return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::SHUTTINGDOWN); -} - -bool -LifecycleManager::register_on_activate(const std::string & node_name, - std::function & fcn) -{ - return impl_->register_callback(node_name, fcn); -} - -bool -LifecycleManager::activate(const std::string & node_name) -{ - return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::ACTIVATING); -} - -bool -LifecycleManager::register_on_deactivate(const std::string & node_name, - std::function & fcn) -{ - return impl_->register_callback(node_name, fcn); -} - -bool -LifecycleManager::deactivate(const std::string & node_name) -{ - return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::DEACTIVATING); -} - -} // namespace lifecycle -} // namespace rclcpp diff --git a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp b/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp deleted file mode 100644 index 877a1e344b..0000000000 --- a/rclcpp_lifecycle/src/rclcpp_lifecycle/lifecycle_manager_impl.hpp +++ /dev/null @@ -1,280 +0,0 @@ -// 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 RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_IMPL_HPP_ -#define RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_IMPL_HPP_ - -#include -#include -#include -#include -#include - -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/lifecycle_manager.hpp" -#include "lifecycle_msgs/srv/get_state.hpp" -#include "lifecycle_msgs/srv/change_state.hpp" - -#include "rcl_lifecycle/rcl_lifecycle.h" - -namespace rclcpp -{ -namespace lifecycle -{ - -using LifecycleInterface = rclcpp::node::lifecycle::LifecycleNodeInterface; -using LifecycleInterfacePtr = std::shared_ptr; -using LifecycleInterfaceWeakPtr = std::weak_ptr; - -struct NodeStateMachine -{ - LifecycleInterfaceWeakPtr weak_node_handle; - rcl_state_machine_t state_machine; - std::map> cb_map; - std::shared_ptr srv_get_state; - std::shared_ptr srv_change_state; -}; - -class LifecycleManager::LifecycleManagerImpl -{ -public: - LifecycleManagerImpl() - : node_base_handle_(std::make_shared("lifecycle_manager")) - {} - - ~LifecycleManagerImpl() - { - for (auto it = node_handle_map_.begin(); it != node_handle_map_.end(); ++it) { - rcl_state_machine_t * rcl_state_machine = &it->second.state_machine; - if (!rcl_state_machine) - { - fprintf(stderr, "%s:%u, FATAL: rcl_state_machine got destroyed externally.\n", - __FILE__, __LINE__); - } else { - rcl_state_machine_fini(rcl_state_machine); - } - } - } - - void - init() - { - srv_get_state_ = node_base_handle_->create_service( - node_base_handle_->get_name() + "__get_state", - std::bind(&LifecycleManagerImpl::on_get_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - srv_change_state_ = node_base_handle_->create_service( - node_base_handle_->get_name() + "__change_state", - std::bind(&LifecycleManagerImpl::on_change_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - } - - void - on_get_state(const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) - { - on_get_single_state(header, req, resp, req->node_name); - } - - void - on_get_single_state(const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp, - std::string node_name) - { - (void)header; - (void)req; - auto node_handle_iter = node_handle_map_.find(node_name); - if (node_handle_iter == node_handle_map_.end()) { - resp->current_state = static_cast(LifecyclePrimaryStatesT::UNKNOWN); - return; - } - resp->current_state = - static_cast(node_handle_iter->second.state_machine.current_state->index); - } - - void - on_change_state(const std::shared_ptrheader, - const std::shared_ptr req, - std::shared_ptr resp) - { - on_change_single_state(header, req, resp, req->node_name); - } - - void - on_change_single_state(const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp, - std::string node_name) - { - (void)header; - auto node_handle_iter = node_handle_map_.find(node_name); - if (node_handle_iter == node_handle_map_.end()) { - resp->success = false; - return; - } - auto transition = static_cast(req->transition); - resp->success = change_state(node_name, transition); - } - - void - add_node_interface(const std::string & node_name, - const LifecycleInterfacePtr & lifecycle_interface) - { - // TODO(karsten1987): clarify what do if node already exists; - NodeStateMachine& node_state_machine = node_handle_map_[node_name]; - node_state_machine.state_machine = rcl_get_zero_initialized_state_machine( - node_base_handle_->get_rcl_node_handle()); - rcl_state_machine_init(&node_state_machine.state_machine, node_name.c_str(), true); - - // srv objects may get destroyed directly here - { // get_state - std::function, - const std::shared_ptr, - std::shared_ptr)> cb = - std::bind(&LifecycleManagerImpl::on_get_single_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - node_name); - - rclcpp::any_service_callback::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - auto srv_get_state = - rclcpp::service::Service::make_shared( - //node_state_machine.state_machine.comm_interface.node_handle, - node_base_handle_->get_rcl_node_handle(), - &node_state_machine.state_machine.comm_interface.srv_get_state, - any_cb); - auto srv_get_state_base = std::dynamic_pointer_cast(srv_get_state); - node_base_handle_->add_service(srv_get_state_base); - node_state_machine.srv_get_state = srv_get_state_base; - } - - { // change_state - std::function, - const std::shared_ptr, - std::shared_ptr)> cb = - std::bind(&LifecycleManagerImpl::on_change_single_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - node_name); - - rclcpp::any_service_callback::AnyServiceCallback any_cb; - any_cb.set(cb); - auto srv_change_state = - std::make_shared>( - //node_state_machine.state_machine.comm_interface.node_handle, - node_base_handle_->get_rcl_node_handle(), - &node_state_machine.state_machine.comm_interface.srv_change_state, - any_cb); - auto srv_change_state_base = std::dynamic_pointer_cast(srv_change_state); - node_base_handle_->add_service(srv_change_state_base); - node_state_machine.srv_change_state = srv_change_state_base; - } - - node_state_machine.weak_node_handle = lifecycle_interface; - // register default callbacks - // maybe optional - std::function cb_configuring = std::bind( - &LifecycleInterface::on_configure, lifecycle_interface); - std::function cb_cleaningup = std::bind( - &LifecycleInterface::on_cleanup, lifecycle_interface); - std::function cb_shuttingdown = std::bind( - &LifecycleInterface::on_shutdown, lifecycle_interface); - std::function cb_activating = std::bind( - &LifecycleInterface::on_activate, lifecycle_interface); - std::function cb_deactivating = std::bind( - &LifecycleInterface::on_deactivate, lifecycle_interface); - std::function cb_error = std::bind( - &LifecycleInterface::on_error, lifecycle_interface); - node_state_machine.cb_map[LifecycleTransitionsT::CONFIGURING] = cb_configuring; - node_state_machine.cb_map[LifecycleTransitionsT::CLEANINGUP] = cb_cleaningup; - node_state_machine.cb_map[LifecycleTransitionsT::SHUTTINGDOWN] = cb_shuttingdown; - node_state_machine.cb_map[LifecycleTransitionsT::ACTIVATING] = cb_activating; - node_state_machine.cb_map[LifecycleTransitionsT::DEACTIVATING] = cb_deactivating; - node_state_machine.cb_map[LifecycleTransitionsT::ERRORPROCESSING] = cb_error; - } - - template - bool - register_callback(const std::string & node_name, std::function & cb) - { - if (node_name.empty()) { - return false; - } - - auto node_handle_iter = node_handle_map_.find(node_name); - if (node_handle_iter == node_handle_map_.end()) { - fprintf(stderr, "Node with name %s is not registered\n", node_name.c_str()); - } - node_handle_iter->second.cb_map[lifecycle_transition] = cb; - return true; - } - - bool - change_state(const std::string & node_name, LifecycleTransitionsT lifecycle_transition) - { - if (node_name.empty()) { - return false; - } - - auto node_handle_iter = node_handle_map_.find(node_name); - if (node_handle_iter == node_handle_map_.end()) { - fprintf(stderr, "%s:%d, Node with name %s is not registered\n", - __FILE__, __LINE__, node_name.c_str()); - return false; - } - - auto node_handle = node_handle_iter->second.weak_node_handle.lock(); - if (!node_handle) { - fprintf(stderr, - "%s:%d, Nodehandle is not available. Was it destroyed outside the lifecycle manager?\n", - __FILE__, __LINE__); - return false; - } - - unsigned int transition_index = static_cast(lifecycle_transition); - if (!rcl_start_transition_by_index(&node_handle_iter->second.state_machine, transition_index)) { - fprintf(stderr, "%s:%d, Unable to start transition %u from current state %s\n", - __FILE__, __LINE__, transition_index, - node_handle_iter->second.state_machine.current_state->label); - return false; - } - - // Since we set always set a default callback, - // we don't have to check for nullptr here - std::function callback = node_handle_iter->second.cb_map[lifecycle_transition]; - auto success = callback(); - - if (!rcl_finish_transition_by_index(&node_handle_iter->second.state_machine, - transition_index, success)) - { - fprintf(stderr, "Failed to finish transition %u. Current state is now: %s\n", - transition_index, node_handle_iter->second.state_machine.current_state->label); - return false; - } - // This true holds in both cases where the actual callback - // was successful or not, since at this point we have a valid transistion - // to either a new primary state or error state - return true; - } - - std::shared_ptr node_base_handle_; - std::shared_ptr> srv_get_state_; - std::shared_ptr> srv_change_state_; - std::map node_handle_map_; -}; - -} // namespace lifecycle -} // namespace rclcpp -#endif // RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_IMPL_HPP_ From 275869108211d2f3b7aa06f26b851a8670b3f082 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 28 Nov 2016 20:19:52 -0800 Subject: [PATCH 29/63] (refactor) extract lifecycle demos --- rclcpp_lifecycle/CMakeLists.txt | 24 +- .../src/demos/lifecycle_listener.cpp | 62 ----- .../src/demos/lifecycle_service_client.cpp | 230 ------------------ .../src/demos/lifecycle_talker.cpp | 95 -------- 4 files changed, 7 insertions(+), 404 deletions(-) delete mode 100644 rclcpp_lifecycle/src/demos/lifecycle_listener.cpp delete mode 100644 rclcpp_lifecycle/src/demos/lifecycle_service_client.cpp delete mode 100644 rclcpp_lifecycle/src/demos/lifecycle_talker.cpp diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 762a25e9d8..2af2525f8d 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -30,25 +30,8 @@ macro(targets) #"rcl_lifecycle${target_suffix}" This doesn't compile? "lifecycle_msgs") - ### demos - add_executable(lifecycle_talker${target_suffix} - src/demos/lifecycle_talker.cpp) - target_link_libraries(lifecycle_talker${target_suffix} - rclcpp_lifecycle${target_suffix}) - add_executable(lifecycle_listener${target_suffix} - src/demos/lifecycle_listener.cpp) - target_link_libraries(lifecycle_listener${target_suffix} - rclcpp_lifecycle${target_suffix}) - add_executable(lifecycle_service_client${target_suffix} - src/demos/lifecycle_service_client.cpp) - target_link_libraries(lifecycle_service_client${target_suffix} - rclcpp_lifecycle${target_suffix}) - install(TARGETS rclcpp_lifecycle${target_suffix} - lifecycle_talker${target_suffix} - lifecycle_listener${target_suffix} - lifecycle_service_client${target_suffix} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) @@ -61,4 +44,11 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +ament_export_dependencies(rclcpp) +ament_export_dependencies(lifecycle_msgs) +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) ament_package() + +install(DIRECTORY include/ + DESTINATION include) diff --git a/rclcpp_lifecycle/src/demos/lifecycle_listener.cpp b/rclcpp_lifecycle/src/demos/lifecycle_listener.cpp deleted file mode 100644 index 96fab8016d..0000000000 --- a/rclcpp_lifecycle/src/demos/lifecycle_listener.cpp +++ /dev/null @@ -1,62 +0,0 @@ -// 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 - -static constexpr auto chatter_topic = "lifecycle_chatter"; -static constexpr auto notification_topic = "lc_talker__transition_notify"; - -class LifecycleListener : public rclcpp::node::Node -{ -public: - explicit LifecycleListener(const std::string & node_name) - : rclcpp::node::Node(node_name) - { - sub_data_ = this->create_subscription(chatter_topic, - std::bind(&LifecycleListener::data_callback, this, std::placeholders::_1)); - - sub_notification_ = this->create_subscription( - notification_topic, std::bind(&LifecycleListener::notification_callback, this, - std::placeholders::_1)); - } - - void data_callback(const std_msgs::msg::String::SharedPtr msg) - { - printf("[%s] data_callback: %s\n", get_name().c_str(), msg->data.c_str()); - } - - void notification_callback(const lifecycle_msgs::msg::Transition::SharedPtr msg) - { - printf("[%s] notify callback: Transition from state %d to %d\n", - get_name().c_str(), static_cast(msg->start_state), static_cast(msg->goal_state)); - } - -private: - std::shared_ptr> sub_data_; - std::shared_ptr> - sub_notification_; -}; - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - - auto lc_listener = std::make_shared("lc_listener"); - rclcpp::spin(lc_listener); - - return 0; -} diff --git a/rclcpp_lifecycle/src/demos/lifecycle_service_client.cpp b/rclcpp_lifecycle/src/demos/lifecycle_service_client.cpp deleted file mode 100644 index ddcc5423db..0000000000 --- a/rclcpp_lifecycle/src/demos/lifecycle_service_client.cpp +++ /dev/null @@ -1,230 +0,0 @@ -// 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 - -// service topics for general lifecycle manager -static constexpr auto manager_get_state_topic = "lifecycle_manager__get_state"; -static constexpr auto manager_change_state_topic = "lifecycle_manager__change_state"; - -// service topics for node-attached services -static const std::string lifecycle_node = "lc_talker"; -static const std::string node_get_state_topic = lifecycle_node+"__get_state"; -static const std::string node_change_state_topic = lifecycle_node+"__change_state"; - -class LifecycleServiceClient : public rclcpp::node::Node -{ -public: - explicit LifecycleServiceClient(const std::string & node_name) - : rclcpp::node::Node(node_name) - {} - - void - init() - { - // Service clients pointing to a global lifecycle manager - client_get_state_ = this->create_client( - manager_get_state_topic); - client_change_state_ = this->create_client( - manager_change_state_topic); - - // Service client pointing to each individual service - client_get_single_state_ = this->create_client( - node_get_state_topic); - client_change_single_state_ = this->create_client( - node_change_state_topic); - } - - unsigned int - get_state(const std::string & node_name, std::chrono::seconds time_out = 3_s) - { - auto request = std::make_shared(); - request->node_name = node_name; - - if (!client_get_state_->wait_for_service(time_out)) { - fprintf(stderr, "Service %s is not available.\n", - client_get_state_->get_service_name().c_str()); - return static_cast(rclcpp::lifecycle::LifecyclePrimaryStatesT::UNKNOWN); - } - - auto result = client_get_state_->async_send_request(request); - // Kind of a hack for making a async request a synchronous one. - while (result.wait_for(std::chrono::milliseconds(500)) != std::future_status::ready) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - - if(result.get()) - { - printf("[%s] Node %s has current state %d.\n", - get_name().c_str(), node_name.c_str(), result.get()->current_state); - return result.get()->current_state; - } else { - fprintf(stderr, "[%s] Failed to get current state for node %s\n", - get_name().c_str(), node_name.c_str()); - return static_cast(rclcpp::lifecycle::LifecyclePrimaryStatesT::UNKNOWN); - } - } - - unsigned int - get_single_state(std::chrono::seconds time_out = 3_s) - { - auto request = std::make_shared(); - - if (!client_get_single_state_->wait_for_service(time_out)) { - fprintf(stderr, "Service %s is not available.\n", - client_get_single_state_->get_service_name().c_str()); - return static_cast(rclcpp::lifecycle::LifecyclePrimaryStatesT::UNKNOWN); - } - - auto result = client_get_single_state_->async_send_request(request); - // Kind of a hack for making a async request a synchronous one. - while (result.wait_for(std::chrono::milliseconds(500)) != std::future_status::ready) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - - if(result.get()) - { - printf("[%s] Node %s has current state %d\n", - get_name().c_str(), lifecycle_node.c_str(), result.get()->current_state); - return result.get()->current_state; - } else { - fprintf(stderr, "[%s] Failed to get current state for node %s\n", - get_name().c_str(), lifecycle_node.c_str()); - return static_cast(rclcpp::lifecycle::LifecyclePrimaryStatesT::UNKNOWN); - } - } - - bool - change_state(const std::string & node_name, rclcpp::lifecycle::LifecycleTransitionsT transition, - std::chrono::seconds time_out = 3_s) - { - auto request = std::make_shared(); - request->node_name = node_name; - request->transition = static_cast(transition); - - if (!client_change_state_->wait_for_service(time_out)) { - fprintf(stderr, "Service %s is not available.\n", - client_change_state_->get_service_name().c_str()); - return false; - } - - auto result = client_change_state_->async_send_request(request); - while (result.wait_for(std::chrono::milliseconds(500)) != std::future_status::ready) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - - if(result.get()->success) - { - printf("[%s] Transition %d successfully triggered.\n", - get_name().c_str(), static_cast(transition)); - return true; - } else { - fprintf(stderr, "[%s] Failed to trigger transition %d\n", - get_name().c_str(), static_cast(transition)); - return false; - } - } - - bool - change_single_state(rclcpp::lifecycle::LifecycleTransitionsT transition, - std::chrono::seconds time_out = 3_s) - { - auto request = std::make_shared(); - request->node_name = lifecycle_node; - request->transition = static_cast(transition); - - if (!client_change_single_state_->wait_for_service(time_out)) { - fprintf(stderr, "Service %s is not available.\n", - client_change_single_state_->get_service_name().c_str()); - return false; - } - - auto result = client_change_single_state_->async_send_request(request); - while (result.wait_for(std::chrono::milliseconds(500)) != std::future_status::ready) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - - if(result.get()->success) - { - printf("[%s] Transition %d successfully triggered.\n", - get_name().c_str(), static_cast(transition)); - return true; - } else { - fprintf(stderr, "[%s] Failed to trigger transition %d\n", - get_name().c_str(), static_cast(transition)); - return false; - } - } - -private: - std::shared_ptr> client_get_state_; - std::shared_ptr> client_change_state_; - std::shared_ptr> client_get_single_state_; - std::shared_ptr> client_change_single_state_; -}; - -void -callee_script(std::shared_ptr lc_client, - const std::string & node_name) -{ - auto sleep_time = 10_s; - - { // configure - lc_client->change_single_state(rclcpp::lifecycle::LifecycleTransitionsT::CONFIGURING); - lc_client->get_single_state(); - } - { // activate single node - std::this_thread::sleep_for(sleep_time); - lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::ACTIVATING); - lc_client->get_state(node_name); - } - { // deactivate single node - std::this_thread::sleep_for(sleep_time); - lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::DEACTIVATING); - lc_client->get_state(node_name); - } - { // activate lifecycle_manager - std::this_thread::sleep_for(sleep_time); - lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::ACTIVATING); - lc_client->get_state(node_name); - } - { // deactivate lifecycle_manager - std::this_thread::sleep_for(sleep_time); - lc_client->change_state(node_name, rclcpp::lifecycle::LifecycleTransitionsT::DEACTIVATING); - lc_client->get_state(node_name); - } -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - - auto lc_client = std::make_shared("lc_client"); - lc_client->init(); - - rclcpp::executors::SingleThreadedExecutor exe; - exe.add_node(lc_client); - - std::shared_future script = std::async(std::launch::async, - std::bind(callee_script, lc_client, lifecycle_node)); - exe.spin_until_future_complete(script); - - return 0; -} diff --git a/rclcpp_lifecycle/src/demos/lifecycle_talker.cpp b/rclcpp_lifecycle/src/demos/lifecycle_talker.cpp deleted file mode 100644 index 88f5a4a263..0000000000 --- a/rclcpp_lifecycle/src/demos/lifecycle_talker.cpp +++ /dev/null @@ -1,95 +0,0 @@ -// 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 "rclcpp/rclcpp.hpp" -#include "rclcpp/publisher.hpp" - -#include "rclcpp_lifecycle/lifecycle_manager.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" - -#include "std_msgs/msg/string.hpp" - -static constexpr auto chatter_topic = "lifecycle_chatter"; - -class LifecycleTalker : public rclcpp::node::lifecycle::LifecycleNode -{ -public: - explicit LifecycleTalker(const std::string & node_name, bool intra_process_comms = false) - : rclcpp::node::lifecycle::LifecycleNode(node_name, intra_process_comms) - { - msg_ = std::make_shared(); - - pub_ = this->create_publisher(chatter_topic); - timer_ = this->get_communication_interface()->create_wall_timer( - 1_s, std::bind(&LifecycleTalker::publish, this)); - } - - void publish() - { - static size_t count = 0; - msg_->data = "Lifecycle HelloWorld #" + std::to_string(++count); - pub_->publish(msg_); - } - - bool on_configure() - { - printf("[%s] on_configure() is called.\n", get_name().c_str()); - return true; - } - - bool on_activate() - { - rclcpp::node::lifecycle::LifecycleNode::enable_communication(); - printf("[%s] on_activate() is called.\n", get_name().c_str()); - return true; - } - - bool on_deactivate() - { - rclcpp::node::lifecycle::LifecycleNode::disable_communication(); - printf("[%s] on_deactivate() is called.\n", get_name().c_str()); - return true; - } - -private: - std::shared_ptr msg_; - std::shared_ptr> pub_; - rclcpp::TimerBase::SharedPtr timer_; -}; - -int main(int argc, char * argv[]) -{ - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - rclcpp::init(argc, argv); - - rclcpp::executors::SingleThreadedExecutor exe; - - std::shared_ptr lc_node = - std::make_shared("lc_talker"); - - rclcpp::lifecycle::LifecycleManager lm; - lm.add_node_interface(lc_node); - - exe.add_node(lc_node->get_communication_interface()); - exe.add_node(lm.get_node_base_interface()); - - exe.spin(); - - return 0; -} From 29271087c2435a61b0a88855f50f0d67d4becbe6 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 28 Nov 2016 21:48:36 -0800 Subject: [PATCH 30/63] (fix) address review comments (fix) address review comments --- rclcpp/include/rclcpp/service.hpp | 25 ++++++++++++------- rclcpp/src/rclcpp/service.cpp | 8 +++++- .../test/test_externally_defined_services.cpp | 1 + 3 files changed, 24 insertions(+), 10 deletions(-) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 9d0d4c0bee..f8f4bb16f6 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -42,7 +42,13 @@ class ServiceBase RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase) RCLCPP_PUBLIC - explicit ServiceBase(rcl_node_t * node_handle); + ServiceBase( + rcl_node_t * node_handle, + const std::string & service_name); + + RCLCPP_PUBLIC + explicit ServiceBase( + rcl_node_t * node_handle); RCLCPP_PUBLIC virtual ~ServiceBase(); @@ -67,8 +73,8 @@ class ServiceBase rcl_node_t * node_handle_; rcl_service_t * service_handle_ = nullptr; - - bool defined_extern_ = false; + std::string service_name_; + bool owns_rcl_handle_ = true; }; using any_service_callback::AnyServiceCallback; @@ -94,7 +100,7 @@ class Service : public ServiceBase const std::string & service_name, AnyServiceCallback any_callback, rcl_service_options_t & service_options) - : ServiceBase(node_handle.get()), any_callback_(any_callback) + : ServiceBase(node_handle.get(), service_name), any_callback_(any_callback) { using rosidl_typesupport_cpp::get_service_type_support_handle; auto service_type_support_handle = get_service_type_support_handle(); @@ -120,14 +126,15 @@ class Service : public ServiceBase any_callback_(any_callback) { // check if service handle was initialized - // TODO(Karsten1987): Can this go directly in RCL? + // see: https://github.com/ros2/rcl/issues/81 if (service_handle->impl == NULL) { throw std::runtime_error( - std::string("rcl_service_t in constructor argument ") + - "has to be initialized beforehand"); + std::string("rcl_service_t in constructor argument ") + + "has to be initialized beforehand"); } service_handle_ = service_handle; - defined_extern_ = true; + service_name_ = std::string(rcl_service_get_service_name(service_handle)); + owns_rcl_handle_ = false; } Service() = delete; @@ -135,7 +142,7 @@ class Service : public ServiceBase virtual ~Service() { // check if you have ownership of the handle - if (!defined_extern_) { + if (owns_rcl_handle_) { if (rcl_service_fini(service_handle_, node_handle_) != RCL_RET_OK) { std::stringstream ss; ss << "Error in destruction of rcl service_handle_ handle: " << diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index a3978a19e0..8fa5e1722f 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -27,6 +27,12 @@ using rclcpp::service::ServiceBase; +ServiceBase::ServiceBase( + rcl_node_t * node_handle, + const std::string & service_name) +: node_handle_(node_handle), service_name_(service_name) +{} + ServiceBase::ServiceBase(rcl_node_t * node_handle) : node_handle_(node_handle) {} @@ -45,7 +51,7 @@ ServiceBase::~ServiceBase() std::string ServiceBase::get_service_name() { - return rcl_service_get_service_name(service_handle_); + return this->service_name_; } const rcl_service_t * diff --git a/rclcpp/test/test_externally_defined_services.cpp b/rclcpp/test/test_externally_defined_services.cpp index 6295f96323..e5ea4078ae 100644 --- a/rclcpp/test/test_externally_defined_services.cpp +++ b/rclcpp/test/test_externally_defined_services.cpp @@ -94,6 +94,7 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) { FAIL(); return; } + rclcpp::any_service_callback::AnyServiceCallback cb; try { From 26ade73281e9512153bfa43790b2853fc4bc1629 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 29 Nov 2016 15:45:49 -0800 Subject: [PATCH 31/63] (fix) make find_package(rmw) required --- rclcpp/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index c4869ded61..30bad0f764 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -143,7 +143,7 @@ if(BUILD_TESTING) endif() get_default_rmw_implementation(default_rmw) - find_package(${default_rmw}) + find_package(${default_rmw} REQUIRED) get_rmw_typesupport(typesupport_impls_cpp "${default_rmw}" LANGUAGE "cpp") get_rmw_typesupport(typesupport_impls_c "${default_rmw}" LANGUAGE "c") set(mock_msg_files From 3a140f7685e9e6ec4cef048f664c331315dbcccb Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 30 Nov 2016 18:53:18 -0800 Subject: [PATCH 32/63] (refactor) attach sm to lifecycle node and disable lc_manager --- rclcpp_lifecycle/CMakeLists.txt | 3 +- .../rclcpp_lifecycle/lifecycle_node.hpp | 124 ++++++++---------- .../rclcpp_lifecycle/lifecycle_publisher.hpp | 16 +-- .../src/lifecycle_manager_impl.hpp | 26 ++-- 4 files changed, 81 insertions(+), 88 deletions(-) diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 2af2525f8d..6b6100f04b 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -22,7 +22,7 @@ macro(targets) ### CPP High level library add_library(rclcpp_lifecycle${target_suffix} SHARED - src/lifecycle_manager.cpp) + src/lifecycle_node.cpp) target_link_libraries(rclcpp_lifecycle${target_suffix} ${rcl_lifecycle_LIBRARIES}) ament_target_dependencies(rclcpp_lifecycle${target_suffix} @@ -45,6 +45,7 @@ if(BUILD_TESTING) endif() ament_export_dependencies(rclcpp) +ament_export_dependencies(rcl_lifecycle) ament_export_dependencies(lifecycle_msgs) ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index da31a3857f..a042f0f4f7 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -20,73 +20,63 @@ #include #include "rclcpp/node.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/visibility_control.h" namespace rclcpp { -namespace node -{ - namespace lifecycle { -template -struct Callback; - -template -struct Callback -{ - template - static Ret callback(Args ... args) {return func(args ...); } - static std::function func; -}; - -// Initialize the static member. -template -std::function Callback::func; - /** * @brief Interface class for a managed node. * Pure virtual functions as defined in * http://design.ros2.org/articles/node_lifecycle.html */ +// *INDENT-OFF class LifecycleNodeInterface { public: - virtual bool on_configure() {return true; } - virtual bool on_cleanup() {return true; } - virtual bool on_shutdown() {return true; } - virtual bool on_activate() {return true; } - virtual bool on_deactivate() {return true; } - virtual bool on_error() {return true; } + virtual bool on_configure() = 0; + virtual bool on_cleanup() = 0; + virtual bool on_shutdown() = 0; + virtual bool on_activate() = 0; + virtual bool on_deactivate() = 0; + virtual bool on_error() = 0; }; +class AbstractLifecycleNode : public LifecycleNodeInterface +{ +public: + virtual bool on_configure() {return true;}; + virtual bool on_cleanup() {return true;}; + virtual bool on_shutdown() {return true;}; + virtual bool on_activate() {return true;}; + virtual bool on_deactivate() {return true;}; + virtual bool on_error() {return true;}; +}; +// *INDENT-ON + /** * @brief LifecycleNode as child class of rclcpp Node * has lifecycle nodeinterface for configuring this node. */ -class LifecycleNode : public lifecycle::LifecycleNodeInterface +class LifecycleNode : public AbstractLifecycleNode { public: - using LifecyclePublisherWeakPtr = - std::weak_ptr; - LIFECYCLE_EXPORT - explicit LifecycleNode(const std::string & node_name, bool use_intra_process_comms = false) - : base_interface_(std::make_shared(node_name, use_intra_process_comms)), - communication_interface_(base_interface_) // MOCK as base/comms interface not done yet - {} + explicit LifecycleNode(const std::string & node_name, bool use_intra_process_comms = false); LIFECYCLE_EXPORT - ~LifecycleNode() {} + virtual ~LifecycleNode(); // MOCK typedefs as node refactor not done yet using BaseInterface = rclcpp::node::Node; std::shared_ptr get_base_interface() { - return base_interface_; + return base_node_handle_; } // MOCK typedefs as node refactor not done yet @@ -100,14 +90,27 @@ class LifecycleNode : public lifecycle::LifecycleNodeInterface std::string get_name() { - return base_interface_->get_name(); + return base_node_handle_->get_name(); } + bool + register_on_configure(std::function fcn); + bool + register_on_cleanup(std::function fcn); + bool + register_on_shutdown(std::function fcn); + bool + register_on_activate(std::function fcn); + bool + register_on_deactivate(std::function fcn); + bool + register_on_error(std::function fcn); + /** * @brief same API for creating publisher as regular Node */ template> - std::shared_ptr> + std::shared_ptr> create_publisher( const std::string & topic_name, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, @@ -115,14 +118,14 @@ class LifecycleNode : public lifecycle::LifecycleNodeInterface { // create regular publisher in rclcpp::Node auto pub = communication_interface_->create_publisher>( + rclcpp::lifecycle::LifecyclePublisher>( topic_name, qos_profile, allocator); + add_publisher_handle(pub); - // keep weak handle for this publisher to enable/disable afterwards - weak_pubs_.push_back(pub); return pub; } + template typename rclcpp::timer::WallTimer::SharedPtr create_wall_timer( @@ -130,46 +133,29 @@ class LifecycleNode : public lifecycle::LifecycleNodeInterface CallbackType callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) { - return communication_interface_->create_wall_timer(period, callback, group); + auto timer = communication_interface_->create_wall_timer(period, callback, group); + add_timer_handle(timer); + + return timer; } +protected: LIFECYCLE_EXPORT - virtual bool - disable_communication() - { - for (auto weak_pub : weak_pubs_) { - auto pub = weak_pub.lock(); - if (!pub) { - return false; - } - pub->on_deactivate(); - } - return true; - } + void + add_publisher_handle(std::shared_ptr pub); LIFECYCLE_EXPORT - virtual bool - enable_communication() - { - for (auto weak_pub : weak_pubs_) { - auto pub = weak_pub.lock(); - if (!pub) { - return false; - } - pub->on_activate(); - } - return true; - } + void + add_timer_handle(std::shared_ptr timer); private: - std::shared_ptr base_interface_; + std::shared_ptr base_node_handle_; std::shared_ptr communication_interface_; - // Placeholder for all pub/sub/srv/clients - std::vector weak_pubs_; + class LifecycleNodeImpl; + std::unique_ptr impl_; }; } // namespace lifecycle -} // namespace node } // namespace rclcpp #endif // RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index af7ba712be..550fb2406a 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -22,10 +22,7 @@ namespace rclcpp { -namespace publisher -{ - -namespace lifecycle_interface +namespace lifecycle { /** * @brief base class with only @@ -35,13 +32,12 @@ namespace lifecycle_interface * It is more a convenient interface class * than a necessary base class. */ -class PublisherInterface +class LifecyclePublisherInterface { public: virtual void on_activate() = 0; virtual void on_deactivate() = 0; }; -} // namespace lifecycle_interface /** * @brief child class of rclcpp Publisher class. @@ -49,8 +45,8 @@ class PublisherInterface * enabled/disabled state. */ template> -class LifecyclePublisher : public rclcpp::publisher::Publisher, - public lifecycle_interface::PublisherInterface +class LifecyclePublisher : public LifecyclePublisherInterface, + public rclcpp::publisher::Publisher { public: using MessageAllocTraits = allocator::AllocRebind; @@ -63,7 +59,7 @@ class LifecyclePublisher : public rclcpp::publisher::Publisher, std::string topic, const rcl_publisher_options_t & publisher_options, std::shared_ptr allocator) - : ::rclcpp::publisher::Publisher( + : rclcpp::publisher::Publisher( node_handle, topic, publisher_options, allocator), enabled_(false) {} @@ -164,7 +160,7 @@ class LifecyclePublisher : public rclcpp::publisher::Publisher, bool enabled_ = false; }; -} // namespace publisher +} // namespace lifecycle } // namespace rclcpp #endif // RCLCPP_LIFECYCLE__LIFECYCLE_PUBLISHER_HPP_ diff --git a/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp b/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp index ff2a55ffc3..08f218c0ee 100644 --- a/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp @@ -21,7 +21,10 @@ #include #include +#include + #include +#include #include #include @@ -63,7 +66,7 @@ class LifecycleManager::LifecycleManagerImpl fprintf(stderr, "%s:%u, FATAL: rcl_state_machine got destroyed externally.\n", __FILE__, __LINE__); } else { - rcl_state_machine_fini(rcl_state_machine); + rcl_state_machine_fini(rcl_state_machine, node_base_handle_->get_rcl_node_handle()); } } } @@ -136,9 +139,18 @@ class LifecycleManager::LifecycleManagerImpl { // TODO(karsten1987): clarify what do if node already exists; NodeStateMachine& node_state_machine = node_handle_map_[node_name]; - node_state_machine.state_machine = rcl_get_zero_initialized_state_machine( - node_base_handle_->get_rcl_node_handle()); - rcl_state_machine_init(&node_state_machine.state_machine, node_name.c_str(), true); + node_state_machine.state_machine = rcl_get_zero_initialized_state_machine(); + rcl_ret_t ret = rcl_state_machine_init(&node_state_machine.state_machine, node_base_handle_->get_rcl_node_handle(), + rosidl_generator_cpp::get_message_type_support_handle(), + rosidl_generator_cpp::get_service_type_support_handle(), + rosidl_generator_cpp::get_service_type_support_handle(), + true); + if (ret != RCL_RET_OK) + { + fprintf(stderr, "Error adding %s: %s\n", + node_name.c_str(), rcl_get_error_string_safe()); + return; + } // srv objects may get destroyed directly here { // get_state @@ -153,8 +165,7 @@ class LifecycleManager::LifecycleManagerImpl any_cb.set(std::move(cb)); auto srv_get_state = rclcpp::service::Service::make_shared( - //node_state_machine.state_machine.comm_interface.node_handle, - node_base_handle_->get_rcl_node_handle(), + node_base_handle_->get_shared_node_handle(), &node_state_machine.state_machine.comm_interface.srv_get_state, any_cb); auto srv_get_state_base = std::dynamic_pointer_cast(srv_get_state); @@ -174,8 +185,7 @@ class LifecycleManager::LifecycleManagerImpl any_cb.set(cb); auto srv_change_state = std::make_shared>( - //node_state_machine.state_machine.comm_interface.node_handle, - node_base_handle_->get_rcl_node_handle(), + node_base_handle_->get_shared_node_handle(), &node_state_machine.state_machine.comm_interface.srv_change_state, any_cb); auto srv_change_state_base = std::dynamic_pointer_cast(srv_change_state); From 4e493c65b19604199706a7c2222e8719e10f0d62 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 1 Dec 2016 16:34:35 -0800 Subject: [PATCH 33/63] (fix) adjust code to rcl_test refactor --- rclcpp_lifecycle/src/lifecycle_node_impl.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp index 43d09ae98c..203a1f6fab 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp @@ -85,7 +85,7 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; any_cb.set(std::move(cb)); srv_get_state_ = std::make_shared>( - base_node_handle_->get_shared_node_handle(), &state_machine_.comm_interface.srv_get_state, + base_node_handle_->get_shared_node_handle(), &state_machine_.com_interface.srv_get_state, any_cb); base_node_handle_->add_service( std::dynamic_pointer_cast(srv_get_state_)); @@ -99,7 +99,7 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; srv_change_state_ = std::make_shared>( base_node_handle_->get_shared_node_handle(), - &state_machine_.comm_interface.srv_change_state, + &state_machine_.com_interface.srv_change_state, any_cb); base_node_handle_->add_service( std::dynamic_pointer_cast(srv_change_state_)); @@ -151,7 +151,7 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; } unsigned int transition_index = static_cast(lifecycle_transition); - if (!rcl_start_transition_by_index(&state_machine_, transition_index)) { + if (!rcl_start_transition_by_index(&state_machine_, transition_index, true)) { fprintf(stderr, "%s:%d, Unable to start transition %u from current state %s\n", __FILE__, __LINE__, transition_index, state_machine_.current_state->label); return false; @@ -170,7 +170,7 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; std::function callback = it->second; auto success = callback(); - if (!rcl_finish_transition_by_index(&state_machine_, transition_index, success)) + if (!rcl_finish_transition_by_index(&state_machine_, transition_index, success, true)) { fprintf(stderr, "Failed to finish transition %u. Current state is now: %s\n", transition_index, state_machine_.current_state->label); From 5d453ec4b7821ec9e3de81340810695f3dc880be Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 6 Dec 2016 16:07:25 -0800 Subject: [PATCH 34/63] (dev) remove unused deps --- rclcpp_lifecycle/CMakeLists.txt | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 6b6100f04b..6d0b3b1899 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -23,11 +23,9 @@ macro(targets) add_library(rclcpp_lifecycle${target_suffix} SHARED src/lifecycle_node.cpp) - target_link_libraries(rclcpp_lifecycle${target_suffix} - ${rcl_lifecycle_LIBRARIES}) ament_target_dependencies(rclcpp_lifecycle${target_suffix} "rclcpp${target_suffix}" - #"rcl_lifecycle${target_suffix}" This doesn't compile? + "rcl_lifecycle" #This doesn't compile? "lifecycle_msgs") install(TARGETS From c090df414d241e356db2ab1693af90f0ca4c9266 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 7 Dec 2016 00:44:33 -0800 Subject: [PATCH 35/63] (rebase) merge commit --- rclcpp/include/rclcpp/service.hpp | 11 +++++++---- rclcpp/src/rclcpp/service.cpp | 8 -------- 2 files changed, 7 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index f8f4bb16f6..1b87c4dd56 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -125,12 +125,15 @@ class Service : public ServiceBase : ServiceBase(node_handle), any_callback_(any_callback) { - // check if service handle was initialized + //check if service handle was initialized + // TODO(karsten1987): Take this verification + // directly in rcl_*_t // see: https://github.com/ros2/rcl/issues/81 - if (service_handle->impl == NULL) { + if (!service_handle->impl) { + // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) throw std::runtime_error( - std::string("rcl_service_t in constructor argument ") + - "has to be initialized beforehand"); + std::string("rcl_service_t in constructor argument must be initialized beforehand.")); + // *INDENT-ON* } service_handle_ = service_handle; service_name_ = std::string(rcl_service_get_service_name(service_handle)); diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 8fa5e1722f..29822df7a7 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -37,14 +37,6 @@ ServiceBase::ServiceBase(rcl_node_t * node_handle) : node_handle_(node_handle) {} -ServiceBase::ServiceBase(std::shared_ptr node_handle) -: node_handle_(node_handle) -{} - -ServiceBase::ServiceBase(std::shared_ptr node_handle) -: node_handle_(node_handle) -{} - ServiceBase::~ServiceBase() {} From 1361abb6803a4e8176c2a06cf7c8d1460973bc8d Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 7 Dec 2016 00:46:22 -0800 Subject: [PATCH 36/63] (bugfix) correct rcl_ret_t error handling --- rclcpp_lifecycle/src/lifecycle_node_impl.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp index 203a1f6fab..e677783cea 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp @@ -150,10 +150,10 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; return false; } - unsigned int transition_index = static_cast(lifecycle_transition); - if (!rcl_start_transition_by_index(&state_machine_, transition_index, true)) { - fprintf(stderr, "%s:%d, Unable to start transition %u from current state %s\n", - __FILE__, __LINE__, transition_index, state_machine_.current_state->label); + unsigned int transition_id = static_cast(lifecycle_transition); + if (rcl_lifecycle_start_transition(&state_machine_, transition_id, true) != RCL_RET_OK) { + fprintf(stderr, "%s:%d, Unable to start transition %u from current state %s: %s\n", + __FILE__, __LINE__, transition_id, state_machine_.current_state->label, rcl_get_error_string_safe()); return false; } From 4fe6d5774014dab05005160482a100c87f049f68 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 7 Dec 2016 00:46:58 -0800 Subject: [PATCH 37/63] (fix) depedencies --- rclcpp_lifecycle/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 6d0b3b1899..6b6100f04b 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -23,9 +23,11 @@ macro(targets) add_library(rclcpp_lifecycle${target_suffix} SHARED src/lifecycle_node.cpp) + target_link_libraries(rclcpp_lifecycle${target_suffix} + ${rcl_lifecycle_LIBRARIES}) ament_target_dependencies(rclcpp_lifecycle${target_suffix} "rclcpp${target_suffix}" - "rcl_lifecycle" #This doesn't compile? + #"rcl_lifecycle${target_suffix}" This doesn't compile? "lifecycle_msgs") install(TARGETS From 9001160d7444c1aa9613fe22c05153862b20b22a Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 7 Dec 2016 00:47:11 -0800 Subject: [PATCH 38/63] (refactor) change to lifecycle_msgs --- rclcpp_lifecycle/src/lifecycle_node.cpp | 15 ++++---- rclcpp_lifecycle/src/lifecycle_node_impl.hpp | 38 ++++++++++---------- 2 files changed, 28 insertions(+), 25 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 61a40b73f8..b7549dda25 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -15,7 +15,8 @@ #include #include -#include +#include +#include #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -47,37 +48,37 @@ LifecycleNode::~LifecycleNode() bool LifecycleNode::register_on_configure(std::function fcn) { - return impl_->register_callback(rcl_state_configuring.index, fcn); + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn); } bool LifecycleNode::register_on_cleanup(std::function fcn) { - return impl_->register_callback(rcl_state_cleaningup.index, fcn); + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn); } bool LifecycleNode::register_on_shutdown(std::function fcn) { - return impl_->register_callback(rcl_state_shuttingdown.index, fcn); + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn); } bool LifecycleNode::register_on_activate(std::function fcn) { - return impl_->register_callback(rcl_state_activating.index, fcn); + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn); } bool LifecycleNode::register_on_deactivate(std::function fcn) { - return impl_->register_callback(rcl_state_deactivating.index, fcn); + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn); } bool LifecycleNode::register_on_error(std::function fcn) { - return impl_->register_callback(rcl_state_errorprocessing.index, fcn); + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING, fcn); } void diff --git a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp index e677783cea..fe24da0a0d 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp @@ -25,7 +25,7 @@ #include -#include +#include #include #include @@ -41,7 +41,7 @@ namespace lifecycle class LifecycleNode::LifecycleNodeImpl { -using TransitionMsg = lifecycle_msgs::msg::Transition; +using TransitionEventMsg = lifecycle_msgs::msg::TransitionEvent; using GetStateSrv = lifecycle_msgs::srv::GetState; using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; @@ -52,21 +52,21 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; ~LifecycleNodeImpl() { - if (rcl_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { fprintf(stderr, "%s:%u, FATAL: rcl_state_machine got destroyed externally.\n", __FILE__, __LINE__); } else { - rcl_state_machine_fini(&state_machine_, base_node_handle_->get_rcl_node_handle()); + rcl_lifecycle_state_machine_fini(&state_machine_, base_node_handle_->get_rcl_node_handle()); } } void init() { - state_machine_ = rcl_get_zero_initialized_state_machine(); - rcl_ret_t ret = rcl_state_machine_init(&state_machine_, base_node_handle_->get_rcl_node_handle(), - rosidl_generator_cpp::get_message_type_support_handle(), + state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); + rcl_ret_t ret = rcl_lifecycle_state_machine_init(&state_machine_, base_node_handle_->get_rcl_node_handle(), + rosidl_generator_cpp::get_message_type_support_handle(), rosidl_generator_cpp::get_service_type_support_handle(), rosidl_generator_cpp::get_service_type_support_handle(), true); @@ -76,6 +76,7 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; base_node_handle_->get_name().c_str(), rcl_get_error_string_safe()); return; } + rcl_print_state_machine(&state_machine_); // srv objects may get destroyed directly here { // get_state @@ -113,12 +114,12 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; { (void)header; (void)req; - if (rcl_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - resp->current_state = static_cast(rcl_state_unknown.index); + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + resp->current_state = static_cast(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN); return; } resp->current_state = - static_cast(state_machine_.current_state->index); + static_cast(state_machine_.current_state->id); } void @@ -126,12 +127,13 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; const std::shared_ptr req, std::shared_ptr resp) { + fprintf(stderr, "Received change request %u\n", req->transition.id); (void)header; - if (rcl_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { resp->success = false; return; } - resp->success = change_state(req->transition); + resp->success = change_state(req->transition.id); } bool @@ -144,7 +146,7 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; bool change_state(std::uint8_t lifecycle_transition) { - if (rcl_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { fprintf(stderr, "%s:%d, Unable to change state for state machine for %s: %s \n", __FILE__, __LINE__, base_node_handle_->get_name().c_str(), rcl_get_error_string_safe()); return false; @@ -158,7 +160,7 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; } fprintf(stderr, "Started Transition %u. SM is now in state %u\n", - lifecycle_transition, state_machine_.current_state->index); + lifecycle_transition, state_machine_.current_state->id); // Since we set always set a default callback, // we don't have to check for nullptr here auto it = cb_map_.find(lifecycle_transition); @@ -170,14 +172,14 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; std::function callback = it->second; auto success = callback(); - if (!rcl_finish_transition_by_index(&state_machine_, transition_index, success, true)) + if (!rcl_lifecycle_finish_transition(&state_machine_, transition_id, success, false)) { fprintf(stderr, "Failed to finish transition %u. Current state is now: %s\n", - transition_index, state_machine_.current_state->label); + transition_id, state_machine_.current_state->label); return false; } fprintf(stderr, "Finished Transition %u. SM is now in state %u\n", - lifecycle_transition, state_machine_.current_state->index); + lifecycle_transition, state_machine_.current_state->id); // This true holds in both cases where the actual callback // was successful or not, since at this point we have a valid transistion // to either a new primary state or error state @@ -196,7 +198,7 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; weak_timers_.push_back(timer); } - rcl_state_machine_t state_machine_; + rcl_lifecycle_state_machine_t state_machine_; std::map> cb_map_; std::shared_ptr base_node_handle_; From 804c5dbaf2cae4828c9930faed3145dac8475b38 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 7 Dec 2016 18:27:47 -0800 Subject: [PATCH 39/63] (fix) correct find_rcl --- rclcpp_lifecycle/CMakeLists.txt | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 6b6100f04b..c63082773a 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -13,21 +13,19 @@ find_package(rmw_implementation_cmake REQUIRED) find_package(std_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) -include_directories( - include) +include_directories(include) macro(targets) get_rclcpp_information("${rmw_implementation}" "rclcpp${target_suffix}") + get_rcl_lifecycle_information("${rmw_implementation}" "rcl_lifecycle${target_suffix}") ### CPP High level library add_library(rclcpp_lifecycle${target_suffix} SHARED src/lifecycle_node.cpp) - target_link_libraries(rclcpp_lifecycle${target_suffix} - ${rcl_lifecycle_LIBRARIES}) ament_target_dependencies(rclcpp_lifecycle${target_suffix} "rclcpp${target_suffix}" - #"rcl_lifecycle${target_suffix}" This doesn't compile? + "rcl_lifecycle${target_suffix}" "lifecycle_msgs") install(TARGETS From fd8a32c69f5eff32a6db9f5f9522ca83edc4d215 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 7 Dec 2016 18:28:08 -0800 Subject: [PATCH 40/63] (refactor) comply for new state machine --- rclcpp_lifecycle/src/lifecycle_node_impl.hpp | 44 +++++++++----------- 1 file changed, 20 insertions(+), 24 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp index fe24da0a0d..8172b9fe73 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp @@ -26,6 +26,7 @@ #include #include +#include // for getting the c-typesupport #include #include @@ -46,7 +47,7 @@ using GetStateSrv = lifecycle_msgs::srv::GetState; using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; public: - LifecycleNodeImpl(std::shared_ptr base_node_handle) + LifecycleNodeImpl(std::shared_ptr(base_node_handle)) : base_node_handle_(base_node_handle) {} @@ -65,18 +66,18 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; init() { state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); - rcl_ret_t ret = rcl_lifecycle_state_machine_init(&state_machine_, base_node_handle_->get_rcl_node_handle(), - rosidl_generator_cpp::get_message_type_support_handle(), - rosidl_generator_cpp::get_service_type_support_handle(), - rosidl_generator_cpp::get_service_type_support_handle(), - true); + rcl_ret_t ret = rcl_lifecycle_state_machine_init( + &state_machine_, base_node_handle_->get_rcl_node_handle(), + ROSIDL_GET_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), + rosidl_generator_cpp::get_service_type_support_handle(), + rosidl_generator_cpp::get_service_type_support_handle(), + true); if (ret != RCL_RET_OK) { fprintf(stderr, "Error adding %s: %s\n", base_node_handle_->get_name().c_str(), rcl_get_error_string_safe()); return; } - rcl_print_state_machine(&state_machine_); // srv objects may get destroyed directly here { // get_state @@ -118,8 +119,7 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; resp->current_state = static_cast(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN); return; } - resp->current_state = - static_cast(state_machine_.current_state->id); + resp->current_state = static_cast(state_machine_.current_state->id); } void @@ -127,7 +127,6 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; const std::shared_ptr req, std::shared_ptr resp) { - fprintf(stderr, "Received change request %u\n", req->transition.id); (void)header; if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { resp->success = false; @@ -153,33 +152,30 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; } unsigned int transition_id = static_cast(lifecycle_transition); - if (rcl_lifecycle_start_transition(&state_machine_, transition_id, true) != RCL_RET_OK) { + if (rcl_lifecycle_start_transition(&state_machine_, transition_id, true, true) != RCL_RET_OK) { fprintf(stderr, "%s:%d, Unable to start transition %u from current state %s: %s\n", - __FILE__, __LINE__, transition_id, state_machine_.current_state->label, rcl_get_error_string_safe()); + __FILE__, __LINE__, transition_id, + state_machine_.current_state->label, rcl_get_error_string_safe()); return false; } - fprintf(stderr, "Started Transition %u. SM is now in state %u\n", - lifecycle_transition, state_machine_.current_state->id); - // Since we set always set a default callback, - // we don't have to check for nullptr here - auto it = cb_map_.find(lifecycle_transition); - if (it == cb_map_.end()) + auto cb_success = true; // in case no callback was attached, we forward directly + auto it = cb_map_.find(state_machine_.current_state->id); + if (it != cb_map_.end()) { + std::function callback = it->second; + cb_success = callback(); + } else { fprintf(stderr, "%s:%d, No callback is registered for transition %u.\n", - __FILE__, __LINE__, lifecycle_transition); + __FILE__, __LINE__, lifecycle_transition); } - std::function callback = it->second; - auto success = callback(); - if (!rcl_lifecycle_finish_transition(&state_machine_, transition_id, success, false)) + if (rcl_lifecycle_start_transition(&state_machine_, transition_id, cb_success, true) != RCL_RET_OK) { fprintf(stderr, "Failed to finish transition %u. Current state is now: %s\n", transition_id, state_machine_.current_state->label); return false; } - fprintf(stderr, "Finished Transition %u. SM is now in state %u\n", - lifecycle_transition, state_machine_.current_state->id); // This true holds in both cases where the actual callback // was successful or not, since at this point we have a valid transistion // to either a new primary state or error state From 3a78ab91073e07df2f1effbf09ae6098a2e89a60 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 8 Dec 2016 14:11:41 -0800 Subject: [PATCH 41/63] visibility control and test api --- rclcpp_lifecycle/CMakeLists.txt | 16 +- .../rclcpp_lifecycle/lifecycle_node.hpp | 86 +++++--- .../include/rclcpp_lifecycle/state.hpp | 60 ++++++ .../include/rclcpp_lifecycle/transition.hpp | 60 ++++++ .../rclcpp_lifecycle/visibility_control.h | 41 +++- rclcpp_lifecycle/package.xml | 1 + .../src/lifecycle_manager_impl.hpp | 28 +-- rclcpp_lifecycle/src/lifecycle_node.cpp | 26 ++- rclcpp_lifecycle/src/lifecycle_node_impl.hpp | 117 +++++++---- rclcpp_lifecycle/src/state.cpp | 70 +++++++ rclcpp_lifecycle/src/transition.cpp | 62 ++++++ rclcpp_lifecycle/test/test_lifecycle_node.cpp | 192 ++++++++++++++++++ 12 files changed, 667 insertions(+), 92 deletions(-) create mode 100644 rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp create mode 100644 rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp create mode 100644 rclcpp_lifecycle/src/state.cpp create mode 100644 rclcpp_lifecycle/src/transition.cpp create mode 100644 rclcpp_lifecycle/test/test_lifecycle_node.cpp diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index c63082773a..fbbb26e53b 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -22,7 +22,10 @@ macro(targets) ### CPP High level library add_library(rclcpp_lifecycle${target_suffix} SHARED - src/lifecycle_node.cpp) + src/lifecycle_node.cpp + src/state.cpp + src/transition.cpp + ) ament_target_dependencies(rclcpp_lifecycle${target_suffix} "rclcpp${target_suffix}" "rcl_lifecycle${target_suffix}" @@ -40,6 +43,17 @@ call_for_each_rmw_implementation(targets GENERATE_DEFAULT) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_lifecycle_node test/test_lifecycle_node.cpp) + if(TARGET test_lifecycle_node) + target_include_directories(test_lifecycle_node PUBLIC + ${rcl_lifecycle_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} + ${rclcpp_lifecycle_INCLUDE_DIRS} + ) + target_link_libraries(test_lifecycle_node ${PROJECT_NAME}) + endif() + endif() ament_export_dependencies(rclcpp) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index a042f0f4f7..13c8fe5e7b 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -23,6 +23,8 @@ #include "rclcpp/timer.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "rclcpp_lifecycle/transition.hpp" #include "rclcpp_lifecycle/visibility_control.h" namespace rclcpp @@ -38,23 +40,23 @@ namespace lifecycle class LifecycleNodeInterface { public: - virtual bool on_configure() = 0; - virtual bool on_cleanup() = 0; - virtual bool on_shutdown() = 0; - virtual bool on_activate() = 0; - virtual bool on_deactivate() = 0; - virtual bool on_error() = 0; + virtual bool on_configure() = 0; + virtual bool on_cleanup() = 0; + virtual bool on_shutdown() = 0; + virtual bool on_activate() = 0; + virtual bool on_deactivate() = 0; + virtual bool on_error() = 0; }; class AbstractLifecycleNode : public LifecycleNodeInterface { public: - virtual bool on_configure() {return true;}; - virtual bool on_cleanup() {return true;}; - virtual bool on_shutdown() {return true;}; - virtual bool on_activate() {return true;}; - virtual bool on_deactivate() {return true;}; - virtual bool on_error() {return true;}; + virtual bool on_configure() {return true;} + virtual bool on_cleanup() {return true;} + virtual bool on_shutdown() {return true;} + virtual bool on_activate() {return true;} + virtual bool on_deactivate() {return true;} + virtual bool on_error() {return true;} }; // *INDENT-ON @@ -65,14 +67,15 @@ class AbstractLifecycleNode : public LifecycleNodeInterface class LifecycleNode : public AbstractLifecycleNode { public: - LIFECYCLE_EXPORT + RCLCPP_LIFECYCLE_PUBLIC explicit LifecycleNode(const std::string & node_name, bool use_intra_process_comms = false); - LIFECYCLE_EXPORT + RCLCPP_LIFECYCLE_PUBLIC virtual ~LifecycleNode(); // MOCK typedefs as node refactor not done yet using BaseInterface = rclcpp::node::Node; + RCLCPP_LIFECYCLE_PUBLIC std::shared_ptr get_base_interface() { @@ -81,31 +84,20 @@ class LifecycleNode : public AbstractLifecycleNode // MOCK typedefs as node refactor not done yet using CommunicationInterface = rclcpp::node::Node; + RCLCPP_LIFECYCLE_PUBLIC std::shared_ptr get_communication_interface() { return communication_interface_; } + RCLCPP_LIFECYCLE_PUBLIC std::string get_name() { return base_node_handle_->get_name(); } - bool - register_on_configure(std::function fcn); - bool - register_on_cleanup(std::function fcn); - bool - register_on_shutdown(std::function fcn); - bool - register_on_activate(std::function fcn); - bool - register_on_deactivate(std::function fcn); - bool - register_on_error(std::function fcn); - /** * @brief same API for creating publisher as regular Node */ @@ -125,7 +117,6 @@ class LifecycleNode : public AbstractLifecycleNode return pub; } - template typename rclcpp::timer::WallTimer::SharedPtr create_wall_timer( @@ -139,12 +130,47 @@ class LifecycleNode : public AbstractLifecycleNode return timer; } + RCLCPP_LIFECYCLE_PUBLIC + const State & + get_current_state(); + + /// trigger the specified transition + /* + * return the new state after this transition + */ + RCLCPP_LIFECYCLE_PUBLIC + const State & + trigger_transition(const Transition & transition); + + RCLCPP_LIFECYCLE_PUBLIC + const State & + trigger_transition(unsigned int transition_id); + + RCLCPP_LIFECYCLE_PUBLIC + bool + register_on_configure(std::function fcn); + RCLCPP_LIFECYCLE_PUBLIC + bool + register_on_cleanup(std::function fcn); + RCLCPP_LIFECYCLE_PUBLIC + bool + register_on_shutdown(std::function fcn); + RCLCPP_LIFECYCLE_PUBLIC + bool + register_on_activate(std::function fcn); + RCLCPP_LIFECYCLE_PUBLIC + bool + register_on_deactivate(std::function fcn); + RCLCPP_LIFECYCLE_PUBLIC + bool + register_on_error(std::function fcn); + protected: - LIFECYCLE_EXPORT + RCLCPP_LIFECYCLE_PUBLIC void add_publisher_handle(std::shared_ptr pub); - LIFECYCLE_EXPORT + RCLCPP_LIFECYCLE_PUBLIC void add_timer_handle(std::shared_ptr timer); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp new file mode 100644 index 0000000000..adbd07db68 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -0,0 +1,60 @@ +// 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 RCLCPP_LIFECYCLE__STATE_HPP_ +#define RCLCPP_LIFECYCLE__STATE_HPP_ + +#include + +#include "rclcpp_lifecycle/visibility_control.h" + +// forward declare rcl_state_t +typedef struct rcl_lifecycle_state_t rcl_lifecycle_state_t; + +namespace rclcpp +{ +namespace lifecycle +{ + +class State +{ +public: + RCLCPP_LIFECYCLE_PUBLIC + State(); + + RCLCPP_LIFECYCLE_PUBLIC + State(unsigned int id, const std::string & label); + + RCLCPP_LIFECYCLE_PUBLIC + State(const rcl_lifecycle_state_t * rcl_lifecycle_state_handle); + + RCLCPP_LIFECYCLE_PUBLIC + virtual ~State(); + + RCLCPP_LIFECYCLE_PUBLIC + unsigned int + id() const; + + RCLCPP_LIFECYCLE_PUBLIC + std::string + label() const; + +protected: + bool owns_rcl_state_handle_; + const rcl_lifecycle_state_t * state_handle_; +}; + +} // namespace rclcpp_lifecycle +} // namespace rclcpp +#endif // RCLCPP_LIFECYCLE__STATE_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp new file mode 100644 index 0000000000..7c237f6b0a --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp @@ -0,0 +1,60 @@ +// 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 RCLCPP_LIFECYCLE__TRANSITION_HPP_ +#define RCLCPP_LIFECYCLE__TRANSITION_HPP_ + +#include + +#include "rclcpp_lifecycle/visibility_control.h" + +// forward declare rcl_transition_t +typedef struct rcl_lifecycle_transition_t rcl_lifecycle_transition_t; + +namespace rclcpp +{ +namespace lifecycle +{ + +class Transition +{ +public: + RCLCPP_LIFECYCLE_PUBLIC + Transition() = delete; + + RCLCPP_LIFECYCLE_PUBLIC + Transition(unsigned int id, const std::string & label = ""); + + RCLCPP_LIFECYCLE_PUBLIC + Transition(const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle); + + RCLCPP_LIFECYCLE_PUBLIC + virtual ~Transition(); + + RCLCPP_LIFECYCLE_PUBLIC + unsigned int + id() const; + + RCLCPP_LIFECYCLE_PUBLIC + std::string + label() const; + +protected: + bool owns_rcl_transition_handle_; + const rcl_lifecycle_transition_t * transition_handle_; +}; + +} // namespace rclcpp_lifecycle +} // namespace rclcpp +#endif // RCLCPP_LIFECYCLE__TRANSITION_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h b/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h index 69e43cd708..93468d04fa 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h @@ -12,18 +12,47 @@ // 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 RCLCPP_LIFECYCLE__VISIBILITY_CONTROL_H_ #define RCLCPP_LIFECYCLE__VISIBILITY_CONTROL_H_ -#ifdef _WIN32 - #define shared_EXPORTS 1 - #ifdef shared_EXPORTS - #define LIFECYCLE_EXPORT __declspec(dllexport) +#include "rmw/rmw.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 RCLCPP_LIFECYCLE_EXPORT __attribute__ ((dllexport)) + #define RCLCPP_LIFECYCLE_IMPORT __attribute__ ((dllimport)) #else - #define LIFECYCLE_EXPORT __declspec(dllimport) + #define RCLCPP_LIFECYCLE_EXPORT __declspec(dllexport) + #define RCLCPP_LIFECYCLE_IMPORT __declspec(dllimport) #endif + #ifdef RCLCPP_LIFECYCLE_BUILDING_DLL + #define RCLCPP_LIFECYCLE_PUBLIC RCLCPP_LIFECYCLE_EXPORT + #else + #define RCLCPP_LIFECYCLE_PUBLIC RCLCPP_LIFECYCLE_IMPORT + #endif + #define RCLCPP_LIFECYCLE_PUBLIC_TYPE RCLCPP_LIFECYCLE_PUBLIC + #define RCLCPP_LIFECYCLE_LOCAL #else - #define LIFECYCLE_EXPORT + #define RCLCPP_LIFECYCLE_EXPORT __attribute__ ((visibility("default"))) + #define RCLCPP_LIFECYCLE_IMPORT + #if __GNUC__ >= 4 + #define RCLCPP_LIFECYCLE_PUBLIC __attribute__ ((visibility("default"))) + #define RCLCPP_LIFECYCLE_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RCLCPP_LIFECYCLE_PUBLIC + #define RCLCPP_LIFECYCLE_LOCAL + #endif + #define RCLCPP_LIFECYCLE_PUBLIC_TYPE #endif #endif // RCLCPP_LIFECYCLE__VISIBILITY_CONTROL_H_ diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index fb7f9fdfb3..7eea88c87e 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -26,6 +26,7 @@ std_msgs lifecycle_msgs + ament_cmake_gtest ament_lint_auto ament_lint_common diff --git a/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp b/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp index 08f218c0ee..a144c52d5b 100644 --- a/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp @@ -61,10 +61,9 @@ class LifecycleManager::LifecycleManagerImpl { for (auto it = node_handle_map_.begin(); it != node_handle_map_.end(); ++it) { rcl_state_machine_t * rcl_state_machine = &it->second.state_machine; - if (!rcl_state_machine) - { + if (!rcl_state_machine) { fprintf(stderr, "%s:%u, FATAL: rcl_state_machine got destroyed externally.\n", - __FILE__, __LINE__); + __FILE__, __LINE__); } else { rcl_state_machine_fini(rcl_state_machine, node_base_handle_->get_rcl_node_handle()); } @@ -110,7 +109,7 @@ class LifecycleManager::LifecycleManagerImpl } void - on_change_state(const std::shared_ptrheader, + on_change_state(const std::shared_ptr header, const std::shared_ptr req, std::shared_ptr resp) { @@ -138,17 +137,17 @@ class LifecycleManager::LifecycleManagerImpl const LifecycleInterfacePtr & lifecycle_interface) { // TODO(karsten1987): clarify what do if node already exists; - NodeStateMachine& node_state_machine = node_handle_map_[node_name]; + NodeStateMachine & node_state_machine = node_handle_map_[node_name]; node_state_machine.state_machine = rcl_get_zero_initialized_state_machine(); - rcl_ret_t ret = rcl_state_machine_init(&node_state_machine.state_machine, node_base_handle_->get_rcl_node_handle(), - rosidl_generator_cpp::get_message_type_support_handle(), - rosidl_generator_cpp::get_service_type_support_handle(), - rosidl_generator_cpp::get_service_type_support_handle(), - true); - if (ret != RCL_RET_OK) - { + rcl_ret_t ret = rcl_state_machine_init(&node_state_machine.state_machine, + node_base_handle_->get_rcl_node_handle(), + rosidl_generator_cpp::get_message_type_support_handle(), + rosidl_generator_cpp::get_service_type_support_handle(), + rosidl_generator_cpp::get_service_type_support_handle(), + true); + if (ret != RCL_RET_OK) { fprintf(stderr, "Error adding %s: %s\n", - node_name.c_str(), rcl_get_error_string_safe()); + node_name.c_str(), rcl_get_error_string_safe()); return; } @@ -188,7 +187,8 @@ class LifecycleManager::LifecycleManagerImpl node_base_handle_->get_shared_node_handle(), &node_state_machine.state_machine.comm_interface.srv_change_state, any_cb); - auto srv_change_state_base = std::dynamic_pointer_cast(srv_change_state); + auto srv_change_state_base = + std::dynamic_pointer_cast(srv_change_state); node_base_handle_->add_service(srv_change_state_base); node_state_machine.srv_change_state = srv_change_state_base; } diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index b7549dda25..4b05171c97 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -43,7 +43,7 @@ LifecycleNode::LifecycleNode(const std::string & node_name, bool use_intra_proce } LifecycleNode::~LifecycleNode() -{}; +{} bool LifecycleNode::register_on_configure(std::function fcn) @@ -78,11 +78,31 @@ LifecycleNode::register_on_deactivate(std::function fcn) bool LifecycleNode::register_on_error(std::function fcn) { - return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING, fcn); + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING, + fcn); +} + +const State & +LifecycleNode::get_current_state() +{ + return impl_->get_current_state(); +} + +const State & +LifecycleNode::trigger_transition(const Transition & transition) +{ + return trigger_transition(transition.id()); +} + +const State & +LifecycleNode::trigger_transition(unsigned int transition_id) +{ + return impl_->trigger_transition(transition_id); } void -LifecycleNode::add_publisher_handle(std::shared_ptr pub) +LifecycleNode::add_publisher_handle( + std::shared_ptr pub) { impl_->add_publisher_handle(pub); } diff --git a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp index 8172b9fe73..6a71b6d732 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp @@ -42,9 +42,9 @@ namespace lifecycle class LifecycleNode::LifecycleNodeImpl { -using TransitionEventMsg = lifecycle_msgs::msg::TransitionEvent; -using GetStateSrv = lifecycle_msgs::srv::GetState; -using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; + using TransitionEventMsg = lifecycle_msgs::msg::TransitionEvent; + using GetStateSrv = lifecycle_msgs::srv::GetState; + using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; public: LifecycleNodeImpl(std::shared_ptr(base_node_handle)) @@ -53,10 +53,9 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; ~LifecycleNodeImpl() { - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) - { + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { fprintf(stderr, "%s:%u, FATAL: rcl_state_machine got destroyed externally.\n", - __FILE__, __LINE__); + __FILE__, __LINE__); } else { rcl_lifecycle_state_machine_fini(&state_machine_, base_node_handle_->get_rcl_node_handle()); } @@ -72,10 +71,9 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; rosidl_generator_cpp::get_service_type_support_handle(), rosidl_generator_cpp::get_service_type_support_handle(), true); - if (ret != RCL_RET_OK) - { + if (ret != RCL_RET_OK) { fprintf(stderr, "Error adding %s: %s\n", - base_node_handle_->get_name().c_str(), rcl_get_error_string_safe()); + base_node_handle_->get_name().c_str(), rcl_get_error_string_safe()); return; } @@ -90,7 +88,7 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; base_node_handle_->get_shared_node_handle(), &state_machine_.com_interface.srv_get_state, any_cb); base_node_handle_->add_service( - std::dynamic_pointer_cast(srv_get_state_)); + std::dynamic_pointer_cast(srv_get_state_)); } { // change_state @@ -104,42 +102,49 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; &state_machine_.com_interface.srv_change_state, any_cb); base_node_handle_->add_service( - std::dynamic_pointer_cast(srv_change_state_)); + std::dynamic_pointer_cast(srv_change_state_)); } } + bool + register_callback(std::uint8_t lifecycle_transition, std::function & cb) + { + cb_map_[lifecycle_transition] = cb; + return true; + } + void - on_get_state(const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) + on_change_state(const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) { (void)header; - (void)req; if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - resp->current_state = static_cast(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN); + resp->success = false; return; } - resp->current_state = static_cast(state_machine_.current_state->id); + resp->success = change_state(req->transition.id); } void - on_change_state(const std::shared_ptrheader, - const std::shared_ptr req, - std::shared_ptr resp) + on_get_state(const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) { (void)header; + (void)req; if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - resp->success = false; + resp->current_state = static_cast(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN); return; } - resp->success = change_state(req->transition.id); + resp->current_state = static_cast(state_machine_.current_state->id); } - bool - register_callback(std::uint8_t lifecycle_transition, std::function & cb) + const State & + get_current_state() { - cb_map_[lifecycle_transition] = cb; - return true; + current_state_ = State(state_machine_.current_state); + return current_state_; } bool @@ -159,29 +164,64 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; return false; } - auto cb_success = true; // in case no callback was attached, we forward directly - auto it = cb_map_.find(state_machine_.current_state->id); - if (it != cb_map_.end()) - { - std::function callback = it->second; - cb_success = callback(); - } else { - fprintf(stderr, "%s:%d, No callback is registered for transition %u.\n", - __FILE__, __LINE__, lifecycle_transition); - } - - if (rcl_lifecycle_start_transition(&state_machine_, transition_id, cb_success, true) != RCL_RET_OK) + auto cb_success = execute_callback(state_machine_.current_state->id); + if (rcl_lifecycle_start_transition( + &state_machine_, transition_id, cb_success, true) != RCL_RET_OK) { fprintf(stderr, "Failed to finish transition %u. Current state is now: %s\n", transition_id, state_machine_.current_state->label); return false; } + + // error handling ?! + if (!cb_success) + { + auto error_resolved = execute_callback(state_machine_.current_state->id); + if (error_resolved) { + // We call cleanup on the error state + rcl_lifecycle_start_transition( + &state_machine_, lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, true, true); + } else { + // We call shutdown on the error state + rcl_lifecycle_start_transition( + &state_machine_, lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN, true, true); + } + } // This true holds in both cases where the actual callback // was successful or not, since at this point we have a valid transistion // to either a new primary state or error state return true; } + bool + execute_callback(unsigned int cb_id) + { + auto cb_success = true; // in case no callback was attached, we forward directly + auto it = cb_map_.find(state_machine_.current_state->id); + if (it != cb_map_.end()) { + std::function callback = it->second; + try{ + cb_success = callback(); + } catch (const std::exception & e) { + fprintf(stderr, "Caught exception in callback for transition %d\n", + it->first); + cb_success = false; + } + } else { + fprintf(stderr, "%s:%d, No callback is registered for transition %u.\n", + __FILE__, __LINE__, cb_id); + } + return cb_success; + } + + + const State & + trigger_transition(unsigned int transition_id) + { + change_state(transition_id); + return get_current_state(); + } + void add_publisher_handle(std::shared_ptr pub) { @@ -195,6 +235,7 @@ using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; } rcl_lifecycle_state_machine_t state_machine_; + State current_state_; std::map> cb_map_; std::shared_ptr base_node_handle_; diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp new file mode 100644 index 0000000000..e14dd59e3b --- /dev/null +++ b/rclcpp_lifecycle/src/state.cpp @@ -0,0 +1,70 @@ +// 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 "rclcpp_lifecycle/state.hpp" + +namespace rclcpp +{ +namespace lifecycle +{ + +State::State(): + State(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, "unknown") +{} + +State::State(unsigned int id, const std::string & label) + : owns_rcl_state_handle_(true) +{ + if (label.empty()) { + throw std::runtime_error("Lifecycle State cannot have an empty label."); + } + + auto state_handle = new rcl_lifecycle_state_t; + state_handle->id = id; + state_handle->label = label.c_str(); + + state_handle_ = state_handle; +} + +State::State(const rcl_lifecycle_state_t * rcl_lifecycle_state_handle) + : owns_rcl_state_handle_(false) +{ + state_handle_ = rcl_lifecycle_state_handle; +} + +State::~State() +{ + if (owns_rcl_state_handle_) + { + delete state_handle_; + } +} + +unsigned int +State::id() const +{ + return state_handle_->id; +} + +std::string +State::label() const +{ + return state_handle_->label; +} + +} // namespace lifecycle +} // namespace rclcpp diff --git a/rclcpp_lifecycle/src/transition.cpp b/rclcpp_lifecycle/src/transition.cpp new file mode 100644 index 0000000000..3326f7b4b0 --- /dev/null +++ b/rclcpp_lifecycle/src/transition.cpp @@ -0,0 +1,62 @@ +// 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 "rclcpp_lifecycle/transition.hpp" + +namespace rclcpp +{ +namespace lifecycle +{ + +Transition::Transition(unsigned int id, const std::string & label) + : owns_rcl_transition_handle_(true) +{ + auto transition_handle = new rcl_lifecycle_transition_t; + transition_handle->id = id; + transition_handle->label = label.c_str(); + + transition_handle_ = transition_handle; +} + +Transition::Transition(const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle) + : owns_rcl_transition_handle_(false) +{ + transition_handle_ = rcl_lifecycle_transition_handle; +} + +Transition::~Transition() +{ + if (owns_rcl_transition_handle_) + { + delete transition_handle_; + } +} + +unsigned int +Transition::id() const +{ + return transition_handle_->id; +} + +std::string +Transition::label() const +{ + return transition_handle_->label; +} + +} // namespace lifecycle +} // namespace rclcpp diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp new file mode 100644 index 0000000000..0db96c6ad3 --- /dev/null +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -0,0 +1,192 @@ +// 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. + +#include +#include + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using lifecycle_msgs::msg::State; +using lifecycle_msgs::msg::Transition; + +struct GoodMood +{ + static constexpr bool cb_ret = true; + static constexpr bool error_ret = true; +}; +struct BadMood +{ + static constexpr bool cb_ret = false; + static constexpr bool error_ret = true; +}; +struct VeryBadMood +{ + static constexpr bool cb_ret = false; + static constexpr bool error_ret = false; +}; + +class TestDefaultStateMachine : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +class EmptyLifecycleNode : public rclcpp::lifecycle::LifecycleNode +{ +public: + EmptyLifecycleNode(std::string node_name) + : rclcpp::lifecycle::LifecycleNode(std::move(node_name)) + {} +}; + +template +class MoodyLifecycleNode : public rclcpp::lifecycle::LifecycleNode +{ +public: + MoodyLifecycleNode(std::string node_name) + : rclcpp::lifecycle::LifecycleNode(std::move(node_name)) + {} + + size_t number_of_callbacks = 0; + +protected: + bool on_configure() + { + EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id()); + ++number_of_callbacks; + return Mood::cb_ret; + } + + bool on_activate() + { + EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id()); + ++number_of_callbacks; + return Mood::cb_ret; + } + + bool on_deactivate() + { + EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id()); + ++number_of_callbacks; + return Mood::cb_ret; + } + + bool on_cleanup() + { + EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id()); + ++number_of_callbacks; + return Mood::cb_ret; + } + + bool on_shutdown() + { + EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id()); + ++number_of_callbacks; + return Mood::cb_ret; + } + + bool on_error(); +}; + +template<> +bool MoodyLifecycleNode::on_error() +{ + EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id()); + ADD_FAILURE(); + return GoodMood::error_ret; +} +template<> +bool MoodyLifecycleNode::on_error() +{ + EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id()); + ++number_of_callbacks; + return BadMood::error_ret; +} +template<> +bool MoodyLifecycleNode::on_error() +{ + EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id()); + ++number_of_callbacks; + return VeryBadMood::error_ret; +} + +TEST_F(TestDefaultStateMachine, empty_initializer) { + auto test_node = std::make_shared("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); +} + +TEST_F(TestDefaultStateMachine, trigger_transition) { + auto test_node = std::make_shared("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( + rclcpp::lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); + EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp::lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + rclcpp::lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); + EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + rclcpp::lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); +} + +TEST_F(TestDefaultStateMachine, good_mood) { + auto test_node = std::make_shared>("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( + rclcpp::lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); + EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp::lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + rclcpp::lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); + EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + rclcpp::lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); + + // check if all callbacks were successfully overwritten + EXPECT_EQ(5, test_node->number_of_callbacks); +} + +TEST_F(TestDefaultStateMachine, bad_mood) { + auto test_node = std::make_shared>("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + + // check if all callbacks were successfully overwritten + EXPECT_EQ(2, test_node->number_of_callbacks); +} + +TEST_F(TestDefaultStateMachine, very_bad_mood) { + auto test_node = std::make_shared>("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + + // check if all callbacks were successfully overwritten + EXPECT_EQ(2, test_node->number_of_callbacks); +} From 14402e79777c1c6e202320f7a242cd09a32f8fda Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 8 Dec 2016 15:06:35 -0800 Subject: [PATCH 42/63] (rebase) change to new typesupport --- rclcpp_lifecycle/src/lifecycle_node_impl.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp index 6a71b6d732..733f0fd425 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp @@ -68,8 +68,8 @@ class LifecycleNode::LifecycleNodeImpl rcl_ret_t ret = rcl_lifecycle_state_machine_init( &state_machine_, base_node_handle_->get_rcl_node_handle(), ROSIDL_GET_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), - rosidl_generator_cpp::get_service_type_support_handle(), - rosidl_generator_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), true); if (ret != RCL_RET_OK) { fprintf(stderr, "Error adding %s: %s\n", From bd5db8d123a78778e12413493d00d37c16020f02 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 8 Dec 2016 15:09:11 -0800 Subject: [PATCH 43/63] uncrustify' --- .../include/rclcpp_lifecycle/state.hpp | 4 +- .../include/rclcpp_lifecycle/transition.hpp | 6 +- rclcpp_lifecycle/src/lifecycle_manager.cpp | 120 -------- .../src/lifecycle_manager_impl.hpp | 291 ------------------ rclcpp_lifecycle/src/lifecycle_node.cpp | 9 +- rclcpp_lifecycle/src/lifecycle_node_impl.hpp | 48 ++- rclcpp_lifecycle/src/state.cpp | 15 +- rclcpp_lifecycle/src/transition.cpp | 11 +- rclcpp_lifecycle/test/test_lifecycle_node.cpp | 39 +-- 9 files changed, 67 insertions(+), 476 deletions(-) delete mode 100644 rclcpp_lifecycle/src/lifecycle_manager.cpp delete mode 100644 rclcpp_lifecycle/src/lifecycle_manager_impl.hpp diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp index adbd07db68..9c6f14ceb7 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -37,7 +37,7 @@ class State State(unsigned int id, const std::string & label); RCLCPP_LIFECYCLE_PUBLIC - State(const rcl_lifecycle_state_t * rcl_lifecycle_state_handle); + explicit State(const rcl_lifecycle_state_t * rcl_lifecycle_state_handle); RCLCPP_LIFECYCLE_PUBLIC virtual ~State(); @@ -55,6 +55,6 @@ class State const rcl_lifecycle_state_t * state_handle_; }; -} // namespace rclcpp_lifecycle +} // namespace lifecycle } // namespace rclcpp #endif // RCLCPP_LIFECYCLE__STATE_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp index 7c237f6b0a..30706eb954 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp @@ -34,10 +34,10 @@ class Transition Transition() = delete; RCLCPP_LIFECYCLE_PUBLIC - Transition(unsigned int id, const std::string & label = ""); + explicit Transition(unsigned int id, const std::string & label = ""); RCLCPP_LIFECYCLE_PUBLIC - Transition(const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle); + explicit Transition(const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle); RCLCPP_LIFECYCLE_PUBLIC virtual ~Transition(); @@ -55,6 +55,6 @@ class Transition const rcl_lifecycle_transition_t * transition_handle_; }; -} // namespace rclcpp_lifecycle +} // namespace lifecycle } // namespace rclcpp #endif // RCLCPP_LIFECYCLE__TRANSITION_HPP_ diff --git a/rclcpp_lifecycle/src/lifecycle_manager.cpp b/rclcpp_lifecycle/src/lifecycle_manager.cpp deleted file mode 100644 index 7ca1b7885d..0000000000 --- a/rclcpp_lifecycle/src/lifecycle_manager.cpp +++ /dev/null @@ -1,120 +0,0 @@ -// 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 "rclcpp_lifecycle/lifecycle_manager.hpp" - -#include "lifecycle_manager_impl.hpp" // implementation - -namespace rclcpp -{ -namespace lifecycle -{ - -LifecycleManager::LifecycleManager() -: impl_(new LifecycleManagerImpl()) -{ - impl_->init(); -} - -LifecycleManager::~LifecycleManager() = default; - -std::shared_ptr -LifecycleManager::get_node_base_interface() -{ - return impl_->node_base_handle_; -} - -void -LifecycleManager::add_node_interface(const NodePtr & node) -{ - add_node_interface(node->get_base_interface()->get_name(), node); -} - -void -LifecycleManager::add_node_interface(const std::string & node_name, - const NodeInterfacePtr & node_interface) -{ - impl_->add_node_interface(node_name, node_interface); -} - -bool -LifecycleManager::register_on_configure(const std::string & node_name, - std::function & fcn) -{ - return impl_->register_callback(node_name, fcn); -} - -bool -LifecycleManager::configure(const std::string & node_name) -{ - return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::CONFIGURING); -} - -bool -LifecycleManager::register_on_cleanup(const std::string & node_name, - std::function & fcn) -{ - return impl_->register_callback(node_name, fcn); -} - -bool -LifecycleManager::cleanup(const std::string & node_name) -{ - return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::CLEANINGUP); -} - -bool -LifecycleManager::register_on_shutdown(const std::string & node_name, - std::function & fcn) -{ - return impl_->register_callback(node_name, fcn); -} - -bool -LifecycleManager::shutdown(const std::string & node_name) -{ - return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::SHUTTINGDOWN); -} - -bool -LifecycleManager::register_on_activate(const std::string & node_name, - std::function & fcn) -{ - return impl_->register_callback(node_name, fcn); -} - -bool -LifecycleManager::activate(const std::string & node_name) -{ - return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::ACTIVATING); -} - -bool -LifecycleManager::register_on_deactivate(const std::string & node_name, - std::function & fcn) -{ - return impl_->register_callback(node_name, fcn); -} - -bool -LifecycleManager::deactivate(const std::string & node_name) -{ - return impl_->change_state(node_name, lifecycle::LifecycleTransitionsT::DEACTIVATING); -} - -} // namespace lifecycle -} // namespace rclcpp diff --git a/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp b/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp deleted file mode 100644 index a144c52d5b..0000000000 --- a/rclcpp_lifecycle/src/lifecycle_manager_impl.hpp +++ /dev/null @@ -1,291 +0,0 @@ -// 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 RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_IMPL_HPP_ -#define RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_IMPL_HPP_ - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/lifecycle_manager.hpp" - - -namespace rclcpp -{ -namespace lifecycle -{ - -using LifecycleInterface = rclcpp::node::lifecycle::LifecycleNodeInterface; -using LifecycleInterfacePtr = std::shared_ptr; -using LifecycleInterfaceWeakPtr = std::weak_ptr; - -struct NodeStateMachine -{ - LifecycleInterfaceWeakPtr weak_node_handle; - rcl_state_machine_t state_machine; - std::map> cb_map; - std::shared_ptr srv_get_state; - std::shared_ptr srv_change_state; -}; - -class LifecycleManager::LifecycleManagerImpl -{ -public: - LifecycleManagerImpl() - : node_base_handle_(std::make_shared("lifecycle_manager")) - {} - - ~LifecycleManagerImpl() - { - for (auto it = node_handle_map_.begin(); it != node_handle_map_.end(); ++it) { - rcl_state_machine_t * rcl_state_machine = &it->second.state_machine; - if (!rcl_state_machine) { - fprintf(stderr, "%s:%u, FATAL: rcl_state_machine got destroyed externally.\n", - __FILE__, __LINE__); - } else { - rcl_state_machine_fini(rcl_state_machine, node_base_handle_->get_rcl_node_handle()); - } - } - } - - void - init() - { - srv_get_state_ = node_base_handle_->create_service( - node_base_handle_->get_name() + "__get_state", - std::bind(&LifecycleManagerImpl::on_get_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - srv_change_state_ = node_base_handle_->create_service( - node_base_handle_->get_name() + "__change_state", - std::bind(&LifecycleManagerImpl::on_change_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - } - - void - on_get_state(const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) - { - on_get_single_state(header, req, resp, req->node_name); - } - - void - on_get_single_state(const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp, - std::string node_name) - { - (void)header; - (void)req; - auto node_handle_iter = node_handle_map_.find(node_name); - if (node_handle_iter == node_handle_map_.end()) { - resp->current_state = static_cast(LifecyclePrimaryStatesT::UNKNOWN); - return; - } - resp->current_state = - static_cast(node_handle_iter->second.state_machine.current_state->index); - } - - void - on_change_state(const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) - { - on_change_single_state(header, req, resp, req->node_name); - } - - void - on_change_single_state(const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp, - std::string node_name) - { - (void)header; - auto node_handle_iter = node_handle_map_.find(node_name); - if (node_handle_iter == node_handle_map_.end()) { - resp->success = false; - return; - } - auto transition = static_cast(req->transition); - resp->success = change_state(node_name, transition); - } - - void - add_node_interface(const std::string & node_name, - const LifecycleInterfacePtr & lifecycle_interface) - { - // TODO(karsten1987): clarify what do if node already exists; - NodeStateMachine & node_state_machine = node_handle_map_[node_name]; - node_state_machine.state_machine = rcl_get_zero_initialized_state_machine(); - rcl_ret_t ret = rcl_state_machine_init(&node_state_machine.state_machine, - node_base_handle_->get_rcl_node_handle(), - rosidl_generator_cpp::get_message_type_support_handle(), - rosidl_generator_cpp::get_service_type_support_handle(), - rosidl_generator_cpp::get_service_type_support_handle(), - true); - if (ret != RCL_RET_OK) { - fprintf(stderr, "Error adding %s: %s\n", - node_name.c_str(), rcl_get_error_string_safe()); - return; - } - - // srv objects may get destroyed directly here - { // get_state - std::function, - const std::shared_ptr, - std::shared_ptr)> cb = - std::bind(&LifecycleManagerImpl::on_get_single_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - node_name); - - rclcpp::any_service_callback::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - auto srv_get_state = - rclcpp::service::Service::make_shared( - node_base_handle_->get_shared_node_handle(), - &node_state_machine.state_machine.comm_interface.srv_get_state, - any_cb); - auto srv_get_state_base = std::dynamic_pointer_cast(srv_get_state); - node_base_handle_->add_service(srv_get_state_base); - node_state_machine.srv_get_state = srv_get_state_base; - } - - { // change_state - std::function, - const std::shared_ptr, - std::shared_ptr)> cb = - std::bind(&LifecycleManagerImpl::on_change_single_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - node_name); - - rclcpp::any_service_callback::AnyServiceCallback any_cb; - any_cb.set(cb); - auto srv_change_state = - std::make_shared>( - node_base_handle_->get_shared_node_handle(), - &node_state_machine.state_machine.comm_interface.srv_change_state, - any_cb); - auto srv_change_state_base = - std::dynamic_pointer_cast(srv_change_state); - node_base_handle_->add_service(srv_change_state_base); - node_state_machine.srv_change_state = srv_change_state_base; - } - - node_state_machine.weak_node_handle = lifecycle_interface; - // register default callbacks - // maybe optional - std::function cb_configuring = std::bind( - &LifecycleInterface::on_configure, lifecycle_interface); - std::function cb_cleaningup = std::bind( - &LifecycleInterface::on_cleanup, lifecycle_interface); - std::function cb_shuttingdown = std::bind( - &LifecycleInterface::on_shutdown, lifecycle_interface); - std::function cb_activating = std::bind( - &LifecycleInterface::on_activate, lifecycle_interface); - std::function cb_deactivating = std::bind( - &LifecycleInterface::on_deactivate, lifecycle_interface); - std::function cb_error = std::bind( - &LifecycleInterface::on_error, lifecycle_interface); - node_state_machine.cb_map[LifecycleTransitionsT::CONFIGURING] = cb_configuring; - node_state_machine.cb_map[LifecycleTransitionsT::CLEANINGUP] = cb_cleaningup; - node_state_machine.cb_map[LifecycleTransitionsT::SHUTTINGDOWN] = cb_shuttingdown; - node_state_machine.cb_map[LifecycleTransitionsT::ACTIVATING] = cb_activating; - node_state_machine.cb_map[LifecycleTransitionsT::DEACTIVATING] = cb_deactivating; - node_state_machine.cb_map[LifecycleTransitionsT::ERRORPROCESSING] = cb_error; - } - - template - bool - register_callback(const std::string & node_name, std::function & cb) - { - if (node_name.empty()) { - return false; - } - - auto node_handle_iter = node_handle_map_.find(node_name); - if (node_handle_iter == node_handle_map_.end()) { - fprintf(stderr, "Node with name %s is not registered\n", node_name.c_str()); - } - node_handle_iter->second.cb_map[lifecycle_transition] = cb; - return true; - } - - bool - change_state(const std::string & node_name, LifecycleTransitionsT lifecycle_transition) - { - if (node_name.empty()) { - return false; - } - - auto node_handle_iter = node_handle_map_.find(node_name); - if (node_handle_iter == node_handle_map_.end()) { - fprintf(stderr, "%s:%d, Node with name %s is not registered\n", - __FILE__, __LINE__, node_name.c_str()); - return false; - } - - auto node_handle = node_handle_iter->second.weak_node_handle.lock(); - if (!node_handle) { - fprintf(stderr, - "%s:%d, Nodehandle is not available. Was it destroyed outside the lifecycle manager?\n", - __FILE__, __LINE__); - return false; - } - - unsigned int transition_index = static_cast(lifecycle_transition); - if (!rcl_start_transition_by_index(&node_handle_iter->second.state_machine, transition_index)) { - fprintf(stderr, "%s:%d, Unable to start transition %u from current state %s\n", - __FILE__, __LINE__, transition_index, - node_handle_iter->second.state_machine.current_state->label); - return false; - } - - // Since we set always set a default callback, - // we don't have to check for nullptr here - std::function callback = node_handle_iter->second.cb_map[lifecycle_transition]; - auto success = callback(); - - if (!rcl_finish_transition_by_index(&node_handle_iter->second.state_machine, - transition_index, success)) - { - fprintf(stderr, "Failed to finish transition %u. Current state is now: %s\n", - transition_index, node_handle_iter->second.state_machine.current_state->label); - return false; - } - // This true holds in both cases where the actual callback - // was successful or not, since at this point we have a valid transistion - // to either a new primary state or error state - return true; - } - - std::shared_ptr node_base_handle_; - std::shared_ptr> srv_get_state_; - std::shared_ptr> srv_change_state_; - std::map node_handle_map_; -}; - -} // namespace lifecycle -} // namespace rclcpp -#endif // RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_IMPL_HPP_ diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 4b05171c97..99467f4b80 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -12,15 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "lifecycle_node_impl.hpp" // implementation #include #include -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -#include "lifecycle_node_impl.hpp" // implementation +#include +#include namespace rclcpp { diff --git a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp index 733f0fd425..c86308fe6a 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp @@ -12,27 +12,28 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ -#define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ +#ifndef LIFECYCLE_NODE_IMPL_HPP_ +#define LIFECYCLE_NODE_IMPL_HPP_ -#include +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include +#include #include +#include #include -#include -#include - -#include +#include -#include +#include "lifecycle_msgs/srv/change_state.hpp" +#include "lifecycle_msgs/srv/get_state.hpp" +#include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport +#include "lifecycle_msgs/msg/transition_event.hpp" -#include -#include // for getting the c-typesupport -#include -#include +#include "rcl/error_handling.h" -#include +#include "rcl_lifecycle/rcl_lifecycle.h" -#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp/node.hpp" namespace rclcpp { @@ -41,13 +42,12 @@ namespace lifecycle class LifecycleNode::LifecycleNodeImpl { - using TransitionEventMsg = lifecycle_msgs::msg::TransitionEvent; using GetStateSrv = lifecycle_msgs::srv::GetState; using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; public: - LifecycleNodeImpl(std::shared_ptr(base_node_handle)) + explicit LifecycleNodeImpl(std::shared_ptr(base_node_handle)) : base_node_handle_(base_node_handle) {} @@ -166,7 +166,7 @@ class LifecycleNode::LifecycleNodeImpl auto cb_success = execute_callback(state_machine_.current_state->id); if (rcl_lifecycle_start_transition( - &state_machine_, transition_id, cb_success, true) != RCL_RET_OK) + &state_machine_, transition_id, cb_success, true) != RCL_RET_OK) { fprintf(stderr, "Failed to finish transition %u. Current state is now: %s\n", transition_id, state_machine_.current_state->label); @@ -174,18 +174,17 @@ class LifecycleNode::LifecycleNodeImpl } // error handling ?! - if (!cb_success) - { + if (!cb_success) { auto error_resolved = execute_callback(state_machine_.current_state->id); if (error_resolved) { // We call cleanup on the error state rcl_lifecycle_start_transition( - &state_machine_, lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, true, true); + &state_machine_, lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, true, true); } else { // We call shutdown on the error state rcl_lifecycle_start_transition( - &state_machine_, lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN, true, true); - } + &state_machine_, lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN, true, true); + } } // This true holds in both cases where the actual callback // was successful or not, since at this point we have a valid transistion @@ -200,7 +199,7 @@ class LifecycleNode::LifecycleNodeImpl auto it = cb_map_.find(state_machine_.current_state->id); if (it != cb_map_.end()) { std::function callback = it->second; - try{ + try { cb_success = callback(); } catch (const std::exception & e) { fprintf(stderr, "Caught exception in callback for transition %d\n", @@ -214,7 +213,6 @@ class LifecycleNode::LifecycleNodeImpl return cb_success; } - const State & trigger_transition(unsigned int transition_id) { @@ -249,4 +247,4 @@ class LifecycleNode::LifecycleNodeImpl } // namespace lifecycle } // namespace rclcpp -#endif // RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_IMPL_HPP_ +#endif // LIFECYCLE_NODE_IMPL_HPP_ diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp index e14dd59e3b..3e3d432b03 100644 --- a/rclcpp_lifecycle/src/state.cpp +++ b/rclcpp_lifecycle/src/state.cpp @@ -12,22 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rclcpp_lifecycle/state.hpp" + #include #include -#include "rclcpp_lifecycle/state.hpp" +#include namespace rclcpp { namespace lifecycle { -State::State(): - State(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, "unknown") +State::State() +: State(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, "unknown") {} State::State(unsigned int id, const std::string & label) - : owns_rcl_state_handle_(true) +: owns_rcl_state_handle_(true) { if (label.empty()) { throw std::runtime_error("Lifecycle State cannot have an empty label."); @@ -41,15 +43,14 @@ State::State(unsigned int id, const std::string & label) } State::State(const rcl_lifecycle_state_t * rcl_lifecycle_state_handle) - : owns_rcl_state_handle_(false) +: owns_rcl_state_handle_(false) { state_handle_ = rcl_lifecycle_state_handle; } State::~State() { - if (owns_rcl_state_handle_) - { + if (owns_rcl_state_handle_) { delete state_handle_; } } diff --git a/rclcpp_lifecycle/src/transition.cpp b/rclcpp_lifecycle/src/transition.cpp index 3326f7b4b0..72ad595dd2 100644 --- a/rclcpp_lifecycle/src/transition.cpp +++ b/rclcpp_lifecycle/src/transition.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rclcpp_lifecycle/transition.hpp" + #include #include -#include "rclcpp_lifecycle/transition.hpp" +#include namespace rclcpp { @@ -23,7 +25,7 @@ namespace lifecycle { Transition::Transition(unsigned int id, const std::string & label) - : owns_rcl_transition_handle_(true) +: owns_rcl_transition_handle_(true) { auto transition_handle = new rcl_lifecycle_transition_t; transition_handle->id = id; @@ -33,15 +35,14 @@ Transition::Transition(unsigned int id, const std::string & label) } Transition::Transition(const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle) - : owns_rcl_transition_handle_(false) +: owns_rcl_transition_handle_(false) { transition_handle_ = rcl_lifecycle_transition_handle; } Transition::~Transition() { - if (owns_rcl_transition_handle_) - { + if (owns_rcl_transition_handle_) { delete transition_handle_; } } diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 0db96c6ad3..a94bfa4882 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -12,11 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. + #include +#include #include +#include -#include -#include +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -52,8 +55,8 @@ class TestDefaultStateMachine : public ::testing::Test class EmptyLifecycleNode : public rclcpp::lifecycle::LifecycleNode { public: - EmptyLifecycleNode(std::string node_name) - : rclcpp::lifecycle::LifecycleNode(std::move(node_name)) + explicit EmptyLifecycleNode(std::string node_name) + : rclcpp::lifecycle::LifecycleNode(std::move(node_name)) {} }; @@ -61,8 +64,8 @@ template class MoodyLifecycleNode : public rclcpp::lifecycle::LifecycleNode { public: - MoodyLifecycleNode(std::string node_name) - : rclcpp::lifecycle::LifecycleNode(std::move(node_name)) + explicit MoodyLifecycleNode(std::string node_name) + : rclcpp::lifecycle::LifecycleNode(std::move(node_name)) {} size_t number_of_callbacks = 0; @@ -139,15 +142,15 @@ TEST_F(TestDefaultStateMachine, trigger_transition) { EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); + rclcpp::lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); + rclcpp::lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); + rclcpp::lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); + rclcpp::lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); } TEST_F(TestDefaultStateMachine, good_mood) { @@ -155,15 +158,15 @@ TEST_F(TestDefaultStateMachine, good_mood) { EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); + rclcpp::lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); + rclcpp::lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); + rclcpp::lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); + rclcpp::lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); // check if all callbacks were successfully overwritten EXPECT_EQ(5, test_node->number_of_callbacks); @@ -174,7 +177,7 @@ TEST_F(TestDefaultStateMachine, bad_mood) { EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); // check if all callbacks were successfully overwritten EXPECT_EQ(2, test_node->number_of_callbacks); @@ -185,7 +188,7 @@ TEST_F(TestDefaultStateMachine, very_bad_mood) { EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); // check if all callbacks were successfully overwritten EXPECT_EQ(2, test_node->number_of_callbacks); From f0e2c1b77a31372701763e9ff120221b12b97047 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 8 Dec 2016 17:07:15 -0800 Subject: [PATCH 44/63] fix visibility control --- rclcpp_lifecycle/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index fbbb26e53b..7c0b6d017b 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -31,6 +31,10 @@ macro(targets) "rcl_lifecycle${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(rclcpp_lifecycle${target_suffix} PRIVATE "RCLCPP_LIFECYCLE_BUILDING_DLL") + install(TARGETS rclcpp_lifecycle${target_suffix} ARCHIVE DESTINATION lib From 99598c2cfa5b48a830fe9f00c738cc91a127b982 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 8 Dec 2016 17:38:05 -0800 Subject: [PATCH 45/63] (fix) correct whitespace --- rclcpp/include/rclcpp/service.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 1b87c4dd56..c7d49c4f96 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -125,7 +125,7 @@ class Service : public ServiceBase : ServiceBase(node_handle), any_callback_(any_callback) { - //check if service handle was initialized + // check if service handle was initialized // TODO(karsten1987): Take this verification // directly in rcl_*_t // see: https://github.com/ros2/rcl/issues/81 From 89a7149b57be30db9ff9075e97339335080711d6 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 8 Dec 2016 17:42:09 -0800 Subject: [PATCH 46/63] (fix) unused variable --- rclcpp/test/test_externally_defined_services.cpp | 2 +- rclcpp_lifecycle/src/lifecycle_node_impl.hpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/test_externally_defined_services.cpp b/rclcpp/test/test_externally_defined_services.cpp index e5ea4078ae..8168054db7 100644 --- a/rclcpp/test/test_externally_defined_services.cpp +++ b/rclcpp/test/test_externally_defined_services.cpp @@ -48,7 +48,7 @@ TEST_F(TestExternallyDefinedServices, default_behavior) { auto srv = node_handle->create_service("test", callback); EXPECT_STREQ(srv->get_service_name().c_str(), "test"); - } catch (const std::exception & e) { + } catch (const std::exception &) { FAIL(); return; } diff --git a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp index c86308fe6a..72c3d33d92 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp @@ -204,6 +204,7 @@ class LifecycleNode::LifecycleNodeImpl } catch (const std::exception & e) { fprintf(stderr, "Caught exception in callback for transition %d\n", it->first); + fprintf(stderr, "Original error msg: %s\n", e.what()); cb_success = false; } } else { From 6a4183981aeb1822a6827fa9969993a76fdbe31c Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 8 Dec 2016 18:43:31 -0800 Subject: [PATCH 47/63] comparison signed and unsigned --- rclcpp_lifecycle/test/test_lifecycle_node.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index a94bfa4882..3e7d2e722c 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -169,7 +169,7 @@ TEST_F(TestDefaultStateMachine, good_mood) { rclcpp::lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); // check if all callbacks were successfully overwritten - EXPECT_EQ(5, test_node->number_of_callbacks); + EXPECT_EQ(static_cast(5), test_node->number_of_callbacks); } TEST_F(TestDefaultStateMachine, bad_mood) { @@ -180,7 +180,7 @@ TEST_F(TestDefaultStateMachine, bad_mood) { rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); // check if all callbacks were successfully overwritten - EXPECT_EQ(2, test_node->number_of_callbacks); + EXPECT_EQ(static_cast(2), test_node->number_of_callbacks); } TEST_F(TestDefaultStateMachine, very_bad_mood) { @@ -191,5 +191,5 @@ TEST_F(TestDefaultStateMachine, very_bad_mood) { rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); // check if all callbacks were successfully overwritten - EXPECT_EQ(2, test_node->number_of_callbacks); + EXPECT_EQ(static_cast(2), test_node->number_of_callbacks); } From c4c2ea1b95d649262f2c23463a6e8b57749450a2 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 9 Dec 2016 08:03:40 -0800 Subject: [PATCH 48/63] get_state returns complete state --- rclcpp_lifecycle/src/lifecycle_node_impl.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp index 72c3d33d92..7506d16c12 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp @@ -134,10 +134,12 @@ class LifecycleNode::LifecycleNodeImpl (void)header; (void)req; if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - resp->current_state = static_cast(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN); + resp->current_state.id = static_cast(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN); + resp->current_state.label = "unknown"; return; } - resp->current_state = static_cast(state_machine_.current_state->id); + resp->current_state.id = static_cast(state_machine_.current_state->id); + resp->current_state.label = state_machine_.current_state->label; } const State & From 35539e3e2eddf4b564071cd6e520080649310845 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 9 Dec 2016 09:40:13 -0800 Subject: [PATCH 49/63] get_available_states service --- rclcpp_lifecycle/src/lifecycle_node_impl.hpp | 66 ++++++++++++++++---- 1 file changed, 53 insertions(+), 13 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp index 7506d16c12..6ede832adc 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp @@ -26,6 +26,7 @@ #include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/get_state.hpp" +#include "lifecycle_msgs/srv/get_available_states.hpp" #include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport #include "lifecycle_msgs/msg/transition_event.hpp" @@ -42,9 +43,10 @@ namespace lifecycle class LifecycleNode::LifecycleNodeImpl { - using TransitionEventMsg = lifecycle_msgs::msg::TransitionEvent; - using GetStateSrv = lifecycle_msgs::srv::GetState; using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; + using GetStateSrv = lifecycle_msgs::srv::GetState; + using GetAvailableStatesSrv = lifecycle_msgs::srv::GetAvailableStates; + using TransitionEventMsg = lifecycle_msgs::msg::TransitionEvent; public: explicit LifecycleNodeImpl(std::shared_ptr(base_node_handle)) @@ -68,8 +70,9 @@ class LifecycleNode::LifecycleNodeImpl rcl_ret_t ret = rcl_lifecycle_state_machine_init( &state_machine_, base_node_handle_->get_rcl_node_handle(), ROSIDL_GET_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), - rosidl_typesupport_cpp::get_service_type_support_handle(), rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), true); if (ret != RCL_RET_OK) { fprintf(stderr, "Error adding %s: %s\n", @@ -77,7 +80,20 @@ class LifecycleNode::LifecycleNodeImpl return; } - // srv objects may get destroyed directly here + { // change_state + auto cb = std::bind(&LifecycleNodeImpl::on_change_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::any_service_callback::AnyServiceCallback any_cb; + any_cb.set(cb); + + srv_change_state_ = std::make_shared>( + base_node_handle_->get_shared_node_handle(), + &state_machine_.com_interface.srv_change_state, + any_cb); + base_node_handle_->add_service( + std::dynamic_pointer_cast(srv_change_state_)); + } + { // get_state auto cb = std::bind(&LifecycleNodeImpl::on_get_state, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); @@ -91,18 +107,17 @@ class LifecycleNode::LifecycleNodeImpl std::dynamic_pointer_cast(srv_get_state_)); } - { // change_state - auto cb = std::bind(&LifecycleNodeImpl::on_change_state, this, + { // get_available_states + auto cb = std::bind(&LifecycleNodeImpl::on_get_available_states, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::any_service_callback::AnyServiceCallback any_cb; - any_cb.set(cb); + rclcpp::any_service_callback::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); - srv_change_state_ = std::make_shared>( - base_node_handle_->get_shared_node_handle(), - &state_machine_.com_interface.srv_change_state, + srv_get_available_states_ = std::make_shared>( + base_node_handle_->get_shared_node_handle(), &state_machine_.com_interface.srv_get_available_states, any_cb); base_node_handle_->add_service( - std::dynamic_pointer_cast(srv_change_state_)); + std::dynamic_pointer_cast(srv_get_available_states_)); } } @@ -134,7 +149,8 @@ class LifecycleNode::LifecycleNodeImpl (void)header; (void)req; if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - resp->current_state.id = static_cast(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN); + resp->current_state.id = + static_cast(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN); resp->current_state.label = "unknown"; return; } @@ -142,6 +158,28 @@ class LifecycleNode::LifecycleNodeImpl resp->current_state.label = state_machine_.current_state->label; } + void + on_get_available_states(const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) + { + (void)header; + (void)req; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + lifecycle_msgs::msg::State unknown_state; + unknown_state.id = static_cast(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN); + unknown_state.label = "unknown"; + resp->available_states.push_back(unknown_state); + return; + } + for (unsigned int i = 0; i < state_machine_.transition_map.size; ++i) { + lifecycle_msgs::msg::State state; + state.id = static_cast(state_machine_.transition_map.states[i].id); + state.label = static_cast(state_machine_.transition_map.states[i].label); + resp->available_states.push_back(state); + } + } + const State & get_current_state() { @@ -242,6 +280,8 @@ class LifecycleNode::LifecycleNodeImpl std::shared_ptr base_node_handle_; std::shared_ptr> srv_change_state_; std::shared_ptr> srv_get_state_; + std::shared_ptr> + srv_get_available_states_; // to controllable things std::vector> weak_pubs_; From 5a7f67c3b06c334521265c97a2e0117c6ba28a6e Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 9 Dec 2016 11:04:08 -0800 Subject: [PATCH 50/63] new service msgs --- rclcpp_lifecycle/src/lifecycle_node_impl.hpp | 84 +++++++++++++++++--- 1 file changed, 71 insertions(+), 13 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp index 6ede832adc..8bfb01e1d4 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp @@ -27,6 +27,8 @@ #include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/get_state.hpp" #include "lifecycle_msgs/srv/get_available_states.hpp" +#include "lifecycle_msgs/srv/get_available_transitions.hpp" +#include "lifecycle_msgs/msg/transition_description.hpp" #include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport #include "lifecycle_msgs/msg/transition_event.hpp" @@ -46,6 +48,7 @@ class LifecycleNode::LifecycleNodeImpl using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; using GetStateSrv = lifecycle_msgs::srv::GetState; using GetAvailableStatesSrv = lifecycle_msgs::srv::GetAvailableStates; + using GetAvailableTransitionsSrv = lifecycle_msgs::srv::GetAvailableTransitions; using TransitionEventMsg = lifecycle_msgs::msg::TransitionEvent; public: @@ -73,6 +76,7 @@ class LifecycleNode::LifecycleNodeImpl rosidl_typesupport_cpp::get_service_type_support_handle(), rosidl_typesupport_cpp::get_service_type_support_handle(), rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), true); if (ret != RCL_RET_OK) { fprintf(stderr, "Error adding %s: %s\n", @@ -101,7 +105,8 @@ class LifecycleNode::LifecycleNodeImpl any_cb.set(std::move(cb)); srv_get_state_ = std::make_shared>( - base_node_handle_->get_shared_node_handle(), &state_machine_.com_interface.srv_get_state, + base_node_handle_->get_shared_node_handle(), + &state_machine_.com_interface.srv_get_state, any_cb); base_node_handle_->add_service( std::dynamic_pointer_cast(srv_get_state_)); @@ -114,11 +119,27 @@ class LifecycleNode::LifecycleNodeImpl any_cb.set(std::move(cb)); srv_get_available_states_ = std::make_shared>( - base_node_handle_->get_shared_node_handle(), &state_machine_.com_interface.srv_get_available_states, + base_node_handle_->get_shared_node_handle(), + &state_machine_.com_interface.srv_get_available_states, any_cb); base_node_handle_->add_service( std::dynamic_pointer_cast(srv_get_available_states_)); } + + { // get_available_transitions + auto cb = std::bind(&LifecycleNodeImpl::on_get_available_transitions, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::any_service_callback::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_available_transitions_ = + std::make_shared>( + base_node_handle_->get_shared_node_handle(), + &state_machine_.com_interface.srv_get_available_transitions, + any_cb); + base_node_handle_->add_service( + std::dynamic_pointer_cast(srv_get_available_transitions_)); + } } bool @@ -130,8 +151,8 @@ class LifecycleNode::LifecycleNodeImpl void on_change_state(const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) + const std::shared_ptr req, + std::shared_ptr resp) { (void)header; if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { @@ -143,8 +164,8 @@ class LifecycleNode::LifecycleNodeImpl void on_get_state(const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) + const std::shared_ptr req, + std::shared_ptr resp) { (void)header; (void)req; @@ -160,8 +181,8 @@ class LifecycleNode::LifecycleNodeImpl void on_get_available_states(const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) + const std::shared_ptr req, + std::shared_ptr resp) { (void)header; (void)req; @@ -180,6 +201,35 @@ class LifecycleNode::LifecycleNodeImpl } } + void + on_get_available_transitions(const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) + { + (void)header; + (void)req; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + return; + } + + for (unsigned int i = 0; i < state_machine_.transition_map.size; ++i) { + // get transitions associated to each primary state + for (unsigned int j = 0; j < state_machine_.transition_map.transition_arrays[i].size; ++j) { + rcl_lifecycle_transition_t rcl_transition = + state_machine_.transition_map.transition_arrays[i].transitions[j]; + + lifecycle_msgs::msg::TransitionDescription trans_desc; + trans_desc.transition.id = rcl_transition.id; + trans_desc.transition.label = rcl_transition.label; + trans_desc.start_state.id = rcl_transition.start->id; + trans_desc.start_state.label = rcl_transition.start->label; + trans_desc.goal_state.id = rcl_transition.goal->id; + trans_desc.goal_state.label = rcl_transition.goal->label; + resp->available_transitions.push_back(trans_desc); + } + } + } + const State & get_current_state() { @@ -277,11 +327,19 @@ class LifecycleNode::LifecycleNodeImpl State current_state_; std::map> cb_map_; - std::shared_ptr base_node_handle_; - std::shared_ptr> srv_change_state_; - std::shared_ptr> srv_get_state_; - std::shared_ptr> - srv_get_available_states_; + using BaseNodePtr = std::shared_ptr; + using ChangeStateSrvPtr = std::shared_ptr>; + using GetStateSrvPtr = std::shared_ptr>; + using GetAvailableStatesSrvPtr = + std::shared_ptr>; + using GetAvailableTransitionsSrvPtr = + std::shared_ptr>; + + BaseNodePtr base_node_handle_; + ChangeStateSrvPtr srv_change_state_; + GetStateSrvPtr srv_get_state_; + GetAvailableStatesSrvPtr srv_get_available_states_; + GetAvailableTransitionsSrvPtr srv_get_available_transitions_; // to controllable things std::vector> weak_pubs_; From 1db9aac33c35d3e8eead31d67bfdfb9575687de2 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 9 Dec 2016 16:00:47 -0800 Subject: [PATCH 51/63] get available states and transitions api --- rclcpp_lifecycle/CMakeLists.txt | 9 +++ .../rclcpp_lifecycle/lifecycle_node.hpp | 8 ++ .../include/rclcpp_lifecycle/transition.hpp | 15 ++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 13 ++++ rclcpp_lifecycle/src/lifecycle_node_impl.hpp | 27 +++++++ rclcpp_lifecycle/src/transition.cpp | 46 ++++++++++++ .../test/test_state_machine_info.cpp | 75 +++++++++++++++++++ 7 files changed, 193 insertions(+) create mode 100644 rclcpp_lifecycle/test/test_state_machine_info.cpp diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 7c0b6d017b..aba182eabc 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -57,6 +57,15 @@ if(BUILD_TESTING) ) target_link_libraries(test_lifecycle_node ${PROJECT_NAME}) endif() + ament_add_gtest(test_state_machine_info test/test_state_machine_info.cpp) + if(TARGET test_state_machine_info) + target_include_directories(test_state_machine_info PUBLIC + ${rcl_lifecycle_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} + ${rclcpp_lifecycle_INCLUDE_DIRS} + ) + target_link_libraries(test_state_machine_info ${PROJECT_NAME}) + endif() endif() diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 13c8fe5e7b..7a8b8f6918 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -134,6 +134,14 @@ class LifecycleNode : public AbstractLifecycleNode const State & get_current_state(); + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_available_states(); + + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_available_transitions(); + /// trigger the specified transition /* * return the new state after this transition diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp index 30706eb954..8478285ea0 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp @@ -17,6 +17,7 @@ #include +#include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/visibility_control.h" // forward declare rcl_transition_t @@ -36,6 +37,11 @@ class Transition RCLCPP_LIFECYCLE_PUBLIC explicit Transition(unsigned int id, const std::string & label = ""); + RCLCPP_LIFECYCLE_PUBLIC + Transition( + unsigned int id, const std::string & label, + State && start, State && goal); + RCLCPP_LIFECYCLE_PUBLIC explicit Transition(const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle); @@ -50,8 +56,17 @@ class Transition std::string label() const; + RCLCPP_LIFECYCLE_PUBLIC + State + start_state() const; + + RCLCPP_LIFECYCLE_PUBLIC + State + goal_state() const; + protected: bool owns_rcl_transition_handle_; + const rcl_lifecycle_transition_t * transition_handle_; }; diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 99467f4b80..291f666aa3 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -20,6 +20,7 @@ #include #include +#include namespace rclcpp { @@ -87,6 +88,18 @@ LifecycleNode::get_current_state() return impl_->get_current_state(); } +std::vector +LifecycleNode::get_available_states() +{ + return impl_->get_available_states(); +} + +std::vector +LifecycleNode::get_available_transitions() +{ + return impl_->get_available_transitions(); +} + const State & LifecycleNode::trigger_transition(const Transition & transition) { diff --git a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp index 8bfb01e1d4..1c8679d248 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp @@ -237,6 +237,33 @@ class LifecycleNode::LifecycleNodeImpl return current_state_; } + std::vector + get_available_states() + { + std::vector states; + for (unsigned int i = 0; i < state_machine_.transition_map.size; ++i) { + State state(&state_machine_.transition_map.states[i]); + states.push_back(state); + } + return states; + } + + std::vector + get_available_transitions() + { + std::vector transitions; + + for (unsigned int i = 0; i < state_machine_.transition_map.size; ++i) { + // get transitions associated to each primary state + for (unsigned int j = 0; j < state_machine_.transition_map.transition_arrays[i].size; ++j) { + Transition transition( + &state_machine_.transition_map.transition_arrays[i].transitions[j]); + transitions.push_back(transition); + } + } + return transitions; + } + bool change_state(std::uint8_t lifecycle_transition) { diff --git a/rclcpp_lifecycle/src/transition.cpp b/rclcpp_lifecycle/src/transition.cpp index 72ad595dd2..1d77f9a60e 100644 --- a/rclcpp_lifecycle/src/transition.cpp +++ b/rclcpp_lifecycle/src/transition.cpp @@ -31,6 +31,30 @@ Transition::Transition(unsigned int id, const std::string & label) transition_handle->id = id; transition_handle->label = label.c_str(); + transition_handle->start = nullptr; + transition_handle->goal = nullptr; + transition_handle_ = transition_handle; +} + +Transition::Transition( + unsigned int id, const std::string & label, + State && start, State && goal) +: owns_rcl_transition_handle_(true) +{ + auto transition_handle = new rcl_lifecycle_transition_t; + transition_handle->id = id; + transition_handle->label = label.c_str(); + + auto start_state = new rcl_lifecycle_state_t; + start_state->id = start.id(); + start_state->label = start.label().c_str(); + + auto goal_state = new rcl_lifecycle_state_t; + goal_state->id = goal.id(); + goal_state->label = start.label().c_str(); + + transition_handle->start = start_state; + transition_handle->goal = goal_state; transition_handle_ = transition_handle; } @@ -43,6 +67,12 @@ Transition::Transition(const rcl_lifecycle_transition_t * rcl_lifecycle_transiti Transition::~Transition() { if (owns_rcl_transition_handle_) { + if (transition_handle_->start) { + delete transition_handle_->start; + } + if (transition_handle_->goal) { + delete transition_handle_->goal; + } delete transition_handle_; } } @@ -59,5 +89,21 @@ Transition::label() const return transition_handle_->label; } +State +Transition::start_state() const +{ + return State( + transition_handle_->start->id, + transition_handle_->start->label); +} + +State +Transition::goal_state() const +{ + return State( + transition_handle_->goal->id, + transition_handle_->goal->label); +} + } // namespace lifecycle } // namespace rclcpp diff --git a/rclcpp_lifecycle/test/test_state_machine_info.cpp b/rclcpp_lifecycle/test/test_state_machine_info.cpp new file mode 100644 index 0000000000..c12b8af584 --- /dev/null +++ b/rclcpp_lifecycle/test/test_state_machine_info.cpp @@ -0,0 +1,75 @@ +// 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. + + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +class TestStateMachineInfo : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +TEST_F(TestStateMachineInfo, available_states) { + auto test_node = std::make_shared("testnode"); + std::vector available_states = + test_node->get_available_states(); + EXPECT_EQ((unsigned int)11, available_states.size()); + + // Primary States + EXPECT_EQ((unsigned int)0, available_states[0].id()); // unknown + EXPECT_EQ((unsigned int)1, available_states[1].id()); // unconfigured + EXPECT_EQ((unsigned int)2, available_states[2].id()); // inactive + EXPECT_EQ((unsigned int)3, available_states[3].id()); // active + EXPECT_EQ((unsigned int)4, available_states[4].id()); // finalized + + // Transition States + EXPECT_EQ((unsigned int)10, available_states[5].id()); // configuring + EXPECT_EQ((unsigned int)11, available_states[6].id()); // cleaningup + EXPECT_EQ((unsigned int)12, available_states[7].id()); // shuttingdown + EXPECT_EQ((unsigned int)13, available_states[8].id()); // activating + EXPECT_EQ((unsigned int)14, available_states[9].id()); // deactivating + EXPECT_EQ((unsigned int)15, available_states[10].id()); // errorprocessing +} + +TEST_F(TestStateMachineInfo, available_transitions) { + auto test_node = std::make_shared("testnode"); + std::vector available_transitions = + test_node->get_available_transitions(); + EXPECT_EQ((unsigned int)14, available_transitions.size()); + for (rclcpp::lifecycle::Transition & transition : available_transitions) { + EXPECT_TRUE(transition.id() <= (unsigned int)5); + EXPECT_FALSE(transition.label().empty()); + + EXPECT_TRUE(transition.start_state().id() <= (unsigned int)4 || + (transition.start_state().id() >= (unsigned int)10 && + (transition.start_state().id() <= (unsigned int)15))); + EXPECT_FALSE(transition.start_state().label().empty()); + + EXPECT_TRUE(transition.goal_state().id() <= (unsigned int)4 || + (transition.goal_state().id() >= (unsigned int)10 && + (transition.goal_state().id() <= (unsigned int)15))); + EXPECT_FALSE(transition.goal_state().label().empty()); + } +} From dd1dc88ee7db9e2404be0f2104bfb88255589a05 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 9 Dec 2016 22:30:39 -0800 Subject: [PATCH 52/63] (broken) state after rebase, does not compile demos --- rclcpp/include/rclcpp/service.hpp | 28 +- rclcpp/src/rclcpp/service.cpp | 4 +- .../test/test_externally_defined_services.cpp | 3 +- rclcpp_lifecycle/CMakeLists.txt | 1 + .../rclcpp_lifecycle/lifecycle_executor.hpp | 45 --- .../rclcpp_lifecycle/lifecycle_manager.hpp | 170 -------- .../rclcpp_lifecycle/lifecycle_node.hpp | 378 ++++++++++++++---- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 206 ++++++++++ .../rclcpp_lifecycle/lifecycle_publisher.hpp | 11 +- .../lifecycle_node_interface.hpp | 82 ++++ .../include/rclcpp_lifecycle/state.hpp | 7 +- .../include/rclcpp_lifecycle/transition.hpp | 7 +- rclcpp_lifecycle/src/lifecycle_node.cpp | 203 +++++++++- rclcpp_lifecycle/src/lifecycle_node_impl.hpp | 70 ++-- .../src/lifecycle_node_interface.cpp | 59 +++ rclcpp_lifecycle/src/state.cpp | 7 +- rclcpp_lifecycle/src/transition.cpp | 7 +- rclcpp_lifecycle/test/test_lifecycle_node.cpp | 32 +- .../test/test_state_machine_info.cpp | 10 +- 19 files changed, 917 insertions(+), 413 deletions(-) delete mode 100644 rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_executor.hpp delete mode 100644 rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp create mode 100644 rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp create mode 100644 rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp create mode 100644 rclcpp_lifecycle/src/lifecycle_node_interface.cpp diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index c7d49c4f96..072c5d77dc 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -1,17 +1,3 @@ -// Copyright 2014 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 RCLCPP__SERVICE_HPP_ #define RCLCPP__SERVICE_HPP_ @@ -43,12 +29,12 @@ class ServiceBase RCLCPP_PUBLIC ServiceBase( - rcl_node_t * node_handle, + std::shared_ptr node_handle, const std::string & service_name); RCLCPP_PUBLIC explicit ServiceBase( - rcl_node_t * node_handle); + std::shared_ptr node_handle); RCLCPP_PUBLIC virtual ~ServiceBase(); @@ -70,7 +56,7 @@ class ServiceBase protected: RCLCPP_DISABLE_COPY(ServiceBase) - rcl_node_t * node_handle_; + std::shared_ptr node_handle_; rcl_service_t * service_handle_ = nullptr; std::string service_name_; @@ -100,7 +86,7 @@ class Service : public ServiceBase const std::string & service_name, AnyServiceCallback any_callback, rcl_service_options_t & service_options) - : ServiceBase(node_handle.get(), service_name), any_callback_(any_callback) + : ServiceBase(node_handle, service_name), any_callback_(any_callback) { using rosidl_typesupport_cpp::get_service_type_support_handle; auto service_type_support_handle = get_service_type_support_handle(); @@ -119,7 +105,7 @@ class Service : public ServiceBase } Service( - rcl_node_t * node_handle, + std::shared_ptr node_handle, rcl_service_t * service_handle, AnyServiceCallback any_callback) : ServiceBase(node_handle), @@ -132,7 +118,7 @@ class Service : public ServiceBase if (!service_handle->impl) { // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) throw std::runtime_error( - std::string("rcl_service_t in constructor argument must be initialized beforehand.")); + std::string("rcl_service_t in constructor argument must be initialized beforehand.")); // *INDENT-ON* } service_handle_ = service_handle; @@ -146,7 +132,7 @@ class Service : public ServiceBase { // check if you have ownership of the handle if (owns_rcl_handle_) { - if (rcl_service_fini(service_handle_, node_handle_) != RCL_RET_OK) { + if (rcl_service_fini(service_handle_, node_handle_.get()) != RCL_RET_OK) { std::stringstream ss; ss << "Error in destruction of rcl service_handle_ handle: " << rcl_get_error_string_safe() << '\n'; diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 29822df7a7..136d6883a0 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -28,12 +28,12 @@ using rclcpp::service::ServiceBase; ServiceBase::ServiceBase( - rcl_node_t * node_handle, + std::shared_ptr node_handle, const std::string & service_name) : node_handle_(node_handle), service_name_(service_name) {} -ServiceBase::ServiceBase(rcl_node_t * node_handle) +ServiceBase::ServiceBase(std::shared_ptr node_handle) : node_handle_(node_handle) {} diff --git a/rclcpp/test/test_externally_defined_services.cpp b/rclcpp/test/test_externally_defined_services.cpp index 8168054db7..efd05db1f6 100644 --- a/rclcpp/test/test_externally_defined_services.cpp +++ b/rclcpp/test/test_externally_defined_services.cpp @@ -47,7 +47,6 @@ TEST_F(TestExternallyDefinedServices, default_behavior) { try { auto srv = node_handle->create_service("test", callback); - EXPECT_STREQ(srv->get_service_name().c_str(), "test"); } catch (const std::exception &) { FAIL(); return; @@ -135,7 +134,7 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) { // Call destructor } - if (service_handle.impl == NULL) { + if (!service_handle.impl) { FAIL(); return; } diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index aba182eabc..fbcbfcd22f 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -23,6 +23,7 @@ macro(targets) add_library(rclcpp_lifecycle${target_suffix} SHARED src/lifecycle_node.cpp + src/lifecycle_node_interface.cpp src/state.cpp src/transition.cpp ) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_executor.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_executor.hpp deleted file mode 100644 index c59c2d2669..0000000000 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_executor.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2014 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 RCLCPP_LIFECYCLE__LIFECYCLE_EXECUTOR_HPP_ -#define RCLCPP_LIFECYCLE__LIFECYCLE_EXECUTOR_HPP_ - -#include - -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/type_traits/is_manageable_node.hpp" -#include "rclcpp/visibility_control.hpp" - -namespace rclcpp -{ -namespace executors -{ - -class LifecycleExecutor : public single_threaded_executor::SingleThreadedExecutor -{ -public: - explicit LifecycleExecutor(const executor::ExecutorArgs & args = - executor::create_default_executor_arguments()) - : SingleThreadedExecutor(args) - {} - - template::value>::type> - void - add_node(std::shared_ptr node_ptr, bool notify = true); -}; - -} // namespace executors -} // namespace rclcpp - -#endif // RCLCPP_LIFECYCLE__LIFECYCLE_EXECUTOR_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp deleted file mode 100644 index d8efbbf9a6..0000000000 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_manager.hpp +++ /dev/null @@ -1,170 +0,0 @@ -// 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 RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_HPP_ -#define RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_HPP_ - -#include -#include -#include - -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -namespace rclcpp -{ -namespace lifecycle -{ - -// *INDENT-OFF* -enum class LifecyclePrimaryStatesT : unsigned int -{ - UNKNOWN = 0, - UNCONFIGURED = 1, - INACTIVE = 2, - ACTIVE = 3, - FINALIZED = 4 -}; - -enum class LifecycleTransitionsT : unsigned int -{ - CONFIGURING = 10, - CLEANINGUP = 11, - SHUTTINGDOWN = 12, - ACTIVATING = 13, - DEACTIVATING = 14, - ERRORPROCESSING = 15 -}; - -class LifecycleManagerInterface -{ - virtual bool configure(const std::string& node_name = "") = 0; - virtual bool cleanup(const std::string& node_name = "") = 0; - virtual bool shutdown(const std::string& node_name = "") = 0; - virtual bool activate(const std::string& node_name = "") = 0; - virtual bool deactivate(const std::string& node_name = "") = 0; -}; -// *INDENT-ON* - -class LifecycleManager : public LifecycleManagerInterface -{ -public: - using NodeInterfacePtr = std::shared_ptr; - using NodePtr = std::shared_ptr; - - LIFECYCLE_EXPORT - LifecycleManager(); - - LIFECYCLE_EXPORT - ~LifecycleManager(); - - LIFECYCLE_EXPORT - std::shared_ptr - get_node_base_interface(); - - LIFECYCLE_EXPORT - void - add_node_interface(const NodePtr & node); - - LIFECYCLE_EXPORT - void - add_node_interface(const std::string & node_name, const NodeInterfacePtr & node_interface); - - template - bool - register_on_configure(const std::string & node_name, bool (T::* method)(void), T * instance) - { - auto cb = std::bind(method, instance); - return register_on_configure(node_name, cb); - } - - LIFECYCLE_EXPORT - bool - register_on_configure(const std::string & node_name, std::function & fcn); - - LIFECYCLE_EXPORT - bool - configure(const std::string & node_name = ""); - - template - bool - register_on_cleanup(const std::string & node_name, bool (T::* method)(void), T * instance) - { - auto cb = std::bind(method, instance); - return register_on_cleanup(node_name, cb); - } - - LIFECYCLE_EXPORT - bool - register_on_cleanup(const std::string & node_name, std::function & fcn); - - LIFECYCLE_EXPORT - bool - cleanup(const std::string & node_name = ""); - - template - bool - register_on_shutdown(const std::string & node_name, bool (T::* method)(void), T * instance) - { - auto cb = std::bind(method, instance); - return register_on_shutdown(node_name, cb); - } - - LIFECYCLE_EXPORT - bool - register_on_shutdown(const std::string & node_name, std::function & fcn); - - LIFECYCLE_EXPORT - bool - shutdown(const std::string & node_name = ""); - - template - bool - register_on_activate(const std::string & node_name, bool (T::* method)(void), T * instance) - { - auto cb = std::bind(method, instance); - return register_on_activate(node_name, cb); - } - - LIFECYCLE_EXPORT - bool - register_on_activate(const std::string & node_name, std::function & fcn); - - LIFECYCLE_EXPORT - bool - activate(const std::string & node_name = ""); - - template - bool - register_on_deactivate(const std::string & node_name, bool (T::* method)(void), T * instance) - { - auto cb = std::bind(method, instance); - return register_on_deactivate(node_name, cb); - } - - LIFECYCLE_EXPORT - bool - register_on_deactivate(const std::string & node_name, std::function & fcn); - - LIFECYCLE_EXPORT - bool - deactivate(const std::string & node_name = ""); - -private: - class LifecycleManagerImpl; - std::unique_ptr impl_; -}; - -} // namespace lifecycle -} // namespace rclcpp -#endif // RCLCPP_LIFECYCLE__LIFECYCLE_MANAGER_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 7a8b8f6918..c58360036a 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -16,120 +16,325 @@ #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ #include +#include #include #include -#include "rclcpp/node.hpp" +#include "rcl/error_handling.h" +#include "rcl/node.h" + +#include "rcl_interfaces/msg/list_parameters_result.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/context.hpp" +#include "rclcpp/event.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/message_memory_strategy.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription.hpp" #include "rclcpp/timer.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/lifecycle_node_impl.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/transition.hpp" #include "rclcpp_lifecycle/visibility_control.h" -namespace rclcpp -{ -namespace lifecycle -{ -/** - * @brief Interface class for a managed node. - * Pure virtual functions as defined in - * http://design.ros2.org/articles/node_lifecycle.html - */ -// *INDENT-OFF -class LifecycleNodeInterface -{ -public: - virtual bool on_configure() = 0; - virtual bool on_cleanup() = 0; - virtual bool on_shutdown() = 0; - virtual bool on_activate() = 0; - virtual bool on_deactivate() = 0; - virtual bool on_error() = 0; -}; - -class AbstractLifecycleNode : public LifecycleNodeInterface +namespace rclcpp_lifecycle { -public: - virtual bool on_configure() {return true;} - virtual bool on_cleanup() {return true;} - virtual bool on_shutdown() {return true;} - virtual bool on_activate() {return true;} - virtual bool on_deactivate() {return true;} - virtual bool on_error() {return true;} -}; -// *INDENT-ON +/// LifecycleNode for creating lifecycle components /** - * @brief LifecycleNode as child class of rclcpp Node * has lifecycle nodeinterface for configuring this node. */ -class LifecycleNode : public AbstractLifecycleNode +class LifecycleNode : public node_interfaces::LifecycleNodeInterface, + public std::enable_shared_from_this { public: + RCLCPP_SMART_PTR_DEFINITIONS(LifecycleNode) + + /// Create a new lifecycle node with the specified name. + /** + * \param[in] node_name Name of the node. + * \param[in] use_intra_process_comms True to use the optimized intra-process communication + * pipeline to pass messages between nodes in the same process using shared memory. + */ RCLCPP_LIFECYCLE_PUBLIC explicit LifecycleNode(const std::string & node_name, bool use_intra_process_comms = false); + /// Create a node based on the node name and a rclcpp::context::Context. + /** + * \param[in] node_name Name of the node. + * \param[in] context The context for the node (usually represents the state of a process). + * \param[in] use_intra_process_comms True to use the optimized intra-process communication + * pipeline to pass messages between nodes in the same process using shared memory. + */ + RCLCPP_LIFECYCLE_PUBLIC + LifecycleNode( + const std::string & node_name, rclcpp::context::Context::SharedPtr context, + bool use_intra_process_comms = false); + RCLCPP_LIFECYCLE_PUBLIC virtual ~LifecycleNode(); - // MOCK typedefs as node refactor not done yet - using BaseInterface = rclcpp::node::Node; + /// Get the name of the node. + // \return The name of the node. RCLCPP_LIFECYCLE_PUBLIC - std::shared_ptr - get_base_interface() - { - return base_node_handle_; - } + const char * + get_name() const; - // MOCK typedefs as node refactor not done yet - using CommunicationInterface = rclcpp::node::Node; + /// Create and return a callback group. RCLCPP_LIFECYCLE_PUBLIC - std::shared_ptr - get_communication_interface() - { - return communication_interface_; - } + rclcpp::callback_group::CallbackGroup::SharedPtr + create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); + /// Return the list of callback groups in the node. RCLCPP_LIFECYCLE_PUBLIC - std::string - get_name() - { - return base_node_handle_->get_name(); - } + const std::vector & + get_callback_groups() const; + + /// Create and return a Publisher. + /** + * \param[in] topic_name The topic for this publisher to publish on. + * \param[in] qos_history_depth The depth of the publisher message queue. + * \return Shared pointer to the created publisher. + */ + template> + std::shared_ptr> + create_publisher( + const std::string & topic_name, size_t qos_history_depth, + std::shared_ptr allocator = nullptr); + /// Create and return a LifecyclePublisher. /** - * @brief same API for creating publisher as regular Node + * \param[in] topic_name The topic for this publisher to publish on. + * \param[in] qos_history_depth The depth of the publisher message queue. + * \return Shared pointer to the created publisher. */ template> - std::shared_ptr> + std::shared_ptr> create_publisher( const std::string & topic_name, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, - std::shared_ptr allocator = nullptr) - { - // create regular publisher in rclcpp::Node - auto pub = communication_interface_->create_publisher>( - topic_name, qos_profile, allocator); - add_publisher_handle(pub); - - return pub; - } - - template - typename rclcpp::timer::WallTimer::SharedPtr + std::shared_ptr allocator = nullptr); + + /// Create and return a Subscription. + /** + * \param[in] topic_name The topic to subscribe on. + * \param[in] callback The user-defined callback function. + * \param[in] qos_profile The quality of service profile to pass on to the rmw implementation. + * \param[in] group The callback group for this subscription. NULL for no callback group. + * \param[in] ignore_local_publications True to ignore local publications. + * \param[in] msg_mem_strat The message memory strategy to use for allocating messages. + * \return Shared pointer to the created subscription. + */ + /* TODO(jacquelinekay): + Windows build breaks when static member function passed as default + argument to msg_mem_strat, nullptr is a workaround. + */ + template< + typename MessageT, + typename CallbackT, + typename Alloc = std::allocator, + typename SubscriptionT = rclcpp::subscription::Subscription> + typename rclcpp::subscription::Subscription::SharedPtr + create_subscription( + const std::string & topic_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, + bool ignore_local_publications = false, + typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr + msg_mem_strat = nullptr, + std::shared_ptr allocator = nullptr); + + /// Create and return a Subscription. + /** + * \param[in] topic_name The topic to subscribe on. + * \param[in] qos_history_depth The depth of the subscription's incoming message queue. + * \param[in] callback The user-defined callback function. + * \param[in] group The callback group for this subscription. NULL for no callback group. + * \param[in] ignore_local_publications True to ignore local publications. + * \param[in] msg_mem_strat The message memory strategy to use for allocating messages. + * \return Shared pointer to the created subscription. + */ + /* TODO(jacquelinekay): + Windows build breaks when static member function passed as default + argument to msg_mem_strat, nullptr is a workaround. + */ + template< + typename MessageT, + typename CallbackT, + typename Alloc = std::allocator, + typename SubscriptionT = rclcpp::subscription::Subscription> + typename rclcpp::subscription::Subscription::SharedPtr + create_subscription( + const std::string & topic_name, + size_t qos_history_depth, + CallbackT && callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, + bool ignore_local_publications = false, + typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr + msg_mem_strat = nullptr, + std::shared_ptr allocator = nullptr); + + /// Create a timer. + /** + * \param[in] period Time interval between triggers of the callback. + * \param[in] callback User-defined callback function. + * \param[in] group Callback group to execute this timer's callback in. + */ + template + typename rclcpp::timer::WallTimer::SharedPtr create_wall_timer( - std::chrono::nanoseconds period, - CallbackType callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) - { - auto timer = communication_interface_->create_wall_timer(period, callback, group); - add_timer_handle(timer); + std::chrono::duration period, + CallbackT callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + + /* Create and return a Client. */ + template + typename rclcpp::client::Client::SharedPtr + create_client( + const std::string & service_name, + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + + /* Create and return a Service. */ + template + typename rclcpp::service::Service::SharedPtr + create_service( + const std::string & service_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + + RCLCPP_LIFECYCLE_PUBLIC + std::vector + set_parameters(const std::vector & parameters); + + RCLCPP_LIFECYCLE_PUBLIC + rcl_interfaces::msg::SetParametersResult + set_parameters_atomically(const std::vector & parameters); + + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_parameters(const std::vector & names) const; - return timer; - } + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::parameter::ParameterVariant + get_parameter(const std::string & name) const; + + RCLCPP_LIFECYCLE_PUBLIC + bool + get_parameter( + const std::string & name, + rclcpp::parameter::ParameterVariant & parameter) const; + + template + bool + get_parameter(const std::string & name, ParameterT & parameter) const; + + RCLCPP_LIFECYCLE_PUBLIC + std::vector + describe_parameters(const std::vector & names) const; + + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_parameter_types(const std::vector & names) const; + + RCLCPP_LIFECYCLE_PUBLIC + rcl_interfaces::msg::ListParametersResult + list_parameters(const std::vector & prefixes, uint64_t depth) const; + + /// Register the callback for parameter changes + /** + * \param[in] User defined callback function, It is expected to atomically set parameters. + * \note Repeated invocations of this function will overwrite previous callbacks + */ + template + void + register_param_change_callback(CallbackT && callback); + + RCLCPP_LIFECYCLE_PUBLIC + std::map + get_topic_names_and_types() const; + + RCLCPP_LIFECYCLE_PUBLIC + size_t + count_publishers(const std::string & topic_name) const; + + RCLCPP_LIFECYCLE_PUBLIC + size_t + count_subscribers(const std::string & topic_name) const; + + /// Return a graph event, which will be set anytime a graph change occurs. + /* The graph Event object is a loan which must be returned. + * The Event object is scoped and therefore to return the load just let it go + * out of scope. + */ + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::event::Event::SharedPtr + get_graph_event(); + + /// Wait for a graph event to occur by waiting on an Event to become set. + /* The given Event must be acquire through the get_graph_event() method. + * + * \throws InvalidEventError if the given event is nullptr + * \throws EventNotRegisteredError if the given event was not acquired with + * get_graph_event(). + */ + RCLCPP_LIFECYCLE_PUBLIC + void + wait_for_graph_change( + rclcpp::event::Event::SharedPtr event, + std::chrono::nanoseconds timeout); + + /// Return the Node's internal NodeBaseInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_base_interface(); + + /// Return the Node's internal NodeGraphInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr + get_node_graph_interface(); + + /// Return the Node's internal NodeTimersInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr + get_node_timers_interface(); + /// Return the Node's internal NodeTopicsInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr + get_node_topics_interface(); + + /// Return the Node's internal NodeServicesInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr + get_node_services_interface(); + + /// Return the Node's internal NodeParametersInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr + get_node_parameters_interface(); + + // + // LIFECYCLE COMPONENTS + // RCLCPP_LIFECYCLE_PUBLIC const State & get_current_state(); @@ -176,20 +381,31 @@ class LifecycleNode : public AbstractLifecycleNode protected: RCLCPP_LIFECYCLE_PUBLIC void - add_publisher_handle(std::shared_ptr pub); + add_publisher_handle(std::shared_ptr pub); RCLCPP_LIFECYCLE_PUBLIC void add_timer_handle(std::shared_ptr timer); private: - std::shared_ptr base_node_handle_; - std::shared_ptr communication_interface_; + RCLCPP_DISABLE_COPY(LifecycleNode) + + RCLCPP_LIFECYCLE_PUBLIC + bool + group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group); + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + + bool use_intra_process_comms_; class LifecycleNodeImpl; std::unique_ptr impl_; }; -} // namespace lifecycle -} // namespace rclcpp +} // namespace rclcpp_lifecycle #endif // RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp new file mode 100644 index 0000000000..4107d1c1d4 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -0,0 +1,206 @@ +// 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 RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ + +#include +#include +#include + +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/create_publisher.hpp" +#include "rclcpp/create_subscription.hpp" +#include "rclcpp/type_support_decl.hpp" + +#include "lifecycle_publisher.hpp" +#include "rclcpp_lifecycle/visibility_control.h" + +#include "lifecycle_node.hpp" + +namespace rclcpp_lifecycle +{ + +template +std::shared_ptr> +LifecycleNode::create_publisher( + const std::string & topic_name, size_t qos_history_depth, + std::shared_ptr allocator) +{ + if (!allocator) { + allocator = std::make_shared(); + } + rmw_qos_profile_t qos = rmw_qos_profile_default; + qos.depth = qos_history_depth; + return this->create_publisher(topic_name, qos, allocator); +} + +template +std::shared_ptr> +LifecycleNode::create_publisher( + const std::string & topic_name, + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, + std::shared_ptr allocator = nullptr) +{ + // create regular publisher in rclcpp::Node + auto pub = rclcpp::create_publisher>( + topic_name, qos_profile, allocator); + add_publisher_handle(pub); + + return pub; +} + +// TODO(karsten1987): Create LifecycleSubscriber +template +typename rclcpp::subscription::Subscription::SharedPtr +LifecycleNode::create_subscription( + const std::string & topic_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile, + rclcpp::callback_group::CallbackGroup::SharedPtr group, + bool ignore_local_publications, + typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr + msg_mem_strat, + std::shared_ptr allocator) +{ + if (!allocator) { + allocator = std::make_shared(); + } + + if (!msg_mem_strat) { + using rclcpp::message_memory_strategy::MessageMemoryStrategy; + msg_mem_strat = MessageMemoryStrategy::create_default(); + } + + return rclcpp::create_subscription< + MessageT, CallbackT, Alloc, + rclcpp::subscription::Subscription>( + this->node_topics_.get(), + topic_name, + std::forward(callback), + qos_profile, + group, + ignore_local_publications, + use_intra_process_comms_, + msg_mem_strat, + allocator); +} + +template +typename rclcpp::subscription::Subscription::SharedPtr +LifecycleNode::create_subscription( + const std::string & topic_name, + size_t qos_history_depth, + CallbackT && callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group, + bool ignore_local_publications, + typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr + msg_mem_strat, + std::shared_ptr allocator) +{ + rmw_qos_profile_t qos = rmw_qos_profile_default; + qos.depth = qos_history_depth; + return this->create_subscription( + topic_name, + std::forward(callback), + qos, + group, + ignore_local_publications, + msg_mem_strat, + allocator); +} + +template +typename rclcpp::timer::WallTimer::SharedPtr +LifecycleNode::create_wall_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + auto timer = rclcpp::timer::WallTimer::make_shared( + std::chrono::duration_cast(period), + std::move(callback)); + node_timers_->add_timer(timer, group); + return timer; +} + +template +typename client::Client::SharedPtr +LifecycleNode::create_client( + const std::string & service_name, + const rmw_qos_profile_t & qos_profile, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + rcl_client_options_t options = rcl_client_get_default_options(); + options.qos = qos_profile; + + using rclcpp::client::Client; + using rclcpp::client::ClientBase; + + auto cli = Client::make_shared( + node_base_->get(), + node_graph_, + service_name, + options); + + auto cli_base_ptr = std::dynamic_pointer_cast(cli); + node_services_->add_client(cli_base_ptr, group); + return cli; +} + +template +typename rclcpp::service::Service::SharedPtr +LifecycleNode::create_service( + const std::string & service_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + rclcpp::service::AnyServiceCallback any_service_callback; + any_service_callback.set(std::forward(callback)); + + rcl_service_options_t service_options = rcl_service_get_default_options(); + service_options.qos = qos_profile; + + auto serv = service::Service::make_shared( + node_base_->get_shared_rcl_node_handle(), + service_name, any_service_callback, service_options); + auto serv_base_ptr = std::dynamic_pointer_cast(serv); + node_services_->add_service(serv_base_ptr, group); + return serv; +} + +template +bool +LifecycleNode::get_parameter(const std::string & name, ParameterT & parameter) const +{ + rclcpp::parameter::ParameterVariant parameter_variant(name, parameter); + bool result = get_parameter(name, parameter_variant); + parameter = parameter_variant.get_value(); + + return result; +} + +template +void +LifecycleNode::register_param_change_callback(CallbackT && callback) +{ + this->node_parameters_->register_param_change_callback(std::forward(callback)); +} + +} // namespace rclcpp_lifecycle +#endif // RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index 550fb2406a..1cabab6fb9 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -20,9 +20,7 @@ #include "rclcpp/publisher.hpp" -namespace rclcpp -{ -namespace lifecycle +namespace rclcpp_lifecycle { /** * @brief base class with only @@ -49,9 +47,9 @@ class LifecyclePublisher : public LifecyclePublisherInterface, public rclcpp::publisher::Publisher { public: - using MessageAllocTraits = allocator::AllocRebind; + using MessageAllocTraits = rclcpp::allocator::AllocRebind; using MessageAlloc = typename MessageAllocTraits::allocator_type; - using MessageDeleter = allocator::Deleter; + using MessageDeleter = rclcpp::allocator::Deleter; using MessageUniquePtr = std::unique_ptr; LifecyclePublisher( @@ -160,7 +158,6 @@ class LifecyclePublisher : public LifecyclePublisherInterface, bool enabled_ = false; }; -} // namespace lifecycle -} // namespace rclcpp +} // namespace rclcpp_lifecycle #endif // RCLCPP_LIFECYCLE__LIFECYCLE_PUBLISHER_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp new file mode 100644 index 0000000000..ead1b60d6e --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -0,0 +1,82 @@ +// 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 RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_ +#define RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_ + +#include "rclcpp_lifecycle/visibility_control.h" + +namespace rclcpp_lifecycle +{ +namespace node_interfaces +{ +/// Interface class for a managed node. +/** Virtual functions as defined in + * http://design.ros2.org/articles/node_lifecycle.html + * + * If the callback function returns successfully, + * the specified transition is completed. + * If the callback function fails or throws an + * uncaught exception, the on_error function is + * called. + * By default, all functions remain optional to overwrite + * and return true. Except the on_error function, which + * returns false and thus goes to shutdown/finalize state. + */ +class LifecycleNodeInterface +{ +protected: + LifecycleNodeInterface() {} + +public: + /// Callback function for configure transition + /* + * \return true by default + */ + virtual bool on_configure(); + + /// Callback function for cleanup transition + /* + * \return true by default + */ + virtual bool on_cleanup(); + + /// Callback function for shutdown transition + /* + * \return true by default + */ + virtual bool on_shutdown(); + + /// Callback function for activate transition + /* + * \return true by default + */ + virtual bool on_activate(); + + /// Callback function for deactivate transition + /* + * \return true by default + */ + virtual bool on_deactivate(); + + /// Callback function for errorneous transition + /* + * \return false by default + */ + virtual bool on_error(); +}; + +} // namespace node_interfaces +} // namespace rclcpp_lifecycle +#endif // RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp index 9c6f14ceb7..3b2d3e294c 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -22,9 +22,7 @@ // forward declare rcl_state_t typedef struct rcl_lifecycle_state_t rcl_lifecycle_state_t; -namespace rclcpp -{ -namespace lifecycle +namespace rclcpp_lifecycle { class State @@ -55,6 +53,5 @@ class State const rcl_lifecycle_state_t * state_handle_; }; -} // namespace lifecycle -} // namespace rclcpp +} // namespace rclcpp_lifecycle #endif // RCLCPP_LIFECYCLE__STATE_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp index 8478285ea0..35bb2960d1 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp @@ -23,9 +23,7 @@ // forward declare rcl_transition_t typedef struct rcl_lifecycle_transition_t rcl_lifecycle_transition_t; -namespace rclcpp -{ -namespace lifecycle +namespace rclcpp_lifecycle { class Transition @@ -70,6 +68,5 @@ class Transition const rcl_lifecycle_transition_t * transition_handle_; }; -} // namespace lifecycle -} // namespace rclcpp +} // namespace rclcpp_lifecycle #endif // RCLCPP_LIFECYCLE__TRANSITION_HPP_ diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 291f666aa3..0dec868b6d 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -13,24 +13,53 @@ // limitations under the License. #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "lifecycle_node_impl.hpp" // implementation - -#include -#include #include +#include #include #include +#include -namespace rclcpp -{ -namespace lifecycle +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/graph_listener.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base.hpp" +#include "rclcpp/node_interfaces/node_graph.hpp" +#include "rclcpp/node_interfaces/node_parameters.hpp" +#include "rclcpp/node_interfaces/node_services.hpp" +#include "rclcpp/node_interfaces/node_timers.hpp" +#include "rclcpp/node_interfaces/node_topics.hpp" + +#include "lifecycle_node_impl.hpp" // implementation + +namespace rclcpp_lifecycle { LifecycleNode::LifecycleNode(const std::string & node_name, bool use_intra_process_comms) -: base_node_handle_(std::make_shared(node_name, use_intra_process_comms)), - communication_interface_(base_node_handle_), - impl_(new LifecycleNodeImpl(base_node_handle_)) +: LifecycleNode( + node_name, + rclcpp::contexts::default_context::get_global_default_context(), + use_intra_process_comms) +{} + +LifecycleNode::LifecycleNode( + const std::string & node_name, + rclcpp::context::Context::SharedPtr context, + bool use_intra_process_comms) +: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, context)), + node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), + node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), + node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())), + node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), + node_parameters_(new rclcpp::node_interfaces::NodeParameters( + node_topics_.get(), + use_intra_process_comms + )), + use_intra_process_comms_(use_intra_process_comms), + impl_(new LifecycleNodeImpl(node_base_, node_services_)) { impl_->init(); @@ -45,6 +74,155 @@ LifecycleNode::LifecycleNode(const std::string & node_name, bool use_intra_proce LifecycleNode::~LifecycleNode() {} +const char * +LifecycleNode::get_name() const +{ + return node_base_->get_name(); +} + +rclcpp::callback_group::CallbackGroup::SharedPtr +LifecycleNode::create_callback_group( + rclcpp::callback_group::CallbackGroupType group_type) +{ + return node_base_->create_callback_group(group_type); +} + +bool +LifecycleNode::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + return node_base_->callback_group_in_node(group); +} + +std::vector +LifecycleNode::set_parameters( + const std::vector & parameters) +{ + return node_parameters_->set_parameters(parameters); +} + +rcl_interfaces::msg::SetParametersResult +LifecycleNode::set_parameters_atomically( + const std::vector & parameters) +{ + return node_parameters_->set_parameters_atomically(parameters); +} + +std::vector +LifecycleNode::get_parameters( + const std::vector & names) const +{ + return node_parameters_->get_parameters(names); +} + +rclcpp::parameter::ParameterVariant +LifecycleNode::get_parameter(const std::string & name) const +{ + return node_parameters_->get_parameter(name); +} + +bool LifecycleNode::get_parameter(const std::string & name, + rclcpp::parameter::ParameterVariant & parameter) const +{ + return node_parameters_->get_parameter(name, parameter); +} + +std::vector +LifecycleNode::describe_parameters( + const std::vector & names) const +{ + return node_parameters_->describe_parameters(names); +} + +std::vector +LifecycleNode::get_parameter_types( + const std::vector & names) const +{ + return node_parameters_->get_parameter_types(names); +} + +rcl_interfaces::msg::ListParametersResult +LifecycleNode::list_parameters( + const std::vector & prefixes, uint64_t depth) const +{ + return node_parameters_->list_parameters(prefixes, depth); +} + +std::map +LifecycleNode::get_topic_names_and_types() const +{ + return node_graph_->get_topic_names_and_types(); +} + +size_t +LifecycleNode::count_publishers(const std::string & topic_name) const +{ + return node_graph_->count_publishers(topic_name); +} + +size_t +LifecycleNode::count_subscribers(const std::string & topic_name) const +{ + return node_graph_->count_subscribers(topic_name); +} + +const std::vector & +LifecycleNode::get_callback_groups() const +{ + return node_base_->get_callback_groups(); +} + +rclcpp::event::Event::SharedPtr +LifecycleNode::get_graph_event() +{ + return node_graph_->get_graph_event(); +} + +void +LifecycleNode::wait_for_graph_change( + rclcpp::event::Event::SharedPtr event, + std::chrono::nanoseconds timeout) +{ + node_graph_->wait_for_graph_change(event, timeout); +} + +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr +LifecycleNode::get_node_base_interface() +{ + return node_base_; +} + +rclcpp::node_interfaces::NodeGraphInterface::SharedPtr +LifecycleNode::get_node_graph_interface() +{ + return node_graph_; +} + +rclcpp::node_interfaces::NodeTimersInterface::SharedPtr +LifecycleNode::get_node_timers_interface() +{ + return node_timers_; +} + +rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr +LifecycleNode::get_node_topics_interface() +{ + return node_topics_; +} + +rclcpp::node_interfaces::NodeServicesInterface::SharedPtr +LifecycleNode::get_node_services_interface() +{ + return node_services_; +} + +rclcpp::node_interfaces::NodeParametersInterface::SharedPtr +LifecycleNode::get_node_parameters_interface() +{ + return node_parameters_; +} + + +//// bool LifecycleNode::register_on_configure(std::function fcn) { @@ -114,7 +292,7 @@ LifecycleNode::trigger_transition(unsigned int transition_id) void LifecycleNode::add_publisher_handle( - std::shared_ptr pub) + std::shared_ptr pub) { impl_->add_publisher_handle(pub); } @@ -125,5 +303,4 @@ LifecycleNode::add_timer_handle(std::shared_ptr timer) impl_->add_timer_handle(timer); } -} // namespace lifecycle -} // namespace rclcpp +} // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp index 1c8679d248..5fc95303c1 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_impl.hpp @@ -24,23 +24,22 @@ #include #include +#include "lifecycle_msgs/msg/transition_description.hpp" +#include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport +#include "lifecycle_msgs/msg/transition_event.hpp" #include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/get_state.hpp" #include "lifecycle_msgs/srv/get_available_states.hpp" #include "lifecycle_msgs/srv/get_available_transitions.hpp" -#include "lifecycle_msgs/msg/transition_description.hpp" -#include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport -#include "lifecycle_msgs/msg/transition_event.hpp" #include "rcl/error_handling.h" #include "rcl_lifecycle/rcl_lifecycle.h" -#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" -namespace rclcpp -{ -namespace lifecycle +namespace rclcpp_lifecycle { class LifecycleNode::LifecycleNodeImpl @@ -52,8 +51,11 @@ class LifecycleNode::LifecycleNodeImpl using TransitionEventMsg = lifecycle_msgs::msg::TransitionEvent; public: - explicit LifecycleNodeImpl(std::shared_ptr(base_node_handle)) - : base_node_handle_(base_node_handle) + LifecycleNodeImpl( + std::shared_ptr node_base_interface, + std::shared_ptr node_services_interface) + : node_base_interface_(node_base_interface), + node_services_interface_(node_services_interface) {} ~LifecycleNodeImpl() @@ -62,7 +64,8 @@ class LifecycleNode::LifecycleNodeImpl fprintf(stderr, "%s:%u, FATAL: rcl_state_machine got destroyed externally.\n", __FILE__, __LINE__); } else { - rcl_lifecycle_state_machine_fini(&state_machine_, base_node_handle_->get_rcl_node_handle()); + rcl_lifecycle_state_machine_fini(&state_machine_, + node_base_interface_->get_rcl_node_handle()); } } @@ -71,7 +74,7 @@ class LifecycleNode::LifecycleNodeImpl { state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); rcl_ret_t ret = rcl_lifecycle_state_machine_init( - &state_machine_, base_node_handle_->get_rcl_node_handle(), + &state_machine_, node_base_interface_->get_rcl_node_handle(), ROSIDL_GET_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), rosidl_typesupport_cpp::get_service_type_support_handle(), rosidl_typesupport_cpp::get_service_type_support_handle(), @@ -80,7 +83,7 @@ class LifecycleNode::LifecycleNodeImpl true); if (ret != RCL_RET_OK) { fprintf(stderr, "Error adding %s: %s\n", - base_node_handle_->get_name().c_str(), rcl_get_error_string_safe()); + node_base_interface_->get_name(), rcl_get_error_string_safe()); return; } @@ -91,11 +94,12 @@ class LifecycleNode::LifecycleNodeImpl any_cb.set(cb); srv_change_state_ = std::make_shared>( - base_node_handle_->get_shared_node_handle(), + node_base_interface_->get_shared_rcl_node_handle(), &state_machine_.com_interface.srv_change_state, any_cb); - base_node_handle_->add_service( - std::dynamic_pointer_cast(srv_change_state_)); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_change_state_), + nullptr); } { // get_state @@ -105,11 +109,12 @@ class LifecycleNode::LifecycleNodeImpl any_cb.set(std::move(cb)); srv_get_state_ = std::make_shared>( - base_node_handle_->get_shared_node_handle(), + node_base_interface_->get_shared_rcl_node_handle(), &state_machine_.com_interface.srv_get_state, any_cb); - base_node_handle_->add_service( - std::dynamic_pointer_cast(srv_get_state_)); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_state_), + nullptr); } { // get_available_states @@ -119,11 +124,12 @@ class LifecycleNode::LifecycleNodeImpl any_cb.set(std::move(cb)); srv_get_available_states_ = std::make_shared>( - base_node_handle_->get_shared_node_handle(), + node_base_interface_->get_shared_rcl_node_handle(), &state_machine_.com_interface.srv_get_available_states, any_cb); - base_node_handle_->add_service( - std::dynamic_pointer_cast(srv_get_available_states_)); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_available_states_), + nullptr); } { // get_available_transitions @@ -134,11 +140,12 @@ class LifecycleNode::LifecycleNodeImpl srv_get_available_transitions_ = std::make_shared>( - base_node_handle_->get_shared_node_handle(), + node_base_interface_->get_shared_rcl_node_handle(), &state_machine_.com_interface.srv_get_available_transitions, any_cb); - base_node_handle_->add_service( - std::dynamic_pointer_cast(srv_get_available_transitions_)); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_available_transitions_), + nullptr); } } @@ -269,7 +276,7 @@ class LifecycleNode::LifecycleNodeImpl { if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { fprintf(stderr, "%s:%d, Unable to change state for state machine for %s: %s \n", - __FILE__, __LINE__, base_node_handle_->get_name().c_str(), rcl_get_error_string_safe()); + __FILE__, __LINE__, node_base_interface_->get_name(), rcl_get_error_string_safe()); return false; } @@ -339,7 +346,7 @@ class LifecycleNode::LifecycleNodeImpl } void - add_publisher_handle(std::shared_ptr pub) + add_publisher_handle(std::shared_ptr pub) { weak_pubs_.push_back(pub); } @@ -354,7 +361,8 @@ class LifecycleNode::LifecycleNodeImpl State current_state_; std::map> cb_map_; - using BaseNodePtr = std::shared_ptr; + using NodeBasePtr = std::shared_ptr; + using NodeServicesPtr = std::shared_ptr; using ChangeStateSrvPtr = std::shared_ptr>; using GetStateSrvPtr = std::shared_ptr>; using GetAvailableStatesSrvPtr = @@ -362,17 +370,17 @@ class LifecycleNode::LifecycleNodeImpl using GetAvailableTransitionsSrvPtr = std::shared_ptr>; - BaseNodePtr base_node_handle_; + NodeBasePtr node_base_interface_; + NodeServicesPtr node_services_interface_; ChangeStateSrvPtr srv_change_state_; GetStateSrvPtr srv_get_state_; GetAvailableStatesSrvPtr srv_get_available_states_; GetAvailableTransitionsSrvPtr srv_get_available_transitions_; // to controllable things - std::vector> weak_pubs_; + std::vector> weak_pubs_; std::vector> weak_timers_; }; -} // namespace lifecycle -} // namespace rclcpp +} // namespace rclcpp_lifecycle #endif // LIFECYCLE_NODE_IMPL_HPP_ diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface.cpp new file mode 100644 index 0000000000..68e0255376 --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_interface.cpp @@ -0,0 +1,59 @@ +// 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 "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +namespace rclcpp_lifecycle +{ +namespace node_interfaces +{ + +bool +LifecycleNodeInterface::on_configure() +{ + return true; +} + +bool +LifecycleNodeInterface::on_cleanup() +{ + return true; +} + +bool +LifecycleNodeInterface::on_shutdown() +{ + return true; +} + +bool +LifecycleNodeInterface::on_activate() +{ + return true; +} + +bool +LifecycleNodeInterface::on_deactivate() +{ + return true; +} + +bool +LifecycleNodeInterface::on_error() +{ + return false; +} + +} // namespace node_interfaces +} // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp index 3e3d432b03..40f8dd9d9e 100644 --- a/rclcpp_lifecycle/src/state.cpp +++ b/rclcpp_lifecycle/src/state.cpp @@ -19,9 +19,7 @@ #include -namespace rclcpp -{ -namespace lifecycle +namespace rclcpp_lifecycle { State::State() @@ -67,5 +65,4 @@ State::label() const return state_handle_->label; } -} // namespace lifecycle -} // namespace rclcpp +} // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/transition.cpp b/rclcpp_lifecycle/src/transition.cpp index 1d77f9a60e..6bf7434d9e 100644 --- a/rclcpp_lifecycle/src/transition.cpp +++ b/rclcpp_lifecycle/src/transition.cpp @@ -19,9 +19,7 @@ #include -namespace rclcpp -{ -namespace lifecycle +namespace rclcpp_lifecycle { Transition::Transition(unsigned int id, const std::string & label) @@ -105,5 +103,4 @@ Transition::goal_state() const transition_handle_->goal->label); } -} // namespace lifecycle -} // namespace rclcpp +} // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 3e7d2e722c..2b20eb8a32 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -52,20 +52,20 @@ class TestDefaultStateMachine : public ::testing::Test } }; -class EmptyLifecycleNode : public rclcpp::lifecycle::LifecycleNode +class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode { public: explicit EmptyLifecycleNode(std::string node_name) - : rclcpp::lifecycle::LifecycleNode(std::move(node_name)) + : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) {} }; template -class MoodyLifecycleNode : public rclcpp::lifecycle::LifecycleNode +class MoodyLifecycleNode : public rclcpp_lifecycle::LifecycleNode { public: explicit MoodyLifecycleNode(std::string node_name) - : rclcpp::lifecycle::LifecycleNode(std::move(node_name)) + : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) {} size_t number_of_callbacks = 0; @@ -142,15 +142,15 @@ TEST_F(TestDefaultStateMachine, trigger_transition) { EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); + rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); + rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); + rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); + rclcpp_lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); } TEST_F(TestDefaultStateMachine, good_mood) { @@ -158,15 +158,15 @@ TEST_F(TestDefaultStateMachine, good_mood) { EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); + rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); + rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); + rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); + rclcpp_lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); // check if all callbacks were successfully overwritten EXPECT_EQ(static_cast(5), test_node->number_of_callbacks); @@ -177,7 +177,7 @@ TEST_F(TestDefaultStateMachine, bad_mood) { EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); // check if all callbacks were successfully overwritten EXPECT_EQ(static_cast(2), test_node->number_of_callbacks); @@ -188,7 +188,7 @@ TEST_F(TestDefaultStateMachine, very_bad_mood) { EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( - rclcpp::lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); // check if all callbacks were successfully overwritten EXPECT_EQ(static_cast(2), test_node->number_of_callbacks); diff --git a/rclcpp_lifecycle/test/test_state_machine_info.cpp b/rclcpp_lifecycle/test/test_state_machine_info.cpp index c12b8af584..518f7b3105 100644 --- a/rclcpp_lifecycle/test/test_state_machine_info.cpp +++ b/rclcpp_lifecycle/test/test_state_machine_info.cpp @@ -32,8 +32,8 @@ class TestStateMachineInfo : public ::testing::Test }; TEST_F(TestStateMachineInfo, available_states) { - auto test_node = std::make_shared("testnode"); - std::vector available_states = + auto test_node = std::make_shared("testnode"); + std::vector available_states = test_node->get_available_states(); EXPECT_EQ((unsigned int)11, available_states.size()); @@ -54,11 +54,11 @@ TEST_F(TestStateMachineInfo, available_states) { } TEST_F(TestStateMachineInfo, available_transitions) { - auto test_node = std::make_shared("testnode"); - std::vector available_transitions = + auto test_node = std::make_shared("testnode"); + std::vector available_transitions = test_node->get_available_transitions(); EXPECT_EQ((unsigned int)14, available_transitions.size()); - for (rclcpp::lifecycle::Transition & transition : available_transitions) { + for (rclcpp_lifecycle::Transition & transition : available_transitions) { EXPECT_TRUE(transition.id() <= (unsigned int)5); EXPECT_FALSE(transition.label().empty()); From e28d800780a33b6856fdc4365acba8486dbad0d8 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sat, 10 Dec 2016 04:12:01 -0800 Subject: [PATCH 53/63] fix the way lifecycle node impl is included --- .../rclcpp_lifecycle/lifecycle_node.hpp | 8 +++-- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 34 +++++++++++-------- 2 files changed, 26 insertions(+), 16 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index c58360036a..63a3896a90 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -47,7 +47,6 @@ #include "rclcpp/timer.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/lifecycle_node_impl.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/transition.hpp" @@ -180,7 +179,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, typename CallbackT, typename Alloc = std::allocator, typename SubscriptionT = rclcpp::subscription::Subscription> - typename rclcpp::subscription::Subscription::SharedPtr + std::shared_ptr create_subscription( const std::string & topic_name, size_t qos_history_depth, @@ -408,4 +407,9 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, } // namespace rclcpp_lifecycle +#ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ +// Template implementations +#include "rclcpp_lifecycle/lifecycle_node_impl.hpp" +#endif + #endif // RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 4107d1c1d4..6d0f6568e7 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -29,7 +29,7 @@ #include "lifecycle_publisher.hpp" #include "rclcpp_lifecycle/visibility_control.h" -#include "lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" namespace rclcpp_lifecycle { @@ -52,16 +52,18 @@ template std::shared_ptr> LifecycleNode::create_publisher( const std::string & topic_name, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, - std::shared_ptr allocator = nullptr) + const rmw_qos_profile_t & qos_profile, + std::shared_ptr allocator) { // create regular publisher in rclcpp::Node - auto pub = rclcpp::create_publisher>( - topic_name, qos_profile, allocator); - add_publisher_handle(pub); + using PublisherT = rclcpp_lifecycle::LifecyclePublisher; - return pub; + return rclcpp::create_publisher( + this->node_topics_.get(), + topic_name, + qos_profile, + use_intra_process_comms_, + allocator); } // TODO(karsten1987): Create LifecycleSubscriber @@ -100,8 +102,12 @@ LifecycleNode::create_subscription( allocator); } -template -typename rclcpp::subscription::Subscription::SharedPtr +template< + typename MessageT, + typename CallbackT, + typename Alloc, + typename SubscriptionT> +std::shared_ptr LifecycleNode::create_subscription( const std::string & topic_name, size_t qos_history_depth, @@ -139,7 +145,7 @@ LifecycleNode::create_wall_timer( } template -typename client::Client::SharedPtr +typename rclcpp::client::Client::SharedPtr LifecycleNode::create_client( const std::string & service_name, const rmw_qos_profile_t & qos_profile, @@ -152,7 +158,7 @@ LifecycleNode::create_client( using rclcpp::client::ClientBase; auto cli = Client::make_shared( - node_base_->get(), + node_base_.get(), node_graph_, service_name, options); @@ -176,10 +182,10 @@ LifecycleNode::create_service( rcl_service_options_t service_options = rcl_service_get_default_options(); service_options.qos = qos_profile; - auto serv = service::Service::make_shared( + auto serv = rclcpp::service::Service::make_shared( node_base_->get_shared_rcl_node_handle(), service_name, any_service_callback, service_options); - auto serv_base_ptr = std::dynamic_pointer_cast(serv); + auto serv_base_ptr = std::dynamic_pointer_cast(serv); node_services_->add_service(serv_base_ptr, group); return serv; } From 1a615e13eccc5dc931af8307802f0269334053fd Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Sat, 10 Dec 2016 12:01:58 -0800 Subject: [PATCH 54/63] fixed rebase compilation errors --- rclcpp/include/rclcpp/service.hpp | 18 +++++++++++++ rclcpp_lifecycle/CMakeLists.txt | 2 +- .../rclcpp_lifecycle/lifecycle_node.hpp | 7 +++--- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 4 +-- .../rclcpp_lifecycle/lifecycle_publisher.hpp | 6 ++--- .../lifecycle_node_interface.hpp | 25 ++++++++++++++----- rclcpp_lifecycle/src/lifecycle_node.cpp | 4 +-- ....hpp => lifecycle_node_interface_impl.hpp} | 20 +++++++-------- .../lifecycle_node_interface.cpp | 0 9 files changed, 59 insertions(+), 27 deletions(-) rename rclcpp_lifecycle/src/{lifecycle_node_impl.hpp => lifecycle_node_interface_impl.hpp} (96%) rename rclcpp_lifecycle/src/{ => node_interfaces}/lifecycle_node_interface.cpp (100%) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 072c5d77dc..f65c730762 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -1,3 +1,21 @@ +// Copyright 2014 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. + +// Based on: http://the-witness.net/news/2012/11/scopeexit-in-c11/ +// But I changed the lambda to include by reference rather than value, see: +// http://the-witness.net/news/2012/11/scopeexit-in-c11/comment-page-1/#comment-86873 + #ifndef RCLCPP__SERVICE_HPP_ #define RCLCPP__SERVICE_HPP_ diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index fbcbfcd22f..a68ed7a673 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -23,7 +23,7 @@ macro(targets) add_library(rclcpp_lifecycle${target_suffix} SHARED src/lifecycle_node.cpp - src/lifecycle_node_interface.cpp + src/node_interfaces/lifecycle_node_interface.cpp src/state.cpp src/transition.cpp ) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 63a3896a90..fd3f4bd308 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -149,7 +149,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, typename CallbackT, typename Alloc = std::allocator, typename SubscriptionT = rclcpp::subscription::Subscription> - typename rclcpp::subscription::Subscription::SharedPtr + std::shared_ptr create_subscription( const std::string & topic_name, CallbackT && callback, @@ -401,8 +401,9 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; bool use_intra_process_comms_; - class LifecycleNodeImpl; - std::unique_ptr impl_; + + class LifecycleNodeInterfaceImpl; + std::unique_ptr impl_; }; } // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 6d0f6568e7..e038fe8fc2 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -55,9 +55,9 @@ LifecycleNode::create_publisher( const rmw_qos_profile_t & qos_profile, std::shared_ptr allocator) { - // create regular publisher in rclcpp::Node using PublisherT = rclcpp_lifecycle::LifecyclePublisher; + // create regular publisher in rclcpp::Node return rclcpp::create_publisher( this->node_topics_.get(), topic_name, @@ -68,7 +68,7 @@ LifecycleNode::create_publisher( // TODO(karsten1987): Create LifecycleSubscriber template -typename rclcpp::subscription::Subscription::SharedPtr +std::shared_ptr LifecycleNode::create_subscription( const std::string & topic_name, CallbackT && callback, diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index 1cabab6fb9..3b6c437b01 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -53,12 +53,12 @@ class LifecyclePublisher : public LifecyclePublisherInterface, using MessageUniquePtr = std::unique_ptr; LifecyclePublisher( - std::shared_ptr node_handle, - std::string topic, + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic, const rcl_publisher_options_t & publisher_options, std::shared_ptr allocator) : rclcpp::publisher::Publisher( - node_handle, topic, publisher_options, allocator), + node_base, topic, publisher_options, allocator), enabled_(false) {} diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp index ead1b60d6e..e041fd76ee 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -37,6 +37,7 @@ namespace node_interfaces class LifecycleNodeInterface { protected: + RCLCPP_LIFECYCLE_PUBLIC LifecycleNodeInterface() {} public: @@ -44,37 +45,49 @@ class LifecycleNodeInterface /* * \return true by default */ - virtual bool on_configure(); + RCLCPP_LIFECYCLE_PUBLIC + virtual bool + on_configure(); /// Callback function for cleanup transition /* * \return true by default */ - virtual bool on_cleanup(); + RCLCPP_LIFECYCLE_PUBLIC + virtual bool + on_cleanup(); /// Callback function for shutdown transition /* * \return true by default */ - virtual bool on_shutdown(); + RCLCPP_LIFECYCLE_PUBLIC + virtual bool + on_shutdown(); /// Callback function for activate transition /* * \return true by default */ - virtual bool on_activate(); + RCLCPP_LIFECYCLE_PUBLIC + virtual bool + on_activate(); /// Callback function for deactivate transition /* * \return true by default */ - virtual bool on_deactivate(); + RCLCPP_LIFECYCLE_PUBLIC + virtual bool + on_deactivate(); /// Callback function for errorneous transition /* * \return false by default */ - virtual bool on_error(); + RCLCPP_LIFECYCLE_PUBLIC + virtual bool + on_error(); }; } // namespace node_interfaces diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 0dec868b6d..753ae68c18 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -33,7 +33,7 @@ #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" -#include "lifecycle_node_impl.hpp" // implementation +#include "lifecycle_node_interface_impl.hpp" // implementation namespace rclcpp_lifecycle { @@ -59,7 +59,7 @@ LifecycleNode::LifecycleNode( use_intra_process_comms )), use_intra_process_comms_(use_intra_process_comms), - impl_(new LifecycleNodeImpl(node_base_, node_services_)) + impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) { impl_->init(); diff --git a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp similarity index 96% rename from rclcpp_lifecycle/src/lifecycle_node_impl.hpp rename to rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 5fc95303c1..e3051ed158 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIFECYCLE_NODE_IMPL_HPP_ -#define LIFECYCLE_NODE_IMPL_HPP_ +#ifndef LIFECYCLE_NODE_INTERFACE_IMPL_HPP_ +#define LIFECYCLE_NODE_INTERFACE_IMPL_HPP_ #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -42,7 +42,7 @@ namespace rclcpp_lifecycle { -class LifecycleNode::LifecycleNodeImpl +class LifecycleNode::LifecycleNodeInterfaceImpl { using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; using GetStateSrv = lifecycle_msgs::srv::GetState; @@ -51,14 +51,14 @@ class LifecycleNode::LifecycleNodeImpl using TransitionEventMsg = lifecycle_msgs::msg::TransitionEvent; public: - LifecycleNodeImpl( + LifecycleNodeInterfaceImpl( std::shared_ptr node_base_interface, std::shared_ptr node_services_interface) : node_base_interface_(node_base_interface), node_services_interface_(node_services_interface) {} - ~LifecycleNodeImpl() + ~LifecycleNodeInterfaceImpl() { if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { fprintf(stderr, "%s:%u, FATAL: rcl_state_machine got destroyed externally.\n", @@ -88,7 +88,7 @@ class LifecycleNode::LifecycleNodeImpl } { // change_state - auto cb = std::bind(&LifecycleNodeImpl::on_change_state, this, + auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_change_state, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); rclcpp::any_service_callback::AnyServiceCallback any_cb; any_cb.set(cb); @@ -103,7 +103,7 @@ class LifecycleNode::LifecycleNodeImpl } { // get_state - auto cb = std::bind(&LifecycleNodeImpl::on_get_state, this, + auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_get_state, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); rclcpp::any_service_callback::AnyServiceCallback any_cb; any_cb.set(std::move(cb)); @@ -118,7 +118,7 @@ class LifecycleNode::LifecycleNodeImpl } { // get_available_states - auto cb = std::bind(&LifecycleNodeImpl::on_get_available_states, this, + auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_get_available_states, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); rclcpp::any_service_callback::AnyServiceCallback any_cb; any_cb.set(std::move(cb)); @@ -133,7 +133,7 @@ class LifecycleNode::LifecycleNodeImpl } { // get_available_transitions - auto cb = std::bind(&LifecycleNodeImpl::on_get_available_transitions, this, + auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_get_available_transitions, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); rclcpp::any_service_callback::AnyServiceCallback any_cb; any_cb.set(std::move(cb)); @@ -383,4 +383,4 @@ class LifecycleNode::LifecycleNodeImpl }; } // namespace rclcpp_lifecycle -#endif // LIFECYCLE_NODE_IMPL_HPP_ +#endif // LIFECYCLE_NODE_INTERFACE_IMPL_HPP_ diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface.cpp b/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp similarity index 100% rename from rclcpp_lifecycle/src/lifecycle_node_interface.cpp rename to rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp From f0416fa5d8b07731c565d4a012fa523a15308a45 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Sat, 10 Dec 2016 15:42:22 -0800 Subject: [PATCH 55/63] remove copy&paste comment --- rclcpp/include/rclcpp/service.hpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index f65c730762..9d32870e5d 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -12,10 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -// Based on: http://the-witness.net/news/2012/11/scopeexit-in-c11/ -// But I changed the lambda to include by reference rather than value, see: -// http://the-witness.net/news/2012/11/scopeexit-in-c11/comment-page-1/#comment-86873 - #ifndef RCLCPP__SERVICE_HPP_ #define RCLCPP__SERVICE_HPP_ From c0ad638298206385d3ac34f894abd7da681100de Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Sat, 10 Dec 2016 15:55:52 -0800 Subject: [PATCH 56/63] remove empty line --- rclcpp_lifecycle/test/test_state_machine_info.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp_lifecycle/test/test_state_machine_info.cpp b/rclcpp_lifecycle/test/test_state_machine_info.cpp index 518f7b3105..098474d397 100644 --- a/rclcpp_lifecycle/test/test_state_machine_info.cpp +++ b/rclcpp_lifecycle/test/test_state_machine_info.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include #include #include From 48211c2a3e43797093c50b9d2e4497bff6cd0fb3 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Sun, 11 Dec 2016 13:57:24 -0800 Subject: [PATCH 57/63] (test) register custom callbacks --- rclcpp_lifecycle/CMakeLists.txt | 10 +- .../test/test_register_custom_callbacks.cpp | 147 ++++++++++++++++++ 2 files changed, 156 insertions(+), 1 deletion(-) create mode 100644 rclcpp_lifecycle/test/test_register_custom_callbacks.cpp diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index a68ed7a673..b0992050c8 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -67,7 +67,15 @@ if(BUILD_TESTING) ) target_link_libraries(test_state_machine_info ${PROJECT_NAME}) endif() - + ament_add_gtest(test_register_custom_callbacks test/test_register_custom_callbacks.cpp) + if(TARGET test_register_custom_callbacks) + target_include_directories(test_register_custom_callbacks PUBLIC + ${rcl_lifecycle_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} + ${rclcpp_lifecycle_INCLUDE_DIRS} + ) + target_link_libraries(test_register_custom_callbacks ${PROJECT_NAME}) + endif() endif() ament_export_dependencies(rclcpp) diff --git a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp new file mode 100644 index 0000000000..b4f8ac8541 --- /dev/null +++ b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp @@ -0,0 +1,147 @@ +// 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. + + +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using lifecycle_msgs::msg::State; +using lifecycle_msgs::msg::Transition; + +class TestRegisterCustomCallbacks : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit CustomLifecycleNode(std::string node_name) + : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) + {} + + size_t number_of_callbacks = 0; + +protected: + bool on_configure() + { + ADD_FAILURE(); + ++number_of_callbacks; + return true; + } + + bool on_activate() + { + ADD_FAILURE(); + ++number_of_callbacks; + return true; + } + + bool on_deactivate() + { + ADD_FAILURE(); + ++number_of_callbacks; + return true; + } + + bool on_cleanup() + { + ADD_FAILURE(); + ++number_of_callbacks; + return true; + } + + bool on_shutdown() + { + ADD_FAILURE(); + ++number_of_callbacks; + return true; + } + + // Custom callbacks + +public: + bool on_custom_configure() + { + EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id()); + ++number_of_callbacks; + return true; + } + + bool on_custom_activate() + { + EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id()); + ++number_of_callbacks; + return true; + } + + bool on_custom_deactivate() + { + EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id()); + ++number_of_callbacks; + return true; + } + + bool on_custom_cleanup() + { + EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id()); + ++number_of_callbacks; + return true; + } + + bool on_custom_shutdown() + { + EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id()); + ++number_of_callbacks; + return true; + } +}; + +TEST_F(TestRegisterCustomCallbacks, custom_callbacks) { + auto test_node = std::make_shared("testnode"); + + test_node->register_on_configure(std::bind(&CustomLifecycleNode::on_custom_configure, test_node)); + test_node->register_on_cleanup(std::bind(&CustomLifecycleNode::on_custom_cleanup, test_node)); + test_node->register_on_shutdown(std::bind(&CustomLifecycleNode::on_custom_shutdown, test_node)); + test_node->register_on_activate(std::bind(&CustomLifecycleNode::on_custom_activate, test_node)); + test_node->register_on_deactivate(std::bind(&CustomLifecycleNode::on_custom_deactivate, + test_node)); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); + EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); + EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); + + // check if all callbacks were successfully overwritten + EXPECT_EQ(static_cast(5), test_node->number_of_callbacks); +} From 1cdb89acc0dd6aafb6e75cc0a9c3ebce31d8175a Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 12 Dec 2016 20:44:53 -0800 Subject: [PATCH 58/63] (dev) return codes --- .../rclcpp_lifecycle/lifecycle_node.hpp | 12 ++-- .../lifecycle_node_interface.hpp | 27 +++++---- rclcpp_lifecycle/src/lifecycle_node.cpp | 28 +++++---- .../src/lifecycle_node_interface_impl.hpp | 49 ++++++++++------ .../lifecycle_node_interface.cpp | 37 ++++++------ rclcpp_lifecycle/test/test_lifecycle_node.cpp | 49 +++++----------- .../test/test_register_custom_callbacks.cpp | 58 +++++++++++-------- 7 files changed, 134 insertions(+), 126 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index fd3f4bd308..586dbe41bd 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -360,22 +360,22 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, RCLCPP_LIFECYCLE_PUBLIC bool - register_on_configure(std::function fcn); + register_on_configure(std::function fcn); RCLCPP_LIFECYCLE_PUBLIC bool - register_on_cleanup(std::function fcn); + register_on_cleanup(std::function fcn); RCLCPP_LIFECYCLE_PUBLIC bool - register_on_shutdown(std::function fcn); + register_on_shutdown(std::function fcn); RCLCPP_LIFECYCLE_PUBLIC bool - register_on_activate(std::function fcn); + register_on_activate(std::function fcn); RCLCPP_LIFECYCLE_PUBLIC bool - register_on_deactivate(std::function fcn); + register_on_deactivate(std::function fcn); RCLCPP_LIFECYCLE_PUBLIC bool - register_on_error(std::function fcn); + register_on_error(std::function fcn); protected: RCLCPP_LIFECYCLE_PUBLIC diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp index e041fd76ee..2e17205bc3 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -15,6 +15,9 @@ #ifndef RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_ #define RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_ +#include "rcl_lifecycle/data_types.h" + +#include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/visibility_control.h" namespace rclcpp_lifecycle @@ -46,48 +49,48 @@ class LifecycleNodeInterface * \return true by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual bool - on_configure(); + virtual rcl_lifecycle_ret_t + on_configure(const State & previous_state); /// Callback function for cleanup transition /* * \return true by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual bool - on_cleanup(); + virtual rcl_lifecycle_ret_t + on_cleanup(const State & previous_state); /// Callback function for shutdown transition /* * \return true by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual bool - on_shutdown(); + virtual rcl_lifecycle_ret_t + on_shutdown(const State & previous_state); /// Callback function for activate transition /* * \return true by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual bool - on_activate(); + virtual rcl_lifecycle_ret_t + on_activate(const State & previous_state); /// Callback function for deactivate transition /* * \return true by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual bool - on_deactivate(); + virtual rcl_lifecycle_ret_t + on_deactivate(const State & previous_state); /// Callback function for errorneous transition /* * \return false by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual bool - on_error(); + virtual rcl_lifecycle_ret_t + on_error(const State & previous_state); }; } // namespace node_interfaces diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 753ae68c18..1998c6d015 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -63,12 +63,16 @@ LifecycleNode::LifecycleNode( { impl_->init(); - register_on_configure(std::bind(&LifecycleNodeInterface::on_configure, this)); - register_on_cleanup(std::bind(&LifecycleNodeInterface::on_cleanup, this)); - register_on_shutdown(std::bind(&LifecycleNodeInterface::on_shutdown, this)); - register_on_activate(std::bind(&LifecycleNodeInterface::on_activate, this)); - register_on_deactivate(std::bind(&LifecycleNodeInterface::on_deactivate, this)); - register_on_error(std::bind(&LifecycleNodeInterface::on_error, this)); + register_on_configure(std::bind(&LifecycleNodeInterface::on_configure, this, + std::placeholders::_1)); + register_on_cleanup(std::bind(&LifecycleNodeInterface::on_cleanup, this, std::placeholders::_1)); + register_on_shutdown(std::bind(&LifecycleNodeInterface::on_shutdown, this, + std::placeholders::_1)); + register_on_activate(std::bind(&LifecycleNodeInterface::on_activate, this, + std::placeholders::_1)); + register_on_deactivate(std::bind(&LifecycleNodeInterface::on_deactivate, this, + std::placeholders::_1)); + register_on_error(std::bind(&LifecycleNodeInterface::on_error, this, std::placeholders::_1)); } LifecycleNode::~LifecycleNode() @@ -224,37 +228,37 @@ LifecycleNode::get_node_parameters_interface() //// bool -LifecycleNode::register_on_configure(std::function fcn) +LifecycleNode::register_on_configure(std::function fcn) { return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn); } bool -LifecycleNode::register_on_cleanup(std::function fcn) +LifecycleNode::register_on_cleanup(std::function fcn) { return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn); } bool -LifecycleNode::register_on_shutdown(std::function fcn) +LifecycleNode::register_on_shutdown(std::function fcn) { return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn); } bool -LifecycleNode::register_on_activate(std::function fcn) +LifecycleNode::register_on_activate(std::function fcn) { return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn); } bool -LifecycleNode::register_on_deactivate(std::function fcn) +LifecycleNode::register_on_deactivate(std::function fcn) { return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn); } bool -LifecycleNode::register_on_error(std::function fcn) +LifecycleNode::register_on_error(std::function fcn) { return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING, fcn); diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index e3051ed158..7307907abd 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -150,7 +150,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl } bool - register_callback(std::uint8_t lifecycle_transition, std::function & cb) + register_callback(std::uint8_t lifecycle_transition, std::function & cb) { cb_map_[lifecycle_transition] = cb; return true; @@ -280,16 +281,23 @@ class LifecycleNode::LifecycleNodeInterfaceImpl return false; } + // keep the initial state to pass to a transition callback + State initial_state(state_machine_.current_state); + unsigned int transition_id = static_cast(lifecycle_transition); - if (rcl_lifecycle_start_transition(&state_machine_, transition_id, true, true) != RCL_RET_OK) { + if (rcl_lifecycle_trigger_transition(&state_machine_, transition_id, RCL_RET_OK, + true) != RCL_RET_OK) + { fprintf(stderr, "%s:%d, Unable to start transition %u from current state %s: %s\n", __FILE__, __LINE__, transition_id, state_machine_.current_state->label, rcl_get_error_string_safe()); return false; } - auto cb_success = execute_callback(state_machine_.current_state->id); - if (rcl_lifecycle_start_transition( + auto cb_success = execute_callback( + state_machine_.current_state->id, initial_state); + + if (rcl_lifecycle_trigger_transition( &state_machine_, transition_id, cb_success, true) != RCL_RET_OK) { fprintf(stderr, "Failed to finish transition %u. Current state is now: %s\n", @@ -298,15 +306,16 @@ class LifecycleNode::LifecycleNodeInterfaceImpl } // error handling ?! - if (!cb_success) { - auto error_resolved = execute_callback(state_machine_.current_state->id); - if (error_resolved) { + // TODO(karsten1987): iterate over possible ret value + if (cb_success == RCL_LIFECYCLE_RET_ERROR) { + auto error_resolved = execute_callback(state_machine_.current_state->id, initial_state); + if (error_resolved == RCL_RET_OK) { // We call cleanup on the error state - rcl_lifecycle_start_transition( + rcl_lifecycle_trigger_transition( &state_machine_, lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, true, true); } else { // We call shutdown on the error state - rcl_lifecycle_start_transition( + rcl_lifecycle_trigger_transition( &state_machine_, lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN, true, true); } } @@ -316,20 +325,24 @@ class LifecycleNode::LifecycleNodeInterfaceImpl return true; } - bool - execute_callback(unsigned int cb_id) + rcl_lifecycle_ret_t + execute_callback(unsigned int cb_id, const State & previous_state) { - auto cb_success = true; // in case no callback was attached, we forward directly - auto it = cb_map_.find(state_machine_.current_state->id); + // in case no callback was attached, we forward directly + auto cb_success = RCL_LIFECYCLE_RET_OK; + + auto it = cb_map_.find(cb_id); if (it != cb_map_.end()) { - std::function callback = it->second; + auto callback = it->second; try { - cb_success = callback(); + cb_success = callback(State(previous_state)); } catch (const std::exception & e) { fprintf(stderr, "Caught exception in callback for transition %d\n", it->first); fprintf(stderr, "Original error msg: %s\n", e.what()); - cb_success = false; + // maybe directly go for error handling here + // and pass exception along with it + cb_success = RCL_LIFECYCLE_RET_ERROR; } } else { fprintf(stderr, "%s:%d, No callback is registered for transition %u.\n", @@ -359,7 +372,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl rcl_lifecycle_state_machine_t state_machine_; State current_state_; - std::map> cb_map_; + std::map< + std::uint8_t, + std::function> cb_map_; using NodeBasePtr = std::shared_ptr; using NodeServicesPtr = std::shared_ptr; diff --git a/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp b/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp index 68e0255376..80c7c68a11 100644 --- a/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp +++ b/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp @@ -18,41 +18,40 @@ namespace rclcpp_lifecycle { namespace node_interfaces { - -bool -LifecycleNodeInterface::on_configure() +rcl_lifecycle_ret_t +LifecycleNodeInterface::on_configure(const State &) { - return true; + return RCL_LIFECYCLE_RET_OK; } -bool -LifecycleNodeInterface::on_cleanup() +rcl_lifecycle_ret_t +LifecycleNodeInterface::on_cleanup(const State &) { - return true; + return RCL_LIFECYCLE_RET_OK; } -bool -LifecycleNodeInterface::on_shutdown() +rcl_lifecycle_ret_t +LifecycleNodeInterface::on_shutdown(const State &) { - return true; + return RCL_LIFECYCLE_RET_OK; } -bool -LifecycleNodeInterface::on_activate() +rcl_lifecycle_ret_t +LifecycleNodeInterface::on_activate(const State &) { - return true; + return RCL_LIFECYCLE_RET_OK; } -bool -LifecycleNodeInterface::on_deactivate() +rcl_lifecycle_ret_t +LifecycleNodeInterface::on_deactivate(const State &) { - return true; + return RCL_LIFECYCLE_RET_OK; } -bool -LifecycleNodeInterface::on_error() +rcl_lifecycle_ret_t +LifecycleNodeInterface::on_error(const State &) { - return false; + return RCL_LIFECYCLE_RET_FAILURE; } } // namespace node_interfaces diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 2b20eb8a32..227b6f0047 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -29,18 +29,15 @@ using lifecycle_msgs::msg::Transition; struct GoodMood { - static constexpr bool cb_ret = true; - static constexpr bool error_ret = true; + static constexpr rcl_lifecycle_ret_t cb_ret = RCL_LIFECYCLE_RET_OK; }; struct BadMood { - static constexpr bool cb_ret = false; - static constexpr bool error_ret = true; + static constexpr rcl_lifecycle_ret_t cb_ret = RCL_LIFECYCLE_RET_FAILURE; }; struct VeryBadMood { - static constexpr bool cb_ret = false; - static constexpr bool error_ret = false; + static constexpr rcl_lifecycle_ret_t cb_ret = RCL_LIFECYCLE_RET_ERROR; }; class TestDefaultStateMachine : public ::testing::Test @@ -71,64 +68,57 @@ class MoodyLifecycleNode : public rclcpp_lifecycle::LifecycleNode size_t number_of_callbacks = 0; protected: - bool on_configure() + rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id()); ++number_of_callbacks; return Mood::cb_ret; } - bool on_activate() + rcl_lifecycle_ret_t on_activate(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id()); ++number_of_callbacks; return Mood::cb_ret; } - bool on_deactivate() + rcl_lifecycle_ret_t on_deactivate(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id()); ++number_of_callbacks; return Mood::cb_ret; } - bool on_cleanup() + rcl_lifecycle_ret_t on_cleanup(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id()); ++number_of_callbacks; return Mood::cb_ret; } - bool on_shutdown() + rcl_lifecycle_ret_t on_shutdown(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id()); ++number_of_callbacks; return Mood::cb_ret; } - bool on_error(); + rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State &); }; template<> -bool MoodyLifecycleNode::on_error() +rcl_lifecycle_ret_t MoodyLifecycleNode::on_error(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id()); ADD_FAILURE(); - return GoodMood::error_ret; + return RCL_LIFECYCLE_RET_ERROR; } template<> -bool MoodyLifecycleNode::on_error() +rcl_lifecycle_ret_t MoodyLifecycleNode::on_error(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id()); ++number_of_callbacks; - return BadMood::error_ret; -} -template<> -bool MoodyLifecycleNode::on_error() -{ - EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id()); - ++number_of_callbacks; - return VeryBadMood::error_ret; + return RCL_LIFECYCLE_RET_OK; } TEST_F(TestDefaultStateMachine, empty_initializer) { @@ -180,16 +170,5 @@ TEST_F(TestDefaultStateMachine, bad_mood) { rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); // check if all callbacks were successfully overwritten - EXPECT_EQ(static_cast(2), test_node->number_of_callbacks); -} - -TEST_F(TestDefaultStateMachine, very_bad_mood) { - auto test_node = std::make_shared>("testnode"); - - EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); - - // check if all callbacks were successfully overwritten - EXPECT_EQ(static_cast(2), test_node->number_of_callbacks); + EXPECT_EQ(static_cast(1), test_node->number_of_callbacks); } diff --git a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp index b4f8ac8541..2ae91b964d 100644 --- a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp +++ b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp @@ -46,89 +46,97 @@ class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode size_t number_of_callbacks = 0; protected: - bool on_configure() + rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &) { ADD_FAILURE(); ++number_of_callbacks; - return true; + return RCL_LIFECYCLE_RET_OK; } - bool on_activate() + rcl_lifecycle_ret_t on_activate(const rclcpp_lifecycle::State &) { ADD_FAILURE(); ++number_of_callbacks; - return true; + return RCL_LIFECYCLE_RET_OK; } - bool on_deactivate() + rcl_lifecycle_ret_t on_deactivate(const rclcpp_lifecycle::State &) { ADD_FAILURE(); ++number_of_callbacks; - return true; + return RCL_LIFECYCLE_RET_OK; } - bool on_cleanup() + rcl_lifecycle_ret_t on_cleanup(const rclcpp_lifecycle::State &) { ADD_FAILURE(); ++number_of_callbacks; - return true; + return RCL_LIFECYCLE_RET_OK; } - bool on_shutdown() + rcl_lifecycle_ret_t on_shutdown(const rclcpp_lifecycle::State &) { ADD_FAILURE(); ++number_of_callbacks; - return true; + return RCL_LIFECYCLE_RET_OK; } // Custom callbacks public: - bool on_custom_configure() + rcl_lifecycle_ret_t on_custom_configure(const rclcpp_lifecycle::State & previous_state) { + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, previous_state.id()); EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id()); ++number_of_callbacks; - return true; + return RCL_LIFECYCLE_RET_OK; } - bool on_custom_activate() + rcl_lifecycle_ret_t on_custom_activate(const rclcpp_lifecycle::State & previous_state) { + EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id()); EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id()); ++number_of_callbacks; - return true; + return RCL_LIFECYCLE_RET_OK; } - bool on_custom_deactivate() + rcl_lifecycle_ret_t on_custom_deactivate(const rclcpp_lifecycle::State & previous_state) { + EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, previous_state.id()); EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id()); ++number_of_callbacks; - return true; + return RCL_LIFECYCLE_RET_OK; } - bool on_custom_cleanup() + rcl_lifecycle_ret_t on_custom_cleanup(const rclcpp_lifecycle::State & previous_state) { + EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id()); EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id()); ++number_of_callbacks; - return true; + return RCL_LIFECYCLE_RET_OK; } - bool on_custom_shutdown() + rcl_lifecycle_ret_t on_custom_shutdown(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id()); ++number_of_callbacks; - return true; + return RCL_LIFECYCLE_RET_OK; } }; TEST_F(TestRegisterCustomCallbacks, custom_callbacks) { auto test_node = std::make_shared("testnode"); - test_node->register_on_configure(std::bind(&CustomLifecycleNode::on_custom_configure, test_node)); - test_node->register_on_cleanup(std::bind(&CustomLifecycleNode::on_custom_cleanup, test_node)); - test_node->register_on_shutdown(std::bind(&CustomLifecycleNode::on_custom_shutdown, test_node)); - test_node->register_on_activate(std::bind(&CustomLifecycleNode::on_custom_activate, test_node)); + test_node->register_on_configure( + std::bind(&CustomLifecycleNode::on_custom_configure, test_node, std::placeholders::_1)); + test_node->register_on_cleanup(std::bind(&CustomLifecycleNode::on_custom_cleanup, test_node, + std::placeholders::_1)); + test_node->register_on_shutdown(std::bind(&CustomLifecycleNode::on_custom_shutdown, test_node, + std::placeholders::_1)); + test_node->register_on_activate(std::bind(&CustomLifecycleNode::on_custom_activate, test_node, + std::placeholders::_1)); test_node->register_on_deactivate(std::bind(&CustomLifecycleNode::on_custom_deactivate, - test_node)); + test_node, std::placeholders::_1)); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( From e2ff2c0fe9569be599d4eef31dd438979edf5b46 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 13 Dec 2016 10:25:46 -0800 Subject: [PATCH 59/63] style --- .../rclcpp_lifecycle/lifecycle_publisher.hpp | 44 ++++++++++--------- .../rclcpp_lifecycle/visibility_control.h | 2 - .../src/lifecycle_node_interface_impl.hpp | 34 +++++++------- 3 files changed, 41 insertions(+), 39 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index 3b6c437b01..c01ad98b9b 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -22,8 +22,8 @@ namespace rclcpp_lifecycle { +/// base class with only /** - * @brief base class with only * pure virtual functions. A managed * node can then deactivate or activate * the publishing. @@ -35,10 +35,11 @@ class LifecyclePublisherInterface public: virtual void on_activate() = 0; virtual void on_deactivate() = 0; + virtual bool is_activated() = 0; }; +/// brief child class of rclcpp Publisher class. /** - * @brief child class of rclcpp Publisher class. * Overrides all publisher functions to check for * enabled/disabled state. */ @@ -64,77 +65,76 @@ class LifecyclePublisher : public LifecyclePublisherInterface, ~LifecyclePublisher() {} + /// LifecyclePublisher pulish function /** - * @brief briefly checks whether this publisher + * The publish function checks whether the communication * was enabled or disabled and forwards the message - * to the actual rclcp Publisher base class + * to the actual rclcpp Publisher base class */ virtual void publish(std::unique_ptr & msg) { if (!enabled_) { - printf("I push every message to /dev/null\n"); return; } rclcpp::publisher::Publisher::publish(msg); } + /// LifecyclePublisher pulish function /** - * @brief briefly checks whether this publisher + * The publish function checks whether the communication * was enabled or disabled and forwards the message - * to the actual rclcp Publisher base class + * to the actual rclcpp Publisher base class */ virtual void publish(const std::shared_ptr & msg) { if (!enabled_) { - printf("I publish message %s to /dev/null\n", msg->data.c_str()); return; } - printf("I publish message %s to DDS\n", msg->data.c_str()); rclcpp::publisher::Publisher::publish(msg); } + /// LifecyclePublisher pulish function /** - * @brief briefly checks whether this publisher + * The publish function checks whether the communication * was enabled or disabled and forwards the message - * to the actual rclcp Publisher base class + * to the actual rclcpp Publisher base class */ virtual void publish(std::shared_ptr msg) { if (!enabled_) { - printf("I am every message to /dev/null\n"); return; } rclcpp::publisher::Publisher::publish(msg); } + /// LifecyclePublisher pulish function /** - * @brief briefly checks whether this publisher + * The publish function checks whether the communication * was enabled or disabled and forwards the message - * to the actual rclcp Publisher base class + * to the actual rclcpp Publisher base class */ virtual void publish(const MessageT & msg) { if (!enabled_) { - printf("I am every message to /dev/null\n"); return; } rclcpp::publisher::Publisher::publish(msg); } + /// LifecyclePublisher pulish function /** - * @brief briefly checks whether this publisher + * The publish function checks whether the communication * was enabled or disabled and forwards the message - * to the actual rclcp Publisher base class + * to the actual rclcpp Publisher base class */ virtual void publish(std::shared_ptr & msg) { if (!enabled_) { - printf("I am every message to /dev/null\n"); return; } rclcpp::publisher::Publisher::publish(msg); @@ -143,17 +143,21 @@ class LifecyclePublisher : public LifecyclePublisherInterface, virtual void on_activate() { - printf("Lifecycle publisher is enabled\n"); enabled_ = true; } virtual void on_deactivate() { - printf("Lifecycle publisher is deactivated\n"); enabled_ = false; } + virtual bool + is_activated() + { + return enabled_; + } + private: bool enabled_ = false; }; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h b/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h index 93468d04fa..b2a8327a69 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h @@ -22,8 +22,6 @@ #ifndef RCLCPP_LIFECYCLE__VISIBILITY_CONTROL_H_ #define RCLCPP_LIFECYCLE__VISIBILITY_CONTROL_H_ -#include "rmw/rmw.h" - // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 7307907abd..2fe7738d4a 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -73,6 +73,12 @@ class LifecycleNode::LifecycleNodeInterfaceImpl init() { state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); + // The call to initialize the state machine takes + // currently five different typesupports for all publishers/services + // created within the RCL_LIFECYCLE structure. + // The publisher takes a C-Typesupport since the publishing (i.e. creating + // the message) is done fully in RCL. + // Services are handled in C++, so that it needs a C++ typesupport structure. rcl_ret_t ret = rcl_lifecycle_state_machine_init( &state_machine_, node_base_interface_->get_rcl_node_handle(), ROSIDL_GET_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), @@ -82,9 +88,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl rosidl_typesupport_cpp::get_service_type_support_handle(), true); if (ret != RCL_RET_OK) { - fprintf(stderr, "Error adding %s: %s\n", - node_base_interface_->get_name(), rcl_get_error_string_safe()); - return; + throw std::runtime_error( + std::string( + "Couldn't initialize state machine for node ") + node_base_interface_->get_name()); } { // change_state @@ -164,8 +170,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl { (void)header; if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - resp->success = false; - return; + throw std::runtime_error( + "Can't get state. State machine is not initialized."); } resp->success = change_state(req->transition.id); } @@ -178,10 +184,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl (void)header; (void)req; if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - resp->current_state.id = - static_cast(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN); - resp->current_state.label = "unknown"; - return; + throw std::runtime_error( + "Can't get state. State machine is not initialized."); } resp->current_state.id = static_cast(state_machine_.current_state->id); resp->current_state.label = state_machine_.current_state->label; @@ -195,11 +199,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl (void)header; (void)req; if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - lifecycle_msgs::msg::State unknown_state; - unknown_state.id = static_cast(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN); - unknown_state.label = "unknown"; - resp->available_states.push_back(unknown_state); - return; + throw std::runtime_error( + "Can't get available states. State machine is not initialized."); } for (unsigned int i = 0; i < state_machine_.transition_map.size; ++i) { lifecycle_msgs::msg::State state; @@ -217,6 +218,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl (void)header; (void)req; if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get available transitions. State machine is not initialized."); return; } @@ -344,9 +347,6 @@ class LifecycleNode::LifecycleNodeInterfaceImpl // and pass exception along with it cb_success = RCL_LIFECYCLE_RET_ERROR; } - } else { - fprintf(stderr, "%s:%d, No callback is registered for transition %u.\n", - __FILE__, __LINE__, cb_id); } return cb_success; } From b38e6111259bc3e8769839ea72e445035d058bb6 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 13 Dec 2016 11:34:23 -0800 Subject: [PATCH 60/63] test for exception handling --- rclcpp_lifecycle/CMakeLists.txt | 9 ++ .../src/lifecycle_node_interface_impl.hpp | 11 +- .../test/test_callback_exceptions.cpp | 106 ++++++++++++++++++ 3 files changed, 122 insertions(+), 4 deletions(-) create mode 100644 rclcpp_lifecycle/test/test_callback_exceptions.cpp diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index b0992050c8..115a9ae840 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -76,6 +76,15 @@ if(BUILD_TESTING) ) target_link_libraries(test_register_custom_callbacks ${PROJECT_NAME}) endif() + ament_add_gtest(test_callback_exceptions test/test_callback_exceptions.cpp) + if(TARGET test_callback_exceptions) + target_include_directories(test_callback_exceptions PUBLIC + ${rcl_lifecycle_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} + ${rclcpp_lifecycle_INCLUDE_DIRS} + ) + target_link_libraries(test_callback_exceptions ${PROJECT_NAME}) + endif() endif() ament_export_dependencies(rclcpp) diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 2fe7738d4a..c6f389c03c 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -297,7 +297,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl return false; } - auto cb_success = execute_callback( + rcl_lifecycle_ret_t cb_success = execute_callback( state_machine_.current_state->id, initial_state); if (rcl_lifecycle_trigger_transition( @@ -307,19 +307,22 @@ class LifecycleNode::LifecycleNodeInterfaceImpl transition_id, state_machine_.current_state->label); return false; } + fprintf(stderr, "current state after first callback%s\n", state_machine_.current_state->label); // error handling ?! // TODO(karsten1987): iterate over possible ret value if (cb_success == RCL_LIFECYCLE_RET_ERROR) { - auto error_resolved = execute_callback(state_machine_.current_state->id, initial_state); + rcl_lifecycle_ret_t error_resolved = execute_callback(state_machine_.current_state->id, initial_state); if (error_resolved == RCL_RET_OK) { + fprintf(stderr, "Exception handling was successful\n"); // We call cleanup on the error state rcl_lifecycle_trigger_transition( - &state_machine_, lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, true, true); + &state_machine_, lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, error_resolved, true); + fprintf(stderr, "current state after error callback%s\n", state_machine_.current_state->label); } else { // We call shutdown on the error state rcl_lifecycle_trigger_transition( - &state_machine_, lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN, true, true); + &state_machine_, lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN, error_resolved, true); } } // This true holds in both cases where the actual callback diff --git a/rclcpp_lifecycle/test/test_callback_exceptions.cpp b/rclcpp_lifecycle/test/test_callback_exceptions.cpp new file mode 100644 index 0000000000..4830ecec9c --- /dev/null +++ b/rclcpp_lifecycle/test/test_callback_exceptions.cpp @@ -0,0 +1,106 @@ +// 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. + + +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using lifecycle_msgs::msg::State; +using lifecycle_msgs::msg::Transition; + +class TestCallbackExceptions : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +class PositiveCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit PositiveCallbackExceptionNode(std::string node_name) + : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) + {} + + size_t number_of_callbacks = 0; + +protected: + rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &) + { + ++number_of_callbacks; + throw std::runtime_error("On_configure exception"); + } + + rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State & previous_state) + { + fprintf(stderr, "Previous state of error was %s\n", + previous_state.label().c_str()); + ++number_of_callbacks; + return RCL_LIFECYCLE_RET_OK; + } +}; + +TEST_F(TestCallbackExceptions, positive_on_error) { + auto test_node = std::make_shared("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + // check if all callbacks were successfully overwritten + EXPECT_EQ(static_cast(2), test_node->number_of_callbacks); +} + +class NegativeCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit NegativeCallbackExceptionNode(std::string node_name) + : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) + {} + + size_t number_of_callbacks = 0; + +protected: + rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &) + { + ++number_of_callbacks; + throw std::runtime_error("On_configure exception"); + } + + rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State & previous_state) + { + fprintf(stderr, "Previous state of error was %s\n", + previous_state.label().c_str()); + ++number_of_callbacks; + return RCL_LIFECYCLE_RET_FAILURE; + } +}; +TEST_F(TestCallbackExceptions, negative_on_error) { + auto test_node = std::make_shared("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + // check if all callbacks were successfully overwritten + EXPECT_EQ(static_cast(2), test_node->number_of_callbacks); +} From 0bda7b73a2e097f2e954af6b5ae7526cd328803c Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 13 Dec 2016 18:47:53 -0800 Subject: [PATCH 61/63] refacotr new state machine --- .../src/lifecycle_node_interface_impl.hpp | 57 ++++++++----------- .../test/test_callback_exceptions.cpp | 4 +- .../test/test_state_machine_info.cpp | 3 +- 3 files changed, 27 insertions(+), 37 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index c6f389c03c..ef30855ac5 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -202,7 +202,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl throw std::runtime_error( "Can't get available states. State machine is not initialized."); } - for (unsigned int i = 0; i < state_machine_.transition_map.size; ++i) { + for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { lifecycle_msgs::msg::State state; state.id = static_cast(state_machine_.transition_map.states[i].id); state.label = static_cast(state_machine_.transition_map.states[i].label); @@ -223,21 +223,16 @@ class LifecycleNode::LifecycleNodeInterfaceImpl return; } - for (unsigned int i = 0; i < state_machine_.transition_map.size; ++i) { - // get transitions associated to each primary state - for (unsigned int j = 0; j < state_machine_.transition_map.transition_arrays[i].size; ++j) { - rcl_lifecycle_transition_t rcl_transition = - state_machine_.transition_map.transition_arrays[i].transitions[j]; - - lifecycle_msgs::msg::TransitionDescription trans_desc; - trans_desc.transition.id = rcl_transition.id; - trans_desc.transition.label = rcl_transition.label; - trans_desc.start_state.id = rcl_transition.start->id; - trans_desc.start_state.label = rcl_transition.start->label; - trans_desc.goal_state.id = rcl_transition.goal->id; - trans_desc.goal_state.label = rcl_transition.goal->label; - resp->available_transitions.push_back(trans_desc); - } + for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { + rcl_lifecycle_transition_t & rcl_transition = state_machine_.transition_map.transitions[i]; + lifecycle_msgs::msg::TransitionDescription trans_desc; + trans_desc.transition.id = rcl_transition.id; + trans_desc.transition.label = rcl_transition.label; + trans_desc.start_state.id = rcl_transition.start->id; + trans_desc.start_state.label = rcl_transition.start->label; + trans_desc.goal_state.id = rcl_transition.goal->id; + trans_desc.goal_state.label = rcl_transition.goal->label; + resp->available_transitions.push_back(trans_desc); } } @@ -252,7 +247,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl get_available_states() { std::vector states; - for (unsigned int i = 0; i < state_machine_.transition_map.size; ++i) { + for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { State state(&state_machine_.transition_map.states[i]); states.push_back(state); } @@ -264,13 +259,10 @@ class LifecycleNode::LifecycleNodeInterfaceImpl { std::vector transitions; - for (unsigned int i = 0; i < state_machine_.transition_map.size; ++i) { - // get transitions associated to each primary state - for (unsigned int j = 0; j < state_machine_.transition_map.transition_arrays[i].size; ++j) { - Transition transition( - &state_machine_.transition_map.transition_arrays[i].transitions[j]); - transitions.push_back(transition); - } + for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { + Transition transition( + &state_machine_.transition_map.transitions[i]); + transitions.push_back(transition); } return transitions; } @@ -288,9 +280,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl State initial_state(state_machine_.current_state); unsigned int transition_id = static_cast(lifecycle_transition); - if (rcl_lifecycle_trigger_transition(&state_machine_, transition_id, RCL_RET_OK, - true) != RCL_RET_OK) - { + if (rcl_lifecycle_trigger_transition(&state_machine_, transition_id, true) != RCL_RET_OK) { fprintf(stderr, "%s:%d, Unable to start transition %u from current state %s: %s\n", __FILE__, __LINE__, transition_id, state_machine_.current_state->label, rcl_get_error_string_safe()); @@ -301,28 +291,29 @@ class LifecycleNode::LifecycleNodeInterfaceImpl state_machine_.current_state->id, initial_state); if (rcl_lifecycle_trigger_transition( - &state_machine_, transition_id, cb_success, true) != RCL_RET_OK) + &state_machine_, cb_success, true) != RCL_RET_OK) { fprintf(stderr, "Failed to finish transition %u. Current state is now: %s\n", transition_id, state_machine_.current_state->label); return false; } - fprintf(stderr, "current state after first callback%s\n", state_machine_.current_state->label); // error handling ?! // TODO(karsten1987): iterate over possible ret value if (cb_success == RCL_LIFECYCLE_RET_ERROR) { - rcl_lifecycle_ret_t error_resolved = execute_callback(state_machine_.current_state->id, initial_state); + rcl_lifecycle_ret_t error_resolved = execute_callback(state_machine_.current_state->id, + initial_state); if (error_resolved == RCL_RET_OK) { fprintf(stderr, "Exception handling was successful\n"); // We call cleanup on the error state rcl_lifecycle_trigger_transition( - &state_machine_, lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, error_resolved, true); - fprintf(stderr, "current state after error callback%s\n", state_machine_.current_state->label); + &state_machine_, error_resolved, true); + fprintf(stderr, "current state after error callback%s\n", + state_machine_.current_state->label); } else { // We call shutdown on the error state rcl_lifecycle_trigger_transition( - &state_machine_, lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN, error_resolved, true); + &state_machine_, error_resolved, true); } } // This true holds in both cases where the actual callback diff --git a/rclcpp_lifecycle/test/test_callback_exceptions.cpp b/rclcpp_lifecycle/test/test_callback_exceptions.cpp index 4830ecec9c..c3450153cc 100644 --- a/rclcpp_lifecycle/test/test_callback_exceptions.cpp +++ b/rclcpp_lifecycle/test/test_callback_exceptions.cpp @@ -55,7 +55,7 @@ class PositiveCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State & previous_state) { fprintf(stderr, "Previous state of error was %s\n", - previous_state.label().c_str()); + previous_state.label().c_str()); ++number_of_callbacks; return RCL_LIFECYCLE_RET_OK; } @@ -90,7 +90,7 @@ class NegativeCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State & previous_state) { fprintf(stderr, "Previous state of error was %s\n", - previous_state.label().c_str()); + previous_state.label().c_str()); ++number_of_callbacks; return RCL_LIFECYCLE_RET_FAILURE; } diff --git a/rclcpp_lifecycle/test/test_state_machine_info.cpp b/rclcpp_lifecycle/test/test_state_machine_info.cpp index 098474d397..caf5e87c4f 100644 --- a/rclcpp_lifecycle/test/test_state_machine_info.cpp +++ b/rclcpp_lifecycle/test/test_state_machine_info.cpp @@ -56,9 +56,8 @@ TEST_F(TestStateMachineInfo, available_transitions) { auto test_node = std::make_shared("testnode"); std::vector available_transitions = test_node->get_available_transitions(); - EXPECT_EQ((unsigned int)14, available_transitions.size()); + EXPECT_EQ((unsigned int)25, available_transitions.size()); for (rclcpp_lifecycle::Transition & transition : available_transitions) { - EXPECT_TRUE(transition.id() <= (unsigned int)5); EXPECT_FALSE(transition.label().empty()); EXPECT_TRUE(transition.start_state().id() <= (unsigned int)4 || From a08c7367f71eb6856457bd193c824e208663e1ed Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 13 Dec 2016 20:34:47 -0800 Subject: [PATCH 62/63] c++14 --- rclcpp_lifecycle/CMakeLists.txt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 115a9ae840..970d9626b4 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -2,8 +2,9 @@ cmake_minimum_required(VERSION 3.5) project(rclcpp_lifecycle) -if(NOT WIN32) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra") +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 REQUIRED) From 49e9ed34b30202a98fd5aa825130e910dc813d34 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 13 Dec 2016 21:37:02 -0800 Subject: [PATCH 63/63] change exception message for windows ci bug change exception message for windows ci bug --- .../src/lifecycle_node_interface_impl.hpp | 10 ++++++---- rclcpp_lifecycle/test/test_callback_exceptions.cpp | 12 ++++-------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index ef30855ac5..ea66d4ad54 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -333,10 +333,12 @@ class LifecycleNode::LifecycleNodeInterfaceImpl auto callback = it->second; try { cb_success = callback(State(previous_state)); - } catch (const std::exception & e) { - fprintf(stderr, "Caught exception in callback for transition %d\n", - it->first); - fprintf(stderr, "Original error msg: %s\n", e.what()); + } catch (const std::exception &) { + // TODO(karsten1987): Windows CI doens't let me print the msg here + // the todo is to forward the exception to the on_error callback + // fprintf(stderr, "Caught exception in callback for transition %d\n", + // it->first); + // fprintf(stderr, "Original error msg: %s\n", e.what()); // maybe directly go for error handling here // and pass exception along with it cb_success = RCL_LIFECYCLE_RET_ERROR; diff --git a/rclcpp_lifecycle/test/test_callback_exceptions.cpp b/rclcpp_lifecycle/test/test_callback_exceptions.cpp index c3450153cc..60fcfa2b89 100644 --- a/rclcpp_lifecycle/test/test_callback_exceptions.cpp +++ b/rclcpp_lifecycle/test/test_callback_exceptions.cpp @@ -49,13 +49,11 @@ class PositiveCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &) { ++number_of_callbacks; - throw std::runtime_error("On_configure exception"); + throw std::runtime_error("custom exception raised in configure callback"); } - rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State & previous_state) + rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State &) { - fprintf(stderr, "Previous state of error was %s\n", - previous_state.label().c_str()); ++number_of_callbacks; return RCL_LIFECYCLE_RET_OK; } @@ -84,13 +82,11 @@ class NegativeCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &) { ++number_of_callbacks; - throw std::runtime_error("On_configure exception"); + throw std::runtime_error("custom exception raised in configure callback"); } - rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State & previous_state) + rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State &) { - fprintf(stderr, "Previous state of error was %s\n", - previous_state.label().c_str()); ++number_of_callbacks; return RCL_LIFECYCLE_RET_FAILURE; }