diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt new file mode 100644 index 0000000..970d962 --- /dev/null +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -0,0 +1,99 @@ +cmake_minimum_required(VERSION 3.5) + +project(rclcpp_lifecycle) + +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) +find_package(rclcpp REQUIRED) +find_package(rcl_lifecycle REQUIRED) +find_package(rmw_implementation_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(lifecycle_msgs REQUIRED) + +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 + src/node_interfaces/lifecycle_node_interface.cpp + src/state.cpp + src/transition.cpp + ) + ament_target_dependencies(rclcpp_lifecycle${target_suffix} + "rclcpp${target_suffix}" + "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 + 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() + + 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() + 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() + 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() + 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) +ament_export_dependencies(rcl_lifecycle) +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/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp new file mode 100644 index 0000000..586dbe4 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -0,0 +1,416 @@ +// 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_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ + +#include +#include +#include +#include + +#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_publisher.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "rclcpp_lifecycle/transition.hpp" +#include "rclcpp_lifecycle/visibility_control.h" + +namespace rclcpp_lifecycle +{ + +/// LifecycleNode for creating lifecycle components +/** + * has lifecycle nodeinterface for configuring this node. + */ +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(); + + /// Get the name of the node. + // \return The name of the node. + RCLCPP_LIFECYCLE_PUBLIC + const char * + get_name() const; + + /// Create and return a callback group. + RCLCPP_LIFECYCLE_PUBLIC + 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 + 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. + /** + * \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, + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, + 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> + std::shared_ptr + 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> + std::shared_ptr + 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::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; + + 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(); + + 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 + */ + 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: + RCLCPP_LIFECYCLE_PUBLIC + void + add_publisher_handle(std::shared_ptr pub); + + RCLCPP_LIFECYCLE_PUBLIC + void + add_timer_handle(std::shared_ptr timer); + +private: + 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 LifecycleNodeInterfaceImpl; + std::unique_ptr impl_; +}; + +} // 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 new file mode 100644 index 0000000..e038fe8 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -0,0 +1,212 @@ +// 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 "rclcpp_lifecycle/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, + std::shared_ptr allocator) +{ + using PublisherT = rclcpp_lifecycle::LifecyclePublisher; + + // create regular publisher in rclcpp::Node + return rclcpp::create_publisher( + this->node_topics_.get(), + topic_name, + qos_profile, + use_intra_process_comms_, + allocator); +} + +// TODO(karsten1987): Create LifecycleSubscriber +template +std::shared_ptr +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 MessageT, + typename CallbackT, + typename Alloc, + typename SubscriptionT> +std::shared_ptr +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 rclcpp::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 = 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); + 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 new file mode 100644 index 0000000..c01ad98 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -0,0 +1,167 @@ +// 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_PUBLISHER_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_PUBLISHER_HPP_ + +#include +#include + +#include "rclcpp/publisher.hpp" + +namespace rclcpp_lifecycle +{ +/// 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 LifecyclePublisherInterface +{ +public: + virtual void on_activate() = 0; + virtual void on_deactivate() = 0; + virtual bool is_activated() = 0; +}; + +/// brief child class of rclcpp Publisher class. +/** + * Overrides all publisher functions to check for + * enabled/disabled state. + */ +template> +class LifecyclePublisher : public LifecyclePublisherInterface, + public rclcpp::publisher::Publisher +{ +public: + using MessageAllocTraits = rclcpp::allocator::AllocRebind; + using MessageAlloc = typename MessageAllocTraits::allocator_type; + using MessageDeleter = rclcpp::allocator::Deleter; + using MessageUniquePtr = std::unique_ptr; + + LifecyclePublisher( + 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_base, topic, publisher_options, allocator), + enabled_(false) + {} + + ~LifecyclePublisher() {} + + /// LifecyclePublisher pulish function + /** + * The publish function checks whether the communication + * was enabled or disabled and forwards the message + * to the actual rclcpp Publisher base class + */ + virtual void + publish(std::unique_ptr & msg) + { + if (!enabled_) { + return; + } + rclcpp::publisher::Publisher::publish(msg); + } + + /// LifecyclePublisher pulish function + /** + * The publish function checks whether the communication + * was enabled or disabled and forwards the message + * to the actual rclcpp Publisher base class + */ + virtual void + publish(const std::shared_ptr & msg) + { + if (!enabled_) { + return; + } + rclcpp::publisher::Publisher::publish(msg); + } + + /// LifecyclePublisher pulish function + /** + * The publish function checks whether the communication + * was enabled or disabled and forwards the message + * to the actual rclcpp Publisher base class + */ + virtual void + publish(std::shared_ptr msg) + { + if (!enabled_) { + return; + } + rclcpp::publisher::Publisher::publish(msg); + } + + /// LifecyclePublisher pulish function + /** + * The publish function checks whether the communication + * was enabled or disabled and forwards the message + * to the actual rclcpp Publisher base class + */ + virtual void + publish(const MessageT & msg) + { + if (!enabled_) { + return; + } + rclcpp::publisher::Publisher::publish(msg); + } + + /// LifecyclePublisher pulish function + /** + * The publish function checks whether the communication + * was enabled or disabled and forwards the message + * to the actual rclcpp Publisher base class + */ + virtual void + publish(std::shared_ptr & msg) + { + if (!enabled_) { + return; + } + rclcpp::publisher::Publisher::publish(msg); + } + + virtual void + on_activate() + { + enabled_ = true; + } + + virtual void + on_deactivate() + { + enabled_ = false; + } + + virtual bool + is_activated() + { + return enabled_; + } + +private: + bool enabled_ = false; +}; + +} // 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 0000000..2e17205 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -0,0 +1,98 @@ +// 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 "rcl_lifecycle/data_types.h" + +#include "rclcpp_lifecycle/state.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: + RCLCPP_LIFECYCLE_PUBLIC + LifecycleNodeInterface() {} + +public: + /// Callback function for configure transition + /* + * \return true by default + */ + RCLCPP_LIFECYCLE_PUBLIC + virtual rcl_lifecycle_ret_t + on_configure(const State & previous_state); + + /// Callback function for cleanup transition + /* + * \return true by default + */ + RCLCPP_LIFECYCLE_PUBLIC + virtual rcl_lifecycle_ret_t + on_cleanup(const State & previous_state); + + /// Callback function for shutdown transition + /* + * \return true by default + */ + RCLCPP_LIFECYCLE_PUBLIC + virtual rcl_lifecycle_ret_t + on_shutdown(const State & previous_state); + + /// Callback function for activate transition + /* + * \return true by default + */ + RCLCPP_LIFECYCLE_PUBLIC + virtual rcl_lifecycle_ret_t + on_activate(const State & previous_state); + + /// Callback function for deactivate transition + /* + * \return true by default + */ + RCLCPP_LIFECYCLE_PUBLIC + virtual rcl_lifecycle_ret_t + on_deactivate(const State & previous_state); + + /// Callback function for errorneous transition + /* + * \return false by default + */ + RCLCPP_LIFECYCLE_PUBLIC + virtual rcl_lifecycle_ret_t + on_error(const State & previous_state); +}; + +} // 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 new file mode 100644 index 0000000..3b2d3e2 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -0,0 +1,57 @@ +// 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_lifecycle +{ + +class State +{ +public: + RCLCPP_LIFECYCLE_PUBLIC + State(); + + RCLCPP_LIFECYCLE_PUBLIC + State(unsigned int id, const std::string & label); + + RCLCPP_LIFECYCLE_PUBLIC + explicit 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 +#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 0000000..35bb296 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp @@ -0,0 +1,72 @@ +// 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/state.hpp" +#include "rclcpp_lifecycle/visibility_control.h" + +// forward declare rcl_transition_t +typedef struct rcl_lifecycle_transition_t rcl_lifecycle_transition_t; + +namespace rclcpp_lifecycle +{ + +class Transition +{ +public: + RCLCPP_LIFECYCLE_PUBLIC + Transition() = delete; + + 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); + + RCLCPP_LIFECYCLE_PUBLIC + virtual ~Transition(); + + RCLCPP_LIFECYCLE_PUBLIC + unsigned int + id() const; + + RCLCPP_LIFECYCLE_PUBLIC + 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_; +}; + +} // namespace rclcpp_lifecycle +#endif // RCLCPP_LIFECYCLE__TRANSITION_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 0000000..c8eeb42 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp @@ -0,0 +1,56 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_LIFECYCLE__TYPE_TRAITS__IS_MANAGEABLE_NODE_HPP_ +#define RCLCPP_LIFECYCLE__TYPE_TRAITS__IS_MANAGEABLE_NODE_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 // 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 new file mode 100644 index 0000000..b2a8327 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef RCLCPP_LIFECYCLE__VISIBILITY_CONTROL_H_ +#define RCLCPP_LIFECYCLE__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RCLCPP_LIFECYCLE_EXPORT __attribute__ ((dllexport)) + #define RCLCPP_LIFECYCLE_IMPORT __attribute__ ((dllimport)) + #else + #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 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 new file mode 100644 index 0000000..7eea88c --- /dev/null +++ b/rclcpp_lifecycle/package.xml @@ -0,0 +1,36 @@ + + + + rclcpp_lifecycle + 0.0.0 + Package containing a prototype for lifecycle implementation + Karsten Knese + Apache License 2.0 + + ament_cmake + rosidl_default_generators + + rclcpp + rcl_lifecycle + rmw_implementation + rmw_implementation_cmake + rosidl_default_generators + std_msgs + lifecycle_msgs + + rclcpp + rcl_lifecycle + rclpy + rmw_implementation + rosidl_default_runtime + std_msgs + lifecycle_msgs + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp new file mode 100644 index 0000000..1998c6d --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -0,0 +1,310 @@ +// 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/lifecycle_node.hpp" + +#include +#include +#include +#include +#include + +#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_interface_impl.hpp" // implementation + +namespace rclcpp_lifecycle +{ + +LifecycleNode::LifecycleNode(const std::string & node_name, bool use_intra_process_comms) +: 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 LifecycleNodeInterfaceImpl(node_base_, node_services_)) +{ + impl_->init(); + + 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() +{} + +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) +{ + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn); +} + +bool +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) +{ + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn); +} + +bool +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) +{ + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn); +} + +bool +LifecycleNode::register_on_error(std::function fcn) +{ + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING, + fcn); +} + +const State & +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) +{ + 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) +{ + impl_->add_publisher_handle(pub); +} + +void +LifecycleNode::add_timer_handle(std::shared_ptr timer) +{ + impl_->add_timer_handle(timer); +} + +} // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp new file mode 100644 index 0000000..ea66d4a --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -0,0 +1,397 @@ +// 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 LIFECYCLE_NODE_INTERFACE_IMPL_HPP_ +#define LIFECYCLE_NODE_INTERFACE_IMPL_HPP_ + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include +#include +#include +#include +#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 "rcl/error_handling.h" + +#include "rcl_lifecycle/rcl_lifecycle.h" + +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" + +namespace rclcpp_lifecycle +{ + +class LifecycleNode::LifecycleNodeInterfaceImpl +{ + 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: + 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) + {} + + ~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", + __FILE__, __LINE__); + } else { + rcl_lifecycle_state_machine_fini(&state_machine_, + node_base_interface_->get_rcl_node_handle()); + } + } + + void + 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), + 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) { + throw std::runtime_error( + std::string( + "Couldn't initialize state machine for node ") + node_base_interface_->get_name()); + } + + { // change_state + 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); + + srv_change_state_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_change_state, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_change_state_), + nullptr); + } + + { // get_state + 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)); + + srv_get_state_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_state, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_state_), + nullptr); + } + + { // get_available_states + 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)); + + srv_get_available_states_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_available_states, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_available_states_), + nullptr); + } + + { // get_available_transitions + 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)); + + srv_get_available_transitions_ = + std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_available_transitions, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_available_transitions_), + nullptr); + } + } + + bool + register_callback(std::uint8_t lifecycle_transition, std::function & cb) + { + cb_map_[lifecycle_transition] = cb; + return true; + } + + void + on_change_state(const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) + { + (void)header; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get state. State machine is not initialized."); + } + resp->success = change_state(req->transition.id); + } + + void + 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) { + 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; + } + + 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) { + throw std::runtime_error( + "Can't get available states. State machine is not initialized."); + } + 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); + resp->available_states.push_back(state); + } + } + + 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) { + throw std::runtime_error( + "Can't get available transitions. State machine is not initialized."); + return; + } + + 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); + } + } + + const State & + get_current_state() + { + current_state_ = State(state_machine_.current_state); + return current_state_; + } + + std::vector + get_available_states() + { + std::vector states; + 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); + } + return states; + } + + std::vector + get_available_transitions() + { + std::vector transitions; + + 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; + } + + bool + change_state(std::uint8_t lifecycle_transition) + { + 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__, node_base_interface_->get_name(), rcl_get_error_string_safe()); + 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_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()); + return false; + } + + rcl_lifecycle_ret_t cb_success = execute_callback( + state_machine_.current_state->id, initial_state); + + if (rcl_lifecycle_trigger_transition( + &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; + } + + // 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); + 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_, 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_, error_resolved, 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; + } + + rcl_lifecycle_ret_t + execute_callback(unsigned int cb_id, const State & previous_state) + { + // 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()) { + auto callback = it->second; + try { + cb_success = callback(State(previous_state)); + } 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; + } + } + 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) + { + weak_pubs_.push_back(pub); + } + + void + add_timer_handle(std::shared_ptr timer) + { + weak_timers_.push_back(timer); + } + + rcl_lifecycle_state_machine_t state_machine_; + State current_state_; + std::map< + std::uint8_t, + std::function> cb_map_; + + using NodeBasePtr = std::shared_ptr; + using NodeServicesPtr = std::shared_ptr; + using ChangeStateSrvPtr = std::shared_ptr>; + using GetStateSrvPtr = std::shared_ptr>; + using GetAvailableStatesSrvPtr = + std::shared_ptr>; + using GetAvailableTransitionsSrvPtr = + std::shared_ptr>; + + 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_timers_; +}; + +} // namespace rclcpp_lifecycle +#endif // LIFECYCLE_NODE_INTERFACE_IMPL_HPP_ diff --git a/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp b/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp new file mode 100644 index 0000000..80c7c68 --- /dev/null +++ b/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp @@ -0,0 +1,58 @@ +// 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 +{ +rcl_lifecycle_ret_t +LifecycleNodeInterface::on_configure(const State &) +{ + return RCL_LIFECYCLE_RET_OK; +} + +rcl_lifecycle_ret_t +LifecycleNodeInterface::on_cleanup(const State &) +{ + return RCL_LIFECYCLE_RET_OK; +} + +rcl_lifecycle_ret_t +LifecycleNodeInterface::on_shutdown(const State &) +{ + return RCL_LIFECYCLE_RET_OK; +} + +rcl_lifecycle_ret_t +LifecycleNodeInterface::on_activate(const State &) +{ + return RCL_LIFECYCLE_RET_OK; +} + +rcl_lifecycle_ret_t +LifecycleNodeInterface::on_deactivate(const State &) +{ + return RCL_LIFECYCLE_RET_OK; +} + +rcl_lifecycle_ret_t +LifecycleNodeInterface::on_error(const State &) +{ + return RCL_LIFECYCLE_RET_FAILURE; +} + +} // namespace node_interfaces +} // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp new file mode 100644 index 0000000..40f8dd9 --- /dev/null +++ b/rclcpp_lifecycle/src/state.cpp @@ -0,0 +1,68 @@ +// 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/state.hpp" + +#include +#include + +#include + +namespace rclcpp_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 rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/transition.cpp b/rclcpp_lifecycle/src/transition.cpp new file mode 100644 index 0000000..6bf7434 --- /dev/null +++ b/rclcpp_lifecycle/src/transition.cpp @@ -0,0 +1,106 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp_lifecycle/transition.hpp" + +#include +#include + +#include + +namespace rclcpp_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->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; +} + +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_) { + if (transition_handle_->start) { + delete transition_handle_->start; + } + if (transition_handle_->goal) { + delete transition_handle_->goal; + } + delete transition_handle_; + } +} + +unsigned int +Transition::id() const +{ + return transition_handle_->id; +} + +std::string +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 rclcpp_lifecycle diff --git a/rclcpp_lifecycle/test/test_callback_exceptions.cpp b/rclcpp_lifecycle/test/test_callback_exceptions.cpp new file mode 100644 index 0000000..60fcfa2 --- /dev/null +++ b/rclcpp_lifecycle/test/test_callback_exceptions.cpp @@ -0,0 +1,102 @@ +// 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("custom exception raised in configure callback"); + } + + rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State &) + { + ++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("custom exception raised in configure callback"); + } + + rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State &) + { + ++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); +} diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp new file mode 100644 index 0000000..227b6f0 --- /dev/null +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -0,0 +1,174 @@ +// 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; + +struct GoodMood +{ + static constexpr rcl_lifecycle_ret_t cb_ret = RCL_LIFECYCLE_RET_OK; +}; +struct BadMood +{ + static constexpr rcl_lifecycle_ret_t cb_ret = RCL_LIFECYCLE_RET_FAILURE; +}; +struct VeryBadMood +{ + static constexpr rcl_lifecycle_ret_t cb_ret = RCL_LIFECYCLE_RET_ERROR; +}; + +class TestDefaultStateMachine : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit EmptyLifecycleNode(std::string node_name) + : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) + {} +}; + +template +class MoodyLifecycleNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit MoodyLifecycleNode(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 &) + { + EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id()); + ++number_of_callbacks; + return Mood::cb_ret; + } + + 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; + } + + 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; + } + + 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; + } + + 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; + } + + rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State &); +}; + +template<> +rcl_lifecycle_ret_t MoodyLifecycleNode::on_error(const rclcpp_lifecycle::State &) +{ + EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id()); + ADD_FAILURE(); + return RCL_LIFECYCLE_RET_ERROR; +} +template<> +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 RCL_LIFECYCLE_RET_OK; +} + +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(static_cast(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(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 new file mode 100644 index 0000000..2ae91b9 --- /dev/null +++ b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp @@ -0,0 +1,155 @@ +// 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: + rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &) + { + ADD_FAILURE(); + ++number_of_callbacks; + return RCL_LIFECYCLE_RET_OK; + } + + rcl_lifecycle_ret_t on_activate(const rclcpp_lifecycle::State &) + { + ADD_FAILURE(); + ++number_of_callbacks; + return RCL_LIFECYCLE_RET_OK; + } + + rcl_lifecycle_ret_t on_deactivate(const rclcpp_lifecycle::State &) + { + ADD_FAILURE(); + ++number_of_callbacks; + return RCL_LIFECYCLE_RET_OK; + } + + rcl_lifecycle_ret_t on_cleanup(const rclcpp_lifecycle::State &) + { + ADD_FAILURE(); + ++number_of_callbacks; + return RCL_LIFECYCLE_RET_OK; + } + + rcl_lifecycle_ret_t on_shutdown(const rclcpp_lifecycle::State &) + { + ADD_FAILURE(); + ++number_of_callbacks; + return RCL_LIFECYCLE_RET_OK; + } + + // Custom callbacks + +public: + 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 RCL_LIFECYCLE_RET_OK; + } + + 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 RCL_LIFECYCLE_RET_OK; + } + + 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 RCL_LIFECYCLE_RET_OK; + } + + 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 RCL_LIFECYCLE_RET_OK; + } + + 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 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, 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, 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( + 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); +} 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 0000000..caf5e87 --- /dev/null +++ b/rclcpp_lifecycle/test/test_state_machine_info.cpp @@ -0,0 +1,73 @@ +// 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)25, available_transitions.size()); + for (rclcpp_lifecycle::Transition & transition : available_transitions) { + 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()); + } +}