diff --git a/composition/CMakeLists.txt b/composition/CMakeLists.txt index cd985ef7e..9c30c0c4a 100644 --- a/composition/CMakeLists.txt +++ b/composition/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(ament_cmake REQUIRED) find_package(example_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(rcutils REQUIRED) find_package(std_msgs REQUIRED) @@ -79,6 +80,18 @@ target_link_libraries(client_component PUBLIC rclcpp_components_register_nodes(client_component "composition::Client") set(node_plugins "${node_plugins}composition::Client;$\n") +add_library(lifecycle_talker_component SHARED + src/lifecycle_talker_component.cpp) +target_compile_definitions(lifecycle_talker_component + PRIVATE "COMPOSITION_BUILDING_DLL") +target_link_libraries(lifecycle_talker_component PUBLIC + rclcpp::rclcpp + rclcpp_components::component + rclcpp_lifecycle::rclcpp_lifecycle + ${std_msgs_TARGETS}) +rclcpp_components_register_nodes(lifecycle_talker_component "composition::LifecycleTalker") +set(node_plugins "${node_plugins}composition::LifecycleTalker;$\n") + # since the package installs libraries without exporting them # it needs to make sure that the library path is being exported if(NOT WIN32) @@ -127,6 +140,7 @@ install(TARGETS node_like_listener_component server_component client_component + lifecycle_talker_component ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) @@ -160,6 +174,7 @@ if(BUILD_TESTING) set(SERVER_LIBRARY $) set(CLIENT_LIBRARY $) set(NODE_LIKE_LISTENER_LIBRARY $) + set(LIFECYCLE_TALKER_LIBRARY $) set(EXPECTED_OUTPUT_ALL "${CMAKE_CURRENT_SOURCE_DIR}/test/composition_all") set(EXPECTED_OUTPUT_PUBSUB "${CMAKE_CURRENT_SOURCE_DIR}/test/composition_pubsub") set(EXPECTED_OUTPUT_SRV "${CMAKE_CURRENT_SOURCE_DIR}/test/composition_srv") diff --git a/composition/include/composition/lifecycle_talker_component.hpp b/composition/include/composition/lifecycle_talker_component.hpp new file mode 100644 index 000000000..e002ceaad --- /dev/null +++ b/composition/include/composition/lifecycle_talker_component.hpp @@ -0,0 +1,66 @@ +// Copyright 2025 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 COMPOSITION__LIFECYCLE_TALKER_COMPONENT_HPP_ +#define COMPOSITION__LIFECYCLE_TALKER_COMPONENT_HPP_ + +#include "composition/visibility_control.h" + +#include "rclcpp/rclcpp.hpp" + +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" + +#include "std_msgs/msg/string.hpp" + +namespace composition +{ + +class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode { +public: + COMPOSITION_PUBLIC + explicit LifecycleTalker(const rclcpp::NodeOptions & options); + +protected: + void publish(); + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & state) override; + +private: + size_t count_; + + // We hold an instance of a lifecycle publisher. This lifecycle publisher + // can be activated or deactivated regarding on which state the lifecycle node + // is in. + // By default, a lifecycle publisher is inactive by creation and has to be + // activated to publish messages into the ROS world. + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pub_; + + // We hold an instance of a timer which periodically triggers the publish function. + // As for the beta version, this is a regular timer. In a future version, a + // lifecycle timer will be created which obeys the same lifecycle management as the + // lifecycle publisher. + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace composition + +#endif // COMPOSITION__LIFECYCLE_TALKER_COMPONENT_HPP_ diff --git a/composition/package.xml b/composition/package.xml index 30bd4b046..93fad249c 100644 --- a/composition/package.xml +++ b/composition/package.xml @@ -18,6 +18,7 @@ example_interfaces rclcpp rclcpp_components + rclcpp_lifecycle rcutils std_msgs @@ -25,6 +26,7 @@ launch_ros rclcpp rclcpp_components + rclcpp_lifecycle rcutils std_msgs diff --git a/composition/src/lifecycle_talker_component.cpp b/composition/src/lifecycle_talker_component.cpp new file mode 100644 index 000000000..2bbc19e89 --- /dev/null +++ b/composition/src/lifecycle_talker_component.cpp @@ -0,0 +1,107 @@ +// Copyright 2025 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 "composition/lifecycle_talker_component.hpp" + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +using namespace std::chrono_literals; + +namespace composition +{ + +LifecycleTalker::LifecycleTalker(const rclcpp::NodeOptions & options) +: LifecycleNode("lifecycle_talker", options), count_(0) +{ +} + +/// Callback for walltimer in order to publish the message. +/** + * Callback for walltimer. This function gets invoked by the timer + * and executes the publishing. + * For this demo, we ask the node for its current state. If the + * lifecycle publisher is not activate, we still invoke publish, but + * the communication is blocked so that no messages is actually transferred. + */ +void LifecycleTalker::publish() +{ + auto msg = std::make_unique(); + msg->data = "Lifecycle Hello World: " + std::to_string(++count_); + + // Print the current state for demo purposes + RCLCPP_INFO_EXPRESSION(this->get_logger(), pub_->is_activated(), + "Lifecycle Publisher is active. Publishing: '%s'", msg->data.c_str()); + RCLCPP_INFO_EXPRESSION(this->get_logger(), !pub_->is_activated(), + "Lifecycle Publisher is inactive. Not publishing: '%s'", msg->data.c_str()); + std::flush(std::cout); + + // We independently from the current state call publish on the lifecycle publisher. + // Only if the publisher is in an active state, the message transfer is + // enabled and the message actually published. + // Put the message into a queue to be processed by the middleware. + // This call is non-blocking. + pub_->publish(std::move(msg)); +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleTalker::on_configure(const rclcpp_lifecycle::State &) +{ + pub_ = create_publisher("lc_chatter", 10); + timer_ = create_wall_timer(1s, [this](){return this->publish();}); + + RCLCPP_INFO(get_logger(), "on_configure() is called."); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleTalker::on_cleanup(const rclcpp_lifecycle::State &) +{ + // In our cleanup phase, we release the shared pointers to the timer and publisher. + // These entities are no longer available and our node is "clean". + timer_.reset(); + pub_.reset(); + + RCLCPP_INFO(get_logger(), "on_cleanup() is called."); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleTalker::on_shutdown(const rclcpp_lifecycle::State & state) +{ + // In our cleanup phase, we release the shared pointers to the timer and publisher. + // These entities are no longer available and our node is "clean". + timer_.reset(); + pub_.reset(); + + RCLCPP_INFO(get_logger(), "on_shutdown() is called from state %s.", state.label().c_str()); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +} // namespace composition + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(composition::LifecycleTalker) diff --git a/composition/test/test_api_lc_pubsub_composition.py.in b/composition/test/test_api_lc_pubsub_composition.py.in new file mode 100644 index 000000000..b1ddc2349 --- /dev/null +++ b/composition/test/test_api_lc_pubsub_composition.py.in @@ -0,0 +1,69 @@ +# Copyright 2025 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. + +import unittest + +from launch import LaunchDescription +from launch.actions import ExecuteProcess + +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing_ros + + +def generate_test_description(): + launch_description = LaunchDescription() + process_under_test = ExecuteProcess( + cmd=['@API_COMPOSITION_EXECUTABLE@'], + name='test_api_composition', + output='screen' + ) + launch_description.add_action(process_under_test) + launch_description.add_action( + ExecuteProcess( + cmd=[ + '@API_COMPOSITION_CLI_EXECUTABLE@', + 'composition', 'composition::LifecycleTalker' + ], + name='load_talker_component' + ) + ) + launch_description.add_action( + ExecuteProcess( + cmd=[ + '@API_COMPOSITION_CLI_EXECUTABLE@', + 'composition', 'composition::Listener' + ], + name='load_listener_component' + ) + ) + launch_description.add_action( + launch_testing.actions.ReadyToTest() + ) + return launch_description, locals() + + +class TestComposition(unittest.TestCase): + + def test_api_pubsub_composition(self, proc_output, process_under_test): + """Test process' output against expectations.""" + output_filter = launch_testing_ros.tools.basic_output_filter( + filtered_rmw_implementation='@rmw_implementation@' + ) + proc_output.assertWaitFor( + expected_output=launch_testing.tools.expected_output_from_file( + path='@EXPECTED_OUTPUT_PUBSUB@' + ), process=process_under_test, output_filter=output_filter, timeout=10 + ) diff --git a/composition/test/test_dlopen_composition.py.in b/composition/test/test_dlopen_composition.py.in index 19b9e5dce..c2c47b6f4 100644 --- a/composition/test/test_dlopen_composition.py.in +++ b/composition/test/test_dlopen_composition.py.in @@ -31,7 +31,8 @@ def generate_test_description(): '@TALKER_LIBRARY@', '@LISTENER_LIBRARY@', '@SERVER_LIBRARY@', - '@CLIENT_LIBRARY@' + '@CLIENT_LIBRARY@', + '@LIFECYCLE_LIBRARY@', ], name='test_dlopen_composition', output='screen'