Skip to content

Automatically transition lifecycle entities when node transitions #1863

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
ivanpauno merged 5 commits into from
Jan 14, 2022
Merged

Automatically transition lifecycle entities when node transitions #1863

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
02901df
Automatically transition LifecyclePublisher(s) between activated and …
ivanpauno Jan 12, 2022
988b77b
Fix: Add created publishers to the managed entities vector
ivanpauno Jan 12, 2022
b2c73ec
enabled_ -> activated_
ivanpauno Jan 13, 2022
469f5d7
Continue setting should_log_ as before
ivanpauno Jan 13, 2022
0035757
Fix visibility attributes so it works on Windows
ivanpauno Jan 14, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions rclcpp_lifecycle/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ find_package(lifecycle_msgs REQUIRED)
### CPP High level library
add_library(rclcpp_lifecycle
src/lifecycle_node.cpp
src/managed_entity.cpp
src/node_interfaces/lifecycle_node_interface.cpp
src/state.cpp
src/transition.cpp
Expand Down
10 changes: 9 additions & 1 deletion rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -968,10 +968,18 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
bool
register_on_error(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);

RCLCPP_LIFECYCLE_PUBLIC
CallbackReturn
on_activate(const State & previous_state) override;

RCLCPP_LIFECYCLE_PUBLIC
CallbackReturn
on_deactivate(const State & previous_state) override;

protected:
RCLCPP_LIFECYCLE_PUBLIC
void
add_publisher_handle(std::shared_ptr<rclcpp_lifecycle::LifecyclePublisherInterface> pub);
add_managed_entity(std::weak_ptr<rclcpp_lifecycle::ManagedEntityInterface> managed_entity);

RCLCPP_LIFECYCLE_PUBLIC
void
Expand Down
4 changes: 3 additions & 1 deletion rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,11 +49,13 @@ LifecycleNode::create_publisher(
const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
{
using PublisherT = rclcpp_lifecycle::LifecyclePublisher<MessageT, AllocatorT>;
return rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
auto pub = rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
*this,
topic_name,
qos,
options);
this->add_managed_entity(pub);
return pub;
}

// TODO(karsten1987): Create LifecycleSubscriber
Expand Down
47 changes: 9 additions & 38 deletions rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,35 +19,20 @@
#include <string>
#include <utility>

#include "rclcpp/logging.hpp"
#include "rclcpp/publisher.hpp"

#include "rclcpp/logging.hpp"
#include "rclcpp_lifecycle/managed_entity.hpp"


namespace rclcpp_lifecycle
{
/// base class with only
/**
* pure virtual functions. A managed
* node can then deactivate or activate
* the publishing.
* It is more a convenient interface class
* than a necessary base class.
*/
class LifecyclePublisherInterface
{
public:
virtual ~LifecyclePublisherInterface() {}
virtual void on_activate() = 0;
virtual void on_deactivate() = 0;
virtual bool is_activated() = 0;
};

/// brief child class of rclcpp Publisher class.
/**
* Overrides all publisher functions to check for enabled/disabled state.
*/
template<typename MessageT, typename Alloc = std::allocator<void>>
class LifecyclePublisher : public LifecyclePublisherInterface,
class LifecyclePublisher : public SimpleManagedEntity,
public rclcpp::Publisher<MessageT, Alloc>
{
public:
Expand All @@ -64,7 +49,6 @@ class LifecyclePublisher : public LifecyclePublisherInterface,
const rclcpp::QoS & qos,
const rclcpp::PublisherOptionsWithAllocator<Alloc> & options)
: rclcpp::Publisher<MessageT, Alloc>(node_base, topic, qos, options),
enabled_(false),
should_log_(true),
logger_(rclcpp::get_logger("LifecyclePublisher"))
{
Expand All @@ -81,7 +65,7 @@ class LifecyclePublisher : public LifecyclePublisherInterface,
virtual void
publish(std::unique_ptr<MessageT, MessageDeleter> msg)
{
if (!enabled_) {
if (!this->is_activated()) {
log_publisher_not_enabled();
return;
}
Expand All @@ -97,32 +81,20 @@ class LifecyclePublisher : public LifecyclePublisherInterface,
virtual void
publish(const MessageT & msg)
{
if (!enabled_) {
if (!this->is_activated()) {
log_publisher_not_enabled();
return;
}
rclcpp::Publisher<MessageT, Alloc>::publish(msg);
}

virtual void
on_activate()
{
enabled_ = true;
}

virtual void
on_deactivate()
void
on_activate() override
{
enabled_ = false;
SimpleManagedEntity::on_activate();
should_log_ = true;
}

virtual bool
is_activated()
{
return enabled_;
}

private:
/// LifecyclePublisher log helper function
/**
Expand All @@ -146,7 +118,6 @@ class LifecyclePublisher : public LifecyclePublisherInterface,
should_log_ = false;
}

bool enabled_ = false;
bool should_log_ = true;
rclcpp::Logger logger_;
};
Expand Down
68 changes: 68 additions & 0 deletions rclcpp_lifecycle/include/rclcpp_lifecycle/managed_entity.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,68 @@
// Copyright 2022 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef RCLCPP_LIFECYCLE__MANAGED_ENTITY_HPP_
#define RCLCPP_LIFECYCLE__MANAGED_ENTITY_HPP_

#include <atomic>

#include "rclcpp_lifecycle/visibility_control.h"

namespace rclcpp_lifecycle
{

/// Base class for lifecycle entities, like `LifecyclePublisher`.
class ManagedEntityInterface
Copy link
Copy Markdown
Member Author

@ivanpauno ivanpauno Jan 13, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The interface in rclpy is a bit different (see here):

  • on_activate()/on_deactivate() receives the previous state as a parameter and can fail.
    • I think in most of the cases on_activate()/on_deactivate() shouldn't fail, the on_shutdown()/on_configure() transitions should be doing the fallible work.
    • Receiving the previous state is not important for this transitions, as it's always 'inactive' for on_activate() and 'activated' for on_deactivate().
  • The rclpy interface allows you to also provide on_shutdown()/on_configure()/on_error() transitions.
    • LifecyclePublisher is not using it, though entities could lazely create resources on on_configure() and destroy them on on_shutdown() if those methods were available.
    • The default on_configure/on_shutdown() implementation in Node might be less useful, as it's not clear what to do if some of the callbacks fail (see this and this).
      In practice, I think people usually want to manually write on_configure()/on_shutdown().

We could either modify this PR to do exactly what rclpy does or reduce rclpy flexibility to match more closely what this PR is adding.
I prefer a bit this less flexible version, which is what we actually seem to need.

Copy link
Copy Markdown
Member

@jacobperron jacobperron Jan 13, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think in most of the cases on_activate()/on_deactivate() shouldn't fail

Having less failure points sounds nice to me. I haven't thought through scenarios where someone may want to fail during activation/deactivation, but we could add it if the need arises.

The default on_configure/on_shutdown() implementation in Node might be less useful, as it's not clear what to do if some of the callbacks fail

If one or more of the callbacks fail, I would think we would want to transition all entities to the unconfigued state (as indicated by the design doc diagram). What do you think?

I agree that in most cases people will probably provide their own on_configure/on_shutdown logic. I think that it still might be nice if we call the transitions on all managed entities though. E.g. call any user-defined callback for the node and then, if that is successful, call the transition callbacks for managed entities. Or do you see something wrong with that behavior?

Copy link
Copy Markdown
Member Author

@ivanpauno ivanpauno Jan 13, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If one or more of the callbacks fail, I would think we would want to transition all entities to the unconfigued state (as indicated by the design doc diagram). What do you think?

I think we could do that.
The logic would be:

  • configure
    • Try to transition all managed entities to configured
    • If transition fails (TRANSITION_FAILURE), shut down all the managed entities that already transitioned to go back to the unconfigured state.
    • If any callback errors (TRANSITION_ERROR), then we immediately return error without doing any more transitions.
      This applies when a on_configure callback errors, or when an on_shutdown callback errors after a previous failure.
  • shutdown
    • Similar, but on failures we do nothing, and continue trying to move the rest of the entities to the shutdown state.

The only problem I see is that the logic is a bit complex and might not be actually useful in practice (we don't have any lifecycle entity that do work on configure/shutdown currently).

I agree that in most cases people will probably provide their own on_configure/on_shutdown logic. I think that it still might be nice if we call the transitions on all managed entities though. E.g. call any user-defined callback for the node and then, if that is successful, call the transition callbacks for managed entities. Or do you see something wrong with that behavior?

I think this has been resolved in the discussion here.

Copy link
Copy Markdown
Member

@jacobperron jacobperron Jan 13, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fair enough. I'm fine keeping the implementation simple for now; we can add complexity if the need arises.

{
public:
RCLCPP_LIFECYCLE_PUBLIC
virtual
~ManagedEntityInterface() {}

RCLCPP_LIFECYCLE_PUBLIC
virtual
void
on_activate() = 0;

RCLCPP_LIFECYCLE_PUBLIC
virtual
void
on_deactivate() = 0;
};

/// A simple implementation of `ManagedEntityInterface`, which toogles a flag.
class SimpleManagedEntity : public ManagedEntityInterface
{
public:
RCLCPP_LIFECYCLE_PUBLIC
~SimpleManagedEntity() override = default;

RCLCPP_LIFECYCLE_PUBLIC
void
on_activate() override;

RCLCPP_LIFECYCLE_PUBLIC
void
on_deactivate() override;

RCLCPP_LIFECYCLE_PUBLIC
bool
is_activated() const;

private:
std::atomic<bool> activated_ = false;
};

} // namespace rclcpp_lifecycle
#endif // RCLCPP_LIFECYCLE__MANAGED_ENTITY_HPP_
20 changes: 17 additions & 3 deletions rclcpp_lifecycle/src/lifecycle_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -639,11 +639,25 @@ LifecycleNode::shutdown(LifecycleNodeInterface::CallbackReturn & cb_return_code)
rcl_lifecycle_shutdown_label, cb_return_code);
}

node_interfaces::LifecycleNodeInterface::CallbackReturn
LifecycleNode::on_activate(const State &)
{
impl_->on_activate();
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

node_interfaces::LifecycleNodeInterface::CallbackReturn
LifecycleNode::on_deactivate(const State &)
{
impl_->on_deactivate();
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

void
LifecycleNode::add_publisher_handle(
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisherInterface> pub)
LifecycleNode::add_managed_entity(
std::weak_ptr<rclcpp_lifecycle::ManagedEntityInterface> managed_entity)
{
impl_->add_publisher_handle(pub);
impl_->add_managed_entity(managed_entity);
}

void
Expand Down
28 changes: 25 additions & 3 deletions rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -501,9 +501,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
}

void
add_publisher_handle(std::shared_ptr<rclcpp_lifecycle::LifecyclePublisherInterface> pub)
add_managed_entity(std::weak_ptr<rclcpp_lifecycle::ManagedEntityInterface> managed_entity)
{
weak_pubs_.push_back(pub);
weak_managed_entities_.push_back(managed_entity);
}

void
Expand All @@ -512,6 +512,28 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
weak_timers_.push_back(timer);
}

void
on_activate()
{
for (const auto & weak_entity : weak_managed_entities_) {
auto entity = weak_entity.lock();
if (entity) {
entity->on_activate();
}
}
}

void
on_deactivate()
{
for (const auto & weak_entity : weak_managed_entities_) {
auto entity = weak_entity.lock();
if (entity) {
entity->on_activate();
}
}
}

rcl_lifecycle_state_machine_t state_machine_;
State current_state_;
std::map<
Expand All @@ -538,7 +560,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
GetTransitionGraphSrvPtr srv_get_transition_graph_;

// to controllable things
std::vector<std::weak_ptr<rclcpp_lifecycle::LifecyclePublisherInterface>> weak_pubs_;
std::vector<std::weak_ptr<rclcpp_lifecycle::ManagedEntityInterface>> weak_managed_entities_;
std::vector<std::weak_ptr<rclcpp::TimerBase>> weak_timers_;
};

Expand Down
35 changes: 35 additions & 0 deletions rclcpp_lifecycle/src/managed_entity.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
// Copyright 2022 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "rclcpp_lifecycle/managed_entity.hpp"

namespace rclcpp_lifecycle
{

void SimpleManagedEntity::on_activate()
{
activated_.store(true);
}

void SimpleManagedEntity::on_deactivate()
{
activated_.store(false);
}

bool SimpleManagedEntity::is_activated() const
{
return activated_.load();
}

} // namespace rclcpp_lifecycle
2 changes: 1 addition & 1 deletion rclcpp_lifecycle/test/test_lifecycle_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode
publisher_ =
std::make_shared<rclcpp_lifecycle::LifecyclePublisher<test_msgs::msg::Empty>>(
get_node_base_interface().get(), std::string("topic"), rclcpp::QoS(10), options);
add_publisher_handle(publisher_);
add_managed_entity(publisher_);

// For coverage this is being added here
auto timer = create_wall_timer(std::chrono::seconds(1), []() {});
Expand Down