add rclcpp lifecycle
* initial state machine implementation (fix) correctly initialize default state machine uncrustify (dev) first high level api interface src/default_state_machine.c (fix) correctly initialize arrays in statemachine (dev) deactivate/activate publisher demo (dev) initial state machine implementation in rcl * (dev) demo application for a managed lifecycle node * add visibility control * correct install of c-library * fix compilation on windows * refactoring of external/internal api * (dev) generate static functions for c-callback * (fix) correct typo * (dev) cleanup for c-statemachine (dev) cleanup for c-statemachine * (dev) cpp callback map * (dev) mv source file into project folders * (dev) more helper functions for valid transition * (dev) pimpl implementation for cpp lifecyclemanager * (dev) register non-default callback functions * (dev) cleanup lifecycle node to serve as base class * (dev) new my_node child of lifecyclenode for demo purpose update stuff (cleanup) remove unused comments (fix) correct dllexport in windows (fix) correctly install libraries (fix) uncrustify (dev) composition over inheritance (dev) publish notification in state_machine transition (dev) lifecycle talker + listener demo for notification (dev) custom transition message generation (dev) publish transition message on state change (dev) correctly malloc/free c data structures (fix) use single thread executor (dev) use services for get state (fix) set freed pointer to NULL (dev) add change state service (dev) introduce services: get_state and change_state in LM (dev) asynchronous caller script for service client (fix) correct dllexport for pimpl (dev) correct constness (dev) concatenate function for topic (fix) uncrustify prepare new service api (tmp) refactor stash (fix) correctly concatenate topics (fix) correctly initialize Service wo/ copy (dev) call both service types extract demo files (fix) remove debug prints (dev) change to lifecycle_msgs (refactor) extract rcl_lifecycle package (refactor) extract lifecycle demos (fix) address review comments (fix) address review comments (fix) pass shared_ptr by value (fix) make find_package(rmw) required (fix) return to shared node handle pointer (refactor) attach sm to lifecycle node and disable lc_manager (dev) construct service from existing rcl_service_t (refactor) extract method for adding a service to a node (fix) stop mock msgs from being installed service takes rcl_node_t* correct typo add_service has to be public uncrustify initial state machine implementation (fix) correctly initialize default state machine uncrustify (dev) first high level api interface src/default_state_machine.c (fix) correctly initialize arrays in statemachine (dev) deactivate/activate publisher demo (dev) initial state machine implementation in rcl (dev) demo application for a managed lifecycle node add visibility control correct install of c-library fix compilation on windows refactoring of external/internal api (dev) generate static functions for c-callback (fix) correct typo (dev) cleanup for c-statemachine (dev) cleanup for c-statemachine (dev) cpp callback map (dev) mv source file into project folders (dev) more helper functions for valid transition (dev) pimpl implementation for cpp lifecyclemanager (dev) register non-default callback functions (dev) cleanup lifecycle node to serve as base class (dev) new my_node child of lifecyclenode for demo purpose update stuff (cleanup) remove unused comments (fix) correct dllexport in windows (fix) correctly install libraries (fix) uncrustify (dev) composition over inheritance (dev) publish notification in state_machine transition (dev) lifecycle talker + listener demo for notification (dev) custom transition message generation (dev) publish transition message on state change (dev) correctly malloc/free c data structures (fix) use single thread executor (dev) use services for get state (fix) set freed pointer to NULL (dev) add change state service (dev) introduce services: get_state and change_state in LM (dev) asynchronous caller script for service client (fix) correct dllexport for pimpl (dev) correct constness (dev) concatenate function for topic (fix) uncrustify prepare new service api (tmp) refactor stash * (dev) construct service from existing rcl_service_t * service takes rcl_node_t* * correct typo * add_service has to be public * uncrustify * (fix) correctly concatenate topics * (fix) correctly initialize Service wo/ copy * (dev) call both service types * extract demo files * (fix) remove debug prints * (dev) change to lifecycle_msgs * (refactor) extract rcl_lifecycle package * (refactor) extract lifecycle demos * (fix) address review comments (fix) address review comments * (fix) make find_package(rmw) required * (refactor) attach sm to lifecycle node and disable lc_manager * (fix) adjust code to rcl_test refactor * (dev) remove unused deps * (rebase) merge commit * (bugfix) correct rcl_ret_t error handling * (fix) depedencies * (refactor) change to lifecycle_msgs * (fix) correct find_rcl * (refactor) comply for new state machine * visibility control and test api * (rebase) change to new typesupport * uncrustify' * fix visibility control * (fix) correct whitespace * (fix) unused variable * comparison signed and unsigned * get_state returns complete state * get_available_states service * new service msgs * get available states and transitions api * (broken) state after rebase, does not compile demos * fix the way lifecycle node impl is included * fixed rebase compilation errors * remove copy&paste comment * remove empty line * (test) register custom callbacks * (dev) return codes * style * test for exception handling * refacotr new state machine * c++14 * change exception message for windows ci bug change exception message for windows ci bug
This commit is contained in:
Karsten Knese
GitHub
parent
d241a730fe
commit
2c6d95946e
19 changed files with 2712 additions and 0 deletions
310
rclcpp_lifecycle/src/lifecycle_node.cpp
Normal file
310
rclcpp_lifecycle/src/lifecycle_node.cpp
Normal file
|
|
@ -0,0 +1,310 @@
|
|||
// Copyright 2016 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
|
||||
#include "lifecycle_msgs/msg/state.hpp"
|
||||
#include "lifecycle_msgs/msg/transition.hpp"
|
||||
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
#include "rclcpp/graph_listener.hpp"
|
||||
#include "rclcpp/node.hpp"
|
||||
#include "rclcpp/node_interfaces/node_base.hpp"
|
||||
#include "rclcpp/node_interfaces/node_graph.hpp"
|
||||
#include "rclcpp/node_interfaces/node_parameters.hpp"
|
||||
#include "rclcpp/node_interfaces/node_services.hpp"
|
||||
#include "rclcpp/node_interfaces/node_timers.hpp"
|
||||
#include "rclcpp/node_interfaces/node_topics.hpp"
|
||||
|
||||
#include "lifecycle_node_interface_impl.hpp" // implementation
|
||||
|
||||
namespace rclcpp_lifecycle
|
||||
{
|
||||
|
||||
LifecycleNode::LifecycleNode(const std::string & node_name, bool use_intra_process_comms)
|
||||
: LifecycleNode(
|
||||
node_name,
|
||||
rclcpp::contexts::default_context::get_global_default_context(),
|
||||
use_intra_process_comms)
|
||||
{}
|
||||
|
||||
LifecycleNode::LifecycleNode(
|
||||
const std::string & node_name,
|
||||
rclcpp::context::Context::SharedPtr context,
|
||||
bool use_intra_process_comms)
|
||||
: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, context)),
|
||||
node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())),
|
||||
node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())),
|
||||
node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())),
|
||||
node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())),
|
||||
node_parameters_(new rclcpp::node_interfaces::NodeParameters(
|
||||
node_topics_.get(),
|
||||
use_intra_process_comms
|
||||
)),
|
||||
use_intra_process_comms_(use_intra_process_comms),
|
||||
impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_))
|
||||
{
|
||||
impl_->init();
|
||||
|
||||
register_on_configure(std::bind(&LifecycleNodeInterface::on_configure, this,
|
||||
std::placeholders::_1));
|
||||
register_on_cleanup(std::bind(&LifecycleNodeInterface::on_cleanup, this, std::placeholders::_1));
|
||||
register_on_shutdown(std::bind(&LifecycleNodeInterface::on_shutdown, this,
|
||||
std::placeholders::_1));
|
||||
register_on_activate(std::bind(&LifecycleNodeInterface::on_activate, this,
|
||||
std::placeholders::_1));
|
||||
register_on_deactivate(std::bind(&LifecycleNodeInterface::on_deactivate, this,
|
||||
std::placeholders::_1));
|
||||
register_on_error(std::bind(&LifecycleNodeInterface::on_error, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
LifecycleNode::~LifecycleNode()
|
||||
{}
|
||||
|
||||
const char *
|
||||
LifecycleNode::get_name() const
|
||||
{
|
||||
return node_base_->get_name();
|
||||
}
|
||||
|
||||
rclcpp::callback_group::CallbackGroup::SharedPtr
|
||||
LifecycleNode::create_callback_group(
|
||||
rclcpp::callback_group::CallbackGroupType group_type)
|
||||
{
|
||||
return node_base_->create_callback_group(group_type);
|
||||
}
|
||||
|
||||
bool
|
||||
LifecycleNode::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group)
|
||||
{
|
||||
return node_base_->callback_group_in_node(group);
|
||||
}
|
||||
|
||||
std::vector<rcl_interfaces::msg::SetParametersResult>
|
||||
LifecycleNode::set_parameters(
|
||||
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
|
||||
{
|
||||
return node_parameters_->set_parameters(parameters);
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
LifecycleNode::set_parameters_atomically(
|
||||
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
|
||||
{
|
||||
return node_parameters_->set_parameters_atomically(parameters);
|
||||
}
|
||||
|
||||
std::vector<rclcpp::parameter::ParameterVariant>
|
||||
LifecycleNode::get_parameters(
|
||||
const std::vector<std::string> & names) const
|
||||
{
|
||||
return node_parameters_->get_parameters(names);
|
||||
}
|
||||
|
||||
rclcpp::parameter::ParameterVariant
|
||||
LifecycleNode::get_parameter(const std::string & name) const
|
||||
{
|
||||
return node_parameters_->get_parameter(name);
|
||||
}
|
||||
|
||||
bool LifecycleNode::get_parameter(const std::string & name,
|
||||
rclcpp::parameter::ParameterVariant & parameter) const
|
||||
{
|
||||
return node_parameters_->get_parameter(name, parameter);
|
||||
}
|
||||
|
||||
std::vector<rcl_interfaces::msg::ParameterDescriptor>
|
||||
LifecycleNode::describe_parameters(
|
||||
const std::vector<std::string> & names) const
|
||||
{
|
||||
return node_parameters_->describe_parameters(names);
|
||||
}
|
||||
|
||||
std::vector<uint8_t>
|
||||
LifecycleNode::get_parameter_types(
|
||||
const std::vector<std::string> & names) const
|
||||
{
|
||||
return node_parameters_->get_parameter_types(names);
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::ListParametersResult
|
||||
LifecycleNode::list_parameters(
|
||||
const std::vector<std::string> & prefixes, uint64_t depth) const
|
||||
{
|
||||
return node_parameters_->list_parameters(prefixes, depth);
|
||||
}
|
||||
|
||||
std::map<std::string, std::string>
|
||||
LifecycleNode::get_topic_names_and_types() const
|
||||
{
|
||||
return node_graph_->get_topic_names_and_types();
|
||||
}
|
||||
|
||||
size_t
|
||||
LifecycleNode::count_publishers(const std::string & topic_name) const
|
||||
{
|
||||
return node_graph_->count_publishers(topic_name);
|
||||
}
|
||||
|
||||
size_t
|
||||
LifecycleNode::count_subscribers(const std::string & topic_name) const
|
||||
{
|
||||
return node_graph_->count_subscribers(topic_name);
|
||||
}
|
||||
|
||||
const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
|
||||
LifecycleNode::get_callback_groups() const
|
||||
{
|
||||
return node_base_->get_callback_groups();
|
||||
}
|
||||
|
||||
rclcpp::event::Event::SharedPtr
|
||||
LifecycleNode::get_graph_event()
|
||||
{
|
||||
return node_graph_->get_graph_event();
|
||||
}
|
||||
|
||||
void
|
||||
LifecycleNode::wait_for_graph_change(
|
||||
rclcpp::event::Event::SharedPtr event,
|
||||
std::chrono::nanoseconds timeout)
|
||||
{
|
||||
node_graph_->wait_for_graph_change(event, timeout);
|
||||
}
|
||||
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
|
||||
LifecycleNode::get_node_base_interface()
|
||||
{
|
||||
return node_base_;
|
||||
}
|
||||
|
||||
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr
|
||||
LifecycleNode::get_node_graph_interface()
|
||||
{
|
||||
return node_graph_;
|
||||
}
|
||||
|
||||
rclcpp::node_interfaces::NodeTimersInterface::SharedPtr
|
||||
LifecycleNode::get_node_timers_interface()
|
||||
{
|
||||
return node_timers_;
|
||||
}
|
||||
|
||||
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr
|
||||
LifecycleNode::get_node_topics_interface()
|
||||
{
|
||||
return node_topics_;
|
||||
}
|
||||
|
||||
rclcpp::node_interfaces::NodeServicesInterface::SharedPtr
|
||||
LifecycleNode::get_node_services_interface()
|
||||
{
|
||||
return node_services_;
|
||||
}
|
||||
|
||||
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
|
||||
LifecycleNode::get_node_parameters_interface()
|
||||
{
|
||||
return node_parameters_;
|
||||
}
|
||||
|
||||
|
||||
////
|
||||
bool
|
||||
LifecycleNode::register_on_configure(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
||||
{
|
||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn);
|
||||
}
|
||||
|
||||
bool
|
||||
LifecycleNode::register_on_cleanup(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
||||
{
|
||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn);
|
||||
}
|
||||
|
||||
bool
|
||||
LifecycleNode::register_on_shutdown(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
||||
{
|
||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn);
|
||||
}
|
||||
|
||||
bool
|
||||
LifecycleNode::register_on_activate(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
||||
{
|
||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn);
|
||||
}
|
||||
|
||||
bool
|
||||
LifecycleNode::register_on_deactivate(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
||||
{
|
||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn);
|
||||
}
|
||||
|
||||
bool
|
||||
LifecycleNode::register_on_error(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
||||
{
|
||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING,
|
||||
fcn);
|
||||
}
|
||||
|
||||
const State &
|
||||
LifecycleNode::get_current_state()
|
||||
{
|
||||
return impl_->get_current_state();
|
||||
}
|
||||
|
||||
std::vector<State>
|
||||
LifecycleNode::get_available_states()
|
||||
{
|
||||
return impl_->get_available_states();
|
||||
}
|
||||
|
||||
std::vector<Transition>
|
||||
LifecycleNode::get_available_transitions()
|
||||
{
|
||||
return impl_->get_available_transitions();
|
||||
}
|
||||
|
||||
const State &
|
||||
LifecycleNode::trigger_transition(const Transition & transition)
|
||||
{
|
||||
return trigger_transition(transition.id());
|
||||
}
|
||||
|
||||
const State &
|
||||
LifecycleNode::trigger_transition(unsigned int transition_id)
|
||||
{
|
||||
return impl_->trigger_transition(transition_id);
|
||||
}
|
||||
|
||||
void
|
||||
LifecycleNode::add_publisher_handle(
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisherInterface> pub)
|
||||
{
|
||||
impl_->add_publisher_handle(pub);
|
||||
}
|
||||
|
||||
void
|
||||
LifecycleNode::add_timer_handle(std::shared_ptr<rclcpp::timer::TimerBase> timer)
|
||||
{
|
||||
impl_->add_timer_handle(timer);
|
||||
}
|
||||
|
||||
} // namespace rclcpp_lifecycle
|
||||
397
rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp
Normal file
397
rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp
Normal file
|
|
@ -0,0 +1,397 @@
|
|||
// Copyright 2016 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef LIFECYCLE_NODE_INTERFACE_IMPL_HPP_
|
||||
#define LIFECYCLE_NODE_INTERFACE_IMPL_HPP_
|
||||
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
|
||||
#include <functional>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
|
||||
#include "lifecycle_msgs/msg/transition_description.hpp"
|
||||
#include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport
|
||||
#include "lifecycle_msgs/msg/transition_event.hpp"
|
||||
#include "lifecycle_msgs/srv/change_state.hpp"
|
||||
#include "lifecycle_msgs/srv/get_state.hpp"
|
||||
#include "lifecycle_msgs/srv/get_available_states.hpp"
|
||||
#include "lifecycle_msgs/srv/get_available_transitions.hpp"
|
||||
|
||||
#include "rcl/error_handling.h"
|
||||
|
||||
#include "rcl_lifecycle/rcl_lifecycle.h"
|
||||
|
||||
#include "rclcpp/node_interfaces/node_base_interface.hpp"
|
||||
#include "rclcpp/node_interfaces/node_services_interface.hpp"
|
||||
|
||||
namespace rclcpp_lifecycle
|
||||
{
|
||||
|
||||
class LifecycleNode::LifecycleNodeInterfaceImpl
|
||||
{
|
||||
using ChangeStateSrv = lifecycle_msgs::srv::ChangeState;
|
||||
using GetStateSrv = lifecycle_msgs::srv::GetState;
|
||||
using GetAvailableStatesSrv = lifecycle_msgs::srv::GetAvailableStates;
|
||||
using GetAvailableTransitionsSrv = lifecycle_msgs::srv::GetAvailableTransitions;
|
||||
using TransitionEventMsg = lifecycle_msgs::msg::TransitionEvent;
|
||||
|
||||
public:
|
||||
LifecycleNodeInterfaceImpl(
|
||||
std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_interface,
|
||||
std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> node_services_interface)
|
||||
: node_base_interface_(node_base_interface),
|
||||
node_services_interface_(node_services_interface)
|
||||
{}
|
||||
|
||||
~LifecycleNodeInterfaceImpl()
|
||||
{
|
||||
if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
|
||||
fprintf(stderr, "%s:%u, FATAL: rcl_state_machine got destroyed externally.\n",
|
||||
__FILE__, __LINE__);
|
||||
} else {
|
||||
rcl_lifecycle_state_machine_fini(&state_machine_,
|
||||
node_base_interface_->get_rcl_node_handle());
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
init()
|
||||
{
|
||||
state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine();
|
||||
// The call to initialize the state machine takes
|
||||
// currently five different typesupports for all publishers/services
|
||||
// created within the RCL_LIFECYCLE structure.
|
||||
// The publisher takes a C-Typesupport since the publishing (i.e. creating
|
||||
// the message) is done fully in RCL.
|
||||
// Services are handled in C++, so that it needs a C++ typesupport structure.
|
||||
rcl_ret_t ret = rcl_lifecycle_state_machine_init(
|
||||
&state_machine_, node_base_interface_->get_rcl_node_handle(),
|
||||
ROSIDL_GET_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent),
|
||||
rosidl_typesupport_cpp::get_service_type_support_handle<ChangeStateSrv>(),
|
||||
rosidl_typesupport_cpp::get_service_type_support_handle<GetStateSrv>(),
|
||||
rosidl_typesupport_cpp::get_service_type_support_handle<GetAvailableStatesSrv>(),
|
||||
rosidl_typesupport_cpp::get_service_type_support_handle<GetAvailableTransitionsSrv>(),
|
||||
true);
|
||||
if (ret != RCL_RET_OK) {
|
||||
throw std::runtime_error(
|
||||
std::string(
|
||||
"Couldn't initialize state machine for node ") + node_base_interface_->get_name());
|
||||
}
|
||||
|
||||
{ // change_state
|
||||
auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_change_state, this,
|
||||
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
|
||||
rclcpp::any_service_callback::AnyServiceCallback<ChangeStateSrv> any_cb;
|
||||
any_cb.set(cb);
|
||||
|
||||
srv_change_state_ = std::make_shared<rclcpp::service::Service<ChangeStateSrv>>(
|
||||
node_base_interface_->get_shared_rcl_node_handle(),
|
||||
&state_machine_.com_interface.srv_change_state,
|
||||
any_cb);
|
||||
node_services_interface_->add_service(
|
||||
std::dynamic_pointer_cast<rclcpp::service::ServiceBase>(srv_change_state_),
|
||||
nullptr);
|
||||
}
|
||||
|
||||
{ // get_state
|
||||
auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_get_state, this,
|
||||
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
|
||||
rclcpp::any_service_callback::AnyServiceCallback<GetStateSrv> any_cb;
|
||||
any_cb.set(std::move(cb));
|
||||
|
||||
srv_get_state_ = std::make_shared<rclcpp::service::Service<GetStateSrv>>(
|
||||
node_base_interface_->get_shared_rcl_node_handle(),
|
||||
&state_machine_.com_interface.srv_get_state,
|
||||
any_cb);
|
||||
node_services_interface_->add_service(
|
||||
std::dynamic_pointer_cast<rclcpp::service::ServiceBase>(srv_get_state_),
|
||||
nullptr);
|
||||
}
|
||||
|
||||
{ // get_available_states
|
||||
auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_get_available_states, this,
|
||||
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
|
||||
rclcpp::any_service_callback::AnyServiceCallback<GetAvailableStatesSrv> any_cb;
|
||||
any_cb.set(std::move(cb));
|
||||
|
||||
srv_get_available_states_ = std::make_shared<rclcpp::service::Service<GetAvailableStatesSrv>>(
|
||||
node_base_interface_->get_shared_rcl_node_handle(),
|
||||
&state_machine_.com_interface.srv_get_available_states,
|
||||
any_cb);
|
||||
node_services_interface_->add_service(
|
||||
std::dynamic_pointer_cast<rclcpp::service::ServiceBase>(srv_get_available_states_),
|
||||
nullptr);
|
||||
}
|
||||
|
||||
{ // get_available_transitions
|
||||
auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_get_available_transitions, this,
|
||||
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
|
||||
rclcpp::any_service_callback::AnyServiceCallback<GetAvailableTransitionsSrv> any_cb;
|
||||
any_cb.set(std::move(cb));
|
||||
|
||||
srv_get_available_transitions_ =
|
||||
std::make_shared<rclcpp::service::Service<GetAvailableTransitionsSrv>>(
|
||||
node_base_interface_->get_shared_rcl_node_handle(),
|
||||
&state_machine_.com_interface.srv_get_available_transitions,
|
||||
any_cb);
|
||||
node_services_interface_->add_service(
|
||||
std::dynamic_pointer_cast<rclcpp::service::ServiceBase>(srv_get_available_transitions_),
|
||||
nullptr);
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
register_callback(std::uint8_t lifecycle_transition, std::function<rcl_lifecycle_ret_t(
|
||||
const State &)> & cb)
|
||||
{
|
||||
cb_map_[lifecycle_transition] = cb;
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
on_change_state(const std::shared_ptr<rmw_request_id_t> header,
|
||||
const std::shared_ptr<ChangeStateSrv::Request> req,
|
||||
std::shared_ptr<ChangeStateSrv::Response> resp)
|
||||
{
|
||||
(void)header;
|
||||
if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
|
||||
throw std::runtime_error(
|
||||
"Can't get state. State machine is not initialized.");
|
||||
}
|
||||
resp->success = change_state(req->transition.id);
|
||||
}
|
||||
|
||||
void
|
||||
on_get_state(const std::shared_ptr<rmw_request_id_t> header,
|
||||
const std::shared_ptr<GetStateSrv::Request> req,
|
||||
std::shared_ptr<GetStateSrv::Response> resp)
|
||||
{
|
||||
(void)header;
|
||||
(void)req;
|
||||
if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
|
||||
throw std::runtime_error(
|
||||
"Can't get state. State machine is not initialized.");
|
||||
}
|
||||
resp->current_state.id = static_cast<uint8_t>(state_machine_.current_state->id);
|
||||
resp->current_state.label = state_machine_.current_state->label;
|
||||
}
|
||||
|
||||
void
|
||||
on_get_available_states(const std::shared_ptr<rmw_request_id_t> header,
|
||||
const std::shared_ptr<GetAvailableStatesSrv::Request> req,
|
||||
std::shared_ptr<GetAvailableStatesSrv::Response> resp)
|
||||
{
|
||||
(void)header;
|
||||
(void)req;
|
||||
if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
|
||||
throw std::runtime_error(
|
||||
"Can't get available states. State machine is not initialized.");
|
||||
}
|
||||
for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) {
|
||||
lifecycle_msgs::msg::State state;
|
||||
state.id = static_cast<uint8_t>(state_machine_.transition_map.states[i].id);
|
||||
state.label = static_cast<std::string>(state_machine_.transition_map.states[i].label);
|
||||
resp->available_states.push_back(state);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
on_get_available_transitions(const std::shared_ptr<rmw_request_id_t> header,
|
||||
const std::shared_ptr<GetAvailableTransitionsSrv::Request> req,
|
||||
std::shared_ptr<GetAvailableTransitionsSrv::Response> resp)
|
||||
{
|
||||
(void)header;
|
||||
(void)req;
|
||||
if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
|
||||
throw std::runtime_error(
|
||||
"Can't get available transitions. State machine is not initialized.");
|
||||
return;
|
||||
}
|
||||
|
||||
for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) {
|
||||
rcl_lifecycle_transition_t & rcl_transition = state_machine_.transition_map.transitions[i];
|
||||
lifecycle_msgs::msg::TransitionDescription trans_desc;
|
||||
trans_desc.transition.id = rcl_transition.id;
|
||||
trans_desc.transition.label = rcl_transition.label;
|
||||
trans_desc.start_state.id = rcl_transition.start->id;
|
||||
trans_desc.start_state.label = rcl_transition.start->label;
|
||||
trans_desc.goal_state.id = rcl_transition.goal->id;
|
||||
trans_desc.goal_state.label = rcl_transition.goal->label;
|
||||
resp->available_transitions.push_back(trans_desc);
|
||||
}
|
||||
}
|
||||
|
||||
const State &
|
||||
get_current_state()
|
||||
{
|
||||
current_state_ = State(state_machine_.current_state);
|
||||
return current_state_;
|
||||
}
|
||||
|
||||
std::vector<State>
|
||||
get_available_states()
|
||||
{
|
||||
std::vector<State> states;
|
||||
for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) {
|
||||
State state(&state_machine_.transition_map.states[i]);
|
||||
states.push_back(state);
|
||||
}
|
||||
return states;
|
||||
}
|
||||
|
||||
std::vector<Transition>
|
||||
get_available_transitions()
|
||||
{
|
||||
std::vector<Transition> transitions;
|
||||
|
||||
for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) {
|
||||
Transition transition(
|
||||
&state_machine_.transition_map.transitions[i]);
|
||||
transitions.push_back(transition);
|
||||
}
|
||||
return transitions;
|
||||
}
|
||||
|
||||
bool
|
||||
change_state(std::uint8_t lifecycle_transition)
|
||||
{
|
||||
if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
|
||||
fprintf(stderr, "%s:%d, Unable to change state for state machine for %s: %s \n",
|
||||
__FILE__, __LINE__, node_base_interface_->get_name(), rcl_get_error_string_safe());
|
||||
return false;
|
||||
}
|
||||
|
||||
// keep the initial state to pass to a transition callback
|
||||
State initial_state(state_machine_.current_state);
|
||||
|
||||
unsigned int transition_id = static_cast<unsigned int>(lifecycle_transition);
|
||||
if (rcl_lifecycle_trigger_transition(&state_machine_, transition_id, true) != RCL_RET_OK) {
|
||||
fprintf(stderr, "%s:%d, Unable to start transition %u from current state %s: %s\n",
|
||||
__FILE__, __LINE__, transition_id,
|
||||
state_machine_.current_state->label, rcl_get_error_string_safe());
|
||||
return false;
|
||||
}
|
||||
|
||||
rcl_lifecycle_ret_t cb_success = execute_callback(
|
||||
state_machine_.current_state->id, initial_state);
|
||||
|
||||
if (rcl_lifecycle_trigger_transition(
|
||||
&state_machine_, cb_success, true) != RCL_RET_OK)
|
||||
{
|
||||
fprintf(stderr, "Failed to finish transition %u. Current state is now: %s\n",
|
||||
transition_id, state_machine_.current_state->label);
|
||||
return false;
|
||||
}
|
||||
|
||||
// error handling ?!
|
||||
// TODO(karsten1987): iterate over possible ret value
|
||||
if (cb_success == RCL_LIFECYCLE_RET_ERROR) {
|
||||
rcl_lifecycle_ret_t error_resolved = execute_callback(state_machine_.current_state->id,
|
||||
initial_state);
|
||||
if (error_resolved == RCL_RET_OK) {
|
||||
fprintf(stderr, "Exception handling was successful\n");
|
||||
// We call cleanup on the error state
|
||||
rcl_lifecycle_trigger_transition(
|
||||
&state_machine_, error_resolved, true);
|
||||
fprintf(stderr, "current state after error callback%s\n",
|
||||
state_machine_.current_state->label);
|
||||
} else {
|
||||
// We call shutdown on the error state
|
||||
rcl_lifecycle_trigger_transition(
|
||||
&state_machine_, error_resolved, true);
|
||||
}
|
||||
}
|
||||
// This true holds in both cases where the actual callback
|
||||
// was successful or not, since at this point we have a valid transistion
|
||||
// to either a new primary state or error state
|
||||
return true;
|
||||
}
|
||||
|
||||
rcl_lifecycle_ret_t
|
||||
execute_callback(unsigned int cb_id, const State & previous_state)
|
||||
{
|
||||
// in case no callback was attached, we forward directly
|
||||
auto cb_success = RCL_LIFECYCLE_RET_OK;
|
||||
|
||||
auto it = cb_map_.find(cb_id);
|
||||
if (it != cb_map_.end()) {
|
||||
auto callback = it->second;
|
||||
try {
|
||||
cb_success = callback(State(previous_state));
|
||||
} catch (const std::exception &) {
|
||||
// TODO(karsten1987): Windows CI doens't let me print the msg here
|
||||
// the todo is to forward the exception to the on_error callback
|
||||
// fprintf(stderr, "Caught exception in callback for transition %d\n",
|
||||
// it->first);
|
||||
// fprintf(stderr, "Original error msg: %s\n", e.what());
|
||||
// maybe directly go for error handling here
|
||||
// and pass exception along with it
|
||||
cb_success = RCL_LIFECYCLE_RET_ERROR;
|
||||
}
|
||||
}
|
||||
return cb_success;
|
||||
}
|
||||
|
||||
const State &
|
||||
trigger_transition(unsigned int transition_id)
|
||||
{
|
||||
change_state(transition_id);
|
||||
return get_current_state();
|
||||
}
|
||||
|
||||
void
|
||||
add_publisher_handle(std::shared_ptr<rclcpp_lifecycle::LifecyclePublisherInterface> pub)
|
||||
{
|
||||
weak_pubs_.push_back(pub);
|
||||
}
|
||||
|
||||
void
|
||||
add_timer_handle(std::shared_ptr<rclcpp::timer::TimerBase> timer)
|
||||
{
|
||||
weak_timers_.push_back(timer);
|
||||
}
|
||||
|
||||
rcl_lifecycle_state_machine_t state_machine_;
|
||||
State current_state_;
|
||||
std::map<
|
||||
std::uint8_t,
|
||||
std::function<rcl_lifecycle_ret_t(const State &)>> cb_map_;
|
||||
|
||||
using NodeBasePtr = std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>;
|
||||
using NodeServicesPtr = std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface>;
|
||||
using ChangeStateSrvPtr = std::shared_ptr<rclcpp::service::Service<ChangeStateSrv>>;
|
||||
using GetStateSrvPtr = std::shared_ptr<rclcpp::service::Service<GetStateSrv>>;
|
||||
using GetAvailableStatesSrvPtr =
|
||||
std::shared_ptr<rclcpp::service::Service<GetAvailableStatesSrv>>;
|
||||
using GetAvailableTransitionsSrvPtr =
|
||||
std::shared_ptr<rclcpp::service::Service<GetAvailableTransitionsSrv>>;
|
||||
|
||||
NodeBasePtr node_base_interface_;
|
||||
NodeServicesPtr node_services_interface_;
|
||||
ChangeStateSrvPtr srv_change_state_;
|
||||
GetStateSrvPtr srv_get_state_;
|
||||
GetAvailableStatesSrvPtr srv_get_available_states_;
|
||||
GetAvailableTransitionsSrvPtr srv_get_available_transitions_;
|
||||
|
||||
// to controllable things
|
||||
std::vector<std::weak_ptr<rclcpp_lifecycle::LifecyclePublisherInterface>> weak_pubs_;
|
||||
std::vector<std::weak_ptr<rclcpp::timer::TimerBase>> weak_timers_;
|
||||
};
|
||||
|
||||
} // namespace rclcpp_lifecycle
|
||||
#endif // LIFECYCLE_NODE_INTERFACE_IMPL_HPP_
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
// Copyright 2016 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
|
||||
|
||||
namespace rclcpp_lifecycle
|
||||
{
|
||||
namespace node_interfaces
|
||||
{
|
||||
rcl_lifecycle_ret_t
|
||||
LifecycleNodeInterface::on_configure(const State &)
|
||||
{
|
||||
return RCL_LIFECYCLE_RET_OK;
|
||||
}
|
||||
|
||||
rcl_lifecycle_ret_t
|
||||
LifecycleNodeInterface::on_cleanup(const State &)
|
||||
{
|
||||
return RCL_LIFECYCLE_RET_OK;
|
||||
}
|
||||
|
||||
rcl_lifecycle_ret_t
|
||||
LifecycleNodeInterface::on_shutdown(const State &)
|
||||
{
|
||||
return RCL_LIFECYCLE_RET_OK;
|
||||
}
|
||||
|
||||
rcl_lifecycle_ret_t
|
||||
LifecycleNodeInterface::on_activate(const State &)
|
||||
{
|
||||
return RCL_LIFECYCLE_RET_OK;
|
||||
}
|
||||
|
||||
rcl_lifecycle_ret_t
|
||||
LifecycleNodeInterface::on_deactivate(const State &)
|
||||
{
|
||||
return RCL_LIFECYCLE_RET_OK;
|
||||
}
|
||||
|
||||
rcl_lifecycle_ret_t
|
||||
LifecycleNodeInterface::on_error(const State &)
|
||||
{
|
||||
return RCL_LIFECYCLE_RET_FAILURE;
|
||||
}
|
||||
|
||||
} // namespace node_interfaces
|
||||
} // namespace rclcpp_lifecycle
|
||||
68
rclcpp_lifecycle/src/state.cpp
Normal file
68
rclcpp_lifecycle/src/state.cpp
Normal file
|
|
@ -0,0 +1,68 @@
|
|||
// Copyright 2016 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "rclcpp_lifecycle/state.hpp"
|
||||
|
||||
#include <lifecycle_msgs/msg/state.hpp>
|
||||
#include <rcl_lifecycle/data_types.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace rclcpp_lifecycle
|
||||
{
|
||||
|
||||
State::State()
|
||||
: State(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, "unknown")
|
||||
{}
|
||||
|
||||
State::State(unsigned int id, const std::string & label)
|
||||
: owns_rcl_state_handle_(true)
|
||||
{
|
||||
if (label.empty()) {
|
||||
throw std::runtime_error("Lifecycle State cannot have an empty label.");
|
||||
}
|
||||
|
||||
auto state_handle = new rcl_lifecycle_state_t;
|
||||
state_handle->id = id;
|
||||
state_handle->label = label.c_str();
|
||||
|
||||
state_handle_ = state_handle;
|
||||
}
|
||||
|
||||
State::State(const rcl_lifecycle_state_t * rcl_lifecycle_state_handle)
|
||||
: owns_rcl_state_handle_(false)
|
||||
{
|
||||
state_handle_ = rcl_lifecycle_state_handle;
|
||||
}
|
||||
|
||||
State::~State()
|
||||
{
|
||||
if (owns_rcl_state_handle_) {
|
||||
delete state_handle_;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned int
|
||||
State::id() const
|
||||
{
|
||||
return state_handle_->id;
|
||||
}
|
||||
|
||||
std::string
|
||||
State::label() const
|
||||
{
|
||||
return state_handle_->label;
|
||||
}
|
||||
|
||||
} // namespace rclcpp_lifecycle
|
||||
106
rclcpp_lifecycle/src/transition.cpp
Normal file
106
rclcpp_lifecycle/src/transition.cpp
Normal file
|
|
@ -0,0 +1,106 @@
|
|||
// Copyright 2016 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "rclcpp_lifecycle/transition.hpp"
|
||||
|
||||
#include <lifecycle_msgs/msg/transition.hpp>
|
||||
#include <rcl_lifecycle/data_types.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace rclcpp_lifecycle
|
||||
{
|
||||
|
||||
Transition::Transition(unsigned int id, const std::string & label)
|
||||
: owns_rcl_transition_handle_(true)
|
||||
{
|
||||
auto transition_handle = new rcl_lifecycle_transition_t;
|
||||
transition_handle->id = id;
|
||||
transition_handle->label = label.c_str();
|
||||
|
||||
transition_handle->start = nullptr;
|
||||
transition_handle->goal = nullptr;
|
||||
transition_handle_ = transition_handle;
|
||||
}
|
||||
|
||||
Transition::Transition(
|
||||
unsigned int id, const std::string & label,
|
||||
State && start, State && goal)
|
||||
: owns_rcl_transition_handle_(true)
|
||||
{
|
||||
auto transition_handle = new rcl_lifecycle_transition_t;
|
||||
transition_handle->id = id;
|
||||
transition_handle->label = label.c_str();
|
||||
|
||||
auto start_state = new rcl_lifecycle_state_t;
|
||||
start_state->id = start.id();
|
||||
start_state->label = start.label().c_str();
|
||||
|
||||
auto goal_state = new rcl_lifecycle_state_t;
|
||||
goal_state->id = goal.id();
|
||||
goal_state->label = start.label().c_str();
|
||||
|
||||
transition_handle->start = start_state;
|
||||
transition_handle->goal = goal_state;
|
||||
transition_handle_ = transition_handle;
|
||||
}
|
||||
|
||||
Transition::Transition(const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle)
|
||||
: owns_rcl_transition_handle_(false)
|
||||
{
|
||||
transition_handle_ = rcl_lifecycle_transition_handle;
|
||||
}
|
||||
|
||||
Transition::~Transition()
|
||||
{
|
||||
if (owns_rcl_transition_handle_) {
|
||||
if (transition_handle_->start) {
|
||||
delete transition_handle_->start;
|
||||
}
|
||||
if (transition_handle_->goal) {
|
||||
delete transition_handle_->goal;
|
||||
}
|
||||
delete transition_handle_;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned int
|
||||
Transition::id() const
|
||||
{
|
||||
return transition_handle_->id;
|
||||
}
|
||||
|
||||
std::string
|
||||
Transition::label() const
|
||||
{
|
||||
return transition_handle_->label;
|
||||
}
|
||||
|
||||
State
|
||||
Transition::start_state() const
|
||||
{
|
||||
return State(
|
||||
transition_handle_->start->id,
|
||||
transition_handle_->start->label);
|
||||
}
|
||||
|
||||
State
|
||||
Transition::goal_state() const
|
||||
{
|
||||
return State(
|
||||
transition_handle_->goal->id,
|
||||
transition_handle_->goal->label);
|
||||
}
|
||||
|
||||
} // namespace rclcpp_lifecycle
|
||||
Loading…
Add table
Add a link
Reference in a new issue