diff --git a/tracetools_test/CMakeLists.txt b/tracetools_test/CMakeLists.txt index b0ee45f..4690fb8 100644 --- a/tracetools_test/CMakeLists.txt +++ b/tracetools_test/CMakeLists.txt @@ -30,10 +30,26 @@ if(BUILD_TESTING) rclcpp std_msgs ) + add_executable(test_ping + src/test_ping.cpp + ) + ament_target_dependencies(test_ping + rclcpp + std_msgs + ) + add_executable(test_pong + src/test_pong.cpp + ) + ament_target_dependencies(test_pong + rclcpp + std_msgs + ) install(TARGETS test_publisher test_subscription + test_ping + test_pong DESTINATION lib/${PROJECT_NAME} ) diff --git a/tracetools_test/src/test_ping.cpp b/tracetools_test/src/test_ping.cpp new file mode 100644 index 0000000..611afeb --- /dev/null +++ b/tracetools_test/src/test_ping.cpp @@ -0,0 +1,62 @@ +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/rate.hpp" +#include "std_msgs/msg/string.hpp" + +using namespace std::chrono_literals; + + +class PingNode : public rclcpp::Node +{ +public: + PingNode(rclcpp::NodeOptions options) : Node("ping_node", options) + { + sub_ = this->create_subscription( + "pong", + rclcpp::QoS(10), + std::bind(&PingNode::callback, this, std::placeholders::_1)); + pub_ = this->create_publisher( + "ping", + rclcpp::QoS(10)); + timer_ = this->create_wall_timer( + 500ms, + std::bind(&PingNode::timer_callback, this)); + } + + +private: + void callback(const std_msgs::msg::String::SharedPtr msg) + { + RCLCPP_INFO(this->get_logger(), "[output] %s", msg->data.c_str()); + rclcpp::shutdown(); + } + + void timer_callback() + { + auto msg = std::make_shared(); + msg->data = "some random ping string"; + pub_->publish(*msg); + } + + rclcpp::Subscription::SharedPtr sub_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + rclcpp::executors::SingleThreadedExecutor exec; + auto ping_node = std::make_shared(rclcpp::NodeOptions()); + 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/tracetools_test/src/test_pong.cpp b/tracetools_test/src/test_pong.cpp new file mode 100644 index 0000000..99e35ce --- /dev/null +++ b/tracetools_test/src/test_pong.cpp @@ -0,0 +1,49 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + + +class PongNode : public rclcpp::Node +{ +public: + PongNode(rclcpp::NodeOptions options) : Node("pong_node", options) + { + sub_ = this->create_subscription( + "ping", + rclcpp::QoS(10), + std::bind(&PongNode::callback, this, std::placeholders::_1)); + pub_ = this->create_publisher( + "pong", + rclcpp::QoS(10)); + } + +private: + void callback(const std_msgs::msg::String::SharedPtr msg) + { + RCLCPP_INFO(this->get_logger(), "[output] %s", msg->data.c_str()); + auto next_msg = std::make_shared(); + next_msg->data = "some random pong string"; + pub_->publish(*next_msg); + rclcpp::shutdown(); + } + + rclcpp::Subscription::SharedPtr sub_; + rclcpp::Publisher::SharedPtr pub_; +}; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + rclcpp::executors::SingleThreadedExecutor exec; + auto pong_node = std::make_shared(rclcpp::NodeOptions()); + 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/tracetools_test/test/test_node.py b/tracetools_test/test/test_node.py index 2757948..cd25e4a 100644 --- a/tracetools_test/test/test_node.py +++ b/tracetools_test/test/test_node.py @@ -17,7 +17,7 @@ class TestNode(unittest.TestCase): def test_creation(self): session_name_prefix = 'session-test-node-creation' base_path = '/tmp' - test_node = 'test_publisher' + test_node = ['test_publisher'] exit_code, full_path = run_and_trace(base_path, session_name_prefix, node_creation_events, None, PKG, test_node) self.assertEqual(exit_code, 0) diff --git a/tracetools_test/test/test_publisher.py b/tracetools_test/test/test_publisher.py index 0c15283..e2ce3d0 100644 --- a/tracetools_test/test/test_publisher.py +++ b/tracetools_test/test/test_publisher.py @@ -16,7 +16,7 @@ class TestPublisher(unittest.TestCase): def test_creation(self): session_name_prefix = 'session-test-publisher-creation' base_path = '/tmp' - test_node = 'test_publisher' + test_node = ['test_publisher'] exit_code, full_path = run_and_trace(base_path, session_name_prefix, publisher_creation_events, None, PKG, test_node) self.assertEqual(exit_code, 0) diff --git a/tracetools_test/test/test_subscription.py b/tracetools_test/test/test_subscription.py index 5caac19..693ba85 100644 --- a/tracetools_test/test/test_subscription.py +++ b/tracetools_test/test/test_subscription.py @@ -12,12 +12,17 @@ subscription_creation_events = [ 'ros2:rclcpp_subscription_callback_added', ] +subscription_callback_events = [ + 'ros2:rclcpp_subscription_callback_start', + 'ros2:rclcpp_subscription_callback_end', +] + class TestSubscription(unittest.TestCase): def test_creation(self): session_name_prefix = 'session-test-subscription-creation' base_path = '/tmp' - test_node = 'test_subscription' + test_node = ['test_subscription'] exit_code, full_path = run_and_trace(base_path, session_name_prefix, subscription_creation_events, None, PKG, test_node) self.assertEqual(exit_code, 0) @@ -28,6 +33,20 @@ class TestSubscription(unittest.TestCase): cleanup_trace(full_path) + def test_callback(self): + session_name_prefix = 'session-test-subscription-callback' + base_path = '/tmp' + test_nodes = ['test_ping', 'test_pong'] + + exit_code, full_path = run_and_trace(base_path, session_name_prefix, subscription_callback_events, None, PKG, test_nodes) + self.assertEqual(exit_code, 0) + + trace_events = get_trace_event_names(full_path) + print(f'trace_events: {trace_events}') + self.assertSetEqual(set(subscription_callback_events), trace_events) + + cleanup_trace(full_path) + if __name__ == '__main__': unittest.main() diff --git a/tracetools_test/tracetools_test/utils.py b/tracetools_test/tracetools_test/utils.py index f104867..462cb23 100644 --- a/tracetools_test/tracetools_test/utils.py +++ b/tracetools_test/tracetools_test/utils.py @@ -15,7 +15,7 @@ from tracetools_trace.tools.lttng import ( lttng_destroy, ) -def run_and_trace(base_path, session_name_prefix, ros_events, kernel_events, package_name, node_executable): +def run_and_trace(base_path, session_name_prefix, ros_events, kernel_events, package_name, node_names): """ Run a node while tracing :param base_path (str): the base path where to put the trace directory @@ -23,7 +23,7 @@ def run_and_trace(base_path, session_name_prefix, ros_events, kernel_events, pac :param ros_events (list(str)): the list of ROS UST events to enable :param kernel_events (list(str)): the list of kernel events to enable :param package_name (str): the name of the package to use - :param node_executable (str): the name of the node to execute + :param node_names (list(str)): the names of the nodes to execute """ session_name = f'{session_name_prefix}-{time.strftime("%Y%m%d%H%M%S")}' full_path = f'{base_path}/{session_name}' @@ -32,10 +32,13 @@ def run_and_trace(base_path, session_name_prefix, ros_events, kernel_events, pac lttng_setup(session_name, full_path, ros_events=ros_events, kernel_events=kernel_events) lttng_start(session_name) - ld = LaunchDescription([ - launch_ros.actions.Node( - package=package_name, node_executable=node_executable, output='screen'), - ]) + nodes = [] + for node_name in node_names: + n = launch_ros.actions.Node(package=package_name, + node_executable=node_name, + output='screen') + nodes.append(n) + ld = LaunchDescription(nodes) ls = LaunchService() ls.include_launch_description(get_default_launch_description()) ls.include_launch_description(ld)