diff --git a/lifecycle/CMakeLists.txt b/lifecycle/CMakeLists.txt index f023e6262..9d190aade 100644 --- a/lifecycle/CMakeLists.txt +++ b/lifecycle/CMakeLists.txt @@ -32,6 +32,15 @@ add_executable(lifecycle_listener ament_target_dependencies(lifecycle_listener "lifecycle_msgs" "rclcpp" + "rclcpp_lifecycle" + "std_msgs" +) +add_executable(lifecycle_node_listener + src/lifecycle_node_listener.cpp) +ament_target_dependencies(lifecycle_node_listener + "lifecycle_msgs" + "rclcpp" + "rclcpp_lifecycle" "std_msgs" ) add_executable(lifecycle_service_client @@ -44,6 +53,7 @@ ament_target_dependencies(lifecycle_service_client install(TARGETS lifecycle_talker lifecycle_listener + lifecycle_node_listener lifecycle_service_client DESTINATION lib/${PROJECT_NAME}) diff --git a/lifecycle/src/lifecycle_node_listener.cpp b/lifecycle/src/lifecycle_node_listener.cpp new file mode 100644 index 000000000..1d1853fbf --- /dev/null +++ b/lifecycle/src/lifecycle_node_listener.cpp @@ -0,0 +1,105 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "lifecycle_msgs/msg/transition_event.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "std_msgs/msg/string.hpp" + +/// LifecycleListener class as a simple listener node +/** + * We subscribe to two topics + * - lifecycle_chatter: The data topic from the talker + * - lc_talker__transition_event: The topic publishing + * notifications about state changes of the node + * lc_talker + */ +class LifecycleListener : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit LifecycleListener(const std::string & node_name) + : LifecycleNode(node_name) {} + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(get_logger(), "on_configure() is called."); + // Data topic from the lc_talker node + sub_data_ = this->create_subscription( + "lifecycle_chatter", 10, + std::bind(&LifecycleListener::data_callback, this, std::placeholders::_1)); + + // Notification event topic. All state changes + // are published here as TransitionEvents with + // a start and goal state indicating the transition + sub_notification_ = this->create_subscription( + "/lc_talker/transition_event", 10, + std::bind(&LifecycleListener::notification_callback, this, std::placeholders::_1)); + + if (!sub_data_ || !sub_notification_) { + RCLCPP_FATAL(get_logger(), "Could not create subscriber."); + RCLCPP_FATAL(get_logger(), "sub_data_: %s", sub_data_ ? "true" : "false"); + RCLCPP_FATAL(get_logger(), "sub_notification_: %s", sub_notification_ ? "true" : "false"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & state) + { + sub_data_.reset(); + sub_notification_.reset(); + LifecycleNode::on_shutdown(state); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + void data_callback(std_msgs::msg::String::ConstSharedPtr msg) + { + RCLCPP_INFO(get_logger(), "data_callback: %s", msg->data.c_str()); + } + + void notification_callback(lifecycle_msgs::msg::TransitionEvent::ConstSharedPtr msg) + { + RCLCPP_INFO( + get_logger(), "notify callback: Transition from state %s to %s", + msg->start_state.label.c_str(), msg->goal_state.label.c_str()); + } + +private: + std::shared_ptr> sub_data_; + std::shared_ptr> + sub_notification_; +}; + +int main(int argc, char ** argv) +{ + // force flush of the stdout buffer. + // this ensures a correct sync of all prints + // even when executed simultaneously within the launch file. + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + rclcpp::init(argc, argv); + + auto lc_listener = std::make_shared("lc_node_listener"); + rclcpp::spin(lc_listener->get_node_base_interface()); + + rclcpp::shutdown(); + + return 0; +} diff --git a/lifecycle/test/test_lifecycle.py b/lifecycle/test/test_lifecycle.py index 85554d843..34da7f0d6 100644 --- a/lifecycle/test/test_lifecycle.py +++ b/lifecycle/test/test_lifecycle.py @@ -39,8 +39,12 @@ def generate_test_description(): package='lifecycle', executable='lifecycle_listener', name='listener', output='screen' ) + listener_lifecycle_node = launch_ros.actions.LifecycleNode( + package='lifecycle', executable='lifecycle_node_listener', + name='lc_node_listener', namespace='', output='screen' + ) return launch.LaunchDescription([ - talker_node, listener_node, + talker_node, listener_node, listener_lifecycle_node, # Right after the talker starts, make it take the 'configure' transition. launch.actions.RegisterEventHandler( launch.event_handlers.on_process_start.OnProcessStart( @@ -53,6 +57,19 @@ def generate_test_description(): ], ) ), + # Same thing for the listener node. + launch.actions.RegisterEventHandler( + launch.event_handlers.on_process_start.OnProcessStart( + target_action=listener_lifecycle_node, + on_start=[ + launch.actions.EmitEvent(event=launch_ros.events.lifecycle.ChangeState( + lifecycle_node_matcher=( + launch.events.matches_action(listener_lifecycle_node)), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + )), + ], + ) + ), # When the talker reaches the 'inactive' state, make it take the 'activate' transition. launch.actions.RegisterEventHandler( launch_ros.event_handlers.OnStateTransition( @@ -66,6 +83,21 @@ def generate_test_description(): ], ) ), + # And for the listener node. + launch.actions.RegisterEventHandler( + launch_ros.event_handlers.OnStateTransition( + target_lifecycle_node=listener_lifecycle_node, + start_state='configuring', goal_state='inactive', + entities=[ + launch.actions.EmitEvent(event=launch_ros.events.lifecycle.ChangeState( + lifecycle_node_matcher=( + launch.events.matches_action(listener_lifecycle_node) + ), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + )), + ], + ) + ), # When the talker node reaches the 'active' state, wait a bit and then make it take the # 'deactivate' transition. launch.actions.RegisterEventHandler( @@ -81,6 +113,24 @@ def generate_test_description(): ], ) ), + # And for the listener node. + launch.actions.RegisterEventHandler( + launch_ros.event_handlers.OnStateTransition( + target_lifecycle_node=listener_lifecycle_node, start_state='activating', + goal_state='active', + entities=[ + launch.actions.TimerAction(period=5.0, actions=[ + launch.actions.EmitEvent(event=launch_ros.events.lifecycle.ChangeState( + lifecycle_node_matcher=( + launch.events.matches_action(listener_lifecycle_node) + ), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_DEACTIVATE, + )), + ]), + ], + ) + ), + # When the talker node reaches the 'inactive' state coming from the 'active' state, # make it take the 'cleanup' transition. launch.actions.RegisterEventHandler( @@ -95,6 +145,21 @@ def generate_test_description(): ], ) ), + # And for the listener node. + launch.actions.RegisterEventHandler( + launch_ros.event_handlers.OnStateTransition( + target_lifecycle_node=listener_lifecycle_node, + start_state='deactivating', goal_state='inactive', + entities=[ + launch.actions.EmitEvent(event=launch_ros.events.lifecycle.ChangeState( + lifecycle_node_matcher=( + launch.events.matches_action(listener_lifecycle_node) + ), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CLEANUP, + )), + ], + ) + ), # When the talker node reaches the 'unconfigured' state after a 'cleanup' transition, # make it take the 'unconfigured_shutdown' transition. launch.actions.RegisterEventHandler( @@ -111,13 +176,35 @@ def generate_test_description(): ], ) ), + # And for the listener node. + launch.actions.RegisterEventHandler( + launch_ros.event_handlers.OnStateTransition( + target_lifecycle_node=listener_lifecycle_node, + start_state='cleaningup', goal_state='unconfigured', + entities=[ + launch.actions.EmitEvent(event=launch_ros.events.lifecycle.ChangeState( + lifecycle_node_matcher=( + launch.events.matches_action(listener_lifecycle_node) + ), + transition_id=( + lifecycle_msgs.msg.Transition.TRANSITION_UNCONFIGURED_SHUTDOWN + ), + )), + ], + ) + ), launch_testing.actions.ReadyToTest() ]), locals() class TestLifecyclePubSub(unittest.TestCase): - def test_talker_lifecycle(self, proc_info, proc_output, talker_node, listener_node): + def test_talker_lifecycle(self, + proc_info, + proc_output, + talker_node, + listener_node, + listener_lifecycle_node): """Test lifecycle talker.""" proc_output.assertWaitFor('on_configure() is called', process=talker_node, timeout=5) proc_output.assertWaitFor('on_activate() is called', process=talker_node, timeout=5) @@ -125,6 +212,9 @@ def test_talker_lifecycle(self, proc_info, proc_output, talker_node, listener_no proc_output.assertWaitFor( expected_output=pattern, process=listener_node, timeout=5 ) + proc_output.assertWaitFor( + expected_output=pattern, process=listener_lifecycle_node, timeout=5 + ) proc_output.assertWaitFor( 'on_deactivate() is called', process=talker_node, timeout=10 ) @@ -140,3 +230,7 @@ class TestLifecyclePubSubAfterShutdown(unittest.TestCase): def test_talker_graceful_shutdown(self, proc_info, talker_node): """Test lifecycle talker graceful shutdown.""" launch_testing.asserts.assertExitCodes(proc_info, process=talker_node) + + def test_listener_lifecycle_graceful_shutdown(self, proc_info, listener_lifecycle_node): + """Test lifecycle listener graceful shutdown.""" + launch_testing.asserts.assertExitCodes(proc_info, process=listener_lifecycle_node)