rename RCL_LIFECYCLE_RET_T to lifecycle_msgs::msgs::Transition::TRANSITION_CALLBACK_* (#345)
This commit is contained in:
parent
a41245e6bf
commit
9281e32f82
8 changed files with 156 additions and 116 deletions
|
@ -373,7 +373,8 @@ public:
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
trigger_transition(const Transition & transition, rcl_lifecycle_ret_t & cb_return_code);
|
trigger_transition(
|
||||||
|
const Transition & transition, rcl_lifecycle_transition_key_t & cb_return_code);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
|
@ -381,7 +382,8 @@ public:
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
trigger_transition(uint8_t transition_id, rcl_lifecycle_ret_t & cb_return_code);
|
trigger_transition(
|
||||||
|
uint8_t transition_id, rcl_lifecycle_transition_key_t & cb_return_code);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
|
@ -389,7 +391,7 @@ public:
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
configure(rcl_lifecycle_ret_t & cb_return_code);
|
configure(rcl_lifecycle_transition_key_t & cb_return_code);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
|
@ -397,7 +399,7 @@ public:
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
cleanup(rcl_lifecycle_ret_t & cb_return_code);
|
cleanup(rcl_lifecycle_transition_key_t & cb_return_code);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
|
@ -405,7 +407,7 @@ public:
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
activate(rcl_lifecycle_ret_t & cb_return_code);
|
activate(rcl_lifecycle_transition_key_t & cb_return_code);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
|
@ -413,7 +415,7 @@ public:
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
deactivate(rcl_lifecycle_ret_t & cb_return_code);
|
deactivate(rcl_lifecycle_transition_key_t & cb_return_code);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
|
@ -421,31 +423,31 @@ public:
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
const State &
|
const State &
|
||||||
shutdown(rcl_lifecycle_ret_t & cb_return_code);
|
shutdown(rcl_lifecycle_transition_key_t & cb_return_code);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
bool
|
bool
|
||||||
register_on_configure(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
|
register_on_configure(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
bool
|
bool
|
||||||
register_on_cleanup(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
|
register_on_cleanup(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
bool
|
bool
|
||||||
register_on_shutdown(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
|
register_on_shutdown(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
bool
|
bool
|
||||||
register_on_activate(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
|
register_on_activate(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
bool
|
bool
|
||||||
register_on_deactivate(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
|
register_on_deactivate(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
|
||||||
|
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
bool
|
bool
|
||||||
register_on_error(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
|
register_on_error(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
|
|
|
@ -49,7 +49,7 @@ public:
|
||||||
* \return true by default
|
* \return true by default
|
||||||
*/
|
*/
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
virtual rcl_lifecycle_ret_t
|
virtual rcl_lifecycle_transition_key_t
|
||||||
on_configure(const State & previous_state);
|
on_configure(const State & previous_state);
|
||||||
|
|
||||||
/// Callback function for cleanup transition
|
/// Callback function for cleanup transition
|
||||||
|
@ -57,7 +57,7 @@ public:
|
||||||
* \return true by default
|
* \return true by default
|
||||||
*/
|
*/
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
virtual rcl_lifecycle_ret_t
|
virtual rcl_lifecycle_transition_key_t
|
||||||
on_cleanup(const State & previous_state);
|
on_cleanup(const State & previous_state);
|
||||||
|
|
||||||
/// Callback function for shutdown transition
|
/// Callback function for shutdown transition
|
||||||
|
@ -65,7 +65,7 @@ public:
|
||||||
* \return true by default
|
* \return true by default
|
||||||
*/
|
*/
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
virtual rcl_lifecycle_ret_t
|
virtual rcl_lifecycle_transition_key_t
|
||||||
on_shutdown(const State & previous_state);
|
on_shutdown(const State & previous_state);
|
||||||
|
|
||||||
/// Callback function for activate transition
|
/// Callback function for activate transition
|
||||||
|
@ -73,7 +73,7 @@ public:
|
||||||
* \return true by default
|
* \return true by default
|
||||||
*/
|
*/
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
virtual rcl_lifecycle_ret_t
|
virtual rcl_lifecycle_transition_key_t
|
||||||
on_activate(const State & previous_state);
|
on_activate(const State & previous_state);
|
||||||
|
|
||||||
/// Callback function for deactivate transition
|
/// Callback function for deactivate transition
|
||||||
|
@ -81,7 +81,7 @@ public:
|
||||||
* \return true by default
|
* \return true by default
|
||||||
*/
|
*/
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
virtual rcl_lifecycle_ret_t
|
virtual rcl_lifecycle_transition_key_t
|
||||||
on_deactivate(const State & previous_state);
|
on_deactivate(const State & previous_state);
|
||||||
|
|
||||||
/// Callback function for errorneous transition
|
/// Callback function for errorneous transition
|
||||||
|
@ -89,7 +89,7 @@ public:
|
||||||
* \return false by default
|
* \return false by default
|
||||||
*/
|
*/
|
||||||
RCLCPP_LIFECYCLE_PUBLIC
|
RCLCPP_LIFECYCLE_PUBLIC
|
||||||
virtual rcl_lifecycle_ret_t
|
virtual rcl_lifecycle_transition_key_t
|
||||||
on_error(const State & previous_state);
|
on_error(const State & previous_state);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -245,40 +245,51 @@ LifecycleNode::get_node_parameters_interface()
|
||||||
|
|
||||||
////
|
////
|
||||||
bool
|
bool
|
||||||
LifecycleNode::register_on_configure(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
LifecycleNode::register_on_configure(
|
||||||
|
std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
|
||||||
{
|
{
|
||||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn);
|
return impl_->register_callback(
|
||||||
|
lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
LifecycleNode::register_on_cleanup(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
LifecycleNode::register_on_cleanup(
|
||||||
|
std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
|
||||||
{
|
{
|
||||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn);
|
return impl_->register_callback(
|
||||||
|
lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
LifecycleNode::register_on_shutdown(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
LifecycleNode::register_on_shutdown(
|
||||||
|
std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
|
||||||
{
|
{
|
||||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn);
|
return impl_->register_callback(
|
||||||
|
lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
LifecycleNode::register_on_activate(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
LifecycleNode::register_on_activate(
|
||||||
|
std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
|
||||||
{
|
{
|
||||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn);
|
return impl_->register_callback(
|
||||||
|
lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
LifecycleNode::register_on_deactivate(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
LifecycleNode::register_on_deactivate(
|
||||||
|
std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
|
||||||
{
|
{
|
||||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn);
|
return impl_->register_callback(
|
||||||
|
lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
LifecycleNode::register_on_error(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
|
LifecycleNode::register_on_error(
|
||||||
|
std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
|
||||||
{
|
{
|
||||||
return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING,
|
return impl_->register_callback(
|
||||||
fcn);
|
lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING, fcn);
|
||||||
}
|
}
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
|
@ -307,7 +318,7 @@ LifecycleNode::trigger_transition(const Transition & transition)
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
LifecycleNode::trigger_transition(
|
LifecycleNode::trigger_transition(
|
||||||
const Transition & transition, rcl_lifecycle_ret_t & cb_return_code)
|
const Transition & transition, rcl_lifecycle_transition_key_t & cb_return_code)
|
||||||
{
|
{
|
||||||
return trigger_transition(transition.id(), cb_return_code);
|
return trigger_transition(transition.id(), cb_return_code);
|
||||||
}
|
}
|
||||||
|
@ -320,7 +331,7 @@ LifecycleNode::trigger_transition(uint8_t transition_id)
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
LifecycleNode::trigger_transition(
|
LifecycleNode::trigger_transition(
|
||||||
uint8_t transition_id, rcl_lifecycle_ret_t & cb_return_code)
|
uint8_t transition_id, rcl_lifecycle_transition_key_t & cb_return_code)
|
||||||
{
|
{
|
||||||
return impl_->trigger_transition(transition_id, cb_return_code);
|
return impl_->trigger_transition(transition_id, cb_return_code);
|
||||||
}
|
}
|
||||||
|
@ -333,7 +344,7 @@ LifecycleNode::configure()
|
||||||
}
|
}
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
LifecycleNode::configure(rcl_lifecycle_ret_t & cb_return_code)
|
LifecycleNode::configure(rcl_lifecycle_transition_key_t & 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);
|
||||||
|
@ -347,7 +358,7 @@ LifecycleNode::cleanup()
|
||||||
}
|
}
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
LifecycleNode::cleanup(rcl_lifecycle_ret_t & cb_return_code)
|
LifecycleNode::cleanup(rcl_lifecycle_transition_key_t & 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);
|
||||||
|
@ -361,7 +372,7 @@ LifecycleNode::activate()
|
||||||
}
|
}
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
LifecycleNode::activate(rcl_lifecycle_ret_t & cb_return_code)
|
LifecycleNode::activate(rcl_lifecycle_transition_key_t & 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);
|
||||||
|
@ -375,7 +386,7 @@ LifecycleNode::deactivate()
|
||||||
}
|
}
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
LifecycleNode::deactivate(rcl_lifecycle_ret_t & cb_return_code)
|
LifecycleNode::deactivate(rcl_lifecycle_transition_key_t & 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);
|
||||||
|
@ -389,7 +400,7 @@ LifecycleNode::shutdown()
|
||||||
}
|
}
|
||||||
|
|
||||||
const State &
|
const State &
|
||||||
LifecycleNode::shutdown(rcl_lifecycle_ret_t & cb_return_code)
|
LifecycleNode::shutdown(rcl_lifecycle_transition_key_t & cb_return_code)
|
||||||
{
|
{
|
||||||
return impl_->trigger_transition(
|
return impl_->trigger_transition(
|
||||||
lifecycle_msgs::msg::Transition::TRANSITION_SHUTDOWN, cb_return_code);
|
lifecycle_msgs::msg::Transition::TRANSITION_SHUTDOWN, cb_return_code);
|
||||||
|
|
|
@ -164,7 +164,7 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
register_callback(std::uint8_t lifecycle_transition, std::function<rcl_lifecycle_ret_t(
|
register_callback(std::uint8_t lifecycle_transition, std::function<rcl_lifecycle_transition_key_t(
|
||||||
const State &)> & cb)
|
const State &)> & cb)
|
||||||
{
|
{
|
||||||
cb_map_[lifecycle_transition] = cb;
|
cb_map_[lifecycle_transition] = cb;
|
||||||
|
@ -181,7 +181,7 @@ 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_ret_t cb_return_code;
|
rcl_lifecycle_transition_key_t 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
|
||||||
|
@ -281,7 +281,7 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_ret_t
|
rcl_ret_t
|
||||||
change_state(std::uint8_t lifecycle_transition, rcl_lifecycle_ret_t & cb_return_code)
|
change_state(std::uint8_t lifecycle_transition, rcl_lifecycle_transition_key_t & 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",
|
||||||
|
@ -312,11 +312,11 @@ public:
|
||||||
|
|
||||||
// error handling ?!
|
// error handling ?!
|
||||||
// TODO(karsten1987): iterate over possible ret value
|
// TODO(karsten1987): iterate over possible ret value
|
||||||
if (cb_return_code == RCL_LIFECYCLE_RET_ERROR) {
|
if (cb_return_code == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR) {
|
||||||
RCUTILS_LOG_WARN("Error occurred while doing error handling.")
|
RCUTILS_LOG_WARN("Error occurred while doing error handling.")
|
||||||
rcl_lifecycle_ret_t error_resolved = execute_callback(state_machine_.current_state->id,
|
rcl_lifecycle_transition_key_t error_resolved = execute_callback(
|
||||||
initial_state);
|
state_machine_.current_state->id, initial_state);
|
||||||
if (error_resolved == RCL_LIFECYCLE_RET_OK) {
|
if (error_resolved == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) {
|
||||||
// We call cleanup on the error state
|
// We call cleanup on the error state
|
||||||
if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, true) != RCL_RET_OK) {
|
if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, true) != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR("Failed to call cleanup on error state")
|
RCUTILS_LOG_ERROR("Failed to call cleanup on error state")
|
||||||
|
@ -336,11 +336,12 @@ public:
|
||||||
return RCL_RET_OK;
|
return RCL_RET_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t
|
rcl_lifecycle_transition_key_t
|
||||||
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
|
||||||
auto cb_success = RCL_LIFECYCLE_RET_OK;
|
rcl_lifecycle_transition_key_t cb_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()) {
|
||||||
|
@ -355,7 +356,7 @@ public:
|
||||||
// fprintf(stderr, "Original error msg: %s\n", e.what());
|
// fprintf(stderr, "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 = RCL_LIFECYCLE_RET_ERROR;
|
cb_success = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return cb_success;
|
return cb_success;
|
||||||
|
@ -364,14 +365,14 @@ public:
|
||||||
const State &
|
const State &
|
||||||
trigger_transition(uint8_t transition_id)
|
trigger_transition(uint8_t transition_id)
|
||||||
{
|
{
|
||||||
rcl_lifecycle_ret_t error;
|
rcl_lifecycle_transition_key_t 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_ret_t & cb_return_code)
|
trigger_transition(uint8_t transition_id, rcl_lifecycle_transition_key_t & 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();
|
||||||
|
@ -393,7 +394,7 @@ public:
|
||||||
State current_state_;
|
State current_state_;
|
||||||
std::map<
|
std::map<
|
||||||
std::uint8_t,
|
std::uint8_t,
|
||||||
std::function<rcl_lifecycle_ret_t(const State &)>> cb_map_;
|
std::function<rcl_lifecycle_transition_key_t(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>;
|
||||||
|
|
|
@ -12,46 +12,48 @@
|
||||||
// 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"
|
||||||
|
|
||||||
namespace rclcpp_lifecycle
|
namespace rclcpp_lifecycle
|
||||||
{
|
{
|
||||||
namespace node_interfaces
|
namespace node_interfaces
|
||||||
{
|
{
|
||||||
rcl_lifecycle_ret_t
|
rcl_lifecycle_transition_key_t
|
||||||
LifecycleNodeInterface::on_configure(const State &)
|
LifecycleNodeInterface::on_configure(const State &)
|
||||||
{
|
{
|
||||||
return RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t
|
rcl_lifecycle_transition_key_t
|
||||||
LifecycleNodeInterface::on_cleanup(const State &)
|
LifecycleNodeInterface::on_cleanup(const State &)
|
||||||
{
|
{
|
||||||
return RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t
|
rcl_lifecycle_transition_key_t
|
||||||
LifecycleNodeInterface::on_shutdown(const State &)
|
LifecycleNodeInterface::on_shutdown(const State &)
|
||||||
{
|
{
|
||||||
return RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t
|
rcl_lifecycle_transition_key_t
|
||||||
LifecycleNodeInterface::on_activate(const State &)
|
LifecycleNodeInterface::on_activate(const State &)
|
||||||
{
|
{
|
||||||
return RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t
|
rcl_lifecycle_transition_key_t
|
||||||
LifecycleNodeInterface::on_deactivate(const State &)
|
LifecycleNodeInterface::on_deactivate(const State &)
|
||||||
{
|
{
|
||||||
return RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t
|
rcl_lifecycle_transition_key_t
|
||||||
LifecycleNodeInterface::on_error(const State &)
|
LifecycleNodeInterface::on_error(const State &)
|
||||||
{
|
{
|
||||||
return RCL_LIFECYCLE_RET_FAILURE;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE;
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace node_interfaces
|
} // namespace node_interfaces
|
||||||
|
|
|
@ -46,16 +46,18 @@ public:
|
||||||
size_t number_of_callbacks = 0;
|
size_t number_of_callbacks = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
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_ret_t on_error(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
on_error(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -73,9 +75,9 @@ 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_ret_t ret = RCL_LIFECYCLE_RET_OK;
|
rcl_lifecycle_transition_key_t ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
test_node->configure(ret);
|
test_node->configure(ret);
|
||||||
EXPECT_EQ(RCL_LIFECYCLE_RET_ERROR, ret);
|
EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR, ret);
|
||||||
}
|
}
|
||||||
|
|
||||||
class NegativeCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode
|
class NegativeCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode
|
||||||
|
@ -88,16 +90,18 @@ public:
|
||||||
size_t number_of_callbacks = 0;
|
size_t number_of_callbacks = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
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_ret_t on_error(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
on_error(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return RCL_LIFECYCLE_RET_FAILURE;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -115,7 +119,7 @@ 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_ret_t ret = RCL_RET_OK;
|
rcl_lifecycle_transition_key_t ret = RCL_RET_OK;
|
||||||
test_node->configure(ret);
|
test_node->configure(ret);
|
||||||
EXPECT_EQ(RCL_LIFECYCLE_RET_ERROR, ret);
|
EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR, ret);
|
||||||
}
|
}
|
||||||
|
|
|
@ -29,11 +29,13 @@ using lifecycle_msgs::msg::Transition;
|
||||||
|
|
||||||
struct GoodMood
|
struct GoodMood
|
||||||
{
|
{
|
||||||
static constexpr rcl_lifecycle_ret_t cb_ret = RCL_LIFECYCLE_RET_OK;
|
static constexpr rcl_lifecycle_transition_key_t cb_ret =
|
||||||
|
lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
};
|
};
|
||||||
struct BadMood
|
struct BadMood
|
||||||
{
|
{
|
||||||
static constexpr rcl_lifecycle_ret_t cb_ret = RCL_LIFECYCLE_RET_FAILURE;
|
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
|
||||||
|
@ -64,57 +66,65 @@ public:
|
||||||
size_t number_of_callbacks = 0;
|
size_t number_of_callbacks = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
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());
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return Mood::cb_ret;
|
return Mood::cb_ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t on_activate(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
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());
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return Mood::cb_ret;
|
return Mood::cb_ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t on_deactivate(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
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());
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return Mood::cb_ret;
|
return Mood::cb_ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t on_cleanup(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
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());
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return Mood::cb_ret;
|
return Mood::cb_ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t on_shutdown(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
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());
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return Mood::cb_ret;
|
return Mood::cb_ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State &);
|
rcl_lifecycle_transition_key_t
|
||||||
|
on_error(const rclcpp_lifecycle::State &);
|
||||||
};
|
};
|
||||||
|
|
||||||
template<>
|
template<>
|
||||||
rcl_lifecycle_ret_t MoodyLifecycleNode<GoodMood>::on_error(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
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 RCL_LIFECYCLE_RET_ERROR;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
|
||||||
}
|
}
|
||||||
template<>
|
template<>
|
||||||
rcl_lifecycle_ret_t MoodyLifecycleNode<BadMood>::on_error(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
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 RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(TestDefaultStateMachine, empty_initializer) {
|
TEST_F(TestDefaultStateMachine, empty_initializer) {
|
||||||
|
@ -143,25 +153,25 @@ TEST_F(TestDefaultStateMachine, trigger_transition) {
|
||||||
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_ret_t ret = RCL_LIFECYCLE_RET_ERROR;
|
rcl_lifecycle_transition_key_t ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
|
||||||
test_node->configure(ret);
|
test_node->configure(ret);
|
||||||
EXPECT_EQ(RCL_LIFECYCLE_RET_OK, ret);
|
EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret);
|
||||||
ret = RCL_LIFECYCLE_RET_ERROR;
|
ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
|
||||||
|
|
||||||
test_node->activate(ret);
|
test_node->activate(ret);
|
||||||
EXPECT_EQ(RCL_LIFECYCLE_RET_OK, ret);
|
EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret);
|
||||||
ret = RCL_LIFECYCLE_RET_ERROR;
|
ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
|
||||||
|
|
||||||
test_node->deactivate(ret);
|
test_node->deactivate(ret);
|
||||||
EXPECT_EQ(RCL_LIFECYCLE_RET_OK, ret);
|
EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret);
|
||||||
ret = RCL_LIFECYCLE_RET_ERROR;
|
ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
|
||||||
|
|
||||||
test_node->cleanup(ret);
|
test_node->cleanup(ret);
|
||||||
EXPECT_EQ(RCL_LIFECYCLE_RET_OK, ret);
|
EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret);
|
||||||
ret = RCL_LIFECYCLE_RET_ERROR;
|
ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
|
||||||
|
|
||||||
test_node->shutdown(ret);
|
test_node->shutdown(ret);
|
||||||
EXPECT_EQ(RCL_LIFECYCLE_RET_OK, ret);
|
EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(TestDefaultStateMachine, good_mood) {
|
TEST_F(TestDefaultStateMachine, good_mood) {
|
||||||
|
|
|
@ -46,81 +46,91 @@ public:
|
||||||
size_t number_of_callbacks = 0;
|
size_t number_of_callbacks = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
on_configure(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
ADD_FAILURE();
|
ADD_FAILURE();
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t on_activate(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
on_activate(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
ADD_FAILURE();
|
ADD_FAILURE();
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t on_deactivate(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
on_deactivate(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
ADD_FAILURE();
|
ADD_FAILURE();
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t on_cleanup(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
on_cleanup(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
ADD_FAILURE();
|
ADD_FAILURE();
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t on_shutdown(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
on_shutdown(const rclcpp_lifecycle::State &)
|
||||||
{
|
{
|
||||||
ADD_FAILURE();
|
ADD_FAILURE();
|
||||||
++number_of_callbacks;
|
++number_of_callbacks;
|
||||||
return RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Custom callbacks
|
// Custom callbacks
|
||||||
|
|
||||||
public:
|
public:
|
||||||
rcl_lifecycle_ret_t on_custom_configure(const rclcpp_lifecycle::State & previous_state)
|
rcl_lifecycle_transition_key_t
|
||||||
|
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 RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t on_custom_activate(const rclcpp_lifecycle::State & previous_state)
|
rcl_lifecycle_transition_key_t
|
||||||
|
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 RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t on_custom_deactivate(const rclcpp_lifecycle::State & previous_state)
|
rcl_lifecycle_transition_key_t
|
||||||
|
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 RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t on_custom_cleanup(const rclcpp_lifecycle::State & previous_state)
|
rcl_lifecycle_transition_key_t
|
||||||
|
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 RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_lifecycle_ret_t on_custom_shutdown(const rclcpp_lifecycle::State &)
|
rcl_lifecycle_transition_key_t
|
||||||
|
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 RCL_LIFECYCLE_RET_OK;
|
return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue