add rclcpp lifecycle
* 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 (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 * (dev) construct service from existing rcl_service_t * service takes rcl_node_t* * correct typo * add_service has to be public * uncrustify * (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) make find_package(rmw) required * (refactor) attach sm to lifecycle node and disable lc_manager * (fix) adjust code to rcl_test refactor * (dev) remove unused deps * (rebase) merge commit * (bugfix) correct rcl_ret_t error handling * (fix) depedencies * (refactor) change to lifecycle_msgs * (fix) correct find_rcl * (refactor) comply for new state machine * visibility control and test api * (rebase) change to new typesupport * uncrustify' * fix visibility control * (fix) correct whitespace * (fix) unused variable * comparison signed and unsigned * get_state returns complete state * get_available_states service * new service msgs * get available states and transitions api * (broken) state after rebase, does not compile demos * fix the way lifecycle node impl is included * fixed rebase compilation errors * remove copy&paste comment * remove empty line * (test) register custom callbacks * (dev) return codes * style * test for exception handling * refacotr new state machine * c++14 * change exception message for windows ci bug change exception message for windows ci bug
This commit is contained in:
Karsten Knese
GitHub
parent
d241a730fe
commit
2c6d95946e
19 changed files with 2712 additions and 0 deletions
99
rclcpp_lifecycle/CMakeLists.txt
Normal file
99
rclcpp_lifecycle/CMakeLists.txt
Normal file
|
|
@ -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)
|
||||
416
rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp
Normal file
416
rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp
Normal file
|
|
@ -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 <string>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#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<LifecycleNode>
|
||||
{
|
||||
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<rclcpp::callback_group::CallbackGroup::WeakPtr> &
|
||||
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<typename MessageT, typename Alloc = std::allocator<void>>
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<MessageT, Alloc>>
|
||||
create_publisher(
|
||||
const std::string & topic_name, size_t qos_history_depth,
|
||||
std::shared_ptr<Alloc> 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<typename MessageT, typename Alloc = std::allocator<void>>
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<MessageT, Alloc>>
|
||||
create_publisher(
|
||||
const std::string & topic_name,
|
||||
const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
|
||||
std::shared_ptr<Alloc> 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<void>,
|
||||
typename SubscriptionT = rclcpp::subscription::Subscription<MessageT, Alloc>>
|
||||
std::shared_ptr<SubscriptionT>
|
||||
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<MessageT, Alloc>::SharedPtr
|
||||
msg_mem_strat = nullptr,
|
||||
std::shared_ptr<Alloc> 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<void>,
|
||||
typename SubscriptionT = rclcpp::subscription::Subscription<MessageT, Alloc>>
|
||||
std::shared_ptr<SubscriptionT>
|
||||
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<MessageT, Alloc>::SharedPtr
|
||||
msg_mem_strat = nullptr,
|
||||
std::shared_ptr<Alloc> 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 DurationT = std::milli, typename CallbackT>
|
||||
typename rclcpp::timer::WallTimer<CallbackT>::SharedPtr
|
||||
create_wall_timer(
|
||||
std::chrono::duration<int64_t, DurationT> period,
|
||||
CallbackT callback,
|
||||
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
|
||||
|
||||
/* Create and return a Client. */
|
||||
template<typename ServiceT>
|
||||
typename rclcpp::client::Client<ServiceT>::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 ServiceT, typename CallbackT>
|
||||
typename rclcpp::service::Service<ServiceT>::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<rcl_interfaces::msg::SetParametersResult>
|
||||
set_parameters(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
|
||||
|
||||
RCLCPP_LIFECYCLE_PUBLIC
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
|
||||
|
||||
RCLCPP_LIFECYCLE_PUBLIC
|
||||
std::vector<rclcpp::parameter::ParameterVariant>
|
||||
get_parameters(const std::vector<std::string> & 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<typename ParameterT>
|
||||
bool
|
||||
get_parameter(const std::string & name, ParameterT & parameter) const;
|
||||
|
||||
RCLCPP_LIFECYCLE_PUBLIC
|
||||
std::vector<rcl_interfaces::msg::ParameterDescriptor>
|
||||
describe_parameters(const std::vector<std::string> & names) const;
|
||||
|
||||
RCLCPP_LIFECYCLE_PUBLIC
|
||||
std::vector<uint8_t>
|
||||
get_parameter_types(const std::vector<std::string> & names) const;
|
||||
|
||||
RCLCPP_LIFECYCLE_PUBLIC
|
||||
rcl_interfaces::msg::ListParametersResult
|
||||
list_parameters(const std::vector<std::string> & 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<typename CallbackT>
|
||||
void
|
||||
register_param_change_callback(CallbackT && callback);
|
||||
|
||||
RCLCPP_LIFECYCLE_PUBLIC
|
||||
std::map<std::string, std::string>
|
||||
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<State>
|
||||
get_available_states();
|
||||
|
||||
RCLCPP_LIFECYCLE_PUBLIC
|
||||
std::vector<Transition>
|
||||
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<rcl_lifecycle_ret_t(const State &)> fcn);
|
||||
RCLCPP_LIFECYCLE_PUBLIC
|
||||
bool
|
||||
register_on_cleanup(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
|
||||
RCLCPP_LIFECYCLE_PUBLIC
|
||||
bool
|
||||
register_on_shutdown(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
|
||||
RCLCPP_LIFECYCLE_PUBLIC
|
||||
bool
|
||||
register_on_activate(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
|
||||
RCLCPP_LIFECYCLE_PUBLIC
|
||||
bool
|
||||
register_on_deactivate(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
|
||||
RCLCPP_LIFECYCLE_PUBLIC
|
||||
bool
|
||||
register_on_error(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
|
||||
|
||||
protected:
|
||||
RCLCPP_LIFECYCLE_PUBLIC
|
||||
void
|
||||
add_publisher_handle(std::shared_ptr<rclcpp_lifecycle::LifecyclePublisherInterface> pub);
|
||||
|
||||
RCLCPP_LIFECYCLE_PUBLIC
|
||||
void
|
||||
add_timer_handle(std::shared_ptr<rclcpp::timer::TimerBase> 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<LifecycleNodeInterfaceImpl> 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_
|
||||
|
|
@ -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 <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#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<typename MessageT, typename Alloc>
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<MessageT, Alloc>>
|
||||
LifecycleNode::create_publisher(
|
||||
const std::string & topic_name, size_t qos_history_depth,
|
||||
std::shared_ptr<Alloc> allocator)
|
||||
{
|
||||
if (!allocator) {
|
||||
allocator = std::make_shared<Alloc>();
|
||||
}
|
||||
rmw_qos_profile_t qos = rmw_qos_profile_default;
|
||||
qos.depth = qos_history_depth;
|
||||
return this->create_publisher<MessageT, Alloc>(topic_name, qos, allocator);
|
||||
}
|
||||
|
||||
template<typename MessageT, typename Alloc>
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<MessageT, Alloc>>
|
||||
LifecycleNode::create_publisher(
|
||||
const std::string & topic_name,
|
||||
const rmw_qos_profile_t & qos_profile,
|
||||
std::shared_ptr<Alloc> allocator)
|
||||
{
|
||||
using PublisherT = rclcpp_lifecycle::LifecyclePublisher<MessageT, Alloc>;
|
||||
|
||||
// create regular publisher in rclcpp::Node
|
||||
return rclcpp::create_publisher<MessageT, Alloc, PublisherT>(
|
||||
this->node_topics_.get(),
|
||||
topic_name,
|
||||
qos_profile,
|
||||
use_intra_process_comms_,
|
||||
allocator);
|
||||
}
|
||||
|
||||
// TODO(karsten1987): Create LifecycleSubscriber
|
||||
template<typename MessageT, typename CallbackT, typename Alloc, typename SubscriptionT>
|
||||
std::shared_ptr<SubscriptionT>
|
||||
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<MessageT, Alloc>::SharedPtr
|
||||
msg_mem_strat,
|
||||
std::shared_ptr<Alloc> allocator)
|
||||
{
|
||||
if (!allocator) {
|
||||
allocator = std::make_shared<Alloc>();
|
||||
}
|
||||
|
||||
if (!msg_mem_strat) {
|
||||
using rclcpp::message_memory_strategy::MessageMemoryStrategy;
|
||||
msg_mem_strat = MessageMemoryStrategy<MessageT, Alloc>::create_default();
|
||||
}
|
||||
|
||||
return rclcpp::create_subscription<
|
||||
MessageT, CallbackT, Alloc,
|
||||
rclcpp::subscription::Subscription<MessageT, Alloc>>(
|
||||
this->node_topics_.get(),
|
||||
topic_name,
|
||||
std::forward<CallbackT>(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<SubscriptionT>
|
||||
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<MessageT, Alloc>::SharedPtr
|
||||
msg_mem_strat,
|
||||
std::shared_ptr<Alloc> allocator)
|
||||
{
|
||||
rmw_qos_profile_t qos = rmw_qos_profile_default;
|
||||
qos.depth = qos_history_depth;
|
||||
return this->create_subscription<MessageT, CallbackT, Alloc>(
|
||||
topic_name,
|
||||
std::forward<CallbackT>(callback),
|
||||
qos,
|
||||
group,
|
||||
ignore_local_publications,
|
||||
msg_mem_strat,
|
||||
allocator);
|
||||
}
|
||||
|
||||
template<typename DurationT, typename CallbackT>
|
||||
typename rclcpp::timer::WallTimer<CallbackT>::SharedPtr
|
||||
LifecycleNode::create_wall_timer(
|
||||
std::chrono::duration<int64_t, DurationT> period,
|
||||
CallbackT callback,
|
||||
rclcpp::callback_group::CallbackGroup::SharedPtr group)
|
||||
{
|
||||
auto timer = rclcpp::timer::WallTimer<CallbackT>::make_shared(
|
||||
std::chrono::duration_cast<std::chrono::nanoseconds>(period),
|
||||
std::move(callback));
|
||||
node_timers_->add_timer(timer, group);
|
||||
return timer;
|
||||
}
|
||||
|
||||
template<typename ServiceT>
|
||||
typename rclcpp::client::Client<ServiceT>::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<ServiceT>::make_shared(
|
||||
node_base_.get(),
|
||||
node_graph_,
|
||||
service_name,
|
||||
options);
|
||||
|
||||
auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
|
||||
node_services_->add_client(cli_base_ptr, group);
|
||||
return cli;
|
||||
}
|
||||
|
||||
template<typename ServiceT, typename CallbackT>
|
||||
typename rclcpp::service::Service<ServiceT>::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<ServiceT> any_service_callback;
|
||||
any_service_callback.set(std::forward<CallbackT>(callback));
|
||||
|
||||
rcl_service_options_t service_options = rcl_service_get_default_options();
|
||||
service_options.qos = qos_profile;
|
||||
|
||||
auto serv = rclcpp::service::Service<ServiceT>::make_shared(
|
||||
node_base_->get_shared_rcl_node_handle(),
|
||||
service_name, any_service_callback, service_options);
|
||||
auto serv_base_ptr = std::dynamic_pointer_cast<rclcpp::service::ServiceBase>(serv);
|
||||
node_services_->add_service(serv_base_ptr, group);
|
||||
return serv;
|
||||
}
|
||||
|
||||
template<typename ParameterT>
|
||||
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<ParameterT>();
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
template<typename CallbackT>
|
||||
void
|
||||
LifecycleNode::register_param_change_callback(CallbackT && callback)
|
||||
{
|
||||
this->node_parameters_->register_param_change_callback(std::forward<CallbackT>(callback));
|
||||
}
|
||||
|
||||
} // namespace rclcpp_lifecycle
|
||||
#endif // RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_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 <memory>
|
||||
#include <string>
|
||||
|
||||
#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<typename MessageT, typename Alloc = std::allocator<void>>
|
||||
class LifecyclePublisher : public LifecyclePublisherInterface,
|
||||
public rclcpp::publisher::Publisher<MessageT, Alloc>
|
||||
{
|
||||
public:
|
||||
using MessageAllocTraits = rclcpp::allocator::AllocRebind<MessageT, Alloc>;
|
||||
using MessageAlloc = typename MessageAllocTraits::allocator_type;
|
||||
using MessageDeleter = rclcpp::allocator::Deleter<MessageAlloc, MessageT>;
|
||||
using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
|
||||
|
||||
LifecyclePublisher(
|
||||
rclcpp::node_interfaces::NodeBaseInterface * node_base,
|
||||
const std::string & topic,
|
||||
const rcl_publisher_options_t & publisher_options,
|
||||
std::shared_ptr<MessageAlloc> allocator)
|
||||
: rclcpp::publisher::Publisher<MessageT, Alloc>(
|
||||
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<MessageT, MessageDeleter> & msg)
|
||||
{
|
||||
if (!enabled_) {
|
||||
return;
|
||||
}
|
||||
rclcpp::publisher::Publisher<MessageT, Alloc>::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<MessageT> & msg)
|
||||
{
|
||||
if (!enabled_) {
|
||||
return;
|
||||
}
|
||||
rclcpp::publisher::Publisher<MessageT, Alloc>::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<const MessageT> msg)
|
||||
{
|
||||
if (!enabled_) {
|
||||
return;
|
||||
}
|
||||
rclcpp::publisher::Publisher<MessageT, Alloc>::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<MessageT, Alloc>::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<const MessageT> & msg)
|
||||
{
|
||||
if (!enabled_) {
|
||||
return;
|
||||
}
|
||||
rclcpp::publisher::Publisher<MessageT, Alloc>::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_
|
||||
|
|
@ -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_
|
||||
57
rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp
Normal file
57
rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp
Normal file
|
|
@ -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 <string>
|
||||
|
||||
#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_
|
||||
72
rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp
Normal file
72
rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp
Normal file
|
|
@ -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 <string>
|
||||
|
||||
#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_
|
||||
|
|
@ -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 <utility>
|
||||
#include <type_traits>
|
||||
|
||||
template<class T, typename = void>
|
||||
struct has_on_activate
|
||||
{
|
||||
static constexpr bool value = false;
|
||||
};
|
||||
|
||||
template<class T>
|
||||
struct has_on_activate<T, typename std::enable_if<
|
||||
std::is_same<void, decltype(std::declval<T>().on_activate())>::value>::type>
|
||||
{
|
||||
static constexpr bool value = true;
|
||||
};
|
||||
|
||||
template<class T, typename = void>
|
||||
struct has_on_deactivate
|
||||
{
|
||||
static constexpr bool value = false;
|
||||
};
|
||||
|
||||
template<class T>
|
||||
struct has_on_deactivate<T, typename std::enable_if<
|
||||
std::is_same<void, decltype(std::declval<T>().on_deactivate())>::value>::type>
|
||||
{
|
||||
static constexpr bool value = true;
|
||||
};
|
||||
|
||||
template<class T, typename = void>
|
||||
struct is_manageable_node : std::false_type
|
||||
{};
|
||||
|
||||
template<class T>
|
||||
struct is_manageable_node<T, typename std::enable_if<
|
||||
has_on_activate<T>::value && has_on_deactivate<T>::value>::type>: std::true_type
|
||||
{};
|
||||
|
||||
#endif // 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.
|
||||
|
||||
/* 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_
|
||||
36
rclcpp_lifecycle/package.xml
Normal file
36
rclcpp_lifecycle/package.xml
Normal file
|
|
@ -0,0 +1,36 @@
|
|||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="2">
|
||||
<name>rclcpp_lifecycle</name>
|
||||
<version>0.0.0</version>
|
||||
<description>Package containing a prototype for lifecycle implementation</description>
|
||||
<maintainer email="karsten@osrfoundation.org">Karsten Knese</maintainer>
|
||||
<license>Apache License 2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
|
||||
<build_depend>rclcpp</build_depend>
|
||||
<build_depend>rcl_lifecycle</build_depend>
|
||||
<build_depend>rmw_implementation</build_depend>
|
||||
<build_depend>rmw_implementation_cmake</build_depend>
|
||||
<build_depend>rosidl_default_generators</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>lifecycle_msgs</build_depend>
|
||||
|
||||
<exec_depend>rclcpp</exec_depend>
|
||||
<exec_depend>rcl_lifecycle</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>rmw_implementation</exec_depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>lifecycle_msgs</exec_depend>
|
||||
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
310
rclcpp_lifecycle/src/lifecycle_node.cpp
Normal file
310
rclcpp_lifecycle/src/lifecycle_node.cpp
Normal file
|
|
@ -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 <string>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
|
||||
#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<rcl_interfaces::msg::SetParametersResult>
|
||||
LifecycleNode::set_parameters(
|
||||
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
|
||||
{
|
||||
return node_parameters_->set_parameters(parameters);
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
LifecycleNode::set_parameters_atomically(
|
||||
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
|
||||
{
|
||||
return node_parameters_->set_parameters_atomically(parameters);
|
||||
}
|
||||
|
||||
std::vector<rclcpp::parameter::ParameterVariant>
|
||||
LifecycleNode::get_parameters(
|
||||
const std::vector<std::string> & 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<rcl_interfaces::msg::ParameterDescriptor>
|
||||
LifecycleNode::describe_parameters(
|
||||
const std::vector<std::string> & names) const
|
||||
{
|
||||
return node_parameters_->describe_parameters(names);
|
||||
}
|
||||
|
||||
std::vector<uint8_t>
|
||||
LifecycleNode::get_parameter_types(
|
||||
const std::vector<std::string> & names) const
|
||||
{
|
||||
return node_parameters_->get_parameter_types(names);
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::ListParametersResult
|
||||
LifecycleNode::list_parameters(
|
||||
const std::vector<std::string> & prefixes, uint64_t depth) const
|
||||
{
|
||||
return node_parameters_->list_parameters(prefixes, depth);
|
||||
}
|
||||
|
||||
std::map<std::string, std::string>
|
||||
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<rclcpp::callback_group::CallbackGroup::WeakPtr> &
|
||||
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<rcl_lifecycle_ret_t(const State &)> fcn)
|
||||
{
|
||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn);
|
||||
}
|
||||
|
||||
bool
|
||||
LifecycleNode::register_on_cleanup(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
||||
{
|
||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn);
|
||||
}
|
||||
|
||||
bool
|
||||
LifecycleNode::register_on_shutdown(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
||||
{
|
||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn);
|
||||
}
|
||||
|
||||
bool
|
||||
LifecycleNode::register_on_activate(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
||||
{
|
||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn);
|
||||
}
|
||||
|
||||
bool
|
||||
LifecycleNode::register_on_deactivate(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
||||
{
|
||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn);
|
||||
}
|
||||
|
||||
bool
|
||||
LifecycleNode::register_on_error(std::function<rcl_lifecycle_ret_t(const State &)> 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<State>
|
||||
LifecycleNode::get_available_states()
|
||||
{
|
||||
return impl_->get_available_states();
|
||||
}
|
||||
|
||||
std::vector<Transition>
|
||||
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<rclcpp_lifecycle::LifecyclePublisherInterface> pub)
|
||||
{
|
||||
impl_->add_publisher_handle(pub);
|
||||
}
|
||||
|
||||
void
|
||||
LifecycleNode::add_timer_handle(std::shared_ptr<rclcpp::timer::TimerBase> timer)
|
||||
{
|
||||
impl_->add_timer_handle(timer);
|
||||
}
|
||||
|
||||
} // namespace rclcpp_lifecycle
|
||||
397
rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp
Normal file
397
rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp
Normal file
|
|
@ -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 <functional>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
|
||||
#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<rclcpp::node_interfaces::NodeBaseInterface> node_base_interface,
|
||||
std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> 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<ChangeStateSrv>(),
|
||||
rosidl_typesupport_cpp::get_service_type_support_handle<GetStateSrv>(),
|
||||
rosidl_typesupport_cpp::get_service_type_support_handle<GetAvailableStatesSrv>(),
|
||||
rosidl_typesupport_cpp::get_service_type_support_handle<GetAvailableTransitionsSrv>(),
|
||||
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<ChangeStateSrv> any_cb;
|
||||
any_cb.set(cb);
|
||||
|
||||
srv_change_state_ = std::make_shared<rclcpp::service::Service<ChangeStateSrv>>(
|
||||
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<rclcpp::service::ServiceBase>(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<GetStateSrv> any_cb;
|
||||
any_cb.set(std::move(cb));
|
||||
|
||||
srv_get_state_ = std::make_shared<rclcpp::service::Service<GetStateSrv>>(
|
||||
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<rclcpp::service::ServiceBase>(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<GetAvailableStatesSrv> any_cb;
|
||||
any_cb.set(std::move(cb));
|
||||
|
||||
srv_get_available_states_ = std::make_shared<rclcpp::service::Service<GetAvailableStatesSrv>>(
|
||||
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<rclcpp::service::ServiceBase>(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<GetAvailableTransitionsSrv> any_cb;
|
||||
any_cb.set(std::move(cb));
|
||||
|
||||
srv_get_available_transitions_ =
|
||||
std::make_shared<rclcpp::service::Service<GetAvailableTransitionsSrv>>(
|
||||
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<rclcpp::service::ServiceBase>(srv_get_available_transitions_),
|
||||
nullptr);
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
register_callback(std::uint8_t lifecycle_transition, std::function<rcl_lifecycle_ret_t(
|
||||
const State &)> & cb)
|
||||
{
|
||||
cb_map_[lifecycle_transition] = cb;
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
on_change_state(const std::shared_ptr<rmw_request_id_t> header,
|
||||
const std::shared_ptr<ChangeStateSrv::Request> req,
|
||||
std::shared_ptr<ChangeStateSrv::Response> 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<rmw_request_id_t> header,
|
||||
const std::shared_ptr<GetStateSrv::Request> req,
|
||||
std::shared_ptr<GetStateSrv::Response> 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<uint8_t>(state_machine_.current_state->id);
|
||||
resp->current_state.label = state_machine_.current_state->label;
|
||||
}
|
||||
|
||||
void
|
||||
on_get_available_states(const std::shared_ptr<rmw_request_id_t> header,
|
||||
const std::shared_ptr<GetAvailableStatesSrv::Request> req,
|
||||
std::shared_ptr<GetAvailableStatesSrv::Response> 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<uint8_t>(state_machine_.transition_map.states[i].id);
|
||||
state.label = static_cast<std::string>(state_machine_.transition_map.states[i].label);
|
||||
resp->available_states.push_back(state);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
on_get_available_transitions(const std::shared_ptr<rmw_request_id_t> header,
|
||||
const std::shared_ptr<GetAvailableTransitionsSrv::Request> req,
|
||||
std::shared_ptr<GetAvailableTransitionsSrv::Response> 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<State>
|
||||
get_available_states()
|
||||
{
|
||||
std::vector<State> 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<Transition>
|
||||
get_available_transitions()
|
||||
{
|
||||
std::vector<Transition> 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<unsigned int>(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<rclcpp_lifecycle::LifecyclePublisherInterface> pub)
|
||||
{
|
||||
weak_pubs_.push_back(pub);
|
||||
}
|
||||
|
||||
void
|
||||
add_timer_handle(std::shared_ptr<rclcpp::timer::TimerBase> timer)
|
||||
{
|
||||
weak_timers_.push_back(timer);
|
||||
}
|
||||
|
||||
rcl_lifecycle_state_machine_t state_machine_;
|
||||
State current_state_;
|
||||
std::map<
|
||||
std::uint8_t,
|
||||
std::function<rcl_lifecycle_ret_t(const State &)>> cb_map_;
|
||||
|
||||
using NodeBasePtr = std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>;
|
||||
using NodeServicesPtr = std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface>;
|
||||
using ChangeStateSrvPtr = std::shared_ptr<rclcpp::service::Service<ChangeStateSrv>>;
|
||||
using GetStateSrvPtr = std::shared_ptr<rclcpp::service::Service<GetStateSrv>>;
|
||||
using GetAvailableStatesSrvPtr =
|
||||
std::shared_ptr<rclcpp::service::Service<GetAvailableStatesSrv>>;
|
||||
using GetAvailableTransitionsSrvPtr =
|
||||
std::shared_ptr<rclcpp::service::Service<GetAvailableTransitionsSrv>>;
|
||||
|
||||
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<std::weak_ptr<rclcpp_lifecycle::LifecyclePublisherInterface>> weak_pubs_;
|
||||
std::vector<std::weak_ptr<rclcpp::timer::TimerBase>> weak_timers_;
|
||||
};
|
||||
|
||||
} // namespace rclcpp_lifecycle
|
||||
#endif // LIFECYCLE_NODE_INTERFACE_IMPL_HPP_
|
||||
|
|
@ -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
|
||||
68
rclcpp_lifecycle/src/state.cpp
Normal file
68
rclcpp_lifecycle/src/state.cpp
Normal file
|
|
@ -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 <lifecycle_msgs/msg/state.hpp>
|
||||
#include <rcl_lifecycle/data_types.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
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
|
||||
106
rclcpp_lifecycle/src/transition.cpp
Normal file
106
rclcpp_lifecycle/src/transition.cpp
Normal file
|
|
@ -0,0 +1,106 @@
|
|||
// Copyright 2016 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "rclcpp_lifecycle/transition.hpp"
|
||||
|
||||
#include <lifecycle_msgs/msg/transition.hpp>
|
||||
#include <rcl_lifecycle/data_types.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
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
|
||||
102
rclcpp_lifecycle/test/test_callback_exceptions.cpp
Normal file
102
rclcpp_lifecycle/test/test_callback_exceptions.cpp
Normal file
|
|
@ -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 <gtest/gtest.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#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<PositiveCallbackExceptionNode>("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<size_t>(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<NegativeCallbackExceptionNode>("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<size_t>(2), test_node->number_of_callbacks);
|
||||
}
|
||||
174
rclcpp_lifecycle/test/test_lifecycle_node.cpp
Normal file
174
rclcpp_lifecycle/test/test_lifecycle_node.cpp
Normal file
|
|
@ -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 <gtest/gtest.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#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 Mood = GoodMood>
|
||||
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<GoodMood>::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<BadMood>::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<EmptyLifecycleNode>("testnode");
|
||||
|
||||
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
|
||||
}
|
||||
|
||||
TEST_F(TestDefaultStateMachine, trigger_transition) {
|
||||
auto test_node = std::make_shared<EmptyLifecycleNode>("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<MoodyLifecycleNode<GoodMood>>("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<size_t>(5), test_node->number_of_callbacks);
|
||||
}
|
||||
|
||||
TEST_F(TestDefaultStateMachine, bad_mood) {
|
||||
auto test_node = std::make_shared<MoodyLifecycleNode<BadMood>>("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<size_t>(1), test_node->number_of_callbacks);
|
||||
}
|
||||
155
rclcpp_lifecycle/test/test_register_custom_callbacks.cpp
Normal file
155
rclcpp_lifecycle/test/test_register_custom_callbacks.cpp
Normal file
|
|
@ -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 <gtest/gtest.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#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<CustomLifecycleNode>("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<size_t>(5), test_node->number_of_callbacks);
|
||||
}
|
||||
73
rclcpp_lifecycle/test/test_state_machine_info.cpp
Normal file
73
rclcpp_lifecycle/test/test_state_machine_info.cpp
Normal file
|
|
@ -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 <gtest/gtest.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#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<rclcpp_lifecycle::LifecycleNode>("testnode");
|
||||
std::vector<rclcpp_lifecycle::State> 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<rclcpp_lifecycle::LifecycleNode>("testnode");
|
||||
std::vector<rclcpp_lifecycle::Transition> 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());
|
||||
}
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue