From d87969c3fc4a9410315a78825407033f61bdc150 Mon Sep 17 00:00:00 2001 From: Adam Milner Date: Sun, 30 Jul 2023 21:10:12 -0700 Subject: [PATCH 1/3] Lifecycle subscription testing --- lifecycle/CMakeLists.txt | 1 + lifecycle/src/lifecycle_listener.cpp | 54 +++++++++++++++++++++++++--- 2 files changed, 50 insertions(+), 5 deletions(-) diff --git a/lifecycle/CMakeLists.txt b/lifecycle/CMakeLists.txt index f023e6262..eb0a8ecfd 100644 --- a/lifecycle/CMakeLists.txt +++ b/lifecycle/CMakeLists.txt @@ -32,6 +32,7 @@ add_executable(lifecycle_listener ament_target_dependencies(lifecycle_listener "lifecycle_msgs" "rclcpp" + "rclcpp_lifecycle" "std_msgs" ) add_executable(lifecycle_service_client diff --git a/lifecycle/src/lifecycle_listener.cpp b/lifecycle/src/lifecycle_listener.cpp index 3f87a9083..c82726c89 100644 --- a/lifecycle/src/lifecycle_listener.cpp +++ b/lifecycle/src/lifecycle_listener.cpp @@ -19,6 +19,7 @@ #include "lifecycle_msgs/msg/transition_event.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_msgs/msg/string.hpp" @@ -30,17 +31,27 @@ * notifications about state changes of the node * lc_talker */ -class LifecycleListener : public rclcpp::Node +class LifecycleListener : public rclcpp_lifecycle::LifecycleNode { public: explicit LifecycleListener(const std::string & node_name) - : Node(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)); + RCLCPP_INFO(get_logger(), "LifecycleListener->on_configure()->create_subscription() returned type: %s", + abi::__cxa_demangle(typeid(sub_data_).name(), NULL, NULL, NULL)); + // Notification event topic. All state changes // are published here as TransitionEvents with // a start and goal state indicating the transition @@ -48,6 +59,39 @@ class LifecycleListener : public rclcpp::Node "/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_activate(const rclcpp_lifecycle::State &state) + { + RCLCPP_INFO(get_logger(), "on_activate() is called."); + LifecycleNode::on_activate(state); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &state) + { + RCLCPP_INFO(get_logger(), "on_deactivate() is called."); + LifecycleNode::on_deactivate(state); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State &state) + { + RCLCPP_INFO(get_logger(), "on_cleanup() is called."); + LifecycleNode::on_cleanup(state); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } void data_callback(std_msgs::msg::String::ConstSharedPtr msg) @@ -63,8 +107,8 @@ class LifecycleListener : public rclcpp::Node } private: - std::shared_ptr> sub_data_; - std::shared_ptr> + std::shared_ptr> sub_data_; + std::shared_ptr> sub_notification_; }; @@ -78,7 +122,7 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); auto lc_listener = std::make_shared("lc_listener"); - rclcpp::spin(lc_listener); + rclcpp::spin(lc_listener->get_node_base_interface()); rclcpp::shutdown(); From 23edc8e39b857d364366abf01d8f9f329e856a02 Mon Sep 17 00:00:00 2001 From: Adam Milner Date: Mon, 31 Jul 2023 16:17:50 -0700 Subject: [PATCH 2/3] Passing tests. Need to add lifecycle_node_listener tests --- lifecycle/CMakeLists.txt | 9 ++ lifecycle/src/lifecycle_listener.cpp | 54 +--------- lifecycle/src/lifecycle_node_listener.cpp | 120 ++++++++++++++++++++++ 3 files changed, 134 insertions(+), 49 deletions(-) create mode 100644 lifecycle/src/lifecycle_node_listener.cpp diff --git a/lifecycle/CMakeLists.txt b/lifecycle/CMakeLists.txt index eb0a8ecfd..9d190aade 100644 --- a/lifecycle/CMakeLists.txt +++ b/lifecycle/CMakeLists.txt @@ -35,6 +35,14 @@ ament_target_dependencies(lifecycle_listener "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 src/lifecycle_service_client.cpp) ament_target_dependencies(lifecycle_service_client @@ -45,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_listener.cpp b/lifecycle/src/lifecycle_listener.cpp index c82726c89..3f87a9083 100644 --- a/lifecycle/src/lifecycle_listener.cpp +++ b/lifecycle/src/lifecycle_listener.cpp @@ -19,7 +19,6 @@ #include "lifecycle_msgs/msg/transition_event.hpp" #include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_msgs/msg/string.hpp" @@ -31,27 +30,17 @@ * notifications about state changes of the node * lc_talker */ -class LifecycleListener : public rclcpp_lifecycle::LifecycleNode +class LifecycleListener : public rclcpp::Node { public: explicit LifecycleListener(const std::string & node_name) - : LifecycleNode(node_name) + : Node(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)); - RCLCPP_INFO(get_logger(), "LifecycleListener->on_configure()->create_subscription() returned type: %s", - abi::__cxa_demangle(typeid(sub_data_).name(), NULL, NULL, NULL)); - // Notification event topic. All state changes // are published here as TransitionEvents with // a start and goal state indicating the transition @@ -59,39 +48,6 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn "/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_activate(const rclcpp_lifecycle::State &state) - { - RCLCPP_INFO(get_logger(), "on_activate() is called."); - LifecycleNode::on_activate(state); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &state) - { - RCLCPP_INFO(get_logger(), "on_deactivate() is called."); - LifecycleNode::on_deactivate(state); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &state) - { - RCLCPP_INFO(get_logger(), "on_cleanup() is called."); - LifecycleNode::on_cleanup(state); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } void data_callback(std_msgs::msg::String::ConstSharedPtr msg) @@ -107,8 +63,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn } private: - std::shared_ptr> sub_data_; - std::shared_ptr> + std::shared_ptr> sub_data_; + std::shared_ptr> sub_notification_; }; @@ -122,7 +78,7 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); auto lc_listener = std::make_shared("lc_listener"); - rclcpp::spin(lc_listener->get_node_base_interface()); + rclcpp::spin(lc_listener); rclcpp::shutdown(); diff --git a/lifecycle/src/lifecycle_node_listener.cpp b/lifecycle/src/lifecycle_node_listener.cpp new file mode 100644 index 000000000..b93c01bce --- /dev/null +++ b/lifecycle/src/lifecycle_node_listener.cpp @@ -0,0 +1,120 @@ +// 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_activate( + const rclcpp_lifecycle::State & state) + { + RCLCPP_INFO(get_logger(), "on_activate() is called."); + LifecycleNode::on_activate(state); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & state) + { + RCLCPP_INFO(get_logger(), "on_deactivate() is called."); + LifecycleNode::on_deactivate(state); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & state) + { + RCLCPP_INFO(get_logger(), "on_cleanup() is called."); + LifecycleNode::on_cleanup(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; +} From f0b7c948085d206f18137f8726bf84848f4ef421 Mon Sep 17 00:00:00 2001 From: Adam Milner Date: Tue, 1 Aug 2023 10:44:23 -0700 Subject: [PATCH 3/3] Working tests for lifecycle subscriber node. --- lifecycle/src/lifecycle_node_listener.cpp | 23 +----- lifecycle/test/test_lifecycle.py | 98 ++++++++++++++++++++++- 2 files changed, 100 insertions(+), 21 deletions(-) diff --git a/lifecycle/src/lifecycle_node_listener.cpp b/lifecycle/src/lifecycle_node_listener.cpp index b93c01bce..1d1853fbf 100644 --- a/lifecycle/src/lifecycle_node_listener.cpp +++ b/lifecycle/src/lifecycle_node_listener.cpp @@ -60,27 +60,12 @@ class LifecycleListener : public rclcpp_lifecycle::LifecycleNode return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state) { - RCLCPP_INFO(get_logger(), "on_activate() is called."); - LifecycleNode::on_activate(state); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & state) - { - RCLCPP_INFO(get_logger(), "on_deactivate() is called."); - LifecycleNode::on_deactivate(state); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( - const rclcpp_lifecycle::State & state) - { - RCLCPP_INFO(get_logger(), "on_cleanup() is called."); - LifecycleNode::on_cleanup(state); + sub_data_.reset(); + sub_notification_.reset(); + LifecycleNode::on_shutdown(state); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } 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)