diff --git a/test_tracetools/CMakeLists.txt b/test_tracetools/CMakeLists.txt index 9a9667cf..9c247bd9 100644 --- a/test_tracetools/CMakeLists.txt +++ b/test_tracetools/CMakeLists.txt @@ -115,6 +115,24 @@ if(BUILD_TESTING) ${std_msgs_TARGETS} ) + add_executable(test_generic_ping + src/test_generic_ping.cpp + ) + target_link_libraries(test_generic_ping PRIVATE + ${PROJECT_NAME}_mark_process + rclcpp::rclcpp + ${std_msgs_TARGETS} + ) + + add_executable(test_generic_pong + src/test_generic_pong.cpp + ) + target_link_libraries(test_generic_pong PRIVATE + ${PROJECT_NAME}_mark_process + rclcpp::rclcpp + ${std_msgs_TARGETS} + ) + add_executable(test_timer src/test_timer.cpp ) @@ -147,6 +165,8 @@ if(BUILD_TESTING) test_lifecycle_client test_ping test_pong + test_generic_ping + test_generic_pong test_publisher test_service_ping test_service_pong @@ -180,9 +200,11 @@ if(BUILD_TESTING) test/test_lifecycle_node.py test/test_node.py test/test_pub_sub.py + test/test_generic_pub_sub.py test/test_publisher.py test/test_service.py test/test_subscription.py + test/test_generic_subscription.py test/test_timer.py ) foreach(_test_path ${_test_tracetools_pytest_tests}) diff --git a/test_tracetools/src/test_generic_ping.cpp b/test_tracetools/src/test_generic_ping.cpp new file mode 100644 index 00000000..8ae18f89 --- /dev/null +++ b/test_tracetools/src/test_generic_ping.cpp @@ -0,0 +1,105 @@ +// Copyright 2019 Robert Bosch GmbH +// Copyright 2024 TIER IV, 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 "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include "test_tracetools/mark_process.hpp" + +using namespace std::chrono_literals; + +#define NODE_NAME "test_generic_ping" +#define SUB_TOPIC_NAME "pong" +#define PUB_TOPIC_NAME "ping" +#define QUEUE_DEPTH 10 + +class PingNode : public rclcpp::Node +{ +public: + PingNode(rclcpp::NodeOptions options, bool do_only_one) + : Node(NODE_NAME, options), do_only_one_(do_only_one) + { + sub_ = this->create_generic_subscription( + SUB_TOPIC_NAME, + "std_msgs/msg/String", + rclcpp::QoS(QUEUE_DEPTH), + std::bind(&PingNode::callback, this, std::placeholders::_1)); + pub_ = this->create_generic_publisher( + PUB_TOPIC_NAME, + "std_msgs/msg/String", + rclcpp::QoS(QUEUE_DEPTH)); + timer_ = this->create_wall_timer( + 500ms, + std::bind(&PingNode::timer_callback, this)); + serializer_ = std::make_shared>(); + } + + explicit PingNode(rclcpp::NodeOptions options) + : PingNode(options, true) {} + +private: + void callback(std::shared_ptr serialized_msg) + { + std_msgs::msg::String deserialized_msg; + serializer_->deserialize_message(serialized_msg.get(), &deserialized_msg); + RCLCPP_INFO(this->get_logger(), "[output] %s", deserialized_msg.data.c_str()); + if (do_only_one_) { + rclcpp::shutdown(); + } + } + + void timer_callback() + { + std_msgs::msg::String msg; + msg.data = "some random ping string"; + rclcpp::SerializedMessage serialized_msg; + serialized_msg.reserve(1024); + serializer_->serialize_message(&msg, &serialized_msg); + pub_->publish(serialized_msg); + } + + rclcpp::GenericSubscription::SharedPtr sub_; + rclcpp::GenericPublisher::SharedPtr pub_; + rclcpp::TimerBase::SharedPtr timer_; + std::shared_ptr> serializer_; + bool do_only_one_; +}; + +int main(int argc, char * argv[]) +{ + test_tracetools::mark_trace_test_process(); + + bool do_only_one = true; + for (int i = 0; i < argc; ++i) { + if (strncmp(argv[i], "do_more", 7) == 0) { + do_only_one = false; + } + } + + rclcpp::init(argc, argv); + + rclcpp::executors::SingleThreadedExecutor exec; + auto ping_node = std::make_shared(rclcpp::NodeOptions(), do_only_one); + exec.add_node(ping_node); + + printf("spinning\n"); + exec.spin(); + + // Will actually be called inside the node's callback + rclcpp::shutdown(); + return 0; +} diff --git a/test_tracetools/src/test_generic_pong.cpp b/test_tracetools/src/test_generic_pong.cpp new file mode 100644 index 00000000..d6398dca --- /dev/null +++ b/test_tracetools/src/test_generic_pong.cpp @@ -0,0 +1,93 @@ +// Copyright 2019 Robert Bosch GmbH +// Copyright 2024 TIER IV, 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 "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include "test_tracetools/mark_process.hpp" + +#define NODE_NAME "test_generic_pong" +#define SUB_TOPIC_NAME "ping" +#define PUB_TOPIC_NAME "pong" + +class PongNode : public rclcpp::Node +{ +public: + PongNode(rclcpp::NodeOptions options, bool do_only_one) + : Node(NODE_NAME, options), do_only_one_(do_only_one) + { + sub_ = this->create_generic_subscription( + SUB_TOPIC_NAME, + "std_msgs/msg/String", + rclcpp::QoS(10), + std::bind(&PongNode::callback, this, std::placeholders::_1)); + pub_ = this->create_generic_publisher( + PUB_TOPIC_NAME, + "std_msgs/msg/String", + rclcpp::QoS(10)); + serializer_ = std::make_shared>(); + } + + explicit PongNode(rclcpp::NodeOptions options) + : PongNode(options, true) {} + +private: + void callback(std::shared_ptr msg) + { + std_msgs::msg::String deserialized_msg; + serializer_->deserialize_message(msg.get(), &deserialized_msg); + RCLCPP_INFO(this->get_logger(), "[output] %s", deserialized_msg.data.c_str()); + std_msgs::msg::String next_msg; + next_msg.data = "some random pong string"; + rclcpp::SerializedMessage serialized_msg; + serialized_msg.reserve(1024); + serializer_->serialize_message(&next_msg, &serialized_msg); + pub_->publish(serialized_msg); + if (do_only_one_) { + rclcpp::shutdown(); + } + } + + rclcpp::GenericSubscription::SharedPtr sub_; + rclcpp::GenericPublisher::SharedPtr pub_; + std::shared_ptr> serializer_; + bool do_only_one_; +}; + +int main(int argc, char * argv[]) +{ + test_tracetools::mark_trace_test_process(); + + bool do_only_one = true; + for (int i = 0; i < argc; ++i) { + if (strncmp(argv[i], "do_more", 7) == 0) { + do_only_one = false; + } + } + + rclcpp::init(argc, argv); + + rclcpp::executors::SingleThreadedExecutor exec; + auto pong_node = std::make_shared(rclcpp::NodeOptions(), do_only_one); + exec.add_node(pong_node); + + printf("spinning\n"); + exec.spin(); + + // Will actually be called inside the node's callback + rclcpp::shutdown(); + return 0; +} diff --git a/test_tracetools/test/test_generic_pub_sub.py b/test_tracetools/test/test_generic_pub_sub.py new file mode 100644 index 00000000..9c773746 --- /dev/null +++ b/test_tracetools/test/test_generic_pub_sub.py @@ -0,0 +1,341 @@ +# Copyright 2021 Christophe Bedard +# Copyright 2024 TIER IV, 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 tracetools_test.case import TraceTestCase +from tracetools_trace.tools import tracepoints as tp +from tracetools_trace.tools.lttng import is_lttng_installed + + +@unittest.skipIf(not is_lttng_installed(minimum_version='2.9.0'), 'LTTng is required') +class TestGenericPubSub(TraceTestCase): + + def __init__(self, *args) -> None: + super().__init__( + *args, + session_name_prefix='session-test-generic-pub-sub', + events_ros=[ + tp.rmw_publisher_init, + tp.rcl_publisher_init, + tp.rmw_publish, + tp.rcl_publish, + tp.rclcpp_publish, + tp.rmw_subscription_init, + tp.rcl_subscription_init, + tp.rclcpp_subscription_init, + tp.rclcpp_subscription_callback_added, + tp.rmw_take, + tp.callback_start, + tp.callback_end, + ], + package='test_tracetools', + nodes=['test_generic_ping', 'test_generic_pong'], + ) + + def test_all(self): + # Check events as set + self.assertEventsSet(self._events_ros) + + # Get publisher init events & publisher handles of test topics + rmw_pub_init_events = self.get_events_with_name(tp.rmw_publisher_init) + rmw_sub_init_events = self.get_events_with_name(tp.rmw_subscription_init) + publisher_init_events = self.get_events_with_name(tp.rcl_publisher_init) + ping_publisher_init_events = self.get_events_with_field_value( + 'topic_name', + '/ping', + publisher_init_events, + ) + pong_publisher_init_events = self.get_events_with_field_value( + 'topic_name', + '/pong', + publisher_init_events, + ) + self.assertNumEventsEqual(ping_publisher_init_events, 1) + self.assertNumEventsEqual(pong_publisher_init_events, 1) + ping_publisher_init_event = ping_publisher_init_events[0] + pong_publisher_init_event = pong_publisher_init_events[0] + ping_pub_handle = self.get_field(ping_publisher_init_event, 'publisher_handle') + ping_rmw_pub_handle = self.get_field(ping_publisher_init_event, 'rmw_publisher_handle') + pong_pub_handle = self.get_field(pong_publisher_init_event, 'publisher_handle') + pong_rmw_pub_handle = self.get_field(pong_publisher_init_event, 'rmw_publisher_handle') + + # Find corresponding rmw_pub_init events + ping_rmw_pub_init_events = self.get_events_with_field_value( + 'rmw_publisher_handle', + ping_rmw_pub_handle, + rmw_pub_init_events, + ) + pong_rmw_pub_init_events = self.get_events_with_field_value( + 'rmw_publisher_handle', + pong_rmw_pub_handle, + rmw_pub_init_events, + ) + self.assertNumEventsEqual(ping_rmw_pub_init_events, 1) + self.assertNumEventsEqual(pong_rmw_pub_init_events, 1) + ping_rmw_pub_init_event = ping_rmw_pub_init_events[0] + pong_rmw_pub_init_event = pong_rmw_pub_init_events[0] + + # Check publisher init order (rmw then rcl) + self.assertEventOrder([ + ping_rmw_pub_init_event, + ping_publisher_init_event, + ]) + self.assertEventOrder([ + pong_rmw_pub_init_event, + pong_publisher_init_event, + ]) + + # Get corresponding rmw/rcl/rclcpp publish events for ping & pong + rmw_publish_events = self.get_events_with_name(tp.rmw_publish) + ping_rmw_pub_events = self.get_events_with_field_value( + 'rmw_publisher_handle', + ping_rmw_pub_handle, + rmw_publish_events, + ) + pong_rmw_pub_events = self.get_events_with_field_value( + 'rmw_publisher_handle', + pong_rmw_pub_handle, + rmw_publish_events, + ) + self.assertNumEventsEqual(ping_rmw_pub_events, 1) + self.assertNumEventsEqual(pong_rmw_pub_events, 1) + ping_rmw_pub_event = ping_rmw_pub_events[0] + pong_rmw_pub_event = pong_rmw_pub_events[0] + ping_pub_message = self.get_field(ping_rmw_pub_event, 'message') + pong_pub_message = self.get_field(pong_rmw_pub_event, 'message') + + rclcpp_publish_events = self.get_events_with_name(tp.rclcpp_publish) + rcl_publish_events = self.get_events_with_name(tp.rcl_publish) + ping_rclcpp_pub_events = self.get_events_with_field_value( + 'message', + ping_pub_message, + rclcpp_publish_events, + ) + pong_rclcpp_pub_events = self.get_events_with_field_value( + 'message', + pong_pub_message, + rclcpp_publish_events, + ) + ping_rcl_pub_events = self.get_events_with_field_value( + 'message', + ping_pub_message, + rcl_publish_events, + ) + pong_rcl_pub_events = self.get_events_with_field_value( + 'message', + pong_pub_message, + rcl_publish_events, + ) + self.assertNumEventsEqual(ping_rclcpp_pub_events, 1) + self.assertNumEventsEqual(pong_rclcpp_pub_events, 1) + self.assertNumEventsEqual(ping_rcl_pub_events, 1) + self.assertNumEventsEqual(pong_rcl_pub_events, 1) + ping_rclcpp_pub_event = ping_rclcpp_pub_events[0] + pong_rclcpp_pub_event = pong_rclcpp_pub_events[0] + ping_rcl_pub_event = ping_rcl_pub_events[0] + pong_rcl_pub_event = pong_rcl_pub_events[0] + self.assertFieldEquals(ping_rcl_pub_event, 'publisher_handle', ping_pub_handle) + self.assertFieldEquals(pong_rcl_pub_event, 'publisher_handle', pong_pub_handle) + + # Get subscription init events & subscription handles of test topics + rcl_subscription_init_events = self.get_events_with_name(tp.rcl_subscription_init) + ping_rcl_subscription_init_events = self.get_events_with_field_value( + 'topic_name', + '/ping', + rcl_subscription_init_events, + ) + pong_rcl_subscription_init_events = self.get_events_with_field_value( + 'topic_name', + '/pong', + rcl_subscription_init_events, + ) + self.assertNumEventsEqual(ping_rcl_subscription_init_events, 1) + self.assertNumEventsEqual(pong_rcl_subscription_init_events, 1) + ping_rcl_subscription_init_event = ping_rcl_subscription_init_events[0] + pong_rcl_subscription_init_event = pong_rcl_subscription_init_events[0] + ping_sub_handle = self.get_field(ping_rcl_subscription_init_event, 'subscription_handle') + ping_rmw_sub_handle = \ + self.get_field(ping_rcl_subscription_init_event, 'rmw_subscription_handle') + pong_sub_handle = self.get_field(pong_rcl_subscription_init_event, 'subscription_handle') + pong_rmw_sub_handle = \ + self.get_field(pong_rcl_subscription_init_event, 'rmw_subscription_handle') + + # Find corresponding rmw_sub_init events + ping_rmw_sub_init_events = self.get_events_with_field_value( + 'rmw_subscription_handle', + ping_rmw_sub_handle, + rmw_sub_init_events, + ) + pong_rmw_sub_init_events = self.get_events_with_field_value( + 'rmw_subscription_handle', + pong_rmw_sub_handle, + rmw_sub_init_events, + ) + self.assertNumEventsEqual(ping_rmw_sub_init_events, 1) + self.assertNumEventsEqual(pong_rmw_sub_init_events, 1) + ping_rmw_sub_init_event = ping_rmw_sub_init_events[0] + pong_rmw_sub_init_event = pong_rmw_sub_init_events[0] + + # Get corresponding subscription objects + rclcpp_subscription_init_events = self.get_events_with_name( + tp.rclcpp_subscription_init, + ) + ping_rclcpp_subscription_init_events = self.get_events_with_field_value( + 'subscription_handle', + ping_sub_handle, + rclcpp_subscription_init_events, + ) + pong_rclcpp_subscription_init_events = self.get_events_with_field_value( + 'subscription_handle', + pong_sub_handle, + rclcpp_subscription_init_events, + ) + self.assertNumEventsEqual(ping_rclcpp_subscription_init_events, 1) + self.assertNumEventsEqual(pong_rclcpp_subscription_init_events, 1) + ping_rclcpp_subscription_init_event = ping_rclcpp_subscription_init_events[0] + pong_rclcpp_subscription_init_event = pong_rclcpp_subscription_init_events[0] + ping_sub_object = self.get_field(ping_rclcpp_subscription_init_event, 'subscription') + pong_sub_object = self.get_field(pong_rclcpp_subscription_init_event, 'subscription') + + # Get corresponding subscription callback objects + rclcpp_subscription_callback_events = self.get_events_with_name( + tp.rclcpp_subscription_callback_added, + ) + ping_rclcpp_subscription_callback_events = self.get_events_with_field_value( + 'subscription', + ping_sub_object, + rclcpp_subscription_callback_events, + ) + pong_rclcpp_subscription_callback_events = self.get_events_with_field_value( + 'subscription', + pong_sub_object, + rclcpp_subscription_callback_events, + ) + self.assertNumEventsEqual(ping_rclcpp_subscription_callback_events, 1) + self.assertNumEventsEqual(pong_rclcpp_subscription_callback_events, 1) + ping_rclcpp_subscription_callback_event = ping_rclcpp_subscription_callback_events[0] + pong_rclcpp_subscription_callback_event = pong_rclcpp_subscription_callback_events[0] + ping_callback_object = self.get_field(ping_rclcpp_subscription_callback_event, 'callback') + pong_callback_object = self.get_field(pong_rclcpp_subscription_callback_event, 'callback') + + # Get rmw_take events + rmw_take_events = self.get_events_with_name(tp.rmw_take) + pong_rmw_take_events = self.get_events_with_field_value( + 'rmw_subscription_handle', + pong_rmw_sub_handle, + rmw_take_events, + ) + ping_rmw_take_events = self.get_events_with_field_value( + 'rmw_subscription_handle', + ping_rmw_sub_handle, + rmw_take_events, + ) + self.assertNumEventsEqual(pong_rmw_take_events, 1) + self.assertNumEventsEqual(ping_rmw_take_events, 1) + pong_rmw_take_event = pong_rmw_take_events[0] + ping_rmw_take_event = ping_rmw_take_events[0] + + # Check that pub->sub timestamps match + ping_timestamp = self.get_field(ping_rmw_pub_event, 'timestamp') + self.assertFieldEquals(ping_rmw_take_event, 'source_timestamp', ping_timestamp) + pong_timestamp = self.get_field(pong_rmw_pub_event, 'timestamp') + self.assertFieldEquals(pong_rmw_take_event, 'source_timestamp', pong_timestamp) + + # Check subscription init order + self.assertEventOrder([ + ping_rmw_sub_init_event, + ping_rcl_subscription_init_event, + ping_rclcpp_subscription_init_event, + ping_rclcpp_subscription_callback_event, + ]) + self.assertEventOrder([ + pong_rmw_sub_init_event, + pong_rcl_subscription_init_event, + pong_rclcpp_subscription_init_event, + pong_rclcpp_subscription_callback_event, + ]) + + # Get corresponding callback start/end events + callback_start_events = self.get_events_with_name(tp.callback_start) + callback_end_events = self.get_events_with_name(tp.callback_end) + ping_callback_start_events = self.get_events_with_field_value( + 'callback', + ping_callback_object, + callback_start_events, + ) + pong_callback_start_events = self.get_events_with_field_value( + 'callback', + pong_callback_object, + callback_start_events, + ) + ping_callback_end_events = self.get_events_with_field_value( + 'callback', + ping_callback_object, + callback_end_events, + ) + pong_callback_end_events = self.get_events_with_field_value( + 'callback', + pong_callback_object, + callback_end_events, + ) + self.assertNumEventsEqual(ping_callback_start_events, 1) + self.assertNumEventsEqual(pong_callback_start_events, 1) + self.assertNumEventsEqual(ping_callback_end_events, 1) + self.assertNumEventsEqual(pong_callback_end_events, 1) + ping_callback_start_event = ping_callback_start_events[0] + pong_callback_start_event = pong_callback_start_events[0] + ping_callback_end_event = ping_callback_end_events[0] + pong_callback_end_event = pong_callback_end_events[0] + + # Check pub/sub order: + # * /ping pub rclcpp_publish + # * /ping pub rcl_publish + # * /ping pub rmw_publish + # * /ping sub rmw_take + # * /ping sub callback_start + # * /pong pub rclcpp_publish + # * /pong pub rcl_publish + # * /pong pub rmw_publish + # ... + # * /ping sub callback_end + # ... we shouldn't necessarily expect the /pong callback to start + # before the /ping callback has ended + # * /pong sub rmw_take + # * /pong sub callback_start + # * /pong sub callback_end + self.assertEventOrder([ + ping_rclcpp_pub_event, + ping_rcl_pub_event, + ping_rmw_pub_event, + ping_rmw_take_event, + ping_callback_start_event, + pong_rclcpp_pub_event, + pong_rcl_pub_event, + pong_rmw_pub_event, + ping_callback_end_event, + ]) + self.assertEventOrder([ + pong_rclcpp_pub_event, + pong_rcl_pub_event, + pong_rmw_pub_event, + pong_rmw_take_event, + pong_callback_start_event, + pong_callback_end_event, + ]) + + +if __name__ == '__main__': + unittest.main() diff --git a/test_tracetools/test/test_generic_subscription.py b/test_tracetools/test/test_generic_subscription.py new file mode 100644 index 00000000..39922a2e --- /dev/null +++ b/test_tracetools/test/test_generic_subscription.py @@ -0,0 +1,272 @@ +# Copyright 2019 Robert Bosch GmbH +# Copyright 2019 Apex.AI, Inc. +# Copyright 2021 Christophe Bedard +# Copyright 2024 TIER IV, 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 tracetools_test.case import TraceTestCase +from tracetools_trace.tools import tracepoints as tp +from tracetools_trace.tools.lttng import is_lttng_installed + + +@unittest.skipIf(not is_lttng_installed(minimum_version='2.9.0'), 'LTTng is required') +class TestGenericSubscription(TraceTestCase): + + def __init__(self, *args) -> None: + super().__init__( + *args, + session_name_prefix='session-test-generic-subscription', + events_ros=[ + tp.rcl_node_init, + tp.rmw_subscription_init, + tp.rcl_subscription_init, + tp.rclcpp_subscription_init, + tp.rclcpp_subscription_callback_added, + tp.rclcpp_callback_register, + tp.rclcpp_executor_execute, + tp.rmw_take, + tp.rcl_take, + tp.rclcpp_take, + tp.callback_start, + tp.callback_end, + ], + package='test_tracetools', + nodes=['test_generic_ping', 'test_generic_pong'], + ) + + def test_all(self): + # Check events as set + self.assertEventsSet(self._events_ros) + + # Check fields + rmw_sub_init_events = self.get_events_with_name(tp.rmw_subscription_init) + rcl_sub_init_events = self.get_events_with_name(tp.rcl_subscription_init) + rclcpp_sub_init_events = self.get_events_with_name(tp.rclcpp_subscription_init) + callback_added_events = self.get_events_with_name(tp.rclcpp_subscription_callback_added) + callback_register_events = self.get_events_with_name(tp.rclcpp_callback_register) + execute_events = self.get_events_with_name(tp.rclcpp_executor_execute) + rmw_take_events = self.get_events_with_name(tp.rmw_take) + rcl_take_events = self.get_events_with_name(tp.rcl_take) + rclcpp_take_events = self.get_events_with_name(tp.rclcpp_take) + start_events = self.get_events_with_name(tp.callback_start) + end_events = self.get_events_with_name(tp.callback_end) + + for event in rmw_sub_init_events: + self.assertValidHandle(event, ['rmw_subscription_handle']) + self.assertValidArray(event, 'gid', int) + for event in rcl_sub_init_events: + self.assertValidHandle( + event, + ['subscription_handle', 'node_handle', 'rmw_subscription_handle'], + ) + self.assertValidQueueDepth(event, 'queue_depth') + self.assertStringFieldNotEmpty(event, 'topic_name') + for event in rclcpp_sub_init_events: + self.assertValidHandle( + event, + ['subscription_handle', 'subscription'], + ) + for event in callback_added_events: + self.assertValidHandle(event, ['subscription', 'callback']) + for event in callback_register_events: + self.assertValidPointer(event, 'callback') + self.assertStringFieldNotEmpty(event, 'symbol') + for event in rmw_take_events: + self.assertValidHandle(event, ['rmw_subscription_handle']) + self.assertValidPointer(event, ['message']) + self.assertFieldType(event, ['source_timestamp', 'taken'], int) + for event in rcl_take_events: + self.assertValidPointer(event, ['message']) + for event in rclcpp_take_events: + self.assertValidPointer(event, ['message']) + for event in start_events: + self.assertValidHandle(event, 'callback') + is_intra_process_value = self.get_field(event, 'is_intra_process') + self.assertIsInstance(is_intra_process_value, int, 'is_intra_process not int') + self.assertTrue( + is_intra_process_value in (0, 1), + f'invalid value for is_intra_process: {is_intra_process_value}', + ) + for event in end_events: + self.assertValidHandle(event, 'callback') + + # Check that the pong test topic name exists + # Note: using the ping node + test_rcl_sub_init_events = self.get_events_with_field_value( + 'topic_name', + '/pong', + rcl_sub_init_events, + ) + self.assertNumEventsEqual(test_rcl_sub_init_events, 1, 'cannot find test topic name') + test_rcl_sub_init_event = test_rcl_sub_init_events[0] + + # Check queue_depth value + self.assertFieldEquals(test_rcl_sub_init_event, 'queue_depth', 10) + + # Check that the node handle matches the node_init event + node_init_events = self.get_events_with_name(tp.rcl_node_init) + test_sub_node_init_events = self.get_events_with_procname( + 'test_generic_ping', + node_init_events, + ) + self.assertNumEventsEqual(test_sub_node_init_events, 1) + test_sub_node_init_event = test_sub_node_init_events[0] + self.assertMatchingField( + test_sub_node_init_event, + 'node_handle', + tp.rcl_subscription_init, + rcl_sub_init_events, + ) + + # Check that subscription handle matches between rcl_sub_init and rclcpp_sub_init + subscription_handle = self.get_field(test_rcl_sub_init_event, 'subscription_handle') + rclcpp_sub_init_matching_events = self.get_events_with_field_value( + 'subscription_handle', + subscription_handle, + rclcpp_sub_init_events, + ) + # Should only have 1 rclcpp_sub_init event, since intra-process is not enabled + self.assertNumEventsEqual(rclcpp_sub_init_matching_events, 1) + # Check that the rmw subscription handle matches between rmw_sub_init and rcl_sub_init + rmw_subscription_handle = self.get_field( + test_rcl_sub_init_event, 'rmw_subscription_handle') + rmw_sub_init_events = self.get_events_with_field_value( + 'rmw_subscription_handle', + rmw_subscription_handle, + rmw_sub_init_events, + ) + self.assertNumEventsEqual(rmw_sub_init_events, 1) + rmw_sub_init_event = rmw_sub_init_events[0] + + # Check that subscription pointer matches between rclcpp_sub_init and sub_callback_added + rclcpp_sub_init_matching_event = rclcpp_sub_init_matching_events[0] + subscription_pointer = self.get_field(rclcpp_sub_init_matching_event, 'subscription') + callback_added_matching_events = self.get_events_with_field_value( + 'subscription', + subscription_pointer, + callback_added_events, + ) + self.assertNumEventsEqual(callback_added_matching_events, 1) + callback_added_matching_event = callback_added_matching_events[0] + + # Check that callback pointer matches between callback_added and callback_register + callback_handle = self.get_field(callback_added_matching_event, 'callback') + test_sub_node_callback_register_events = self.get_events_with_procname( + 'test_generic_ping', + callback_register_events, + ) + callback_register_matching_events = self.get_events_with_field_value( + 'callback', + callback_handle, + test_sub_node_callback_register_events, + ) + self.assertNumEventsEqual(callback_register_matching_events, 1) + callback_register_matching_event = callback_register_matching_events[0] + + # Check susbcription creation events order + self.assertEventOrder([ + rmw_sub_init_event, + test_rcl_sub_init_event, + rclcpp_sub_init_matching_event, + callback_added_matching_event, + callback_register_matching_event, + ]) + + # Get executor_execute and *_take events, there should only be one message received + test_execute_events = self.get_events_with_field_value( + 'handle', + subscription_handle, + execute_events, + ) + self.assertNumEventsEqual(test_execute_events, 1) + test_execute_event = test_execute_events[0] + test_rmw_take_events = self.get_events_with_field_value( + 'rmw_subscription_handle', + rmw_subscription_handle, + rmw_take_events, + ) + self.assertNumEventsEqual(test_execute_events, 1) + test_rmw_take_event = test_rmw_take_events[0] + test_taken_msg = self.get_field(test_rmw_take_event, 'message') + self.assertFieldEquals(test_rmw_take_event, 'taken', 1, 'test message not taken') + test_rcl_take_events = self.get_events_with_field_value( + 'message', + test_taken_msg, + rcl_take_events, + ) + self.assertNumEventsEqual(test_rcl_take_events, 1) + test_rcl_take_event = test_rcl_take_events[0] + test_rclcpp_take_events = self.get_events_with_field_value( + 'message', + test_taken_msg, + rclcpp_take_events, + ) + self.assertNumEventsEqual(test_rcl_take_events, 1) + test_rclcpp_take_event = test_rclcpp_take_events[0] + + # Check that each start:end pair has a common callback handle + ping_events = self.get_events_with_procname('test_generic_ping') + pong_events = self.get_events_with_procname('test_generic_pong') + ping_events_start = self.get_events_with_name(tp.callback_start, ping_events) + pong_events_start = self.get_events_with_name(tp.callback_start, pong_events) + for ping_start in ping_events_start: + self.assertMatchingField( + ping_start, + 'callback', + tp.callback_end, + ping_events, + ) + for pong_start in pong_events_start: + self.assertMatchingField( + pong_start, + 'callback', + tp.callback_end, + pong_events, + ) + + # Check that callback pointer matches between sub_callback_added and callback_start/end + # There is only one callback for /pong topic in ping node + callback_added_matching_event = callback_added_matching_events[0] + callback_pointer = self.get_field(callback_added_matching_event, 'callback') + callback_start_matching_events = self.get_events_with_field_value( + 'callback', + callback_pointer, + ping_events_start, + ) + self.assertNumEventsEqual(callback_start_matching_events, 1) + callback_start_matching_event = callback_start_matching_events[0] + ping_events_end = self.get_events_with_name(tp.callback_end, ping_events) + callback_end_matching_events = self.get_events_with_field_value( + 'callback', + callback_pointer, + ping_events_end, + ) + self.assertNumEventsEqual(callback_end_matching_events, 1) + callback_end_matching_event = callback_end_matching_events[0] + + # Check execute+take+callback order + self.assertEventOrder([ + test_execute_event, + test_rmw_take_event, + test_rcl_take_event, + test_rclcpp_take_event, + callback_start_matching_event, + callback_end_matching_event, + ]) + + +if __name__ == '__main__': + unittest.main()