From 72c3ff66255017c2f7327f341cb689c4e42df6a9 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 10 Dec 2021 17:01:50 -0300 Subject: [PATCH 01/34] wip Signed-off-by: Ivan Santiago Paunovic --- rclpy/CMakeLists.txt | 10 +- rclpy/package.xml | 1 + rclpy/src/rclpy/_rclpy_pybind11.cpp | 2 + rclpy/src/rclpy/lifecycle.cpp | 196 ++++++++++++++++++++++++++++ rclpy/src/rclpy/lifecycle.hpp | 31 +++++ rclpy/src/rclpy/service.cpp | 11 +- rclpy/src/rclpy/service.hpp | 5 +- 7 files changed, 250 insertions(+), 6 deletions(-) create mode 100644 rclpy/src/rclpy/lifecycle.cpp create mode 100644 rclpy/src/rclpy/lifecycle.hpp diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index 694c3e312..7e8780023 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -16,9 +16,11 @@ endif() find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) +find_package(lifecycle_msgs REQUIRED) find_package(rcl REQUIRED) -find_package(rcl_logging_interface REQUIRED) find_package(rcl_action REQUIRED) +find_package(rcl_lifecycle REQUIRED) +find_package(rcl_logging_interface REQUIRED) find_package(rcl_yaml_param_parser REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) @@ -113,6 +115,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED src/rclpy/exceptions.cpp src/rclpy/graph.cpp src/rclpy/guard_condition.cpp + src/rclpy/lifecycle.cpp src/rclpy/logging.cpp src/rclpy/names.cpp src/rclpy/node.cpp @@ -132,9 +135,12 @@ pybind11_add_module(_rclpy_pybind11 SHARED target_include_directories(_rclpy_pybind11 PRIVATE src/rclpy/ ) -target_link_libraries(_rclpy_pybind11 PRIVATE +target_link_libraries(_rclpy_pybind11 PRIVATE + lifecycle_msgs::lifecycle_msgs__rosidl_generator_c + lifecycle_msgs::lifecycle_msgs__rosidl_typesupport_c rcl::rcl rcl_action::rcl_action + rcl_lifecycle::rcl_lifecycle rcl_logging_interface::rcl_logging_interface rcpputils::rcpputils rcutils::rcutils diff --git a/rclpy/package.xml b/rclpy/package.xml index 80aacc07d..722c71063 100644 --- a/rclpy/package.xml +++ b/rclpy/package.xml @@ -25,6 +25,7 @@ rmw_implementation rcl + rcl_lifecycle rcl_logging_interface rcl_action rcl_yaml_param_parser diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index 3a78e6b5e..f8230b88f 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -29,6 +29,7 @@ #include "exceptions.hpp" #include "graph.hpp" #include "guard_condition.hpp" +#include "lifecycle.hpp" #include "logging.hpp" #include "logging_api.hpp" #include "names.hpp" @@ -230,4 +231,5 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { rclpy::define_logging_api(m); rclpy::define_signal_handler_api(m); rclpy::define_clock_event(m); + rclpy::define_lifecycle_api(m); } diff --git a/rclpy/src/rclpy/lifecycle.cpp b/rclpy/src/rclpy/lifecycle.cpp new file mode 100644 index 000000000..3ab01201e --- /dev/null +++ b/rclpy/src/rclpy/lifecycle.cpp @@ -0,0 +1,196 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "destroyable.hpp" +#include "exceptions.hpp" +#include "lifecycle.hpp" +#include "node.hpp" + +namespace py = pybind11; + +namespace +{ +class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shared_from_this +{ +public: + LifecycleStateMachine( + rclpy::Node node, bool enable_com_interface) + : node_(std::move(node)) + { + state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); + auto state_machine_options = rcl_lifecycle_get_default_state_machine_options(); + state_machine_options.enable_com_interface = enable_com_interface; + rcl_ret_t ret = rcl_lifecycle_state_machine_init( + &state_machine_, + node_.rcl_ptr(), + ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), + ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, ChangeState), + ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetState), + ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableStates), + ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableTransitions), + ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableTransitions), + &state_machine_options); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to create lifecycle state machine"); + } + } + + void init() + { + if (state_machine_.options.enable_com_interface) { + + } + // if (enable_com_interface) { + // { + // srv_change_state_ = rclpy::Service( + // node, + // &state_machine_.com_interface.srv_change_state); + // node_services_interface_->add_service( + // std::dynamic_pointer_cast(srv_change_state_), + // nullptr); + // } + + // { // get_state + // auto cb = std::bind( + // &LifecycleNodeInterfaceImpl::on_get_state, this, + // std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + // rclcpp::AnyServiceCallback any_cb; + // any_cb.set(std::move(cb)); + + // srv_get_state_ = std::make_shared>( + // node_base_interface_->get_shared_rcl_node_handle(), + // &state_machine_.com_interface.srv_get_state, + // any_cb); + // node_services_interface_->add_service( + // std::dynamic_pointer_cast(srv_get_state_), + // nullptr); + // } + + // { // get_available_states + // auto cb = std::bind( + // &LifecycleNodeInterfaceImpl::on_get_available_states, this, + // std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + // rclcpp::AnyServiceCallback any_cb; + // any_cb.set(std::move(cb)); + + // srv_get_available_states_ = std::make_shared>( + // node_base_interface_->get_shared_rcl_node_handle(), + // &state_machine_.com_interface.srv_get_available_states, + // any_cb); + // node_services_interface_->add_service( + // std::dynamic_pointer_cast(srv_get_available_states_), + // nullptr); + // } + + // { // get_available_transitions + // auto cb = std::bind( + // &LifecycleNodeInterfaceImpl::on_get_available_transitions, this, + // std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + // rclcpp::AnyServiceCallback any_cb; + // any_cb.set(std::move(cb)); + + // srv_get_available_transitions_ = + // std::make_shared>( + // node_base_interface_->get_shared_rcl_node_handle(), + // &state_machine_.com_interface.srv_get_available_transitions, + // any_cb); + // node_services_interface_->add_service( + // std::dynamic_pointer_cast(srv_get_available_transitions_), + // nullptr); + // } + + // { // get_transition_graph + // auto cb = std::bind( + // &LifecycleNodeInterfaceImpl::on_get_transition_graph, this, + // std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + // rclcpp::AnyServiceCallback any_cb; + // any_cb.set(std::move(cb)); + + // srv_get_transition_graph_ = + // std::make_shared>( + // node_base_interface_->get_shared_rcl_node_handle(), + // &state_machine_.com_interface.srv_get_transition_graph, + // any_cb); + // node_services_interface_->add_service( + // std::dynamic_pointer_cast(srv_get_transition_graph_), + // nullptr); + // } + // } + } + + ~LifecycleStateMachine() + { + + } + + bool + is_initialized() + { + return RCL_RET_OK == rcl_lifecycle_state_machine_is_initialized(&state_machine_); + } + + void + trigger_transition_by_id(uint8_t transition_id, bool publish_update) + { + if ( + rcl_lifecycle_trigger_transition_by_id( + &state_machine_, transition_id, publish_update) != RCL_RET_OK) + { + throw rclpy::RCLError("Failed to trigger lifecycle state machine transition");; + } + } + + void + trigger_transition_by_label(std::string label, bool publish_update) + { + if ( + rcl_lifecycle_trigger_transition_by_label( + &state_machine_, label.c_str(), publish_update) != RCL_RET_OK) + { + throw rclpy::RCLError("Failed to trigger lifecycle state machine transition");; + } + } + + void + print() + { + rcl_print_state_machine(&state_machine_); + } + +private: + rclpy::Node node_; + rcl_lifecycle_state_machine_t state_machine_; +}; +} + +namespace rclpy +{ +void +define_lifecycle_api(py::module m) +{ +} +} // namespace rclpy diff --git a/rclpy/src/rclpy/lifecycle.hpp b/rclpy/src/rclpy/lifecycle.hpp new file mode 100644 index 000000000..55f841a01 --- /dev/null +++ b/rclpy/src/rclpy/lifecycle.hpp @@ -0,0 +1,31 @@ +// Copyright 2021 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 RCLPY__LIFECYCLE_API_HPP_ +#define RCLPY__LIFECYCLE_API_HPP_ + +#include + +namespace py = pybind11; + +namespace rclpy +{ +/// Define methods on a module for the lifecycle API +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +define_lifecycle_api(py::module module); +} // namespace rclpy +#endif // RCLPY__LIFECYCLE_API_HPP_ diff --git a/rclpy/src/rclpy/service.cpp b/rclpy/src/rclpy/service.cpp index 952f46cd1..dce00cf9a 100644 --- a/rclpy/src/rclpy/service.cpp +++ b/rclpy/src/rclpy/service.cpp @@ -34,9 +34,9 @@ Service::destroy() } Service::Service( - Node & node, py::object pysrv_type, std::string service_name, + Node node, py::object pysrv_type, std::string service_name, py::object pyqos_profile) -: node_(node) +: node_(std::move(node)) { auto srv_type = static_cast( common_get_type_support(pysrv_type)); @@ -86,6 +86,11 @@ Service::Service( } } +Service::Service( + Node node, std::shared_ptr rcl_service) +: node_(std::move(node)), rcl_service_(rcl_service) +{} + void Service::service_send_response(py::object pyresponse, rmw_request_id_t * header) { @@ -126,7 +131,7 @@ void define_service(py::object module) { py::class_>(module, "Service") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Service & service) { return reinterpret_cast(service.rcl_ptr()); diff --git a/rclpy/src/rclpy/service.hpp b/rclpy/src/rclpy/service.hpp index 73faf3fe0..451c88b6a 100644 --- a/rclpy/src/rclpy/service.hpp +++ b/rclpy/src/rclpy/service.hpp @@ -52,9 +52,12 @@ class Service : public Destroyable, public std::enable_shared_from_this * \return capsule containing the rcl_service_t */ Service( - Node & node, py::object pysrv_type, std::string service_name, + Node node, py::object pysrv_type, std::string service_name, py::object pyqos_profile); + Service( + Node node, std::shared_ptr rcl_service); + ~Service() = default; /// Publish a response message From d72dc291decd19a07b6cf11afed1b0c0fdb4c2ed Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 13 Dec 2021 17:05:42 -0300 Subject: [PATCH 02/34] wip 2 Signed-off-by: Ivan Santiago Paunovic --- rclpy/src/rclpy/lifecycle.cpp | 204 +++++++++++++++++++++------------- rclpy/src/rclpy/service.cpp | 10 +- rclpy/src/rclpy/service.hpp | 4 +- 3 files changed, 132 insertions(+), 86 deletions(-) diff --git a/rclpy/src/rclpy/lifecycle.cpp b/rclpy/src/rclpy/lifecycle.cpp index 3ab01201e..2e9396d94 100644 --- a/rclpy/src/rclpy/lifecycle.cpp +++ b/rclpy/src/rclpy/lifecycle.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include #include @@ -29,6 +31,7 @@ #include "exceptions.hpp" #include "lifecycle.hpp" #include "node.hpp" +#include "service.hpp" namespace py = pybind11; @@ -58,93 +61,56 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar throw rclpy::RCLError("Failed to create lifecycle state machine"); } } + + ~LifecycleStateMachine() + { + this->destroy(); + } void init() { if (state_machine_.options.enable_com_interface) { - + srv_change_state_ = std::make_shared( + node_, + std::shared_ptr( + this->shared_from_this(), &state_machine_.com_interface.srv_change_state)); + srv_get_state_ = std::make_shared( + node_, + std::shared_ptr( + this->shared_from_this(), &state_machine_.com_interface.srv_get_state)); + srv_get_available_states_ = std::make_shared( + node_, + std::shared_ptr( + this->shared_from_this(), &state_machine_.com_interface.srv_get_available_states)); + srv_get_available_transitions_ = std::make_shared( + node_, + std::shared_ptr( + this->shared_from_this(), &state_machine_.com_interface.srv_get_available_transitions)); + srv_get_transition_graph_ = std::make_shared( + node_, + std::shared_ptr( + this->shared_from_this(), &state_machine_.com_interface.srv_get_transition_graph)); } - // if (enable_com_interface) { - // { - // srv_change_state_ = rclpy::Service( - // node, - // &state_machine_.com_interface.srv_change_state); - // node_services_interface_->add_service( - // std::dynamic_pointer_cast(srv_change_state_), - // nullptr); - // } - - // { // get_state - // auto cb = std::bind( - // &LifecycleNodeInterfaceImpl::on_get_state, this, - // std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - // rclcpp::AnyServiceCallback any_cb; - // any_cb.set(std::move(cb)); - - // srv_get_state_ = std::make_shared>( - // node_base_interface_->get_shared_rcl_node_handle(), - // &state_machine_.com_interface.srv_get_state, - // any_cb); - // node_services_interface_->add_service( - // std::dynamic_pointer_cast(srv_get_state_), - // nullptr); - // } - - // { // get_available_states - // auto cb = std::bind( - // &LifecycleNodeInterfaceImpl::on_get_available_states, this, - // std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - // rclcpp::AnyServiceCallback any_cb; - // any_cb.set(std::move(cb)); - - // srv_get_available_states_ = std::make_shared>( - // node_base_interface_->get_shared_rcl_node_handle(), - // &state_machine_.com_interface.srv_get_available_states, - // any_cb); - // node_services_interface_->add_service( - // std::dynamic_pointer_cast(srv_get_available_states_), - // nullptr); - // } - - // { // get_available_transitions - // auto cb = std::bind( - // &LifecycleNodeInterfaceImpl::on_get_available_transitions, this, - // std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - // rclcpp::AnyServiceCallback any_cb; - // any_cb.set(std::move(cb)); - - // srv_get_available_transitions_ = - // std::make_shared>( - // node_base_interface_->get_shared_rcl_node_handle(), - // &state_machine_.com_interface.srv_get_available_transitions, - // any_cb); - // node_services_interface_->add_service( - // std::dynamic_pointer_cast(srv_get_available_transitions_), - // nullptr); - // } - - // { // get_transition_graph - // auto cb = std::bind( - // &LifecycleNodeInterfaceImpl::on_get_transition_graph, this, - // std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - // rclcpp::AnyServiceCallback any_cb; - // any_cb.set(std::move(cb)); - - // srv_get_transition_graph_ = - // std::make_shared>( - // node_base_interface_->get_shared_rcl_node_handle(), - // &state_machine_.com_interface.srv_get_transition_graph, - // any_cb); - // node_services_interface_->add_service( - // std::dynamic_pointer_cast(srv_get_transition_graph_), - // nullptr); - // } - // } } - ~LifecycleStateMachine() + void + destroy() override { - + srv_change_state_.reset(); + srv_get_state_.reset(); + srv_get_available_states_.reset(); + srv_get_available_transitions_.reset(); + srv_get_transition_graph_.reset(); + rcl_ret_t ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_.rcl_ptr()); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, "Failed to fini lifecycle state machine: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + node_.destroy(); } bool @@ -175,6 +141,36 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar } } + std::shared_ptr + get_srv_change_state() + { + return srv_change_state_; + } + + std::shared_ptr + get_srv_get_state() + { + return srv_get_state_; + } + + std::shared_ptr + get_srv_get_available_states() + { + return srv_get_available_states_; + } + + std::shared_ptr + get_srv_get_available_transitions() + { + return srv_get_available_transitions_; + } + + std::shared_ptr + get_srv_get_transition_graph() + { + return srv_get_transition_graph_; + } + void print() { @@ -183,8 +179,21 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar private: rclpy::Node node_; + std::shared_ptr srv_change_state_; + std::shared_ptr srv_get_state_; + std::shared_ptr srv_get_available_states_; + std::shared_ptr srv_get_available_transitions_; + std::shared_ptr srv_get_transition_graph_; rcl_lifecycle_state_machine_t state_machine_; }; + +enum class TransitionCallbackReturnType +{ + Success = lifecycle_msgs__msg__Transition__TRANSITION_CALLBACK_SUCCESS, + Failure = lifecycle_msgs__msg__Transition__TRANSITION_CALLBACK_FAILURE, + Error = lifecycle_msgs__msg__Transition__TRANSITION_CALLBACK_ERROR, +}; + } namespace rclpy @@ -192,5 +201,42 @@ namespace rclpy void define_lifecycle_api(py::module m) { + py::class_>( + m, "LifecycleStateMachine") + .def(py::init([](Node node, bool enable_com_interface) + { + auto state_machine = std::make_shared(std::move(node), enable_com_interface); + state_machine->init(); + return state_machine; + } + )) + .def( + "is_initialized", &LifecycleStateMachine::is_initialized, + "Check if state machine is initialized.") + .def( + "trigger_transition_by_id", &LifecycleStateMachine::trigger_transition_by_id, + "Trigger a transition by transition id.") + .def( + "trigger_transition_by_label", &LifecycleStateMachine::trigger_transition_by_id, + "Trigger a transition by label.") + .def( + "get_srv_change_state", &LifecycleStateMachine::get_srv_change_state, + "Get the change state service.") + .def( + "get_srv_get_state", &LifecycleStateMachine::get_srv_get_state, + "Get the get state service.") + .def( + "get_srv_get_available_states", &LifecycleStateMachine::get_srv_get_available_states, + "Get the get available states service.") + .def( + "get_srv_get_available_transitions", &LifecycleStateMachine::get_srv_get_available_transitions, + "Get the get available transitions service.") + .def( + "get_srv_get_transition_graph", &LifecycleStateMachine::get_srv_get_transition_graph, + "Get the get transition graph service."); + py::enum_(m, "TransitionCallbackReturnType") + .value("SUCCESS", TransitionCallbackReturnType::Success) + .value("FAILURE", TransitionCallbackReturnType::Failure) + .value("ERROR", TransitionCallbackReturnType::Error); } } // namespace rclpy diff --git a/rclpy/src/rclpy/service.cpp b/rclpy/src/rclpy/service.cpp index dce00cf9a..c3c6189fd 100644 --- a/rclpy/src/rclpy/service.cpp +++ b/rclpy/src/rclpy/service.cpp @@ -34,9 +34,9 @@ Service::destroy() } Service::Service( - Node node, py::object pysrv_type, std::string service_name, + Node & node, py::object pysrv_type, std::string service_name, py::object pyqos_profile) -: node_(std::move(node)) +: node_(node) { auto srv_type = static_cast( common_get_type_support(pysrv_type)); @@ -87,8 +87,8 @@ Service::Service( } Service::Service( - Node node, std::shared_ptr rcl_service) -: node_(std::move(node)), rcl_service_(rcl_service) + Node & node, std::shared_ptr rcl_service) +: node_(node), rcl_service_(rcl_service) {} void @@ -131,7 +131,7 @@ void define_service(py::object module) { py::class_>(module, "Service") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Service & service) { return reinterpret_cast(service.rcl_ptr()); diff --git a/rclpy/src/rclpy/service.hpp b/rclpy/src/rclpy/service.hpp index 451c88b6a..7bcce6249 100644 --- a/rclpy/src/rclpy/service.hpp +++ b/rclpy/src/rclpy/service.hpp @@ -52,11 +52,11 @@ class Service : public Destroyable, public std::enable_shared_from_this * \return capsule containing the rcl_service_t */ Service( - Node node, py::object pysrv_type, std::string service_name, + Node & node, py::object pysrv_type, std::string service_name, py::object pyqos_profile); Service( - Node node, std::shared_ptr rcl_service); + Node & node, std::shared_ptr rcl_service); ~Service() = default; From fb170601bfe1195763489268fe2f6a54e6cb9ba5 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 13 Dec 2021 17:31:46 -0300 Subject: [PATCH 03/34] Fix possible ownership issue Signed-off-by: Ivan Santiago Paunovic --- rclpy/src/rclpy/lifecycle.cpp | 84 ++++++++++++++++------------------- 1 file changed, 39 insertions(+), 45 deletions(-) diff --git a/rclpy/src/rclpy/lifecycle.cpp b/rclpy/src/rclpy/lifecycle.cpp index 2e9396d94..9cb199996 100644 --- a/rclpy/src/rclpy/lifecycle.cpp +++ b/rclpy/src/rclpy/lifecycle.cpp @@ -37,18 +37,30 @@ namespace py = pybind11; namespace { -class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shared_from_this +class LifecycleStateMachine : public rclpy::Destroyable { public: LifecycleStateMachine( rclpy::Node node, bool enable_com_interface) : node_(std::move(node)) { - state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); + state_machine_ = std::shared_ptr( + new rcl_lifecycle_state_machine_t(rcl_lifecycle_get_zero_initialized_state_machine()), + [node] (rcl_lifecycle_state_machine_t * state_machine) { + rcl_ret_t ret = rcl_lifecycle_state_machine_fini(state_machine, node.rcl_ptr()); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, "Failed to fini lifecycle state machine: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + }); auto state_machine_options = rcl_lifecycle_get_default_state_machine_options(); state_machine_options.enable_com_interface = enable_com_interface; rcl_ret_t ret = rcl_lifecycle_state_machine_init( - &state_machine_, + state_machine_.get(), node_.rcl_ptr(), ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, ChangeState), @@ -60,39 +72,35 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar if (RCL_RET_OK != ret) { throw rclpy::RCLError("Failed to create lifecycle state machine"); } - } - - ~LifecycleStateMachine() - { - this->destroy(); - } - - void init() - { - if (state_machine_.options.enable_com_interface) { + if (state_machine_->options.enable_com_interface) { srv_change_state_ = std::make_shared( node_, std::shared_ptr( - this->shared_from_this(), &state_machine_.com_interface.srv_change_state)); + state_machine_, &state_machine_->com_interface.srv_change_state)); srv_get_state_ = std::make_shared( node_, std::shared_ptr( - this->shared_from_this(), &state_machine_.com_interface.srv_get_state)); + state_machine_, &state_machine_->com_interface.srv_get_state)); srv_get_available_states_ = std::make_shared( node_, std::shared_ptr( - this->shared_from_this(), &state_machine_.com_interface.srv_get_available_states)); + state_machine_, &state_machine_->com_interface.srv_get_available_states)); srv_get_available_transitions_ = std::make_shared( node_, std::shared_ptr( - this->shared_from_this(), &state_machine_.com_interface.srv_get_available_transitions)); + state_machine_, &state_machine_->com_interface.srv_get_available_transitions)); srv_get_transition_graph_ = std::make_shared( node_, std::shared_ptr( - this->shared_from_this(), &state_machine_.com_interface.srv_get_transition_graph)); + state_machine_, &state_machine_->com_interface.srv_get_transition_graph)); } } + ~LifecycleStateMachine() + { + this->destroy(); + } + void destroy() override { @@ -101,22 +109,14 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar srv_get_available_states_.reset(); srv_get_available_transitions_.reset(); srv_get_transition_graph_.reset(); - rcl_ret_t ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_.rcl_ptr()); - if (RCL_RET_OK != ret) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "Failed to fini lifecycle state machine: %s", - rcl_get_error_string().str); - rcl_reset_error(); - } + state_machine_.reset(); node_.destroy(); } bool is_initialized() { - return RCL_RET_OK == rcl_lifecycle_state_machine_is_initialized(&state_machine_); + return RCL_RET_OK == rcl_lifecycle_state_machine_is_initialized(state_machine_.get()); } void @@ -124,7 +124,7 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar { if ( rcl_lifecycle_trigger_transition_by_id( - &state_machine_, transition_id, publish_update) != RCL_RET_OK) + state_machine_.get(), transition_id, publish_update) != RCL_RET_OK) { throw rclpy::RCLError("Failed to trigger lifecycle state machine transition");; } @@ -135,12 +135,18 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar { if ( rcl_lifecycle_trigger_transition_by_label( - &state_machine_, label.c_str(), publish_update) != RCL_RET_OK) + state_machine_.get(), label.c_str(), publish_update) != RCL_RET_OK) { throw rclpy::RCLError("Failed to trigger lifecycle state machine transition");; } } + void + print() + { + rcl_print_state_machine(state_machine_.get()); + } + std::shared_ptr get_srv_change_state() { @@ -171,12 +177,6 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar return srv_get_transition_graph_; } - void - print() - { - rcl_print_state_machine(&state_machine_); - } - private: rclpy::Node node_; std::shared_ptr srv_change_state_; @@ -184,7 +184,7 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar std::shared_ptr srv_get_available_states_; std::shared_ptr srv_get_available_transitions_; std::shared_ptr srv_get_transition_graph_; - rcl_lifecycle_state_machine_t state_machine_; + std::shared_ptr state_machine_; }; enum class TransitionCallbackReturnType @@ -201,15 +201,9 @@ namespace rclpy void define_lifecycle_api(py::module m) { - py::class_>( + py::class_( m, "LifecycleStateMachine") - .def(py::init([](Node node, bool enable_com_interface) - { - auto state_machine = std::make_shared(std::move(node), enable_com_interface); - state_machine->init(); - return state_machine; - } - )) + .def(py::init()) .def( "is_initialized", &LifecycleStateMachine::is_initialized, "Check if state machine is initialized.") From b0bf63688cce11dc7f3922a40c9af6e199e27306 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 14 Dec 2021 15:50:36 -0300 Subject: [PATCH 04/34] make basic example work Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle.py | 56 +++++++++++++++++++++++++++++++++++ rclpy/src/rclpy/lifecycle.cpp | 32 ++++++++++---------- rclpy/test/test_lifecycle.py | 21 +++++++++++++ 3 files changed, 93 insertions(+), 16 deletions(-) create mode 100644 rclpy/rclpy/lifecycle.py create mode 100644 rclpy/test/test_lifecycle.py diff --git a/rclpy/rclpy/lifecycle.py b/rclpy/rclpy/lifecycle.py new file mode 100644 index 000000000..b98949dec --- /dev/null +++ b/rclpy/rclpy/lifecycle.py @@ -0,0 +1,56 @@ +# Copyright 2021 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. + +from typing import List + +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.node import Node +from rclpy.service import Service + + +class LifecycleMixin: + def __init__(self, *, enable_communication_interface: bool = True): + self.create_state_machine(enable_communication_interface) + + + def create_state_machine(self, enable_communication_interface: bool): + with self.handle: + # self._state_machine = StateMachine( + # _rclpy.LifecycleStateMachine(self.handle, enable_communication_interface)) + self._state_machine = _rclpy.LifecycleStateMachine(self.handle, enable_communication_interface) + self._Node__services.extend( + [ + self._state_machine.service_change_state, + self._state_machine.service_get_state, + self._state_machine.service_get_available_states, + self._state_machine.service_get_available_transitions, + self._state_machine.service_get_transition_graph, + ] + ) + + +class LifecycleNode(LifecycleMixin, Node): + def __init__(self, node_name, *, enable_communication_interface: bool = True, **kwargs): + Node.__init__(self, node_name, **kwargs) + LifecycleMixin.__init__(self, + enable_communication_interface=enable_communication_interface) + + +class StateMachine: + """Lifecycle state machine implementation.""" + + def __init__(self, state_machine_impl: _rclpy.LifecycleStateMachine): + self._state_machine = state_machine_impl + + # def diff --git a/rclpy/src/rclpy/lifecycle.cpp b/rclpy/src/rclpy/lifecycle.cpp index 9cb199996..17905feec 100644 --- a/rclpy/src/rclpy/lifecycle.cpp +++ b/rclpy/src/rclpy/lifecycle.cpp @@ -37,12 +37,12 @@ namespace py = pybind11; namespace { -class LifecycleStateMachine : public rclpy::Destroyable +class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shared_from_this { public: LifecycleStateMachine( - rclpy::Node node, bool enable_com_interface) - : node_(std::move(node)) + rclpy::Node & node, bool enable_com_interface) + : node_(node) { state_machine_ = std::shared_ptr( new rcl_lifecycle_state_machine_t(rcl_lifecycle_get_zero_initialized_state_machine()), @@ -104,12 +104,12 @@ class LifecycleStateMachine : public rclpy::Destroyable void destroy() override { + state_machine_.reset(); srv_change_state_.reset(); srv_get_state_.reset(); srv_get_available_states_.reset(); srv_get_available_transitions_.reset(); srv_get_transition_graph_.reset(); - state_machine_.reset(); node_.destroy(); } @@ -201,9 +201,9 @@ namespace rclpy void define_lifecycle_api(py::module m) { - py::class_( + py::class_>( m, "LifecycleStateMachine") - .def(py::init()) + .def(py::init()) .def( "is_initialized", &LifecycleStateMachine::is_initialized, "Check if state machine is initialized.") @@ -213,20 +213,20 @@ define_lifecycle_api(py::module m) .def( "trigger_transition_by_label", &LifecycleStateMachine::trigger_transition_by_id, "Trigger a transition by label.") - .def( - "get_srv_change_state", &LifecycleStateMachine::get_srv_change_state, + .def_property_readonly( + "service_change_state", &LifecycleStateMachine::get_srv_change_state, "Get the change state service.") - .def( - "get_srv_get_state", &LifecycleStateMachine::get_srv_get_state, + .def_property_readonly( + "service_get_state", &LifecycleStateMachine::get_srv_get_state, "Get the get state service.") - .def( - "get_srv_get_available_states", &LifecycleStateMachine::get_srv_get_available_states, + .def_property_readonly( + "service_get_available_states", &LifecycleStateMachine::get_srv_get_available_states, "Get the get available states service.") - .def( - "get_srv_get_available_transitions", &LifecycleStateMachine::get_srv_get_available_transitions, + .def_property_readonly( + "service_get_available_transitions", &LifecycleStateMachine::get_srv_get_available_transitions, "Get the get available transitions service.") - .def( - "get_srv_get_transition_graph", &LifecycleStateMachine::get_srv_get_transition_graph, + .def_property_readonly( + "service_get_transition_graph", &LifecycleStateMachine::get_srv_get_transition_graph, "Get the get transition graph service."); py::enum_(m, "TransitionCallbackReturnType") .value("SUCCESS", TransitionCallbackReturnType::Success) diff --git a/rclpy/test/test_lifecycle.py b/rclpy/test/test_lifecycle.py new file mode 100644 index 000000000..828c15b51 --- /dev/null +++ b/rclpy/test/test_lifecycle.py @@ -0,0 +1,21 @@ +# Copyright 2021 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. + +import rclpy +from rclpy.lifecycle import LifecycleNode + + +def test_lifecycle_node_init(): + rclpy.init() + node = LifecycleNode('my_lifecycle_node') From 19d5234ed07e931302353b97a87b1542f4122b24 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 15 Dec 2021 17:20:53 -0300 Subject: [PATCH 05/34] Make all the services work, checked by doing manual testing Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle.py | 236 ++++++++++++++++++++++++++++++---- rclpy/src/rclpy/lifecycle.cpp | 120 ++++++++++++++++- rclpy/src/rclpy/service.cpp | 22 +++- rclpy/src/rclpy/service.hpp | 6 + rclpy/test/test_lifecycle.py | 6 + 5 files changed, 361 insertions(+), 29 deletions(-) diff --git a/rclpy/rclpy/lifecycle.py b/rclpy/rclpy/lifecycle.py index b98949dec..17ba5c17d 100644 --- a/rclpy/rclpy/lifecycle.py +++ b/rclpy/rclpy/lifecycle.py @@ -12,32 +12,229 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import List +from typing import Callable +from typing import Dict +from typing import Optional + +import lifecycle_msgs.srv from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.callback_groups import CallbackGroup from rclpy.node import Node +from rclpy.qos import QoSProfile from rclpy.service import Service +from rclpy.type_support import check_is_valid_msg_type +from rclpy.type_support import check_is_valid_srv_type + +TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType class LifecycleMixin: - def __init__(self, *, enable_communication_interface: bool = True): - self.create_state_machine(enable_communication_interface) + # TODO(ivanpauno): We could make this a bit nicer by adding State, Transition, StateMachine abstractions + # in this module. + # Anyways those are not directly useful to the user and the implementation is not too bad without them. + def __init__( + self, + *, + enable_communication_interface: bool = True, + callback_group: Optional[CallbackGroup] = None, + ): + if callback_group is None: + callback_group = self.default_callback_group + self._callbacks : Dict[int, Callable[[int], TransitionCallbackReturn]] = {} + for srv_type in ( + lifecycle_msgs.srv.ChangeState, + lifecycle_msgs.srv.GetState, + lifecycle_msgs.srv.GetAvailableStates, + lifecycle_msgs.srv.GetAvailableTransitions, + ): + # this doesn't only checks, but also imports some stuff we need later + check_is_valid_srv_type(srv_type) - def create_state_machine(self, enable_communication_interface: bool): with self.handle: - # self._state_machine = StateMachine( - # _rclpy.LifecycleStateMachine(self.handle, enable_communication_interface)) self._state_machine = _rclpy.LifecycleStateMachine(self.handle, enable_communication_interface) - self._Node__services.extend( - [ - self._state_machine.service_change_state, - self._state_machine.service_get_state, - self._state_machine.service_get_available_states, - self._state_machine.service_get_available_transitions, - self._state_machine.service_get_transition_graph, - ] - ) + self._service_change_state = Service( + self._state_machine.service_change_state, + lifecycle_msgs.srv.ChangeState, + self._state_machine.service_change_state.name, + self.__on_change_state, + callback_group, + QoSProfile(**self._state_machine.service_change_state.qos)) + self._service_get_state = Service( + self._state_machine.service_get_state, + lifecycle_msgs.srv.GetState, + self._state_machine.service_get_state.name, + self._on_get_state, + callback_group, + QoSProfile(**self._state_machine.service_get_state.qos)) + self._service_get_available_states = Service( + self._state_machine.service_get_available_states, + lifecycle_msgs.srv.GetAvailableStates, + self._state_machine.service_get_available_states.name, + self._on_get_available_states, + callback_group, + QoSProfile(**self._state_machine.service_get_available_states.qos)) + self._service_get_available_transitions = Service( + self._state_machine.service_get_available_transitions, + lifecycle_msgs.srv.GetAvailableTransitions, + self._state_machine.service_get_available_transitions.name, + self._on_get_available_transitions, + callback_group, + QoSProfile(**self._state_machine.service_get_available_transitions.qos)) + self._service_get_transition_graph = Service( + self._state_machine.service_get_transition_graph, + lifecycle_msgs.srv.GetAvailableTransitions, + self._state_machine.service_get_transition_graph.name, + self._on_get_transition_graph, + callback_group, + QoSProfile(**self._state_machine.service_get_transition_graph.qos)) + + lifecycle_services = [ + self._service_change_state, + self._service_get_state, + self._service_get_available_states, + self._service_get_available_transitions, + self._service_get_transition_graph, + ] + for s in lifecycle_services: + callback_group.add_entity(s) + # TODO(ivanpauno): Modify attribute in Node to be "protected" instead of "private". + # i.e. Node.__services -> Node._services + # Maybe the same with similar attributes (__publishers, etc). + # Maybe have some interface to add a service/etc instead (?). + self._Node__services.extend(lifecycle_services) + + def register_callback( + self, state_id: int, callback: Callable[[int], TransitionCallbackReturn] + ) -> bool: + self._callbacks[state_id] = callback + # TODO(ivanpauno): Copying rclcpp style. + # Maybe having a return value doesn't make sense (?). + # Should we error/warn if overridding an existing callback? + return True + + def __execute_callback(self, current_state: int, previous_state: int) -> TransitionCallbackReturn: + cb = self._callbacks.get(current_state, None) + if not cb: + return TransitionCallbackReturn.SUCCESS + try: + return cb(previous_state) + except Exception: + # TODO(ivanpauno): log sth here + return TransitionCallbackReturn.ERROR + + def __change_state(self, transition_id: int): + self.__check_is_initialized() + initial_state_id = self._state_machine.current_state[0] + self._state_machine.trigger_transition_by_id(transition_id, True) + + cb_return_code = self.__execute_callback(self._state_machine.current_state[0], initial_state_id) + self._state_machine.trigger_transition_by_label(cb_return_code.to_label(), True) + + + if cb_return_code == TransitionCallbackReturn.ERROR: + # TODO(ivanpauno): I don't understand what rclcpp is doing here ... + # It's triggering the error transition twice (?) + # https://github.com/ros2/rclcpp/blob/b2b676d3172ada509e58fa552a676a446809d83c/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp#L428-L443 + pass + return cb_return_code + + def __check_is_initialized(self): + # TODO(ivanpauno): This sanity check is probably not needed, just doing the same checks as + # rclcpp for the moment. + if not self._state_machine.initialized: + raise RuntimeError( + 'Internal error: got service request while lifecycle state machine ' + 'is not initialized.') + + def __on_change_state( + self, + req: lifecycle_msgs.srv.ChangeState.Request, + resp: lifecycle_msgs.srv.ChangeState.Response + ): + self.__check_is_initialized() + transition_id = req.transition.id; + if req.transition.label: + try: + transition_id = self._state_machine.get_transition_by_label(req.transition.label) + except _rclpy.RCLError: + resp.success = False + return resp + cb_return_code = self.__change_state(transition_id); + resp.success = cb_return_code == TransitionCallbackReturn.SUCCESS + return resp + + def _on_get_state( + self, + req: lifecycle_msgs.srv.GetState.Request, + resp: lifecycle_msgs.srv.GetState.Response + ): + self.__check_is_initialized() + resp.current_state.id, resp.current_state.label = self._state_machine.current_state + return resp + + def _on_get_available_states( + self, + req: lifecycle_msgs.srv.GetAvailableStates.Request, + resp: lifecycle_msgs.srv.GetAvailableStates.Response + ): + self.__check_is_initialized() + for id, label in self._state_machine.available_states: + resp.available_states.append(lifecycle_msgs.msg.State(id=id, label=label)) + return resp + + def _on_get_available_transitions( + self, + req: lifecycle_msgs.srv.GetAvailableTransitions.Request, + resp: lifecycle_msgs.srv.GetAvailableTransitions.Response + ): + self.__check_is_initialized() + for id, label, start_id, start_label, goal_id, goal_label in self._state_machine.available_transitions: + item = lifecycle_msgs.msg.TransitionDescription() + item.transition.id = id + item.transition.label = label + item.start_state.id = start_id + item.start_state.label = start_label + item.goal_state.id = goal_id + item.goal_state.label = goal_label + resp.available_transitions.append(item) + return resp + + def _on_get_transition_graph( + self, + req: lifecycle_msgs.srv.GetAvailableTransitions.Request, + resp: lifecycle_msgs.srv.GetAvailableTransitions.Response + ): + self.__check_is_initialized() + for id, label, start_id, start_label, goal_id, goal_label in self._state_machine.transition_graph: + item = lifecycle_msgs.msg.TransitionDescription() + item.transition.id = id + item.transition.label = label + item.start_state.id = start_id + item.start_state.label = start_label + item.goal_state.id = goal_id + item.goal_state.label = goal_label + resp.available_transitions.append(item) + return resp + + def on_configure(self, state) -> TransitionCallbackReturn: + return TransitionCallbackReturn.SUCCESS + + def on_cleanup(self, state) -> TransitionCallbackReturn: + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state) -> TransitionCallbackReturn: + return TransitionCallbackReturn.SUCCESS + + def on_activate(self, state) -> TransitionCallbackReturn: + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state) -> TransitionCallbackReturn: + return TransitionCallbackReturn.SUCCESS + + def on_error(self, state) -> TransitionCallbackReturn: + return TransitionCallbackReturn.SUCCESS class LifecycleNode(LifecycleMixin, Node): @@ -45,12 +242,3 @@ def __init__(self, node_name, *, enable_communication_interface: bool = True, ** Node.__init__(self, node_name, **kwargs) LifecycleMixin.__init__(self, enable_communication_interface=enable_communication_interface) - - -class StateMachine: - """Lifecycle state machine implementation.""" - - def __init__(self, state_machine_impl: _rclpy.LifecycleStateMachine): - self._state_machine = state_machine_impl - - # def diff --git a/rclpy/src/rclpy/lifecycle.cpp b/rclpy/src/rclpy/lifecycle.cpp index 17905feec..5699db28e 100644 --- a/rclpy/src/rclpy/lifecycle.cpp +++ b/rclpy/src/rclpy/lifecycle.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include @@ -141,6 +142,84 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar } } + uint8_t + get_transition_by_label(std::string label) + { + const auto * transition = rcl_lifecycle_get_transition_by_label( + state_machine_->current_state, label.c_str()); + if (nullptr == transition) + { + throw rclpy::RCLError("Failed to get transition from label"); + } + return transition->id; + } + + std::string + std_string_from_maybe_nullptr(const char * input) + { + if (input) { + return input; + } + return std::string{}; + } + + py::tuple + get_current_state() + { + return py::make_tuple( + state_machine_->current_state->id, + state_machine_->current_state->label); + } + + std::vector> + get_available_states() + { + std::vector> ret; + ret.reserve(state_machine_->transition_map.states_size); + for (size_t i = 0; i <= state_machine_->transition_map.states_size; ++i) { + ret.emplace_back(std::make_tuple( + state_machine_->transition_map.states[i].id, + std_string_from_maybe_nullptr(state_machine_->transition_map.states[i].label))); + } + return ret; + } + + std::vector> + get_available_transitions() + { + std::vector> ret; + ret.reserve(state_machine_->current_state->valid_transition_size); + for (size_t i = 0; i < state_machine_->current_state->valid_transition_size; ++i) { + ret.emplace_back(std::make_tuple( + state_machine_->current_state->valid_transitions[i].id, + std_string_from_maybe_nullptr(state_machine_->current_state->valid_transitions[i].label), + state_machine_->current_state->valid_transitions[i].start->id, + std_string_from_maybe_nullptr( + state_machine_->current_state->valid_transitions[i].start->label), + state_machine_->current_state->valid_transitions[i].goal->id, + std_string_from_maybe_nullptr( + state_machine_->current_state->valid_transitions[i].goal->label))); + } + return ret; + } + + std::vector> + get_transition_graph() + { + std::vector> ret; + ret.reserve(state_machine_->transition_map.transitions_size); + for (size_t i = 0; i < state_machine_->transition_map.transitions_size; ++i) { + ret.emplace_back(std::make_tuple( + state_machine_->transition_map.transitions[i].id, + state_machine_->transition_map.transitions[i].label, + state_machine_->transition_map.transitions[i].start->id, + state_machine_->transition_map.transitions[i].start->label, + state_machine_->transition_map.transitions[i].goal->id, + state_machine_->transition_map.transitions[i].goal->label)); + } + return ret; + } + void print() { @@ -194,6 +273,21 @@ enum class TransitionCallbackReturnType Error = lifecycle_msgs__msg__Transition__TRANSITION_CALLBACK_ERROR, }; +std::string +convert_callback_ret_code_to_label(TransitionCallbackReturnType cb_ret) +{ + if (cb_ret == TransitionCallbackReturnType::Success) { + return rcl_lifecycle_transition_success_label; + } + if (cb_ret == TransitionCallbackReturnType::Failure) { + return rcl_lifecycle_transition_failure_label; + } + if (cb_ret == TransitionCallbackReturnType::Error) { + return rcl_lifecycle_transition_error_label; + } + throw std::runtime_error{"Invalid transition callback return type"}; +} + } namespace rclpy @@ -204,14 +298,29 @@ define_lifecycle_api(py::module m) py::class_>( m, "LifecycleStateMachine") .def(py::init()) - .def( - "is_initialized", &LifecycleStateMachine::is_initialized, + .def_property_readonly( + "initialized", &LifecycleStateMachine::is_initialized, "Check if state machine is initialized.") + .def_property_readonly( + "current_state", &LifecycleStateMachine::get_current_state, + "Get the current state machine state.") + .def_property_readonly( + "available_states", &LifecycleStateMachine::get_available_states, + "Get the available states.") + .def_property_readonly( + "available_transitions", &LifecycleStateMachine::get_available_transitions, + "Get the available transitions.") + .def_property_readonly( + "transition_graph", &LifecycleStateMachine::get_transition_graph, + "Get the transition graph.") + .def( + "get_transition_by_label", &LifecycleStateMachine::get_transition_by_label, + "Get the transition id from a transition label.") .def( "trigger_transition_by_id", &LifecycleStateMachine::trigger_transition_by_id, "Trigger a transition by transition id.") .def( - "trigger_transition_by_label", &LifecycleStateMachine::trigger_transition_by_id, + "trigger_transition_by_label", &LifecycleStateMachine::trigger_transition_by_label, "Trigger a transition by label.") .def_property_readonly( "service_change_state", &LifecycleStateMachine::get_srv_change_state, @@ -231,6 +340,9 @@ define_lifecycle_api(py::module m) py::enum_(m, "TransitionCallbackReturnType") .value("SUCCESS", TransitionCallbackReturnType::Success) .value("FAILURE", TransitionCallbackReturnType::Failure) - .value("ERROR", TransitionCallbackReturnType::Error); + .value("ERROR", TransitionCallbackReturnType::Error) + .def( + "to_label", &convert_callback_ret_code_to_label, + "Convert the transition callback return code to a transition label"); } } // namespace rclpy diff --git a/rclpy/src/rclpy/service.cpp b/rclpy/src/rclpy/service.cpp index c3c6189fd..4f9d96eff 100644 --- a/rclpy/src/rclpy/service.cpp +++ b/rclpy/src/rclpy/service.cpp @@ -22,6 +22,7 @@ #include "exceptions.hpp" #include "service.hpp" +#include "utils.hpp" namespace rclpy { @@ -109,7 +110,6 @@ py::tuple Service::service_take_request(py::object pyrequest_type) { auto taken_request = create_from_py(pyrequest_type); - rmw_service_info_t header; py::tuple result_tuple(2); @@ -127,6 +127,20 @@ Service::service_take_request(py::object pyrequest_type) return result_tuple; } + +const char * +Service::get_service_name() +{ + return rcl_service_get_service_name(rcl_service_.get()); +} + +py::dict +Service::get_qos_profile() +{ + const auto * options = rcl_service_get_options(rcl_service_.get()); + return rclpy::convert_to_qos_dict(&options->qos); +} + void define_service(py::object module) { @@ -137,6 +151,12 @@ define_service(py::object module) return reinterpret_cast(service.rcl_ptr()); }, "Get the address of the entity as an integer") + .def_property_readonly( + "name", &Service::get_service_name, + "Get the name of the service") + .def_property_readonly( + "qos", &Service::get_qos_profile, + "Get the qos profile of the service") .def( "service_send_response", &Service::service_send_response, "Send a response") diff --git a/rclpy/src/rclpy/service.hpp b/rclpy/src/rclpy/service.hpp index 7bcce6249..9e07ea213 100644 --- a/rclpy/src/rclpy/service.hpp +++ b/rclpy/src/rclpy/service.hpp @@ -91,6 +91,12 @@ class Service : public Destroyable, public std::enable_shared_from_this return rcl_service_.get(); } + const char * + get_service_name(); + + py::dict + get_qos_profile(); + /// Force an early destruction of this object void destroy() override; diff --git a/rclpy/test/test_lifecycle.py b/rclpy/test/test_lifecycle.py index 828c15b51..0fc1282c8 100644 --- a/rclpy/test/test_lifecycle.py +++ b/rclpy/test/test_lifecycle.py @@ -19,3 +19,9 @@ def test_lifecycle_node_init(): rclpy.init() node = LifecycleNode('my_lifecycle_node') + + +if __name__ == '__main__': + rclpy.init() + node = LifecycleNode('my_lifecycle_node') + rclpy.spin(node) From c01fe6f81ea3b145d21ea0f884dfb5a574916291 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 15 Dec 2021 17:23:41 -0300 Subject: [PATCH 06/34] More consistent style Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/rclpy/rclpy/lifecycle.py b/rclpy/rclpy/lifecycle.py index 17ba5c17d..92f76d52a 100644 --- a/rclpy/rclpy/lifecycle.py +++ b/rclpy/rclpy/lifecycle.py @@ -65,28 +65,28 @@ def __init__( self._state_machine.service_get_state, lifecycle_msgs.srv.GetState, self._state_machine.service_get_state.name, - self._on_get_state, + self.__on_get_state, callback_group, QoSProfile(**self._state_machine.service_get_state.qos)) self._service_get_available_states = Service( self._state_machine.service_get_available_states, lifecycle_msgs.srv.GetAvailableStates, self._state_machine.service_get_available_states.name, - self._on_get_available_states, + self.__on_get_available_states, callback_group, QoSProfile(**self._state_machine.service_get_available_states.qos)) self._service_get_available_transitions = Service( self._state_machine.service_get_available_transitions, lifecycle_msgs.srv.GetAvailableTransitions, self._state_machine.service_get_available_transitions.name, - self._on_get_available_transitions, + self.__on_get_available_transitions, callback_group, QoSProfile(**self._state_machine.service_get_available_transitions.qos)) self._service_get_transition_graph = Service( self._state_machine.service_get_transition_graph, lifecycle_msgs.srv.GetAvailableTransitions, self._state_machine.service_get_transition_graph.name, - self._on_get_transition_graph, + self.__on_get_transition_graph, callback_group, QoSProfile(**self._state_machine.service_get_transition_graph.qos)) @@ -165,7 +165,7 @@ def __on_change_state( resp.success = cb_return_code == TransitionCallbackReturn.SUCCESS return resp - def _on_get_state( + def __on_get_state( self, req: lifecycle_msgs.srv.GetState.Request, resp: lifecycle_msgs.srv.GetState.Response @@ -174,7 +174,7 @@ def _on_get_state( resp.current_state.id, resp.current_state.label = self._state_machine.current_state return resp - def _on_get_available_states( + def __on_get_available_states( self, req: lifecycle_msgs.srv.GetAvailableStates.Request, resp: lifecycle_msgs.srv.GetAvailableStates.Response @@ -184,7 +184,7 @@ def _on_get_available_states( resp.available_states.append(lifecycle_msgs.msg.State(id=id, label=label)) return resp - def _on_get_available_transitions( + def __on_get_available_transitions( self, req: lifecycle_msgs.srv.GetAvailableTransitions.Request, resp: lifecycle_msgs.srv.GetAvailableTransitions.Response @@ -201,7 +201,7 @@ def _on_get_available_transitions( resp.available_transitions.append(item) return resp - def _on_get_transition_graph( + def __on_get_transition_graph( self, req: lifecycle_msgs.srv.GetAvailableTransitions.Request, resp: lifecycle_msgs.srv.GetAvailableTransitions.Response From 0c3b02b72149ed20fdfe8c04b844d6b9122573a8 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 15 Dec 2021 17:39:24 -0300 Subject: [PATCH 07/34] Add some docs, add ManagedEntity concept Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle.py | 77 ++++++++++++++++++++++++++++++---------- 1 file changed, 58 insertions(+), 19 deletions(-) diff --git a/rclpy/rclpy/lifecycle.py b/rclpy/rclpy/lifecycle.py index 92f76d52a..e070be6c0 100644 --- a/rclpy/rclpy/lifecycle.py +++ b/rclpy/rclpy/lifecycle.py @@ -29,17 +29,56 @@ TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType -class LifecycleMixin: +class ManagedEntity: + def on_configure(self, state) -> TransitionCallbackReturn: + """Callback that will be triggered when a configure transition is requested.""" + return TransitionCallbackReturn.SUCCESS + + def on_cleanup(self, state) -> TransitionCallbackReturn: + """Callback that will be triggered when a cleanup transition is requested.""" + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state) -> TransitionCallbackReturn: + """Callback that will be triggered when a shutdown transition is requested.""" + return TransitionCallbackReturn.SUCCESS + + def on_activate(self, state) -> TransitionCallbackReturn: + """Callback that will be triggered when an activate transition is requested.""" + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state) -> TransitionCallbackReturn: + """Callback that will be triggered when an deactivate transition is requested.""" + return TransitionCallbackReturn.SUCCESS + + def on_error(self, state) -> TransitionCallbackReturn: + """Callback that will be triggered when an error transition is requested.""" + return TransitionCallbackReturn.SUCCESS + + +class LifecycleMixin(ManagedEntity): + """ + Mixin class to share as most code as possible between `Node` and `LifecycleNode`. + + This class is not useful if not used in multiple inheritance together with `Node`, + as it access attributes created by `Node` directly here! + """ # TODO(ivanpauno): We could make this a bit nicer by adding State, Transition, StateMachine abstractions # in this module. # Anyways those are not directly useful to the user and the implementation is not too bad without them. + # TODO(ivanpauno): We actually need a State abstraction to make the register_callback() API nicer! def __init__( self, *, enable_communication_interface: bool = True, callback_group: Optional[CallbackGroup] = None, ): + """ + Initialize a lifecycle node. + + :param enable_communication_interface: Creates the lifecycle nodes services and publisher if True. + :param callback_group: Callback group that will be used by all the lifecycle node services. + """ if callback_group is None: callback_group = self.default_callback_group self._callbacks : Dict[int, Callable[[int], TransitionCallbackReturn]] = {} @@ -108,6 +147,12 @@ def __init__( def register_callback( self, state_id: int, callback: Callable[[int], TransitionCallbackReturn] ) -> bool: + """ + Register a callback that will be triggered when transitioning to state_id. + + The registered callback takes as an argument the previous state id, and returns + a TransitionCallbackReturn value. + """ self._callbacks[state_id] = callback # TODO(ivanpauno): Copying rclcpp style. # Maybe having a return value doesn't make sense (?). @@ -218,27 +263,21 @@ def __on_get_transition_graph( resp.available_transitions.append(item) return resp - def on_configure(self, state) -> TransitionCallbackReturn: - return TransitionCallbackReturn.SUCCESS - - def on_cleanup(self, state) -> TransitionCallbackReturn: - return TransitionCallbackReturn.SUCCESS - - def on_shutdown(self, state) -> TransitionCallbackReturn: - return TransitionCallbackReturn.SUCCESS - - def on_activate(self, state) -> TransitionCallbackReturn: - return TransitionCallbackReturn.SUCCESS - - def on_deactivate(self, state) -> TransitionCallbackReturn: - return TransitionCallbackReturn.SUCCESS - - def on_error(self, state) -> TransitionCallbackReturn: - return TransitionCallbackReturn.SUCCESS - class LifecycleNode(LifecycleMixin, Node): + """ + A ROS 2 managed node. + + This class extends Node with the methods provided by LifecycleMixin. + Methods in LifecycleMixin overridde the ones in Node. + """ def __init__(self, node_name, *, enable_communication_interface: bool = True, **kwargs): + """ + Create a lifecycle node. + + See rclpy.lifecycle.LifecycleMixin.__init__() and rclpy.node.Node() + for the documentation of each parameter. + """ Node.__init__(self, node_name, **kwargs) LifecycleMixin.__init__(self, enable_communication_interface=enable_communication_interface) From 8efe11c5e3181b96e1013e6cf589e2a4ec326b85 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 15 Dec 2021 18:04:08 -0300 Subject: [PATCH 08/34] Make all linters happy Signed-off-by: Ivan Santiago Paunovic --- rclpy/CMakeLists.txt | 2 +- rclpy/rclpy/lifecycle.py | 78 ++++++++++------- rclpy/src/rclpy/lifecycle.cpp | 155 ++++++++++++++++++---------------- rclpy/src/rclpy/lifecycle.hpp | 6 +- rclpy/test/test_lifecycle.py | 1 + 5 files changed, 132 insertions(+), 110 deletions(-) diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index 7e8780023..d531bd228 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -135,7 +135,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED target_include_directories(_rclpy_pybind11 PRIVATE src/rclpy/ ) -target_link_libraries(_rclpy_pybind11 PRIVATE +target_link_libraries(_rclpy_pybind11 PRIVATE lifecycle_msgs::lifecycle_msgs__rosidl_generator_c lifecycle_msgs::lifecycle_msgs__rosidl_typesupport_c rcl::rcl diff --git a/rclpy/rclpy/lifecycle.py b/rclpy/rclpy/lifecycle.py index e070be6c0..48f521859 100644 --- a/rclpy/rclpy/lifecycle.py +++ b/rclpy/rclpy/lifecycle.py @@ -18,40 +18,41 @@ import lifecycle_msgs.srv -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.callback_groups import CallbackGroup +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.node import Node from rclpy.qos import QoSProfile from rclpy.service import Service -from rclpy.type_support import check_is_valid_msg_type from rclpy.type_support import check_is_valid_srv_type TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType + class ManagedEntity: + def on_configure(self, state) -> TransitionCallbackReturn: - """Callback that will be triggered when a configure transition is requested.""" + """Handle configure transition request.""" return TransitionCallbackReturn.SUCCESS def on_cleanup(self, state) -> TransitionCallbackReturn: - """Callback that will be triggered when a cleanup transition is requested.""" + """Handle cleanup transition request.""" return TransitionCallbackReturn.SUCCESS def on_shutdown(self, state) -> TransitionCallbackReturn: - """Callback that will be triggered when a shutdown transition is requested.""" + """Handle shutdown transition request.""" return TransitionCallbackReturn.SUCCESS def on_activate(self, state) -> TransitionCallbackReturn: - """Callback that will be triggered when an activate transition is requested.""" + """Handle activate transition request.""" return TransitionCallbackReturn.SUCCESS def on_deactivate(self, state) -> TransitionCallbackReturn: - """Callback that will be triggered when an deactivate transition is requested.""" + """Handle deactivate transition request.""" return TransitionCallbackReturn.SUCCESS def on_error(self, state) -> TransitionCallbackReturn: - """Callback that will be triggered when an error transition is requested.""" + """Handle error transition request.""" return TransitionCallbackReturn.SUCCESS @@ -63,10 +64,12 @@ class LifecycleMixin(ManagedEntity): as it access attributes created by `Node` directly here! """ - # TODO(ivanpauno): We could make this a bit nicer by adding State, Transition, StateMachine abstractions - # in this module. - # Anyways those are not directly useful to the user and the implementation is not too bad without them. - # TODO(ivanpauno): We actually need a State abstraction to make the register_callback() API nicer! + # TODO(ivanpauno): We could make this a bit nicer by adding State, Transition, StateMachine + # abstractions in this module. + # Anyways those are not directly useful to the user and the implementation is not too bad + # without them. + # TODO(ivanpauno): We actually need a State abstraction to make the register_callback() + # API nicer! def __init__( self, *, @@ -76,12 +79,14 @@ def __init__( """ Initialize a lifecycle node. - :param enable_communication_interface: Creates the lifecycle nodes services and publisher if True. - :param callback_group: Callback group that will be used by all the lifecycle node services. + :param enable_communication_interface: Creates the lifecycle nodes services and publisher + if True. + :param callback_group: Callback group that will be used by all the lifecycle + node services. """ if callback_group is None: callback_group = self.default_callback_group - self._callbacks : Dict[int, Callable[[int], TransitionCallbackReturn]] = {} + self._callbacks: Dict[int, Callable[[int], TransitionCallbackReturn]] = {} for srv_type in ( lifecycle_msgs.srv.ChangeState, lifecycle_msgs.srv.GetState, @@ -92,7 +97,8 @@ def __init__( check_is_valid_srv_type(srv_type) with self.handle: - self._state_machine = _rclpy.LifecycleStateMachine(self.handle, enable_communication_interface) + self._state_machine = _rclpy.LifecycleStateMachine( + self.handle, enable_communication_interface) self._service_change_state = Service( self._state_machine.service_change_state, lifecycle_msgs.srv.ChangeState, @@ -159,7 +165,9 @@ def register_callback( # Should we error/warn if overridding an existing callback? return True - def __execute_callback(self, current_state: int, previous_state: int) -> TransitionCallbackReturn: + def __execute_callback( + self, current_state: int, previous_state: int + ) -> TransitionCallbackReturn: cb = self._callbacks.get(current_state, None) if not cb: return TransitionCallbackReturn.SUCCESS @@ -174,10 +182,10 @@ def __change_state(self, transition_id: int): initial_state_id = self._state_machine.current_state[0] self._state_machine.trigger_transition_by_id(transition_id, True) - cb_return_code = self.__execute_callback(self._state_machine.current_state[0], initial_state_id) + cb_return_code = self.__execute_callback( + self._state_machine.current_state[0], initial_state_id) self._state_machine.trigger_transition_by_label(cb_return_code.to_label(), True) - if cb_return_code == TransitionCallbackReturn.ERROR: # TODO(ivanpauno): I don't understand what rclcpp is doing here ... # It's triggering the error transition twice (?) @@ -199,14 +207,14 @@ def __on_change_state( resp: lifecycle_msgs.srv.ChangeState.Response ): self.__check_is_initialized() - transition_id = req.transition.id; + transition_id = req.transition.id if req.transition.label: try: transition_id = self._state_machine.get_transition_by_label(req.transition.label) except _rclpy.RCLError: resp.success = False return resp - cb_return_code = self.__change_state(transition_id); + cb_return_code = self.__change_state(transition_id) resp.success = cb_return_code == TransitionCallbackReturn.SUCCESS return resp @@ -225,8 +233,8 @@ def __on_get_available_states( resp: lifecycle_msgs.srv.GetAvailableStates.Response ): self.__check_is_initialized() - for id, label in self._state_machine.available_states: - resp.available_states.append(lifecycle_msgs.msg.State(id=id, label=label)) + for state_id, label in self._state_machine.available_states: + resp.available_states.append(lifecycle_msgs.msg.State(id=state_id, label=label)) return resp def __on_get_available_transitions( @@ -235,10 +243,12 @@ def __on_get_available_transitions( resp: lifecycle_msgs.srv.GetAvailableTransitions.Response ): self.__check_is_initialized() - for id, label, start_id, start_label, goal_id, goal_label in self._state_machine.available_transitions: + for transition_description in self._state_machine.available_transitions: + transition_id, transition_label, start_id, start_label, goal_id, goal_label = \ + transition_description item = lifecycle_msgs.msg.TransitionDescription() - item.transition.id = id - item.transition.label = label + item.transition.id = transition_id + item.transition.label = transition_label item.start_state.id = start_id item.start_state.label = start_label item.goal_state.id = goal_id @@ -252,10 +262,12 @@ def __on_get_transition_graph( resp: lifecycle_msgs.srv.GetAvailableTransitions.Response ): self.__check_is_initialized() - for id, label, start_id, start_label, goal_id, goal_label in self._state_machine.transition_graph: + for transition_description in self._state_machine.transition_graph: + transition_id, transition_label, start_id, start_label, goal_id, goal_label = \ + transition_description item = lifecycle_msgs.msg.TransitionDescription() - item.transition.id = id - item.transition.label = label + item.transition.id = transition_id + item.transition.label = transition_label item.start_state.id = start_id item.start_state.label = start_label item.goal_state.id = goal_id @@ -267,17 +279,19 @@ def __on_get_transition_graph( class LifecycleNode(LifecycleMixin, Node): """ A ROS 2 managed node. - + This class extends Node with the methods provided by LifecycleMixin. Methods in LifecycleMixin overridde the ones in Node. """ + def __init__(self, node_name, *, enable_communication_interface: bool = True, **kwargs): """ Create a lifecycle node. - + See rclpy.lifecycle.LifecycleMixin.__init__() and rclpy.node.Node() for the documentation of each parameter. """ Node.__init__(self, node_name, **kwargs) - LifecycleMixin.__init__(self, + LifecycleMixin.__init__( + self, enable_communication_interface=enable_communication_interface) diff --git a/rclpy/src/rclpy/lifecycle.cpp b/rclpy/src/rclpy/lifecycle.cpp index 5699db28e..101cf6791 100644 --- a/rclpy/src/rclpy/lifecycle.cpp +++ b/rclpy/src/rclpy/lifecycle.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include @@ -28,6 +26,11 @@ #include #include +#include +#include +#include +#include + #include "destroyable.hpp" #include "exceptions.hpp" #include "lifecycle.hpp" @@ -38,7 +41,8 @@ namespace py = pybind11; namespace { -class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shared_from_this +class LifecycleStateMachine : public rclpy::Destroyable, + public std::enable_shared_from_this { public: LifecycleStateMachine( @@ -47,7 +51,7 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar { state_machine_ = std::shared_ptr( new rcl_lifecycle_state_machine_t(rcl_lifecycle_get_zero_initialized_state_machine()), - [node] (rcl_lifecycle_state_machine_t * state_machine) { + [node](rcl_lifecycle_state_machine_t * state_machine) { rcl_ret_t ret = rcl_lifecycle_state_machine_fini(state_machine, node.rcl_ptr()); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame @@ -127,7 +131,7 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar rcl_lifecycle_trigger_transition_by_id( state_machine_.get(), transition_id, publish_update) != RCL_RET_OK) { - throw rclpy::RCLError("Failed to trigger lifecycle state machine transition");; + throw rclpy::RCLError("Failed to trigger lifecycle state machine transition"); } } @@ -138,7 +142,7 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar rcl_lifecycle_trigger_transition_by_label( state_machine_.get(), label.c_str(), publish_update) != RCL_RET_OK) { - throw rclpy::RCLError("Failed to trigger lifecycle state machine transition");; + throw rclpy::RCLError("Failed to trigger lifecycle state machine transition"); } } @@ -147,8 +151,7 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar { const auto * transition = rcl_lifecycle_get_transition_by_label( state_machine_->current_state, label.c_str()); - if (nullptr == transition) - { + if (nullptr == transition) { throw rclpy::RCLError("Failed to get transition from label"); } return transition->id; @@ -177,9 +180,10 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar std::vector> ret; ret.reserve(state_machine_->transition_map.states_size); for (size_t i = 0; i <= state_machine_->transition_map.states_size; ++i) { - ret.emplace_back(std::make_tuple( - state_machine_->transition_map.states[i].id, - std_string_from_maybe_nullptr(state_machine_->transition_map.states[i].label))); + ret.emplace_back( + std::make_tuple( + state_machine_->transition_map.states[i].id, + std_string_from_maybe_nullptr(state_machine_->transition_map.states[i].label))); } return ret; } @@ -190,15 +194,17 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar std::vector> ret; ret.reserve(state_machine_->current_state->valid_transition_size); for (size_t i = 0; i < state_machine_->current_state->valid_transition_size; ++i) { - ret.emplace_back(std::make_tuple( - state_machine_->current_state->valid_transitions[i].id, - std_string_from_maybe_nullptr(state_machine_->current_state->valid_transitions[i].label), - state_machine_->current_state->valid_transitions[i].start->id, - std_string_from_maybe_nullptr( - state_machine_->current_state->valid_transitions[i].start->label), - state_machine_->current_state->valid_transitions[i].goal->id, - std_string_from_maybe_nullptr( - state_machine_->current_state->valid_transitions[i].goal->label))); + ret.emplace_back( + std::make_tuple( + state_machine_->current_state->valid_transitions[i].id, + std_string_from_maybe_nullptr( + state_machine_->current_state->valid_transitions[i].label), + state_machine_->current_state->valid_transitions[i].start->id, + std_string_from_maybe_nullptr( + state_machine_->current_state->valid_transitions[i].start->label), + state_machine_->current_state->valid_transitions[i].goal->id, + std_string_from_maybe_nullptr( + state_machine_->current_state->valid_transitions[i].goal->label))); } return ret; } @@ -209,13 +215,14 @@ class LifecycleStateMachine : public rclpy::Destroyable, public std::enable_shar std::vector> ret; ret.reserve(state_machine_->transition_map.transitions_size); for (size_t i = 0; i < state_machine_->transition_map.transitions_size; ++i) { - ret.emplace_back(std::make_tuple( - state_machine_->transition_map.transitions[i].id, - state_machine_->transition_map.transitions[i].label, - state_machine_->transition_map.transitions[i].start->id, - state_machine_->transition_map.transitions[i].start->label, - state_machine_->transition_map.transitions[i].goal->id, - state_machine_->transition_map.transitions[i].goal->label)); + ret.emplace_back( + std::make_tuple( + state_machine_->transition_map.transitions[i].id, + state_machine_->transition_map.transitions[i].label, + state_machine_->transition_map.transitions[i].start->id, + state_machine_->transition_map.transitions[i].start->label, + state_machine_->transition_map.transitions[i].goal->id, + state_machine_->transition_map.transitions[i].goal->label)); } return ret; } @@ -288,7 +295,7 @@ convert_callback_ret_code_to_label(TransitionCallbackReturnType cb_ret) throw std::runtime_error{"Invalid transition callback return type"}; } -} +} // namespace namespace rclpy { @@ -297,52 +304,52 @@ define_lifecycle_api(py::module m) { py::class_>( m, "LifecycleStateMachine") - .def(py::init()) - .def_property_readonly( - "initialized", &LifecycleStateMachine::is_initialized, - "Check if state machine is initialized.") - .def_property_readonly( - "current_state", &LifecycleStateMachine::get_current_state, - "Get the current state machine state.") - .def_property_readonly( - "available_states", &LifecycleStateMachine::get_available_states, - "Get the available states.") - .def_property_readonly( - "available_transitions", &LifecycleStateMachine::get_available_transitions, - "Get the available transitions.") - .def_property_readonly( - "transition_graph", &LifecycleStateMachine::get_transition_graph, - "Get the transition graph.") - .def( - "get_transition_by_label", &LifecycleStateMachine::get_transition_by_label, - "Get the transition id from a transition label.") - .def( - "trigger_transition_by_id", &LifecycleStateMachine::trigger_transition_by_id, - "Trigger a transition by transition id.") - .def( - "trigger_transition_by_label", &LifecycleStateMachine::trigger_transition_by_label, - "Trigger a transition by label.") - .def_property_readonly( - "service_change_state", &LifecycleStateMachine::get_srv_change_state, - "Get the change state service.") - .def_property_readonly( - "service_get_state", &LifecycleStateMachine::get_srv_get_state, - "Get the get state service.") - .def_property_readonly( - "service_get_available_states", &LifecycleStateMachine::get_srv_get_available_states, - "Get the get available states service.") - .def_property_readonly( - "service_get_available_transitions", &LifecycleStateMachine::get_srv_get_available_transitions, - "Get the get available transitions service.") - .def_property_readonly( - "service_get_transition_graph", &LifecycleStateMachine::get_srv_get_transition_graph, - "Get the get transition graph service."); + .def(py::init()) + .def_property_readonly( + "initialized", &LifecycleStateMachine::is_initialized, + "Check if state machine is initialized.") + .def_property_readonly( + "current_state", &LifecycleStateMachine::get_current_state, + "Get the current state machine state.") + .def_property_readonly( + "available_states", &LifecycleStateMachine::get_available_states, + "Get the available states.") + .def_property_readonly( + "available_transitions", &LifecycleStateMachine::get_available_transitions, + "Get the available transitions.") + .def_property_readonly( + "transition_graph", &LifecycleStateMachine::get_transition_graph, + "Get the transition graph.") + .def( + "get_transition_by_label", &LifecycleStateMachine::get_transition_by_label, + "Get the transition id from a transition label.") + .def( + "trigger_transition_by_id", &LifecycleStateMachine::trigger_transition_by_id, + "Trigger a transition by transition id.") + .def( + "trigger_transition_by_label", &LifecycleStateMachine::trigger_transition_by_label, + "Trigger a transition by label.") + .def_property_readonly( + "service_change_state", &LifecycleStateMachine::get_srv_change_state, + "Get the change state service.") + .def_property_readonly( + "service_get_state", &LifecycleStateMachine::get_srv_get_state, + "Get the get state service.") + .def_property_readonly( + "service_get_available_states", &LifecycleStateMachine::get_srv_get_available_states, + "Get the get available states service.") + .def_property_readonly( + "service_get_available_transitions", &LifecycleStateMachine::get_srv_get_available_transitions, + "Get the get available transitions service.") + .def_property_readonly( + "service_get_transition_graph", &LifecycleStateMachine::get_srv_get_transition_graph, + "Get the get transition graph service."); py::enum_(m, "TransitionCallbackReturnType") - .value("SUCCESS", TransitionCallbackReturnType::Success) - .value("FAILURE", TransitionCallbackReturnType::Failure) - .value("ERROR", TransitionCallbackReturnType::Error) - .def( - "to_label", &convert_callback_ret_code_to_label, - "Convert the transition callback return code to a transition label"); + .value("SUCCESS", TransitionCallbackReturnType::Success) + .value("FAILURE", TransitionCallbackReturnType::Failure) + .value("ERROR", TransitionCallbackReturnType::Error) + .def( + "to_label", &convert_callback_ret_code_to_label, + "Convert the transition callback return code to a transition label"); } } // namespace rclpy diff --git a/rclpy/src/rclpy/lifecycle.hpp b/rclpy/src/rclpy/lifecycle.hpp index 55f841a01..8c341521c 100644 --- a/rclpy/src/rclpy/lifecycle.hpp +++ b/rclpy/src/rclpy/lifecycle.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLPY__LIFECYCLE_API_HPP_ -#define RCLPY__LIFECYCLE_API_HPP_ +#ifndef RCLPY__LIFECYCLE_HPP_ +#define RCLPY__LIFECYCLE_HPP_ #include @@ -28,4 +28,4 @@ namespace rclpy void define_lifecycle_api(py::module module); } // namespace rclpy -#endif // RCLPY__LIFECYCLE_API_HPP_ +#endif // RCLPY__LIFECYCLE_HPP_ diff --git a/rclpy/test/test_lifecycle.py b/rclpy/test/test_lifecycle.py index 0fc1282c8..72ec59b18 100644 --- a/rclpy/test/test_lifecycle.py +++ b/rclpy/test/test_lifecycle.py @@ -19,6 +19,7 @@ def test_lifecycle_node_init(): rclpy.init() node = LifecycleNode('my_lifecycle_node') + assert node if __name__ == '__main__': From 581d030992d6dd1983c292a59d567869328a5e99 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 15 Dec 2021 18:47:52 -0300 Subject: [PATCH 09/34] Quick lifecycle publisher implementation Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle.py | 48 +++++++++++++++++++++++++++++++++++++++- rclpy/rclpy/node.py | 3 ++- 2 files changed, 49 insertions(+), 2 deletions(-) diff --git a/rclpy/rclpy/lifecycle.py b/rclpy/rclpy/lifecycle.py index 48f521859..ecde683a0 100644 --- a/rclpy/rclpy/lifecycle.py +++ b/rclpy/rclpy/lifecycle.py @@ -12,8 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Callable +from functools import wraps +from typing import Callable, overload from typing import Dict +from typing import Type from typing import Optional import lifecycle_msgs.srv @@ -21,6 +23,7 @@ from rclpy.callback_groups import CallbackGroup from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.node import Node +from rclpy.publisher import Publisher from rclpy.qos import QoSProfile from rclpy.service import Service from rclpy.type_support import check_is_valid_srv_type @@ -165,6 +168,12 @@ def register_callback( # Should we error/warn if overridding an existing callback? return True + def create_publisher(self, *args, **kwargs): + if 'publisher_class' in kwargs: + raise TypeError( + "create_publisher() got an unexpected keyword argument 'publisher_class'") + return Node.create_publisher(self, *args, **kwargs, publisher_class=LifecyclePublisher) + def __execute_callback( self, current_state: int, previous_state: int ) -> TransitionCallbackReturn: @@ -295,3 +304,40 @@ def __init__(self, node_name, *, enable_communication_interface: bool = True, ** LifecycleMixin.__init__( self, enable_communication_interface=enable_communication_interface) + + +class SimpleManagedEntity(ManagedEntity): + """A simple implementation of a managed entity that only sets a flag when (de)activated.""" + + def __init__(self): + self._enabled = False + + def on_activate(self, state) -> TransitionCallbackReturn: + self._enabled = True + return super().on_activate(state) + + def on_deactivate(self, state) -> TransitionCallbackReturn: + self._enabled = False + return super().on_deactivate(state) + + @property + def enabled(self): + return self._enabled + + def when_enabled(self, wrapped, when_not_enabled=None): + @wraps + def only_when_enabled_wrapper(*args, **kwargs): + if not self.enabled: + if when_not_enabled is not None: + when_not_enabled() + return + wrapped(*args, **kwargs) + + return only_when_enabled_wrapper + + +class LifecyclePublisher(SimpleManagedEntity, Publisher): + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.publish = self.when_enabled(self.publish) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 296ee75a1..a931bc9fc 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -1245,6 +1245,7 @@ def create_publisher( callback_group: Optional[CallbackGroup] = None, event_callbacks: Optional[PublisherEventCallbacks] = None, qos_overriding_options: Optional[QoSOverridingOptions] = None, + publisher_class: Type[Publisher] = Publisher, ) -> Publisher: """ Create a new publisher. @@ -1294,7 +1295,7 @@ def create_publisher( self._validate_topic_or_service_name(topic) try: - publisher = Publisher( + publisher = publisher_class( publisher_object, msg_type, topic, qos_profile, event_callbacks=event_callbacks or PublisherEventCallbacks(), callback_group=callback_group) From d8451beec458405286aaeebd68cb26fcac7551cd Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 15 Dec 2021 18:49:14 -0300 Subject: [PATCH 10/34] Minor style change Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclpy/rclpy/lifecycle.py b/rclpy/rclpy/lifecycle.py index ecde683a0..54c77b6fc 100644 --- a/rclpy/rclpy/lifecycle.py +++ b/rclpy/rclpy/lifecycle.py @@ -314,11 +314,11 @@ def __init__(self): def on_activate(self, state) -> TransitionCallbackReturn: self._enabled = True - return super().on_activate(state) + return TransitionCallbackReturn.SUCCESS def on_deactivate(self, state) -> TransitionCallbackReturn: self._enabled = False - return super().on_deactivate(state) + return TransitionCallbackReturn.SUCCESS @property def enabled(self): From f5967cfe9cd974535811b90d6327c963eb6a0aed Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 15 Dec 2021 19:06:38 -0300 Subject: [PATCH 11/34] Fix issues introduced when implementing LifecyclePublisher, automatically manage child entities Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle.py | 138 +++++++++++++++++++++++++++------------ rclpy/rclpy/node.py | 1 + 2 files changed, 99 insertions(+), 40 deletions(-) diff --git a/rclpy/rclpy/lifecycle.py b/rclpy/rclpy/lifecycle.py index 54c77b6fc..778c18148 100644 --- a/rclpy/rclpy/lifecycle.py +++ b/rclpy/rclpy/lifecycle.py @@ -13,10 +13,10 @@ # limitations under the License. from functools import wraps -from typing import Callable, overload +from typing import Callable from typing import Dict -from typing import Type from typing import Optional +from typing import Set import lifecycle_msgs.srv @@ -59,6 +59,43 @@ def on_error(self, state) -> TransitionCallbackReturn: return TransitionCallbackReturn.SUCCESS +class SimpleManagedEntity(ManagedEntity): + """A simple implementation of a managed entity that only sets a flag when (de)activated.""" + + def __init__(self): + self._enabled = False + + def on_activate(self, state) -> TransitionCallbackReturn: + self._enabled = True + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state) -> TransitionCallbackReturn: + self._enabled = False + return TransitionCallbackReturn.SUCCESS + + @property + def enabled(self): + return self._enabled + + def when_enabled(self, wrapped, when_not_enabled=None): + @wraps + def only_when_enabled_wrapper(*args, **kwargs): + if not self.enabled: + if when_not_enabled is not None: + when_not_enabled() + return + wrapped(*args, **kwargs) + + return only_when_enabled_wrapper + + +class LifecyclePublisher(SimpleManagedEntity, Publisher): + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.publish = self.when_enabled(self.publish) + + class LifecycleMixin(ManagedEntity): """ Mixin class to share as most code as possible between `Node` and `LifecycleNode`. @@ -90,6 +127,7 @@ def __init__( if callback_group is None: callback_group = self.default_callback_group self._callbacks: Dict[int, Callable[[int], TransitionCallbackReturn]] = {} + self._managed_entities: Set[ManagedEntity] = set() for srv_type in ( lifecycle_msgs.srv.ChangeState, lifecycle_msgs.srv.GetState, @@ -168,11 +206,68 @@ def register_callback( # Should we error/warn if overridding an existing callback? return True + def add_managed_entity(self, entity: ManagedEntity): + self._managed_entities.add(entity) + + def on_configure(self, state) -> TransitionCallbackReturn: + for entity in self._managed_entities: + ret = entity.on_configure(state) + # TODO(ivanpauno): Should we stop calling the other managed entities callabacks + # if one fails or errors? + # Should the behavior be the same in all the other cases? + if ret != TransitionCallbackReturn.SUCCESS: + return ret + return TransitionCallbackReturn.SUCCESS + + def on_cleanup(self, state) -> TransitionCallbackReturn: + for entity in self._managed_entities: + ret = entity.on_cleanup(state) + if ret != TransitionCallbackReturn.SUCCESS: + return ret + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state) -> TransitionCallbackReturn: + for entity in self._managed_entities: + ret = entity.on_shutdown(state) + if ret != TransitionCallbackReturn.SUCCESS: + return ret + return TransitionCallbackReturn.SUCCESS + + def on_activate(self, state) -> TransitionCallbackReturn: + for entity in self._managed_entities: + ret = entity.on_activate(state) + if ret != TransitionCallbackReturn.SUCCESS: + return ret + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state) -> TransitionCallbackReturn: + for entity in self._managed_entities: + ret = entity.on_deactivate(state) + if ret != TransitionCallbackReturn.SUCCESS: + return ret + return TransitionCallbackReturn.SUCCESS + + def on_error(self, state) -> TransitionCallbackReturn: + for entity in self._managed_entities: + ret = entity.on_error(state) + if ret != TransitionCallbackReturn.SUCCESS: + return ret + return TransitionCallbackReturn.SUCCESS + def create_publisher(self, *args, **kwargs): if 'publisher_class' in kwargs: raise TypeError( "create_publisher() got an unexpected keyword argument 'publisher_class'") - return Node.create_publisher(self, *args, **kwargs, publisher_class=LifecyclePublisher) + pub = Node.create_publisher(self, *args, **kwargs, publisher_class=LifecyclePublisher) + self._managed_entities.add(pub) + return pub + + def destroy_publisher(self, publisher: LifecyclePublisher): + try: + self._managed_entities.remove(publisher) + except KeyError: + pass + return Node.destroy_publisher(self, publisher) def __execute_callback( self, current_state: int, previous_state: int @@ -304,40 +399,3 @@ def __init__(self, node_name, *, enable_communication_interface: bool = True, ** LifecycleMixin.__init__( self, enable_communication_interface=enable_communication_interface) - - -class SimpleManagedEntity(ManagedEntity): - """A simple implementation of a managed entity that only sets a flag when (de)activated.""" - - def __init__(self): - self._enabled = False - - def on_activate(self, state) -> TransitionCallbackReturn: - self._enabled = True - return TransitionCallbackReturn.SUCCESS - - def on_deactivate(self, state) -> TransitionCallbackReturn: - self._enabled = False - return TransitionCallbackReturn.SUCCESS - - @property - def enabled(self): - return self._enabled - - def when_enabled(self, wrapped, when_not_enabled=None): - @wraps - def only_when_enabled_wrapper(*args, **kwargs): - if not self.enabled: - if when_not_enabled is not None: - when_not_enabled() - return - wrapped(*args, **kwargs) - - return only_when_enabled_wrapper - - -class LifecyclePublisher(SimpleManagedEntity, Publisher): - - def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) - self.publish = self.when_enabled(self.publish) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index a931bc9fc..9a6a8e2e6 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -22,6 +22,7 @@ from typing import Optional from typing import Sequence from typing import Tuple +from typing import Type from typing import TypeVar from typing import Union From ed57e05c894992246911ba182238ed1f5dc99e77 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 15 Dec 2021 19:14:38 -0300 Subject: [PATCH 12/34] Fixes to make it work again Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/rclpy/rclpy/lifecycle.py b/rclpy/rclpy/lifecycle.py index 778c18148..fc9b892b2 100644 --- a/rclpy/rclpy/lifecycle.py +++ b/rclpy/rclpy/lifecycle.py @@ -92,7 +92,8 @@ def only_when_enabled_wrapper(*args, **kwargs): class LifecyclePublisher(SimpleManagedEntity, Publisher): def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) + SimpleManagedEntity.__init__(self) + Publisher.__init__(self, *args, **kwargs) self.publish = self.when_enabled(self.publish) @@ -254,7 +255,13 @@ def on_error(self, state) -> TransitionCallbackReturn: return ret return TransitionCallbackReturn.SUCCESS - def create_publisher(self, *args, **kwargs): + def create_lifecycle_publisher(self, *args, **kwargs): + # TODO(ivanpauno): Should we override lifecycle publisher? + # There is an issue with python using the overridden method + # when creating publishers for builitin publishers (like parameters events). + # We could override them after init, similar to what we do to override publish() + # in LifecycleNode. + # Having both options seem fine. if 'publisher_class' in kwargs: raise TypeError( "create_publisher() got an unexpected keyword argument 'publisher_class'") @@ -262,7 +269,7 @@ def create_publisher(self, *args, **kwargs): self._managed_entities.add(pub) return pub - def destroy_publisher(self, publisher: LifecyclePublisher): + def destroy_lifecycle_publisher(self, publisher: LifecyclePublisher): try: self._managed_entities.remove(publisher) except KeyError: From 30cb7a0a02ce88d6a3016a07bcb033d0bdb951c9 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 16 Dec 2021 17:01:56 -0300 Subject: [PATCH 13/34] Modularize the implementation Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/__init__.py | 38 +++++++++ rclpy/rclpy/lifecycle/managed_entity.py | 77 +++++++++++++++++++ .../rclpy/{lifecycle.py => lifecycle/node.py} | 70 +---------------- rclpy/rclpy/lifecycle/publisher.py | 24 ++++++ rclpy/src/rclpy/lifecycle.cpp | 12 ++- 5 files changed, 152 insertions(+), 69 deletions(-) create mode 100644 rclpy/rclpy/lifecycle/__init__.py create mode 100644 rclpy/rclpy/lifecycle/managed_entity.py rename rclpy/rclpy/{lifecycle.py => lifecycle/node.py} (86%) create mode 100644 rclpy/rclpy/lifecycle/publisher.py diff --git a/rclpy/rclpy/lifecycle/__init__.py b/rclpy/rclpy/lifecycle/__init__.py new file mode 100644 index 000000000..edf248db9 --- /dev/null +++ b/rclpy/rclpy/lifecycle/__init__.py @@ -0,0 +1,38 @@ +# Copyright 2021 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. + +from .managed_entity import ManagedEntity +from .managed_entity import SimpleManagedEntity +from .node import LifecycleMixin +from .node import LifecycleNode +from .publisher import LifecyclePublisher + +from ..impl.implementation_singleton import rclpy_implementation as _rclpy + +# reexport LifecycleNode as Node, so it's possible to write: +# from rclpy.lifecycle import Node +# Do not include that in __all__ to avoid mixing it up with rclpy.node.Node. +Node = LifecycleNode +# same idea here +Publisher = LifecyclePublisher +# enum defined in pybind11 plugin +TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType + + +__all__ = [ + 'LifecycleMixin', + 'LifecyclePublisher', + 'ManagedEntity', + 'SimpleManagedEntity', +] diff --git a/rclpy/rclpy/lifecycle/managed_entity.py b/rclpy/rclpy/lifecycle/managed_entity.py new file mode 100644 index 000000000..0b09bf33d --- /dev/null +++ b/rclpy/rclpy/lifecycle/managed_entity.py @@ -0,0 +1,77 @@ +# Copyright 2021 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. + +from functools import wraps + +from ..impl.implementation_singleton import rclpy_implementation as _rclpy + + +TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType + + +class ManagedEntity: + + def on_configure(self, state) -> TransitionCallbackReturn: + """Handle configure transition request.""" + return TransitionCallbackReturn.SUCCESS + + def on_cleanup(self, state) -> TransitionCallbackReturn: + """Handle cleanup transition request.""" + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state) -> TransitionCallbackReturn: + """Handle shutdown transition request.""" + return TransitionCallbackReturn.SUCCESS + + def on_activate(self, state) -> TransitionCallbackReturn: + """Handle activate transition request.""" + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state) -> TransitionCallbackReturn: + """Handle deactivate transition request.""" + return TransitionCallbackReturn.SUCCESS + + def on_error(self, state) -> TransitionCallbackReturn: + """Handle error transition request.""" + return TransitionCallbackReturn.SUCCESS + + +class SimpleManagedEntity(ManagedEntity): + """A simple managed entity that only sets a flag when activated/deactivated.""" + + def __init__(self): + self._enabled = False + + def on_activate(self, state) -> TransitionCallbackReturn: + self._enabled = True + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state) -> TransitionCallbackReturn: + self._enabled = False + return TransitionCallbackReturn.SUCCESS + + @property + def enabled(self): + return self._enabled + + def when_enabled(self, wrapped, when_not_enabled=None): + @wraps + def only_when_enabled_wrapper(*args, **kwargs): + if not self.enabled: + if when_not_enabled is not None: + when_not_enabled() + return + wrapped(*args, **kwargs) + + return only_when_enabled_wrapper diff --git a/rclpy/rclpy/lifecycle.py b/rclpy/rclpy/lifecycle/node.py similarity index 86% rename from rclpy/rclpy/lifecycle.py rename to rclpy/rclpy/lifecycle/node.py index fc9b892b2..38ffd620f 100644 --- a/rclpy/rclpy/lifecycle.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from functools import wraps from typing import Callable from typing import Dict from typing import Optional @@ -23,78 +22,17 @@ from rclpy.callback_groups import CallbackGroup from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.node import Node -from rclpy.publisher import Publisher from rclpy.qos import QoSProfile from rclpy.service import Service from rclpy.type_support import check_is_valid_srv_type +from .managed_entity import ManagedEntity +from .publisher import LifecyclePublisher -TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType - - -class ManagedEntity: - - def on_configure(self, state) -> TransitionCallbackReturn: - """Handle configure transition request.""" - return TransitionCallbackReturn.SUCCESS - - def on_cleanup(self, state) -> TransitionCallbackReturn: - """Handle cleanup transition request.""" - return TransitionCallbackReturn.SUCCESS +from ..impl.implementation_singleton import rclpy_implementation as _rclpy - def on_shutdown(self, state) -> TransitionCallbackReturn: - """Handle shutdown transition request.""" - return TransitionCallbackReturn.SUCCESS - - def on_activate(self, state) -> TransitionCallbackReturn: - """Handle activate transition request.""" - return TransitionCallbackReturn.SUCCESS - def on_deactivate(self, state) -> TransitionCallbackReturn: - """Handle deactivate transition request.""" - return TransitionCallbackReturn.SUCCESS - - def on_error(self, state) -> TransitionCallbackReturn: - """Handle error transition request.""" - return TransitionCallbackReturn.SUCCESS - - -class SimpleManagedEntity(ManagedEntity): - """A simple implementation of a managed entity that only sets a flag when (de)activated.""" - - def __init__(self): - self._enabled = False - - def on_activate(self, state) -> TransitionCallbackReturn: - self._enabled = True - return TransitionCallbackReturn.SUCCESS - - def on_deactivate(self, state) -> TransitionCallbackReturn: - self._enabled = False - return TransitionCallbackReturn.SUCCESS - - @property - def enabled(self): - return self._enabled - - def when_enabled(self, wrapped, when_not_enabled=None): - @wraps - def only_when_enabled_wrapper(*args, **kwargs): - if not self.enabled: - if when_not_enabled is not None: - when_not_enabled() - return - wrapped(*args, **kwargs) - - return only_when_enabled_wrapper - - -class LifecyclePublisher(SimpleManagedEntity, Publisher): - - def __init__(self, *args, **kwargs): - SimpleManagedEntity.__init__(self) - Publisher.__init__(self, *args, **kwargs) - self.publish = self.when_enabled(self.publish) +TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType class LifecycleMixin(ManagedEntity): diff --git a/rclpy/rclpy/lifecycle/publisher.py b/rclpy/rclpy/lifecycle/publisher.py new file mode 100644 index 000000000..755975e4e --- /dev/null +++ b/rclpy/rclpy/lifecycle/publisher.py @@ -0,0 +1,24 @@ +# Copyright 2021 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. + +from rclpy.publisher import Publisher +from .managed_entity import SimpleManagedEntity + + +class LifecyclePublisher(SimpleManagedEntity, Publisher): + + def __init__(self, *args, **kwargs): + SimpleManagedEntity.__init__(self) + Publisher.__init__(self, *args, **kwargs) + self.publish = self.when_enabled(self.publish) diff --git a/rclpy/src/rclpy/lifecycle.cpp b/rclpy/src/rclpy/lifecycle.cpp index 101cf6791..a5c3e0d0c 100644 --- a/rclpy/src/rclpy/lifecycle.cpp +++ b/rclpy/src/rclpy/lifecycle.cpp @@ -345,9 +345,15 @@ define_lifecycle_api(py::module m) "service_get_transition_graph", &LifecycleStateMachine::get_srv_get_transition_graph, "Get the get transition graph service."); py::enum_(m, "TransitionCallbackReturnType") - .value("SUCCESS", TransitionCallbackReturnType::Success) - .value("FAILURE", TransitionCallbackReturnType::Failure) - .value("ERROR", TransitionCallbackReturnType::Error) + .value( + "SUCCESS", TransitionCallbackReturnType::Success, + "Callback succeeded.") + .value( + "FAILURE", TransitionCallbackReturnType::Failure, + "Callback failed.") + .value( + "ERROR", TransitionCallbackReturnType::Error, + "Callback had an error.") .def( "to_label", &convert_callback_ret_code_to_label, "Convert the transition callback return code to a transition label"); From b53bb36ab92e916ac7ed2acf4dbbbb6ef78c00be Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 16 Dec 2021 17:16:14 -0300 Subject: [PATCH 14/34] Improve SimpleManagedEntity.when_enabled Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/managed_entity.py | 24 ++++++++++++++---------- rclpy/rclpy/lifecycle/publisher.py | 9 ++++++++- 2 files changed, 22 insertions(+), 11 deletions(-) diff --git a/rclpy/rclpy/lifecycle/managed_entity.py b/rclpy/rclpy/lifecycle/managed_entity.py index 0b09bf33d..08eb788df 100644 --- a/rclpy/rclpy/lifecycle/managed_entity.py +++ b/rclpy/rclpy/lifecycle/managed_entity.py @@ -65,13 +65,17 @@ def on_deactivate(self, state) -> TransitionCallbackReturn: def enabled(self): return self._enabled - def when_enabled(self, wrapped, when_not_enabled=None): - @wraps - def only_when_enabled_wrapper(*args, **kwargs): - if not self.enabled: - if when_not_enabled is not None: - when_not_enabled() - return - wrapped(*args, **kwargs) - - return only_when_enabled_wrapper + @staticmethod + def when_enabled(wrapped=None, *, when_not_enabled=None): + def decorator(wrapped): + @wraps + def only_when_enabled_wrapper(self: SimpleManagedEntity, *args, **kwargs): + if not self.enabled: + if when_not_enabled is not None: + when_not_enabled() + return + wrapped(*args, **kwargs) + return only_when_enabled_wrapper + if wrapped is None: + return decorator + return decorator(wrapped) diff --git a/rclpy/rclpy/lifecycle/publisher.py b/rclpy/rclpy/lifecycle/publisher.py index 755975e4e..f2a90db51 100644 --- a/rclpy/rclpy/lifecycle/publisher.py +++ b/rclpy/rclpy/lifecycle/publisher.py @@ -12,7 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. +from typing import Union + +from rclpy.publisher import MsgType from rclpy.publisher import Publisher + from .managed_entity import SimpleManagedEntity @@ -21,4 +25,7 @@ class LifecyclePublisher(SimpleManagedEntity, Publisher): def __init__(self, *args, **kwargs): SimpleManagedEntity.__init__(self) Publisher.__init__(self, *args, **kwargs) - self.publish = self.when_enabled(self.publish) + + @SimpleManagedEntity.when_enabled + def publish(self, msg: Union[MsgType, bytes]) -> None: + Publisher.publish(msg) From d1ff70895e9359f5c0fd71e14a5bd549af5ea33e Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 16 Dec 2021 17:18:17 -0300 Subject: [PATCH 15/34] Add some docs to lifecycle publisher Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/publisher.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/rclpy/rclpy/lifecycle/publisher.py b/rclpy/rclpy/lifecycle/publisher.py index f2a90db51..1f0b6beb6 100644 --- a/rclpy/rclpy/lifecycle/publisher.py +++ b/rclpy/rclpy/lifecycle/publisher.py @@ -21,6 +21,7 @@ class LifecyclePublisher(SimpleManagedEntity, Publisher): + """Managed publisher entity.""" def __init__(self, *args, **kwargs): SimpleManagedEntity.__init__(self) @@ -28,4 +29,9 @@ def __init__(self, *args, **kwargs): @SimpleManagedEntity.when_enabled def publish(self, msg: Union[MsgType, bytes]) -> None: + """ + Publish a message if the lifecycle publisher is enabled. + + See rclpy.publisher.Publisher.publish() for more details. + """ Publisher.publish(msg) From 2ac7da136a2009dcec30cb3e7362ca7eb597a8f6 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 16 Dec 2021 17:58:47 -0300 Subject: [PATCH 16/34] Register all necessary callbacks, add State abstraction, other minor improvements Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/__init__.py | 5 ++++ rclpy/rclpy/lifecycle/node.py | 43 ++++++++++++++++++++++++++----- 2 files changed, 41 insertions(+), 7 deletions(-) diff --git a/rclpy/rclpy/lifecycle/__init__.py b/rclpy/rclpy/lifecycle/__init__.py index edf248db9..5bd91a836 100644 --- a/rclpy/rclpy/lifecycle/__init__.py +++ b/rclpy/rclpy/lifecycle/__init__.py @@ -16,6 +16,7 @@ from .managed_entity import SimpleManagedEntity from .node import LifecycleMixin from .node import LifecycleNode +from .node import LifecycleState from .publisher import LifecyclePublisher from ..impl.implementation_singleton import rclpy_implementation as _rclpy @@ -25,13 +26,17 @@ # Do not include that in __all__ to avoid mixing it up with rclpy.node.Node. Node = LifecycleNode # same idea here +State = LifecycleState Publisher = LifecyclePublisher + # enum defined in pybind11 plugin TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType __all__ = [ 'LifecycleMixin', + 'LifecycleNode', + 'LifecycleState', 'LifecyclePublisher', 'ManagedEntity', 'SimpleManagedEntity', diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 38ffd620f..365f2ad26 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -14,9 +14,11 @@ from typing import Callable from typing import Dict +from typing import NamedTuple from typing import Optional from typing import Set +import lifecycle_msgs.msg import lifecycle_msgs.srv from rclpy.callback_groups import CallbackGroup @@ -35,6 +37,11 @@ TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType +class LifecycleState(NamedTuple): + label: str + id: int + + class LifecycleMixin(ManagedEntity): """ Mixin class to share as most code as possible between `Node` and `LifecycleNode`. @@ -63,9 +70,29 @@ def __init__( :param callback_group: Callback group that will be used by all the lifecycle node services. """ + self._callbacks: Dict[int, Callable[[LifecycleState], TransitionCallbackReturn]] = {} + # register all state machine transition callbacks + self.__register_callback( + lifecycle_msgs.msg.State.TRANSITION_STATE_CONFIGURING, + self.on_configure) + self.__register_callback( + lifecycle_msgs.msg.State.TRANSITION_STATE_CLEANINGUP, + self.on_cleanup) + self.__register_callback( + lifecycle_msgs.msg.State.TRANSITION_STATE_SHUTTINGDOWN, + self.on_shutdown) + self.__register_callback( + lifecycle_msgs.msg.State.TRANSITION_STATE_ACTIVATING, + self.on_activate) + self.__register_callback( + lifecycle_msgs.msg.State.TRANSITION_STATE_DEACTIVATING, + self.on_deactivate) + self.__register_callback( + lifecycle_msgs.msg.State.TRANSITION_STATE_ERRORPROCESSING, + self.on_error) + if callback_group is None: callback_group = self.default_callback_group - self._callbacks: Dict[int, Callable[[int], TransitionCallbackReturn]] = {} self._managed_entities: Set[ManagedEntity] = set() for srv_type in ( lifecycle_msgs.srv.ChangeState, @@ -130,8 +157,9 @@ def __init__( # Maybe have some interface to add a service/etc instead (?). self._Node__services.extend(lifecycle_services) - def register_callback( - self, state_id: int, callback: Callable[[int], TransitionCallbackReturn] + + def __register_callback( + self, state_id: int, callback: Callable[[LifecycleState], TransitionCallbackReturn] ) -> bool: """ Register a callback that will be triggered when transitioning to state_id. @@ -215,9 +243,9 @@ def destroy_lifecycle_publisher(self, publisher: LifecyclePublisher): return Node.destroy_publisher(self, publisher) def __execute_callback( - self, current_state: int, previous_state: int + self, current_state_id: int, previous_state: LifecycleState ) -> TransitionCallbackReturn: - cb = self._callbacks.get(current_state, None) + cb = self._callbacks.get(current_state_id, None) if not cb: return TransitionCallbackReturn.SUCCESS try: @@ -228,11 +256,12 @@ def __execute_callback( def __change_state(self, transition_id: int): self.__check_is_initialized() - initial_state_id = self._state_machine.current_state[0] + initial_state = self._state_machine.current_state + initial_state = LifecycleState(id=initial_state[0], label=initial_state[1]) self._state_machine.trigger_transition_by_id(transition_id, True) cb_return_code = self.__execute_callback( - self._state_machine.current_state[0], initial_state_id) + self._state_machine.current_state[0], initial_state) self._state_machine.trigger_transition_by_label(cb_return_code.to_label(), True) if cb_return_code == TransitionCallbackReturn.ERROR: From 33157f1b5f7614dcf31768909e89e42efc967a4b Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 17 Dec 2021 10:41:26 -0300 Subject: [PATCH 17/34] Minor style changes, add local API to trigger transitions Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/__init__.py | 5 ++- rclpy/rclpy/lifecycle/node.py | 61 ++++++++++++++++++------------- 2 files changed, 39 insertions(+), 27 deletions(-) diff --git a/rclpy/rclpy/lifecycle/__init__.py b/rclpy/rclpy/lifecycle/__init__.py index 5bd91a836..37ec1ad49 100644 --- a/rclpy/rclpy/lifecycle/__init__.py +++ b/rclpy/rclpy/lifecycle/__init__.py @@ -14,7 +14,7 @@ from .managed_entity import ManagedEntity from .managed_entity import SimpleManagedEntity -from .node import LifecycleMixin +from .node import LifecycleNodeMixin from .node import LifecycleNode from .node import LifecycleState from .publisher import LifecyclePublisher @@ -26,6 +26,7 @@ # Do not include that in __all__ to avoid mixing it up with rclpy.node.Node. Node = LifecycleNode # same idea here +NodeMixin = LifecycleNodeMixin State = LifecycleState Publisher = LifecyclePublisher @@ -34,7 +35,7 @@ __all__ = [ - 'LifecycleMixin', + 'LifecycleNodeMixin', 'LifecycleNode', 'LifecycleState', 'LifecyclePublisher', diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 365f2ad26..7f73c63fe 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -42,7 +42,7 @@ class LifecycleState(NamedTuple): id: int -class LifecycleMixin(ManagedEntity): +class LifecycleNodeMixin(ManagedEntity): """ Mixin class to share as most code as possible between `Node` and `LifecycleNode`. @@ -50,12 +50,6 @@ class LifecycleMixin(ManagedEntity): as it access attributes created by `Node` directly here! """ - # TODO(ivanpauno): We could make this a bit nicer by adding State, Transition, StateMachine - # abstractions in this module. - # Anyways those are not directly useful to the user and the implementation is not too bad - # without them. - # TODO(ivanpauno): We actually need a State abstraction to make the register_callback() - # API nicer! def __init__( self, *, @@ -157,21 +151,23 @@ def __init__( # Maybe have some interface to add a service/etc instead (?). self._Node__services.extend(lifecycle_services) + def trigger_configure(self): + self.__change_state(lifecycle_msgs.msg.TRANSITION_CONFIGURE) - def __register_callback( - self, state_id: int, callback: Callable[[LifecycleState], TransitionCallbackReturn] - ) -> bool: - """ - Register a callback that will be triggered when transitioning to state_id. + def trigger_cleanup(self): + self.__change_state(lifecycle_msgs.msg.TRANSITION_CLEANUP) - The registered callback takes as an argument the previous state id, and returns - a TransitionCallbackReturn value. - """ - self._callbacks[state_id] = callback - # TODO(ivanpauno): Copying rclcpp style. - # Maybe having a return value doesn't make sense (?). - # Should we error/warn if overridding an existing callback? - return True + def trigger_shutdown(self): + self.__change_state(lifecycle_msgs.msg.TRANSITION_SHUTDOWN) + + def trigger_activate(self): + self.__change_state(lifecycle_msgs.msg.TRANSITION_ACTIVATE) + + def trigger_deactivate(self): + self.__change_state(lifecycle_msgs.msg.TRANSITION_DEACTIVATE) + + def trigger_error(self): + self.__change_state(lifecycle_msgs.msg.TRANSITION_ERROR) def add_managed_entity(self, entity: ManagedEntity): self._managed_entities.add(entity) @@ -242,6 +238,21 @@ def destroy_lifecycle_publisher(self, publisher: LifecyclePublisher): pass return Node.destroy_publisher(self, publisher) + def __register_callback( + self, state_id: int, callback: Callable[[LifecycleState], TransitionCallbackReturn] + ) -> bool: + """ + Register a callback that will be triggered when transitioning to state_id. + + The registered callback takes as an argument the previous state id, and returns + a TransitionCallbackReturn value. + """ + self._callbacks[state_id] = callback + # TODO(ivanpauno): Copying rclcpp style. + # Maybe having a return value doesn't make sense (?). + # Should we error/warn if overridding an existing callback? + return True + def __execute_callback( self, current_state_id: int, previous_state: LifecycleState ) -> TransitionCallbackReturn: @@ -354,22 +365,22 @@ def __on_get_transition_graph( return resp -class LifecycleNode(LifecycleMixin, Node): +class LifecycleNode(LifecycleNodeMixin, Node): """ A ROS 2 managed node. - This class extends Node with the methods provided by LifecycleMixin. - Methods in LifecycleMixin overridde the ones in Node. + This class extends Node with the methods provided by LifecycleNodeMixin. + Methods in LifecycleNodeMixin overridde the ones in Node. """ def __init__(self, node_name, *, enable_communication_interface: bool = True, **kwargs): """ Create a lifecycle node. - See rclpy.lifecycle.LifecycleMixin.__init__() and rclpy.node.Node() + See rclpy.lifecycle.LifecycleNodeMixin.__init__() and rclpy.node.Node() for the documentation of each parameter. """ Node.__init__(self, node_name, **kwargs) - LifecycleMixin.__init__( + LifecycleNodeMixin.__init__( self, enable_communication_interface=enable_communication_interface) From ebd1d9a78edf393a58156b545a12ed7cf7d54a0b Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 17 Dec 2021 11:11:39 -0300 Subject: [PATCH 18/34] Test LifecycleNode constructor, fix when enable_communication_interface=False Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/node.py | 101 +++++++++++++++++----------------- rclpy/rclpy/node.py | 2 +- rclpy/test/test_lifecycle.py | 22 +++++++- 3 files changed, 73 insertions(+), 52 deletions(-) diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 7f73c63fe..5284ae92a 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -100,56 +100,57 @@ def __init__( with self.handle: self._state_machine = _rclpy.LifecycleStateMachine( self.handle, enable_communication_interface) - self._service_change_state = Service( - self._state_machine.service_change_state, - lifecycle_msgs.srv.ChangeState, - self._state_machine.service_change_state.name, - self.__on_change_state, - callback_group, - QoSProfile(**self._state_machine.service_change_state.qos)) - self._service_get_state = Service( - self._state_machine.service_get_state, - lifecycle_msgs.srv.GetState, - self._state_machine.service_get_state.name, - self.__on_get_state, - callback_group, - QoSProfile(**self._state_machine.service_get_state.qos)) - self._service_get_available_states = Service( - self._state_machine.service_get_available_states, - lifecycle_msgs.srv.GetAvailableStates, - self._state_machine.service_get_available_states.name, - self.__on_get_available_states, - callback_group, - QoSProfile(**self._state_machine.service_get_available_states.qos)) - self._service_get_available_transitions = Service( - self._state_machine.service_get_available_transitions, - lifecycle_msgs.srv.GetAvailableTransitions, - self._state_machine.service_get_available_transitions.name, - self.__on_get_available_transitions, - callback_group, - QoSProfile(**self._state_machine.service_get_available_transitions.qos)) - self._service_get_transition_graph = Service( - self._state_machine.service_get_transition_graph, - lifecycle_msgs.srv.GetAvailableTransitions, - self._state_machine.service_get_transition_graph.name, - self.__on_get_transition_graph, - callback_group, - QoSProfile(**self._state_machine.service_get_transition_graph.qos)) - - lifecycle_services = [ - self._service_change_state, - self._service_get_state, - self._service_get_available_states, - self._service_get_available_transitions, - self._service_get_transition_graph, - ] - for s in lifecycle_services: - callback_group.add_entity(s) - # TODO(ivanpauno): Modify attribute in Node to be "protected" instead of "private". - # i.e. Node.__services -> Node._services - # Maybe the same with similar attributes (__publishers, etc). - # Maybe have some interface to add a service/etc instead (?). - self._Node__services.extend(lifecycle_services) + if enable_communication_interface: + self._service_change_state = Service( + self._state_machine.service_change_state, + lifecycle_msgs.srv.ChangeState, + self._state_machine.service_change_state.name, + self.__on_change_state, + callback_group, + QoSProfile(**self._state_machine.service_change_state.qos)) + self._service_get_state = Service( + self._state_machine.service_get_state, + lifecycle_msgs.srv.GetState, + self._state_machine.service_get_state.name, + self.__on_get_state, + callback_group, + QoSProfile(**self._state_machine.service_get_state.qos)) + self._service_get_available_states = Service( + self._state_machine.service_get_available_states, + lifecycle_msgs.srv.GetAvailableStates, + self._state_machine.service_get_available_states.name, + self.__on_get_available_states, + callback_group, + QoSProfile(**self._state_machine.service_get_available_states.qos)) + self._service_get_available_transitions = Service( + self._state_machine.service_get_available_transitions, + lifecycle_msgs.srv.GetAvailableTransitions, + self._state_machine.service_get_available_transitions.name, + self.__on_get_available_transitions, + callback_group, + QoSProfile(**self._state_machine.service_get_available_transitions.qos)) + self._service_get_transition_graph = Service( + self._state_machine.service_get_transition_graph, + lifecycle_msgs.srv.GetAvailableTransitions, + self._state_machine.service_get_transition_graph.name, + self.__on_get_transition_graph, + callback_group, + QoSProfile(**self._state_machine.service_get_transition_graph.qos)) + + lifecycle_services = [ + self._service_change_state, + self._service_get_state, + self._service_get_available_states, + self._service_get_available_transitions, + self._service_get_transition_graph, + ] + for s in lifecycle_services: + callback_group.add_entity(s) + # TODO(ivanpauno): Modify attribute in Node to be "protected" instead of "private". + # i.e. Node.__services -> Node._services + # Maybe the same with similar attributes (__publishers, etc). + # Maybe have some interface to add a service/etc instead (?). + self._Node__services.extend(lifecycle_services) def trigger_configure(self): self.__change_state(lifecycle_msgs.msg.TRANSITION_CONFIGURE) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 9a6a8e2e6..db4d793f3 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -1652,7 +1652,7 @@ def destroy_rate(self, rate: Rate): self.destroy_timer(rate._timer) rate.destroy() - def destroy_node(self) -> bool: + def destroy_node(self): """ Destroy the node. diff --git a/rclpy/test/test_lifecycle.py b/rclpy/test/test_lifecycle.py index 72ec59b18..6adabe09c 100644 --- a/rclpy/test/test_lifecycle.py +++ b/rclpy/test/test_lifecycle.py @@ -12,14 +12,34 @@ # See the License for the specific language governing permissions and # limitations under the License. +import pytest + import rclpy from rclpy.lifecycle import LifecycleNode def test_lifecycle_node_init(): rclpy.init() - node = LifecycleNode('my_lifecycle_node') + node = LifecycleNode('test_lifecycle_node_init_1') + assert node + node.destroy_node() + node = LifecycleNode('test_lifecycle_node_init_2', enable_communication_interface=False) assert node + # Make sure services were not created + assert not hasattr(node, '_service_change_state') + assert not hasattr(node, '_service_get_state') + assert not hasattr(node, '_service_get_available_states') + assert not hasattr(node, '_service_get_available_transitions') + assert not hasattr(node, '_service_get_transition_graph') + # Make sure also that the services were not created in the pybind11 plugin + assert not node._state_machine.service_change_state + assert not node._state_machine.service_get_state + assert not node._state_machine.service_get_available_states + assert not node._state_machine.service_get_available_transitions + assert not node._state_machine.service_get_transition_graph + node.destroy_node() + with pytest.raises(TypeError): + LifecycleNode('test_lifecycle_node_init_3', enable_communication_interface='asd') if __name__ == '__main__': From c6c4d73970e4e9054a67eb0d97d41f5c95e0cefa Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 17 Dec 2021 12:17:59 -0300 Subject: [PATCH 19/34] test transitions + fixes Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/node.py | 39 +++++++++++++++++++++-------------- rclpy/test/test_lifecycle.py | 39 +++++++++++++++++++++++++++++++++++ 2 files changed, 63 insertions(+), 15 deletions(-) diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 5284ae92a..099c05fe5 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -153,22 +153,25 @@ def __init__( self._Node__services.extend(lifecycle_services) def trigger_configure(self): - self.__change_state(lifecycle_msgs.msg.TRANSITION_CONFIGURE) + return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE) def trigger_cleanup(self): - self.__change_state(lifecycle_msgs.msg.TRANSITION_CLEANUP) + return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_CLEANUP) def trigger_shutdown(self): - self.__change_state(lifecycle_msgs.msg.TRANSITION_SHUTDOWN) + current_state = self._state_machine.current_state[1] + if current_state == 'unconfigured': + return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_UNCONFIGURED_SHUTDOWN) + if current_state == 'inactive': + return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_INACTIVE_SHUTDOWN) + if current_state == 'active': + return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN) def trigger_activate(self): - self.__change_state(lifecycle_msgs.msg.TRANSITION_ACTIVATE) + return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE) def trigger_deactivate(self): - self.__change_state(lifecycle_msgs.msg.TRANSITION_DEACTIVATE) - - def trigger_error(self): - self.__change_state(lifecycle_msgs.msg.TRANSITION_ERROR) + return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_DEACTIVATE) def add_managed_entity(self, entity: ManagedEntity): self._managed_entities.add(entity) @@ -258,15 +261,20 @@ def __execute_callback( self, current_state_id: int, previous_state: LifecycleState ) -> TransitionCallbackReturn: cb = self._callbacks.get(current_state_id, None) - if not cb: + if current_state_id == lifecycle_msgs.msg.State.TRANSITION_STATE_CONFIGURING: + print(f'cb: {cb}') + if cb is None: return TransitionCallbackReturn.SUCCESS try: - return cb(previous_state) + ret = cb(previous_state) + if current_state_id == lifecycle_msgs.msg.State.TRANSITION_STATE_CONFIGURING: + print(f'ret: {ret}') + return ret except Exception: # TODO(ivanpauno): log sth here return TransitionCallbackReturn.ERROR - def __change_state(self, transition_id: int): + def __change_state(self, transition_id: int) -> TransitionCallbackReturn: self.__check_is_initialized() initial_state = self._state_machine.current_state initial_state = LifecycleState(id=initial_state[0], label=initial_state[1]) @@ -277,10 +285,11 @@ def __change_state(self, transition_id: int): self._state_machine.trigger_transition_by_label(cb_return_code.to_label(), True) if cb_return_code == TransitionCallbackReturn.ERROR: - # TODO(ivanpauno): I don't understand what rclcpp is doing here ... - # It's triggering the error transition twice (?) - # https://github.com/ros2/rclcpp/blob/b2b676d3172ada509e58fa552a676a446809d83c/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp#L428-L443 - pass + # Now we're in the errorprocessing state, trigger the on_error callback + # and transition again based on the return code. + error_cb_ret_code = self.__execute_callback( + self._state_machine.current_state[0], initial_state) + self._state_machine.trigger_transition_by_label(error_cb_ret_code.to_label(), True) return cb_return_code def __check_is_initialized(self): diff --git a/rclpy/test/test_lifecycle.py b/rclpy/test/test_lifecycle.py index 6adabe09c..41d175cbc 100644 --- a/rclpy/test/test_lifecycle.py +++ b/rclpy/test/test_lifecycle.py @@ -15,7 +15,9 @@ import pytest import rclpy +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.lifecycle import LifecycleNode +from rclpy.lifecycle import TransitionCallbackReturn def test_lifecycle_node_init(): @@ -42,6 +44,43 @@ def test_lifecycle_node_init(): LifecycleNode('test_lifecycle_node_init_3', enable_communication_interface='asd') +def test_lifecycle_state_transitions(): + node = LifecycleNode('test_lifecycle_state_transitions_1', enable_communication_interface=False) + # normal transitions + assert node.trigger_configure() == TransitionCallbackReturn.SUCCESS + assert node.trigger_activate() == TransitionCallbackReturn.SUCCESS + assert node.trigger_deactivate() == TransitionCallbackReturn.SUCCESS + assert node.trigger_cleanup() == TransitionCallbackReturn.SUCCESS + # some that are not possible from the current state + with pytest.raises(_rclpy.RCLError): + node.trigger_activate() + with pytest.raises(_rclpy.RCLError): + node.trigger_deactivate() + assert node.trigger_shutdown() == TransitionCallbackReturn.SUCCESS + node.destroy_node() + + class ErrorOnConfigureHandledCorrectlyNode(LifecycleNode): + on_configure = lambda: TransitionCallbackReturn.ERROR + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + + node = ErrorOnConfigureHandledCorrectlyNode('test_lifecycle_state_transitions_2', enable_communication_interface=False) + assert node.trigger_configure() == TransitionCallbackReturn.ERROR + assert node._state_machine.current_state[1] == 'unconfigured' + + class ErrorOnConfigureHandledInCorrectlyNode(LifecycleNode): + on_configure = lambda: TransitionCallbackReturn.ERROR + on_error = lambda: TransitionCallbackReturn.FAILURE + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + + node = ErrorOnConfigureHandledInCorrectlyNode('test_lifecycle_state_transitions_3', enable_communication_interface=False) + assert node.trigger_configure() == TransitionCallbackReturn.ERROR + assert node._state_machine.current_state[1] == 'finalized' + + if __name__ == '__main__': rclpy.init() node = LifecycleNode('my_lifecycle_node') From 7517641f013ad4eb16fff972f05a10ae279a4b73 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 17 Dec 2021 17:35:29 -0300 Subject: [PATCH 20/34] Add tests for lifecycle publisher Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/managed_entity.py | 4 ++-- rclpy/rclpy/lifecycle/node.py | 4 ---- rclpy/rclpy/lifecycle/publisher.py | 2 +- rclpy/test/test_lifecycle.py | 21 +++++++++++++++++++++ 4 files changed, 24 insertions(+), 7 deletions(-) diff --git a/rclpy/rclpy/lifecycle/managed_entity.py b/rclpy/rclpy/lifecycle/managed_entity.py index 08eb788df..a3e5d8e37 100644 --- a/rclpy/rclpy/lifecycle/managed_entity.py +++ b/rclpy/rclpy/lifecycle/managed_entity.py @@ -68,13 +68,13 @@ def enabled(self): @staticmethod def when_enabled(wrapped=None, *, when_not_enabled=None): def decorator(wrapped): - @wraps + @wraps(wrapped) def only_when_enabled_wrapper(self: SimpleManagedEntity, *args, **kwargs): if not self.enabled: if when_not_enabled is not None: when_not_enabled() return - wrapped(*args, **kwargs) + wrapped(self, *args, **kwargs) return only_when_enabled_wrapper if wrapped is None: return decorator diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 099c05fe5..2036a7b56 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -261,14 +261,10 @@ def __execute_callback( self, current_state_id: int, previous_state: LifecycleState ) -> TransitionCallbackReturn: cb = self._callbacks.get(current_state_id, None) - if current_state_id == lifecycle_msgs.msg.State.TRANSITION_STATE_CONFIGURING: - print(f'cb: {cb}') if cb is None: return TransitionCallbackReturn.SUCCESS try: ret = cb(previous_state) - if current_state_id == lifecycle_msgs.msg.State.TRANSITION_STATE_CONFIGURING: - print(f'ret: {ret}') return ret except Exception: # TODO(ivanpauno): log sth here diff --git a/rclpy/rclpy/lifecycle/publisher.py b/rclpy/rclpy/lifecycle/publisher.py index 1f0b6beb6..7c31868a8 100644 --- a/rclpy/rclpy/lifecycle/publisher.py +++ b/rclpy/rclpy/lifecycle/publisher.py @@ -34,4 +34,4 @@ def publish(self, msg: Union[MsgType, bytes]) -> None: See rclpy.publisher.Publisher.publish() for more details. """ - Publisher.publish(msg) + Publisher.publish(self, msg) diff --git a/rclpy/test/test_lifecycle.py b/rclpy/test/test_lifecycle.py index 41d175cbc..c88f14319 100644 --- a/rclpy/test/test_lifecycle.py +++ b/rclpy/test/test_lifecycle.py @@ -13,11 +13,15 @@ # limitations under the License. import pytest +from unittest import mock import rclpy from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.lifecycle import LifecycleNode from rclpy.lifecycle import TransitionCallbackReturn +from rclpy.publisher import Publisher + +from test_msgs.msg import BasicTypes def test_lifecycle_node_init(): @@ -80,7 +84,24 @@ def __init__(self, *args, **kwargs): assert node.trigger_configure() == TransitionCallbackReturn.ERROR assert node._state_machine.current_state[1] == 'finalized' +# TODO(ivanpauno): Add automated tests for lifecycle services!! + +def test_lifecycle_publisher(): + node = LifecycleNode('test_lifecycle_publisher', enable_communication_interface=False) + with mock.patch.object(Publisher, 'publish') as mock_publish: + pub = node.create_lifecycle_publisher(BasicTypes, 'test_lifecycle_publisher_topic', 10) + pub.publish(BasicTypes()) + mock_publish.assert_not_called() + assert node.trigger_configure() == TransitionCallbackReturn.SUCCESS + node.trigger_activate() + msg = BasicTypes() + pub.publish(msg) + mock_publish.assert_called() + mock_publish.assert_called_with(pub, msg) + +# TODO(ivanpauno): DELETE THIS BEFORE MERGING +# CREATE A lifecycle_py demo if __name__ == '__main__': rclpy.init() node = LifecycleNode('my_lifecycle_node') From ce0d256c989de6acdabbbe573a8d3e137898bc69 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 17 Dec 2021 17:57:22 -0300 Subject: [PATCH 21/34] Please flake8 Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/__init__.py | 2 +- rclpy/rclpy/lifecycle/node.py | 12 ++++++------ rclpy/test/test_lifecycle.py | 25 ++++++++++++++++++------- 3 files changed, 25 insertions(+), 14 deletions(-) diff --git a/rclpy/rclpy/lifecycle/__init__.py b/rclpy/rclpy/lifecycle/__init__.py index 37ec1ad49..64f168873 100644 --- a/rclpy/rclpy/lifecycle/__init__.py +++ b/rclpy/rclpy/lifecycle/__init__.py @@ -14,8 +14,8 @@ from .managed_entity import ManagedEntity from .managed_entity import SimpleManagedEntity -from .node import LifecycleNodeMixin from .node import LifecycleNode +from .node import LifecycleNodeMixin from .node import LifecycleState from .publisher import LifecyclePublisher diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 2036a7b56..5855b622f 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -31,15 +31,13 @@ from .managed_entity import ManagedEntity from .publisher import LifecyclePublisher -from ..impl.implementation_singleton import rclpy_implementation as _rclpy - TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType class LifecycleState(NamedTuple): label: str - id: int + state_id: int class LifecycleNodeMixin(ManagedEntity): @@ -161,9 +159,11 @@ def trigger_cleanup(self): def trigger_shutdown(self): current_state = self._state_machine.current_state[1] if current_state == 'unconfigured': - return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_UNCONFIGURED_SHUTDOWN) + return self.__change_state( + lifecycle_msgs.msg.Transition.TRANSITION_UNCONFIGURED_SHUTDOWN) if current_state == 'inactive': - return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_INACTIVE_SHUTDOWN) + return self.__change_state( + lifecycle_msgs.msg.Transition.TRANSITION_INACTIVE_SHUTDOWN) if current_state == 'active': return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN) @@ -273,7 +273,7 @@ def __execute_callback( def __change_state(self, transition_id: int) -> TransitionCallbackReturn: self.__check_is_initialized() initial_state = self._state_machine.current_state - initial_state = LifecycleState(id=initial_state[0], label=initial_state[1]) + initial_state = LifecycleState(state_id=initial_state[0], label=initial_state[1]) self._state_machine.trigger_transition_by_id(transition_id, True) cb_return_code = self.__execute_callback( diff --git a/rclpy/test/test_lifecycle.py b/rclpy/test/test_lifecycle.py index c88f14319..57b08aede 100644 --- a/rclpy/test/test_lifecycle.py +++ b/rclpy/test/test_lifecycle.py @@ -12,9 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -import pytest from unittest import mock +import pytest + import rclpy from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.lifecycle import LifecycleNode @@ -49,7 +50,8 @@ def test_lifecycle_node_init(): def test_lifecycle_state_transitions(): - node = LifecycleNode('test_lifecycle_state_transitions_1', enable_communication_interface=False) + node = LifecycleNode( + 'test_lifecycle_state_transitions_1', enable_communication_interface=False) # normal transitions assert node.trigger_configure() == TransitionCallbackReturn.SUCCESS assert node.trigger_activate() == TransitionCallbackReturn.SUCCESS @@ -64,28 +66,37 @@ def test_lifecycle_state_transitions(): node.destroy_node() class ErrorOnConfigureHandledCorrectlyNode(LifecycleNode): - on_configure = lambda: TransitionCallbackReturn.ERROR + + def on_configure(self): + return TransitionCallbackReturn.ERROR def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) - node = ErrorOnConfigureHandledCorrectlyNode('test_lifecycle_state_transitions_2', enable_communication_interface=False) + node = ErrorOnConfigureHandledCorrectlyNode( + 'test_lifecycle_state_transitions_2', enable_communication_interface=False) assert node.trigger_configure() == TransitionCallbackReturn.ERROR assert node._state_machine.current_state[1] == 'unconfigured' class ErrorOnConfigureHandledInCorrectlyNode(LifecycleNode): - on_configure = lambda: TransitionCallbackReturn.ERROR - on_error = lambda: TransitionCallbackReturn.FAILURE + + def on_configure(self): + return TransitionCallbackReturn.ERROR + + def on_error(self): + return TransitionCallbackReturn.ERROR def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) - node = ErrorOnConfigureHandledInCorrectlyNode('test_lifecycle_state_transitions_3', enable_communication_interface=False) + node = ErrorOnConfigureHandledInCorrectlyNode( + 'test_lifecycle_state_transitions_3', enable_communication_interface=False) assert node.trigger_configure() == TransitionCallbackReturn.ERROR assert node._state_machine.current_state[1] == 'finalized' # TODO(ivanpauno): Add automated tests for lifecycle services!! + def test_lifecycle_publisher(): node = LifecycleNode('test_lifecycle_publisher', enable_communication_interface=False) with mock.patch.object(Publisher, 'publish') as mock_publish: From bd253ac081a3d647dd7bd48f427fc37380bb8b21 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 20 Dec 2021 17:17:08 -0300 Subject: [PATCH 22/34] Minor style change after writting the demos Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/managed_entity.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclpy/rclpy/lifecycle/managed_entity.py b/rclpy/rclpy/lifecycle/managed_entity.py index a3e5d8e37..93aaf9b62 100644 --- a/rclpy/rclpy/lifecycle/managed_entity.py +++ b/rclpy/rclpy/lifecycle/managed_entity.py @@ -62,7 +62,7 @@ def on_deactivate(self, state) -> TransitionCallbackReturn: return TransitionCallbackReturn.SUCCESS @property - def enabled(self): + def is_activated(self): return self._enabled @staticmethod @@ -70,7 +70,7 @@ def when_enabled(wrapped=None, *, when_not_enabled=None): def decorator(wrapped): @wraps(wrapped) def only_when_enabled_wrapper(self: SimpleManagedEntity, *args, **kwargs): - if not self.enabled: + if not self._enabled: if when_not_enabled is not None: when_not_enabled() return From 324d4ef39fcba3216c05e4a3c089af009fd1c174 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 20 Dec 2021 17:30:28 -0300 Subject: [PATCH 23/34] Fix misspelled word Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Audrow Nash --- rclpy/rclpy/lifecycle/node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 5855b622f..9e8ee6f4a 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -376,7 +376,7 @@ class LifecycleNode(LifecycleNodeMixin, Node): A ROS 2 managed node. This class extends Node with the methods provided by LifecycleNodeMixin. - Methods in LifecycleNodeMixin overridde the ones in Node. + Methods in LifecycleNodeMixin override the ones in Node. """ def __init__(self, node_name, *, enable_communication_interface: bool = True, **kwargs): From 261631567771643ff72f9a35173963da05f12792 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 20 Dec 2021 17:39:54 -0300 Subject: [PATCH 24/34] Make some attributes in Node "protected" instead of private for easier reuse in LifecycleNode and other derived classes Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/node.py | 7 +-- rclpy/rclpy/node.py | 84 +++++++++++++++++------------------ 2 files changed, 44 insertions(+), 47 deletions(-) diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 9e8ee6f4a..752b87fff 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -144,11 +144,8 @@ def __init__( ] for s in lifecycle_services: callback_group.add_entity(s) - # TODO(ivanpauno): Modify attribute in Node to be "protected" instead of "private". - # i.e. Node.__services -> Node._services - # Maybe the same with similar attributes (__publishers, etc). - # Maybe have some interface to add a service/etc instead (?). - self._Node__services.extend(lifecycle_services) + # Extend base class list of services, so they are added to the executor when spinning. + self._services.extend(lifecycle_services) def trigger_configure(self): return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index db4d793f3..f00765308 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -153,12 +153,12 @@ def __init__( self.__handle = None self._context = get_default_context() if context is None else context self._parameters: dict = {} - self.__publishers: List[Publisher] = [] - self.__subscriptions: List[Subscription] = [] - self.__clients: List[Client] = [] - self.__services: List[Service] = [] - self.__timers: List[Timer] = [] - self.__guards: List[GuardCondition] = [] + self._publishers: List[Publisher] = [] + self._subscriptions: List[Subscription] = [] + self._clients: List[Client] = [] + self._services: List[Service] = [] + self._timers: List[Timer] = [] + self._guards: List[GuardCondition] = [] self.__waitables: List[Waitable] = [] self._default_callback_group = MutuallyExclusiveCallbackGroup() self._parameters_callbacks: List[Callable[[List[Parameter]], SetParametersResult]] = [] @@ -228,32 +228,32 @@ def __init__( @property def publishers(self) -> Iterator[Publisher]: """Get publishers that have been created on this node.""" - yield from self.__publishers + yield from self._publishers @property def subscriptions(self) -> Iterator[Subscription]: """Get subscriptions that have been created on this node.""" - yield from self.__subscriptions + yield from self._subscriptions @property def clients(self) -> Iterator[Client]: """Get clients that have been created on this node.""" - yield from self.__clients + yield from self._clients @property def services(self) -> Iterator[Service]: """Get services that have been created on this node.""" - yield from self.__services + yield from self._services @property def timers(self) -> Iterator[Timer]: """Get timers that have been created on this node.""" - yield from self.__timers + yield from self._timers @property def guards(self) -> Iterator[GuardCondition]: """Get guards that have been created on this node.""" - yield from self.__guards + yield from self._guards @property def waitables(self) -> Iterator[Waitable]: @@ -1303,7 +1303,7 @@ def create_publisher( except Exception: publisher_object.destroy_when_not_in_use() raise - self.__publishers.append(publisher) + self._publishers.append(publisher) self._wake_executor() for event_callback in publisher.event_handlers: @@ -1381,7 +1381,7 @@ def create_subscription( subscription_object.destroy_when_not_in_use() raise callback_group.add_entity(subscription) - self.__subscriptions.append(subscription) + self._subscriptions.append(subscription) self._wake_executor() for event_handler in subscription.event_handlers: @@ -1427,7 +1427,7 @@ def create_client( client_impl, srv_type, srv_name, qos_profile, callback_group) callback_group.add_entity(client) - self.__clients.append(client) + self._clients.append(client) self._wake_executor() return client @@ -1471,7 +1471,7 @@ def create_service( service_impl, srv_type, srv_name, callback, callback_group, qos_profile) callback_group.add_entity(service) - self.__services.append(service) + self._services.append(service) self._wake_executor() return service @@ -1502,7 +1502,7 @@ def create_timer( timer = Timer(callback, callback_group, timer_period_nsec, clock, context=self.context) callback_group.add_entity(timer) - self.__timers.append(timer) + self._timers.append(timer) self._wake_executor() return timer @@ -1517,7 +1517,7 @@ def create_guard_condition( guard = GuardCondition(callback, callback_group, context=self.context) callback_group.add_entity(guard) - self.__guards.append(guard) + self._guards.append(guard) self._wake_executor() return guard @@ -1549,8 +1549,8 @@ def destroy_publisher(self, publisher: Publisher) -> bool: :return: ``True`` if successful, ``False`` otherwise. """ - if publisher in self.__publishers: - self.__publishers.remove(publisher) + if publisher in self._publishers: + self._publishers.remove(publisher) for event_handler in publisher.event_handlers: self.__waitables.remove(event_handler) try: @@ -1567,8 +1567,8 @@ def destroy_subscription(self, subscription: Subscription) -> bool: :return: ``True`` if succesful, ``False`` otherwise. """ - if subscription in self.__subscriptions: - self.__subscriptions.remove(subscription) + if subscription in self._subscriptions: + self._subscriptions.remove(subscription) for event_handler in subscription.event_handlers: self.__waitables.remove(event_handler) try: @@ -1585,8 +1585,8 @@ def destroy_client(self, client: Client) -> bool: :return: ``True`` if successful, ``False`` otherwise. """ - if client in self.__clients: - self.__clients.remove(client) + if client in self._clients: + self._clients.remove(client) try: client.destroy() except InvalidHandle: @@ -1601,8 +1601,8 @@ def destroy_service(self, service: Service) -> bool: :return: ``True`` if successful, ``False`` otherwise. """ - if service in self.__services: - self.__services.remove(service) + if service in self._services: + self._services.remove(service) try: service.destroy() except InvalidHandle: @@ -1617,8 +1617,8 @@ def destroy_timer(self, timer: Timer) -> bool: :return: ``True`` if successful, ``False`` otherwise. """ - if timer in self.__timers: - self.__timers.remove(timer) + if timer in self._timers: + self._timers.remove(timer) try: timer.destroy() except InvalidHandle: @@ -1633,8 +1633,8 @@ def destroy_guard_condition(self, guard: GuardCondition) -> bool: :return: ``True`` if successful, ``False`` otherwise. """ - if guard in self.__guards: - self.__guards.remove(guard) + if guard in self._guards: + self._guards.remove(guard) try: guard.destroy() except InvalidHandle: @@ -1672,18 +1672,18 @@ def destroy_node(self): # Destroy dependent items eagerly to work around a possible hang # https://github.com/ros2/build_cop/issues/248 - while self.__publishers: - self.destroy_publisher(self.__publishers[0]) - while self.__subscriptions: - self.destroy_subscription(self.__subscriptions[0]) - while self.__clients: - self.destroy_client(self.__clients[0]) - while self.__services: - self.destroy_service(self.__services[0]) - while self.__timers: - self.destroy_timer(self.__timers[0]) - while self.__guards: - self.destroy_guard_condition(self.__guards[0]) + while self._publishers: + self.destroy_publisher(self._publishers[0]) + while self._subscriptions: + self.destroy_subscription(self._subscriptions[0]) + while self._clients: + self.destroy_client(self._clients[0]) + while self._services: + self.destroy_service(self._services[0]) + while self._timers: + self.destroy_timer(self._timers[0]) + while self._guards: + self.destroy_guard_condition(self._guards[0]) self.__node.destroy_when_not_in_use() self._wake_executor() From b57a7794b97b41167ff34b9502d25377f93dd575 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 20 Dec 2021 17:47:43 -0300 Subject: [PATCH 25/34] Remove TODO, add better docs Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/node.py | 69 +++++++++++++++++++++++++++++++++-- 1 file changed, 66 insertions(+), 3 deletions(-) diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 752b87fff..ca8060182 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -174,16 +174,35 @@ def add_managed_entity(self, entity: ManagedEntity): self._managed_entities.add(entity) def on_configure(self, state) -> TransitionCallbackReturn: + """ + Handle a configuring transition. + + This is the default on_configure() callback. + It will call all on_configure() callbacks of managed entities, giving up at the first + entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR. + + It's possible to override this callback if the default behavior is not desired. + If you only want to extend what this callback does, make sure to call + super().on_configure() in derived classes. + """ for entity in self._managed_entities: ret = entity.on_configure(state) - # TODO(ivanpauno): Should we stop calling the other managed entities callabacks - # if one fails or errors? - # Should the behavior be the same in all the other cases? if ret != TransitionCallbackReturn.SUCCESS: return ret return TransitionCallbackReturn.SUCCESS def on_cleanup(self, state) -> TransitionCallbackReturn: + """ + Handle a cleaning up transition. + + This is the default on_cleanup() callback. + It will call all on_cleanup() callbacks of managed entities, giving up at the first + entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR. + + It's possible to override this callback if the default behavior is not desired. + If you only want to extend what this callback does, make sure to call + super().on_cleanup() in derived classes. + """ for entity in self._managed_entities: ret = entity.on_cleanup(state) if ret != TransitionCallbackReturn.SUCCESS: @@ -191,6 +210,17 @@ def on_cleanup(self, state) -> TransitionCallbackReturn: return TransitionCallbackReturn.SUCCESS def on_shutdown(self, state) -> TransitionCallbackReturn: + """ + Handle a shutting down transition. + + This is the default on_shutdown() callback. + It will call all on_shutdown() callbacks of managed entities, giving up at the first + entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR. + + It's possible to override this callback if the default behavior is not desired. + If you only want to extend what this callback does, make sure to call + super().on_shutdown() in derived classes. + """ for entity in self._managed_entities: ret = entity.on_shutdown(state) if ret != TransitionCallbackReturn.SUCCESS: @@ -198,6 +228,17 @@ def on_shutdown(self, state) -> TransitionCallbackReturn: return TransitionCallbackReturn.SUCCESS def on_activate(self, state) -> TransitionCallbackReturn: + """ + Handle an activating transition. + + This is the default on_activate() callback. + It will call all on_activate() callbacks of managed entities, giving up at the first + entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR. + + It's possible to override this callback if the default behavior is not desired. + If you only want to extend what this callback does, make sure to call + super().on_activate() in derived classes. + """ for entity in self._managed_entities: ret = entity.on_activate(state) if ret != TransitionCallbackReturn.SUCCESS: @@ -205,6 +246,17 @@ def on_activate(self, state) -> TransitionCallbackReturn: return TransitionCallbackReturn.SUCCESS def on_deactivate(self, state) -> TransitionCallbackReturn: + """ + Handle a deactivating transition. + + This is the default on_deactivate() callback. + It will call all on_deactivate() callbacks of managed entities, giving up at the first + entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR. + + It's possible to override this callback if the default behavior is not desired. + If you only want to extend what this callback does, make sure to call + super().on_deactivate() in derived classes. + """ for entity in self._managed_entities: ret = entity.on_deactivate(state) if ret != TransitionCallbackReturn.SUCCESS: @@ -212,6 +264,17 @@ def on_deactivate(self, state) -> TransitionCallbackReturn: return TransitionCallbackReturn.SUCCESS def on_error(self, state) -> TransitionCallbackReturn: + """ + Handle a transition error. + + This is the default on_error() callback. + It will call all on_error() callbacks of managed entities, giving up at the first + entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR. + + It's possible to override this callback if the default behavior is not desired. + If you only want to extend what this callback does, make sure to call + super().on_error() in derived classes. + """ for entity in self._managed_entities: ret = entity.on_error(state) if ret != TransitionCallbackReturn.SUCCESS: From 041ea6de108d6299f514c3eb717eea5e9c0cb337 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 20 Dec 2021 17:49:36 -0300 Subject: [PATCH 26/34] Remove TODO in __register_callback Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/node.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index ca8060182..7bfac795c 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -312,9 +312,6 @@ def __register_callback( a TransitionCallbackReturn value. """ self._callbacks[state_id] = callback - # TODO(ivanpauno): Copying rclcpp style. - # Maybe having a return value doesn't make sense (?). - # Should we error/warn if overridding an existing callback? return True def __execute_callback( From a72645bb734f039c6ae81dd4c1d7b1e7c7b5ddb0 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 20 Dec 2021 18:01:51 -0300 Subject: [PATCH 27/34] Remove TODO Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/node.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 7bfac795c..ed175857f 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -346,8 +346,6 @@ def __change_state(self, transition_id: int) -> TransitionCallbackReturn: return cb_return_code def __check_is_initialized(self): - # TODO(ivanpauno): This sanity check is probably not needed, just doing the same checks as - # rclcpp for the moment. if not self._state_machine.initialized: raise RuntimeError( 'Internal error: got service request while lifecycle state machine ' From bc982471c1109292b950d6c61b8f3cfbc34724a6 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 20 Dec 2021 18:20:24 -0300 Subject: [PATCH 28/34] Check transition callbacks return code type. Check type of added managed entitities. Share implementation of default transtion callbacks Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/node.py | 63 +++++++++++++++-------------------- 1 file changed, 27 insertions(+), 36 deletions(-) diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index ed175857f..0b9143e4e 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -171,9 +171,24 @@ def trigger_deactivate(self): return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_DEACTIVATE) def add_managed_entity(self, entity: ManagedEntity): + if not isinstance(entity, ManagedEntity): + raise TypeError('Expected a rclpy.lifecycle.ManagedEntity instance.') self._managed_entities.add(entity) - def on_configure(self, state) -> TransitionCallbackReturn: + def __transition_callback_impl(self, callback_name: str, state: LifecycleState): + for entity in self._managed_entities: + cb = getattr(entity, callback_name) + ret = cb(state) + if not isinstance(ret, TransitionCallbackReturn): + raise TypeError( + f'{callback_name}() return value of class {type(entity)} should be' + ' `TransitionCallbackReturn`.\n' + f'Instance of the class that caused the failure: {entity}') + if ret != TransitionCallbackReturn.SUCCESS: + return ret + return TransitionCallbackReturn.SUCCESS + + def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: """ Handle a configuring transition. @@ -185,13 +200,9 @@ def on_configure(self, state) -> TransitionCallbackReturn: If you only want to extend what this callback does, make sure to call super().on_configure() in derived classes. """ - for entity in self._managed_entities: - ret = entity.on_configure(state) - if ret != TransitionCallbackReturn.SUCCESS: - return ret - return TransitionCallbackReturn.SUCCESS + return self.__transition_callback_impl('on_configure', state) - def on_cleanup(self, state) -> TransitionCallbackReturn: + def on_cleanup(self, state: LifecycleState) -> TransitionCallbackReturn: """ Handle a cleaning up transition. @@ -203,13 +214,9 @@ def on_cleanup(self, state) -> TransitionCallbackReturn: If you only want to extend what this callback does, make sure to call super().on_cleanup() in derived classes. """ - for entity in self._managed_entities: - ret = entity.on_cleanup(state) - if ret != TransitionCallbackReturn.SUCCESS: - return ret - return TransitionCallbackReturn.SUCCESS + return self.__transition_callback_impl('on_cleanup', state) - def on_shutdown(self, state) -> TransitionCallbackReturn: + def on_shutdown(self, state: LifecycleState) -> TransitionCallbackReturn: """ Handle a shutting down transition. @@ -221,13 +228,9 @@ def on_shutdown(self, state) -> TransitionCallbackReturn: If you only want to extend what this callback does, make sure to call super().on_shutdown() in derived classes. """ - for entity in self._managed_entities: - ret = entity.on_shutdown(state) - if ret != TransitionCallbackReturn.SUCCESS: - return ret - return TransitionCallbackReturn.SUCCESS + return self.__transition_callback_impl('on_shutdown', state) - def on_activate(self, state) -> TransitionCallbackReturn: + def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn: """ Handle an activating transition. @@ -239,13 +242,9 @@ def on_activate(self, state) -> TransitionCallbackReturn: If you only want to extend what this callback does, make sure to call super().on_activate() in derived classes. """ - for entity in self._managed_entities: - ret = entity.on_activate(state) - if ret != TransitionCallbackReturn.SUCCESS: - return ret - return TransitionCallbackReturn.SUCCESS + return self.__transition_callback_impl('on_activate', state) - def on_deactivate(self, state) -> TransitionCallbackReturn: + def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn: """ Handle a deactivating transition. @@ -257,13 +256,9 @@ def on_deactivate(self, state) -> TransitionCallbackReturn: If you only want to extend what this callback does, make sure to call super().on_deactivate() in derived classes. """ - for entity in self._managed_entities: - ret = entity.on_deactivate(state) - if ret != TransitionCallbackReturn.SUCCESS: - return ret - return TransitionCallbackReturn.SUCCESS + return self.__transition_callback_impl('on_deactivate', state) - def on_error(self, state) -> TransitionCallbackReturn: + def on_error(self, state: LifecycleState) -> TransitionCallbackReturn: """ Handle a transition error. @@ -275,11 +270,7 @@ def on_error(self, state) -> TransitionCallbackReturn: If you only want to extend what this callback does, make sure to call super().on_error() in derived classes. """ - for entity in self._managed_entities: - ret = entity.on_error(state) - if ret != TransitionCallbackReturn.SUCCESS: - return ret - return TransitionCallbackReturn.SUCCESS + return self.__transition_callback_impl('on_error', state) def create_lifecycle_publisher(self, *args, **kwargs): # TODO(ivanpauno): Should we override lifecycle publisher? From 6c20f80e64b23dcec76afdc19189e5857c117e96 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 22 Dec 2021 15:42:05 -0300 Subject: [PATCH 29/34] Check that state is 'unconfigured' also before transition Signed-off-by: Ivan Santiago Paunovic --- rclpy/test/test_lifecycle.py | 1 + 1 file changed, 1 insertion(+) diff --git a/rclpy/test/test_lifecycle.py b/rclpy/test/test_lifecycle.py index 57b08aede..0dc78b556 100644 --- a/rclpy/test/test_lifecycle.py +++ b/rclpy/test/test_lifecycle.py @@ -75,6 +75,7 @@ def __init__(self, *args, **kwargs): node = ErrorOnConfigureHandledCorrectlyNode( 'test_lifecycle_state_transitions_2', enable_communication_interface=False) + assert node._state_machine.current_state[1] == 'unconfigured' assert node.trigger_configure() == TransitionCallbackReturn.ERROR assert node._state_machine.current_state[1] == 'unconfigured' From 928ee797ce0c72cb0b3aa80f3d5de797e5d6da6c Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 22 Dec 2021 17:04:28 -0300 Subject: [PATCH 30/34] Test lifecycle services Signed-off-by: Ivan Santiago Paunovic --- rclpy/test/test_lifecycle.py | 76 +++++++++++++++++++++++++++++++++++- 1 file changed, 75 insertions(+), 1 deletion(-) diff --git a/rclpy/test/test_lifecycle.py b/rclpy/test/test_lifecycle.py index 0dc78b556..d042c240d 100644 --- a/rclpy/test/test_lifecycle.py +++ b/rclpy/test/test_lifecycle.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +from threading import Thread from unittest import mock import pytest @@ -20,8 +21,12 @@ from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.lifecycle import LifecycleNode from rclpy.lifecycle import TransitionCallbackReturn +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node from rclpy.publisher import Publisher +import lifecycle_msgs.msg +import lifecycle_msgs.srv from test_msgs.msg import BasicTypes @@ -95,7 +100,76 @@ def __init__(self, *args, **kwargs): assert node.trigger_configure() == TransitionCallbackReturn.ERROR assert node._state_machine.current_state[1] == 'finalized' -# TODO(ivanpauno): Add automated tests for lifecycle services!! + +def test_lifecycle_services(request): + lc_node_name = 'test_lifecycle_services_lifecycle' + lc_node = LifecycleNode(lc_node_name) + client_node = Node('test_lifecycle_services_client') + get_state_cli = client_node.create_client( + lifecycle_msgs.srv.GetState, + f'/{lc_node_name}/get_state') + change_state_cli = client_node.create_client( + lifecycle_msgs.srv.ChangeState, + f'/{lc_node_name}/change_state') + get_available_states_cli = client_node.create_client( + lifecycle_msgs.srv.GetAvailableStates, + f'/{lc_node_name}/get_available_states') + get_available_transitions_cli = client_node.create_client( + lifecycle_msgs.srv.GetAvailableTransitions, + f'/{lc_node_name}/get_available_transitions') + get_transition_graph_cli = client_node.create_client( + lifecycle_msgs.srv.GetAvailableTransitions, + f'/{lc_node_name}/get_transition_graph') + for cli in ( + get_state_cli, + change_state_cli, + get_available_states_cli, + get_available_transitions_cli, + get_transition_graph_cli, + ): + assert cli.wait_for_service(5.) + # lunch a thread to spin the executor, so we can make sync service calls easily + exec = SingleThreadedExecutor() + exec.add_node(client_node) + exec.add_node(lc_node) + thread = Thread(target=exec.spin) + thread.start() + def cleanup(): + # Stop executor and join thread. + # This cleanup is run even if an assertion fails. + exec.shutdown() + thread.join() + request.addfinalizer(cleanup) + + # test all services + req = lifecycle_msgs.srv.GetState.Request() + resp = get_state_cli.call(req) + assert resp.current_state.label == 'unconfigured' + req = lifecycle_msgs.srv.ChangeState.Request() + req.transition.id = lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE + resp = change_state_cli.call(req) + assert resp.success + req = lifecycle_msgs.srv.GetState.Request() + resp = get_state_cli.call(req) + assert resp.current_state.label == 'inactive' + req = lifecycle_msgs.srv.GetAvailableStates.Request() + resp = get_available_states_cli.call(req) + states_labels = {state.label for state in resp.available_states} + assert states_labels == { + 'unknown', 'unconfigured', 'inactive', 'active', 'finalized', 'configuring', 'cleaningup', + 'shuttingdown', 'activating', 'deactivating', 'errorprocessing', '' + } + req = lifecycle_msgs.srv.GetAvailableTransitions.Request() + resp = get_available_transitions_cli.call(req) + transitions_labels = {transition_def.transition.label for transition_def in resp.available_transitions} + assert transitions_labels == {'activate', 'cleanup', 'shutdown'} + req = lifecycle_msgs.srv.GetAvailableTransitions.Request() + resp = get_transition_graph_cli.call(req) + transitions_labels = {transition_def.transition.label for transition_def in resp.available_transitions} + assert transitions_labels == { + 'configure' ,'activate', 'cleanup', 'shutdown', 'deactivate', 'transition_error', + 'transition_failure', 'transition_success' + } def test_lifecycle_publisher(): From 59fcc4c416452e7ad9c514aeb230c46857ea8449 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 22 Dec 2021 17:07:35 -0300 Subject: [PATCH 31/34] Delete early debugging code Signed-off-by: Ivan Santiago Paunovic --- rclpy/test/test_lifecycle.py | 8 -------- 1 file changed, 8 deletions(-) diff --git a/rclpy/test/test_lifecycle.py b/rclpy/test/test_lifecycle.py index d042c240d..a011fc200 100644 --- a/rclpy/test/test_lifecycle.py +++ b/rclpy/test/test_lifecycle.py @@ -184,11 +184,3 @@ def test_lifecycle_publisher(): pub.publish(msg) mock_publish.assert_called() mock_publish.assert_called_with(pub, msg) - - -# TODO(ivanpauno): DELETE THIS BEFORE MERGING -# CREATE A lifecycle_py demo -if __name__ == '__main__': - rclpy.init() - node = LifecycleNode('my_lifecycle_node') - rclpy.spin(node) From c31b4f6cf5acb116773299966b53cdaa2f5ae2a8 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 22 Dec 2021 17:12:17 -0300 Subject: [PATCH 32/34] Please linters Signed-off-by: Ivan Santiago Paunovic --- rclpy/test/test_lifecycle.py | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/rclpy/test/test_lifecycle.py b/rclpy/test/test_lifecycle.py index a011fc200..107415d48 100644 --- a/rclpy/test/test_lifecycle.py +++ b/rclpy/test/test_lifecycle.py @@ -15,18 +15,19 @@ from threading import Thread from unittest import mock +import lifecycle_msgs.msg +import lifecycle_msgs.srv + import pytest import rclpy +from rclpy.executors import SingleThreadedExecutor from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.lifecycle import LifecycleNode from rclpy.lifecycle import TransitionCallbackReturn -from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node from rclpy.publisher import Publisher -import lifecycle_msgs.msg -import lifecycle_msgs.srv from test_msgs.msg import BasicTypes @@ -129,15 +130,16 @@ def test_lifecycle_services(request): ): assert cli.wait_for_service(5.) # lunch a thread to spin the executor, so we can make sync service calls easily - exec = SingleThreadedExecutor() - exec.add_node(client_node) - exec.add_node(lc_node) - thread = Thread(target=exec.spin) + executor = SingleThreadedExecutor() + executor.add_node(client_node) + executor.add_node(lc_node) + thread = Thread(target=executor.spin) thread.start() + def cleanup(): # Stop executor and join thread. # This cleanup is run even if an assertion fails. - exec.shutdown() + executor.shutdown() thread.join() request.addfinalizer(cleanup) @@ -161,13 +163,15 @@ def cleanup(): } req = lifecycle_msgs.srv.GetAvailableTransitions.Request() resp = get_available_transitions_cli.call(req) - transitions_labels = {transition_def.transition.label for transition_def in resp.available_transitions} + transitions_labels = { + transition_def.transition.label for transition_def in resp.available_transitions} assert transitions_labels == {'activate', 'cleanup', 'shutdown'} req = lifecycle_msgs.srv.GetAvailableTransitions.Request() resp = get_transition_graph_cli.call(req) - transitions_labels = {transition_def.transition.label for transition_def in resp.available_transitions} + transitions_labels = { + transition_def.transition.label for transition_def in resp.available_transitions} assert transitions_labels == { - 'configure' ,'activate', 'cleanup', 'shutdown', 'deactivate', 'transition_error', + 'configure', 'activate', 'cleanup', 'shutdown', 'deactivate', 'transition_error', 'transition_failure', 'transition_success' } From 4ca2a368ee36920190d4535fbbf5d10da22cc7fd Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 22 Dec 2021 17:19:37 -0300 Subject: [PATCH 33/34] Test the two remaining shutdown transitions. Correct error handling when shutdown transition is not possible Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/lifecycle/node.py | 1 + rclpy/test/test_lifecycle.py | 12 ++++++++++++ 2 files changed, 13 insertions(+) diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 0b9143e4e..6528007d5 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -163,6 +163,7 @@ def trigger_shutdown(self): lifecycle_msgs.msg.Transition.TRANSITION_INACTIVE_SHUTDOWN) if current_state == 'active': return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN) + raise _rclpy.RCLError('Shutdown transtion not possible') def trigger_activate(self): return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE) diff --git a/rclpy/test/test_lifecycle.py b/rclpy/test/test_lifecycle.py index 107415d48..ac20bc04c 100644 --- a/rclpy/test/test_lifecycle.py +++ b/rclpy/test/test_lifecycle.py @@ -69,7 +69,19 @@ def test_lifecycle_state_transitions(): with pytest.raises(_rclpy.RCLError): node.trigger_deactivate() assert node.trigger_shutdown() == TransitionCallbackReturn.SUCCESS + with pytest.raises(_rclpy.RCLError): + node.trigger_shutdown() node.destroy_node() + # Again but trigger shutdown from 'inactive' instead of 'unconfigured' + node = LifecycleNode( + 'test_lifecycle_state_transitions_2', enable_communication_interface=False) + assert node.trigger_shutdown() == TransitionCallbackReturn.SUCCESS + # Again but trigger shutdown from 'active' + node = LifecycleNode( + 'test_lifecycle_state_transitions_3', enable_communication_interface=False) + assert node.trigger_configure() == TransitionCallbackReturn.SUCCESS + assert node.trigger_activate() == TransitionCallbackReturn.SUCCESS + assert node.trigger_shutdown() == TransitionCallbackReturn.SUCCESS class ErrorOnConfigureHandledCorrectlyNode(LifecycleNode): From c69339833e41dd12a127958e9999d0dba68d3ac8 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 23 Dec 2021 10:30:49 -0300 Subject: [PATCH 34/34] Address peer review comments Signed-off-by: Ivan Santiago Paunovic --- rclpy/test/test_lifecycle.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/rclpy/test/test_lifecycle.py b/rclpy/test/test_lifecycle.py index ac20bc04c..2cd425967 100644 --- a/rclpy/test/test_lifecycle.py +++ b/rclpy/test/test_lifecycle.py @@ -60,8 +60,13 @@ def test_lifecycle_state_transitions(): 'test_lifecycle_state_transitions_1', enable_communication_interface=False) # normal transitions assert node.trigger_configure() == TransitionCallbackReturn.SUCCESS - assert node.trigger_activate() == TransitionCallbackReturn.SUCCESS - assert node.trigger_deactivate() == TransitionCallbackReturn.SUCCESS + # test many times back to back, to make sure it works robustly + for _ in range(10): + assert node.trigger_cleanup() == TransitionCallbackReturn.SUCCESS + assert node.trigger_configure() == TransitionCallbackReturn.SUCCESS + for _ in range(10): + assert node.trigger_activate() == TransitionCallbackReturn.SUCCESS + assert node.trigger_deactivate() == TransitionCallbackReturn.SUCCESS assert node.trigger_cleanup() == TransitionCallbackReturn.SUCCESS # some that are not possible from the current state with pytest.raises(_rclpy.RCLError):