service for transition graph (#555)
* service for transition graph * remove keys, transition id unique, label ambiguous * semicolon for macro call
This commit is contained in:
parent
bedb3ae361
commit
3353ffbb15
9 changed files with 250 additions and 168 deletions
|
@ -416,7 +416,7 @@ public:
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
trigger_transition(
|
trigger_transition(
|
||||||
const Transition & transition, rcl_lifecycle_transition_key_t & cb_return_code);
|
const Transition & transition, LifecycleNodeInterface::CallbackReturn & cb_return_code);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
|
@ -425,7 +425,7 @@ public:
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
trigger_transition(
|
trigger_transition(
|
||||||
uint8_t transition_id, rcl_lifecycle_transition_key_t & cb_return_code);
|
uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
|
@ -433,7 +433,7 @@ public:
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
configure(rcl_lifecycle_transition_key_t & cb_return_code);
|
configure(LifecycleNodeInterface::CallbackReturn & cb_return_code);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
|
@ -441,7 +441,7 @@ public:
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
cleanup(rcl_lifecycle_transition_key_t & cb_return_code);
|
cleanup(LifecycleNodeInterface::CallbackReturn & cb_return_code);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
|
@ -449,7 +449,7 @@ public:
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
activate(rcl_lifecycle_transition_key_t & cb_return_code);
|
activate(LifecycleNodeInterface::CallbackReturn & cb_return_code);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
|
@ -457,7 +457,7 @@ public:
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
deactivate(rcl_lifecycle_transition_key_t & cb_return_code);
|
deactivate(LifecycleNodeInterface::CallbackReturn & cb_return_code);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
|
@ -465,31 +465,31 @@ public:
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
shutdown(rcl_lifecycle_transition_key_t & cb_return_code);
|
shutdown(LifecycleNodeInterface::CallbackReturn & cb_return_code);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
bool
|
bool
|
||||||
register_on_configure(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
|
register_on_configure(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
bool
|
bool
|
||||||
register_on_cleanup(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
|
register_on_cleanup(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
bool
|
bool
|
||||||
register_on_shutdown(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
|
register_on_shutdown(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
bool
|
bool
|
||||||
register_on_activate(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
|
register_on_activate(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
bool
|
bool
|
||||||
register_on_deactivate(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
|
register_on_deactivate(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
bool
|
bool
|
||||||
register_on_error(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
|
register_on_error(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
|
|
|
@ -15,7 +15,9 @@
|
||||||
#ifndef RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_
|
#ifndef RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_
|
||||||
#define RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_
|
#define RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_
|
||||||
|
|
||||||
#include "rcl_lifecycle/data_types.h"
|
#include "lifecycle_msgs/msg/transition.hpp"
|
||||||
|
|
||||||
|
#include "rcl_lifecycle/rcl_lifecycle.h"
|
||||||
|
|
||||||
#include "rclcpp_lifecycle/state.hpp"
|
#include "rclcpp_lifecycle/state.hpp"
|
||||||
#include "rclcpp_lifecycle/visibility_control.h"
|
#include "rclcpp_lifecycle/visibility_control.h"
|
||||||
|
@ -44,12 +46,19 @@ protected:
|
||||||
LifecycleNodeInterface() {}
|
LifecycleNodeInterface() {}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
enum class CallbackReturn : uint8_t
|
||||||
|
{
|
||||||
|
SUCCESS = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS,
|
||||||
|
FAILURE = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE,
|
||||||
|
ERROR = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR
|
||||||
|
};
|
||||||
|
|
||||||
/// Callback function for configure transition
|
/// Callback function for configure transition
|
||||||
/*
|
/*
|
||||||
* \return true by default
|
* \return true by default
|
||||||
*/
|
*/
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
virtual rcl_lifecycle_transition_key_t
|
virtual CallbackReturn
|
||||||
on_configure(const State & previous_state);
|
on_configure(const State & previous_state);
|
||||||
|
|
||||||
/// Callback function for cleanup transition
|
/// Callback function for cleanup transition
|
||||||
|
@ -57,7 +66,7 @@ public:
|
||||||
* \return true by default
|
* \return true by default
|
||||||
*/
|
*/
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
virtual rcl_lifecycle_transition_key_t
|
virtual CallbackReturn
|
||||||
on_cleanup(const State & previous_state);
|
on_cleanup(const State & previous_state);
|
||||||
|
|
||||||
/// Callback function for shutdown transition
|
/// Callback function for shutdown transition
|
||||||
|
@ -65,7 +74,7 @@ public:
|
||||||
* \return true by default
|
* \return true by default
|
||||||
*/
|
*/
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
virtual rcl_lifecycle_transition_key_t
|
virtual CallbackReturn
|
||||||
on_shutdown(const State & previous_state);
|
on_shutdown(const State & previous_state);
|
||||||
|
|
||||||
/// Callback function for activate transition
|
/// Callback function for activate transition
|
||||||
|
@ -73,7 +82,7 @@ public:
|
||||||
* \return true by default
|
* \return true by default
|
||||||
*/
|
*/
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
virtual rcl_lifecycle_transition_key_t
|
virtual CallbackReturn
|
||||||
on_activate(const State & previous_state);
|
on_activate(const State & previous_state);
|
||||||
|
|
||||||
/// Callback function for deactivate transition
|
/// Callback function for deactivate transition
|
||||||
|
@ -81,7 +90,7 @@ public:
|
||||||
* \return true by default
|
* \return true by default
|
||||||
*/
|
*/
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
virtual rcl_lifecycle_transition_key_t
|
virtual CallbackReturn
|
||||||
on_deactivate(const State & previous_state);
|
on_deactivate(const State & previous_state);
|
||||||
|
|
||||||
/// Callback function for errorneous transition
|
/// Callback function for errorneous transition
|
||||||
|
@ -89,7 +98,7 @@ public:
|
||||||
* \return false by default
|
* \return false by default
|
||||||
*/
|
*/
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
virtual rcl_lifecycle_transition_key_t
|
virtual CallbackReturn
|
||||||
on_error(const State & previous_state);
|
on_error(const State & previous_state);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -301,7 +301,7 @@ LifecycleNode::get_node_parameters_interface()
|
||||||
////
|
////
|
||||||
bool
|
bool
|
||||||
LifecycleNode::register_on_configure(
|
LifecycleNode::register_on_configure(
|
||||||
std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
|
std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn)
|
||||||
{
|
{
|
||||||
return impl_->register_callback(
|
return impl_->register_callback(
|
||||||
lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn);
|
lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn);
|
||||||
|
@ -309,7 +309,7 @@ LifecycleNode::register_on_configure(
|
||||||
|
|
||||||
bool
|
bool
|
||||||
LifecycleNode::register_on_cleanup(
|
LifecycleNode::register_on_cleanup(
|
||||||
std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
|
std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn)
|
||||||
{
|
{
|
||||||
return impl_->register_callback(
|
return impl_->register_callback(
|
||||||
lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn);
|
lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn);
|
||||||
|
@ -317,7 +317,7 @@ LifecycleNode::register_on_cleanup(
|
||||||
|
|
||||||
bool
|
bool
|
||||||
LifecycleNode::register_on_shutdown(
|
LifecycleNode::register_on_shutdown(
|
||||||
std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
|
std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn)
|
||||||
{
|
{
|
||||||
return impl_->register_callback(
|
return impl_->register_callback(
|
||||||
lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn);
|
lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn);
|
||||||
|
@ -325,7 +325,7 @@ LifecycleNode::register_on_shutdown(
|
||||||
|
|
||||||
bool
|
bool
|
||||||
LifecycleNode::register_on_activate(
|
LifecycleNode::register_on_activate(
|
||||||
std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
|
std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn)
|
||||||
{
|
{
|
||||||
return impl_->register_callback(
|
return impl_->register_callback(
|
||||||
lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn);
|
lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn);
|
||||||
|
@ -333,7 +333,7 @@ LifecycleNode::register_on_activate(
|
||||||
|
|
||||||
bool
|
bool
|
||||||
LifecycleNode::register_on_deactivate(
|
LifecycleNode::register_on_deactivate(
|
||||||
std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
|
std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn)
|
||||||
{
|
{
|
||||||
return impl_->register_callback(
|
return impl_->register_callback(
|
||||||
lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn);
|
lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn);
|
||||||
|
@ -341,7 +341,7 @@ LifecycleNode::register_on_deactivate(
|
||||||
|
|
||||||
bool
|
bool
|
||||||
LifecycleNode::register_on_error(
|
LifecycleNode::register_on_error(
|
||||||
std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
|
std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn)
|
||||||
{
|
{
|
||||||
return impl_->register_callback(
|
return impl_->register_callback(
|
||||||
lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING, fcn);
|
lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING, fcn);
|
||||||
|
@ -373,7 +373,7 @@ LifecycleNode::trigger_transition(const Transition & transition)
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
LifecycleNode::trigger_transition(
|
LifecycleNode::trigger_transition(
|
||||||
const Transition & transition, rcl_lifecycle_transition_key_t & cb_return_code)
|
const Transition & transition, LifecycleNodeInterface::CallbackReturn & cb_return_code)
|
||||||
{
|
{
|
||||||
return trigger_transition(transition.id(), cb_return_code);
|
return trigger_transition(transition.id(), cb_return_code);
|
||||||
}
|
}
|
||||||
|
@ -386,7 +386,7 @@ LifecycleNode::trigger_transition(uint8_t transition_id)
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
LifecycleNode::trigger_transition(
|
LifecycleNode::trigger_transition(
|
||||||
uint8_t transition_id, rcl_lifecycle_transition_key_t & cb_return_code)
|
uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code)
|
||||||
{
|
{
|
||||||
return impl_->trigger_transition(transition_id, cb_return_code);
|
return impl_->trigger_transition(transition_id, cb_return_code);
|
||||||
}
|
}
|
||||||
|
@ -399,7 +399,7 @@ LifecycleNode::configure()
|
||||||
}
|
}
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
LifecycleNode::configure(rcl_lifecycle_transition_key_t & cb_return_code)
|
LifecycleNode::configure(LifecycleNodeInterface::CallbackReturn & cb_return_code)
|
||||||
{
|
{
|
||||||
return impl_->trigger_transition(
|
return impl_->trigger_transition(
|
||||||
lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, cb_return_code);
|
lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, cb_return_code);
|
||||||
|
@ -413,7 +413,7 @@ LifecycleNode::cleanup()
|
||||||
}
|
}
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
LifecycleNode::cleanup(rcl_lifecycle_transition_key_t & cb_return_code)
|
LifecycleNode::cleanup(LifecycleNodeInterface::CallbackReturn & cb_return_code)
|
||||||
{
|
{
|
||||||
return impl_->trigger_transition(
|
return impl_->trigger_transition(
|
||||||
lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, cb_return_code);
|
lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, cb_return_code);
|
||||||
|
@ -427,7 +427,7 @@ LifecycleNode::activate()
|
||||||
}
|
}
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
LifecycleNode::activate(rcl_lifecycle_transition_key_t & cb_return_code)
|
LifecycleNode::activate(LifecycleNodeInterface::CallbackReturn & cb_return_code)
|
||||||
{
|
{
|
||||||
return impl_->trigger_transition(
|
return impl_->trigger_transition(
|
||||||
lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, cb_return_code);
|
lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, cb_return_code);
|
||||||
|
@ -441,7 +441,7 @@ LifecycleNode::deactivate()
|
||||||
}
|
}
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
LifecycleNode::deactivate(rcl_lifecycle_transition_key_t & cb_return_code)
|
LifecycleNode::deactivate(LifecycleNodeInterface::CallbackReturn & cb_return_code)
|
||||||
{
|
{
|
||||||
return impl_->trigger_transition(
|
return impl_->trigger_transition(
|
||||||
lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, cb_return_code);
|
lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, cb_return_code);
|
||||||
|
@ -451,14 +451,14 @@ const State &
|
||||||
LifecycleNode::shutdown()
|
LifecycleNode::shutdown()
|
||||||
{
|
{
|
||||||
return impl_->trigger_transition(
|
return impl_->trigger_transition(
|
||||||
lifecycle_msgs::msg::Transition::TRANSITION_SHUTDOWN);
|
rcl_lifecycle_shutdown_label);
|
||||||
}
|
}
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
LifecycleNode::shutdown(rcl_lifecycle_transition_key_t & cb_return_code)
|
LifecycleNode::shutdown(LifecycleNodeInterface::CallbackReturn & cb_return_code)
|
||||||
{
|
{
|
||||||
return impl_->trigger_transition(
|
return impl_->trigger_transition(
|
||||||
lifecycle_msgs::msg::Transition::TRANSITION_SHUTDOWN, cb_return_code);
|
rcl_lifecycle_shutdown_label, cb_return_code);
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
|
|
@ -95,6 +95,7 @@ public:
|
||||||
rosidl_typesupport_cpp::get_service_type_support_handle<GetStateSrv>(),
|
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<GetAvailableStatesSrv>(),
|
||||||
rosidl_typesupport_cpp::get_service_type_support_handle<GetAvailableTransitionsSrv>(),
|
rosidl_typesupport_cpp::get_service_type_support_handle<GetAvailableTransitionsSrv>(),
|
||||||
|
rosidl_typesupport_cpp::get_service_type_support_handle<GetAvailableTransitionsSrv>(),
|
||||||
true,
|
true,
|
||||||
&node_options->allocator);
|
&node_options->allocator);
|
||||||
if (ret != RCL_RET_OK) {
|
if (ret != RCL_RET_OK) {
|
||||||
|
@ -163,12 +164,28 @@ public:
|
||||||
std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_available_transitions_),
|
std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_available_transitions_),
|
||||||
nullptr);
|
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<GetAvailableTransitionsSrv> any_cb;
|
||||||
|
any_cb.set(std::move(cb));
|
||||||
|
|
||||||
|
srv_get_transition_graph_ =
|
||||||
|
std::make_shared<rclcpp::Service<GetAvailableTransitionsSrv>>(
|
||||||
|
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<rclcpp::ServiceBase>(srv_get_transition_graph_),
|
||||||
|
nullptr);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
register_callback(
|
register_callback(
|
||||||
std::uint8_t lifecycle_transition, std::function<rcl_lifecycle_transition_key_t(
|
std::uint8_t lifecycle_transition,
|
||||||
const State &)> & cb)
|
std::function<node_interfaces::LifecycleNodeInterface::CallbackReturn(const State &)> & cb)
|
||||||
{
|
{
|
||||||
cb_map_[lifecycle_transition] = cb;
|
cb_map_[lifecycle_transition] = cb;
|
||||||
return true;
|
return true;
|
||||||
|
@ -185,14 +202,14 @@ public:
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
"Can't get state. State machine is not initialized.");
|
"Can't get state. State machine is not initialized.");
|
||||||
}
|
}
|
||||||
rcl_lifecycle_transition_key_t cb_return_code;
|
node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code;
|
||||||
auto ret = change_state(req->transition.id, cb_return_code);
|
auto ret = change_state(req->transition.id, cb_return_code);
|
||||||
(void) ret;
|
(void) ret;
|
||||||
// TODO(karsten1987): Lifecycle msgs have to be extended to keep both returns
|
// TODO(karsten1987): Lifecycle msgs have to be extended to keep both returns
|
||||||
// 1. return is the actual transition
|
// 1. return is the actual transition
|
||||||
// 2. return is whether an error occurred or not
|
// 2. return is whether an error occurred or not
|
||||||
resp->success =
|
resp->success =
|
||||||
(cb_return_code == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS);
|
(cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS);
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -244,8 +261,34 @@ public:
|
||||||
"Can't get available transitions. State machine is not initialized.");
|
"Can't get available transitions. State machine is not initialized.");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
for (uint8_t i = 0; i < state_machine_.current_state->valid_transition_size; ++i) {
|
||||||
|
auto rcl_transition = state_machine_.current_state->valid_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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
on_get_transition_graph(
|
||||||
|
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.");
|
||||||
|
}
|
||||||
|
|
||||||
for (uint8_t i = 0; i < state_machine_.transition_map.transitions_size; ++i) {
|
for (uint8_t i = 0; i < state_machine_.transition_map.transitions_size; ++i) {
|
||||||
rcl_lifecycle_transition_t & rcl_transition = state_machine_.transition_map.transitions[i];
|
auto rcl_transition = state_machine_.transition_map.transitions[i];
|
||||||
lifecycle_msgs::msg::TransitionDescription trans_desc;
|
lifecycle_msgs::msg::TransitionDescription trans_desc;
|
||||||
trans_desc.transition.id = rcl_transition.id;
|
trans_desc.transition.id = rcl_transition.id;
|
||||||
trans_desc.transition.label = rcl_transition.label;
|
trans_desc.transition.label = rcl_transition.label;
|
||||||
|
@ -289,7 +332,7 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_ret_t
|
rcl_ret_t
|
||||||
change_state(std::uint8_t lifecycle_transition, rcl_lifecycle_transition_key_t & cb_return_code)
|
change_state(std::uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code)
|
||||||
{
|
{
|
||||||
if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
|
if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR("Unable to change state for state machine for %s: %s",
|
RCUTILS_LOG_ERROR("Unable to change state for state machine for %s: %s",
|
||||||
|
@ -301,8 +344,7 @@ public:
|
||||||
// keep the initial state to pass to a transition callback
|
// keep the initial state to pass to a transition callback
|
||||||
State initial_state(state_machine_.current_state);
|
State initial_state(state_machine_.current_state);
|
||||||
|
|
||||||
uint8_t transition_id = lifecycle_transition;
|
if (rcl_lifecycle_trigger_transition_by_id(
|
||||||
if (rcl_lifecycle_trigger_transition(
|
|
||||||
&state_machine_, transition_id, publish_update) != RCL_RET_OK)
|
&state_machine_, transition_id, publish_update) != RCL_RET_OK)
|
||||||
{
|
{
|
||||||
RCUTILS_LOG_ERROR("Unable to start transition %u from current state %s: %s",
|
RCUTILS_LOG_ERROR("Unable to start transition %u from current state %s: %s",
|
||||||
|
@ -310,11 +352,22 @@ public:
|
||||||
return RCL_RET_ERROR;
|
return RCL_RET_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
cb_return_code = execute_callback(
|
auto get_label_for_return_code =
|
||||||
state_machine_.current_state->id, initial_state);
|
[](node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) -> const char *{
|
||||||
|
auto cb_id = static_cast<uint8_t>(cb_return_code);
|
||||||
|
if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) {
|
||||||
|
return rcl_lifecycle_transition_success_label;
|
||||||
|
} else if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE) {
|
||||||
|
return rcl_lifecycle_transition_failure_label;
|
||||||
|
}
|
||||||
|
return rcl_lifecycle_transition_error_label;
|
||||||
|
};
|
||||||
|
|
||||||
if (rcl_lifecycle_trigger_transition(
|
cb_return_code = execute_callback(state_machine_.current_state->id, initial_state);
|
||||||
&state_machine_, cb_return_code, publish_update) != RCL_RET_OK)
|
auto transition_label = get_label_for_return_code(cb_return_code);
|
||||||
|
|
||||||
|
if (rcl_lifecycle_trigger_transition_by_label(
|
||||||
|
&state_machine_, transition_label, publish_update) != RCL_RET_OK)
|
||||||
{
|
{
|
||||||
RCUTILS_LOG_ERROR("Failed to finish transition %u. Current state is now: %s",
|
RCUTILS_LOG_ERROR("Failed to finish transition %u. Current state is now: %s",
|
||||||
transition_id, state_machine_.current_state->label);
|
transition_id, state_machine_.current_state->label);
|
||||||
|
@ -323,26 +376,16 @@ public:
|
||||||
|
|
||||||
// error handling ?!
|
// error handling ?!
|
||||||
// TODO(karsten1987): iterate over possible ret value
|
// TODO(karsten1987): iterate over possible ret value
|
||||||
if (cb_return_code == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR) {
|
if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) {
|
||||||
RCUTILS_LOG_WARN("Error occurred while doing error handling.");
|
RCUTILS_LOG_WARN("Error occurred while doing error handling.");
|
||||||
rcl_lifecycle_transition_key_t error_resolved = execute_callback(
|
|
||||||
state_machine_.current_state->id, initial_state);
|
auto error_cb_code = execute_callback(state_machine_.current_state->id, initial_state);
|
||||||
if (error_resolved == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) {
|
auto error_cb_label = get_label_for_return_code(error_cb_code);
|
||||||
// We call cleanup on the error state
|
if (rcl_lifecycle_trigger_transition_by_label(
|
||||||
if (rcl_lifecycle_trigger_transition(
|
&state_machine_, error_cb_label, publish_update) != RCL_RET_OK)
|
||||||
&state_machine_, error_resolved, publish_update) != RCL_RET_OK)
|
{
|
||||||
{
|
RCUTILS_LOG_ERROR("Failed to call cleanup on error state");
|
||||||
RCUTILS_LOG_ERROR("Failed to call cleanup on error state");
|
return RCL_RET_ERROR;
|
||||||
return RCL_RET_ERROR;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
// We call shutdown on the error state
|
|
||||||
if (rcl_lifecycle_trigger_transition(
|
|
||||||
&state_machine_, error_resolved, publish_update) != RCL_RET_OK)
|
|
||||||
{
|
|
||||||
RCUTILS_LOG_ERROR("Failed to call cleanup on error state");
|
|
||||||
return RCL_RET_ERROR;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// This true holds in both cases where the actual callback
|
// This true holds in both cases where the actual callback
|
||||||
|
@ -351,12 +394,11 @@ public:
|
||||||
return RCL_RET_OK;
|
return RCL_RET_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
LifecycleNodeInterface::CallbackReturn
|
||||||
execute_callback(unsigned int cb_id, const State & previous_state)
|
execute_callback(unsigned int cb_id, const State & previous_state)
|
||||||
{
|
{
|
||||||
// in case no callback was attached, we forward directly
|
// in case no callback was attached, we forward directly
|
||||||
rcl_lifecycle_transition_key_t cb_success =
|
auto cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
|
||||||
|
|
||||||
auto it = cb_map_.find(cb_id);
|
auto it = cb_map_.find(cb_id);
|
||||||
if (it != cb_map_.end()) {
|
if (it != cb_map_.end()) {
|
||||||
|
@ -371,23 +413,40 @@ public:
|
||||||
// RCUTILS_LOG_ERROR("Original error msg: %s\n", e.what());
|
// RCUTILS_LOG_ERROR("Original error msg: %s\n", e.what());
|
||||||
// maybe directly go for error handling here
|
// maybe directly go for error handling here
|
||||||
// and pass exception along with it
|
// and pass exception along with it
|
||||||
cb_success = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
|
cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return cb_success;
|
return cb_success;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
const State & trigger_transition(const char * transition_label)
|
||||||
|
{
|
||||||
|
LifecycleNodeInterface::CallbackReturn error;
|
||||||
|
return trigger_transition(transition_label, error);
|
||||||
|
}
|
||||||
|
|
||||||
|
const State & trigger_transition(
|
||||||
|
const char * transition_label, LifecycleNodeInterface::CallbackReturn & cb_return_code)
|
||||||
|
{
|
||||||
|
auto transition =
|
||||||
|
rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label);
|
||||||
|
if (transition) {
|
||||||
|
change_state(transition->id, cb_return_code);
|
||||||
|
}
|
||||||
|
return get_current_state();
|
||||||
|
}
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
trigger_transition(uint8_t transition_id)
|
trigger_transition(uint8_t transition_id)
|
||||||
{
|
{
|
||||||
rcl_lifecycle_transition_key_t error;
|
LifecycleNodeInterface::CallbackReturn error;
|
||||||
change_state(transition_id, error);
|
change_state(transition_id, error);
|
||||||
(void) error;
|
(void) error;
|
||||||
return get_current_state();
|
return get_current_state();
|
||||||
}
|
}
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
trigger_transition(uint8_t transition_id, rcl_lifecycle_transition_key_t & cb_return_code)
|
trigger_transition(uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code)
|
||||||
{
|
{
|
||||||
change_state(transition_id, cb_return_code);
|
change_state(transition_id, cb_return_code);
|
||||||
return get_current_state();
|
return get_current_state();
|
||||||
|
@ -409,7 +468,7 @@ public:
|
||||||
State current_state_;
|
State current_state_;
|
||||||
std::map<
|
std::map<
|
||||||
std::uint8_t,
|
std::uint8_t,
|
||||||
std::function<rcl_lifecycle_transition_key_t(const State &)>> cb_map_;
|
std::function<LifecycleNodeInterface::CallbackReturn(const State &)>> cb_map_;
|
||||||
|
|
||||||
using NodeBasePtr = std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>;
|
using NodeBasePtr = std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>;
|
||||||
using NodeServicesPtr = std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface>;
|
using NodeServicesPtr = std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface>;
|
||||||
|
@ -419,6 +478,8 @@ public:
|
||||||
std::shared_ptr<rclcpp::Service<GetAvailableStatesSrv>>;
|
std::shared_ptr<rclcpp::Service<GetAvailableStatesSrv>>;
|
||||||
using GetAvailableTransitionsSrvPtr =
|
using GetAvailableTransitionsSrvPtr =
|
||||||
std::shared_ptr<rclcpp::Service<GetAvailableTransitionsSrv>>;
|
std::shared_ptr<rclcpp::Service<GetAvailableTransitionsSrv>>;
|
||||||
|
using GetTransitionGraphSrvPtr =
|
||||||
|
std::shared_ptr<rclcpp::Service<GetAvailableTransitionsSrv>>;
|
||||||
|
|
||||||
NodeBasePtr node_base_interface_;
|
NodeBasePtr node_base_interface_;
|
||||||
NodeServicesPtr node_services_interface_;
|
NodeServicesPtr node_services_interface_;
|
||||||
|
@ -426,6 +487,7 @@ public:
|
||||||
GetStateSrvPtr srv_get_state_;
|
GetStateSrvPtr srv_get_state_;
|
||||||
GetAvailableStatesSrvPtr srv_get_available_states_;
|
GetAvailableStatesSrvPtr srv_get_available_states_;
|
||||||
GetAvailableTransitionsSrvPtr srv_get_available_transitions_;
|
GetAvailableTransitionsSrvPtr srv_get_available_transitions_;
|
||||||
|
GetTransitionGraphSrvPtr srv_get_transition_graph_;
|
||||||
|
|
||||||
// to controllable things
|
// to controllable things
|
||||||
std::vector<std::weak_ptr<rclcpp_lifecycle::LifecyclePublisherInterface>> weak_pubs_;
|
std::vector<std::weak_ptr<rclcpp_lifecycle::LifecyclePublisherInterface>> weak_pubs_;
|
||||||
|
|
|
@ -12,48 +12,49 @@
|
||||||
// See the License for the specific language governing permissions and
|
// See the License for the specific language governing permissions and
|
||||||
// limitations under the License.
|
// limitations under the License.
|
||||||
|
|
||||||
#include "lifecycle_msgs/msg/transition.hpp"
|
|
||||||
|
|
||||||
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
|
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
|
||||||
|
|
||||||
|
#include "rcl_lifecycle/rcl_lifecycle.h"
|
||||||
|
|
||||||
namespace rclcpp_lifecycle
|
namespace rclcpp_lifecycle
|
||||||
{
|
{
|
||||||
namespace node_interfaces
|
namespace node_interfaces
|
||||||
{
|
{
|
||||||
rcl_lifecycle_transition_key_t
|
|
||||||
|
LifecycleNodeInterface::CallbackReturn
|
||||||
LifecycleNodeInterface::on_configure(const State &)
|
LifecycleNodeInterface::on_configure(const State &)
|
||||||
{
|
{
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
LifecycleNodeInterface::CallbackReturn
|
||||||
LifecycleNodeInterface::on_cleanup(const State &)
|
LifecycleNodeInterface::on_cleanup(const State &)
|
||||||
{
|
{
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
LifecycleNodeInterface::CallbackReturn
|
||||||
LifecycleNodeInterface::on_shutdown(const State &)
|
LifecycleNodeInterface::on_shutdown(const State &)
|
||||||
{
|
{
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
LifecycleNodeInterface::CallbackReturn
|
||||||
LifecycleNodeInterface::on_activate(const State &)
|
LifecycleNodeInterface::on_activate(const State &)
|
||||||
{
|
{
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
LifecycleNodeInterface::CallbackReturn
|
||||||
LifecycleNodeInterface::on_deactivate(const State &)
|
LifecycleNodeInterface::on_deactivate(const State &)
|
||||||
{
|
{
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
LifecycleNodeInterface::CallbackReturn
|
||||||
LifecycleNodeInterface::on_error(const State &)
|
LifecycleNodeInterface::on_error(const State &)
|
||||||
{
|
{
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE;
|
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace node_interfaces
|
} // namespace node_interfaces
|
||||||
|
|
|
@ -46,18 +46,18 @@ public:
|
||||||
size_t number_of_callbacks = 0;
|
size_t number_of_callbacks = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_configure(const rclcpp_lifecycle::State &)
|
on_configure(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
throw std::runtime_error("custom exception raised in configure callback");
|
throw std::runtime_error("custom exception raised in configure callback");
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_error(const rclcpp_lifecycle::State &)
|
on_error(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -68,16 +68,17 @@ TEST_F(TestCallbackExceptions, positive_on_error) {
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition(
|
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition(
|
||||||
rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
|
rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
|
||||||
// check if all callbacks were successfully overwritten
|
// check if all callbacks were successfully overwritten
|
||||||
EXPECT_EQ(static_cast<size_t>(2), test_node->number_of_callbacks);
|
EXPECT_EQ(2u, test_node->number_of_callbacks);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(TestCallbackExceptions, positive_on_error_with_code) {
|
TEST_F(TestCallbackExceptions, positive_on_error_with_code) {
|
||||||
auto test_node = std::make_shared<PositiveCallbackExceptionNode>("testnode");
|
auto test_node = std::make_shared<PositiveCallbackExceptionNode>("testnode");
|
||||||
|
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
|
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
|
||||||
rcl_lifecycle_transition_key_t ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ret =
|
||||||
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
test_node->configure(ret);
|
test_node->configure(ret);
|
||||||
EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR, ret);
|
EXPECT_EQ(rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR, ret);
|
||||||
}
|
}
|
||||||
|
|
||||||
class NegativeCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode
|
class NegativeCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode
|
||||||
|
@ -90,18 +91,18 @@ public:
|
||||||
size_t number_of_callbacks = 0;
|
size_t number_of_callbacks = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_configure(const rclcpp_lifecycle::State &)
|
on_configure(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
throw std::runtime_error("custom exception raised in configure callback");
|
throw std::runtime_error("custom exception raised in configure callback");
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_error(const rclcpp_lifecycle::State &)
|
on_error(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE;
|
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -112,14 +113,14 @@ TEST_F(TestCallbackExceptions, negative_on_error) {
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition(
|
EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition(
|
||||||
rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
|
rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
|
||||||
// check if all callbacks were successfully overwritten
|
// check if all callbacks were successfully overwritten
|
||||||
EXPECT_EQ(static_cast<size_t>(2), test_node->number_of_callbacks);
|
EXPECT_EQ(2u, test_node->number_of_callbacks);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(TestCallbackExceptions, negative_on_error_with_code) {
|
TEST_F(TestCallbackExceptions, negative_on_error_with_code) {
|
||||||
auto test_node = std::make_shared<NegativeCallbackExceptionNode>("testnode");
|
auto test_node = std::make_shared<NegativeCallbackExceptionNode>("testnode");
|
||||||
|
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
|
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
|
||||||
rcl_lifecycle_transition_key_t ret = RCL_RET_OK;
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ret;
|
||||||
test_node->configure(ret);
|
test_node->configure(ret);
|
||||||
EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR, ret);
|
EXPECT_EQ(rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR, ret);
|
||||||
}
|
}
|
||||||
|
|
|
@ -27,17 +27,6 @@
|
||||||
using lifecycle_msgs::msg::State;
|
using lifecycle_msgs::msg::State;
|
||||||
using lifecycle_msgs::msg::Transition;
|
using lifecycle_msgs::msg::Transition;
|
||||||
|
|
||||||
struct GoodMood
|
|
||||||
{
|
|
||||||
static constexpr rcl_lifecycle_transition_key_t cb_ret =
|
|
||||||
lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
|
||||||
};
|
|
||||||
struct BadMood
|
|
||||||
{
|
|
||||||
static constexpr rcl_lifecycle_transition_key_t cb_ret =
|
|
||||||
lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE;
|
|
||||||
};
|
|
||||||
|
|
||||||
class TestDefaultStateMachine : public ::testing::Test
|
class TestDefaultStateMachine : public ::testing::Test
|
||||||
{
|
{
|
||||||
protected:
|
protected:
|
||||||
|
@ -55,6 +44,21 @@ public:
|
||||||
{}
|
{}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
struct GoodMood
|
||||||
|
{
|
||||||
|
using CallbackReturnT =
|
||||||
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
|
||||||
|
static constexpr CallbackReturnT cb_ret = static_cast<CallbackReturnT>(
|
||||||
|
lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS);
|
||||||
|
};
|
||||||
|
struct BadMood
|
||||||
|
{
|
||||||
|
using CallbackReturnT =
|
||||||
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
|
||||||
|
static constexpr CallbackReturnT cb_ret = static_cast<CallbackReturnT>(
|
||||||
|
lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE);
|
||||||
|
};
|
||||||
|
|
||||||
template<class Mood = GoodMood>
|
template<class Mood = GoodMood>
|
||||||
class MoodyLifecycleNode : public rclcpp_lifecycle::LifecycleNode
|
class MoodyLifecycleNode : public rclcpp_lifecycle::LifecycleNode
|
||||||
{
|
{
|
||||||
|
@ -66,7 +70,7 @@ public:
|
||||||
size_t number_of_callbacks = 0;
|
size_t number_of_callbacks = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_configure(const rclcpp_lifecycle::State &)
|
on_configure(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id());
|
EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id());
|
||||||
|
@ -74,7 +78,7 @@ protected:
|
||||||
return Mood::cb_ret;
|
return Mood::cb_ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_activate(const rclcpp_lifecycle::State &)
|
on_activate(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id());
|
EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id());
|
||||||
|
@ -82,7 +86,7 @@ protected:
|
||||||
return Mood::cb_ret;
|
return Mood::cb_ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_deactivate(const rclcpp_lifecycle::State &)
|
on_deactivate(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id());
|
EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id());
|
||||||
|
@ -90,7 +94,7 @@ protected:
|
||||||
return Mood::cb_ret;
|
return Mood::cb_ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_cleanup(const rclcpp_lifecycle::State &)
|
on_cleanup(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id());
|
EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id());
|
||||||
|
@ -98,7 +102,7 @@ protected:
|
||||||
return Mood::cb_ret;
|
return Mood::cb_ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_shutdown(const rclcpp_lifecycle::State &)
|
on_shutdown(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id());
|
EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id());
|
||||||
|
@ -106,25 +110,26 @@ protected:
|
||||||
return Mood::cb_ret;
|
return Mood::cb_ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_error(const rclcpp_lifecycle::State &);
|
on_error(const rclcpp_lifecycle::State &);
|
||||||
};
|
};
|
||||||
|
|
||||||
template<>
|
template<>
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
MoodyLifecycleNode<GoodMood>::on_error(const rclcpp_lifecycle::State &)
|
MoodyLifecycleNode<GoodMood>::on_error(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id());
|
EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id());
|
||||||
ADD_FAILURE();
|
ADD_FAILURE();
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
|
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<>
|
template<>
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
MoodyLifecycleNode<BadMood>::on_error(const rclcpp_lifecycle::State &)
|
MoodyLifecycleNode<BadMood>::on_error(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id());
|
EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id());
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(TestDefaultStateMachine, empty_initializer) {
|
TEST_F(TestDefaultStateMachine, empty_initializer) {
|
||||||
|
@ -138,40 +143,44 @@ TEST_F(TestDefaultStateMachine, trigger_transition) {
|
||||||
auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
|
auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
|
||||||
|
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
|
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(
|
ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(
|
||||||
rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
|
rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition(
|
ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition(
|
||||||
rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id());
|
rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id());
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(
|
ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(
|
||||||
rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id());
|
rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id());
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition(
|
ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition(
|
||||||
rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id());
|
rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id());
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition(
|
ASSERT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition(
|
||||||
rclcpp_lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id());
|
rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id());
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(TestDefaultStateMachine, trigger_transition_with_error_code) {
|
TEST_F(TestDefaultStateMachine, trigger_transition_with_error_code) {
|
||||||
auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
|
auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
|
auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
|
auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
|
||||||
|
auto ret = reset_key;
|
||||||
|
|
||||||
test_node->configure(ret);
|
test_node->configure(ret);
|
||||||
EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret);
|
|
||||||
ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
|
EXPECT_EQ(success, ret);
|
||||||
|
ret = reset_key;
|
||||||
|
|
||||||
test_node->activate(ret);
|
test_node->activate(ret);
|
||||||
EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret);
|
EXPECT_EQ(success, ret);
|
||||||
ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
|
ret = reset_key;
|
||||||
|
|
||||||
test_node->deactivate(ret);
|
test_node->deactivate(ret);
|
||||||
EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret);
|
EXPECT_EQ(success, ret);
|
||||||
ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
|
ret = reset_key;
|
||||||
|
|
||||||
test_node->cleanup(ret);
|
test_node->cleanup(ret);
|
||||||
EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret);
|
EXPECT_EQ(success, ret);
|
||||||
ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
|
ret = reset_key;
|
||||||
|
|
||||||
test_node->shutdown(ret);
|
test_node->shutdown(ret);
|
||||||
EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret);
|
EXPECT_EQ(success, ret);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(TestDefaultStateMachine, good_mood) {
|
TEST_F(TestDefaultStateMachine, good_mood) {
|
||||||
|
@ -187,10 +196,10 @@ TEST_F(TestDefaultStateMachine, good_mood) {
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition(
|
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition(
|
||||||
rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id());
|
rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id());
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition(
|
EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition(
|
||||||
rclcpp_lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id());
|
rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id());
|
||||||
|
|
||||||
// check if all callbacks were successfully overwritten
|
// check if all callbacks were successfully overwritten
|
||||||
EXPECT_EQ(static_cast<size_t>(5), test_node->number_of_callbacks);
|
EXPECT_EQ(5u, test_node->number_of_callbacks);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(TestDefaultStateMachine, bad_mood) {
|
TEST_F(TestDefaultStateMachine, bad_mood) {
|
||||||
|
@ -201,7 +210,7 @@ TEST_F(TestDefaultStateMachine, bad_mood) {
|
||||||
rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
|
rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
|
||||||
|
|
||||||
// check if all callbacks were successfully overwritten
|
// check if all callbacks were successfully overwritten
|
||||||
EXPECT_EQ(static_cast<size_t>(1), test_node->number_of_callbacks);
|
EXPECT_EQ(1u, test_node->number_of_callbacks);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(TestDefaultStateMachine, lifecycle_subscriber) {
|
TEST_F(TestDefaultStateMachine, lifecycle_subscriber) {
|
||||||
|
|
|
@ -46,91 +46,91 @@ public:
|
||||||
size_t number_of_callbacks = 0;
|
size_t number_of_callbacks = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_configure(const rclcpp_lifecycle::State &)
|
on_configure(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
ADD_FAILURE();
|
ADD_FAILURE();
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_activate(const rclcpp_lifecycle::State &)
|
on_activate(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
ADD_FAILURE();
|
ADD_FAILURE();
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_deactivate(const rclcpp_lifecycle::State &)
|
on_deactivate(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
ADD_FAILURE();
|
ADD_FAILURE();
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_cleanup(const rclcpp_lifecycle::State &)
|
on_cleanup(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
ADD_FAILURE();
|
ADD_FAILURE();
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_shutdown(const rclcpp_lifecycle::State &)
|
on_shutdown(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
ADD_FAILURE();
|
ADD_FAILURE();
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Custom callbacks
|
// Custom callbacks
|
||||||
|
|
||||||
public:
|
public:
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_custom_configure(const rclcpp_lifecycle::State & previous_state)
|
on_custom_configure(const rclcpp_lifecycle::State & previous_state)
|
||||||
{
|
{
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, previous_state.id());
|
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, previous_state.id());
|
||||||
EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id());
|
EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id());
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_custom_activate(const rclcpp_lifecycle::State & previous_state)
|
on_custom_activate(const rclcpp_lifecycle::State & previous_state)
|
||||||
{
|
{
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id());
|
EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id());
|
||||||
EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id());
|
EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id());
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_custom_deactivate(const rclcpp_lifecycle::State & previous_state)
|
on_custom_deactivate(const rclcpp_lifecycle::State & previous_state)
|
||||||
{
|
{
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, previous_state.id());
|
EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, previous_state.id());
|
||||||
EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id());
|
EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id());
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_custom_cleanup(const rclcpp_lifecycle::State & previous_state)
|
on_custom_cleanup(const rclcpp_lifecycle::State & previous_state)
|
||||||
{
|
{
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id());
|
EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id());
|
||||||
EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id());
|
EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id());
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_transition_key_t
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
on_custom_shutdown(const rclcpp_lifecycle::State &)
|
on_custom_shutdown(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id());
|
EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id());
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -158,8 +158,8 @@ TEST_F(TestRegisterCustomCallbacks, custom_callbacks) {
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition(
|
EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition(
|
||||||
rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id());
|
rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id());
|
||||||
EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition(
|
EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition(
|
||||||
rclcpp_lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id());
|
rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id());
|
||||||
|
|
||||||
// check if all callbacks were successfully overwritten
|
// check if all callbacks were successfully overwritten
|
||||||
EXPECT_EQ(static_cast<size_t>(5), test_node->number_of_callbacks);
|
EXPECT_EQ(5u, test_node->number_of_callbacks);
|
||||||
}
|
}
|
||||||
|
|
|
@ -44,7 +44,7 @@ TEST_F(TestStateWrapper, wrapper) {
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
rcl_lifecycle_state_t lc_state = {"my_c_state", 2, NULL, NULL, 0};
|
rcl_lifecycle_state_t lc_state = {"my_c_state", 2, NULL, 0};
|
||||||
rclcpp_lifecycle::State c_state(lc_state.id, lc_state.label);
|
rclcpp_lifecycle::State c_state(lc_state.id, lc_state.label);
|
||||||
EXPECT_EQ(2, c_state.id());
|
EXPECT_EQ(2, c_state.id());
|
||||||
EXPECT_FALSE(c_state.label().empty());
|
EXPECT_FALSE(c_state.label().empty());
|
||||||
|
@ -52,7 +52,7 @@ TEST_F(TestStateWrapper, wrapper) {
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
rcl_lifecycle_state_t lc_state = {"my_c_state", 2, NULL, NULL, 0};
|
rcl_lifecycle_state_t lc_state = {"my_c_state", 2, NULL, 0};
|
||||||
rclcpp_lifecycle::State c_state(&lc_state);
|
rclcpp_lifecycle::State c_state(&lc_state);
|
||||||
EXPECT_EQ(2, c_state.id());
|
EXPECT_EQ(2, c_state.id());
|
||||||
EXPECT_FALSE(c_state.label().empty());
|
EXPECT_FALSE(c_state.label().empty());
|
||||||
|
@ -61,7 +61,7 @@ TEST_F(TestStateWrapper, wrapper) {
|
||||||
|
|
||||||
{
|
{
|
||||||
rcl_lifecycle_state_t * lc_state =
|
rcl_lifecycle_state_t * lc_state =
|
||||||
new rcl_lifecycle_state_t {"my_c_state", 3, NULL, NULL, 0};
|
new rcl_lifecycle_state_t {"my_c_state", 3, NULL, 0};
|
||||||
rclcpp_lifecycle::State c_state(lc_state->id, lc_state->label);
|
rclcpp_lifecycle::State c_state(lc_state->id, lc_state->label);
|
||||||
EXPECT_EQ(3, c_state.id());
|
EXPECT_EQ(3, c_state.id());
|
||||||
EXPECT_FALSE(c_state.label().empty());
|
EXPECT_FALSE(c_state.label().empty());
|
||||||
|
@ -107,12 +107,12 @@ TEST_F(TestStateWrapper, assignment_operator) {
|
||||||
TEST_F(TestStateWrapper, assignment_operator2) {
|
TEST_F(TestStateWrapper, assignment_operator2) {
|
||||||
// Non-owning State
|
// Non-owning State
|
||||||
rcl_lifecycle_state_t * lc_state1 =
|
rcl_lifecycle_state_t * lc_state1 =
|
||||||
new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, NULL, 0};
|
new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, 0};
|
||||||
auto non_owning_state1 = std::make_shared<rclcpp_lifecycle::State>(lc_state1);
|
auto non_owning_state1 = std::make_shared<rclcpp_lifecycle::State>(lc_state1);
|
||||||
|
|
||||||
// Non-owning State
|
// Non-owning State
|
||||||
rcl_lifecycle_state_t * lc_state2 =
|
rcl_lifecycle_state_t * lc_state2 =
|
||||||
new rcl_lifecycle_state_t{"my_c_state2", 2, NULL, NULL, 0};
|
new rcl_lifecycle_state_t{"my_c_state2", 2, NULL, 0};
|
||||||
auto non_owning_state2 = std::make_shared<rclcpp_lifecycle::State>(lc_state2);
|
auto non_owning_state2 = std::make_shared<rclcpp_lifecycle::State>(lc_state2);
|
||||||
|
|
||||||
*non_owning_state2 = *non_owning_state1;
|
*non_owning_state2 = *non_owning_state1;
|
||||||
|
@ -130,7 +130,7 @@ TEST_F(TestStateWrapper, assignment_operator2) {
|
||||||
TEST_F(TestStateWrapper, assignment_operator3) {
|
TEST_F(TestStateWrapper, assignment_operator3) {
|
||||||
// Non-owning State
|
// Non-owning State
|
||||||
rcl_lifecycle_state_t * lc_state1 =
|
rcl_lifecycle_state_t * lc_state1 =
|
||||||
new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, NULL, 0};
|
new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, 0};
|
||||||
auto non_owning_state1 = std::make_shared<rclcpp_lifecycle::State>(lc_state1);
|
auto non_owning_state1 = std::make_shared<rclcpp_lifecycle::State>(lc_state1);
|
||||||
|
|
||||||
// owning State
|
// owning State
|
||||||
|
@ -150,7 +150,7 @@ TEST_F(TestStateWrapper, assignment_operator3) {
|
||||||
TEST_F(TestStateWrapper, assignment_operator4) {
|
TEST_F(TestStateWrapper, assignment_operator4) {
|
||||||
// Non-owning State
|
// Non-owning State
|
||||||
rcl_lifecycle_state_t * lc_state1 =
|
rcl_lifecycle_state_t * lc_state1 =
|
||||||
new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, NULL, 0};
|
new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, 0};
|
||||||
auto non_owning_state1 = std::make_shared<rclcpp_lifecycle::State>(lc_state1);
|
auto non_owning_state1 = std::make_shared<rclcpp_lifecycle::State>(lc_state1);
|
||||||
|
|
||||||
// owning State
|
// owning State
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue